1794 files changed, 66250 insertions, 39263 deletions

diff --git a/drivers/acpi/ac.c b/drivers/acpi/ac.c
index 36b0e61f9c09..bbcc2b5a70d4 100644
--- a/drivers/acpi/ac.c
+++ b/drivers/acpi/ac.c
@@ -95,13 +95,14 @@ static struct acpi_driver acpi_ac_driver = {
 };
 
 struct acpi_ac {
-	struct power_supply charger;
+	struct power_supply *charger;
+	struct power_supply_desc charger_desc;
 	struct acpi_device * device;
 	unsigned long long state;
 	struct notifier_block battery_nb;
 };
 
-#define to_acpi_ac(x) container_of(x, struct acpi_ac, charger)
+#define to_acpi_ac(x) power_supply_get_drvdata(x)
 
 #ifdef CONFIG_ACPI_PROCFS_POWER
 static const struct file_operations acpi_ac_fops = {
@@ -275,7 +276,7 @@ static void acpi_ac_notify(struct acpi_device *device, u32 event)
 						  dev_name(&device->dev), event,
 						  (u32) ac->state);
 		acpi_notifier_call_chain(device, event, (u32) ac->state);
-		kobject_uevent(&ac->charger.dev->kobj, KOBJ_CHANGE);
+		kobject_uevent(&ac->charger->dev.kobj, KOBJ_CHANGE);
 	}
 
 	return;
@@ -321,6 +322,7 @@ static struct dmi_system_id ac_dmi_table[] = {
 
 static int acpi_ac_add(struct acpi_device *device)
 {
+	struct power_supply_config psy_cfg = {};
 	int result = 0;
 	struct acpi_ac *ac = NULL;
 
@@ -341,19 +343,24 @@ static int acpi_ac_add(struct acpi_device *device)
 	if (result)
 		goto end;
 
-	ac->charger.name = acpi_device_bid(device);
+	psy_cfg.drv_data = ac;
+
+	ac->charger_desc.name = acpi_device_bid(device);
 #ifdef CONFIG_ACPI_PROCFS_POWER
 	result = acpi_ac_add_fs(ac);
 	if (result)
 		goto end;
 #endif
-	ac->charger.type = POWER_SUPPLY_TYPE_MAINS;
-	ac->charger.properties = ac_props;
-	ac->charger.num_properties = ARRAY_SIZE(ac_props);
-	ac->charger.get_property = get_ac_property;
-	result = power_supply_register(&ac->device->dev, &ac->charger);
-	if (result)
+	ac->charger_desc.type = POWER_SUPPLY_TYPE_MAINS;
+	ac->charger_desc.properties = ac_props;
+	ac->charger_desc.num_properties = ARRAY_SIZE(ac_props);
+	ac->charger_desc.get_property = get_ac_property;
+	ac->charger = power_supply_register(&ac->device->dev,
+					    &ac->charger_desc, &psy_cfg);
+	if (IS_ERR(ac->charger)) {
+		result = PTR_ERR(ac->charger);
 		goto end;
+	}
 
 	printk(KERN_INFO PREFIX "%s [%s] (%s)\n",
 	       acpi_device_name(device), acpi_device_bid(device),
@@ -390,7 +397,7 @@ static int acpi_ac_resume(struct device *dev)
 	if (acpi_ac_get_state(ac))
 		return 0;
 	if (old_state != ac->state)
-		kobject_uevent(&ac->charger.dev->kobj, KOBJ_CHANGE);
+		kobject_uevent(&ac->charger->dev.kobj, KOBJ_CHANGE);
 	return 0;
 }
 #else
@@ -407,8 +414,7 @@ static int acpi_ac_remove(struct acpi_device *device)
 
 	ac = acpi_driver_data(device);
 
-	if (ac->charger.dev)
-		power_supply_unregister(&ac->charger);
+	power_supply_unregister(ac->charger);
 	unregister_acpi_notifier(&ac->battery_nb);
 
 #ifdef CONFIG_ACPI_PROCFS_POWER
diff --git a/drivers/acpi/acpi_lpss.c b/drivers/acpi/acpi_lpss.c
index 657964e8ab7e..37fb19047603 100644
--- a/drivers/acpi/acpi_lpss.c
+++ b/drivers/acpi/acpi_lpss.c
@@ -65,6 +65,7 @@ struct lpss_private_data;
 
 struct lpss_device_desc {
 	unsigned int flags;
+	const char *clk_con_id;
 	unsigned int prv_offset;
 	size_t prv_size_override;
 	void (*setup)(struct lpss_private_data *pdata);
@@ -140,6 +141,7 @@ static struct lpss_device_desc lpt_i2c_dev_desc = {
 
 static struct lpss_device_desc lpt_uart_dev_desc = {
 	.flags = LPSS_CLK | LPSS_CLK_GATE | LPSS_CLK_DIVIDER | LPSS_LTR,
+	.clk_con_id = "baudclk",
 	.prv_offset = 0x800,
 	.setup = lpss_uart_setup,
 };
@@ -156,6 +158,7 @@ static struct lpss_device_desc byt_pwm_dev_desc = {
 
 static struct lpss_device_desc byt_uart_dev_desc = {
 	.flags = LPSS_CLK | LPSS_CLK_GATE | LPSS_CLK_DIVIDER | LPSS_SAVE_CTX,
+	.clk_con_id = "baudclk",
 	.prv_offset = 0x800,
 	.setup = lpss_uart_setup,
 };
@@ -313,7 +316,7 @@ out:
 		return PTR_ERR(clk);
 
 	pdata->clk = clk;
-	clk_register_clkdev(clk, NULL, devname);
+	clk_register_clkdev(clk, dev_desc->clk_con_id, devname);
 	return 0;
 }
 
diff --git a/drivers/acpi/acpi_pad.c b/drivers/acpi/acpi_pad.c
index c7b105c0e1d3..6bc9cbc01ad6 100644
--- a/drivers/acpi/acpi_pad.c
+++ b/drivers/acpi/acpi_pad.c
@@ -26,7 +26,7 @@
 #include <linux/kthread.h>
 #include <linux/freezer.h>
 #include <linux/cpu.h>
-#include <linux/clockchips.h>
+#include <linux/tick.h>
 #include <linux/slab.h>
 #include <linux/acpi.h>
 #include <asm/mwait.h>
@@ -41,8 +41,6 @@ static unsigned long power_saving_mwait_eax;
 
 static unsigned char tsc_detected_unstable;
 static unsigned char tsc_marked_unstable;
-static unsigned char lapic_detected_unstable;
-static unsigned char lapic_marked_unstable;
 
 static void power_saving_mwait_init(void)
 {
@@ -82,13 +80,10 @@ static void power_saving_mwait_init(void)
 		 */
 		if (!boot_cpu_has(X86_FEATURE_NONSTOP_TSC))
 			tsc_detected_unstable = 1;
-		if (!boot_cpu_has(X86_FEATURE_ARAT))
-			lapic_detected_unstable = 1;
 		break;
 	default:
-		/* TSC & LAPIC could halt in idle */
+		/* TSC could halt in idle */
 		tsc_detected_unstable = 1;
-		lapic_detected_unstable = 1;
 	}
 #endif
 }
@@ -155,7 +150,6 @@ static int power_saving_thread(void *data)
 	sched_setscheduler(current, SCHED_RR, &param);
 
 	while (!kthread_should_stop()) {
-		int cpu;
 		unsigned long expire_time;
 
 		try_to_freeze();
@@ -177,28 +171,15 @@ static int power_saving_thread(void *data)
 				mark_tsc_unstable("TSC halts in idle");
 				tsc_marked_unstable = 1;
 			}
-			if (lapic_detected_unstable && !lapic_marked_unstable) {
-				int i;
-				/* LAPIC could halt in idle, so notify users */
-				for_each_online_cpu(i)
-					clockevents_notify(
-						CLOCK_EVT_NOTIFY_BROADCAST_ON,
-						&i);
-				lapic_marked_unstable = 1;
-			}
 			local_irq_disable();
-			cpu = smp_processor_id();
-			if (lapic_marked_unstable)
-				clockevents_notify(
-					CLOCK_EVT_NOTIFY_BROADCAST_ENTER, &cpu);
+			tick_broadcast_enable();
+			tick_broadcast_enter();
 			stop_critical_timings();
 
 			mwait_idle_with_hints(power_saving_mwait_eax, 1);
 
 			start_critical_timings();
-			if (lapic_marked_unstable)
-				clockevents_notify(
-					CLOCK_EVT_NOTIFY_BROADCAST_EXIT, &cpu);
+			tick_broadcast_exit();
 			local_irq_enable();
 
 			if (time_before(expire_time, jiffies)) {
diff --git a/drivers/acpi/battery.c b/drivers/acpi/battery.c
index d98ba4355819..672263a3832c 100644
--- a/drivers/acpi/battery.c
+++ b/drivers/acpi/battery.c
@@ -117,7 +117,8 @@ enum {
 struct acpi_battery {
 	struct mutex lock;
 	struct mutex sysfs_lock;
-	struct power_supply bat;
+	struct power_supply *bat;
+	struct power_supply_desc bat_desc;
 	struct acpi_device *device;
 	struct notifier_block pm_nb;
 	unsigned long update_time;
@@ -149,7 +150,7 @@ struct acpi_battery {
 	unsigned long flags;
 };
 
-#define to_acpi_battery(x) container_of(x, struct acpi_battery, bat)
+#define to_acpi_battery(x) power_supply_get_drvdata(x)
 
 static inline int acpi_battery_present(struct acpi_battery *battery)
 {
@@ -608,40 +609,45 @@ static struct device_attribute alarm_attr = {
 
 static int sysfs_add_battery(struct acpi_battery *battery)
 {
-	int result;
+	struct power_supply_config psy_cfg = { .drv_data = battery, };
 
 	if (battery->power_unit == ACPI_BATTERY_POWER_UNIT_MA) {
-		battery->bat.properties = charge_battery_props;
-		battery->bat.num_properties =
+		battery->bat_desc.properties = charge_battery_props;
+		battery->bat_desc.num_properties =
 			ARRAY_SIZE(charge_battery_props);
 	} else {
-		battery->bat.properties = energy_battery_props;
-		battery->bat.num_properties =
+		battery->bat_desc.properties = energy_battery_props;
+		battery->bat_desc.num_properties =
 			ARRAY_SIZE(energy_battery_props);
 	}
 
-	battery->bat.name = acpi_device_bid(battery->device);
-	battery->bat.type = POWER_SUPPLY_TYPE_BATTERY;
-	battery->bat.get_property = acpi_battery_get_property;
+	battery->bat_desc.name = acpi_device_bid(battery->device);
+	battery->bat_desc.type = POWER_SUPPLY_TYPE_BATTERY;
+	battery->bat_desc.get_property = acpi_battery_get_property;
 
-	result = power_supply_register_no_ws(&battery->device->dev, &battery->bat);
+	battery->bat = power_supply_register_no_ws(&battery->device->dev,
+				&battery->bat_desc, &psy_cfg);
 
-	if (result)
+	if (IS_ERR(battery->bat)) {
+		int result = PTR_ERR(battery->bat);
+
+		battery->bat = NULL;
 		return result;
-	return device_create_file(battery->bat.dev, &alarm_attr);
+	}
+	return device_create_file(&battery->bat->dev, &alarm_attr);
 }
 
 static void sysfs_remove_battery(struct acpi_battery *battery)
 {
 	mutex_lock(&battery->sysfs_lock);
-	if (!battery->bat.dev) {
+	if (!battery->bat) {
 		mutex_unlock(&battery->sysfs_lock);
 		return;
 	}
 
-	device_remove_file(battery->bat.dev, &alarm_attr);
-	power_supply_unregister(&battery->bat);
-	battery->bat.dev = NULL;
+	device_remove_file(&battery->bat->dev, &alarm_attr);
+	power_supply_unregister(battery->bat);
+	battery->bat = NULL;
 	mutex_unlock(&battery->sysfs_lock);
 }
 
@@ -738,7 +744,7 @@ static int acpi_battery_update(struct acpi_battery *battery, bool resume)
 			return result;
 		acpi_battery_init_alarm(battery);
 	}
-	if (!battery->bat.dev) {
+	if (!battery->bat) {
 		result = sysfs_add_battery(battery);
 		if (result)
 			return result;
@@ -764,7 +770,7 @@ static void acpi_battery_refresh(struct acpi_battery *battery)
 {
 	int power_unit;
 
-	if (!battery->bat.dev)
+	if (!battery->bat)
 		return;
 
 	power_unit = battery->power_unit;
@@ -1062,11 +1068,11 @@ static void acpi_battery_remove_fs(struct acpi_device *device)
 static void acpi_battery_notify(struct acpi_device *device, u32 event)
 {
 	struct acpi_battery *battery = acpi_driver_data(device);
-	struct device *old;
+	struct power_supply *old;
 
 	if (!battery)
 		return;
-	old = battery->bat.dev;
+	old = battery->bat;
 	/*
 	* On Acer Aspire V5-573G notifications are sometimes triggered too
 	* early. For example, when AC is unplugged and notification is
@@ -1083,8 +1089,8 @@ static void acpi_battery_notify(struct acpi_device *device, u32 event)
 					acpi_battery_present(battery));
 	acpi_notifier_call_chain(device, event, acpi_battery_present(battery));
 	/* acpi_battery_update could remove power_supply object */
-	if (old && battery->bat.dev)
-		power_supply_changed(&battery->bat);
+	if (old && battery->bat)
+		power_supply_changed(battery->bat);
 }
 
 static int battery_notify(struct notifier_block *nb,
@@ -1100,7 +1106,7 @@ static int battery_notify(struct notifier_block *nb,
 		if (!acpi_battery_present(battery))
 			return 0;
 
-		if (!battery->bat.dev) {
+		if (!battery->bat) {
 			result = acpi_battery_get_info(battery);
 			if (result)
 				return result;
diff --git a/drivers/acpi/pci_irq.c b/drivers/acpi/pci_irq.c
index e7f718d6918a..b1def411c0b8 100644
--- a/drivers/acpi/pci_irq.c
+++ b/drivers/acpi/pci_irq.c
@@ -485,6 +485,14 @@ void acpi_pci_irq_disable(struct pci_dev *dev)
 	if (!pin || !dev->irq_managed || dev->irq <= 0)
 		return;
 
+	/* Keep IOAPIC pin configuration when suspending */
+	if (dev->dev.power.is_prepared)
+		return;
+#ifdef	CONFIG_PM
+	if (dev->dev.power.runtime_status == RPM_SUSPENDING)
+		return;
+#endif
+
 	entry = acpi_pci_irq_lookup(dev, pin);
 	if (!entry)
 		return;
@@ -505,6 +513,5 @@ void acpi_pci_irq_disable(struct pci_dev *dev)
 	if (gsi >= 0) {
 		acpi_unregister_gsi(gsi);
 		dev->irq_managed = 0;
-		dev->irq = 0;
 	}
 }
diff --git a/drivers/acpi/pci_root.c b/drivers/acpi/pci_root.c
index 68a5f712cd19..1b5569c092c6 100644
--- a/drivers/acpi/pci_root.c
+++ b/drivers/acpi/pci_root.c
@@ -423,8 +423,7 @@ out:
 }
 EXPORT_SYMBOL(acpi_pci_osc_control_set);
 
-static void negotiate_os_control(struct acpi_pci_root *root, int *no_aspm,
-				 int *clear_aspm)
+static void negotiate_os_control(struct acpi_pci_root *root, int *no_aspm)
 {
 	u32 support, control, requested;
 	acpi_status status;
@@ -495,10 +494,12 @@ static void negotiate_os_control(struct acpi_pci_root *root, int *no_aspm,
 		decode_osc_control(root, "OS now controls", control);
 		if (acpi_gbl_FADT.boot_flags & ACPI_FADT_NO_ASPM) {
 			/*
-			 * We have ASPM control, but the FADT indicates
-			 * that it's unsupported. Clear it.
+			 * We have ASPM control, but the FADT indicates that
+			 * it's unsupported. Leave existing configuration
+			 * intact and prevent the OS from touching it.
 			 */
-			*clear_aspm = 1;
+			dev_info(&device->dev, "FADT indicates ASPM is unsupported, using BIOS configuration\n");
+			*no_aspm = 1;
 		}
 	} else {
 		decode_osc_control(root, "OS requested", requested);
@@ -525,7 +526,7 @@ static int acpi_pci_root_add(struct acpi_device *device,
 	int result;
 	struct acpi_pci_root *root;
 	acpi_handle handle = device->handle;
-	int no_aspm = 0, clear_aspm = 0;
+	int no_aspm = 0;
 	bool hotadd = system_state != SYSTEM_BOOTING;
 
 	root = kzalloc(sizeof(struct acpi_pci_root), GFP_KERNEL);
@@ -584,7 +585,7 @@ static int acpi_pci_root_add(struct acpi_device *device,
 
 	root->mcfg_addr = acpi_pci_root_get_mcfg_addr(handle);
 
-	negotiate_os_control(root, &no_aspm, &clear_aspm);
+	negotiate_os_control(root, &no_aspm);
 
 	/*
 	 * TBD: Need PCI interface for enumeration/configuration of roots.
@@ -607,10 +608,6 @@ static int acpi_pci_root_add(struct acpi_device *device,
 		goto remove_dmar;
 	}
 
-	if (clear_aspm) {
-		dev_info(&device->dev, "Disabling ASPM (FADT indicates it is unsupported)\n");
-		pcie_clear_aspm(root->bus);
-	}
 	if (no_aspm)
 		pcie_no_aspm();
 
diff --git a/drivers/acpi/processor_idle.c b/drivers/acpi/processor_idle.c
index c6bb9f1257c9..39e0c8e36244 100644
--- a/drivers/acpi/processor_idle.c
+++ b/drivers/acpi/processor_idle.c
@@ -32,7 +32,7 @@
 #include <linux/acpi.h>
 #include <linux/dmi.h>
 #include <linux/sched.h>       /* need_resched() */
-#include <linux/clockchips.h>
+#include <linux/tick.h>
 #include <linux/cpuidle.h>
 #include <linux/syscore_ops.h>
 #include <acpi/processor.h>
@@ -157,12 +157,11 @@ static void lapic_timer_check_state(int state, struct acpi_processor *pr,
 static void __lapic_timer_propagate_broadcast(void *arg)
 {
 	struct acpi_processor *pr = (struct acpi_processor *) arg;
-	unsigned long reason;
 
-	reason = pr->power.timer_broadcast_on_state < INT_MAX ?
-		CLOCK_EVT_NOTIFY_BROADCAST_ON : CLOCK_EVT_NOTIFY_BROADCAST_OFF;
-
-	clockevents_notify(reason, &pr->id);
+	if (pr->power.timer_broadcast_on_state < INT_MAX)
+		tick_broadcast_enable();
+	else
+		tick_broadcast_disable();
 }
 
 static void lapic_timer_propagate_broadcast(struct acpi_processor *pr)
@@ -179,11 +178,10 @@ static void lapic_timer_state_broadcast(struct acpi_processor *pr,
 	int state = cx - pr->power.states;
 
 	if (state >= pr->power.timer_broadcast_on_state) {
-		unsigned long reason;
-
-		reason = broadcast ?  CLOCK_EVT_NOTIFY_BROADCAST_ENTER :
-			CLOCK_EVT_NOTIFY_BROADCAST_EXIT;
-		clockevents_notify(reason, &pr->id);
+		if (broadcast)
+			tick_broadcast_enter();
+		else
+			tick_broadcast_exit();
 	}
 }
 
@@ -922,7 +920,7 @@ static int acpi_processor_setup_cpuidle_states(struct acpi_processor *pr)
 		return -EINVAL;
 
 	drv->safe_state_index = -1;
-	for (i = 0; i < CPUIDLE_STATE_MAX; i++) {
+	for (i = CPUIDLE_DRIVER_STATE_START; i < CPUIDLE_STATE_MAX; i++) {
 		drv->states[i].name[0] = '\0';
 		drv->states[i].desc[0] = '\0';
 	}
diff --git a/drivers/acpi/resource.c b/drivers/acpi/resource.c
index c723668e3e27..5589a6e2a023 100644
--- a/drivers/acpi/resource.c
+++ b/drivers/acpi/resource.c
@@ -42,8 +42,10 @@ static bool acpi_dev_resource_len_valid(u64 start, u64 end, u64 len, bool io)
 	 * CHECKME: len might be required to check versus a minimum
 	 * length as well. 1 for io is fine, but for memory it does
 	 * not make any sense at all.
+	 * Note: some BIOSes report incorrect length for ACPI address space
+	 * descriptor, so remove check of 'reslen == len' to avoid regression.
 	 */
-	if (len && reslen && reslen == len && start <= end)
+	if (len && reslen && start <= end)
 		return true;
 
 	pr_debug("ACPI: invalid or unassigned resource %s [%016llx - %016llx] length [%016llx]\n",
diff --git a/drivers/acpi/sbs.c b/drivers/acpi/sbs.c
index a7a3edd28beb..cd827625cf07 100644
--- a/drivers/acpi/sbs.c
+++ b/drivers/acpi/sbs.c
@@ -74,7 +74,8 @@ static const struct acpi_device_id sbs_device_ids[] = {
 MODULE_DEVICE_TABLE(acpi, sbs_device_ids);
 
 struct acpi_battery {
-	struct power_supply bat;
+	struct power_supply *bat;
+	struct power_supply_desc bat_desc;
 	struct acpi_sbs *sbs;
 	unsigned long update_time;
 	char name[8];
@@ -101,10 +102,10 @@ struct acpi_battery {
 	u8 have_sysfs_alarm:1;
 };
 
-#define to_acpi_battery(x) container_of(x, struct acpi_battery, bat)
+#define to_acpi_battery(x) power_supply_get_drvdata(x)
 
 struct acpi_sbs {
-	struct power_supply charger;
+	struct power_supply *charger;
 	struct acpi_device *device;
 	struct acpi_smb_hc *hc;
 	struct mutex lock;
@@ -115,7 +116,7 @@ struct acpi_sbs {
 	u8 charger_exists:1;
 };
 
-#define to_acpi_sbs(x) container_of(x, struct acpi_sbs, charger)
+#define to_acpi_sbs(x) power_supply_get_drvdata(x)
 
 static int acpi_sbs_remove(struct acpi_device *device);
 static int acpi_battery_get_state(struct acpi_battery *battery);
@@ -303,6 +304,13 @@ static enum power_supply_property sbs_energy_battery_props[] = {
 	POWER_SUPPLY_PROP_MANUFACTURER,
 };
 
+static const struct power_supply_desc acpi_sbs_charger_desc = {
+	.name		= "sbs-charger",
+	.type		= POWER_SUPPLY_TYPE_MAINS,
+	.properties	= sbs_ac_props,
+	.num_properties	= ARRAY_SIZE(sbs_ac_props),
+	.get_property	= sbs_get_ac_property,
+};
 
 /* --------------------------------------------------------------------------
                             Smart Battery System Management
@@ -519,6 +527,7 @@ static int acpi_battery_read(struct acpi_battery *battery)
 static int acpi_battery_add(struct acpi_sbs *sbs, int id)
 {
 	struct acpi_battery *battery = &sbs->battery[id];
+	struct power_supply_config psy_cfg = { .drv_data = battery, };
 	int result;
 
 	battery->id = id;
@@ -528,23 +537,27 @@ static int acpi_battery_add(struct acpi_sbs *sbs, int id)
 		return result;
 
 	sprintf(battery->name, ACPI_BATTERY_DIR_NAME, id);
-	battery->bat.name = battery->name;
-	battery->bat.type = POWER_SUPPLY_TYPE_BATTERY;
+	battery->bat_desc.name = battery->name;
+	battery->bat_desc.type = POWER_SUPPLY_TYPE_BATTERY;
 	if (!acpi_battery_mode(battery)) {
-		battery->bat.properties = sbs_charge_battery_props;
-		battery->bat.num_properties =
+		battery->bat_desc.properties = sbs_charge_battery_props;
+		battery->bat_desc.num_properties =
 		    ARRAY_SIZE(sbs_charge_battery_props);
 	} else {
-		battery->bat.properties = sbs_energy_battery_props;
-		battery->bat.num_properties =
+		battery->bat_desc.properties = sbs_energy_battery_props;
+		battery->bat_desc.num_properties =
 		    ARRAY_SIZE(sbs_energy_battery_props);
 	}
-	battery->bat.get_property = acpi_sbs_battery_get_property;
-	result = power_supply_register(&sbs->device->dev, &battery->bat);
-	if (result)
+	battery->bat_desc.get_property = acpi_sbs_battery_get_property;
+	battery->bat = power_supply_register(&sbs->device->dev,
+					&battery->bat_desc, &psy_cfg);
+	if (IS_ERR(battery->bat)) {
+		result = PTR_ERR(battery->bat);
+		battery->bat = NULL;
 		goto end;
+	}
 
-	result = device_create_file(battery->bat.dev, &alarm_attr);
+	result = device_create_file(&battery->bat->dev, &alarm_attr);
 	if (result)
 		goto end;
 	battery->have_sysfs_alarm = 1;
@@ -559,28 +572,29 @@ static void acpi_battery_remove(struct acpi_sbs *sbs, int id)
 {
 	struct acpi_battery *battery = &sbs->battery[id];
 
-	if (battery->bat.dev) {
+	if (battery->bat) {
 		if (battery->have_sysfs_alarm)
-			device_remove_file(battery->bat.dev, &alarm_attr);
-		power_supply_unregister(&battery->bat);
+			device_remove_file(&battery->bat->dev, &alarm_attr);
+		power_supply_unregister(battery->bat);
 	}
 }
 
 static int acpi_charger_add(struct acpi_sbs *sbs)
 {
 	int result;
+	struct power_supply_config psy_cfg = { .drv_data = sbs, };
 
 	result = acpi_ac_get_present(sbs);
 	if (result)
 		goto end;
 
 	sbs->charger_exists = 1;
-	sbs->charger.name = "sbs-charger";
-	sbs->charger.type = POWER_SUPPLY_TYPE_MAINS;
-	sbs->charger.properties = sbs_ac_props;
-	sbs->charger.num_properties = ARRAY_SIZE(sbs_ac_props);
-	sbs->charger.get_property = sbs_get_ac_property;
-	power_supply_register(&sbs->device->dev, &sbs->charger);
+	sbs->charger = power_supply_register(&sbs->device->dev,
+					&acpi_sbs_charger_desc, &psy_cfg);
+	if (IS_ERR(sbs->charger)) {
+		result = PTR_ERR(sbs->charger);
+		sbs->charger = NULL;
+	}
 	printk(KERN_INFO PREFIX "%s [%s]: AC Adapter [%s] (%s)\n",
 	       ACPI_SBS_DEVICE_NAME, acpi_device_bid(sbs->device),
 	       ACPI_AC_DIR_NAME, sbs->charger_present ? "on-line" : "off-line");
@@ -590,8 +604,8 @@ static int acpi_charger_add(struct acpi_sbs *sbs)
 
 static void acpi_charger_remove(struct acpi_sbs *sbs)
 {
-	if (sbs->charger.dev)
-		power_supply_unregister(&sbs->charger);
+	if (sbs->charger)
+		power_supply_unregister(sbs->charger);
 }
 
 static void acpi_sbs_callback(void *context)
@@ -605,7 +619,7 @@ static void acpi_sbs_callback(void *context)
 	if (sbs->charger_exists) {
 		acpi_ac_get_present(sbs);
 		if (sbs->charger_present != saved_charger_state)
-			kobject_uevent(&sbs->charger.dev->kobj, KOBJ_CHANGE);
+			kobject_uevent(&sbs->charger->dev.kobj, KOBJ_CHANGE);
 	}
 
 	if (sbs->manager_present) {
@@ -617,7 +631,7 @@ static void acpi_sbs_callback(void *context)
 			acpi_battery_read(bat);
 			if (saved_battery_state == bat->present)
 				continue;
-			kobject_uevent(&bat->bat.dev->kobj, KOBJ_CHANGE);
+			kobject_uevent(&bat->bat->dev.kobj, KOBJ_CHANGE);
 		}
 	}
 }
diff --git a/drivers/acpi/video.c b/drivers/acpi/video.c
index debd30917010..26eb70c8f518 100644
--- a/drivers/acpi/video.c
+++ b/drivers/acpi/video.c
@@ -2110,7 +2110,8 @@ static int __init intel_opregion_present(void)
 
 int acpi_video_register(void)
 {
-	int result = 0;
+	int ret;
+
 	if (register_count) {
 		/*
 		 * if the function of acpi_video_register is already called,
@@ -2122,9 +2123,9 @@ int acpi_video_register(void)
 	mutex_init(&video_list_lock);
 	INIT_LIST_HEAD(&video_bus_head);
 
-	result = acpi_bus_register_driver(&acpi_video_bus);
-	if (result < 0)
-		return -ENODEV;
+	ret = acpi_bus_register_driver(&acpi_video_bus);
+	if (ret)
+		return ret;
 
 	/*
 	 * When the acpi_video_bus is loaded successfully, increase
@@ -2176,6 +2177,17 @@ EXPORT_SYMBOL(acpi_video_unregister_backlight);
 
 static int __init acpi_video_init(void)
 {
+	/*
+	 * Let the module load even if ACPI is disabled (e.g. due to
+	 * a broken BIOS) so that i915.ko can still be loaded on such
+	 * old systems without an AcpiOpRegion.
+	 *
+	 * acpi_video_register() will report -ENODEV later as well due
+	 * to acpi_disabled when i915.ko tries to register itself afterwards.
+	 */
+	if (acpi_disabled)
+		return 0;
+
 	dmi_check_system(video_dmi_table);
 
 	if (intel_opregion_present())
diff --git a/drivers/android/binder.c b/drivers/android/binder.c
index 33b09b6568a4..6607f3c6ace1 100644
--- a/drivers/android/binder.c
+++ b/drivers/android/binder.c
@@ -551,7 +551,6 @@ static int binder_update_page_range(struct binder_proc *proc, int allocate,
 {
 	void *page_addr;
 	unsigned long user_page_addr;
-	struct vm_struct tmp_area;
 	struct page **page;
 	struct mm_struct *mm;
 
@@ -600,10 +599,11 @@ static int binder_update_page_range(struct binder_proc *proc, int allocate,
 				proc->pid, page_addr);
 			goto err_alloc_page_failed;
 		}
-		tmp_area.addr = page_addr;
-		tmp_area.size = PAGE_SIZE + PAGE_SIZE /* guard page? */;
-		ret = map_vm_area(&tmp_area, PAGE_KERNEL, page);
-		if (ret) {
+		ret = map_kernel_range_noflush((unsigned long)page_addr,
+					PAGE_SIZE, PAGE_KERNEL, page);
+		flush_cache_vmap((unsigned long)page_addr,
+				(unsigned long)page_addr + PAGE_SIZE);
+		if (ret != 1) {
 			pr_err("%d: binder_alloc_buf failed to map page at %p in kernel\n",
 			       proc->pid, page_addr);
 			goto err_map_kernel_failed;
diff --git a/drivers/ata/Makefile b/drivers/ata/Makefile
index ae41107afc1f..b67e995179a9 100644
--- a/drivers/ata/Makefile
+++ b/drivers/ata/Makefile
@@ -111,7 +111,8 @@ obj-$(CONFIG_ATA_GENERIC)	+= ata_generic.o
 # Should be last libata driver
 obj-$(CONFIG_PATA_LEGACY)	+= pata_legacy.o
 
-libata-y	:= libata-core.o libata-scsi.o libata-eh.o libata-transport.o
+libata-y	:= libata-core.o libata-scsi.o libata-eh.o \
+	libata-transport.o libata-trace.o
 libata-$(CONFIG_ATA_SFF)	+= libata-sff.o
 libata-$(CONFIG_SATA_PMP)	+= libata-pmp.o
 libata-$(CONFIG_ATA_ACPI)	+= libata-acpi.o
diff --git a/drivers/ata/acard-ahci.c b/drivers/ata/acard-ahci.c
index c962886d7e71..12489ce863c4 100644
--- a/drivers/ata/acard-ahci.c
+++ b/drivers/ata/acard-ahci.c
@@ -181,10 +181,10 @@ static int acard_ahci_configure_dma_masks(struct pci_dev *pdev, int using_dac)
 	int rc;
 
 	if (using_dac &&
-	    !pci_set_dma_mask(pdev, DMA_BIT_MASK(64))) {
-		rc = pci_set_consistent_dma_mask(pdev, DMA_BIT_MASK(64));
+	    !dma_set_mask(&pdev->dev, DMA_BIT_MASK(64))) {
+		rc = dma_set_coherent_mask(&pdev->dev, DMA_BIT_MASK(64));
 		if (rc) {
-			rc = pci_set_consistent_dma_mask(pdev, DMA_BIT_MASK(32));
+			rc = dma_set_coherent_mask(&pdev->dev, DMA_BIT_MASK(32));
 			if (rc) {
 				dev_err(&pdev->dev,
 					   "64-bit DMA enable failed\n");
@@ -192,12 +192,12 @@ static int acard_ahci_configure_dma_masks(struct pci_dev *pdev, int using_dac)
 			}
 		}
 	} else {
-		rc = pci_set_dma_mask(pdev, DMA_BIT_MASK(32));
+		rc = dma_set_mask(&pdev->dev, DMA_BIT_MASK(32));
 		if (rc) {
 			dev_err(&pdev->dev, "32-bit DMA enable failed\n");
 			return rc;
 		}
-		rc = pci_set_consistent_dma_mask(pdev, DMA_BIT_MASK(32));
+		rc = dma_set_coherent_mask(&pdev->dev, DMA_BIT_MASK(32));
 		if (rc) {
 			dev_err(&pdev->dev,
 				"32-bit consistent DMA enable failed\n");
diff --git a/drivers/ata/ahci.c b/drivers/ata/ahci.c
index 33bb06e006c9..c7a92a743ed0 100644
--- a/drivers/ata/ahci.c
+++ b/drivers/ata/ahci.c
@@ -738,10 +738,10 @@ static int ahci_configure_dma_masks(struct pci_dev *pdev, int using_dac)
 		return 0;
 
 	if (using_dac &&
-	    !pci_set_dma_mask(pdev, DMA_BIT_MASK(64))) {
-		rc = pci_set_consistent_dma_mask(pdev, DMA_BIT_MASK(64));
+	    !dma_set_mask(&pdev->dev, DMA_BIT_MASK(64))) {
+		rc = dma_set_coherent_mask(&pdev->dev, DMA_BIT_MASK(64));
 		if (rc) {
-			rc = pci_set_consistent_dma_mask(pdev, DMA_BIT_MASK(32));
+			rc = dma_set_coherent_mask(&pdev->dev, DMA_BIT_MASK(32));
 			if (rc) {
 				dev_err(&pdev->dev,
 					"64-bit DMA enable failed\n");
@@ -749,12 +749,12 @@ static int ahci_configure_dma_masks(struct pci_dev *pdev, int using_dac)
 			}
 		}
 	} else {
-		rc = pci_set_dma_mask(pdev, DMA_BIT_MASK(32));
+		rc = dma_set_mask(&pdev->dev, DMA_BIT_MASK(32));
 		if (rc) {
 			dev_err(&pdev->dev, "32-bit DMA enable failed\n");
 			return rc;
 		}
-		rc = pci_set_consistent_dma_mask(pdev, DMA_BIT_MASK(32));
+		rc = dma_set_coherent_mask(&pdev->dev, DMA_BIT_MASK(32));
 		if (rc) {
 			dev_err(&pdev->dev,
 				"32-bit consistent DMA enable failed\n");
diff --git a/drivers/ata/ahci_st.c b/drivers/ata/ahci_st.c
index bc971af262e7..ea0ff005b86c 100644
--- a/drivers/ata/ahci_st.c
+++ b/drivers/ata/ahci_st.c
@@ -68,8 +68,6 @@ static int st_ahci_deassert_resets(struct device *dev)
 		}
 	}
 
-	st_ahci_configure_oob(drv_data->hpriv->mmio);
-
 	if (drv_data->sw_rst) {
 		err = reset_control_deassert(drv_data->sw_rst);
 		if (err) {
@@ -172,6 +170,8 @@ static int st_ahci_probe(struct platform_device *pdev)
 	if (err)
 		return err;
 
+	st_ahci_configure_oob(drv_data->hpriv->mmio);
+
 	err = ahci_platform_init_host(pdev, hpriv, &st_ahci_port_info,
 				      &ahci_platform_sht);
 	if (err) {
@@ -222,6 +222,8 @@ static int st_ahci_resume(struct device *dev)
 		return err;
 	}
 
+	st_ahci_configure_oob(drv_data->hpriv->mmio);
+
 	return ahci_platform_resume_host(dev);
 }
 #endif
diff --git a/drivers/ata/ahci_xgene.c b/drivers/ata/ahci_xgene.c
index 2e8bb603e447..2b78510d94dd 100644
--- a/drivers/ata/ahci_xgene.c
+++ b/drivers/ata/ahci_xgene.c
@@ -22,6 +22,7 @@
  * NOTE: PM support is not currently available.
  *
  */
+#include <linux/acpi.h>
 #include <linux/module.h>
 #include <linux/platform_device.h>
 #include <linux/ahci_platform.h>
@@ -718,6 +719,14 @@ disable_resources:
 	return rc;
 }
 
+#ifdef CONFIG_ACPI
+static const struct acpi_device_id xgene_ahci_acpi_match[] = {
+	{ "APMC0D0D", },
+	{ }
+};
+MODULE_DEVICE_TABLE(acpi, xgene_ahci_acpi_match);
+#endif
+
 static const struct of_device_id xgene_ahci_of_match[] = {
 	{.compatible = "apm,xgene-ahci"},
 	{},
@@ -730,6 +739,7 @@ static struct platform_driver xgene_ahci_driver = {
 	.driver = {
 		.name = DRV_NAME,
 		.of_match_table = xgene_ahci_of_match,
+		.acpi_match_table = ACPI_PTR(xgene_ahci_acpi_match),
 	},
 };
 
diff --git a/drivers/ata/libata-core.c b/drivers/ata/libata-core.c
index 4c35f0822d06..f6cb1f1b30b7 100644
--- a/drivers/ata/libata-core.c
+++ b/drivers/ata/libata-core.c
@@ -70,6 +70,9 @@
 #include <linux/pm_runtime.h>
 #include <linux/platform_device.h>
 
+#define CREATE_TRACE_POINTS
+#include <trace/events/libata.h>
+
 #include "libata.h"
 #include "libata-transport.h"
 
@@ -691,11 +694,11 @@ static int ata_rwcmd_protocol(struct ata_taskfile *tf, struct ata_device *dev)
  *	RETURNS:
  *	Block address read from @tf.
  */
-u64 ata_tf_read_block(struct ata_taskfile *tf, struct ata_device *dev)
+u64 ata_tf_read_block(const struct ata_taskfile *tf, struct ata_device *dev)
 {
 	u64 block = 0;
 
-	if (tf->flags & ATA_TFLAG_LBA) {
+	if (!dev || tf->flags & ATA_TFLAG_LBA) {
 		if (tf->flags & ATA_TFLAG_LBA48) {
 			block |= (u64)tf->hob_lbah << 40;
 			block |= (u64)tf->hob_lbam << 32;
@@ -2144,6 +2147,24 @@ static int ata_dev_config_ncq(struct ata_device *dev,
 	return 0;
 }
 
+static void ata_dev_config_sense_reporting(struct ata_device *dev)
+{
+	unsigned int err_mask;
+
+	if (!ata_id_has_sense_reporting(dev->id))
+		return;
+
+	if (ata_id_sense_reporting_enabled(dev->id))
+		return;
+
+	err_mask = ata_dev_set_feature(dev, SETFEATURE_SENSE_DATA, 0x1);
+	if (err_mask) {
+		ata_dev_dbg(dev,
+			    "failed to enable Sense Data Reporting, Emask 0x%x\n",
+			    err_mask);
+	}
+}
+
 /**
  *	ata_dev_configure - Configure the specified ATA/ATAPI device
  *	@dev: Target device to configure
@@ -2366,7 +2387,7 @@ int ata_dev_configure(struct ata_device *dev)
 					dev->devslp_timing[i] = sata_setting[j];
 				}
 		}
-
+		ata_dev_config_sense_reporting(dev);
 		dev->cdb_len = 16;
 	}
 
@@ -4204,9 +4225,18 @@ static const struct ata_blacklist_entry ata_device_blacklist [] = {
 	{ "PIONEER DVD-RW  DVR-216D",	NULL,	ATA_HORKAGE_NOSETXFER },
 
 	/* devices that don't properly handle queued TRIM commands */
-	{ "Micron_M[56]*",		NULL,	ATA_HORKAGE_NO_NCQ_TRIM |
+	{ "Micron_M500*",		NULL,	ATA_HORKAGE_NO_NCQ_TRIM |
+						ATA_HORKAGE_ZERO_AFTER_TRIM, },
+	{ "Crucial_CT*M500*",		NULL,	ATA_HORKAGE_NO_NCQ_TRIM |
+						ATA_HORKAGE_ZERO_AFTER_TRIM, },
+	{ "Micron_M5[15]0*",		"MU01",	ATA_HORKAGE_NO_NCQ_TRIM |
+						ATA_HORKAGE_ZERO_AFTER_TRIM, },
+	{ "Crucial_CT*M550*",		"MU01",	ATA_HORKAGE_NO_NCQ_TRIM |
+						ATA_HORKAGE_ZERO_AFTER_TRIM, },
+	{ "Crucial_CT*MX100*",		"MU01",	ATA_HORKAGE_NO_NCQ_TRIM |
+						ATA_HORKAGE_ZERO_AFTER_TRIM, },
+	{ "Samsung SSD 850 PRO*",	NULL,	ATA_HORKAGE_NO_NCQ_TRIM |
 						ATA_HORKAGE_ZERO_AFTER_TRIM, },
-	{ "Crucial_CT*SSD*",		NULL,	ATA_HORKAGE_NO_NCQ_TRIM, },
 
 	/*
 	 * As defined, the DRAT (Deterministic Read After Trim) and RZAT
@@ -4226,6 +4256,8 @@ static const struct ata_blacklist_entry ata_device_blacklist [] = {
 	 */
 	{ "INTEL*SSDSC2MH*",		NULL,	0, },
 
+	{ "Micron*",			NULL,	ATA_HORKAGE_ZERO_AFTER_TRIM, },
+	{ "Crucial*",			NULL,	ATA_HORKAGE_ZERO_AFTER_TRIM, },
 	{ "INTEL*SSD*", 		NULL,	ATA_HORKAGE_ZERO_AFTER_TRIM, },
 	{ "SSD*INTEL*",			NULL,	ATA_HORKAGE_ZERO_AFTER_TRIM, },
 	{ "Samsung*SSD*",		NULL,	ATA_HORKAGE_ZERO_AFTER_TRIM, },
@@ -4737,7 +4769,7 @@ struct ata_queued_cmd *ata_qc_new_init(struct ata_device *dev, int tag)
 		return NULL;
 
 	/* libsas case */
-	if (!ap->scsi_host) {
+	if (ap->flags & ATA_FLAG_SAS_HOST) {
 		tag = ata_sas_allocate_tag(ap);
 		if (tag < 0)
 			return NULL;
@@ -4776,7 +4808,7 @@ void ata_qc_free(struct ata_queued_cmd *qc)
 	tag = qc->tag;
 	if (likely(ata_tag_valid(tag))) {
 		qc->tag = ATA_TAG_POISON;
-		if (!ap->scsi_host)
+		if (ap->flags & ATA_FLAG_SAS_HOST)
 			ata_sas_free_tag(tag, ap);
 	}
 }
@@ -4886,6 +4918,7 @@ void ata_qc_complete(struct ata_queued_cmd *qc)
 		 */
 		if (unlikely(ata_tag_internal(qc->tag))) {
 			fill_result_tf(qc);
+			trace_ata_qc_complete_internal(qc);
 			__ata_qc_complete(qc);
 			return;
 		}
@@ -4896,6 +4929,7 @@ void ata_qc_complete(struct ata_queued_cmd *qc)
 		 */
 		if (unlikely(qc->flags & ATA_QCFLAG_FAILED)) {
 			fill_result_tf(qc);
+			trace_ata_qc_complete_failed(qc);
 			ata_qc_schedule_eh(qc);
 			return;
 		}
@@ -4906,6 +4940,7 @@ void ata_qc_complete(struct ata_queued_cmd *qc)
 		if (qc->flags & ATA_QCFLAG_RESULT_TF)
 			fill_result_tf(qc);
 
+		trace_ata_qc_complete_done(qc);
 		/* Some commands need post-processing after successful
 		 * completion.
 		 */
@@ -5053,7 +5088,7 @@ void ata_qc_issue(struct ata_queued_cmd *qc)
 	}
 
 	ap->ops->qc_prep(qc);
-
+	trace_ata_qc_issue(qc);
 	qc->err_mask |= ap->ops->qc_issue(qc);
 	if (unlikely(qc->err_mask))
 		goto err;
diff --git a/drivers/ata/libata-eh.c b/drivers/ata/libata-eh.c
index d2029a462e2c..07f41be38fbe 100644
--- a/drivers/ata/libata-eh.c
+++ b/drivers/ata/libata-eh.c
@@ -46,6 +46,7 @@
 
 #include <linux/libata.h>
 
+#include <trace/events/libata.h>
 #include "libata.h"
 
 enum {
@@ -1510,13 +1511,18 @@ unsigned int ata_read_log_page(struct ata_device *dev, u8 log,
 	DPRINTK("read log page - log 0x%x, page 0x%x\n", log, page);
 
 	ata_tf_init(dev, &tf);
-	tf.command = ATA_CMD_READ_LOG_EXT;
+	if (dev->dma_mode && ata_id_has_read_log_dma_ext(dev->id)) {
+		tf.command = ATA_CMD_READ_LOG_DMA_EXT;
+		tf.protocol = ATA_PROT_DMA;
+	} else {
+		tf.command = ATA_CMD_READ_LOG_EXT;
+		tf.protocol = ATA_PROT_PIO;
+	}
 	tf.lbal = log;
 	tf.lbam = page;
 	tf.nsect = sectors;
 	tf.hob_nsect = sectors >> 8;
 	tf.flags |= ATA_TFLAG_ISADDR | ATA_TFLAG_LBA48 | ATA_TFLAG_DEVICE;
-	tf.protocol = ATA_PROT_PIO;
 
 	err_mask = ata_exec_internal(dev, &tf, NULL, DMA_FROM_DEVICE,
 				     buf, sectors * ATA_SECT_SIZE, 0);
@@ -1575,6 +1581,8 @@ static int ata_eh_read_log_10h(struct ata_device *dev,
 	tf->hob_lbah = buf[10];
 	tf->nsect = buf[12];
 	tf->hob_nsect = buf[13];
+	if (ata_id_has_ncq_autosense(dev->id))
+		tf->auxiliary = buf[14] << 16 | buf[15] << 8 | buf[16];
 
 	return 0;
 }
@@ -1611,6 +1619,70 @@ unsigned int atapi_eh_tur(struct ata_device *dev, u8 *r_sense_key)
 }
 
 /**
+ *	ata_eh_request_sense - perform REQUEST_SENSE_DATA_EXT
+ *	@dev: device to perform REQUEST_SENSE_SENSE_DATA_EXT to
+ *	@sense_buf: result sense data buffer (SCSI_SENSE_BUFFERSIZE bytes long)
+ *	@dfl_sense_key: default sense key to use
+ *
+ *	Perform REQUEST_SENSE_DATA_EXT after the device reported CHECK
+ *	SENSE.  This function is EH helper.
+ *
+ *	LOCKING:
+ *	Kernel thread context (may sleep).
+ *
+ *	RETURNS:
+ *	encoded sense data on success, 0 on failure or if sense data
+ *	is not available.
+ */
+static u32 ata_eh_request_sense(struct ata_queued_cmd *qc,
+				struct scsi_cmnd *cmd)
+{
+	struct ata_device *dev = qc->dev;
+	struct ata_taskfile tf;
+	unsigned int err_mask;
+
+	if (!cmd)
+		return 0;
+
+	DPRINTK("ATA request sense\n");
+	ata_dev_warn(dev, "request sense\n");
+	if (!ata_id_sense_reporting_enabled(dev->id)) {
+		ata_dev_warn(qc->dev, "sense data reporting disabled\n");
+		return 0;
+	}
+	ata_tf_init(dev, &tf);
+
+	tf.flags |= ATA_TFLAG_ISADDR | ATA_TFLAG_DEVICE;
+	tf.flags |= ATA_TFLAG_LBA | ATA_TFLAG_LBA48;
+	tf.command = ATA_CMD_REQ_SENSE_DATA;
+	tf.protocol = ATA_PROT_NODATA;
+
+	err_mask = ata_exec_internal(dev, &tf, NULL, DMA_NONE, NULL, 0, 0);
+	/*
+	 * ACS-4 states:
+	 * The device may set the SENSE DATA AVAILABLE bit to one in the
+	 * STATUS field and clear the ERROR bit to zero in the STATUS field
+	 * to indicate that the command returned completion without an error
+	 * and the sense data described in table 306 is available.
+	 *
+	 * IOW the 'ATA_SENSE' bit might not be set even though valid
+	 * sense data is available.
+	 * So check for both.
+	 */
+	if ((tf.command & ATA_SENSE) ||
+		tf.lbah != 0 || tf.lbam != 0 || tf.lbal != 0) {
+		ata_scsi_set_sense(cmd, tf.lbah, tf.lbam, tf.lbal);
+		qc->flags |= ATA_QCFLAG_SENSE_VALID;
+		ata_dev_warn(dev, "sense data %02x/%02x/%02x\n",
+			     tf.lbah, tf.lbam, tf.lbal);
+	} else {
+		ata_dev_warn(dev, "request sense failed stat %02x emask %x\n",
+			     tf.command, err_mask);
+	}
+	return err_mask;
+}
+
+/**
  *	atapi_eh_request_sense - perform ATAPI REQUEST_SENSE
  *	@dev: device to perform REQUEST_SENSE to
  *	@sense_buf: result sense data buffer (SCSI_SENSE_BUFFERSIZE bytes long)
@@ -1772,6 +1844,19 @@ void ata_eh_analyze_ncq_error(struct ata_link *link)
 	memcpy(&qc->result_tf, &tf, sizeof(tf));
 	qc->result_tf.flags = ATA_TFLAG_ISADDR | ATA_TFLAG_LBA | ATA_TFLAG_LBA48;
 	qc->err_mask |= AC_ERR_DEV | AC_ERR_NCQ;
+	if (qc->result_tf.auxiliary) {
+		char sense_key, asc, ascq;
+
+		sense_key = (qc->result_tf.auxiliary >> 16) & 0xff;
+		asc = (qc->result_tf.auxiliary >> 8) & 0xff;
+		ascq = qc->result_tf.auxiliary & 0xff;
+		ata_dev_dbg(dev, "NCQ Autosense %02x/%02x/%02x\n",
+			    sense_key, asc, ascq);
+		ata_scsi_set_sense(qc->scsicmd, sense_key, asc, ascq);
+		ata_scsi_set_sense_information(qc->scsicmd, &qc->result_tf);
+		qc->flags |= ATA_QCFLAG_SENSE_VALID;
+	}
+
 	ehc->i.err_mask &= ~AC_ERR_DEV;
 }
 
@@ -1801,6 +1886,27 @@ static unsigned int ata_eh_analyze_tf(struct ata_queued_cmd *qc,
 		return ATA_EH_RESET;
 	}
 
+	/*
+	 * Sense data reporting does not work if the
+	 * device fault bit is set.
+	 */
+	if ((stat & ATA_SENSE) && !(stat & ATA_DF) &&
+	    !(qc->flags & ATA_QCFLAG_SENSE_VALID)) {
+		if (!(qc->ap->pflags & ATA_PFLAG_FROZEN)) {
+			tmp = ata_eh_request_sense(qc, qc->scsicmd);
+			if (tmp)
+				qc->err_mask |= tmp;
+			else
+				ata_scsi_set_sense_information(qc->scsicmd, tf);
+		} else {
+			ata_dev_warn(qc->dev, "sense data available but port frozen\n");
+		}
+	}
+
+	/* Set by NCQ autosense or request sense above */
+	if (qc->flags & ATA_QCFLAG_SENSE_VALID)
+		return 0;
+
 	if (stat & (ATA_ERR | ATA_DF))
 		qc->err_mask |= AC_ERR_DEV;
 	else
@@ -2186,6 +2292,7 @@ static void ata_eh_link_autopsy(struct ata_link *link)
 		all_err_mask |= qc->err_mask;
 		if (qc->flags & ATA_QCFLAG_IO)
 			eflags |= ATA_EFLAG_IS_IO;
+		trace_ata_eh_link_autopsy_qc(qc);
 	}
 
 	/* enforce default EH actions */
@@ -2220,7 +2327,7 @@ static void ata_eh_link_autopsy(struct ata_link *link)
 			eflags |= ATA_EFLAG_DUBIOUS_XFER;
 		ehc->i.action |= ata_eh_speed_down(dev, eflags, all_err_mask);
 	}
-
+	trace_ata_eh_link_autopsy(dev, ehc->i.action, all_err_mask);
 	DPRINTK("EXIT\n");
 }
 
@@ -2289,27 +2396,27 @@ const char *ata_get_cmd_descript(u8 command)
 		const char *text;
 	} cmd_descr[] = {
 		{ ATA_CMD_DEV_RESET,		"DEVICE RESET" },
-		{ ATA_CMD_CHK_POWER, 		"CHECK POWER MODE" },
-		{ ATA_CMD_STANDBY, 		"STANDBY" },
-		{ ATA_CMD_IDLE, 		"IDLE" },
-		{ ATA_CMD_EDD, 			"EXECUTE DEVICE DIAGNOSTIC" },
-		{ ATA_CMD_DOWNLOAD_MICRO,   	"DOWNLOAD MICROCODE" },
+		{ ATA_CMD_CHK_POWER,		"CHECK POWER MODE" },
+		{ ATA_CMD_STANDBY,		"STANDBY" },
+		{ ATA_CMD_IDLE,			"IDLE" },
+		{ ATA_CMD_EDD,			"EXECUTE DEVICE DIAGNOSTIC" },
+		{ ATA_CMD_DOWNLOAD_MICRO,	"DOWNLOAD MICROCODE" },
 		{ ATA_CMD_DOWNLOAD_MICRO_DMA,	"DOWNLOAD MICROCODE DMA" },
 		{ ATA_CMD_NOP,			"NOP" },
-		{ ATA_CMD_FLUSH, 		"FLUSH CACHE" },
-		{ ATA_CMD_FLUSH_EXT, 		"FLUSH CACHE EXT" },
-		{ ATA_CMD_ID_ATA,  		"IDENTIFY DEVICE" },
-		{ ATA_CMD_ID_ATAPI, 		"IDENTIFY PACKET DEVICE" },
-		{ ATA_CMD_SERVICE, 		"SERVICE" },
-		{ ATA_CMD_READ, 		"READ DMA" },
-		{ ATA_CMD_READ_EXT, 		"READ DMA EXT" },
-		{ ATA_CMD_READ_QUEUED, 		"READ DMA QUEUED" },
-		{ ATA_CMD_READ_STREAM_EXT, 	"READ STREAM EXT" },
+		{ ATA_CMD_FLUSH,		"FLUSH CACHE" },
+		{ ATA_CMD_FLUSH_EXT,		"FLUSH CACHE EXT" },
+		{ ATA_CMD_ID_ATA,		"IDENTIFY DEVICE" },
+		{ ATA_CMD_ID_ATAPI,		"IDENTIFY PACKET DEVICE" },
+		{ ATA_CMD_SERVICE,		"SERVICE" },
+		{ ATA_CMD_READ,			"READ DMA" },
+		{ ATA_CMD_READ_EXT,		"READ DMA EXT" },
+		{ ATA_CMD_READ_QUEUED,		"READ DMA QUEUED" },
+		{ ATA_CMD_READ_STREAM_EXT,	"READ STREAM EXT" },
 		{ ATA_CMD_READ_STREAM_DMA_EXT,  "READ STREAM DMA EXT" },
-		{ ATA_CMD_WRITE, 		"WRITE DMA" },
-		{ ATA_CMD_WRITE_EXT, 		"WRITE DMA EXT" },
-		{ ATA_CMD_WRITE_QUEUED, 	"WRITE DMA QUEUED EXT" },
-		{ ATA_CMD_WRITE_STREAM_EXT, 	"WRITE STREAM EXT" },
+		{ ATA_CMD_WRITE,		"WRITE DMA" },
+		{ ATA_CMD_WRITE_EXT,		"WRITE DMA EXT" },
+		{ ATA_CMD_WRITE_QUEUED,		"WRITE DMA QUEUED EXT" },
+		{ ATA_CMD_WRITE_STREAM_EXT,	"WRITE STREAM EXT" },
 		{ ATA_CMD_WRITE_STREAM_DMA_EXT, "WRITE STREAM DMA EXT" },
 		{ ATA_CMD_WRITE_FUA_EXT,	"WRITE DMA FUA EXT" },
 		{ ATA_CMD_WRITE_QUEUED_FUA_EXT, "WRITE DMA QUEUED FUA EXT" },
@@ -2325,7 +2432,7 @@ const char *ata_get_cmd_descript(u8 command)
 		{ ATA_CMD_READ_MULTI_EXT,	"READ MULTIPLE EXT" },
 		{ ATA_CMD_WRITE_MULTI,		"WRITE MULTIPLE" },
 		{ ATA_CMD_WRITE_MULTI_EXT,	"WRITE MULTIPLE EXT" },
-		{ ATA_CMD_WRITE_MULTI_FUA_EXT, 	"WRITE MULTIPLE FUA EXT" },
+		{ ATA_CMD_WRITE_MULTI_FUA_EXT,	"WRITE MULTIPLE FUA EXT" },
 		{ ATA_CMD_SET_FEATURES,		"SET FEATURES" },
 		{ ATA_CMD_SET_MULTI,		"SET MULTIPLE MODE" },
 		{ ATA_CMD_VERIFY,		"READ VERIFY SECTOR(S)" },
@@ -2342,12 +2449,12 @@ const char *ata_get_cmd_descript(u8 command)
 		{ ATA_CMD_READ_LOG_EXT,		"READ LOG EXT" },
 		{ ATA_CMD_WRITE_LOG_EXT,	"WRITE LOG EXT" },
 		{ ATA_CMD_READ_LOG_DMA_EXT,	"READ LOG DMA EXT" },
-		{ ATA_CMD_WRITE_LOG_DMA_EXT, 	"WRITE LOG DMA EXT" },
+		{ ATA_CMD_WRITE_LOG_DMA_EXT,	"WRITE LOG DMA EXT" },
 		{ ATA_CMD_TRUSTED_NONDATA,	"TRUSTED NON-DATA" },
 		{ ATA_CMD_TRUSTED_RCV,		"TRUSTED RECEIVE" },
-		{ ATA_CMD_TRUSTED_RCV_DMA, 	"TRUSTED RECEIVE DMA" },
+		{ ATA_CMD_TRUSTED_RCV_DMA,	"TRUSTED RECEIVE DMA" },
 		{ ATA_CMD_TRUSTED_SND,		"TRUSTED SEND" },
-		{ ATA_CMD_TRUSTED_SND_DMA, 	"TRUSTED SEND DMA" },
+		{ ATA_CMD_TRUSTED_SND_DMA,	"TRUSTED SEND DMA" },
 		{ ATA_CMD_PMP_READ,		"READ BUFFER" },
 		{ ATA_CMD_PMP_READ_DMA,		"READ BUFFER DMA" },
 		{ ATA_CMD_PMP_WRITE,		"WRITE BUFFER" },
@@ -2364,12 +2471,12 @@ const char *ata_get_cmd_descript(u8 command)
 		{ ATA_CMD_MEDIA_LOCK,		"DOOR LOCK" },
 		{ ATA_CMD_MEDIA_UNLOCK,		"DOOR UNLOCK" },
 		{ ATA_CMD_DSM,			"DATA SET MANAGEMENT" },
-		{ ATA_CMD_CHK_MED_CRD_TYP, 	"CHECK MEDIA CARD TYPE" },
-		{ ATA_CMD_CFA_REQ_EXT_ERR, 	"CFA REQUEST EXTENDED ERROR" },
+		{ ATA_CMD_CHK_MED_CRD_TYP,	"CHECK MEDIA CARD TYPE" },
+		{ ATA_CMD_CFA_REQ_EXT_ERR,	"CFA REQUEST EXTENDED ERROR" },
 		{ ATA_CMD_CFA_WRITE_NE,		"CFA WRITE SECTORS WITHOUT ERASE" },
 		{ ATA_CMD_CFA_TRANS_SECT,	"CFA TRANSLATE SECTOR" },
 		{ ATA_CMD_CFA_ERASE,		"CFA ERASE SECTORS" },
-		{ ATA_CMD_CFA_WRITE_MULT_NE, 	"CFA WRITE MULTIPLE WITHOUT ERASE" },
+		{ ATA_CMD_CFA_WRITE_MULT_NE,	"CFA WRITE MULTIPLE WITHOUT ERASE" },
 		{ ATA_CMD_REQ_SENSE_DATA,	"REQUEST SENSE DATA EXT" },
 		{ ATA_CMD_SANITIZE_DEVICE,	"SANITIZE DEVICE" },
 		{ ATA_CMD_READ_LONG,		"READ LONG (with retries)" },
@@ -2543,14 +2650,15 @@ static void ata_eh_link_report(struct ata_link *link)
 
 #ifdef CONFIG_ATA_VERBOSE_ERROR
 		if (res->command & (ATA_BUSY | ATA_DRDY | ATA_DF | ATA_DRQ |
-				    ATA_ERR)) {
+				    ATA_SENSE | ATA_ERR)) {
 			if (res->command & ATA_BUSY)
 				ata_dev_err(qc->dev, "status: { Busy }\n");
 			else
-				ata_dev_err(qc->dev, "status: { %s%s%s%s}\n",
+				ata_dev_err(qc->dev, "status: { %s%s%s%s%s}\n",
 				  res->command & ATA_DRDY ? "DRDY " : "",
 				  res->command & ATA_DF ? "DF " : "",
 				  res->command & ATA_DRQ ? "DRQ " : "",
+				  res->command & ATA_SENSE ? "SENSE " : "",
 				  res->command & ATA_ERR ? "ERR " : "");
 		}
 
diff --git a/drivers/ata/libata-scsi.c b/drivers/ata/libata-scsi.c
index b061ba2c31d8..3131adcc1f87 100644
--- a/drivers/ata/libata-scsi.c
+++ b/drivers/ata/libata-scsi.c
@@ -270,13 +270,28 @@ DEVICE_ATTR(unload_heads, S_IRUGO | S_IWUSR,
 	    ata_scsi_park_show, ata_scsi_park_store);
 EXPORT_SYMBOL_GPL(dev_attr_unload_heads);
 
-static void ata_scsi_set_sense(struct scsi_cmnd *cmd, u8 sk, u8 asc, u8 ascq)
+void ata_scsi_set_sense(struct scsi_cmnd *cmd, u8 sk, u8 asc, u8 ascq)
 {
+	if (!cmd)
+		return;
+
 	cmd->result = (DRIVER_SENSE << 24) | SAM_STAT_CHECK_CONDITION;
 
 	scsi_build_sense_buffer(0, cmd->sense_buffer, sk, asc, ascq);
 }
 
+void ata_scsi_set_sense_information(struct scsi_cmnd *cmd,
+				    const struct ata_taskfile *tf)
+{
+	u64 information;
+
+	if (!cmd)
+		return;
+
+	information = ata_tf_read_block(tf, NULL);
+	scsi_set_sense_information(cmd->sense_buffer, information);
+}
+
 static ssize_t
 ata_scsi_em_message_store(struct device *dev, struct device_attribute *attr,
 			  const char *buf, size_t count)
@@ -799,26 +814,27 @@ static void ata_dump_status(unsigned id, struct ata_taskfile *tf)
 	if (stat & ATA_BUSY) {
 		printk("Busy }\n");	/* Data is not valid in this case */
 	} else {
-		if (stat & 0x40)	printk("DriveReady ");
-		if (stat & 0x20)	printk("DeviceFault ");
-		if (stat & 0x10)	printk("SeekComplete ");
-		if (stat & 0x08)	printk("DataRequest ");
-		if (stat & 0x04)	printk("CorrectedError ");
-		if (stat & 0x02)	printk("Index ");
-		if (stat & 0x01)	printk("Error ");
+		if (stat & ATA_DRDY)	printk("DriveReady ");
+		if (stat & ATA_DF)	printk("DeviceFault ");
+		if (stat & ATA_DSC)	printk("SeekComplete ");
+		if (stat & ATA_DRQ)	printk("DataRequest ");
+		if (stat & ATA_CORR)	printk("CorrectedError ");
+		if (stat & ATA_SENSE)	printk("Sense ");
+		if (stat & ATA_ERR)	printk("Error ");
 		printk("}\n");
 
 		if (err) {
 			printk(KERN_WARNING "ata%u: error=0x%02x { ", id, err);
-			if (err & 0x04)		printk("DriveStatusError ");
-			if (err & 0x80) {
-				if (err & 0x04)	printk("BadCRC ");
+			if (err & ATA_ABORTED)	printk("DriveStatusError ");
+			if (err & ATA_ICRC) {
+				if (err & ATA_ABORTED)
+						printk("BadCRC ");
 				else		printk("Sector ");
 			}
-			if (err & 0x40)		printk("UncorrectableError ");
-			if (err & 0x10)		printk("SectorIdNotFound ");
-			if (err & 0x02)		printk("TrackZeroNotFound ");
-			if (err & 0x01)		printk("AddrMarkNotFound ");
+			if (err & ATA_UNC)	printk("UncorrectableError ");
+			if (err & ATA_IDNF)	printk("SectorIdNotFound ");
+			if (err & ATA_TRK0NF)	printk("TrackZeroNotFound ");
+			if (err & ATA_AMNF)	printk("AddrMarkNotFound ");
 			printk("}\n");
 		}
 	}
@@ -849,40 +865,59 @@ static void ata_to_sense_error(unsigned id, u8 drv_stat, u8 drv_err, u8 *sk,
 	/* Based on the 3ware driver translation table */
 	static const unsigned char sense_table[][4] = {
 		/* BBD|ECC|ID|MAR */
-		{0xd1, 		ABORTED_COMMAND, 0x00, 0x00}, 	// Device busy                  Aborted command
+		{0xd1,		ABORTED_COMMAND, 0x00, 0x00},
+			// Device busy                  Aborted command
 		/* BBD|ECC|ID */
-		{0xd0,  	ABORTED_COMMAND, 0x00, 0x00}, 	// Device busy                  Aborted command
+		{0xd0,		ABORTED_COMMAND, 0x00, 0x00},
+			// Device busy                  Aborted command
 		/* ECC|MC|MARK */
-		{0x61, 		HARDWARE_ERROR, 0x00, 0x00}, 	// Device fault                 Hardware error
+		{0x61,		HARDWARE_ERROR, 0x00, 0x00},
+			// Device fault                 Hardware error
 		/* ICRC|ABRT */		/* NB: ICRC & !ABRT is BBD */
-		{0x84, 		ABORTED_COMMAND, 0x47, 0x00}, 	// Data CRC error               SCSI parity error
+		{0x84,		ABORTED_COMMAND, 0x47, 0x00},
+			// Data CRC error               SCSI parity error
 		/* MC|ID|ABRT|TRK0|MARK */
-		{0x37, 		NOT_READY, 0x04, 0x00}, 	// Unit offline                 Not ready
+		{0x37,		NOT_READY, 0x04, 0x00},
+			// Unit offline                 Not ready
 		/* MCR|MARK */
-		{0x09, 		NOT_READY, 0x04, 0x00}, 	// Unrecovered disk error       Not ready
+		{0x09,		NOT_READY, 0x04, 0x00},
+			// Unrecovered disk error       Not ready
 		/*  Bad address mark */
-		{0x01, 		MEDIUM_ERROR, 0x13, 0x00}, 	// Address mark not found       Address mark not found for data field
-		/* TRK0 */
-		{0x02, 		HARDWARE_ERROR, 0x00, 0x00}, 	// Track 0 not found		Hardware error
+		{0x01,		MEDIUM_ERROR, 0x13, 0x00},
+			// Address mark not found for data field
+		/* TRK0 - Track 0 not found */
+		{0x02,		HARDWARE_ERROR, 0x00, 0x00},
+			// Hardware error
 		/* Abort: 0x04 is not translated here, see below */
 		/* Media change request */
-		{0x08, 		NOT_READY, 0x04, 0x00}, 	// Media change request	  FIXME: faking offline
-		/* SRV/IDNF */
-		{0x10, 		ILLEGAL_REQUEST, 0x21, 0x00}, 	// ID not found                 Logical address out of range
-		/* MC */
-		{0x20, 		UNIT_ATTENTION, 0x28, 0x00}, 	// Media Changed		Not ready to ready change, medium may have changed
-		/* ECC */
-		{0x40, 		MEDIUM_ERROR, 0x11, 0x04}, 	// Uncorrectable ECC error      Unrecovered read error
+		{0x08,		NOT_READY, 0x04, 0x00},
+			// FIXME: faking offline
+		/* SRV/IDNF - ID not found */
+		{0x10,		ILLEGAL_REQUEST, 0x21, 0x00},
+			// Logical address out of range
+		/* MC - Media Changed */
+		{0x20,		UNIT_ATTENTION, 0x28, 0x00},
+			// Not ready to ready change, medium may have changed
+		/* ECC - Uncorrectable ECC error */
+		{0x40,		MEDIUM_ERROR, 0x11, 0x04},
+			// Unrecovered read error
 		/* BBD - block marked bad */
-		{0x80, 		MEDIUM_ERROR, 0x11, 0x04}, 	// Block marked bad		Medium error, unrecovered read error
+		{0x80,		MEDIUM_ERROR, 0x11, 0x04},
+			// Block marked bad	Medium error, unrecovered read error
 		{0xFF, 0xFF, 0xFF, 0xFF}, // END mark
 	};
 	static const unsigned char stat_table[][4] = {
 		/* Must be first because BUSY means no other bits valid */
-		{0x80, 		ABORTED_COMMAND, 0x47, 0x00},	// Busy, fake parity for now
-		{0x20, 		HARDWARE_ERROR,  0x44, 0x00}, 	// Device fault, internal target failure
-		{0x08, 		ABORTED_COMMAND, 0x47, 0x00},	// Timed out in xfer, fake parity for now
-		{0x04, 		RECOVERED_ERROR, 0x11, 0x00},	// Recovered ECC error	  Medium error, recovered
+		{0x80,		ABORTED_COMMAND, 0x47, 0x00},
+		// Busy, fake parity for now
+		{0x40,		ILLEGAL_REQUEST, 0x21, 0x04},
+		// Device ready, unaligned write command
+		{0x20,		HARDWARE_ERROR,  0x44, 0x00},
+		// Device fault, internal target failure
+		{0x08,		ABORTED_COMMAND, 0x47, 0x00},
+		// Timed out in xfer, fake parity for now
+		{0x04,		RECOVERED_ERROR, 0x11, 0x00},
+		// Recovered ECC error	  Medium error, recovered
 		{0xFF, 0xFF, 0xFF, 0xFF}, // END mark
 	};
 
@@ -1757,7 +1792,9 @@ static void ata_scsi_qc_complete(struct ata_queued_cmd *qc)
 	    ((cdb[2] & 0x20) || need_sense)) {
 		ata_gen_passthru_sense(qc);
 	} else {
-		if (!need_sense) {
+		if (qc->flags & ATA_QCFLAG_SENSE_VALID) {
+			cmd->result = SAM_STAT_CHECK_CONDITION;
+		} else if (!need_sense) {
 			cmd->result = SAM_STAT_GOOD;
 		} else {
 			/* TODO: decide which descriptor format to use
@@ -4240,10 +4277,7 @@ int ata_sas_allocate_tag(struct ata_port *ap)
 	unsigned int i, tag;
 
 	for (i = 0, tag = ap->sas_last_tag + 1; i < max_queue; i++, tag++) {
-		if (ap->flags & ATA_FLAG_LOWTAG)
-			tag = 1;
-		else
-			tag = tag < max_queue ? tag : 0;
+		tag = tag < max_queue ? tag : 0;
 
 		/* the last tag is reserved for internal command. */
 		if (tag == ATA_TAG_INTERNAL)
diff --git a/drivers/ata/libata-sff.c b/drivers/ata/libata-sff.c
index 2e86e3b85266..cdf6215a9a22 100644
--- a/drivers/ata/libata-sff.c
+++ b/drivers/ata/libata-sff.c
@@ -3220,11 +3220,11 @@ void ata_pci_bmdma_init(struct ata_host *host)
 	 * ->sff_irq_clear method.  Try to initialize bmdma_addr
 	 * regardless of dma masks.
 	 */
-	rc = pci_set_dma_mask(pdev, ATA_DMA_MASK);
+	rc = dma_set_mask(&pdev->dev, ATA_DMA_MASK);
 	if (rc)
 		ata_bmdma_nodma(host, "failed to set dma mask");
 	if (!rc) {
-		rc = pci_set_consistent_dma_mask(pdev, ATA_DMA_MASK);
+		rc = dma_set_coherent_mask(&pdev->dev, ATA_DMA_MASK);
 		if (rc)
 			ata_bmdma_nodma(host,
 					"failed to set consistent dma mask");
diff --git a/drivers/ata/libata-trace.c b/drivers/ata/libata-trace.c
new file mode 100644
index 000000000000..fd30b8c10cf5
--- /dev/null
+++ b/drivers/ata/libata-trace.c
@@ -0,0 +1,151 @@
+/*
+ * libata-trace.c - trace functions for libata
+ *
+ * Copyright 2015 Hannes Reinecke
+ * Copyright 2015 SUSE Linux GmbH
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2, or (at your option)
+ *  any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; see the file COPYING.  If not, write to
+ *  the Free Software Foundation, 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#include <linux/kernel.h>
+#include <linux/trace_seq.h>
+#include <trace/events/libata.h>
+
+const char *
+libata_trace_parse_status(struct trace_seq *p, unsigned char status)
+{
+	const char *ret = trace_seq_buffer_ptr(p);
+
+	trace_seq_printf(p, "{ ");
+	if (status & ATA_BUSY)
+		trace_seq_printf(p, "BUSY ");
+	if (status & ATA_DRDY)
+		trace_seq_printf(p, "DRDY ");
+	if (status & ATA_DF)
+		trace_seq_printf(p, "DF ");
+	if (status & ATA_DSC)
+		trace_seq_printf(p, "DSC ");
+	if (status & ATA_DRQ)
+		trace_seq_printf(p, "DRQ ");
+	if (status & ATA_CORR)
+		trace_seq_printf(p, "CORR ");
+	if (status & ATA_SENSE)
+		trace_seq_printf(p, "SENSE ");
+	if (status & ATA_ERR)
+		trace_seq_printf(p, "ERR ");
+	trace_seq_putc(p, '}');
+	trace_seq_putc(p, 0);
+
+	return ret;
+}
+
+const char *
+libata_trace_parse_eh_action(struct trace_seq *p, unsigned int eh_action)
+{
+	const char *ret = trace_seq_buffer_ptr(p);
+
+	trace_seq_printf(p, "%x", eh_action);
+	if (eh_action) {
+		trace_seq_printf(p, "{ ");
+		if (eh_action & ATA_EH_REVALIDATE)
+			trace_seq_printf(p, "REVALIDATE ");
+		if (eh_action & (ATA_EH_SOFTRESET | ATA_EH_HARDRESET))
+			trace_seq_printf(p, "RESET ");
+		else if (eh_action & ATA_EH_SOFTRESET)
+			trace_seq_printf(p, "SOFTRESET ");
+		else if (eh_action & ATA_EH_HARDRESET)
+			trace_seq_printf(p, "HARDRESET ");
+		if (eh_action & ATA_EH_ENABLE_LINK)
+			trace_seq_printf(p, "ENABLE_LINK ");
+		if (eh_action & ATA_EH_PARK)
+			trace_seq_printf(p, "PARK ");
+		trace_seq_putc(p, '}');
+	}
+	trace_seq_putc(p, 0);
+
+	return ret;
+}
+
+const char *
+libata_trace_parse_eh_err_mask(struct trace_seq *p, unsigned int eh_err_mask)
+{
+	const char *ret = trace_seq_buffer_ptr(p);
+
+	trace_seq_printf(p, "%x", eh_err_mask);
+	if (eh_err_mask) {
+		trace_seq_printf(p, "{ ");
+		if (eh_err_mask & AC_ERR_DEV)
+			trace_seq_printf(p, "DEV ");
+		if (eh_err_mask & AC_ERR_HSM)
+			trace_seq_printf(p, "HSM ");
+		if (eh_err_mask & AC_ERR_TIMEOUT)
+			trace_seq_printf(p, "TIMEOUT ");
+		if (eh_err_mask & AC_ERR_MEDIA)
+			trace_seq_printf(p, "MEDIA ");
+		if (eh_err_mask & AC_ERR_ATA_BUS)
+			trace_seq_printf(p, "ATA_BUS ");
+		if (eh_err_mask & AC_ERR_HOST_BUS)
+			trace_seq_printf(p, "HOST_BUS ");
+		if (eh_err_mask & AC_ERR_SYSTEM)
+			trace_seq_printf(p, "SYSTEM ");
+		if (eh_err_mask & AC_ERR_INVALID)
+			trace_seq_printf(p, "INVALID ");
+		if (eh_err_mask & AC_ERR_OTHER)
+			trace_seq_printf(p, "OTHER ");
+		if (eh_err_mask & AC_ERR_NODEV_HINT)
+			trace_seq_printf(p, "NODEV_HINT ");
+		if (eh_err_mask & AC_ERR_NCQ)
+			trace_seq_printf(p, "NCQ ");
+		trace_seq_putc(p, '}');
+	}
+	trace_seq_putc(p, 0);
+
+	return ret;
+}
+
+const char *
+libata_trace_parse_qc_flags(struct trace_seq *p, unsigned int qc_flags)
+{
+	const char *ret = trace_seq_buffer_ptr(p);
+
+	trace_seq_printf(p, "%x", qc_flags);
+	if (qc_flags) {
+		trace_seq_printf(p, "{ ");
+		if (qc_flags & ATA_QCFLAG_ACTIVE)
+			trace_seq_printf(p, "ACTIVE ");
+		if (qc_flags & ATA_QCFLAG_DMAMAP)
+			trace_seq_printf(p, "DMAMAP ");
+		if (qc_flags & ATA_QCFLAG_IO)
+			trace_seq_printf(p, "IO ");
+		if (qc_flags & ATA_QCFLAG_RESULT_TF)
+			trace_seq_printf(p, "RESULT_TF ");
+		if (qc_flags & ATA_QCFLAG_CLEAR_EXCL)
+			trace_seq_printf(p, "CLEAR_EXCL ");
+		if (qc_flags & ATA_QCFLAG_QUIET)
+			trace_seq_printf(p, "QUIET ");
+		if (qc_flags & ATA_QCFLAG_RETRY)
+			trace_seq_printf(p, "RETRY ");
+		if (qc_flags & ATA_QCFLAG_FAILED)
+			trace_seq_printf(p, "FAILED ");
+		if (qc_flags & ATA_QCFLAG_SENSE_VALID)
+			trace_seq_printf(p, "SENSE_VALID ");
+		if (qc_flags & ATA_QCFLAG_EH_SCHEDULED)
+			trace_seq_printf(p, "EH_SCHEDULED ");
+		trace_seq_putc(p, '}');
+	}
+	trace_seq_putc(p, 0);
+
+	return ret;
+}
diff --git a/drivers/ata/libata.h b/drivers/ata/libata.h
index f840ca18a7c0..a998a175f9f1 100644
--- a/drivers/ata/libata.h
+++ b/drivers/ata/libata.h
@@ -67,7 +67,8 @@ extern struct ata_queued_cmd *ata_qc_new_init(struct ata_device *dev, int tag);
 extern int ata_build_rw_tf(struct ata_taskfile *tf, struct ata_device *dev,
 			   u64 block, u32 n_block, unsigned int tf_flags,
 			   unsigned int tag);
-extern u64 ata_tf_read_block(struct ata_taskfile *tf, struct ata_device *dev);
+extern u64 ata_tf_read_block(const struct ata_taskfile *tf,
+			     struct ata_device *dev);
 extern unsigned ata_exec_internal(struct ata_device *dev,
 				  struct ata_taskfile *tf, const u8 *cdb,
 				  int dma_dir, void *buf, unsigned int buflen,
@@ -137,6 +138,9 @@ extern int ata_scsi_add_hosts(struct ata_host *host,
 			      struct scsi_host_template *sht);
 extern void ata_scsi_scan_host(struct ata_port *ap, int sync);
 extern int ata_scsi_offline_dev(struct ata_device *dev);
+extern void ata_scsi_set_sense(struct scsi_cmnd *cmd, u8 sk, u8 asc, u8 ascq);
+extern void ata_scsi_set_sense_information(struct scsi_cmnd *cmd,
+					   const struct ata_taskfile *tf);
 extern void ata_scsi_media_change_notify(struct ata_device *dev);
 extern void ata_scsi_hotplug(struct work_struct *work);
 extern void ata_schedule_scsi_eh(struct Scsi_Host *shost);
diff --git a/drivers/ata/pata_atp867x.c b/drivers/ata/pata_atp867x.c
index a705cfca90f7..3ea50dc5ea47 100644
--- a/drivers/ata/pata_atp867x.c
+++ b/drivers/ata/pata_atp867x.c
@@ -475,11 +475,11 @@ static int atp867x_ata_pci_sff_init_host(struct ata_host *host)
 
 	atp867x_fixup(host);
 
-	rc = pci_set_dma_mask(pdev, ATA_DMA_MASK);
+	rc = dma_set_mask(&pdev->dev, ATA_DMA_MASK);
 	if (rc)
 		return rc;
 
-	rc = pci_set_consistent_dma_mask(pdev, ATA_DMA_MASK);
+	rc = dma_set_coherent_mask(&pdev->dev, ATA_DMA_MASK);
 	return rc;
 }
 
diff --git a/drivers/ata/pata_cs5520.c b/drivers/ata/pata_cs5520.c
index d65cb9d2fa8c..4cb24070cc2d 100644
--- a/drivers/ata/pata_cs5520.c
+++ b/drivers/ata/pata_cs5520.c
@@ -164,11 +164,11 @@ static int cs5520_init_one(struct pci_dev *pdev, const struct pci_device_id *id)
 		return -ENODEV;
 	}
 
-	if (pci_set_dma_mask(pdev, DMA_BIT_MASK(32))) {
+	if (dma_set_mask(&pdev->dev, DMA_BIT_MASK(32))) {
 		printk(KERN_ERR DRV_NAME ": unable to configure DMA mask.\n");
 		return -ENODEV;
 	}
-	if (pci_set_consistent_dma_mask(pdev, DMA_BIT_MASK(32))) {
+	if (dma_set_coherent_mask(&pdev->dev, DMA_BIT_MASK(32))) {
 		printk(KERN_ERR DRV_NAME ": unable to configure consistent DMA mask.\n");
 		return -ENODEV;
 	}
diff --git a/drivers/ata/pata_hpt3x3.c b/drivers/ata/pata_hpt3x3.c
index d019cdd5bc9f..b2fc023783b1 100644
--- a/drivers/ata/pata_hpt3x3.c
+++ b/drivers/ata/pata_hpt3x3.c
@@ -221,10 +221,10 @@ static int hpt3x3_init_one(struct pci_dev *pdev, const struct pci_device_id *id)
 	if (rc)
 		return rc;
 	host->iomap = pcim_iomap_table(pdev);
-	rc = pci_set_dma_mask(pdev, ATA_DMA_MASK);
+	rc = dma_set_mask(&pdev->dev, ATA_DMA_MASK);
 	if (rc)
 		return rc;
-	rc = pci_set_consistent_dma_mask(pdev, ATA_DMA_MASK);
+	rc = dma_set_coherent_mask(&pdev->dev, ATA_DMA_MASK);
 	if (rc)
 		return rc;
 
diff --git a/drivers/ata/pata_ninja32.c b/drivers/ata/pata_ninja32.c
index efb272da8567..633aa2934a18 100644
--- a/drivers/ata/pata_ninja32.c
+++ b/drivers/ata/pata_ninja32.c
@@ -122,10 +122,10 @@ static int ninja32_init_one(struct pci_dev *dev, const struct pci_device_id *id)
 		return rc;
 
 	host->iomap = pcim_iomap_table(dev);
-	rc = pci_set_dma_mask(dev, ATA_DMA_MASK);
+	rc = dma_set_mask(&dev->dev, ATA_DMA_MASK);
 	if (rc)
 		return rc;
-	rc = pci_set_consistent_dma_mask(dev, ATA_DMA_MASK);
+	rc = dma_set_coherent_mask(&dev->dev, ATA_DMA_MASK);
 	if (rc)
 		return rc;
 	pci_set_master(dev);
diff --git a/drivers/ata/pata_pdc2027x.c b/drivers/ata/pata_pdc2027x.c
index dca8251b1aea..d9ef9e276225 100644
--- a/drivers/ata/pata_pdc2027x.c
+++ b/drivers/ata/pata_pdc2027x.c
@@ -730,11 +730,11 @@ static int pdc2027x_init_one(struct pci_dev *pdev,
 		return rc;
 	host->iomap = pcim_iomap_table(pdev);
 
-	rc = pci_set_dma_mask(pdev, ATA_DMA_MASK);
+	rc = dma_set_mask(&pdev->dev, ATA_DMA_MASK);
 	if (rc)
 		return rc;
 
-	rc = pci_set_consistent_dma_mask(pdev, ATA_DMA_MASK);
+	rc = dma_set_coherent_mask(&pdev->dev, ATA_DMA_MASK);
 	if (rc)
 		return rc;
 
diff --git a/drivers/ata/pata_scc.c b/drivers/ata/pata_scc.c
index 7f4cb76ed9fa..5cd60d6388ec 100644
--- a/drivers/ata/pata_scc.c
+++ b/drivers/ata/pata_scc.c
@@ -1029,10 +1029,10 @@ static int scc_host_init(struct ata_host *host)
 	if (rc)
 		return rc;
 
-	rc = pci_set_dma_mask(pdev, ATA_DMA_MASK);
+	rc = dma_set_mask(&pdev->dev, ATA_DMA_MASK);
 	if (rc)
 		return rc;
-	rc = pci_set_consistent_dma_mask(pdev, ATA_DMA_MASK);
+	rc = dma_set_coherent_mask(&pdev->dev, ATA_DMA_MASK);
 	if (rc)
 		return rc;
 
diff --git a/drivers/ata/pata_sil680.c b/drivers/ata/pata_sil680.c
index f597edccedec..c14071be4f55 100644
--- a/drivers/ata/pata_sil680.c
+++ b/drivers/ata/pata_sil680.c
@@ -374,10 +374,10 @@ static int sil680_init_one(struct pci_dev *pdev, const struct pci_device_id *id)
 	host->iomap = pcim_iomap_table(pdev);
 
 	/* Setup DMA masks */
-	rc = pci_set_dma_mask(pdev, ATA_DMA_MASK);
+	rc = dma_set_mask(&pdev->dev, ATA_DMA_MASK);
 	if (rc)
 		return rc;
-	rc = pci_set_consistent_dma_mask(pdev, ATA_DMA_MASK);
+	rc = dma_set_coherent_mask(&pdev->dev, ATA_DMA_MASK);
 	if (rc)
 		return rc;
 	pci_set_master(pdev);
diff --git a/drivers/ata/pdc_adma.c b/drivers/ata/pdc_adma.c
index f10631beffa8..64d682c6ee57 100644
--- a/drivers/ata/pdc_adma.c
+++ b/drivers/ata/pdc_adma.c
@@ -593,12 +593,12 @@ static int adma_set_dma_masks(struct pci_dev *pdev, void __iomem *mmio_base)
 {
 	int rc;
 
-	rc = pci_set_dma_mask(pdev, DMA_BIT_MASK(32));
+	rc = dma_set_mask(&pdev->dev, DMA_BIT_MASK(32));
 	if (rc) {
 		dev_err(&pdev->dev, "32-bit DMA enable failed\n");
 		return rc;
 	}
-	rc = pci_set_consistent_dma_mask(pdev, DMA_BIT_MASK(32));
+	rc = dma_set_coherent_mask(&pdev->dev, DMA_BIT_MASK(32));
 	if (rc) {
 		dev_err(&pdev->dev, "32-bit consistent DMA enable failed\n");
 		return rc;
diff --git a/drivers/ata/sata_dwc_460ex.c b/drivers/ata/sata_dwc_460ex.c
index fdb0f2879ea7..902034991517 100644
--- a/drivers/ata/sata_dwc_460ex.c
+++ b/drivers/ata/sata_dwc_460ex.c
@@ -36,11 +36,16 @@
 #include <linux/platform_device.h>
 #include <linux/libata.h>
 #include <linux/slab.h>
+
 #include "libata.h"
 
 #include <scsi/scsi_host.h>
 #include <scsi/scsi_cmnd.h>
 
+/* Supported DMA engine drivers */
+#include <linux/platform_data/dma-dw.h>
+#include <linux/dma/dw.h>
+
 /* These two are defined in "libata.h" */
 #undef	DRV_NAME
 #undef	DRV_VERSION
@@ -60,153 +65,9 @@
 #define NO_IRQ		0
 #endif
 
-/* SATA DMA driver Globals */
-#define DMA_NUM_CHANS		1
-#define DMA_NUM_CHAN_REGS	8
-
-/* SATA DMA Register definitions */
 #define AHB_DMA_BRST_DFLT	64	/* 16 data items burst length*/
 
-struct dmareg {
-	u32 low;		/* Low bits 0-31 */
-	u32 high;		/* High bits 32-63 */
-};
-
-/* DMA Per Channel registers */
-struct dma_chan_regs {
-	struct dmareg sar;	/* Source Address */
-	struct dmareg dar;	/* Destination address */
-	struct dmareg llp;	/* Linked List Pointer */
-	struct dmareg ctl;	/* Control */
-	struct dmareg sstat;	/* Source Status not implemented in core */
-	struct dmareg dstat;	/* Destination Status not implemented in core*/
-	struct dmareg sstatar;	/* Source Status Address not impl in core */
-	struct dmareg dstatar;	/* Destination Status Address not implemente */
-	struct dmareg cfg;	/* Config */
-	struct dmareg sgr;	/* Source Gather */
-	struct dmareg dsr;	/* Destination Scatter */
-};
-
-/* Generic Interrupt Registers */
-struct dma_interrupt_regs {
-	struct dmareg tfr;	/* Transfer Interrupt */
-	struct dmareg block;	/* Block Interrupt */
-	struct dmareg srctran;	/* Source Transfer Interrupt */
-	struct dmareg dsttran;	/* Dest Transfer Interrupt */
-	struct dmareg error;	/* Error */
-};
-
-struct ahb_dma_regs {
-	struct dma_chan_regs	chan_regs[DMA_NUM_CHAN_REGS];
-	struct dma_interrupt_regs interrupt_raw;	/* Raw Interrupt */
-	struct dma_interrupt_regs interrupt_status;	/* Interrupt Status */
-	struct dma_interrupt_regs interrupt_mask;	/* Interrupt Mask */
-	struct dma_interrupt_regs interrupt_clear;	/* Interrupt Clear */
-	struct dmareg		statusInt;	/* Interrupt combined*/
-	struct dmareg		rq_srcreg;	/* Src Trans Req */
-	struct dmareg		rq_dstreg;	/* Dst Trans Req */
-	struct dmareg		rq_sgl_srcreg;	/* Sngl Src Trans Req*/
-	struct dmareg		rq_sgl_dstreg;	/* Sngl Dst Trans Req*/
-	struct dmareg		rq_lst_srcreg;	/* Last Src Trans Req*/
-	struct dmareg		rq_lst_dstreg;	/* Last Dst Trans Req*/
-	struct dmareg		dma_cfg;		/* DMA Config */
-	struct dmareg		dma_chan_en;		/* DMA Channel Enable*/
-	struct dmareg		dma_id;			/* DMA ID */
-	struct dmareg		dma_test;		/* DMA Test */
-	struct dmareg		res1;			/* reserved */
-	struct dmareg		res2;			/* reserved */
-	/*
-	 * DMA Comp Params
-	 * Param 6 = dma_param[0], Param 5 = dma_param[1],
-	 * Param 4 = dma_param[2] ...
-	 */
-	struct dmareg		dma_params[6];
-};
-
-/* Data structure for linked list item */
-struct lli {
-	u32		sar;		/* Source Address */
-	u32		dar;		/* Destination address */
-	u32		llp;		/* Linked List Pointer */
-	struct dmareg	ctl;		/* Control */
-	struct dmareg	dstat;		/* Destination Status */
-};
-
-enum {
-	SATA_DWC_DMAC_LLI_SZ =	(sizeof(struct lli)),
-	SATA_DWC_DMAC_LLI_NUM =	256,
-	SATA_DWC_DMAC_LLI_TBL_SZ = (SATA_DWC_DMAC_LLI_SZ * \
-					SATA_DWC_DMAC_LLI_NUM),
-	SATA_DWC_DMAC_TWIDTH_BYTES = 4,
-	SATA_DWC_DMAC_CTRL_TSIZE_MAX = (0x00000800 * \
-						SATA_DWC_DMAC_TWIDTH_BYTES),
-};
-
-/* DMA Register Operation Bits */
-enum {
-	DMA_EN	=		0x00000001, /* Enable AHB DMA */
-	DMA_CTL_LLP_SRCEN =	0x10000000, /* Blk chain enable Src */
-	DMA_CTL_LLP_DSTEN =	0x08000000, /* Blk chain enable Dst */
-};
-
-#define	DMA_CTL_BLK_TS(size)	((size) & 0x000000FFF)	/* Blk Transfer size */
-#define DMA_CHANNEL(ch)		(0x00000001 << (ch))	/* Select channel */
-	/* Enable channel */
-#define	DMA_ENABLE_CHAN(ch)	((0x00000001 << (ch)) |			\
-				 ((0x000000001 << (ch)) << 8))
-	/* Disable channel */
-#define	DMA_DISABLE_CHAN(ch)	(0x00000000 | ((0x000000001 << (ch)) << 8))
-	/* Transfer Type & Flow Controller */
-#define	DMA_CTL_TTFC(type)	(((type) & 0x7) << 20)
-#define	DMA_CTL_SMS(num)	(((num) & 0x3) << 25) /* Src Master Select */
-#define	DMA_CTL_DMS(num)	(((num) & 0x3) << 23)/* Dst Master Select */
-	/* Src Burst Transaction Length */
-#define DMA_CTL_SRC_MSIZE(size) (((size) & 0x7) << 14)
-	/* Dst Burst Transaction Length */
-#define	DMA_CTL_DST_MSIZE(size) (((size) & 0x7) << 11)
-	/* Source Transfer Width */
-#define	DMA_CTL_SRC_TRWID(size) (((size) & 0x7) << 4)
-	/* Destination Transfer Width */
-#define	DMA_CTL_DST_TRWID(size) (((size) & 0x7) << 1)
-
-/* Assign HW handshaking interface (x) to destination / source peripheral */
-#define	DMA_CFG_HW_HS_DEST(int_num) (((int_num) & 0xF) << 11)
-#define	DMA_CFG_HW_HS_SRC(int_num) (((int_num) & 0xF) << 7)
-#define	DMA_CFG_HW_CH_PRIOR(int_num) (((int_num) & 0xF) << 5)
-#define	DMA_LLP_LMS(addr, master) (((addr) & 0xfffffffc) | (master))
-
-/*
- * This define is used to set block chaining disabled in the control low
- * register.  It is already in little endian format so it can be &'d dirctly.
- * It is essentially: cpu_to_le32(~(DMA_CTL_LLP_SRCEN | DMA_CTL_LLP_DSTEN))
- */
 enum {
-	DMA_CTL_LLP_DISABLE_LE32 = 0xffffffe7,
-	DMA_CTL_TTFC_P2M_DMAC =	0x00000002, /* Per to mem, DMAC cntr */
-	DMA_CTL_TTFC_M2P_PER =	0x00000003, /* Mem to per, peripheral cntr */
-	DMA_CTL_SINC_INC =	0x00000000, /* Source Address Increment */
-	DMA_CTL_SINC_DEC =	0x00000200,
-	DMA_CTL_SINC_NOCHANGE =	0x00000400,
-	DMA_CTL_DINC_INC =	0x00000000, /* Destination Address Increment */
-	DMA_CTL_DINC_DEC =	0x00000080,
-	DMA_CTL_DINC_NOCHANGE =	0x00000100,
-	DMA_CTL_INT_EN =	0x00000001, /* Interrupt Enable */
-
-/* Channel Configuration Register high bits */
-	DMA_CFG_FCMOD_REQ =	0x00000001, /* Flow Control - request based */
-	DMA_CFG_PROTCTL	=	(0x00000003 << 2),/* Protection Control */
-
-/* Channel Configuration Register low bits */
-	DMA_CFG_RELD_DST =	0x80000000, /* Reload Dest / Src Addr */
-	DMA_CFG_RELD_SRC =	0x40000000,
-	DMA_CFG_HS_SELSRC =	0x00000800, /* Software handshake Src/ Dest */
-	DMA_CFG_HS_SELDST =	0x00000400,
-	DMA_CFG_FIFOEMPTY =     (0x00000001 << 9), /* FIFO Empty bit */
-
-/* Channel Linked List Pointer Register */
-	DMA_LLP_AHBMASTER1 =	0,	/* List Master Select */
-	DMA_LLP_AHBMASTER2 =	1,
-
 	SATA_DWC_MAX_PORTS = 1,
 
 	SATA_DWC_SCR_OFFSET = 0x24,
@@ -287,7 +148,7 @@ struct sata_dwc_device {
 	struct ata_host		*host;
 	u8 __iomem		*reg_base;
 	struct sata_dwc_regs	*sata_dwc_regs;	/* DW Synopsys SATA specific */
-	int			irq_dma;
+	struct dw_dma_chip	*dma;
 };
 
 #define SATA_DWC_QCMD_MAX	32
@@ -295,10 +156,13 @@ struct sata_dwc_device {
 struct sata_dwc_device_port {
 	struct sata_dwc_device	*hsdev;
 	int			cmd_issued[SATA_DWC_QCMD_MAX];
-	struct lli		*llit[SATA_DWC_QCMD_MAX];  /* DMA LLI table */
-	dma_addr_t		llit_dma[SATA_DWC_QCMD_MAX];
-	u32			dma_chan[SATA_DWC_QCMD_MAX];
 	int			dma_pending[SATA_DWC_QCMD_MAX];
+
+	/* DMA info */
+	struct dw_dma_slave		*dws;
+	struct dma_chan			*chan;
+	struct dma_async_tx_descriptor	*desc[SATA_DWC_QCMD_MAX];
+	u32				dma_interrupt_count;
 };
 
 /*
@@ -330,14 +194,17 @@ struct sata_dwc_host_priv {
 	void	__iomem	 *scr_addr_sstatus;
 	u32	sata_dwc_sactive_issued ;
 	u32	sata_dwc_sactive_queued ;
-	u32	dma_interrupt_count;
-	struct	ahb_dma_regs	*sata_dma_regs;
-	struct	device	*dwc_dev;
-	int	dma_channel;
 };
 
 static struct sata_dwc_host_priv host_pvt;
 
+static struct dw_dma_slave sata_dwc_dma_dws = {
+	.src_id = 0,
+	.dst_id = 0,
+	.src_master = 0,
+	.dst_master = 1,
+};
+
 /*
  * Prototypes
  */
@@ -347,12 +214,6 @@ static int sata_dwc_qc_complete(struct ata_port *ap, struct ata_queued_cmd *qc,
 static void sata_dwc_dma_xfer_complete(struct ata_port *ap, u32 check_status);
 static void sata_dwc_port_stop(struct ata_port *ap);
 static void sata_dwc_clear_dmacr(struct sata_dwc_device_port *hsdevp, u8 tag);
-static int dma_dwc_init(struct sata_dwc_device *hsdev, int irq);
-static void dma_dwc_exit(struct sata_dwc_device *hsdev);
-static int dma_dwc_xfer_setup(struct scatterlist *sg, int num_elems,
-			      struct lli *lli, dma_addr_t dma_lli,
-			      void __iomem *addr, int dir);
-static void dma_dwc_xfer_start(int dma_ch);
 
 static const char *get_prot_descript(u8 protocol)
 {
@@ -390,90 +251,23 @@ static const char *get_dma_dir_descript(int dma_dir)
 	}
 }
 
-static void sata_dwc_tf_dump(struct ata_taskfile *tf)
+static void sata_dwc_tf_dump(struct ata_port *ap, struct ata_taskfile *tf)
 {
-	dev_vdbg(host_pvt.dwc_dev, "taskfile cmd: 0x%02x protocol: %s flags:"
-		"0x%lx device: %x\n", tf->command,
-		get_prot_descript(tf->protocol), tf->flags, tf->device);
-	dev_vdbg(host_pvt.dwc_dev, "feature: 0x%02x nsect: 0x%x lbal: 0x%x "
-		"lbam: 0x%x lbah: 0x%x\n", tf->feature, tf->nsect, tf->lbal,
-		 tf->lbam, tf->lbah);
-	dev_vdbg(host_pvt.dwc_dev, "hob_feature: 0x%02x hob_nsect: 0x%x "
-		"hob_lbal: 0x%x hob_lbam: 0x%x hob_lbah: 0x%x\n",
+	dev_vdbg(ap->dev,
+		"taskfile cmd: 0x%02x protocol: %s flags: 0x%lx device: %x\n",
+		tf->command, get_prot_descript(tf->protocol), tf->flags,
+		tf->device);
+	dev_vdbg(ap->dev,
+		"feature: 0x%02x nsect: 0x%x lbal: 0x%x lbam: 0x%x lbah: 0x%x\n",
+		tf->feature, tf->nsect, tf->lbal, tf->lbam, tf->lbah);
+	dev_vdbg(ap->dev,
+		"hob_feature: 0x%02x hob_nsect: 0x%x hob_lbal: 0x%x hob_lbam: 0x%x hob_lbah: 0x%x\n",
 		tf->hob_feature, tf->hob_nsect, tf->hob_lbal, tf->hob_lbam,
 		tf->hob_lbah);
 }
 
-/*
- * Function: get_burst_length_encode
- * arguments: datalength: length in bytes of data
- * returns value to be programmed in register corresponding to data length
- * This value is effectively the log(base 2) of the length
- */
-static  int get_burst_length_encode(int datalength)
-{
-	int items = datalength >> 2;	/* div by 4 to get lword count */
-
-	if (items >= 64)
-		return 5;
-
-	if (items >= 32)
-		return 4;
-
-	if (items >= 16)
-		return 3;
-
-	if (items >= 8)
-		return 2;
-
-	if (items >= 4)
-		return 1;
-
-	return 0;
-}
-
-static  void clear_chan_interrupts(int c)
+static void dma_dwc_xfer_done(void *hsdev_instance)
 {
-	out_le32(&(host_pvt.sata_dma_regs->interrupt_clear.tfr.low),
-		 DMA_CHANNEL(c));
-	out_le32(&(host_pvt.sata_dma_regs->interrupt_clear.block.low),
-		 DMA_CHANNEL(c));
-	out_le32(&(host_pvt.sata_dma_regs->interrupt_clear.srctran.low),
-		 DMA_CHANNEL(c));
-	out_le32(&(host_pvt.sata_dma_regs->interrupt_clear.dsttran.low),
-		 DMA_CHANNEL(c));
-	out_le32(&(host_pvt.sata_dma_regs->interrupt_clear.error.low),
-		 DMA_CHANNEL(c));
-}
-
-/*
- * Function: dma_request_channel
- * arguments: None
- * returns channel number if available else -1
- * This function assigns the next available DMA channel from the list to the
- * requester
- */
-static int dma_request_channel(void)
-{
-	/* Check if the channel is not currently in use */
-	if (!(in_le32(&(host_pvt.sata_dma_regs->dma_chan_en.low)) &
-		DMA_CHANNEL(host_pvt.dma_channel)))
-		return host_pvt.dma_channel;
-	dev_err(host_pvt.dwc_dev, "%s Channel %d is currently in use\n",
-		__func__, host_pvt.dma_channel);
-	return -1;
-}
-
-/*
- * Function: dma_dwc_interrupt
- * arguments: irq, dev_id, pt_regs
- * returns channel number if available else -1
- * Interrupt Handler for DW AHB SATA DMA
- */
-static irqreturn_t dma_dwc_interrupt(int irq, void *hsdev_instance)
-{
-	int chan;
-	u32 tfr_reg, err_reg;
 	unsigned long flags;
 	struct sata_dwc_device *hsdev = hsdev_instance;
 	struct ata_host *host = (struct ata_host *)hsdev->host;
@@ -487,341 +281,65 @@ static irqreturn_t dma_dwc_interrupt(int irq, void *hsdev_instance)
 	hsdevp = HSDEVP_FROM_AP(ap);
 	tag = ap->link.active_tag;
 
-	tfr_reg = in_le32(&(host_pvt.sata_dma_regs->interrupt_status.tfr\
-			.low));
-	err_reg = in_le32(&(host_pvt.sata_dma_regs->interrupt_status.error\
-			.low));
-
-	dev_dbg(ap->dev, "eot=0x%08x err=0x%08x pending=%d active port=%d\n",
-		tfr_reg, err_reg, hsdevp->dma_pending[tag], port);
-
-	chan = host_pvt.dma_channel;
-	if (chan >= 0) {
-		/* Check for end-of-transfer interrupt. */
-		if (tfr_reg & DMA_CHANNEL(chan)) {
-			/*
-			 * Each DMA command produces 2 interrupts.  Only
-			 * complete the command after both interrupts have been
-			 * seen. (See sata_dwc_isr())
-			 */
-			host_pvt.dma_interrupt_count++;
-			sata_dwc_clear_dmacr(hsdevp, tag);
-
-			if (hsdevp->dma_pending[tag] ==
-			    SATA_DWC_DMA_PENDING_NONE) {
-				dev_err(ap->dev, "DMA not pending eot=0x%08x "
-					"err=0x%08x tag=0x%02x pending=%d\n",
-					tfr_reg, err_reg, tag,
-					hsdevp->dma_pending[tag]);
-			}
-
-			if ((host_pvt.dma_interrupt_count % 2) == 0)
-				sata_dwc_dma_xfer_complete(ap, 1);
-
-			/* Clear the interrupt */
-			out_le32(&(host_pvt.sata_dma_regs->interrupt_clear\
-				.tfr.low),
-				 DMA_CHANNEL(chan));
-		}
-
-		/* Check for error interrupt. */
-		if (err_reg & DMA_CHANNEL(chan)) {
-			/* TODO Need error handler ! */
-			dev_err(ap->dev, "error interrupt err_reg=0x%08x\n",
-				err_reg);
-
-			/* Clear the interrupt. */
-			out_le32(&(host_pvt.sata_dma_regs->interrupt_clear\
-				.error.low),
-				 DMA_CHANNEL(chan));
-		}
-	}
-	spin_unlock_irqrestore(&host->lock, flags);
-	return IRQ_HANDLED;
-}
-
-/*
- * Function: dma_request_interrupts
- * arguments: hsdev
- * returns status
- * This function registers ISR for a particular DMA channel interrupt
- */
-static int dma_request_interrupts(struct sata_dwc_device *hsdev, int irq)
-{
-	int retval = 0;
-	int chan = host_pvt.dma_channel;
-
-	if (chan >= 0) {
-		/* Unmask error interrupt */
-		out_le32(&(host_pvt.sata_dma_regs)->interrupt_mask.error.low,
-			 DMA_ENABLE_CHAN(chan));
-
-		/* Unmask end-of-transfer interrupt */
-		out_le32(&(host_pvt.sata_dma_regs)->interrupt_mask.tfr.low,
-			 DMA_ENABLE_CHAN(chan));
-	}
-
-	retval = request_irq(irq, dma_dwc_interrupt, 0, "SATA DMA", hsdev);
-	if (retval) {
-		dev_err(host_pvt.dwc_dev, "%s: could not get IRQ %d\n",
-		__func__, irq);
-		return -ENODEV;
-	}
-
-	/* Mark this interrupt as requested */
-	hsdev->irq_dma = irq;
-	return 0;
-}
-
-/*
- * Function: map_sg_to_lli
- * The Synopsis driver has a comment proposing that better performance
- * is possible by only enabling interrupts on the last item in the linked list.
- * However, it seems that could be a problem if an error happened on one of the
- * first items.  The transfer would halt, but no error interrupt would occur.
- * Currently this function sets interrupts enabled for each linked list item:
- * DMA_CTL_INT_EN.
- */
-static int map_sg_to_lli(struct scatterlist *sg, int num_elems,
-			struct lli *lli, dma_addr_t dma_lli,
-			void __iomem *dmadr_addr, int dir)
-{
-	int i, idx = 0;
-	int fis_len = 0;
-	dma_addr_t next_llp;
-	int bl;
-	int sms_val, dms_val;
-
-	sms_val = 0;
-	dms_val = 1 + host_pvt.dma_channel;
-	dev_dbg(host_pvt.dwc_dev,
-		"%s: sg=%p nelem=%d lli=%p dma_lli=0x%pad dmadr=0x%p\n",
-		__func__, sg, num_elems, lli, &dma_lli, dmadr_addr);
-
-	bl = get_burst_length_encode(AHB_DMA_BRST_DFLT);
-
-	for (i = 0; i < num_elems; i++, sg++) {
-		u32 addr, offset;
-		u32 sg_len, len;
-
-		addr = (u32) sg_dma_address(sg);
-		sg_len = sg_dma_len(sg);
-
-		dev_dbg(host_pvt.dwc_dev, "%s: elem=%d sg_addr=0x%x sg_len"
-			"=%d\n", __func__, i, addr, sg_len);
-
-		while (sg_len) {
-			if (idx >= SATA_DWC_DMAC_LLI_NUM) {
-				/* The LLI table is not large enough. */
-				dev_err(host_pvt.dwc_dev, "LLI table overrun "
-				"(idx=%d)\n", idx);
-				break;
-			}
-			len = (sg_len > SATA_DWC_DMAC_CTRL_TSIZE_MAX) ?
-				SATA_DWC_DMAC_CTRL_TSIZE_MAX : sg_len;
-
-			offset = addr & 0xffff;
-			if ((offset + sg_len) > 0x10000)
-				len = 0x10000 - offset;
-
-			/*
-			 * Make sure a LLI block is not created that will span
-			 * 8K max FIS boundary.  If the block spans such a FIS
-			 * boundary, there is a chance that a DMA burst will
-			 * cross that boundary -- this results in an error in
-			 * the host controller.
-			 */
-			if (fis_len + len > 8192) {
-				dev_dbg(host_pvt.dwc_dev, "SPLITTING: fis_len="
-					"%d(0x%x) len=%d(0x%x)\n", fis_len,
-					 fis_len, len, len);
-				len = 8192 - fis_len;
-				fis_len = 0;
-			} else {
-				fis_len += len;
-			}
-			if (fis_len == 8192)
-				fis_len = 0;
-
-			/*
-			 * Set DMA addresses and lower half of control register
-			 * based on direction.
-			 */
-			if (dir == DMA_FROM_DEVICE) {
-				lli[idx].dar = cpu_to_le32(addr);
-				lli[idx].sar = cpu_to_le32((u32)dmadr_addr);
-
-				lli[idx].ctl.low = cpu_to_le32(
-					DMA_CTL_TTFC(DMA_CTL_TTFC_P2M_DMAC) |
-					DMA_CTL_SMS(sms_val) |
-					DMA_CTL_DMS(dms_val) |
-					DMA_CTL_SRC_MSIZE(bl) |
-					DMA_CTL_DST_MSIZE(bl) |
-					DMA_CTL_SINC_NOCHANGE |
-					DMA_CTL_SRC_TRWID(2) |
-					DMA_CTL_DST_TRWID(2) |
-					DMA_CTL_INT_EN |
-					DMA_CTL_LLP_SRCEN |
-					DMA_CTL_LLP_DSTEN);
-			} else {	/* DMA_TO_DEVICE */
-				lli[idx].sar = cpu_to_le32(addr);
-				lli[idx].dar = cpu_to_le32((u32)dmadr_addr);
-
-				lli[idx].ctl.low = cpu_to_le32(
-					DMA_CTL_TTFC(DMA_CTL_TTFC_M2P_PER) |
-					DMA_CTL_SMS(dms_val) |
-					DMA_CTL_DMS(sms_val) |
-					DMA_CTL_SRC_MSIZE(bl) |
-					DMA_CTL_DST_MSIZE(bl) |
-					DMA_CTL_DINC_NOCHANGE |
-					DMA_CTL_SRC_TRWID(2) |
-					DMA_CTL_DST_TRWID(2) |
-					DMA_CTL_INT_EN |
-					DMA_CTL_LLP_SRCEN |
-					DMA_CTL_LLP_DSTEN);
-			}
-
-			dev_dbg(host_pvt.dwc_dev, "%s setting ctl.high len: "
-				"0x%08x val: 0x%08x\n", __func__,
-				len, DMA_CTL_BLK_TS(len / 4));
-
-			/* Program the LLI CTL high register */
-			lli[idx].ctl.high = cpu_to_le32(DMA_CTL_BLK_TS\
-						(len / 4));
-
-			/* Program the next pointer.  The next pointer must be
-			 * the physical address, not the virtual address.
-			 */
-			next_llp = (dma_lli + ((idx + 1) * sizeof(struct \
-							lli)));
-
-			/* The last 2 bits encode the list master select. */
-			next_llp = DMA_LLP_LMS(next_llp, DMA_LLP_AHBMASTER2);
-
-			lli[idx].llp = cpu_to_le32(next_llp);
-			idx++;
-			sg_len -= len;
-			addr += len;
-		}
-	}
-
 	/*
-	 * The last next ptr has to be zero and the last control low register
-	 * has to have LLP_SRC_EN and LLP_DST_EN (linked list pointer source
-	 * and destination enable) set back to 0 (disabled.) This is what tells
-	 * the core that this is the last item in the linked list.
+	 * Each DMA command produces 2 interrupts.  Only
+	 * complete the command after both interrupts have been
+	 * seen. (See sata_dwc_isr())
 	 */
-	if (idx) {
-		lli[idx-1].llp = 0x00000000;
-		lli[idx-1].ctl.low &= DMA_CTL_LLP_DISABLE_LE32;
+	hsdevp->dma_interrupt_count++;
+	sata_dwc_clear_dmacr(hsdevp, tag);
 
-		/* Flush cache to memory */
-		dma_cache_sync(NULL, lli, (sizeof(struct lli) * idx),
-			       DMA_BIDIRECTIONAL);
+	if (hsdevp->dma_pending[tag] == SATA_DWC_DMA_PENDING_NONE) {
+		dev_err(ap->dev, "DMA not pending tag=0x%02x pending=%d\n",
+			tag, hsdevp->dma_pending[tag]);
 	}
 
-	return idx;
-}
+	if ((hsdevp->dma_interrupt_count % 2) == 0)
+		sata_dwc_dma_xfer_complete(ap, 1);
 
-/*
- * Function: dma_dwc_xfer_start
- * arguments: Channel number
- * Return : None
- * Enables the DMA channel
- */
-static void dma_dwc_xfer_start(int dma_ch)
-{
-	/* Enable the DMA channel */
-	out_le32(&(host_pvt.sata_dma_regs->dma_chan_en.low),
-		 in_le32(&(host_pvt.sata_dma_regs->dma_chan_en.low)) |
-		 DMA_ENABLE_CHAN(dma_ch));
+	spin_unlock_irqrestore(&host->lock, flags);
 }
 
-static int dma_dwc_xfer_setup(struct scatterlist *sg, int num_elems,
-			      struct lli *lli, dma_addr_t dma_lli,
-			      void __iomem *addr, int dir)
+static struct dma_async_tx_descriptor *dma_dwc_xfer_setup(struct ata_queued_cmd *qc)
 {
-	int dma_ch;
-	int num_lli;
-	/* Acquire DMA channel */
-	dma_ch = dma_request_channel();
-	if (dma_ch == -1) {
-		dev_err(host_pvt.dwc_dev, "%s: dma channel unavailable\n",
-			 __func__);
-		return -EAGAIN;
+	struct ata_port *ap = qc->ap;
+	struct sata_dwc_device_port *hsdevp = HSDEVP_FROM_AP(ap);
+	struct sata_dwc_device *hsdev = HSDEV_FROM_AP(ap);
+	dma_addr_t addr = (dma_addr_t)&hsdev->sata_dwc_regs->dmadr;
+	struct dma_slave_config sconf;
+	struct dma_async_tx_descriptor *desc;
+
+	if (qc->dma_dir == DMA_DEV_TO_MEM) {
+		sconf.src_addr = addr;
+		sconf.device_fc = true;
+	} else {	/* DMA_MEM_TO_DEV */
+		sconf.dst_addr = addr;
+		sconf.device_fc = false;
 	}
 
-	/* Convert SG list to linked list of items (LLIs) for AHB DMA */
-	num_lli = map_sg_to_lli(sg, num_elems, lli, dma_lli, addr, dir);
-
-	dev_dbg(host_pvt.dwc_dev, "%s sg: 0x%p, count: %d lli: %p dma_lli:"
-		" 0x%0xlx addr: %p lli count: %d\n", __func__, sg, num_elems,
-		 lli, (u32)dma_lli, addr, num_lli);
-
-	clear_chan_interrupts(dma_ch);
+	sconf.direction = qc->dma_dir;
+	sconf.src_maxburst = AHB_DMA_BRST_DFLT;
+	sconf.dst_maxburst = AHB_DMA_BRST_DFLT;
+	sconf.src_addr_width = DMA_SLAVE_BUSWIDTH_4_BYTES;
+	sconf.dst_addr_width = DMA_SLAVE_BUSWIDTH_4_BYTES;
 
-	/* Program the CFG register. */
-	out_le32(&(host_pvt.sata_dma_regs->chan_regs[dma_ch].cfg.high),
-		 DMA_CFG_HW_HS_SRC(dma_ch) | DMA_CFG_HW_HS_DEST(dma_ch) |
-		 DMA_CFG_PROTCTL | DMA_CFG_FCMOD_REQ);
-	out_le32(&(host_pvt.sata_dma_regs->chan_regs[dma_ch].cfg.low),
-		 DMA_CFG_HW_CH_PRIOR(dma_ch));
+	dmaengine_slave_config(hsdevp->chan, &sconf);
 
-	/* Program the address of the linked list */
-	out_le32(&(host_pvt.sata_dma_regs->chan_regs[dma_ch].llp.low),
-		 DMA_LLP_LMS(dma_lli, DMA_LLP_AHBMASTER2));
-
-	/* Program the CTL register with src enable / dst enable */
-	out_le32(&(host_pvt.sata_dma_regs->chan_regs[dma_ch].ctl.low),
-		 DMA_CTL_LLP_SRCEN | DMA_CTL_LLP_DSTEN);
-	return dma_ch;
-}
-
-/*
- * Function: dma_dwc_exit
- * arguments: None
- * returns status
- * This function exits the SATA DMA driver
- */
-static void dma_dwc_exit(struct sata_dwc_device *hsdev)
-{
-	dev_dbg(host_pvt.dwc_dev, "%s:\n", __func__);
-	if (host_pvt.sata_dma_regs) {
-		iounmap((void __iomem *)host_pvt.sata_dma_regs);
-		host_pvt.sata_dma_regs = NULL;
-	}
-
-	if (hsdev->irq_dma) {
-		free_irq(hsdev->irq_dma, hsdev);
-		hsdev->irq_dma = 0;
-	}
-}
-
-/*
- * Function: dma_dwc_init
- * arguments: hsdev
- * returns status
- * This function initializes the SATA DMA driver
- */
-static int dma_dwc_init(struct sata_dwc_device *hsdev, int irq)
-{
-	int err;
+	/* Convert SG list to linked list of items (LLIs) for AHB DMA */
+	desc = dmaengine_prep_slave_sg(hsdevp->chan, qc->sg, qc->n_elem,
+				       qc->dma_dir,
+				       DMA_PREP_INTERRUPT | DMA_CTRL_ACK);
 
-	err = dma_request_interrupts(hsdev, irq);
-	if (err) {
-		dev_err(host_pvt.dwc_dev, "%s: dma_request_interrupts returns"
-			" %d\n", __func__, err);
-		return err;
-	}
+	if (!desc)
+		return NULL;
 
-	/* Enabe DMA */
-	out_le32(&(host_pvt.sata_dma_regs->dma_cfg.low), DMA_EN);
+	desc->callback = dma_dwc_xfer_done;
+	desc->callback_param = hsdev;
 
-	dev_notice(host_pvt.dwc_dev, "DMA initialized\n");
-	dev_dbg(host_pvt.dwc_dev, "SATA DMA registers=0x%p\n", host_pvt.\
-		sata_dma_regs);
+	dev_dbg(hsdev->dev, "%s sg: 0x%p, count: %d addr: %pad\n",
+		__func__, qc->sg, qc->n_elem, &addr);
 
-	return 0;
+	return desc;
 }
 
 static int sata_dwc_scr_read(struct ata_link *link, unsigned int scr, u32 *val)
@@ -891,21 +409,18 @@ static void sata_dwc_error_intr(struct ata_port *ap,
 	struct ata_queued_cmd *qc;
 	u32 serror;
 	u8 status, tag;
-	u32 err_reg;
 
 	ata_ehi_clear_desc(ehi);
 
 	serror = core_scr_read(SCR_ERROR);
 	status = ap->ops->sff_check_status(ap);
 
-	err_reg = in_le32(&(host_pvt.sata_dma_regs->interrupt_status.error.\
-			low));
 	tag = ap->link.active_tag;
 
-	dev_err(ap->dev, "%s SCR_ERROR=0x%08x intpr=0x%08x status=0x%08x "
-		"dma_intp=%d pending=%d issued=%d dma_err_status=0x%08x\n",
-		__func__, serror, intpr, status, host_pvt.dma_interrupt_count,
-		hsdevp->dma_pending[tag], hsdevp->cmd_issued[tag], err_reg);
+	dev_err(ap->dev,
+		"%s SCR_ERROR=0x%08x intpr=0x%08x status=0x%08x dma_intp=%d pending=%d issued=%d",
+		__func__, serror, intpr, status, hsdevp->dma_interrupt_count,
+		hsdevp->dma_pending[tag], hsdevp->cmd_issued[tag]);
 
 	/* Clear error register and interrupt bit */
 	clear_serror();
@@ -1003,8 +518,9 @@ static irqreturn_t sata_dwc_isr(int irq, void *dev_instance)
 
 		/* DEV interrupt w/ no active qc? */
 		if (unlikely(!qc || (qc->tf.flags & ATA_TFLAG_POLLING))) {
-			dev_err(ap->dev, "%s interrupt with no active qc "
-				"qc=%p\n", __func__, qc);
+			dev_err(ap->dev,
+				"%s interrupt with no active qc qc=%p\n",
+				__func__, qc);
 			ap->ops->sff_check_status(ap);
 			handled = 1;
 			goto DONE;
@@ -1031,16 +547,16 @@ DRVSTILLBUSY:
 			 * operation done interrupt. The command should be
 			 * completed only after both interrupts are seen.
 			 */
-			host_pvt.dma_interrupt_count++;
+			hsdevp->dma_interrupt_count++;
 			if (hsdevp->dma_pending[tag] == \
 					SATA_DWC_DMA_PENDING_NONE) {
-				dev_err(ap->dev, "%s: DMA not pending "
-					"intpr=0x%08x status=0x%08x pending"
-					"=%d\n", __func__, intpr, status,
+				dev_err(ap->dev,
+					"%s: DMA not pending intpr=0x%08x status=0x%08x pending=%d\n",
+					__func__, intpr, status,
 					hsdevp->dma_pending[tag]);
 			}
 
-			if ((host_pvt.dma_interrupt_count % 2) == 0)
+			if ((hsdevp->dma_interrupt_count % 2) == 0)
 				sata_dwc_dma_xfer_complete(ap, 1);
 		} else if (ata_is_pio(qc->tf.protocol)) {
 			ata_sff_hsm_move(ap, qc, status, 0);
@@ -1068,17 +584,17 @@ DRVSTILLBUSY:
 
 	if (sactive != 0 || (host_pvt.sata_dwc_sactive_issued) > 1 || \
 							tag_mask > 1) {
-		dev_dbg(ap->dev, "%s NCQ:sactive=0x%08x  sactive_issued=0x%08x"
-			"tag_mask=0x%08x\n", __func__, sactive,
-			host_pvt.sata_dwc_sactive_issued, tag_mask);
+		dev_dbg(ap->dev,
+			"%s NCQ:sactive=0x%08x  sactive_issued=0x%08x tag_mask=0x%08x\n",
+			__func__, sactive, host_pvt.sata_dwc_sactive_issued,
+			tag_mask);
 	}
 
 	if ((tag_mask | (host_pvt.sata_dwc_sactive_issued)) != \
 					(host_pvt.sata_dwc_sactive_issued)) {
-		dev_warn(ap->dev, "Bad tag mask?  sactive=0x%08x "
-			 "(host_pvt.sata_dwc_sactive_issued)=0x%08x  tag_mask"
-			 "=0x%08x\n", sactive, host_pvt.sata_dwc_sactive_issued,
-			  tag_mask);
+		dev_warn(ap->dev,
+			 "Bad tag mask?  sactive=0x%08x (host_pvt.sata_dwc_sactive_issued)=0x%08x  tag_mask=0x%08x\n",
+			 sactive, host_pvt.sata_dwc_sactive_issued, tag_mask);
 	}
 
 	/* read just to clear ... not bad if currently still busy */
@@ -1114,12 +630,12 @@ DRVSTILLBUSY:
 		dev_dbg(ap->dev, "%s NCQ command, protocol: %s\n", __func__,
 			get_prot_descript(qc->tf.protocol));
 		if (ata_is_dma(qc->tf.protocol)) {
-			host_pvt.dma_interrupt_count++;
+			hsdevp->dma_interrupt_count++;
 			if (hsdevp->dma_pending[tag] == \
 					SATA_DWC_DMA_PENDING_NONE)
 				dev_warn(ap->dev, "%s: DMA not pending?\n",
 					__func__);
-			if ((host_pvt.dma_interrupt_count % 2) == 0)
+			if ((hsdevp->dma_interrupt_count % 2) == 0)
 				sata_dwc_dma_xfer_complete(ap, 1);
 		} else {
 			if (unlikely(sata_dwc_qc_complete(ap, qc, 1)))
@@ -1142,8 +658,9 @@ STILLBUSY:
 	 */
 	sactive2 = core_scr_read(SCR_ACTIVE);
 	if (sactive2 != sactive) {
-		dev_dbg(ap->dev, "More completed - sactive=0x%x sactive2"
-			"=0x%x\n", sactive, sactive2);
+		dev_dbg(ap->dev,
+			"More completed - sactive=0x%x sactive2=0x%x\n",
+			sactive, sactive2);
 	}
 	handled = 1;
 
@@ -1169,11 +686,10 @@ static void sata_dwc_clear_dmacr(struct sata_dwc_device_port *hsdevp, u8 tag)
 		 * This should not happen, it indicates the driver is out of
 		 * sync.  If it does happen, clear dmacr anyway.
 		 */
-		dev_err(host_pvt.dwc_dev, "%s DMA protocol RX and"
-			"TX DMA not pending tag=0x%02x pending=%d"
-			" dmacr: 0x%08x\n", __func__, tag,
-			hsdevp->dma_pending[tag],
-			in_le32(&(hsdev->sata_dwc_regs->dmacr)));
+		dev_err(hsdev->dev,
+			"%s DMA protocol RX and TX DMA not pending tag=0x%02x pending=%d dmacr: 0x%08x\n",
+			__func__, tag, hsdevp->dma_pending[tag],
+			in_le32(&hsdev->sata_dwc_regs->dmacr));
 		out_le32(&(hsdev->sata_dwc_regs->dmacr),
 			SATA_DWC_DMACR_TXRXCH_CLEAR);
 	}
@@ -1195,8 +711,9 @@ static void sata_dwc_dma_xfer_complete(struct ata_port *ap, u32 check_status)
 
 #ifdef DEBUG_NCQ
 	if (tag > 0) {
-		dev_info(ap->dev, "%s tag=%u cmd=0x%02x dma dir=%s proto=%s "
-			 "dmacr=0x%08x\n", __func__, qc->tag, qc->tf.command,
+		dev_info(ap->dev,
+			 "%s tag=%u cmd=0x%02x dma dir=%s proto=%s dmacr=0x%08x\n",
+			 __func__, qc->tag, qc->tf.command,
 			 get_dma_dir_descript(qc->dma_dir),
 			 get_prot_descript(qc->tf.protocol),
 			 in_le32(&(hsdev->sata_dwc_regs->dmacr)));
@@ -1205,8 +722,9 @@ static void sata_dwc_dma_xfer_complete(struct ata_port *ap, u32 check_status)
 
 	if (ata_is_dma(qc->tf.protocol)) {
 		if (hsdevp->dma_pending[tag] == SATA_DWC_DMA_PENDING_NONE) {
-			dev_err(ap->dev, "%s DMA protocol RX and TX DMA not "
-				"pending dmacr: 0x%08x\n", __func__,
+			dev_err(ap->dev,
+				"%s DMA protocol RX and TX DMA not pending dmacr: 0x%08x\n",
+				__func__,
 				in_le32(&(hsdev->sata_dwc_regs->dmacr)));
 		}
 
@@ -1232,9 +750,9 @@ static int sata_dwc_qc_complete(struct ata_port *ap, struct ata_queued_cmd *qc,
 		dev_err(ap->dev, "TX DMA PENDING\n");
 	else if (hsdevp->dma_pending[tag] == SATA_DWC_DMA_PENDING_RX)
 		dev_err(ap->dev, "RX DMA PENDING\n");
-	dev_dbg(ap->dev, "QC complete cmd=0x%02x status=0x%02x ata%u:"
-		" protocol=%d\n", qc->tf.command, status, ap->print_id,
-		 qc->tf.protocol);
+	dev_dbg(ap->dev,
+		"QC complete cmd=0x%02x status=0x%02x ata%u: protocol=%d\n",
+		qc->tf.command, status, ap->print_id, qc->tf.protocol);
 
 	/* clear active bit */
 	mask = (~(qcmd_tag_to_mask(tag)));
@@ -1260,11 +778,23 @@ static void sata_dwc_enable_interrupts(struct sata_dwc_device *hsdev)
 	 */
 	out_le32(&hsdev->sata_dwc_regs->errmr, SATA_DWC_SERROR_ERR_BITS);
 
-	dev_dbg(host_pvt.dwc_dev, "%s: INTMR = 0x%08x, ERRMR = 0x%08x\n",
+	dev_dbg(hsdev->dev, "%s: INTMR = 0x%08x, ERRMR = 0x%08x\n",
 		 __func__, in_le32(&hsdev->sata_dwc_regs->intmr),
 		in_le32(&hsdev->sata_dwc_regs->errmr));
 }
 
+static bool sata_dwc_dma_filter(struct dma_chan *chan, void *param)
+{
+	struct sata_dwc_device_port *hsdevp = param;
+	struct dw_dma_slave *dws = hsdevp->dws;
+
+	if (dws->dma_dev != chan->device->dev)
+		return false;
+
+	chan->private = dws;
+	return true;
+}
+
 static void sata_dwc_setup_port(struct ata_ioports *port, unsigned long base)
 {
 	port->cmd_addr = (void __iomem *)base + 0x00;
@@ -1299,6 +829,7 @@ static int sata_dwc_port_start(struct ata_port *ap)
 	struct sata_dwc_device *hsdev;
 	struct sata_dwc_device_port *hsdevp = NULL;
 	struct device *pdev;
+	dma_cap_mask_t mask;
 	int i;
 
 	hsdev = HSDEV_FROM_AP(ap);
@@ -1322,29 +853,27 @@ static int sata_dwc_port_start(struct ata_port *ap)
 	}
 	hsdevp->hsdev = hsdev;
 
+	hsdevp->dws = &sata_dwc_dma_dws;
+	hsdevp->dws->dma_dev = hsdev->dev;
+
+	dma_cap_zero(mask);
+	dma_cap_set(DMA_SLAVE, mask);
+
+	/* Acquire DMA channel */
+	hsdevp->chan = dma_request_channel(mask, sata_dwc_dma_filter, hsdevp);
+	if (!hsdevp->chan) {
+		dev_err(hsdev->dev, "%s: dma channel unavailable\n",
+			 __func__);
+		err = -EAGAIN;
+		goto CLEANUP_ALLOC;
+	}
+
 	for (i = 0; i < SATA_DWC_QCMD_MAX; i++)
 		hsdevp->cmd_issued[i] = SATA_DWC_CMD_ISSUED_NOT;
 
 	ap->bmdma_prd = NULL;	/* set these so libata doesn't use them */
 	ap->bmdma_prd_dma = 0;
 
-	/*
-	 * DMA - Assign scatter gather LLI table. We can't use the libata
-	 * version since it's PRD is IDE PCI specific.
-	 */
-	for (i = 0; i < SATA_DWC_QCMD_MAX; i++) {
-		hsdevp->llit[i] = dma_alloc_coherent(pdev,
-						     SATA_DWC_DMAC_LLI_TBL_SZ,
-						     &(hsdevp->llit_dma[i]),
-						     GFP_ATOMIC);
-		if (!hsdevp->llit[i]) {
-			dev_err(ap->dev, "%s: dma_alloc_coherent failed\n",
-				 __func__);
-			err = -ENOMEM;
-			goto CLEANUP_ALLOC;
-		}
-	}
-
 	if (ap->port_no == 0)  {
 		dev_dbg(ap->dev, "%s: clearing TXCHEN, RXCHEN in DMAC\n",
 			__func__);
@@ -1373,22 +902,14 @@ CLEANUP:
 
 static void sata_dwc_port_stop(struct ata_port *ap)
 {
-	int i;
-	struct sata_dwc_device *hsdev = HSDEV_FROM_AP(ap);
 	struct sata_dwc_device_port *hsdevp = HSDEVP_FROM_AP(ap);
 
 	dev_dbg(ap->dev, "%s: ap->id = %d\n", __func__, ap->print_id);
 
-	if (hsdevp && hsdev) {
-		/* deallocate LLI table */
-		for (i = 0; i < SATA_DWC_QCMD_MAX; i++) {
-			dma_free_coherent(ap->host->dev,
-					  SATA_DWC_DMAC_LLI_TBL_SZ,
-					 hsdevp->llit[i], hsdevp->llit_dma[i]);
-		}
+	dmaengine_terminate_all(hsdevp->chan);
+	dma_release_channel(hsdevp->chan);
 
-		kfree(hsdevp);
-	}
+	kfree(hsdevp);
 	ap->private_data = NULL;
 }
 
@@ -1444,12 +965,12 @@ static void sata_dwc_bmdma_setup(struct ata_queued_cmd *qc)
 static void sata_dwc_bmdma_start_by_tag(struct ata_queued_cmd *qc, u8 tag)
 {
 	int start_dma;
-	u32 reg, dma_chan;
+	u32 reg;
 	struct sata_dwc_device *hsdev = HSDEV_FROM_QC(qc);
 	struct ata_port *ap = qc->ap;
 	struct sata_dwc_device_port *hsdevp = HSDEVP_FROM_AP(ap);
+	struct dma_async_tx_descriptor *desc = hsdevp->desc[tag];
 	int dir = qc->dma_dir;
-	dma_chan = hsdevp->dma_chan[tag];
 
 	if (hsdevp->cmd_issued[tag] != SATA_DWC_CMD_ISSUED_NOT) {
 		start_dma = 1;
@@ -1458,16 +979,17 @@ static void sata_dwc_bmdma_start_by_tag(struct ata_queued_cmd *qc, u8 tag)
 		else
 			hsdevp->dma_pending[tag] = SATA_DWC_DMA_PENDING_RX;
 	} else {
-		dev_err(ap->dev, "%s: Command not pending cmd_issued=%d "
-			"(tag=%d) DMA NOT started\n", __func__,
-			hsdevp->cmd_issued[tag], tag);
+		dev_err(ap->dev,
+			"%s: Command not pending cmd_issued=%d (tag=%d) DMA NOT started\n",
+			__func__, hsdevp->cmd_issued[tag], tag);
 		start_dma = 0;
 	}
 
-	dev_dbg(ap->dev, "%s qc=%p tag: %x cmd: 0x%02x dma_dir: %s "
-		"start_dma? %x\n", __func__, qc, tag, qc->tf.command,
+	dev_dbg(ap->dev,
+		"%s qc=%p tag: %x cmd: 0x%02x dma_dir: %s start_dma? %x\n",
+		__func__, qc, tag, qc->tf.command,
 		get_dma_dir_descript(qc->dma_dir), start_dma);
-	sata_dwc_tf_dump(&(qc->tf));
+	sata_dwc_tf_dump(ap, &qc->tf);
 
 	if (start_dma) {
 		reg = core_scr_read(SCR_ERROR);
@@ -1484,7 +1006,8 @@ static void sata_dwc_bmdma_start_by_tag(struct ata_queued_cmd *qc, u8 tag)
 				SATA_DWC_DMACR_RXCHEN);
 
 		/* Enable AHB DMA transfer on the specified channel */
-		dma_dwc_xfer_start(dma_chan);
+		dmaengine_submit(desc);
+		dma_async_issue_pending(hsdevp->chan);
 	}
 }
 
@@ -1510,26 +1033,21 @@ static void sata_dwc_bmdma_start(struct ata_queued_cmd *qc)
  */
 static void sata_dwc_qc_prep_by_tag(struct ata_queued_cmd *qc, u8 tag)
 {
-	struct scatterlist *sg = qc->sg;
+	struct dma_async_tx_descriptor *desc;
 	struct ata_port *ap = qc->ap;
-	int dma_chan;
-	struct sata_dwc_device *hsdev = HSDEV_FROM_AP(ap);
 	struct sata_dwc_device_port *hsdevp = HSDEVP_FROM_AP(ap);
 
 	dev_dbg(ap->dev, "%s: port=%d dma dir=%s n_elem=%d\n",
 		__func__, ap->port_no, get_dma_dir_descript(qc->dma_dir),
 		 qc->n_elem);
 
-	dma_chan = dma_dwc_xfer_setup(sg, qc->n_elem, hsdevp->llit[tag],
-				      hsdevp->llit_dma[tag],
-				      (void __iomem *)&hsdev->sata_dwc_regs->dmadr,
-				      qc->dma_dir);
-	if (dma_chan < 0) {
-		dev_err(ap->dev, "%s: dma_dwc_xfer_setup returns err %d\n",
-			__func__, dma_chan);
+	desc = dma_dwc_xfer_setup(qc);
+	if (!desc) {
+		dev_err(ap->dev, "%s: dma_dwc_xfer_setup returns NULL\n",
+			__func__);
 		return;
 	}
-	hsdevp->dma_chan[tag] = dma_chan;
+	hsdevp->desc[tag] = desc;
 }
 
 static unsigned int sata_dwc_qc_issue(struct ata_queued_cmd *qc)
@@ -1540,8 +1058,8 @@ static unsigned int sata_dwc_qc_issue(struct ata_queued_cmd *qc)
 
 #ifdef DEBUG_NCQ
 	if (qc->tag > 0 || ap->link.sactive > 1)
-		dev_info(ap->dev, "%s ap id=%d cmd(0x%02x)=%s qc tag=%d "
-			 "prot=%s ap active_tag=0x%08x ap sactive=0x%08x\n",
+		dev_info(ap->dev,
+			 "%s ap id=%d cmd(0x%02x)=%s qc tag=%d prot=%s ap active_tag=0x%08x ap sactive=0x%08x\n",
 			 __func__, ap->print_id, qc->tf.command,
 			 ata_get_cmd_descript(qc->tf.command),
 			 qc->tag, get_prot_descript(qc->tf.protocol),
@@ -1557,9 +1075,9 @@ static unsigned int sata_dwc_qc_issue(struct ata_queued_cmd *qc)
 		sactive |= (0x00000001 << tag);
 		core_scr_write(SCR_ACTIVE, sactive);
 
-		dev_dbg(qc->ap->dev, "%s: tag=%d ap->link.sactive = 0x%08x "
-			"sactive=0x%08x\n", __func__, tag, qc->ap->link.sactive,
-			sactive);
+		dev_dbg(qc->ap->dev,
+			"%s: tag=%d ap->link.sactive = 0x%08x sactive=0x%08x\n",
+			__func__, tag, qc->ap->link.sactive, sactive);
 
 		ap->ops->sff_tf_load(ap, &qc->tf);
 		sata_dwc_exec_command_by_tag(ap, &qc->tf, qc->tag,
@@ -1673,7 +1191,6 @@ static int sata_dwc_probe(struct platform_device *ofdev)
 	struct ata_port_info pi = sata_dwc_port_info[0];
 	const struct ata_port_info *ppi[] = { &pi, NULL };
 	struct device_node *np = ofdev->dev.of_node;
-	u32 dma_chan;
 
 	/* Allocate DWC SATA device */
 	host = ata_host_alloc_pinfo(&ofdev->dev, ppi, SATA_DWC_MAX_PORTS);
@@ -1683,18 +1200,11 @@ static int sata_dwc_probe(struct platform_device *ofdev)
 
 	host->private_data = hsdev;
 
-	if (of_property_read_u32(np, "dma-channel", &dma_chan)) {
-		dev_warn(&ofdev->dev, "no dma-channel property set."
-			 " Use channel 0\n");
-		dma_chan = 0;
-	}
-	host_pvt.dma_channel = dma_chan;
-
 	/* Ioremap SATA registers */
 	base = of_iomap(np, 0);
 	if (!base) {
-		dev_err(&ofdev->dev, "ioremap failed for SATA register"
-			" address\n");
+		dev_err(&ofdev->dev,
+			"ioremap failed for SATA register address\n");
 		return -ENODEV;
 	}
 	hsdev->reg_base = base;
@@ -1716,27 +1226,29 @@ static int sata_dwc_probe(struct platform_device *ofdev)
 		   idr, ver[0], ver[1], ver[2]);
 
 	/* Get SATA DMA interrupt number */
-	irq = irq_of_parse_and_map(np, 1);
-	if (irq == NO_IRQ) {
+	hsdev->dma->irq = irq_of_parse_and_map(np, 1);
+	if (hsdev->dma->irq == NO_IRQ) {
 		dev_err(&ofdev->dev, "no SATA DMA irq\n");
 		err = -ENODEV;
 		goto error_iomap;
 	}
 
 	/* Get physical SATA DMA register base address */
-	host_pvt.sata_dma_regs = (void *)of_iomap(np, 1);
-	if (!(host_pvt.sata_dma_regs)) {
-		dev_err(&ofdev->dev, "ioremap failed for AHBDMA register"
-			" address\n");
+	hsdev->dma->regs = of_iomap(np, 1);
+	if (!hsdev->dma->regs) {
+		dev_err(&ofdev->dev,
+			"ioremap failed for AHBDMA register address\n");
 		err = -ENODEV;
 		goto error_iomap;
 	}
 
 	/* Save dev for later use in dev_xxx() routines */
-	host_pvt.dwc_dev = &ofdev->dev;
+	hsdev->dev = &ofdev->dev;
+
+	hsdev->dma->dev = &ofdev->dev;
 
 	/* Initialize AHB DMAC */
-	err = dma_dwc_init(hsdev, irq);
+	err = dw_dma_probe(hsdev->dma, NULL);
 	if (err)
 		goto error_dma_iomap;
 
@@ -1765,9 +1277,9 @@ static int sata_dwc_probe(struct platform_device *ofdev)
 
 error_out:
 	/* Free SATA DMA resources */
-	dma_dwc_exit(hsdev);
+	dw_dma_remove(hsdev->dma);
 error_dma_iomap:
-	iounmap((void __iomem *)host_pvt.sata_dma_regs);
+	iounmap(hsdev->dma->regs);
 error_iomap:
 	iounmap(base);
 	return err;
@@ -1782,9 +1294,9 @@ static int sata_dwc_remove(struct platform_device *ofdev)
 	ata_host_detach(host);
 
 	/* Free SATA DMA resources */
-	dma_dwc_exit(hsdev);
+	dw_dma_remove(hsdev->dma);
 
-	iounmap((void __iomem *)host_pvt.sata_dma_regs);
+	iounmap(hsdev->dma->regs);
 	iounmap(hsdev->reg_base);
 	dev_dbg(&ofdev->dev, "done\n");
 	return 0;
@@ -1809,5 +1321,5 @@ module_platform_driver(sata_dwc_driver);
 
 MODULE_LICENSE("GPL");
 MODULE_AUTHOR("Mark Miesfeld <mmiesfeld@amcc.com>");
-MODULE_DESCRIPTION("DesignWare Cores SATA controller low lever driver");
+MODULE_DESCRIPTION("DesignWare Cores SATA controller low level driver");
 MODULE_VERSION(DRV_VERSION);
diff --git a/drivers/ata/sata_fsl.c b/drivers/ata/sata_fsl.c
index f9054cd36a72..5389579c5120 100644
--- a/drivers/ata/sata_fsl.c
+++ b/drivers/ata/sata_fsl.c
@@ -869,6 +869,8 @@ try_offline_again:
 	 */
 	ata_msleep(ap, 1);
 
+	sata_set_spd(link);
+
 	/*
 	 * Now, bring the host controller online again, this can take time
 	 * as PHY reset and communication establishment, 1st D2H FIS and
diff --git a/drivers/ata/sata_inic162x.c b/drivers/ata/sata_inic162x.c
index 069827826b20..e81a8217f1ff 100644
--- a/drivers/ata/sata_inic162x.c
+++ b/drivers/ata/sata_inic162x.c
@@ -856,13 +856,13 @@ static int inic_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 	}
 
 	/* Set dma_mask.  This devices doesn't support 64bit addressing. */
-	rc = pci_set_dma_mask(pdev, DMA_BIT_MASK(32));
+	rc = dma_set_mask(&pdev->dev, DMA_BIT_MASK(32));
 	if (rc) {
 		dev_err(&pdev->dev, "32-bit DMA enable failed\n");
 		return rc;
 	}
 
-	rc = pci_set_consistent_dma_mask(pdev, DMA_BIT_MASK(32));
+	rc = dma_set_coherent_mask(&pdev->dev, DMA_BIT_MASK(32));
 	if (rc) {
 		dev_err(&pdev->dev, "32-bit consistent DMA enable failed\n");
 		return rc;
diff --git a/drivers/ata/sata_mv.c b/drivers/ata/sata_mv.c
index f8c33e3772b8..bd74ee555278 100644
--- a/drivers/ata/sata_mv.c
+++ b/drivers/ata/sata_mv.c
@@ -306,6 +306,11 @@ enum {
 	MV5_PHY_CTL		= 0x0C,
 	SATA_IFCFG		= 0x050,
 	LP_PHY_CTL		= 0x058,
+	LP_PHY_CTL_PIN_PU_PLL   = (1 << 0),
+	LP_PHY_CTL_PIN_PU_RX    = (1 << 1),
+	LP_PHY_CTL_PIN_PU_TX    = (1 << 2),
+	LP_PHY_CTL_GEN_TX_3G    = (1 << 5),
+	LP_PHY_CTL_GEN_RX_3G    = (1 << 9),
 
 	MV_M2_PREAMP_MASK	= 0x7e0,
 
@@ -1391,10 +1396,17 @@ static int mv_scr_write(struct ata_link *link, unsigned int sc_reg_in, u32 val)
 				/*
 				 * Set PHY speed according to SControl speed.
 				 */
-				if ((val & 0xf0) == 0x10)
-					writelfl(0x7, lp_phy_addr);
-				else
-					writelfl(0x227, lp_phy_addr);
+				u32 lp_phy_val =
+					LP_PHY_CTL_PIN_PU_PLL |
+					LP_PHY_CTL_PIN_PU_RX  |
+					LP_PHY_CTL_PIN_PU_TX;
+
+				if ((val & 0xf0) != 0x10)
+					lp_phy_val |=
+						LP_PHY_CTL_GEN_TX_3G |
+						LP_PHY_CTL_GEN_RX_3G;
+
+				writelfl(lp_phy_val, lp_phy_addr);
 			}
 		}
 		writelfl(val, addr);
@@ -4308,10 +4320,10 @@ static int pci_go_64(struct pci_dev *pdev)
 {
 	int rc;
 
-	if (!pci_set_dma_mask(pdev, DMA_BIT_MASK(64))) {
-		rc = pci_set_consistent_dma_mask(pdev, DMA_BIT_MASK(64));
+	if (!dma_set_mask(&pdev->dev, DMA_BIT_MASK(64))) {
+		rc = dma_set_coherent_mask(&pdev->dev, DMA_BIT_MASK(64));
 		if (rc) {
-			rc = pci_set_consistent_dma_mask(pdev, DMA_BIT_MASK(32));
+			rc = dma_set_coherent_mask(&pdev->dev, DMA_BIT_MASK(32));
 			if (rc) {
 				dev_err(&pdev->dev,
 					"64-bit DMA enable failed\n");
@@ -4319,12 +4331,12 @@ static int pci_go_64(struct pci_dev *pdev)
 			}
 		}
 	} else {
-		rc = pci_set_dma_mask(pdev, DMA_BIT_MASK(32));
+		rc = dma_set_mask(&pdev->dev, DMA_BIT_MASK(32));
 		if (rc) {
 			dev_err(&pdev->dev, "32-bit DMA enable failed\n");
 			return rc;
 		}
-		rc = pci_set_consistent_dma_mask(pdev, DMA_BIT_MASK(32));
+		rc = dma_set_coherent_mask(&pdev->dev, DMA_BIT_MASK(32));
 		if (rc) {
 			dev_err(&pdev->dev,
 				"32-bit consistent DMA enable failed\n");
diff --git a/drivers/ata/sata_nv.c b/drivers/ata/sata_nv.c
index 1db6f5ce5e89..7ece85f43020 100644
--- a/drivers/ata/sata_nv.c
+++ b/drivers/ata/sata_nv.c
@@ -756,10 +756,10 @@ static int nv_adma_slave_config(struct scsi_device *sdev)
 			blk_queue_bounce_limit(sdev1->request_queue,
 					       ATA_DMA_MASK);
 
-		pci_set_dma_mask(pdev, ATA_DMA_MASK);
+		dma_set_mask(&pdev->dev, ATA_DMA_MASK);
 	} else {
 		/** This shouldn't fail as it was set to this value before */
-		pci_set_dma_mask(pdev, pp->adma_dma_mask);
+		dma_set_mask(&pdev->dev, pp->adma_dma_mask);
 		if (sdev0)
 			blk_queue_bounce_limit(sdev0->request_queue,
 					       pp->adma_dma_mask);
@@ -1133,10 +1133,10 @@ static int nv_adma_port_start(struct ata_port *ap)
 
 	/* Ensure DMA mask is set to 32-bit before allocating legacy PRD and
 	   pad buffers */
-	rc = pci_set_dma_mask(pdev, DMA_BIT_MASK(32));
+	rc = dma_set_mask(&pdev->dev, DMA_BIT_MASK(32));
 	if (rc)
 		return rc;
-	rc = pci_set_consistent_dma_mask(pdev, DMA_BIT_MASK(32));
+	rc = dma_set_coherent_mask(&pdev->dev, DMA_BIT_MASK(32));
 	if (rc)
 		return rc;
 
@@ -1161,8 +1161,8 @@ static int nv_adma_port_start(struct ata_port *ap)
 	   These are allowed to fail since we store the value that ends up
 	   being used to set as the bounce limit in slave_config later if
 	   needed. */
-	pci_set_dma_mask(pdev, DMA_BIT_MASK(64));
-	pci_set_consistent_dma_mask(pdev, DMA_BIT_MASK(64));
+	dma_set_mask(&pdev->dev, DMA_BIT_MASK(64));
+	dma_set_coherent_mask(&pdev->dev, DMA_BIT_MASK(64));
 	pp->adma_dma_mask = *dev->dma_mask;
 
 	mem = dmam_alloc_coherent(dev, NV_ADMA_PORT_PRIV_DMA_SZ,
diff --git a/drivers/ata/sata_promise.c b/drivers/ata/sata_promise.c
index 3638887476f6..0fa211e2831c 100644
--- a/drivers/ata/sata_promise.c
+++ b/drivers/ata/sata_promise.c
@@ -1246,10 +1246,10 @@ static int pdc_ata_init_one(struct pci_dev *pdev,
 	/* initialize adapter */
 	pdc_host_init(host);
 
-	rc = pci_set_dma_mask(pdev, ATA_DMA_MASK);
+	rc = dma_set_mask(&pdev->dev, ATA_DMA_MASK);
 	if (rc)
 		return rc;
-	rc = pci_set_consistent_dma_mask(pdev, ATA_DMA_MASK);
+	rc = dma_set_coherent_mask(&pdev->dev, ATA_DMA_MASK);
 	if (rc)
 		return rc;
 
diff --git a/drivers/ata/sata_qstor.c b/drivers/ata/sata_qstor.c
index 9a6bd4cd29a0..af987a4f33d1 100644
--- a/drivers/ata/sata_qstor.c
+++ b/drivers/ata/sata_qstor.c
@@ -557,10 +557,10 @@ static int qs_set_dma_masks(struct pci_dev *pdev, void __iomem *mmio_base)
 	int rc, have_64bit_bus = (bus_info & QS_HPHY_64BIT);
 
 	if (have_64bit_bus &&
-	    !pci_set_dma_mask(pdev, DMA_BIT_MASK(64))) {
-		rc = pci_set_consistent_dma_mask(pdev, DMA_BIT_MASK(64));
+	    !dma_set_mask(&pdev->dev, DMA_BIT_MASK(64))) {
+		rc = dma_set_coherent_mask(&pdev->dev, DMA_BIT_MASK(64));
 		if (rc) {
-			rc = pci_set_consistent_dma_mask(pdev, DMA_BIT_MASK(32));
+			rc = dma_set_coherent_mask(&pdev->dev, DMA_BIT_MASK(32));
 			if (rc) {
 				dev_err(&pdev->dev,
 					"64-bit DMA enable failed\n");
@@ -568,12 +568,12 @@ static int qs_set_dma_masks(struct pci_dev *pdev, void __iomem *mmio_base)
 			}
 		}
 	} else {
-		rc = pci_set_dma_mask(pdev, DMA_BIT_MASK(32));
+		rc = dma_set_mask(&pdev->dev, DMA_BIT_MASK(32));
 		if (rc) {
 			dev_err(&pdev->dev, "32-bit DMA enable failed\n");
 			return rc;
 		}
-		rc = pci_set_consistent_dma_mask(pdev, DMA_BIT_MASK(32));
+		rc = dma_set_coherent_mask(&pdev->dev, DMA_BIT_MASK(32));
 		if (rc) {
 			dev_err(&pdev->dev,
 				"32-bit consistent DMA enable failed\n");
diff --git a/drivers/ata/sata_sil.c b/drivers/ata/sata_sil.c
index 40b76b2d18c6..dea6edcbf145 100644
--- a/drivers/ata/sata_sil.c
+++ b/drivers/ata/sata_sil.c
@@ -770,10 +770,10 @@ static int sil_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 		return rc;
 	host->iomap = pcim_iomap_table(pdev);
 
-	rc = pci_set_dma_mask(pdev, ATA_DMA_MASK);
+	rc = dma_set_mask(&pdev->dev, ATA_DMA_MASK);
 	if (rc)
 		return rc;
-	rc = pci_set_consistent_dma_mask(pdev, ATA_DMA_MASK);
+	rc = dma_set_coherent_mask(&pdev->dev, ATA_DMA_MASK);
 	if (rc)
 		return rc;
 
diff --git a/drivers/ata/sata_sil24.c b/drivers/ata/sata_sil24.c
index ba2667fa0528..4b1995e2d044 100644
--- a/drivers/ata/sata_sil24.c
+++ b/drivers/ata/sata_sil24.c
@@ -246,7 +246,7 @@ enum {
 	/* host flags */
 	SIL24_COMMON_FLAGS	= ATA_FLAG_SATA | ATA_FLAG_PIO_DMA |
 				  ATA_FLAG_NCQ | ATA_FLAG_ACPI_SATA |
-				  ATA_FLAG_AN | ATA_FLAG_PMP | ATA_FLAG_LOWTAG,
+				  ATA_FLAG_AN | ATA_FLAG_PMP,
 	SIL24_FLAG_PCIX_IRQ_WOC	= (1 << 24), /* IRQ loss errata on PCI-X */
 
 	IRQ_STAT_4PORTS		= 0xf,
@@ -1312,10 +1312,10 @@ static int sil24_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 	host->iomap = iomap;
 
 	/* configure and activate the device */
-	if (!pci_set_dma_mask(pdev, DMA_BIT_MASK(64))) {
-		rc = pci_set_consistent_dma_mask(pdev, DMA_BIT_MASK(64));
+	if (!dma_set_mask(&pdev->dev, DMA_BIT_MASK(64))) {
+		rc = dma_set_coherent_mask(&pdev->dev, DMA_BIT_MASK(64));
 		if (rc) {
-			rc = pci_set_consistent_dma_mask(pdev, DMA_BIT_MASK(32));
+			rc = dma_set_coherent_mask(&pdev->dev, DMA_BIT_MASK(32));
 			if (rc) {
 				dev_err(&pdev->dev,
 					"64-bit DMA enable failed\n");
@@ -1323,12 +1323,12 @@ static int sil24_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 			}
 		}
 	} else {
-		rc = pci_set_dma_mask(pdev, DMA_BIT_MASK(32));
+		rc = dma_set_mask(&pdev->dev, DMA_BIT_MASK(32));
 		if (rc) {
 			dev_err(&pdev->dev, "32-bit DMA enable failed\n");
 			return rc;
 		}
-		rc = pci_set_consistent_dma_mask(pdev, DMA_BIT_MASK(32));
+		rc = dma_set_coherent_mask(&pdev->dev, DMA_BIT_MASK(32));
 		if (rc) {
 			dev_err(&pdev->dev,
 				"32-bit consistent DMA enable failed\n");
diff --git a/drivers/ata/sata_svw.c b/drivers/ata/sata_svw.c
index c630fa812624..ff8307b30ff0 100644
--- a/drivers/ata/sata_svw.c
+++ b/drivers/ata/sata_svw.c
@@ -496,10 +496,10 @@ static int k2_sata_init_one(struct pci_dev *pdev, const struct pci_device_id *en
 		ata_port_pbar_desc(ap, 5, offset, "port");
 	}
 
-	rc = pci_set_dma_mask(pdev, ATA_DMA_MASK);
+	rc = dma_set_mask(&pdev->dev, ATA_DMA_MASK);
 	if (rc)
 		return rc;
-	rc = pci_set_consistent_dma_mask(pdev, ATA_DMA_MASK);
+	rc = dma_set_coherent_mask(&pdev->dev, ATA_DMA_MASK);
 	if (rc)
 		return rc;
 
diff --git a/drivers/ata/sata_sx4.c b/drivers/ata/sata_sx4.c
index 39b5de60a1f9..3a18a8a719b4 100644
--- a/drivers/ata/sata_sx4.c
+++ b/drivers/ata/sata_sx4.c
@@ -1476,10 +1476,10 @@ static int pdc_sata_init_one(struct pci_dev *pdev,
 	}
 
 	/* configure and activate */
-	rc = pci_set_dma_mask(pdev, ATA_DMA_MASK);
+	rc = dma_set_mask(&pdev->dev, ATA_DMA_MASK);
 	if (rc)
 		return rc;
-	rc = pci_set_consistent_dma_mask(pdev, ATA_DMA_MASK);
+	rc = dma_set_coherent_mask(&pdev->dev, ATA_DMA_MASK);
 	if (rc)
 		return rc;
 
diff --git a/drivers/ata/sata_via.c b/drivers/ata/sata_via.c
index 47bf89464cef..17d31fc009ab 100644
--- a/drivers/ata/sata_via.c
+++ b/drivers/ata/sata_via.c
@@ -502,10 +502,10 @@ static int vt6421_prepare_host(struct pci_dev *pdev, struct ata_host **r_host)
 	for (i = 0; i < host->n_ports; i++)
 		vt6421_init_addrs(host->ports[i]);
 
-	rc = pci_set_dma_mask(pdev, ATA_DMA_MASK);
+	rc = dma_set_mask(&pdev->dev, ATA_DMA_MASK);
 	if (rc)
 		return rc;
-	rc = pci_set_consistent_dma_mask(pdev, ATA_DMA_MASK);
+	rc = dma_set_coherent_mask(&pdev->dev, ATA_DMA_MASK);
 	if (rc)
 		return rc;
 
diff --git a/drivers/ata/sata_vsc.c b/drivers/ata/sata_vsc.c
index 29e847aac34b..183eb52085df 100644
--- a/drivers/ata/sata_vsc.c
+++ b/drivers/ata/sata_vsc.c
@@ -387,10 +387,10 @@ static int vsc_sata_init_one(struct pci_dev *pdev,
 	/*
 	 * Use 32 bit DMA mask, because 64 bit address support is poor.
 	 */
-	rc = pci_set_dma_mask(pdev, DMA_BIT_MASK(32));
+	rc = dma_set_mask(&pdev->dev, DMA_BIT_MASK(32));
 	if (rc)
 		return rc;
-	rc = pci_set_consistent_dma_mask(pdev, DMA_BIT_MASK(32));
+	rc = dma_set_coherent_mask(&pdev->dev, DMA_BIT_MASK(32));
 	if (rc)
 		return rc;
 
diff --git a/drivers/base/attribute_container.c b/drivers/base/attribute_container.c
index 3ead3af4be61..2ba4cac080c5 100644
--- a/drivers/base/attribute_container.c
+++ b/drivers/base/attribute_container.c
@@ -94,6 +94,7 @@ int
 attribute_container_unregister(struct attribute_container *cont)
 {
 	int retval = -EBUSY;
+
 	mutex_lock(&attribute_container_mutex);
 	spin_lock(&cont->containers.k_lock);
 	if (!list_empty(&cont->containers.k_list))
@@ -349,6 +350,7 @@ int
 attribute_container_add_class_device(struct device *classdev)
 {
 	int error = device_add(classdev);
+
 	if (error)
 		return error;
 	return attribute_container_add_attrs(classdev);
diff --git a/drivers/base/bus.c b/drivers/base/bus.c
index 876bae5ade33..79bc203f51ef 100644
--- a/drivers/base/bus.c
+++ b/drivers/base/bus.c
@@ -515,11 +515,11 @@ int bus_add_device(struct device *dev)
 			goto out_put;
 		error = device_add_groups(dev, bus->dev_groups);
 		if (error)
-			goto out_groups;
+			goto out_id;
 		error = sysfs_create_link(&bus->p->devices_kset->kobj,
 						&dev->kobj, dev_name(dev));
 		if (error)
-			goto out_id;
+			goto out_groups;
 		error = sysfs_create_link(&dev->kobj,
 				&dev->bus->p->subsys.kobj, "subsystem");
 		if (error)
diff --git a/drivers/base/cacheinfo.c b/drivers/base/cacheinfo.c
index 6e64563361f0..9c2ba1c97c42 100644
--- a/drivers/base/cacheinfo.c
+++ b/drivers/base/cacheinfo.c
@@ -62,15 +62,21 @@ static int cache_setup_of_node(unsigned int cpu)
 		return -ENOENT;
 	}
 
-	while (np && index < cache_leaves(cpu)) {
+	while (index < cache_leaves(cpu)) {
 		this_leaf = this_cpu_ci->info_list + index;
 		if (this_leaf->level != 1)
 			np = of_find_next_cache_node(np);
 		else
 			np = of_node_get(np);/* cpu node itself */
+		if (!np)
+			break;
 		this_leaf->of_node = np;
 		index++;
 	}
+
+	if (index != cache_leaves(cpu)) /* not all OF nodes populated */
+		return -ENOENT;
+
 	return 0;
 }
 
@@ -189,8 +195,11 @@ static int detect_cache_attributes(unsigned int cpu)
 	 * will be set up here only if they are not populated already
 	 */
 	ret = cache_shared_cpu_map_setup(cpu);
-	if (ret)
+	if (ret) {
+		pr_warn("Unable to detect cache hierarcy from DT for CPU %d\n",
+			cpu);
 		goto free_ci;
+	}
 	return 0;
 
 free_ci:
diff --git a/drivers/base/class.c b/drivers/base/class.c
index f96f70419a78..6e810881e48b 100644
--- a/drivers/base/class.c
+++ b/drivers/base/class.c
@@ -90,6 +90,7 @@ int class_create_file_ns(struct class *cls, const struct class_attribute *attr,
 			 const void *ns)
 {
 	int error;
+
 	if (cls)
 		error = sysfs_create_file_ns(&cls->p->subsys.kobj,
 					     &attr->attr, ns);
@@ -488,6 +489,7 @@ ssize_t show_class_attr_string(struct class *class,
 			       struct class_attribute *attr, char *buf)
 {
 	struct class_attribute_string *cs;
+
 	cs = container_of(attr, struct class_attribute_string, attr);
 	return snprintf(buf, PAGE_SIZE, "%s\n", cs->str);
 }
diff --git a/drivers/base/core.c b/drivers/base/core.c
index 07304a3b9ee2..cadf165651d8 100644
--- a/drivers/base/core.c
+++ b/drivers/base/core.c
@@ -805,8 +805,16 @@ static void cleanup_device_parent(struct device *dev)
 
 static int device_add_class_symlinks(struct device *dev)
 {
+	struct device_node *of_node = dev_of_node(dev);
 	int error;
 
+	if (of_node) {
+		error = sysfs_create_link(&dev->kobj, &of_node->kobj,"of_node");
+		if (error)
+			dev_warn(dev, "Error %d creating of_node link\n",error);
+		/* An error here doesn't warrant bringing down the device */
+	}
+
 	if (!dev->class)
 		return 0;
 
@@ -814,7 +822,7 @@ static int device_add_class_symlinks(struct device *dev)
 				  &dev->class->p->subsys.kobj,
 				  "subsystem");
 	if (error)
-		goto out;
+		goto out_devnode;
 
 	if (dev->parent && device_is_not_partition(dev)) {
 		error = sysfs_create_link(&dev->kobj, &dev->parent->kobj,
@@ -842,12 +850,16 @@ out_device:
 
 out_subsys:
 	sysfs_remove_link(&dev->kobj, "subsystem");
-out:
+out_devnode:
+	sysfs_remove_link(&dev->kobj, "of_node");
 	return error;
 }
 
 static void device_remove_class_symlinks(struct device *dev)
 {
+	if (dev_of_node(dev))
+		sysfs_remove_link(&dev->kobj, "of_node");
+
 	if (!dev->class)
 		return;
 
@@ -1095,8 +1107,7 @@ done:
 	kobject_del(&dev->kobj);
  Error:
 	cleanup_device_parent(dev);
-	if (parent)
-		put_device(parent);
+	put_device(parent);
 name_error:
 	kfree(dev->p);
 	dev->p = NULL;
diff --git a/drivers/base/dd.c b/drivers/base/dd.c
index cdc779cf79a3..49a4a12fafef 100644
--- a/drivers/base/dd.c
+++ b/drivers/base/dd.c
@@ -320,21 +320,25 @@ probe_failed:
 	dev->driver = NULL;
 	dev_set_drvdata(dev, NULL);
 
-	if (ret == -EPROBE_DEFER) {
+	switch (ret) {
+	case -EPROBE_DEFER:
 		/* Driver requested deferred probing */
-		dev_info(dev, "Driver %s requests probe deferral\n", drv->name);
+		dev_dbg(dev, "Driver %s requests probe deferral\n", drv->name);
 		driver_deferred_probe_add(dev);
 		/* Did a trigger occur while probing? Need to re-trigger if yes */
 		if (local_trigger_count != atomic_read(&deferred_trigger_count))
 			driver_deferred_probe_trigger();
-	} else if (ret != -ENODEV && ret != -ENXIO) {
+		break;
+	case -ENODEV:
+	case -ENXIO:
+		pr_debug("%s: probe of %s rejects match %d\n",
+			 drv->name, dev_name(dev), ret);
+		break;
+	default:
 		/* driver matched but the probe failed */
 		printk(KERN_WARNING
 		       "%s: probe of %s failed with error %d\n",
 		       drv->name, dev_name(dev), ret);
-	} else {
-		pr_debug("%s: probe of %s rejects match %d\n",
-		       drv->name, dev_name(dev), ret);
 	}
 	/*
 	 * Ignore errors returned by ->probe so that the next driver can try
diff --git a/drivers/base/dma-mapping.c b/drivers/base/dma-mapping.c
index 9e8bbdd470ca..d95c5971c225 100644
--- a/drivers/base/dma-mapping.c
+++ b/drivers/base/dma-mapping.c
@@ -62,7 +62,7 @@ static int dmam_match(struct device *dev, void *res, void *match_data)
  * RETURNS:
  * Pointer to allocated memory on success, NULL on failure.
  */
-void * dmam_alloc_coherent(struct device *dev, size_t size,
+void *dmam_alloc_coherent(struct device *dev, size_t size,
 			   dma_addr_t *dma_handle, gfp_t gfp)
 {
 	struct dma_devres *dr;
diff --git a/drivers/base/driver.c b/drivers/base/driver.c
index 9e29943e56ca..4eabfe28d2b3 100644
--- a/drivers/base/driver.c
+++ b/drivers/base/driver.c
@@ -103,6 +103,7 @@ int driver_create_file(struct device_driver *drv,
 		       const struct driver_attribute *attr)
 {
 	int error;
+
 	if (drv)
 		error = sysfs_create_file(&drv->p->kobj, &attr->attr);
 	else
diff --git a/drivers/base/firmware_class.c b/drivers/base/firmware_class.c
index 6c5c9edf5ff6..171841ad1008 100644
--- a/drivers/base/firmware_class.c
+++ b/drivers/base/firmware_class.c
@@ -181,7 +181,7 @@ static struct firmware_buf *__allocate_fw_buf(const char *fw_name,
 {
 	struct firmware_buf *buf;
 
-	buf = kzalloc(sizeof(*buf) + strlen(fw_name) + 1 , GFP_ATOMIC);
+	buf = kzalloc(sizeof(*buf) + strlen(fw_name) + 1, GFP_ATOMIC);
 
 	if (!buf)
 		return buf;
@@ -835,6 +835,26 @@ static struct bin_attribute firmware_attr_data = {
 	.write = firmware_data_write,
 };
 
+static struct attribute *fw_dev_attrs[] = {
+	&dev_attr_loading.attr,
+	NULL
+};
+
+static struct bin_attribute *fw_dev_bin_attrs[] = {
+	&firmware_attr_data,
+	NULL
+};
+
+static const struct attribute_group fw_dev_attr_group = {
+	.attrs = fw_dev_attrs,
+	.bin_attrs = fw_dev_bin_attrs,
+};
+
+static const struct attribute_group *fw_dev_attr_groups[] = {
+	&fw_dev_attr_group,
+	NULL
+};
+
 static struct firmware_priv *
 fw_create_instance(struct firmware *firmware, const char *fw_name,
 		   struct device *device, unsigned int opt_flags)
@@ -856,6 +876,7 @@ fw_create_instance(struct firmware *firmware, const char *fw_name,
 	dev_set_name(f_dev, "%s", fw_name);
 	f_dev->parent = device;
 	f_dev->class = &firmware_class;
+	f_dev->groups = fw_dev_attr_groups;
 exit:
 	return fw_priv;
 }
@@ -879,25 +900,10 @@ static int _request_firmware_load(struct firmware_priv *fw_priv,
 		goto err_put_dev;
 	}
 
-	retval = device_create_bin_file(f_dev, &firmware_attr_data);
-	if (retval) {
-		dev_err(f_dev, "%s: sysfs_create_bin_file failed\n", __func__);
-		goto err_del_dev;
-	}
-
 	mutex_lock(&fw_lock);
 	list_add(&buf->pending_list, &pending_fw_head);
 	mutex_unlock(&fw_lock);
 
-	retval = device_create_file(f_dev, &dev_attr_loading);
-	if (retval) {
-		mutex_lock(&fw_lock);
-		list_del_init(&buf->pending_list);
-		mutex_unlock(&fw_lock);
-		dev_err(f_dev, "%s: device_create_file failed\n", __func__);
-		goto err_del_bin_attr;
-	}
-
 	if (opt_flags & FW_OPT_UEVENT) {
 		buf->need_uevent = true;
 		dev_set_uevent_suppress(f_dev, false);
@@ -913,6 +919,8 @@ static int _request_firmware_load(struct firmware_priv *fw_priv,
 		mutex_lock(&fw_lock);
 		fw_load_abort(fw_priv);
 		mutex_unlock(&fw_lock);
+	} else if (retval > 0) {
+		retval = 0;
 	}
 
 	if (is_fw_load_aborted(buf))
@@ -920,10 +928,6 @@ static int _request_firmware_load(struct firmware_priv *fw_priv,
 	else if (!buf->data)
 		retval = -ENOMEM;
 
-	device_remove_file(f_dev, &dev_attr_loading);
-err_del_bin_attr:
-	device_remove_bin_file(f_dev, &firmware_attr_data);
-err_del_dev:
 	device_del(f_dev);
 err_put_dev:
 	put_device(f_dev);
@@ -1168,7 +1172,7 @@ _request_firmware(const struct firmware **firmware_p, const char *name,
  **/
 int
 request_firmware(const struct firmware **firmware_p, const char *name,
-                 struct device *device)
+		 struct device *device)
 {
 	int ret;
 
@@ -1196,6 +1200,7 @@ int request_firmware_direct(const struct firmware **firmware_p,
 			    const char *name, struct device *device)
 {
 	int ret;
+
 	__module_get(THIS_MODULE);
 	ret = _request_firmware(firmware_p, name, device,
 				FW_OPT_UEVENT | FW_OPT_NO_WARN);
@@ -1276,7 +1281,7 @@ request_firmware_nowait(
 {
 	struct firmware_work *fw_work;
 
-	fw_work = kzalloc(sizeof (struct firmware_work), gfp);
+	fw_work = kzalloc(sizeof(struct firmware_work), gfp);
 	if (!fw_work)
 		return -ENOMEM;
 
diff --git a/drivers/base/map.c b/drivers/base/map.c
index e87017f36853..c1d38234d725 100644
--- a/drivers/base/map.c
+++ b/drivers/base/map.c
@@ -41,8 +41,7 @@ int kobj_map(struct kobj_map *domain, dev_t dev, unsigned long range,
 	if (n > 255)
 		n = 255;
 
-	p = kmalloc(sizeof(struct probe) * n, GFP_KERNEL);
-
+	p = kmalloc_array(n, sizeof(struct probe), GFP_KERNEL);
 	if (p == NULL)
 		return -ENOMEM;
 
diff --git a/drivers/base/memory.c b/drivers/base/memory.c
index 85be040a21c8..af9c911cd6b5 100644
--- a/drivers/base/memory.c
+++ b/drivers/base/memory.c
@@ -52,13 +52,13 @@ static BLOCKING_NOTIFIER_HEAD(memory_chain);
 
 int register_memory_notifier(struct notifier_block *nb)
 {
-        return blocking_notifier_chain_register(&memory_chain, nb);
+	return blocking_notifier_chain_register(&memory_chain, nb);
 }
 EXPORT_SYMBOL(register_memory_notifier);
 
 void unregister_memory_notifier(struct notifier_block *nb)
 {
-        blocking_notifier_chain_unregister(&memory_chain, nb);
+	blocking_notifier_chain_unregister(&memory_chain, nb);
 }
 EXPORT_SYMBOL(unregister_memory_notifier);
 
@@ -152,20 +152,20 @@ static ssize_t show_mem_state(struct device *dev,
 	 * so that they're not open-coded
 	 */
 	switch (mem->state) {
-		case MEM_ONLINE:
-			len = sprintf(buf, "online\n");
-			break;
-		case MEM_OFFLINE:
-			len = sprintf(buf, "offline\n");
-			break;
-		case MEM_GOING_OFFLINE:
-			len = sprintf(buf, "going-offline\n");
-			break;
-		default:
-			len = sprintf(buf, "ERROR-UNKNOWN-%ld\n",
-					mem->state);
-			WARN_ON(1);
-			break;
+	case MEM_ONLINE:
+		len = sprintf(buf, "online\n");
+		break;
+	case MEM_OFFLINE:
+		len = sprintf(buf, "offline\n");
+		break;
+	case MEM_GOING_OFFLINE:
+		len = sprintf(buf, "going-offline\n");
+		break;
+	default:
+		len = sprintf(buf, "ERROR-UNKNOWN-%ld\n",
+				mem->state);
+		WARN_ON(1);
+		break;
 	}
 
 	return len;
@@ -232,19 +232,19 @@ memory_block_action(unsigned long phys_index, unsigned long action, int online_t
 	first_page = pfn_to_page(start_pfn);
 
 	switch (action) {
-		case MEM_ONLINE:
-			if (!pages_correctly_reserved(start_pfn))
-				return -EBUSY;
-
-			ret = online_pages(start_pfn, nr_pages, online_type);
-			break;
-		case MEM_OFFLINE:
-			ret = offline_pages(start_pfn, nr_pages);
-			break;
-		default:
-			WARN(1, KERN_WARNING "%s(%ld, %ld) unknown action: "
-			     "%ld\n", __func__, phys_index, action, action);
-			ret = -EINVAL;
+	case MEM_ONLINE:
+		if (!pages_correctly_reserved(start_pfn))
+			return -EBUSY;
+
+		ret = online_pages(start_pfn, nr_pages, online_type);
+		break;
+	case MEM_OFFLINE:
+		ret = offline_pages(start_pfn, nr_pages);
+		break;
+	default:
+		WARN(1, KERN_WARNING "%s(%ld, %ld) unknown action: "
+		     "%ld\n", __func__, phys_index, action, action);
+		ret = -EINVAL;
 	}
 
 	return ret;
diff --git a/drivers/base/node.c b/drivers/base/node.c
index 36fabe43cd44..a2aa65b4215d 100644
--- a/drivers/base/node.c
+++ b/drivers/base/node.c
@@ -180,7 +180,7 @@ static ssize_t node_read_vmstat(struct device *dev,
 static DEVICE_ATTR(vmstat, S_IRUGO, node_read_vmstat, NULL);
 
 static ssize_t node_read_distance(struct device *dev,
-			struct device_attribute *attr, char * buf)
+			struct device_attribute *attr, char *buf)
 {
 	int nid = dev->id;
 	int len = 0;
@@ -200,6 +200,17 @@ static ssize_t node_read_distance(struct device *dev,
 }
 static DEVICE_ATTR(distance, S_IRUGO, node_read_distance, NULL);
 
+static struct attribute *node_dev_attrs[] = {
+	&dev_attr_cpumap.attr,
+	&dev_attr_cpulist.attr,
+	&dev_attr_meminfo.attr,
+	&dev_attr_numastat.attr,
+	&dev_attr_distance.attr,
+	&dev_attr_vmstat.attr,
+	NULL
+};
+ATTRIBUTE_GROUPS(node_dev);
+
 #ifdef CONFIG_HUGETLBFS
 /*
  * hugetlbfs per node attributes registration interface:
@@ -273,16 +284,10 @@ static int register_node(struct node *node, int num, struct node *parent)
 	node->dev.id = num;
 	node->dev.bus = &node_subsys;
 	node->dev.release = node_device_release;
+	node->dev.groups = node_dev_groups;
 	error = device_register(&node->dev);
 
 	if (!error){
-		device_create_file(&node->dev, &dev_attr_cpumap);
-		device_create_file(&node->dev, &dev_attr_cpulist);
-		device_create_file(&node->dev, &dev_attr_meminfo);
-		device_create_file(&node->dev, &dev_attr_numastat);
-		device_create_file(&node->dev, &dev_attr_distance);
-		device_create_file(&node->dev, &dev_attr_vmstat);
-
 		hugetlb_register_node(node);
 
 		compaction_register_node(node);
@@ -299,13 +304,6 @@ static int register_node(struct node *node, int num, struct node *parent)
  */
 void unregister_node(struct node *node)
 {
-	device_remove_file(&node->dev, &dev_attr_cpumap);
-	device_remove_file(&node->dev, &dev_attr_cpulist);
-	device_remove_file(&node->dev, &dev_attr_meminfo);
-	device_remove_file(&node->dev, &dev_attr_numastat);
-	device_remove_file(&node->dev, &dev_attr_distance);
-	device_remove_file(&node->dev, &dev_attr_vmstat);
-
 	hugetlb_unregister_node(node);		/* no-op, if memoryless node */
 
 	device_unregister(&node->dev);
diff --git a/drivers/base/platform.c b/drivers/base/platform.c
index 9421fed40905..e68ab79df28b 100644
--- a/drivers/base/platform.c
+++ b/drivers/base/platform.c
@@ -101,6 +101,15 @@ int platform_get_irq(struct platform_device *dev, unsigned int num)
 	}
 
 	r = platform_get_resource(dev, IORESOURCE_IRQ, num);
+	/*
+	 * The resources may pass trigger flags to the irqs that need
+	 * to be set up. It so happens that the trigger flags for
+	 * IORESOURCE_BITS correspond 1-to-1 to the IRQF_TRIGGER*
+	 * settings.
+	 */
+	if (r && r->flags & IORESOURCE_BITS)
+		irqd_set_trigger_type(irq_get_irq_data(r->start),
+				      r->flags & IORESOURCE_BITS);
 
 	return r ? r->start : -ENXIO;
 #endif
diff --git a/drivers/base/power/domain.c b/drivers/base/power/domain.c
index ba4abbe4693c..45937f88e77c 100644
--- a/drivers/base/power/domain.c
+++ b/drivers/base/power/domain.c
@@ -2242,7 +2242,7 @@ static void rtpm_status_str(struct seq_file *s, struct device *dev)
 }
 
 static int pm_genpd_summary_one(struct seq_file *s,
-		struct generic_pm_domain *gpd)
+				struct generic_pm_domain *genpd)
 {
 	static const char * const status_lookup[] = {
 		[GPD_STATE_ACTIVE] = "on",
@@ -2256,26 +2256,26 @@ static int pm_genpd_summary_one(struct seq_file *s,
 	struct gpd_link *link;
 	int ret;
 
-	ret = mutex_lock_interruptible(&gpd->lock);
+	ret = mutex_lock_interruptible(&genpd->lock);
 	if (ret)
 		return -ERESTARTSYS;
 
-	if (WARN_ON(gpd->status >= ARRAY_SIZE(status_lookup)))
+	if (WARN_ON(genpd->status >= ARRAY_SIZE(status_lookup)))
 		goto exit;
-	seq_printf(s, "%-30s  %-15s  ", gpd->name, status_lookup[gpd->status]);
+	seq_printf(s, "%-30s  %-15s  ", genpd->name, status_lookup[genpd->status]);
 
 	/*
 	 * Modifications on the list require holding locks on both
 	 * master and slave, so we are safe.
-	 * Also gpd->name is immutable.
+	 * Also genpd->name is immutable.
 	 */
-	list_for_each_entry(link, &gpd->master_links, master_node) {
+	list_for_each_entry(link, &genpd->master_links, master_node) {
 		seq_printf(s, "%s", link->slave->name);
-		if (!list_is_last(&link->master_node, &gpd->master_links))
+		if (!list_is_last(&link->master_node, &genpd->master_links))
 			seq_puts(s, ", ");
 	}
 
-	list_for_each_entry(pm_data, &gpd->dev_list, list_node) {
+	list_for_each_entry(pm_data, &genpd->dev_list, list_node) {
 		kobj_path = kobject_get_path(&pm_data->dev->kobj, GFP_KERNEL);
 		if (kobj_path == NULL)
 			continue;
@@ -2287,14 +2287,14 @@ static int pm_genpd_summary_one(struct seq_file *s,
 
 	seq_puts(s, "\n");
 exit:
-	mutex_unlock(&gpd->lock);
+	mutex_unlock(&genpd->lock);
 
 	return 0;
 }
 
 static int pm_genpd_summary_show(struct seq_file *s, void *data)
 {
-	struct generic_pm_domain *gpd;
+	struct generic_pm_domain *genpd;
 	int ret = 0;
 
 	seq_puts(s, "    domain                      status         slaves\n");
@@ -2305,8 +2305,8 @@ static int pm_genpd_summary_show(struct seq_file *s, void *data)
 	if (ret)
 		return -ERESTARTSYS;
 
-	list_for_each_entry(gpd, &gpd_list, gpd_list_node) {
-		ret = pm_genpd_summary_one(s, gpd);
+	list_for_each_entry(genpd, &gpd_list, gpd_list_node) {
+		ret = pm_genpd_summary_one(s, genpd);
 		if (ret)
 			break;
 	}
diff --git a/drivers/base/power/wakeup.c b/drivers/base/power/wakeup.c
index c2744b30d5d9..aab7158d2afe 100644
--- a/drivers/base/power/wakeup.c
+++ b/drivers/base/power/wakeup.c
@@ -730,6 +730,7 @@ void pm_system_wakeup(void)
 	pm_abort_suspend = true;
 	freeze_wake();
 }
+EXPORT_SYMBOL_GPL(pm_system_wakeup);
 
 void pm_wakeup_clear(void)
 {
diff --git a/drivers/base/property.c b/drivers/base/property.c
index c45845874d4f..423df593f262 100644
--- a/drivers/base/property.c
+++ b/drivers/base/property.c
@@ -365,7 +365,7 @@ int fwnode_property_read_string(struct fwnode_handle *fwnode,
 				const char *propname, const char **val)
 {
 	if (is_of_node(fwnode))
-		return of_property_read_string(of_node(fwnode),propname, val);
+		return of_property_read_string(of_node(fwnode), propname, val);
 	else if (is_acpi_node(fwnode))
 		return acpi_dev_prop_read(acpi_node(fwnode), propname,
 					  DEV_PROP_STRING, val, 1);
diff --git a/drivers/base/regmap/Makefile b/drivers/base/regmap/Makefile
index 0a533653ef3b..609e4c84f485 100644
--- a/drivers/base/regmap/Makefile
+++ b/drivers/base/regmap/Makefile
@@ -1,3 +1,6 @@
+# For include/trace/define_trace.h to include trace.h
+CFLAGS_regmap.o := -I$(src)
+
 obj-$(CONFIG_REGMAP) += regmap.o regcache.o
 obj-$(CONFIG_REGMAP) += regcache-rbtree.o regcache-lzo.o regcache-flat.o
 obj-$(CONFIG_DEBUG_FS) += regmap-debugfs.o
diff --git a/drivers/base/regmap/internal.h b/drivers/base/regmap/internal.h
index beb8b27d4621..a13587b5c2be 100644
--- a/drivers/base/regmap/internal.h
+++ b/drivers/base/regmap/internal.h
@@ -243,4 +243,12 @@ extern struct regcache_ops regcache_rbtree_ops;
 extern struct regcache_ops regcache_lzo_ops;
 extern struct regcache_ops regcache_flat_ops;
 
+static inline const char *regmap_name(const struct regmap *map)
+{
+	if (map->dev)
+		return dev_name(map->dev);
+
+	return map->name;
+}
+
 #endif
diff --git a/drivers/base/regmap/regcache-rbtree.c b/drivers/base/regmap/regcache-rbtree.c
index d453a2c98ad0..81751a49d8bf 100644
--- a/drivers/base/regmap/regcache-rbtree.c
+++ b/drivers/base/regmap/regcache-rbtree.c
@@ -307,7 +307,7 @@ static int regcache_rbtree_insert_to_block(struct regmap *map,
 	if (pos == 0) {
 		memmove(blk + offset * map->cache_word_size,
 			blk, rbnode->blklen * map->cache_word_size);
-		bitmap_shift_right(present, present, offset, blklen);
+		bitmap_shift_left(present, present, offset, blklen);
 	}
 
 	/* update the rbnode block, its size and the base register */
diff --git a/drivers/base/regmap/regcache.c b/drivers/base/regmap/regcache.c
index f373c35f9e1d..7eb7b3b98794 100644
--- a/drivers/base/regmap/regcache.c
+++ b/drivers/base/regmap/regcache.c
@@ -15,8 +15,8 @@
 #include <linux/export.h>
 #include <linux/slab.h>
 #include <linux/sort.h>
-#include <trace/events/regmap.h>
 
+#include "trace.h"
 #include "internal.h"
 
 static const struct regcache_ops *cache_types[] = {
@@ -218,7 +218,7 @@ int regcache_read(struct regmap *map,
 		ret = map->cache_ops->read(map, reg, value);
 
 		if (ret == 0)
-			trace_regmap_reg_read_cache(map->dev, reg, *value);
+			trace_regmap_reg_read_cache(map, reg, *value);
 
 		return ret;
 	}
@@ -311,7 +311,7 @@ int regcache_sync(struct regmap *map)
 	dev_dbg(map->dev, "Syncing %s cache\n",
 		map->cache_ops->name);
 	name = map->cache_ops->name;
-	trace_regcache_sync(map->dev, name, "start");
+	trace_regcache_sync(map, name, "start");
 
 	if (!map->cache_dirty)
 		goto out;
@@ -346,7 +346,7 @@ out:
 
 	regmap_async_complete(map);
 
-	trace_regcache_sync(map->dev, name, "stop");
+	trace_regcache_sync(map, name, "stop");
 
 	return ret;
 }
@@ -381,7 +381,7 @@ int regcache_sync_region(struct regmap *map, unsigned int min,
 	name = map->cache_ops->name;
 	dev_dbg(map->dev, "Syncing %s cache from %d-%d\n", name, min, max);
 
-	trace_regcache_sync(map->dev, name, "start region");
+	trace_regcache_sync(map, name, "start region");
 
 	if (!map->cache_dirty)
 		goto out;
@@ -401,7 +401,7 @@ out:
 
 	regmap_async_complete(map);
 
-	trace_regcache_sync(map->dev, name, "stop region");
+	trace_regcache_sync(map, name, "stop region");
 
 	return ret;
 }
@@ -428,7 +428,7 @@ int regcache_drop_region(struct regmap *map, unsigned int min,
 
 	map->lock(map->lock_arg);
 
-	trace_regcache_drop_region(map->dev, min, max);
+	trace_regcache_drop_region(map, min, max);
 
 	ret = map->cache_ops->drop(map, min, max);
 
@@ -455,7 +455,7 @@ void regcache_cache_only(struct regmap *map, bool enable)
 	map->lock(map->lock_arg);
 	WARN_ON(map->cache_bypass && enable);
 	map->cache_only = enable;
-	trace_regmap_cache_only(map->dev, enable);
+	trace_regmap_cache_only(map, enable);
 	map->unlock(map->lock_arg);
 }
 EXPORT_SYMBOL_GPL(regcache_cache_only);
@@ -493,7 +493,7 @@ void regcache_cache_bypass(struct regmap *map, bool enable)
 	map->lock(map->lock_arg);
 	WARN_ON(map->cache_only && enable);
 	map->cache_bypass = enable;
-	trace_regmap_cache_bypass(map->dev, enable);
+	trace_regmap_cache_bypass(map, enable);
 	map->unlock(map->lock_arg);
 }
 EXPORT_SYMBOL_GPL(regcache_cache_bypass);
@@ -608,7 +608,8 @@ static int regcache_sync_block_single(struct regmap *map, void *block,
 	for (i = start; i < end; i++) {
 		regtmp = block_base + (i * map->reg_stride);
 
-		if (!regcache_reg_present(cache_present, i))
+		if (!regcache_reg_present(cache_present, i) ||
+		    !regmap_writeable(map, regtmp))
 			continue;
 
 		val = regcache_get_val(map, block, i);
@@ -677,7 +678,8 @@ static int regcache_sync_block_raw(struct regmap *map, void *block,
 	for (i = start; i < end; i++) {
 		regtmp = block_base + (i * map->reg_stride);
 
-		if (!regcache_reg_present(cache_present, i)) {
+		if (!regcache_reg_present(cache_present, i) ||
+		    !regmap_writeable(map, regtmp)) {
 			ret = regcache_sync_block_raw_flush(map, &data,
 							    base, regtmp);
 			if (ret != 0)
diff --git a/drivers/base/regmap/regmap-irq.c b/drivers/base/regmap/regmap-irq.c
index 6299a50a5960..a6c3f75b4b01 100644
--- a/drivers/base/regmap/regmap-irq.c
+++ b/drivers/base/regmap/regmap-irq.c
@@ -499,7 +499,8 @@ int regmap_add_irq_chip(struct regmap *map, int irq, int irq_flags,
 		goto err_alloc;
 	}
 
-	ret = request_threaded_irq(irq, NULL, regmap_irq_thread, irq_flags,
+	ret = request_threaded_irq(irq, NULL, regmap_irq_thread,
+				   irq_flags | IRQF_ONESHOT,
 				   chip->name, d);
 	if (ret != 0) {
 		dev_err(map->dev, "Failed to request IRQ %d for %s: %d\n",
diff --git a/drivers/base/regmap/regmap.c b/drivers/base/regmap/regmap.c
index f99b098ddabf..6273ff072f3e 100644
--- a/drivers/base/regmap/regmap.c
+++ b/drivers/base/regmap/regmap.c
@@ -20,7 +20,7 @@
 #include <linux/sched.h>
 
 #define CREATE_TRACE_POINTS
-#include <trace/events/regmap.h>
+#include "trace.h"
 
 #include "internal.h"
 
@@ -1281,7 +1281,7 @@ int _regmap_raw_write(struct regmap *map, unsigned int reg,
 	if (map->async && map->bus->async_write) {
 		struct regmap_async *async;
 
-		trace_regmap_async_write_start(map->dev, reg, val_len);
+		trace_regmap_async_write_start(map, reg, val_len);
 
 		spin_lock_irqsave(&map->async_lock, flags);
 		async = list_first_entry_or_null(&map->async_free,
@@ -1339,8 +1339,7 @@ int _regmap_raw_write(struct regmap *map, unsigned int reg,
 		return ret;
 	}
 
-	trace_regmap_hw_write_start(map->dev, reg,
-				    val_len / map->format.val_bytes);
+	trace_regmap_hw_write_start(map, reg, val_len / map->format.val_bytes);
 
 	/* If we're doing a single register write we can probably just
 	 * send the work_buf directly, otherwise try to do a gather
@@ -1372,8 +1371,7 @@ int _regmap_raw_write(struct regmap *map, unsigned int reg,
 		kfree(buf);
 	}
 
-	trace_regmap_hw_write_done(map->dev, reg,
-				   val_len / map->format.val_bytes);
+	trace_regmap_hw_write_done(map, reg, val_len / map->format.val_bytes);
 
 	return ret;
 }
@@ -1407,12 +1405,12 @@ static int _regmap_bus_formatted_write(void *context, unsigned int reg,
 
 	map->format.format_write(map, reg, val);
 
-	trace_regmap_hw_write_start(map->dev, reg, 1);
+	trace_regmap_hw_write_start(map, reg, 1);
 
 	ret = map->bus->write(map->bus_context, map->work_buf,
 			      map->format.buf_size);
 
-	trace_regmap_hw_write_done(map->dev, reg, 1);
+	trace_regmap_hw_write_done(map, reg, 1);
 
 	return ret;
 }
@@ -1470,7 +1468,7 @@ int _regmap_write(struct regmap *map, unsigned int reg,
 		dev_info(map->dev, "%x <= %x\n", reg, val);
 #endif
 
-	trace_regmap_reg_write(map->dev, reg, val);
+	trace_regmap_reg_write(map, reg, val);
 
 	return map->reg_write(context, reg, val);
 }
@@ -1773,7 +1771,7 @@ static int _regmap_raw_multi_reg_write(struct regmap *map,
 	for (i = 0; i < num_regs; i++) {
 		int reg = regs[i].reg;
 		int val = regs[i].def;
-		trace_regmap_hw_write_start(map->dev, reg, 1);
+		trace_regmap_hw_write_start(map, reg, 1);
 		map->format.format_reg(u8, reg, map->reg_shift);
 		u8 += reg_bytes + pad_bytes;
 		map->format.format_val(u8, val, 0);
@@ -1788,7 +1786,7 @@ static int _regmap_raw_multi_reg_write(struct regmap *map,
 
 	for (i = 0; i < num_regs; i++) {
 		int reg = regs[i].reg;
-		trace_regmap_hw_write_done(map->dev, reg, 1);
+		trace_regmap_hw_write_done(map, reg, 1);
 	}
 	return ret;
 }
@@ -2059,15 +2057,13 @@ static int _regmap_raw_read(struct regmap *map, unsigned int reg, void *val,
 	 */
 	u8[0] |= map->read_flag_mask;
 
-	trace_regmap_hw_read_start(map->dev, reg,
-				   val_len / map->format.val_bytes);
+	trace_regmap_hw_read_start(map, reg, val_len / map->format.val_bytes);
 
 	ret = map->bus->read(map->bus_context, map->work_buf,
 			     map->format.reg_bytes + map->format.pad_bytes,
 			     val, val_len);
 
-	trace_regmap_hw_read_done(map->dev, reg,
-				  val_len / map->format.val_bytes);
+	trace_regmap_hw_read_done(map, reg, val_len / map->format.val_bytes);
 
 	return ret;
 }
@@ -2123,7 +2119,7 @@ static int _regmap_read(struct regmap *map, unsigned int reg,
 			dev_info(map->dev, "%x => %x\n", reg, *val);
 #endif
 
-		trace_regmap_reg_read(map->dev, reg, *val);
+		trace_regmap_reg_read(map, reg, *val);
 
 		if (!map->cache_bypass)
 			regcache_write(map, reg, *val);
@@ -2480,7 +2476,7 @@ void regmap_async_complete_cb(struct regmap_async *async, int ret)
 	struct regmap *map = async->map;
 	bool wake;
 
-	trace_regmap_async_io_complete(map->dev);
+	trace_regmap_async_io_complete(map);
 
 	spin_lock(&map->async_lock);
 	list_move(&async->list, &map->async_free);
@@ -2525,7 +2521,7 @@ int regmap_async_complete(struct regmap *map)
 	if (!map->bus || !map->bus->async_write)
 		return 0;
 
-	trace_regmap_async_complete_start(map->dev);
+	trace_regmap_async_complete_start(map);
 
 	wait_event(map->async_waitq, regmap_async_is_done(map));
 
@@ -2534,7 +2530,7 @@ int regmap_async_complete(struct regmap *map)
 	map->async_ret = 0;
 	spin_unlock_irqrestore(&map->async_lock, flags);
 
-	trace_regmap_async_complete_done(map->dev);
+	trace_regmap_async_complete_done(map);
 
 	return ret;
 }
diff --git a/drivers/base/regmap/trace.h b/drivers/base/regmap/trace.h
new file mode 100644
index 000000000000..64586a1c5a42
--- /dev/null
+++ b/drivers/base/regmap/trace.h
@@ -0,0 +1,257 @@
+#undef TRACE_SYSTEM
+#define TRACE_SYSTEM regmap
+
+#if !defined(_TRACE_REGMAP_H) || defined(TRACE_HEADER_MULTI_READ)
+#define _TRACE_REGMAP_H
+
+#include <linux/ktime.h>
+#include <linux/tracepoint.h>
+
+#include "internal.h"
+
+/*
+ * Log register events
+ */
+DECLARE_EVENT_CLASS(regmap_reg,
+
+	TP_PROTO(struct regmap *map, unsigned int reg,
+		 unsigned int val),
+
+	TP_ARGS(map, reg, val),
+
+	TP_STRUCT__entry(
+		__string(	name,		regmap_name(map)	)
+		__field(	unsigned int,	reg			)
+		__field(	unsigned int,	val			)
+	),
+
+	TP_fast_assign(
+		__assign_str(name, regmap_name(map));
+		__entry->reg = reg;
+		__entry->val = val;
+	),
+
+	TP_printk("%s reg=%x val=%x", __get_str(name),
+		  (unsigned int)__entry->reg,
+		  (unsigned int)__entry->val)
+);
+
+DEFINE_EVENT(regmap_reg, regmap_reg_write,
+
+	TP_PROTO(struct regmap *map, unsigned int reg,
+		 unsigned int val),
+
+	TP_ARGS(map, reg, val)
+
+);
+
+DEFINE_EVENT(regmap_reg, regmap_reg_read,
+
+	TP_PROTO(struct regmap *map, unsigned int reg,
+		 unsigned int val),
+
+	TP_ARGS(map, reg, val)
+
+);
+
+DEFINE_EVENT(regmap_reg, regmap_reg_read_cache,
+
+	TP_PROTO(struct regmap *map, unsigned int reg,
+		 unsigned int val),
+
+	TP_ARGS(map, reg, val)
+
+);
+
+DECLARE_EVENT_CLASS(regmap_block,
+
+	TP_PROTO(struct regmap *map, unsigned int reg, int count),
+
+	TP_ARGS(map, reg, count),
+
+	TP_STRUCT__entry(
+		__string(	name,		regmap_name(map)	)
+		__field(	unsigned int,	reg			)
+		__field(	int,		count			)
+	),
+
+	TP_fast_assign(
+		__assign_str(name, regmap_name(map));
+		__entry->reg = reg;
+		__entry->count = count;
+	),
+
+	TP_printk("%s reg=%x count=%d", __get_str(name),
+		  (unsigned int)__entry->reg,
+		  (int)__entry->count)
+);
+
+DEFINE_EVENT(regmap_block, regmap_hw_read_start,
+
+	TP_PROTO(struct regmap *map, unsigned int reg, int count),
+
+	TP_ARGS(map, reg, count)
+);
+
+DEFINE_EVENT(regmap_block, regmap_hw_read_done,
+
+	TP_PROTO(struct regmap *map, unsigned int reg, int count),
+
+	TP_ARGS(map, reg, count)
+);
+
+DEFINE_EVENT(regmap_block, regmap_hw_write_start,
+
+	TP_PROTO(struct regmap *map, unsigned int reg, int count),
+
+	TP_ARGS(map, reg, count)
+);
+
+DEFINE_EVENT(regmap_block, regmap_hw_write_done,
+
+	TP_PROTO(struct regmap *map, unsigned int reg, int count),
+
+	TP_ARGS(map, reg, count)
+);
+
+TRACE_EVENT(regcache_sync,
+
+	TP_PROTO(struct regmap *map, const char *type,
+		 const char *status),
+
+	TP_ARGS(map, type, status),
+
+	TP_STRUCT__entry(
+		__string(       name,           regmap_name(map)	)
+		__string(	status,		status			)
+		__string(	type,		type			)
+		__field(	int,		type			)
+	),
+
+	TP_fast_assign(
+		__assign_str(name, regmap_name(map));
+		__assign_str(status, status);
+		__assign_str(type, type);
+	),
+
+	TP_printk("%s type=%s status=%s", __get_str(name),
+		  __get_str(type), __get_str(status))
+);
+
+DECLARE_EVENT_CLASS(regmap_bool,
+
+	TP_PROTO(struct regmap *map, bool flag),
+
+	TP_ARGS(map, flag),
+
+	TP_STRUCT__entry(
+		__string(	name,		regmap_name(map)	)
+		__field(	int,		flag			)
+	),
+
+	TP_fast_assign(
+		__assign_str(name, regmap_name(map));
+		__entry->flag = flag;
+	),
+
+	TP_printk("%s flag=%d", __get_str(name),
+		  (int)__entry->flag)
+);
+
+DEFINE_EVENT(regmap_bool, regmap_cache_only,
+
+	TP_PROTO(struct regmap *map, bool flag),
+
+	TP_ARGS(map, flag)
+
+);
+
+DEFINE_EVENT(regmap_bool, regmap_cache_bypass,
+
+	TP_PROTO(struct regmap *map, bool flag),
+
+	TP_ARGS(map, flag)
+
+);
+
+DECLARE_EVENT_CLASS(regmap_async,
+
+	TP_PROTO(struct regmap *map),
+
+	TP_ARGS(map),
+
+	TP_STRUCT__entry(
+		__string(	name,		regmap_name(map)	)
+	),
+
+	TP_fast_assign(
+		__assign_str(name, regmap_name(map));
+	),
+
+	TP_printk("%s", __get_str(name))
+);
+
+DEFINE_EVENT(regmap_block, regmap_async_write_start,
+
+	TP_PROTO(struct regmap *map, unsigned int reg, int count),
+
+	TP_ARGS(map, reg, count)
+);
+
+DEFINE_EVENT(regmap_async, regmap_async_io_complete,
+
+	TP_PROTO(struct regmap *map),
+
+	TP_ARGS(map)
+
+);
+
+DEFINE_EVENT(regmap_async, regmap_async_complete_start,
+
+	TP_PROTO(struct regmap *map),
+
+	TP_ARGS(map)
+
+);
+
+DEFINE_EVENT(regmap_async, regmap_async_complete_done,
+
+	TP_PROTO(struct regmap *map),
+
+	TP_ARGS(map)
+
+);
+
+TRACE_EVENT(regcache_drop_region,
+
+	TP_PROTO(struct regmap *map, unsigned int from,
+		 unsigned int to),
+
+	TP_ARGS(map, from, to),
+
+	TP_STRUCT__entry(
+		__string(       name,           regmap_name(map)	)
+		__field(	unsigned int,	from			)
+		__field(	unsigned int,	to			)
+	),
+
+	TP_fast_assign(
+		__assign_str(name, regmap_name(map));
+		__entry->from = from;
+		__entry->to = to;
+	),
+
+	TP_printk("%s %u-%u", __get_str(name), (unsigned int)__entry->from,
+		  (unsigned int)__entry->to)
+);
+
+#endif /* _TRACE_REGMAP_H */
+
+#undef TRACE_INCLUDE_PATH
+#define TRACE_INCLUDE_PATH .
+
+#undef TRACE_INCLUDE_FILE
+#define TRACE_INCLUDE_FILE trace
+
+/* This part must be outside protection */
+#include <trace/define_trace.h>
diff --git a/drivers/base/soc.c b/drivers/base/soc.c
index 72b5e7280d14..39fca01c8fa1 100644
--- a/drivers/base/soc.c
+++ b/drivers/base/soc.c
@@ -43,8 +43,8 @@ struct device *soc_device_to_device(struct soc_device *soc_dev)
 }
 
 static umode_t soc_attribute_mode(struct kobject *kobj,
-                                 struct attribute *attr,
-                                 int index)
+				struct attribute *attr,
+				int index)
 {
 	struct device *dev = container_of(kobj, struct device, kobj);
 	struct soc_device *soc_dev = container_of(dev, struct soc_device, dev);
@@ -60,7 +60,7 @@ static umode_t soc_attribute_mode(struct kobject *kobj,
 		return attr->mode;
 	if ((attr == &dev_attr_soc_id.attr)
 	    && (soc_dev->attr->soc_id != NULL))
-	        return attr->mode;
+		return attr->mode;
 
 	/* Unknown or unfilled attribute. */
 	return 0;
@@ -117,7 +117,7 @@ struct soc_device *soc_device_register(struct soc_device_attribute *soc_dev_attr
 
 	soc_dev = kzalloc(sizeof(*soc_dev), GFP_KERNEL);
 	if (!soc_dev) {
-	        ret = -ENOMEM;
+		ret = -ENOMEM;
 		goto out1;
 	}
 
@@ -135,7 +135,7 @@ struct soc_device *soc_device_register(struct soc_device_attribute *soc_dev_attr
 	} while (ret == -EAGAIN);
 
 	if (ret)
-	         goto out2;
+		goto out2;
 
 	soc_dev->attr = soc_dev_attr;
 	soc_dev->dev.bus = &soc_bus_type;
diff --git a/drivers/block/cpqarray.c b/drivers/block/cpqarray.c
index 2b9440384536..f749df9e15cd 100644
--- a/drivers/block/cpqarray.c
+++ b/drivers/block/cpqarray.c
@@ -405,8 +405,8 @@ static int cpqarray_register_ctlr(int i, struct pci_dev *pdev)
 		goto Enomem4;
 	}
 	hba[i]->access.set_intr_mask(hba[i], 0);
-	if (request_irq(hba[i]->intr, do_ida_intr,
-		IRQF_DISABLED|IRQF_SHARED, hba[i]->devname, hba[i]))
+	if (request_irq(hba[i]->intr, do_ida_intr, IRQF_SHARED,
+			hba[i]->devname, hba[i]))
 	{
 		printk(KERN_ERR "cpqarray: Unable to get irq %d for %s\n",
 				hba[i]->intr, hba[i]->devname);
diff --git a/drivers/block/nbd.c b/drivers/block/nbd.c
index 4bc2a5cb9935..a98c41f72c63 100644
--- a/drivers/block/nbd.c
+++ b/drivers/block/nbd.c
@@ -803,10 +803,6 @@ static int __init nbd_init(void)
 		return -EINVAL;
 	}
 
-	nbd_dev = kcalloc(nbds_max, sizeof(*nbd_dev), GFP_KERNEL);
-	if (!nbd_dev)
-		return -ENOMEM;
-
 	part_shift = 0;
 	if (max_part > 0) {
 		part_shift = fls(max_part);
@@ -828,6 +824,10 @@ static int __init nbd_init(void)
 	if (nbds_max > 1UL << (MINORBITS - part_shift))
 		return -EINVAL;
 
+	nbd_dev = kcalloc(nbds_max, sizeof(*nbd_dev), GFP_KERNEL);
+	if (!nbd_dev)
+		return -ENOMEM;
+
 	for (i = 0; i < nbds_max; i++) {
 		struct gendisk *disk = alloc_disk(1 << part_shift);
 		if (!disk)
diff --git a/drivers/block/nvme-core.c b/drivers/block/nvme-core.c
index b64bccbb78c9..e23be20a3417 100644
--- a/drivers/block/nvme-core.c
+++ b/drivers/block/nvme-core.c
@@ -482,6 +482,7 @@ static int nvme_error_status(u16 status)
 	}
 }
 
+#ifdef CONFIG_BLK_DEV_INTEGRITY
 static void nvme_dif_prep(u32 p, u32 v, struct t10_pi_tuple *pi)
 {
 	if (be32_to_cpu(pi->ref_tag) == v)
@@ -538,6 +539,58 @@ static void nvme_dif_remap(struct request *req,
 	kunmap_atomic(pmap);
 }
 
+static int nvme_noop_verify(struct blk_integrity_iter *iter)
+{
+	return 0;
+}
+
+static int nvme_noop_generate(struct blk_integrity_iter *iter)
+{
+	return 0;
+}
+
+struct blk_integrity nvme_meta_noop = {
+	.name			= "NVME_META_NOOP",
+	.generate_fn		= nvme_noop_generate,
+	.verify_fn		= nvme_noop_verify,
+};
+
+static void nvme_init_integrity(struct nvme_ns *ns)
+{
+	struct blk_integrity integrity;
+
+	switch (ns->pi_type) {
+	case NVME_NS_DPS_PI_TYPE3:
+		integrity = t10_pi_type3_crc;
+		break;
+	case NVME_NS_DPS_PI_TYPE1:
+	case NVME_NS_DPS_PI_TYPE2:
+		integrity = t10_pi_type1_crc;
+		break;
+	default:
+		integrity = nvme_meta_noop;
+		break;
+	}
+	integrity.tuple_size = ns->ms;
+	blk_integrity_register(ns->disk, &integrity);
+	blk_queue_max_integrity_segments(ns->queue, 1);
+}
+#else /* CONFIG_BLK_DEV_INTEGRITY */
+static void nvme_dif_remap(struct request *req,
+			void (*dif_swap)(u32 p, u32 v, struct t10_pi_tuple *pi))
+{
+}
+static void nvme_dif_prep(u32 p, u32 v, struct t10_pi_tuple *pi)
+{
+}
+static void nvme_dif_complete(u32 p, u32 v, struct t10_pi_tuple *pi)
+{
+}
+static void nvme_init_integrity(struct nvme_ns *ns)
+{
+}
+#endif
+
 static void req_completion(struct nvme_queue *nvmeq, void *ctx,
 						struct nvme_completion *cqe)
 {
@@ -1959,43 +2012,6 @@ static void nvme_config_discard(struct nvme_ns *ns)
 	queue_flag_set_unlocked(QUEUE_FLAG_DISCARD, ns->queue);
 }
 
-static int nvme_noop_verify(struct blk_integrity_iter *iter)
-{
-	return 0;
-}
-
-static int nvme_noop_generate(struct blk_integrity_iter *iter)
-{
-	return 0;
-}
-
-struct blk_integrity nvme_meta_noop = {
-	.name			= "NVME_META_NOOP",
-	.generate_fn		= nvme_noop_generate,
-	.verify_fn		= nvme_noop_verify,
-};
-
-static void nvme_init_integrity(struct nvme_ns *ns)
-{
-	struct blk_integrity integrity;
-
-	switch (ns->pi_type) {
-	case NVME_NS_DPS_PI_TYPE3:
-		integrity = t10_pi_type3_crc;
-		break;
-	case NVME_NS_DPS_PI_TYPE1:
-	case NVME_NS_DPS_PI_TYPE2:
-		integrity = t10_pi_type1_crc;
-		break;
-	default:
-		integrity = nvme_meta_noop;
-		break;
-	}
-	integrity.tuple_size = ns->ms;
-	blk_integrity_register(ns->disk, &integrity);
-	blk_queue_max_integrity_segments(ns->queue, 1);
-}
-
 static int nvme_revalidate_disk(struct gendisk *disk)
 {
 	struct nvme_ns *ns = disk->private_data;
@@ -2036,7 +2052,8 @@ static int nvme_revalidate_disk(struct gendisk *disk)
 	pi_type = ns->ms == sizeof(struct t10_pi_tuple) ?
 					id->dps & NVME_NS_DPS_PI_MASK : 0;
 
-	if (disk->integrity && (ns->pi_type != pi_type || ns->ms != old_ms ||
+	if (blk_get_integrity(disk) && (ns->pi_type != pi_type ||
+				ns->ms != old_ms ||
 				bs != queue_logical_block_size(disk->queue) ||
 				(ns->ms && id->flbas & NVME_NS_FLBAS_META_EXT)))
 		blk_integrity_unregister(disk);
@@ -2044,11 +2061,11 @@ static int nvme_revalidate_disk(struct gendisk *disk)
 	ns->pi_type = pi_type;
 	blk_queue_logical_block_size(ns->queue, bs);
 
-	if (ns->ms && !disk->integrity && (disk->flags & GENHD_FL_UP) &&
+	if (ns->ms && !blk_get_integrity(disk) && (disk->flags & GENHD_FL_UP) &&
 				!(id->flbas & NVME_NS_FLBAS_META_EXT))
 		nvme_init_integrity(ns);
 
-	if (id->ncap == 0 || (ns->ms && !disk->integrity))
+	if (id->ncap == 0 || (ns->ms && !blk_get_integrity(disk)))
 		set_capacity(disk, 0);
 	else
 		set_capacity(disk, le64_to_cpup(&id->nsze) << (ns->lba_shift - 9));
@@ -2652,7 +2669,7 @@ static void nvme_dev_remove(struct nvme_dev *dev)
 
 	list_for_each_entry(ns, &dev->namespaces, list) {
 		if (ns->disk->flags & GENHD_FL_UP) {
-			if (ns->disk->integrity)
+			if (blk_get_integrity(ns->disk))
 				blk_integrity_unregister(ns->disk);
 			del_gendisk(ns->disk);
 		}
@@ -2986,6 +3003,7 @@ static int nvme_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 	}
 	get_device(dev->device);
 
+	INIT_LIST_HEAD(&dev->node);
 	INIT_WORK(&dev->probe_work, nvme_async_probe);
 	schedule_work(&dev->probe_work);
 	return 0;
diff --git a/drivers/block/zram/zram_drv.c b/drivers/block/zram/zram_drv.c
index 8e233edd7a09..871bd3550cb0 100644
--- a/drivers/block/zram/zram_drv.c
+++ b/drivers/block/zram/zram_drv.c
@@ -528,7 +528,7 @@ out_cleanup:
 static inline void update_used_max(struct zram *zram,
 					const unsigned long pages)
 {
-	int old_max, cur_max;
+	unsigned long old_max, cur_max;
 
 	old_max = atomic_long_read(&zram->stats.max_used_pages);
 
diff --git a/drivers/bluetooth/btusb.c b/drivers/bluetooth/btusb.c
index b87688881143..8bfc4c2bba87 100644
--- a/drivers/bluetooth/btusb.c
+++ b/drivers/bluetooth/btusb.c
@@ -272,6 +272,7 @@ static const struct usb_device_id blacklist_table[] = {
 	{ USB_DEVICE(0x1286, 0x2046), .driver_info = BTUSB_MARVELL },
 
 	/* Intel Bluetooth devices */
+	{ USB_DEVICE(0x8087, 0x07da), .driver_info = BTUSB_CSR },
 	{ USB_DEVICE(0x8087, 0x07dc), .driver_info = BTUSB_INTEL },
 	{ USB_DEVICE(0x8087, 0x0a2a), .driver_info = BTUSB_INTEL },
 	{ USB_DEVICE(0x8087, 0x0a2b), .driver_info = BTUSB_INTEL_NEW },
diff --git a/drivers/bus/omap_l3_noc.c b/drivers/bus/omap_l3_noc.c
index 029bc73de001..11f7982cbdb3 100644
--- a/drivers/bus/omap_l3_noc.c
+++ b/drivers/bus/omap_l3_noc.c
@@ -284,7 +284,7 @@ static int omap_l3_probe(struct platform_device *pdev)
 	 */
 	l3->debug_irq = platform_get_irq(pdev, 0);
 	ret = devm_request_irq(l3->dev, l3->debug_irq, l3_interrupt_handler,
-			       IRQF_DISABLED, "l3-dbg-irq", l3);
+			       0x0, "l3-dbg-irq", l3);
 	if (ret) {
 		dev_err(l3->dev, "request_irq failed for %d\n",
 			l3->debug_irq);
@@ -293,7 +293,7 @@ static int omap_l3_probe(struct platform_device *pdev)
 
 	l3->app_irq = platform_get_irq(pdev, 1);
 	ret = devm_request_irq(l3->dev, l3->app_irq, l3_interrupt_handler,
-			       IRQF_DISABLED, "l3-app-irq", l3);
+			       0x0, "l3-app-irq", l3);
 	if (ret)
 		dev_err(l3->dev, "request_irq failed for %d\n", l3->app_irq);
 
diff --git a/drivers/bus/omap_l3_smx.c b/drivers/bus/omap_l3_smx.c
index 597fdaee7315..360a5c0a4ee0 100644
--- a/drivers/bus/omap_l3_smx.c
+++ b/drivers/bus/omap_l3_smx.c
@@ -251,18 +251,16 @@ static int omap3_l3_probe(struct platform_device *pdev)
 	}
 
 	l3->debug_irq = platform_get_irq(pdev, 0);
-	ret = request_irq(l3->debug_irq, omap3_l3_app_irq,
-		IRQF_DISABLED | IRQF_TRIGGER_RISING,
-		"l3-debug-irq", l3);
+	ret = request_irq(l3->debug_irq, omap3_l3_app_irq, IRQF_TRIGGER_RISING,
+			  "l3-debug-irq", l3);
 	if (ret) {
 		dev_err(&pdev->dev, "couldn't request debug irq\n");
 		goto err1;
 	}
 
 	l3->app_irq = platform_get_irq(pdev, 1);
-	ret = request_irq(l3->app_irq, omap3_l3_app_irq,
-		IRQF_DISABLED | IRQF_TRIGGER_RISING,
-		"l3-app-irq", l3);
+	ret = request_irq(l3->app_irq, omap3_l3_app_irq, IRQF_TRIGGER_RISING,
+			  "l3-app-irq", l3);
 	if (ret) {
 		dev_err(&pdev->dev, "couldn't request app irq\n");
 		goto err2;
diff --git a/drivers/char/ipmi/ipmi_powernv.c b/drivers/char/ipmi/ipmi_powernv.c
index 79524ed2a3cb..8753b0f6a317 100644
--- a/drivers/char/ipmi/ipmi_powernv.c
+++ b/drivers/char/ipmi/ipmi_powernv.c
@@ -125,6 +125,7 @@ static int ipmi_powernv_recv(struct ipmi_smi_powernv *smi)
 	spin_lock_irqsave(&smi->msg_lock, flags);
 
 	if (!smi->cur_msg) {
+		spin_unlock_irqrestore(&smi->msg_lock, flags);
 		pr_warn("no current message?\n");
 		return 0;
 	}
diff --git a/drivers/char/ipmi/ipmi_si_intf.c b/drivers/char/ipmi/ipmi_si_intf.c
index f6646ed3047e..518585c1ce94 100644
--- a/drivers/char/ipmi/ipmi_si_intf.c
+++ b/drivers/char/ipmi/ipmi_si_intf.c
@@ -263,6 +263,11 @@ struct smi_info {
 	bool supports_event_msg_buff;
 
 	/*
+	 * Can we clear the global enables receive irq bit?
+	 */
+	bool cannot_clear_recv_irq_bit;
+
+	/*
 	 * Did we get an attention that we did not handle?
 	 */
 	bool got_attn;
@@ -461,6 +466,9 @@ static void smi_mod_timer(struct smi_info *smi_info, unsigned long new_val)
  * allocate messages, we just leave them in the BMC and run the system
  * polled until we can allocate some memory.  Once we have some
  * memory, we will re-enable the interrupt.
+ *
+ * Note that we cannot just use disable_irq(), since the interrupt may
+ * be shared.
  */
 static inline bool disable_si_irq(struct smi_info *smi_info)
 {
@@ -549,20 +557,15 @@ static u8 current_global_enables(struct smi_info *smi_info, u8 base,
 
 	if (smi_info->supports_event_msg_buff)
 		enables |= IPMI_BMC_EVT_MSG_BUFF;
-	else
-		enables &= ~IPMI_BMC_EVT_MSG_BUFF;
 
-	if (smi_info->irq && !smi_info->interrupt_disabled)
+	if ((smi_info->irq && !smi_info->interrupt_disabled) ||
+	    smi_info->cannot_clear_recv_irq_bit)
 		enables |= IPMI_BMC_RCV_MSG_INTR;
-	else
-		enables &= ~IPMI_BMC_RCV_MSG_INTR;
 
 	if (smi_info->supports_event_msg_buff &&
 	    smi_info->irq && !smi_info->interrupt_disabled)
 
 		enables |= IPMI_BMC_EVT_MSG_INTR;
-	else
-		enables &= ~IPMI_BMC_EVT_MSG_INTR;
 
 	*irq_on = enables & (IPMI_BMC_EVT_MSG_INTR | IPMI_BMC_RCV_MSG_INTR);
 
@@ -2900,6 +2903,96 @@ static int try_get_dev_id(struct smi_info *smi_info)
 	return rv;
 }
 
+/*
+ * Some BMCs do not support clearing the receive irq bit in the global
+ * enables (even if they don't support interrupts on the BMC).  Check
+ * for this and handle it properly.
+ */
+static void check_clr_rcv_irq(struct smi_info *smi_info)
+{
+	unsigned char         msg[3];
+	unsigned char         *resp;
+	unsigned long         resp_len;
+	int                   rv;
+
+	resp = kmalloc(IPMI_MAX_MSG_LENGTH, GFP_KERNEL);
+	if (!resp) {
+		printk(KERN_WARNING PFX "Out of memory allocating response for"
+		       " global enables command, cannot check recv irq bit"
+		       " handling.\n");
+		return;
+	}
+
+	msg[0] = IPMI_NETFN_APP_REQUEST << 2;
+	msg[1] = IPMI_GET_BMC_GLOBAL_ENABLES_CMD;
+	smi_info->handlers->start_transaction(smi_info->si_sm, msg, 2);
+
+	rv = wait_for_msg_done(smi_info);
+	if (rv) {
+		printk(KERN_WARNING PFX "Error getting response from get"
+		       " global enables command, cannot check recv irq bit"
+		       " handling.\n");
+		goto out;
+	}
+
+	resp_len = smi_info->handlers->get_result(smi_info->si_sm,
+						  resp, IPMI_MAX_MSG_LENGTH);
+
+	if (resp_len < 4 ||
+			resp[0] != (IPMI_NETFN_APP_REQUEST | 1) << 2 ||
+			resp[1] != IPMI_GET_BMC_GLOBAL_ENABLES_CMD   ||
+			resp[2] != 0) {
+		printk(KERN_WARNING PFX "Invalid return from get global"
+		       " enables command, cannot check recv irq bit"
+		       " handling.\n");
+		rv = -EINVAL;
+		goto out;
+	}
+
+	if ((resp[3] & IPMI_BMC_RCV_MSG_INTR) == 0)
+		/* Already clear, should work ok. */
+		goto out;
+
+	msg[0] = IPMI_NETFN_APP_REQUEST << 2;
+	msg[1] = IPMI_SET_BMC_GLOBAL_ENABLES_CMD;
+	msg[2] = resp[3] & ~IPMI_BMC_RCV_MSG_INTR;
+	smi_info->handlers->start_transaction(smi_info->si_sm, msg, 3);
+
+	rv = wait_for_msg_done(smi_info);
+	if (rv) {
+		printk(KERN_WARNING PFX "Error getting response from set"
+		       " global enables command, cannot check recv irq bit"
+		       " handling.\n");
+		goto out;
+	}
+
+	resp_len = smi_info->handlers->get_result(smi_info->si_sm,
+						  resp, IPMI_MAX_MSG_LENGTH);
+
+	if (resp_len < 3 ||
+			resp[0] != (IPMI_NETFN_APP_REQUEST | 1) << 2 ||
+			resp[1] != IPMI_SET_BMC_GLOBAL_ENABLES_CMD) {
+		printk(KERN_WARNING PFX "Invalid return from get global"
+		       " enables command, cannot check recv irq bit"
+		       " handling.\n");
+		rv = -EINVAL;
+		goto out;
+	}
+
+	if (resp[2] != 0) {
+		/*
+		 * An error when setting the event buffer bit means
+		 * clearing the bit is not supported.
+		 */
+		printk(KERN_WARNING PFX "The BMC does not support clearing"
+		       " the recv irq bit, compensating, but the BMC needs to"
+		       " be fixed.\n");
+		smi_info->cannot_clear_recv_irq_bit = true;
+	}
+ out:
+	kfree(resp);
+}
+
 static int try_enable_event_buffer(struct smi_info *smi_info)
 {
 	unsigned char         msg[3];
@@ -3395,6 +3488,8 @@ static int try_smi_init(struct smi_info *new_smi)
 		goto out_err;
 	}
 
+	check_clr_rcv_irq(new_smi);
+
 	setup_oem_data_handler(new_smi);
 	setup_xaction_handlers(new_smi);
 
diff --git a/drivers/char/ipmi/ipmi_ssif.c b/drivers/char/ipmi/ipmi_ssif.c
index f6e378dac5f5..f40e3bd2c69c 100644
--- a/drivers/char/ipmi/ipmi_ssif.c
+++ b/drivers/char/ipmi/ipmi_ssif.c
@@ -468,11 +468,13 @@ static int ipmi_ssif_thread(void *data)
 		int result;
 
 		/* Wait for something to do */
-		wait_for_completion(&ssif_info->wake_thread);
-		init_completion(&ssif_info->wake_thread);
-
+		result = wait_for_completion_interruptible(
+						&ssif_info->wake_thread);
 		if (ssif_info->stopping)
 			break;
+		if (result == -ERESTARTSYS)
+			continue;
+		init_completion(&ssif_info->wake_thread);
 
 		if (ssif_info->i2c_read_write == I2C_SMBUS_WRITE) {
 			result = i2c_smbus_write_block_data(
diff --git a/drivers/char/tpm/tpm-chip.c b/drivers/char/tpm/tpm-chip.c
index 1d278ccd751f..e096e9cddb40 100644
--- a/drivers/char/tpm/tpm-chip.c
+++ b/drivers/char/tpm/tpm-chip.c
@@ -140,24 +140,24 @@ static int tpm_dev_add_device(struct tpm_chip *chip)
 {
 	int rc;
 
-	rc = device_add(&chip->dev);
+	rc = cdev_add(&chip->cdev, chip->dev.devt, 1);
 	if (rc) {
 		dev_err(&chip->dev,
-			"unable to device_register() %s, major %d, minor %d, err=%d\n",
+			"unable to cdev_add() %s, major %d, minor %d, err=%d\n",
 			chip->devname, MAJOR(chip->dev.devt),
 			MINOR(chip->dev.devt), rc);
 
+		device_unregister(&chip->dev);
 		return rc;
 	}
 
-	rc = cdev_add(&chip->cdev, chip->dev.devt, 1);
+	rc = device_add(&chip->dev);
 	if (rc) {
 		dev_err(&chip->dev,
-			"unable to cdev_add() %s, major %d, minor %d, err=%d\n",
+			"unable to device_register() %s, major %d, minor %d, err=%d\n",
 			chip->devname, MAJOR(chip->dev.devt),
 			MINOR(chip->dev.devt), rc);
 
-		device_unregister(&chip->dev);
 		return rc;
 	}
 
@@ -174,27 +174,17 @@ static void tpm_dev_del_device(struct tpm_chip *chip)
  * tpm_chip_register() - create a character device for the TPM chip
  * @chip: TPM chip to use.
  *
- * Creates a character device for the TPM chip and adds sysfs interfaces for
- * the device, PPI and TCPA. As the last step this function adds the
- * chip to the list of TPM chips available for use.
+ * Creates a character device for the TPM chip and adds sysfs attributes for
+ * the device. As the last step this function adds the chip to the list of TPM
+ * chips available for in-kernel use.
  *
- * NOTE: This function should be only called after the chip initialization
- * is complete.
- *
- * Called from tpm_<specific>.c probe function only for devices
- * the driver has determined it should claim.  Prior to calling
- * this function the specific probe function has called pci_enable_device
- * upon errant exit from this function specific probe function should call
- * pci_disable_device
+ * This function should be only called after the chip initialization is
+ * complete.
  */
 int tpm_chip_register(struct tpm_chip *chip)
 {
 	int rc;
 
-	rc = tpm_dev_add_device(chip);
-	if (rc)
-		return rc;
-
 	/* Populate sysfs for TPM1 devices. */
 	if (!(chip->flags & TPM_CHIP_FLAG_TPM2)) {
 		rc = tpm_sysfs_add_device(chip);
@@ -208,6 +198,10 @@ int tpm_chip_register(struct tpm_chip *chip)
 		chip->bios_dir = tpm_bios_log_setup(chip->devname);
 	}
 
+	rc = tpm_dev_add_device(chip);
+	if (rc)
+		return rc;
+
 	/* Make the chip available. */
 	spin_lock(&driver_lock);
 	list_add_rcu(&chip->list, &tpm_chip_list);
diff --git a/drivers/char/tpm/tpm_ibmvtpm.c b/drivers/char/tpm/tpm_ibmvtpm.c
index b1e53e3aece5..42ffa5e7a1e0 100644
--- a/drivers/char/tpm/tpm_ibmvtpm.c
+++ b/drivers/char/tpm/tpm_ibmvtpm.c
@@ -124,7 +124,7 @@ static int tpm_ibmvtpm_send(struct tpm_chip *chip, u8 *buf, size_t count)
 {
 	struct ibmvtpm_dev *ibmvtpm;
 	struct ibmvtpm_crq crq;
-	u64 *word = (u64 *) &crq;
+	__be64 *word = (__be64 *)&crq;
 	int rc;
 
 	ibmvtpm = (struct ibmvtpm_dev *)TPM_VPRIV(chip);
@@ -145,11 +145,11 @@ static int tpm_ibmvtpm_send(struct tpm_chip *chip, u8 *buf, size_t count)
 	memcpy((void *)ibmvtpm->rtce_buf, (void *)buf, count);
 	crq.valid = (u8)IBMVTPM_VALID_CMD;
 	crq.msg = (u8)VTPM_TPM_COMMAND;
-	crq.len = (u16)count;
-	crq.data = ibmvtpm->rtce_dma_handle;
+	crq.len = cpu_to_be16(count);
+	crq.data = cpu_to_be32(ibmvtpm->rtce_dma_handle);
 
-	rc = ibmvtpm_send_crq(ibmvtpm->vdev, cpu_to_be64(word[0]),
-			      cpu_to_be64(word[1]));
+	rc = ibmvtpm_send_crq(ibmvtpm->vdev, be64_to_cpu(word[0]),
+			      be64_to_cpu(word[1]));
 	if (rc != H_SUCCESS) {
 		dev_err(ibmvtpm->dev, "tpm_ibmvtpm_send failed rc=%d\n", rc);
 		rc = 0;
diff --git a/drivers/char/tpm/tpm_ibmvtpm.h b/drivers/char/tpm/tpm_ibmvtpm.h
index f595f14426bf..6af92890518f 100644
--- a/drivers/char/tpm/tpm_ibmvtpm.h
+++ b/drivers/char/tpm/tpm_ibmvtpm.h
@@ -22,9 +22,9 @@
 struct ibmvtpm_crq {
 	u8 valid;
 	u8 msg;
-	u16 len;
-	u32 data;
-	u64 reserved;
+	__be16 len;
+	__be32 data;
+	__be64 reserved;
 } __attribute__((packed, aligned(8)));
 
 struct ibmvtpm_crq_queue {
diff --git a/drivers/char/virtio_console.c b/drivers/char/virtio_console.c
index fae2dbbf5745..72d7028f779b 100644
--- a/drivers/char/virtio_console.c
+++ b/drivers/char/virtio_console.c
@@ -142,6 +142,7 @@ struct ports_device {
 	 * notification
 	 */
 	struct work_struct control_work;
+	struct work_struct config_work;
 
 	struct list_head ports;
 
@@ -1837,10 +1838,21 @@ static void config_intr(struct virtio_device *vdev)
 
 	portdev = vdev->priv;
 
+	if (!use_multiport(portdev))
+		schedule_work(&portdev->config_work);
+}
+
+static void config_work_handler(struct work_struct *work)
+{
+	struct ports_device *portdev;
+
+	portdev = container_of(work, struct ports_device, control_work);
 	if (!use_multiport(portdev)) {
+		struct virtio_device *vdev;
 		struct port *port;
 		u16 rows, cols;
 
+		vdev = portdev->vdev;
 		virtio_cread(vdev, struct virtio_console_config, cols, &cols);
 		virtio_cread(vdev, struct virtio_console_config, rows, &rows);
 
@@ -2040,12 +2052,14 @@ static int virtcons_probe(struct virtio_device *vdev)
 
 	virtio_device_ready(portdev->vdev);
 
+	INIT_WORK(&portdev->config_work, &config_work_handler);
+	INIT_WORK(&portdev->control_work, &control_work_handler);
+
 	if (multiport) {
 		unsigned int nr_added_bufs;
 
 		spin_lock_init(&portdev->c_ivq_lock);
 		spin_lock_init(&portdev->c_ovq_lock);
-		INIT_WORK(&portdev->control_work, &control_work_handler);
 
 		nr_added_bufs = fill_queue(portdev->c_ivq,
 					   &portdev->c_ivq_lock);
@@ -2113,6 +2127,8 @@ static void virtcons_remove(struct virtio_device *vdev)
 	/* Finish up work that's lined up */
 	if (use_multiport(portdev))
 		cancel_work_sync(&portdev->control_work);
+	else
+		cancel_work_sync(&portdev->config_work);
 
 	list_for_each_entry_safe(port, port2, &portdev->ports, list)
 		unplug_port(port);
@@ -2164,6 +2180,7 @@ static int virtcons_freeze(struct virtio_device *vdev)
 
 	virtqueue_disable_cb(portdev->c_ivq);
 	cancel_work_sync(&portdev->control_work);
+	cancel_work_sync(&portdev->config_work);
 	/*
 	 * Once more: if control_work_handler() was running, it would
 	 * enable the cb as the last step.
diff --git a/drivers/clk/at91/pmc.c b/drivers/clk/at91/pmc.c
index f07c8152e5cc..3f27d21fb729 100644
--- a/drivers/clk/at91/pmc.c
+++ b/drivers/clk/at91/pmc.c
@@ -89,12 +89,29 @@ static int pmc_irq_set_type(struct irq_data *d, unsigned type)
 	return 0;
 }
 
+static void pmc_irq_suspend(struct irq_data *d)
+{
+	struct at91_pmc *pmc = irq_data_get_irq_chip_data(d);
+
+	pmc->imr = pmc_read(pmc, AT91_PMC_IMR);
+	pmc_write(pmc, AT91_PMC_IDR, pmc->imr);
+}
+
+static void pmc_irq_resume(struct irq_data *d)
+{
+	struct at91_pmc *pmc = irq_data_get_irq_chip_data(d);
+
+	pmc_write(pmc, AT91_PMC_IER, pmc->imr);
+}
+
 static struct irq_chip pmc_irq = {
 	.name = "PMC",
 	.irq_disable = pmc_irq_mask,
 	.irq_mask = pmc_irq_mask,
 	.irq_unmask = pmc_irq_unmask,
 	.irq_set_type = pmc_irq_set_type,
+	.irq_suspend = pmc_irq_suspend,
+	.irq_resume = pmc_irq_resume,
 };
 
 static struct lock_class_key pmc_lock_class;
@@ -224,7 +241,8 @@ static struct at91_pmc *__init at91_pmc_init(struct device_node *np,
 		goto out_free_pmc;
 
 	pmc_write(pmc, AT91_PMC_IDR, 0xffffffff);
-	if (request_irq(pmc->virq, pmc_irq_handler, IRQF_SHARED, "pmc", pmc))
+	if (request_irq(pmc->virq, pmc_irq_handler,
+			IRQF_SHARED | IRQF_COND_SUSPEND, "pmc", pmc))
 		goto out_remove_irqdomain;
 
 	return pmc;
diff --git a/drivers/clk/at91/pmc.h b/drivers/clk/at91/pmc.h
index 52d2041fa3f6..69abb08cf146 100644
--- a/drivers/clk/at91/pmc.h
+++ b/drivers/clk/at91/pmc.h
@@ -33,6 +33,7 @@ struct at91_pmc {
 	spinlock_t lock;
 	const struct at91_pmc_caps *caps;
 	struct irq_domain *irqdomain;
+	u32 imr;
 };
 
 static inline void pmc_lock(struct at91_pmc *pmc)
diff --git a/drivers/clk/clk-divider.c b/drivers/clk/clk-divider.c
index db7f8bce7467..25006a8bb8e6 100644
--- a/drivers/clk/clk-divider.c
+++ b/drivers/clk/clk-divider.c
@@ -144,12 +144,6 @@ static unsigned long clk_divider_recalc_rate(struct clk_hw *hw,
 				   divider->flags);
 }
 
-/*
- * The reverse of DIV_ROUND_UP: The maximum number which
- * divided by m is r
- */
-#define MULT_ROUND_UP(r, m) ((r) * (m) + (m) - 1)
-
 static bool _is_valid_table_div(const struct clk_div_table *table,
 							 unsigned int div)
 {
@@ -225,19 +219,24 @@ static int _div_round_closest(const struct clk_div_table *table,
 			      unsigned long parent_rate, unsigned long rate,
 			      unsigned long flags)
 {
-	int up, down, div;
+	int up, down;
+	unsigned long up_rate, down_rate;
 
-	up = down = div = DIV_ROUND_CLOSEST(parent_rate, rate);
+	up = DIV_ROUND_UP(parent_rate, rate);
+	down = parent_rate / rate;
 
 	if (flags & CLK_DIVIDER_POWER_OF_TWO) {
-		up = __roundup_pow_of_two(div);
-		down = __rounddown_pow_of_two(div);
+		up = __roundup_pow_of_two(up);
+		down = __rounddown_pow_of_two(down);
 	} else if (table) {
-		up = _round_up_table(table, div);
-		down = _round_down_table(table, div);
+		up = _round_up_table(table, up);
+		down = _round_down_table(table, down);
 	}
 
-	return (up - div) <= (div - down) ? up : down;
+	up_rate = DIV_ROUND_UP(parent_rate, up);
+	down_rate = DIV_ROUND_UP(parent_rate, down);
+
+	return (rate - up_rate) <= (down_rate - rate) ? up : down;
 }
 
 static int _div_round(const struct clk_div_table *table,
@@ -313,7 +312,7 @@ static int clk_divider_bestdiv(struct clk_hw *hw, unsigned long rate,
 			return i;
 		}
 		parent_rate = __clk_round_rate(__clk_get_parent(hw->clk),
-				MULT_ROUND_UP(rate, i));
+					       rate * i);
 		now = DIV_ROUND_UP(parent_rate, i);
 		if (_is_best_div(rate, now, best, flags)) {
 			bestdiv = i;
@@ -353,7 +352,7 @@ static long clk_divider_round_rate(struct clk_hw *hw, unsigned long rate,
 		bestdiv = readl(divider->reg) >> divider->shift;
 		bestdiv &= div_mask(divider->width);
 		bestdiv = _get_div(divider->table, bestdiv, divider->flags);
-		return bestdiv;
+		return DIV_ROUND_UP(*prate, bestdiv);
 	}
 
 	return divider_round_rate(hw, rate, prate, divider->table,
diff --git a/drivers/clk/clk.c b/drivers/clk/clk.c
index eb0152961d3c..237f23f68bfc 100644
--- a/drivers/clk/clk.c
+++ b/drivers/clk/clk.c
@@ -1350,7 +1350,6 @@ static unsigned long clk_core_get_rate(struct clk_core *clk)
 
 	return rate;
 }
-EXPORT_SYMBOL_GPL(clk_core_get_rate);
 
 /**
  * clk_get_rate - return the rate of clk
@@ -2171,6 +2170,32 @@ int clk_get_phase(struct clk *clk)
 }
 
 /**
+ * clk_is_match - check if two clk's point to the same hardware clock
+ * @p: clk compared against q
+ * @q: clk compared against p
+ *
+ * Returns true if the two struct clk pointers both point to the same hardware
+ * clock node. Put differently, returns true if struct clk *p and struct clk *q
+ * share the same struct clk_core object.
+ *
+ * Returns false otherwise. Note that two NULL clks are treated as matching.
+ */
+bool clk_is_match(const struct clk *p, const struct clk *q)
+{
+	/* trivial case: identical struct clk's or both NULL */
+	if (p == q)
+		return true;
+
+	/* true if clk->core pointers match. Avoid derefing garbage */
+	if (!IS_ERR_OR_NULL(p) && !IS_ERR_OR_NULL(q))
+		if (p->core == q->core)
+			return true;
+
+	return false;
+}
+EXPORT_SYMBOL_GPL(clk_is_match);
+
+/**
  * __clk_init - initialize the data structures in a struct clk
  * @dev:	device initializing this clk, placeholder for now
  * @clk:	clk being initialized
diff --git a/drivers/clk/qcom/gcc-msm8960.c b/drivers/clk/qcom/gcc-msm8960.c
index b0b562b9ce0e..e60feffc10a1 100644
--- a/drivers/clk/qcom/gcc-msm8960.c
+++ b/drivers/clk/qcom/gcc-msm8960.c
@@ -48,6 +48,17 @@ static struct clk_pll pll3 = {
 	},
 };
 
+static struct clk_regmap pll4_vote = {
+	.enable_reg = 0x34c0,
+	.enable_mask = BIT(4),
+	.hw.init = &(struct clk_init_data){
+		.name = "pll4_vote",
+		.parent_names = (const char *[]){ "pll4" },
+		.num_parents = 1,
+		.ops = &clk_pll_vote_ops,
+	},
+};
+
 static struct clk_pll pll8 = {
 	.l_reg = 0x3144,
 	.m_reg = 0x3148,
@@ -3023,6 +3034,7 @@ static struct clk_branch rpm_msg_ram_h_clk = {
 
 static struct clk_regmap *gcc_msm8960_clks[] = {
 	[PLL3] = &pll3.clkr,
+	[PLL4_VOTE] = &pll4_vote,
 	[PLL8] = &pll8.clkr,
 	[PLL8_VOTE] = &pll8_vote,
 	[PLL14] = &pll14.clkr,
@@ -3247,6 +3259,7 @@ static const struct qcom_reset_map gcc_msm8960_resets[] = {
 
 static struct clk_regmap *gcc_apq8064_clks[] = {
 	[PLL3] = &pll3.clkr,
+	[PLL4_VOTE] = &pll4_vote,
 	[PLL8] = &pll8.clkr,
 	[PLL8_VOTE] = &pll8_vote,
 	[PLL14] = &pll14.clkr,
diff --git a/drivers/clk/qcom/lcc-ipq806x.c b/drivers/clk/qcom/lcc-ipq806x.c
index 121ffde25dc3..c9ff27b4648b 100644
--- a/drivers/clk/qcom/lcc-ipq806x.c
+++ b/drivers/clk/qcom/lcc-ipq806x.c
@@ -462,7 +462,6 @@ static struct platform_driver lcc_ipq806x_driver = {
 	.remove		= lcc_ipq806x_remove,
 	.driver		= {
 		.name	= "lcc-ipq806x",
-		.owner	= THIS_MODULE,
 		.of_match_table = lcc_ipq806x_match_table,
 	},
 };
diff --git a/drivers/clk/qcom/lcc-msm8960.c b/drivers/clk/qcom/lcc-msm8960.c
index a75a408cfccd..e2c863295f00 100644
--- a/drivers/clk/qcom/lcc-msm8960.c
+++ b/drivers/clk/qcom/lcc-msm8960.c
@@ -417,8 +417,8 @@ static struct clk_rcg slimbus_src = {
 		.mnctr_en_bit = 8,
 		.mnctr_reset_bit = 7,
 		.mnctr_mode_shift = 5,
-		.n_val_shift = 16,
-		.m_val_shift = 16,
+		.n_val_shift = 24,
+		.m_val_shift = 8,
 		.width = 8,
 	},
 	.p = {
@@ -547,7 +547,7 @@ static int lcc_msm8960_probe(struct platform_device *pdev)
 		return PTR_ERR(regmap);
 
 	/* Use the correct frequency plan depending on speed of PLL4 */
-	val = regmap_read(regmap, 0x4, &val);
+	regmap_read(regmap, 0x4, &val);
 	if (val == 0x12) {
 		slimbus_src.freq_tbl = clk_tbl_aif_osr_492;
 		mi2s_osr_src.freq_tbl = clk_tbl_aif_osr_492;
@@ -574,7 +574,6 @@ static struct platform_driver lcc_msm8960_driver = {
 	.remove		= lcc_msm8960_remove,
 	.driver		= {
 		.name	= "lcc-msm8960",
-		.owner	= THIS_MODULE,
 		.of_match_table = lcc_msm8960_match_table,
 	},
 };
diff --git a/drivers/clk/ti/fapll.c b/drivers/clk/ti/fapll.c
index 6ef89639a9f6..d21640634adf 100644
--- a/drivers/clk/ti/fapll.c
+++ b/drivers/clk/ti/fapll.c
@@ -84,7 +84,7 @@ static int ti_fapll_enable(struct clk_hw *hw)
 	struct fapll_data *fd = to_fapll(hw);
 	u32 v = readl_relaxed(fd->base);
 
-	v |= (1 << FAPLL_MAIN_PLLEN);
+	v |= FAPLL_MAIN_PLLEN;
 	writel_relaxed(v, fd->base);
 
 	return 0;
@@ -95,7 +95,7 @@ static void ti_fapll_disable(struct clk_hw *hw)
 	struct fapll_data *fd = to_fapll(hw);
 	u32 v = readl_relaxed(fd->base);
 
-	v &= ~(1 << FAPLL_MAIN_PLLEN);
+	v &= ~FAPLL_MAIN_PLLEN;
 	writel_relaxed(v, fd->base);
 }
 
@@ -104,7 +104,7 @@ static int ti_fapll_is_enabled(struct clk_hw *hw)
 	struct fapll_data *fd = to_fapll(hw);
 	u32 v = readl_relaxed(fd->base);
 
-	return v & (1 << FAPLL_MAIN_PLLEN);
+	return v & FAPLL_MAIN_PLLEN;
 }
 
 static unsigned long ti_fapll_recalc_rate(struct clk_hw *hw,
diff --git a/drivers/clocksource/Kconfig b/drivers/clocksource/Kconfig
index 1c2506f68122..a0b036ccb118 100644
--- a/drivers/clocksource/Kconfig
+++ b/drivers/clocksource/Kconfig
@@ -63,6 +63,11 @@ config VT8500_TIMER
 config CADENCE_TTC_TIMER
 	bool
 
+config ASM9260_TIMER
+	bool
+	select CLKSRC_MMIO
+	select CLKSRC_OF
+
 config CLKSRC_NOMADIK_MTU
 	bool
 	depends on (ARCH_NOMADIK || ARCH_U8500)
@@ -187,6 +192,7 @@ config SYS_SUPPORTS_EM_STI
 config SH_TIMER_CMT
 	bool "Renesas CMT timer driver" if COMPILE_TEST
 	depends on GENERIC_CLOCKEVENTS
+	depends on HAS_IOMEM
 	default SYS_SUPPORTS_SH_CMT
 	help
 	  This enables build of a clocksource and clockevent driver for
@@ -196,6 +202,7 @@ config SH_TIMER_CMT
 config SH_TIMER_MTU2
 	bool "Renesas MTU2 timer driver" if COMPILE_TEST
 	depends on GENERIC_CLOCKEVENTS
+	depends on HAS_IOMEM
 	default SYS_SUPPORTS_SH_MTU2
 	help
 	  This enables build of a clockevent driver for the Multi-Function
@@ -205,6 +212,7 @@ config SH_TIMER_MTU2
 config SH_TIMER_TMU
 	bool "Renesas TMU timer driver" if COMPILE_TEST
 	depends on GENERIC_CLOCKEVENTS
+	depends on HAS_IOMEM
 	default SYS_SUPPORTS_SH_TMU
 	help
 	  This enables build of a clocksource and clockevent driver for
@@ -245,15 +253,4 @@ config CLKSRC_PXA
 	help
 	  This enables OST0 support available on PXA and SA-11x0
 	  platforms.
-
-config ASM9260_TIMER
-	bool "Alphascale ASM9260 timer driver"
-	depends on GENERIC_CLOCKEVENTS
-	select CLKSRC_MMIO
-	select CLKSRC_OF
-	default y if MACH_ASM9260
-	help
-	  This enables build of a clocksource and clockevent driver for
-	  the 32-bit System Timer hardware available on a Alphascale ASM9260.
-
 endmenu
diff --git a/drivers/clocksource/arm_arch_timer.c b/drivers/clocksource/arm_arch_timer.c
index a3025e7ae35f..266469691e58 100644
--- a/drivers/clocksource/arm_arch_timer.c
+++ b/drivers/clocksource/arm_arch_timer.c
@@ -661,17 +661,17 @@ static const struct of_device_id arch_timer_mem_of_match[] __initconst = {
 };
 
 static bool __init
-arch_timer_probed(int type, const struct of_device_id *matches)
+arch_timer_needs_probing(int type, const struct of_device_id *matches)
 {
 	struct device_node *dn;
-	bool probed = true;
+	bool needs_probing = false;
 
 	dn = of_find_matching_node(NULL, matches);
 	if (dn && of_device_is_available(dn) && !(arch_timers_present & type))
-		probed = false;
+		needs_probing = true;
 	of_node_put(dn);
 
-	return probed;
+	return needs_probing;
 }
 
 static void __init arch_timer_common_init(void)
@@ -680,9 +680,9 @@ static void __init arch_timer_common_init(void)
 
 	/* Wait until both nodes are probed if we have two timers */
 	if ((arch_timers_present & mask) != mask) {
-		if (!arch_timer_probed(ARCH_MEM_TIMER, arch_timer_mem_of_match))
+		if (arch_timer_needs_probing(ARCH_MEM_TIMER, arch_timer_mem_of_match))
 			return;
-		if (!arch_timer_probed(ARCH_CP15_TIMER, arch_timer_of_match))
+		if (arch_timer_needs_probing(ARCH_CP15_TIMER, arch_timer_of_match))
 			return;
 	}
 
diff --git a/drivers/clocksource/dw_apb_timer_of.c b/drivers/clocksource/dw_apb_timer_of.c
index d305fb089767..a19a3f619cc7 100644
--- a/drivers/clocksource/dw_apb_timer_of.c
+++ b/drivers/clocksource/dw_apb_timer_of.c
@@ -108,7 +108,7 @@ static void __init add_clocksource(struct device_node *source_timer)
 
 static u64 notrace read_sched_clock(void)
 {
-	return ~__raw_readl(sched_io_base);
+	return ~readl_relaxed(sched_io_base);
 }
 
 static const struct of_device_id sptimer_ids[] __initconst = {
diff --git a/drivers/clocksource/em_sti.c b/drivers/clocksource/em_sti.c
index d0a7bd66b8b9..dc3c6ee04aaa 100644
--- a/drivers/clocksource/em_sti.c
+++ b/drivers/clocksource/em_sti.c
@@ -210,7 +210,7 @@ static int em_sti_clocksource_enable(struct clocksource *cs)
 
 	ret = em_sti_start(p, USER_CLOCKSOURCE);
 	if (!ret)
-		__clocksource_updatefreq_hz(cs, p->rate);
+		__clocksource_update_freq_hz(cs, p->rate);
 	return ret;
 }
 
diff --git a/drivers/clocksource/mtk_timer.c b/drivers/clocksource/mtk_timer.c
index 32a3d25795d3..68ab42356d0e 100644
--- a/drivers/clocksource/mtk_timer.c
+++ b/drivers/clocksource/mtk_timer.c
@@ -224,6 +224,8 @@ static void __init mtk_timer_init(struct device_node *node)
 	}
 	rate = clk_get_rate(clk);
 
+	mtk_timer_global_reset(evt);
+
 	if (request_irq(evt->dev.irq, mtk_timer_interrupt,
 			IRQF_TIMER | IRQF_IRQPOLL, "mtk_timer", evt)) {
 		pr_warn("failed to setup irq %d\n", evt->dev.irq);
@@ -232,8 +234,6 @@ static void __init mtk_timer_init(struct device_node *node)
 
 	evt->ticks_per_jiffy = DIV_ROUND_UP(rate, HZ);
 
-	mtk_timer_global_reset(evt);
-
 	/* Configure clock source */
 	mtk_timer_setup(evt, GPT_CLK_SRC, TIMER_CTRL_OP_FREERUN);
 	clocksource_mmio_init(evt->gpt_base + TIMER_CNT_REG(GPT_CLK_SRC),
@@ -241,10 +241,11 @@ static void __init mtk_timer_init(struct device_node *node)
 
 	/* Configure clock event */
 	mtk_timer_setup(evt, GPT_CLK_EVT, TIMER_CTRL_OP_REPEAT);
-	mtk_timer_enable_irq(evt, GPT_CLK_EVT);
-
 	clockevents_config_and_register(&evt->dev, rate, 0x3,
 					0xffffffff);
+
+	mtk_timer_enable_irq(evt, GPT_CLK_EVT);
+
 	return;
 
 err_clk_disable:
diff --git a/drivers/clocksource/pxa_timer.c b/drivers/clocksource/pxa_timer.c
index 941f3f344e08..d9438af2bbd6 100644
--- a/drivers/clocksource/pxa_timer.c
+++ b/drivers/clocksource/pxa_timer.c
@@ -163,7 +163,7 @@ static struct irqaction pxa_ost0_irq = {
 	.dev_id		= &ckevt_pxa_osmr0,
 };
 
-static void pxa_timer_common_init(int irq, unsigned long clock_tick_rate)
+static void __init pxa_timer_common_init(int irq, unsigned long clock_tick_rate)
 {
 	timer_writel(0, OIER);
 	timer_writel(OSSR_M0 | OSSR_M1 | OSSR_M2 | OSSR_M3, OSSR);
diff --git a/drivers/clocksource/sh_cmt.c b/drivers/clocksource/sh_cmt.c
index 2bd13b53b727..b8ff3c64cc45 100644
--- a/drivers/clocksource/sh_cmt.c
+++ b/drivers/clocksource/sh_cmt.c
@@ -641,7 +641,7 @@ static int sh_cmt_clocksource_enable(struct clocksource *cs)
 
 	ret = sh_cmt_start(ch, FLAG_CLOCKSOURCE);
 	if (!ret) {
-		__clocksource_updatefreq_hz(cs, ch->rate);
+		__clocksource_update_freq_hz(cs, ch->rate);
 		ch->cs_enabled = true;
 	}
 	return ret;
diff --git a/drivers/clocksource/sh_tmu.c b/drivers/clocksource/sh_tmu.c
index f150ca82bfaf..b6b8fa3cd211 100644
--- a/drivers/clocksource/sh_tmu.c
+++ b/drivers/clocksource/sh_tmu.c
@@ -272,7 +272,7 @@ static int sh_tmu_clocksource_enable(struct clocksource *cs)
 
 	ret = sh_tmu_enable(ch);
 	if (!ret) {
-		__clocksource_updatefreq_hz(cs, ch->rate);
+		__clocksource_update_freq_hz(cs, ch->rate);
 		ch->cs_enabled = true;
 	}
 
diff --git a/drivers/clocksource/sun4i_timer.c b/drivers/clocksource/sun4i_timer.c
index f4a9c0058b4d..1928a8912584 100644
--- a/drivers/clocksource/sun4i_timer.c
+++ b/drivers/clocksource/sun4i_timer.c
@@ -170,7 +170,15 @@ static void __init sun4i_timer_init(struct device_node *node)
 	       TIMER_CTL_CLK_SRC(TIMER_CTL_CLK_SRC_OSC24M),
 	       timer_base + TIMER_CTL_REG(1));
 
-	sched_clock_register(sun4i_timer_sched_read, 32, rate);
+	/*
+	 * sched_clock_register does not have priorities, and on sun6i and
+	 * later there is a better sched_clock registered by arm_arch_timer.c
+	 */
+	if (of_machine_is_compatible("allwinner,sun4i-a10") ||
+	    of_machine_is_compatible("allwinner,sun5i-a13") ||
+	    of_machine_is_compatible("allwinner,sun5i-a10s"))
+		sched_clock_register(sun4i_timer_sched_read, 32, rate);
+
 	clocksource_mmio_init(timer_base + TIMER_CNTVAL_REG(1), node->name,
 			      rate, 350, 32, clocksource_mmio_readl_down);
 
diff --git a/drivers/clocksource/tegra20_timer.c b/drivers/clocksource/tegra20_timer.c
index d2616ef16770..5a112d72fc2d 100644
--- a/drivers/clocksource/tegra20_timer.c
+++ b/drivers/clocksource/tegra20_timer.c
@@ -51,15 +51,15 @@
 static void __iomem *timer_reg_base;
 static void __iomem *rtc_base;
 
-static struct timespec persistent_ts;
+static struct timespec64 persistent_ts;
 static u64 persistent_ms, last_persistent_ms;
 
 static struct delay_timer tegra_delay_timer;
 
 #define timer_writel(value, reg) \
-	__raw_writel(value, timer_reg_base + (reg))
+	writel_relaxed(value, timer_reg_base + (reg))
 #define timer_readl(reg) \
-	__raw_readl(timer_reg_base + (reg))
+	readl_relaxed(timer_reg_base + (reg))
 
 static int tegra_timer_set_next_event(unsigned long cycles,
 					 struct clock_event_device *evt)
@@ -120,26 +120,25 @@ static u64 tegra_rtc_read_ms(void)
 }
 
 /*
- * tegra_read_persistent_clock -  Return time from a persistent clock.
+ * tegra_read_persistent_clock64 -  Return time from a persistent clock.
  *
  * Reads the time from a source which isn't disabled during PM, the
  * 32k sync timer.  Convert the cycles elapsed since last read into
- * nsecs and adds to a monotonically increasing timespec.
+ * nsecs and adds to a monotonically increasing timespec64.
  * Care must be taken that this funciton is not called while the
  * tegra_rtc driver could be executing to avoid race conditions
  * on the RTC shadow register
  */
-static void tegra_read_persistent_clock(struct timespec *ts)
+static void tegra_read_persistent_clock64(struct timespec64 *ts)
 {
 	u64 delta;
-	struct timespec *tsp = &persistent_ts;
 
 	last_persistent_ms = persistent_ms;
 	persistent_ms = tegra_rtc_read_ms();
 	delta = persistent_ms - last_persistent_ms;
 
-	timespec_add_ns(tsp, delta * NSEC_PER_MSEC);
-	*ts = *tsp;
+	timespec64_add_ns(&persistent_ts, delta * NSEC_PER_MSEC);
+	*ts = persistent_ts;
 }
 
 static unsigned long tegra_delay_timer_read_counter_long(void)
@@ -252,7 +251,7 @@ static void __init tegra20_init_rtc(struct device_node *np)
 	else
 		clk_prepare_enable(clk);
 
-	register_persistent_clock(NULL, tegra_read_persistent_clock);
+	register_persistent_clock(NULL, tegra_read_persistent_clock64);
 }
 CLOCKSOURCE_OF_DECLARE(tegra20_rtc, "nvidia,tegra20-rtc", tegra20_init_rtc);
 
diff --git a/drivers/clocksource/time-efm32.c b/drivers/clocksource/time-efm32.c
index bba62f9deefb..5b6e3d5644c9 100644
--- a/drivers/clocksource/time-efm32.c
+++ b/drivers/clocksource/time-efm32.c
@@ -111,7 +111,7 @@ static irqreturn_t efm32_clock_event_handler(int irq, void *dev_id)
 static struct efm32_clock_event_ddata clock_event_ddata = {
 	.evtdev = {
 		.name = "efm32 clockevent",
-		.features = CLOCK_EVT_FEAT_ONESHOT | CLOCK_EVT_MODE_PERIODIC,
+		.features = CLOCK_EVT_FEAT_ONESHOT | CLOCK_EVT_FEAT_PERIODIC,
 		.set_mode = efm32_clock_event_set_mode,
 		.set_next_event = efm32_clock_event_set_next_event,
 		.rating = 200,
@@ -225,12 +225,12 @@ static int __init efm32_clockevent_init(struct device_node *np)
 	clock_event_ddata.base = base;
 	clock_event_ddata.periodic_top = DIV_ROUND_CLOSEST(rate, 1024 * HZ);
 
-	setup_irq(irq, &efm32_clock_event_irq);
-
 	clockevents_config_and_register(&clock_event_ddata.evtdev,
 					DIV_ROUND_CLOSEST(rate, 1024),
 					0xf, 0xffff);
 
+	setup_irq(irq, &efm32_clock_event_irq);
+
 	return 0;
 
 err_get_irq:
diff --git a/drivers/clocksource/timer-atmel-pit.c b/drivers/clocksource/timer-atmel-pit.c
index b5b4d4585c9a..c0304ff608b0 100644
--- a/drivers/clocksource/timer-atmel-pit.c
+++ b/drivers/clocksource/timer-atmel-pit.c
@@ -61,12 +61,12 @@ static inline struct pit_data *clkevt_to_pit_data(struct clock_event_device *clk
 
 static inline unsigned int pit_read(void __iomem *base, unsigned int reg_offset)
 {
-	return __raw_readl(base + reg_offset);
+	return readl_relaxed(base + reg_offset);
 }
 
 static inline void pit_write(void __iomem *base, unsigned int reg_offset, unsigned long value)
 {
-	__raw_writel(value, base + reg_offset);
+	writel_relaxed(value, base + reg_offset);
 }
 
 /*
diff --git a/drivers/clocksource/timer-sun5i.c b/drivers/clocksource/timer-sun5i.c
index 02268448dc85..28aa4b7bb602 100644
--- a/drivers/clocksource/timer-sun5i.c
+++ b/drivers/clocksource/timer-sun5i.c
@@ -17,7 +17,7 @@
 #include <linux/irq.h>
 #include <linux/irqreturn.h>
 #include <linux/reset.h>
-#include <linux/sched_clock.h>
+#include <linux/slab.h>
 #include <linux/of.h>
 #include <linux/of_address.h>
 #include <linux/of_irq.h>
@@ -37,8 +37,31 @@
 
 #define TIMER_SYNC_TICKS	3
 
-static void __iomem *timer_base;
-static u32 ticks_per_jiffy;
+struct sun5i_timer {
+	void __iomem		*base;
+	struct clk		*clk;
+	struct notifier_block	clk_rate_cb;
+	u32			ticks_per_jiffy;
+};
+
+#define to_sun5i_timer(x) \
+	container_of(x, struct sun5i_timer, clk_rate_cb)
+
+struct sun5i_timer_clksrc {
+	struct sun5i_timer	timer;
+	struct clocksource	clksrc;
+};
+
+#define to_sun5i_timer_clksrc(x) \
+	container_of(x, struct sun5i_timer_clksrc, clksrc)
+
+struct sun5i_timer_clkevt {
+	struct sun5i_timer		timer;
+	struct clock_event_device	clkevt;
+};
+
+#define to_sun5i_timer_clkevt(x) \
+	container_of(x, struct sun5i_timer_clkevt, clkevt)
 
 /*
  * When we disable a timer, we need to wait at least for 2 cycles of
@@ -46,30 +69,30 @@ static u32 ticks_per_jiffy;
  * that is already setup and runs at the same frequency than the other
  * timers, and we never will be disabled.
  */
-static void sun5i_clkevt_sync(void)
+static void sun5i_clkevt_sync(struct sun5i_timer_clkevt *ce)
 {
-	u32 old = readl(timer_base + TIMER_CNTVAL_LO_REG(1));
+	u32 old = readl(ce->timer.base + TIMER_CNTVAL_LO_REG(1));
 
-	while ((old - readl(timer_base + TIMER_CNTVAL_LO_REG(1))) < TIMER_SYNC_TICKS)
+	while ((old - readl(ce->timer.base + TIMER_CNTVAL_LO_REG(1))) < TIMER_SYNC_TICKS)
 		cpu_relax();
 }
 
-static void sun5i_clkevt_time_stop(u8 timer)
+static void sun5i_clkevt_time_stop(struct sun5i_timer_clkevt *ce, u8 timer)
 {
-	u32 val = readl(timer_base + TIMER_CTL_REG(timer));
-	writel(val & ~TIMER_CTL_ENABLE, timer_base + TIMER_CTL_REG(timer));
+	u32 val = readl(ce->timer.base + TIMER_CTL_REG(timer));
+	writel(val & ~TIMER_CTL_ENABLE, ce->timer.base + TIMER_CTL_REG(timer));
 
-	sun5i_clkevt_sync();
+	sun5i_clkevt_sync(ce);
 }
 
-static void sun5i_clkevt_time_setup(u8 timer, u32 delay)
+static void sun5i_clkevt_time_setup(struct sun5i_timer_clkevt *ce, u8 timer, u32 delay)
 {
-	writel(delay, timer_base + TIMER_INTVAL_LO_REG(timer));
+	writel(delay, ce->timer.base + TIMER_INTVAL_LO_REG(timer));
 }
 
-static void sun5i_clkevt_time_start(u8 timer, bool periodic)
+static void sun5i_clkevt_time_start(struct sun5i_timer_clkevt *ce, u8 timer, bool periodic)
 {
-	u32 val = readl(timer_base + TIMER_CTL_REG(timer));
+	u32 val = readl(ce->timer.base + TIMER_CTL_REG(timer));
 
 	if (periodic)
 		val &= ~TIMER_CTL_ONESHOT;
@@ -77,80 +100,230 @@ static void sun5i_clkevt_time_start(u8 timer, bool periodic)
 		val |= TIMER_CTL_ONESHOT;
 
 	writel(val | TIMER_CTL_ENABLE | TIMER_CTL_RELOAD,
-	       timer_base + TIMER_CTL_REG(timer));
+	       ce->timer.base + TIMER_CTL_REG(timer));
 }
 
 static void sun5i_clkevt_mode(enum clock_event_mode mode,
-			      struct clock_event_device *clk)
+			      struct clock_event_device *clkevt)
 {
+	struct sun5i_timer_clkevt *ce = to_sun5i_timer_clkevt(clkevt);
+
 	switch (mode) {
 	case CLOCK_EVT_MODE_PERIODIC:
-		sun5i_clkevt_time_stop(0);
-		sun5i_clkevt_time_setup(0, ticks_per_jiffy);
-		sun5i_clkevt_time_start(0, true);
+		sun5i_clkevt_time_stop(ce, 0);
+		sun5i_clkevt_time_setup(ce, 0, ce->timer.ticks_per_jiffy);
+		sun5i_clkevt_time_start(ce, 0, true);
 		break;
 	case CLOCK_EVT_MODE_ONESHOT:
-		sun5i_clkevt_time_stop(0);
-		sun5i_clkevt_time_start(0, false);
+		sun5i_clkevt_time_stop(ce, 0);
+		sun5i_clkevt_time_start(ce, 0, false);
 		break;
 	case CLOCK_EVT_MODE_UNUSED:
 	case CLOCK_EVT_MODE_SHUTDOWN:
 	default:
-		sun5i_clkevt_time_stop(0);
+		sun5i_clkevt_time_stop(ce, 0);
 		break;
 	}
 }
 
 static int sun5i_clkevt_next_event(unsigned long evt,
-				   struct clock_event_device *unused)
+				   struct clock_event_device *clkevt)
 {
-	sun5i_clkevt_time_stop(0);
-	sun5i_clkevt_time_setup(0, evt - TIMER_SYNC_TICKS);
-	sun5i_clkevt_time_start(0, false);
+	struct sun5i_timer_clkevt *ce = to_sun5i_timer_clkevt(clkevt);
+
+	sun5i_clkevt_time_stop(ce, 0);
+	sun5i_clkevt_time_setup(ce, 0, evt - TIMER_SYNC_TICKS);
+	sun5i_clkevt_time_start(ce, 0, false);
 
 	return 0;
 }
 
-static struct clock_event_device sun5i_clockevent = {
-	.name = "sun5i_tick",
-	.rating = 340,
-	.features = CLOCK_EVT_FEAT_PERIODIC | CLOCK_EVT_FEAT_ONESHOT,
-	.set_mode = sun5i_clkevt_mode,
-	.set_next_event = sun5i_clkevt_next_event,
-};
-
-
 static irqreturn_t sun5i_timer_interrupt(int irq, void *dev_id)
 {
-	struct clock_event_device *evt = (struct clock_event_device *)dev_id;
+	struct sun5i_timer_clkevt *ce = (struct sun5i_timer_clkevt *)dev_id;
 
-	writel(0x1, timer_base + TIMER_IRQ_ST_REG);
-	evt->event_handler(evt);
+	writel(0x1, ce->timer.base + TIMER_IRQ_ST_REG);
+	ce->clkevt.event_handler(&ce->clkevt);
 
 	return IRQ_HANDLED;
 }
 
-static struct irqaction sun5i_timer_irq = {
-	.name = "sun5i_timer0",
-	.flags = IRQF_TIMER | IRQF_IRQPOLL,
-	.handler = sun5i_timer_interrupt,
-	.dev_id = &sun5i_clockevent,
-};
+static cycle_t sun5i_clksrc_read(struct clocksource *clksrc)
+{
+	struct sun5i_timer_clksrc *cs = to_sun5i_timer_clksrc(clksrc);
+
+	return ~readl(cs->timer.base + TIMER_CNTVAL_LO_REG(1));
+}
 
-static u64 sun5i_timer_sched_read(void)
+static int sun5i_rate_cb_clksrc(struct notifier_block *nb,
+				unsigned long event, void *data)
 {
-	return ~readl(timer_base + TIMER_CNTVAL_LO_REG(1));
+	struct clk_notifier_data *ndata = data;
+	struct sun5i_timer *timer = to_sun5i_timer(nb);
+	struct sun5i_timer_clksrc *cs = container_of(timer, struct sun5i_timer_clksrc, timer);
+
+	switch (event) {
+	case PRE_RATE_CHANGE:
+		clocksource_unregister(&cs->clksrc);
+		break;
+
+	case POST_RATE_CHANGE:
+		clocksource_register_hz(&cs->clksrc, ndata->new_rate);
+		break;
+
+	default:
+		break;
+	}
+
+	return NOTIFY_DONE;
+}
+
+static int __init sun5i_setup_clocksource(struct device_node *node,
+					  void __iomem *base,
+					  struct clk *clk, int irq)
+{
+	struct sun5i_timer_clksrc *cs;
+	unsigned long rate;
+	int ret;
+
+	cs = kzalloc(sizeof(*cs), GFP_KERNEL);
+	if (!cs)
+		return -ENOMEM;
+
+	ret = clk_prepare_enable(clk);
+	if (ret) {
+		pr_err("Couldn't enable parent clock\n");
+		goto err_free;
+	}
+
+	rate = clk_get_rate(clk);
+
+	cs->timer.base = base;
+	cs->timer.clk = clk;
+	cs->timer.clk_rate_cb.notifier_call = sun5i_rate_cb_clksrc;
+	cs->timer.clk_rate_cb.next = NULL;
+
+	ret = clk_notifier_register(clk, &cs->timer.clk_rate_cb);
+	if (ret) {
+		pr_err("Unable to register clock notifier.\n");
+		goto err_disable_clk;
+	}
+
+	writel(~0, base + TIMER_INTVAL_LO_REG(1));
+	writel(TIMER_CTL_ENABLE | TIMER_CTL_RELOAD,
+	       base + TIMER_CTL_REG(1));
+
+	cs->clksrc.name = node->name;
+	cs->clksrc.rating = 340;
+	cs->clksrc.read = sun5i_clksrc_read;
+	cs->clksrc.mask = CLOCKSOURCE_MASK(32);
+	cs->clksrc.flags = CLOCK_SOURCE_IS_CONTINUOUS;
+
+	ret = clocksource_register_hz(&cs->clksrc, rate);
+	if (ret) {
+		pr_err("Couldn't register clock source.\n");
+		goto err_remove_notifier;
+	}
+
+	return 0;
+
+err_remove_notifier:
+	clk_notifier_unregister(clk, &cs->timer.clk_rate_cb);
+err_disable_clk:
+	clk_disable_unprepare(clk);
+err_free:
+	kfree(cs);
+	return ret;
+}
+
+static int sun5i_rate_cb_clkevt(struct notifier_block *nb,
+				unsigned long event, void *data)
+{
+	struct clk_notifier_data *ndata = data;
+	struct sun5i_timer *timer = to_sun5i_timer(nb);
+	struct sun5i_timer_clkevt *ce = container_of(timer, struct sun5i_timer_clkevt, timer);
+
+	if (event == POST_RATE_CHANGE) {
+		clockevents_update_freq(&ce->clkevt, ndata->new_rate);
+		ce->timer.ticks_per_jiffy = DIV_ROUND_UP(ndata->new_rate, HZ);
+	}
+
+	return NOTIFY_DONE;
+}
+
+static int __init sun5i_setup_clockevent(struct device_node *node, void __iomem *base,
+					 struct clk *clk, int irq)
+{
+	struct sun5i_timer_clkevt *ce;
+	unsigned long rate;
+	int ret;
+	u32 val;
+
+	ce = kzalloc(sizeof(*ce), GFP_KERNEL);
+	if (!ce)
+		return -ENOMEM;
+
+	ret = clk_prepare_enable(clk);
+	if (ret) {
+		pr_err("Couldn't enable parent clock\n");
+		goto err_free;
+	}
+
+	rate = clk_get_rate(clk);
+
+	ce->timer.base = base;
+	ce->timer.ticks_per_jiffy = DIV_ROUND_UP(rate, HZ);
+	ce->timer.clk = clk;
+	ce->timer.clk_rate_cb.notifier_call = sun5i_rate_cb_clkevt;
+	ce->timer.clk_rate_cb.next = NULL;
+
+	ret = clk_notifier_register(clk, &ce->timer.clk_rate_cb);
+	if (ret) {
+		pr_err("Unable to register clock notifier.\n");
+		goto err_disable_clk;
+	}
+
+	ce->clkevt.name = node->name;
+	ce->clkevt.features = CLOCK_EVT_FEAT_PERIODIC | CLOCK_EVT_FEAT_ONESHOT;
+	ce->clkevt.set_next_event = sun5i_clkevt_next_event;
+	ce->clkevt.set_mode = sun5i_clkevt_mode;
+	ce->clkevt.rating = 340;
+	ce->clkevt.irq = irq;
+	ce->clkevt.cpumask = cpu_possible_mask;
+
+	/* Enable timer0 interrupt */
+	val = readl(base + TIMER_IRQ_EN_REG);
+	writel(val | TIMER_IRQ_EN(0), base + TIMER_IRQ_EN_REG);
+
+	clockevents_config_and_register(&ce->clkevt, rate,
+					TIMER_SYNC_TICKS, 0xffffffff);
+
+	ret = request_irq(irq, sun5i_timer_interrupt, IRQF_TIMER | IRQF_IRQPOLL,
+			  "sun5i_timer0", ce);
+	if (ret) {
+		pr_err("Unable to register interrupt\n");
+		goto err_remove_notifier;
+	}
+
+	return 0;
+
+err_remove_notifier:
+	clk_notifier_unregister(clk, &ce->timer.clk_rate_cb);
+err_disable_clk:
+	clk_disable_unprepare(clk);
+err_free:
+	kfree(ce);
+	return ret;
 }
 
 static void __init sun5i_timer_init(struct device_node *node)
 {
 	struct reset_control *rstc;
-	unsigned long rate;
+	void __iomem *timer_base;
 	struct clk *clk;
-	int ret, irq;
-	u32 val;
+	int irq;
 
-	timer_base = of_iomap(node, 0);
+	timer_base = of_io_request_and_map(node, 0, of_node_full_name(node));
 	if (!timer_base)
 		panic("Can't map registers");
 
@@ -161,36 +334,13 @@ static void __init sun5i_timer_init(struct device_node *node)
 	clk = of_clk_get(node, 0);
 	if (IS_ERR(clk))
 		panic("Can't get timer clock");
-	clk_prepare_enable(clk);
-	rate = clk_get_rate(clk);
 
 	rstc = of_reset_control_get(node, NULL);
 	if (!IS_ERR(rstc))
 		reset_control_deassert(rstc);
 
-	writel(~0, timer_base + TIMER_INTVAL_LO_REG(1));
-	writel(TIMER_CTL_ENABLE | TIMER_CTL_RELOAD,
-	       timer_base + TIMER_CTL_REG(1));
-
-	sched_clock_register(sun5i_timer_sched_read, 32, rate);
-	clocksource_mmio_init(timer_base + TIMER_CNTVAL_LO_REG(1), node->name,
-			      rate, 340, 32, clocksource_mmio_readl_down);
-
-	ticks_per_jiffy = DIV_ROUND_UP(rate, HZ);
-
-	ret = setup_irq(irq, &sun5i_timer_irq);
-	if (ret)
-		pr_warn("failed to setup irq %d\n", irq);
-
-	/* Enable timer0 interrupt */
-	val = readl(timer_base + TIMER_IRQ_EN_REG);
-	writel(val | TIMER_IRQ_EN(0), timer_base + TIMER_IRQ_EN_REG);
-
-	sun5i_clockevent.cpumask = cpu_possible_mask;
-	sun5i_clockevent.irq = irq;
-
-	clockevents_config_and_register(&sun5i_clockevent, rate,
-					TIMER_SYNC_TICKS, 0xffffffff);
+	sun5i_setup_clocksource(node, timer_base, clk, irq);
+	sun5i_setup_clockevent(node, timer_base, clk, irq);
 }
 CLOCKSOURCE_OF_DECLARE(sun5i_a13, "allwinner,sun5i-a13-hstimer",
 		       sun5i_timer_init);
diff --git a/drivers/cpufreq/cpufreq.c b/drivers/cpufreq/cpufreq.c
index 28e59a48b35f..8ae655c364f4 100644
--- a/drivers/cpufreq/cpufreq.c
+++ b/drivers/cpufreq/cpufreq.c
@@ -1698,15 +1698,18 @@ void cpufreq_resume(void)
 		    || __cpufreq_governor(policy, CPUFREQ_GOV_LIMITS))
 			pr_err("%s: Failed to start governor for policy: %p\n",
 				__func__, policy);
-
-		/*
-		 * schedule call cpufreq_update_policy() for boot CPU, i.e. last
-		 * policy in list. It will verify that the current freq is in
-		 * sync with what we believe it to be.
-		 */
-		if (list_is_last(&policy->policy_list, &cpufreq_policy_list))
-			schedule_work(&policy->update);
 	}
+
+	/*
+	 * schedule call cpufreq_update_policy() for first-online CPU, as that
+	 * wouldn't be hotplugged-out on suspend. It will verify that the
+	 * current freq is in sync with what we believe it to be.
+	 */
+	policy = cpufreq_cpu_get_raw(cpumask_first(cpu_online_mask));
+	if (WARN_ON(!policy))
+		return;
+
+	schedule_work(&policy->update);
 }
 
 /**
diff --git a/drivers/cpufreq/exynos-cpufreq.c b/drivers/cpufreq/exynos-cpufreq.c
index 5e98c6b1f284..82d2fbb20f7e 100644
--- a/drivers/cpufreq/exynos-cpufreq.c
+++ b/drivers/cpufreq/exynos-cpufreq.c
@@ -159,7 +159,7 @@ static struct cpufreq_driver exynos_driver = {
 
 static int exynos_cpufreq_probe(struct platform_device *pdev)
 {
-	struct device_node *cpus, *np;
+	struct device_node *cpu0;
 	int ret = -EINVAL;
 
 	exynos_info = kzalloc(sizeof(*exynos_info), GFP_KERNEL);
@@ -206,28 +206,19 @@ static int exynos_cpufreq_probe(struct platform_device *pdev)
 	if (ret)
 		goto err_cpufreq_reg;
 
-	cpus = of_find_node_by_path("/cpus");
-	if (!cpus) {
-		pr_err("failed to find cpus node\n");
+	cpu0 = of_get_cpu_node(0, NULL);
+	if (!cpu0) {
+		pr_err("failed to find cpu0 node\n");
 		return 0;
 	}
 
-	np = of_get_next_child(cpus, NULL);
-	if (!np) {
-		pr_err("failed to find cpus child node\n");
-		of_node_put(cpus);
-		return 0;
-	}
-
-	if (of_find_property(np, "#cooling-cells", NULL)) {
-		cdev = of_cpufreq_cooling_register(np,
+	if (of_find_property(cpu0, "#cooling-cells", NULL)) {
+		cdev = of_cpufreq_cooling_register(cpu0,
 						   cpu_present_mask);
 		if (IS_ERR(cdev))
 			pr_err("running cpufreq without cooling device: %ld\n",
 			       PTR_ERR(cdev));
 	}
-	of_node_put(np);
-	of_node_put(cpus);
 
 	return 0;
 
diff --git a/drivers/cpufreq/ppc-corenet-cpufreq.c b/drivers/cpufreq/ppc-corenet-cpufreq.c
index bee5df7794d3..7cb4b766cf94 100644
--- a/drivers/cpufreq/ppc-corenet-cpufreq.c
+++ b/drivers/cpufreq/ppc-corenet-cpufreq.c
@@ -22,6 +22,8 @@
 #include <linux/smp.h>
 #include <sysdev/fsl_soc.h>
 
+#include <asm/smp.h>	/* for get_hard_smp_processor_id() in UP configs */
+
 /**
  * struct cpu_data - per CPU data struct
  * @parent: the parent node of cpu clock
diff --git a/drivers/cpuidle/cpuidle-mvebu-v7.c b/drivers/cpuidle/cpuidle-mvebu-v7.c
index 38e68618513a..980151f34707 100644
--- a/drivers/cpuidle/cpuidle-mvebu-v7.c
+++ b/drivers/cpuidle/cpuidle-mvebu-v7.c
@@ -37,11 +37,11 @@ static int mvebu_v7_enter_idle(struct cpuidle_device *dev,
 		deepidle = true;
 
 	ret = mvebu_v7_cpu_suspend(deepidle);
+	cpu_pm_exit();
+
 	if (ret)
 		return ret;
 
-	cpu_pm_exit();
-
 	return index;
 }
 
@@ -50,17 +50,17 @@ static struct cpuidle_driver armadaxp_idle_driver = {
 	.states[0]		= ARM_CPUIDLE_WFI_STATE,
 	.states[1]		= {
 		.enter			= mvebu_v7_enter_idle,
-		.exit_latency		= 10,
+		.exit_latency		= 100,
 		.power_usage		= 50,
-		.target_residency	= 100,
+		.target_residency	= 1000,
 		.name			= "MV CPU IDLE",
 		.desc			= "CPU power down",
 	},
 	.states[2]		= {
 		.enter			= mvebu_v7_enter_idle,
-		.exit_latency		= 100,
+		.exit_latency		= 1000,
 		.power_usage		= 5,
-		.target_residency	= 1000,
+		.target_residency	= 10000,
 		.flags			= MVEBU_V7_FLAG_DEEP_IDLE,
 		.name			= "MV CPU DEEP IDLE",
 		.desc			= "CPU and L2 Fabric power down",
diff --git a/drivers/cpuidle/cpuidle.c b/drivers/cpuidle/cpuidle.c
index 4d534582514e..7a73a279e179 100644
--- a/drivers/cpuidle/cpuidle.c
+++ b/drivers/cpuidle/cpuidle.c
@@ -44,6 +44,12 @@ void disable_cpuidle(void)
 	off = 1;
 }
 
+bool cpuidle_not_available(struct cpuidle_driver *drv,
+			   struct cpuidle_device *dev)
+{
+	return off || !initialized || !drv || !dev || !dev->enabled;
+}
+
 /**
  * cpuidle_play_dead - cpu off-lining
  *
@@ -66,14 +72,8 @@ int cpuidle_play_dead(void)
 	return -ENODEV;
 }
 
-/**
- * cpuidle_find_deepest_state - Find deepest state meeting specific conditions.
- * @drv: cpuidle driver for the given CPU.
- * @dev: cpuidle device for the given CPU.
- * @freeze: Whether or not the state should be suitable for suspend-to-idle.
- */
-static int cpuidle_find_deepest_state(struct cpuidle_driver *drv,
-				      struct cpuidle_device *dev, bool freeze)
+static int find_deepest_state(struct cpuidle_driver *drv,
+			      struct cpuidle_device *dev, bool freeze)
 {
 	unsigned int latency_req = 0;
 	int i, ret = freeze ? -1 : CPUIDLE_DRIVER_STATE_START - 1;
@@ -92,6 +92,17 @@ static int cpuidle_find_deepest_state(struct cpuidle_driver *drv,
 	return ret;
 }
 
+/**
+ * cpuidle_find_deepest_state - Find the deepest available idle state.
+ * @drv: cpuidle driver for the given CPU.
+ * @dev: cpuidle device for the given CPU.
+ */
+int cpuidle_find_deepest_state(struct cpuidle_driver *drv,
+			       struct cpuidle_device *dev)
+{
+	return find_deepest_state(drv, dev, false);
+}
+
 static void enter_freeze_proper(struct cpuidle_driver *drv,
 				struct cpuidle_device *dev, int index)
 {
@@ -113,15 +124,14 @@ static void enter_freeze_proper(struct cpuidle_driver *drv,
 
 /**
  * cpuidle_enter_freeze - Enter an idle state suitable for suspend-to-idle.
+ * @drv: cpuidle driver for the given CPU.
+ * @dev: cpuidle device for the given CPU.
  *
  * If there are states with the ->enter_freeze callback, find the deepest of
- * them and enter it with frozen tick.  Otherwise, find the deepest state
- * available and enter it normally.
+ * them and enter it with frozen tick.
  */
-void cpuidle_enter_freeze(void)
+int cpuidle_enter_freeze(struct cpuidle_driver *drv, struct cpuidle_device *dev)
 {
-	struct cpuidle_device *dev = __this_cpu_read(cpuidle_devices);
-	struct cpuidle_driver *drv = cpuidle_get_cpu_driver(dev);
 	int index;
 
 	/*
@@ -129,24 +139,11 @@ void cpuidle_enter_freeze(void)
 	 * that interrupts won't be enabled when it exits and allows the tick to
 	 * be frozen safely.
 	 */
-	index = cpuidle_find_deepest_state(drv, dev, true);
-	if (index >= 0) {
-		enter_freeze_proper(drv, dev, index);
-		return;
-	}
-
-	/*
-	 * It is not safe to freeze the tick, find the deepest state available
-	 * at all and try to enter it normally.
-	 */
-	index = cpuidle_find_deepest_state(drv, dev, false);
+	index = find_deepest_state(drv, dev, true);
 	if (index >= 0)
-		cpuidle_enter(drv, dev, index);
-	else
-		arch_cpu_idle();
+		enter_freeze_proper(drv, dev, index);
 
-	/* Interrupts are enabled again here. */
-	local_irq_disable();
+	return index;
 }
 
 /**
@@ -205,12 +202,6 @@ int cpuidle_enter_state(struct cpuidle_device *dev, struct cpuidle_driver *drv,
  */
 int cpuidle_select(struct cpuidle_driver *drv, struct cpuidle_device *dev)
 {
-	if (off || !initialized)
-		return -ENODEV;
-
-	if (!drv || !dev || !dev->enabled)
-		return -EBUSY;
-
 	return cpuidle_curr_governor->select(drv, dev);
 }
 
@@ -339,9 +330,6 @@ int cpuidle_enable_device(struct cpuidle_device *dev)
 	if (!dev->registered)
 		return -EINVAL;
 
-	if (!dev->state_count)
-		dev->state_count = drv->state_count;
-
 	ret = cpuidle_add_device_sysfs(dev);
 	if (ret)
 		return ret;
diff --git a/drivers/cpuidle/driver.c b/drivers/cpuidle/driver.c
index 2697e87d5b34..5db147859b90 100644
--- a/drivers/cpuidle/driver.c
+++ b/drivers/cpuidle/driver.c
@@ -13,7 +13,7 @@
 #include <linux/sched.h>
 #include <linux/cpuidle.h>
 #include <linux/cpumask.h>
-#include <linux/clockchips.h>
+#include <linux/tick.h>
 
 #include "cpuidle.h"
 
@@ -130,21 +130,20 @@ static inline void __cpuidle_unset_driver(struct cpuidle_driver *drv)
 #endif
 
 /**
- * cpuidle_setup_broadcast_timer - enable/disable the broadcast timer
+ * cpuidle_setup_broadcast_timer - enable/disable the broadcast timer on a cpu
  * @arg: a void pointer used to match the SMP cross call API
  *
- * @arg is used as a value of type 'long' with one of the two values:
- * - CLOCK_EVT_NOTIFY_BROADCAST_ON
- * - CLOCK_EVT_NOTIFY_BROADCAST_OFF
+ * If @arg is NULL broadcast is disabled otherwise enabled
  *
- * Set the broadcast timer notification for the current CPU.  This function
- * is executed per CPU by an SMP cross call.  It not supposed to be called
- * directly.
+ * This function is executed per CPU by an SMP cross call.  It's not
+ * supposed to be called directly.
  */
 static void cpuidle_setup_broadcast_timer(void *arg)
 {
-	int cpu = smp_processor_id();
-	clockevents_notify((long)(arg), &cpu);
+	if (arg)
+		tick_broadcast_enable();
+	else
+		tick_broadcast_disable();
 }
 
 /**
@@ -239,7 +238,7 @@ static int __cpuidle_register_driver(struct cpuidle_driver *drv)
 
 	if (drv->bctimer)
 		on_each_cpu_mask(drv->cpumask, cpuidle_setup_broadcast_timer,
-				 (void *)CLOCK_EVT_NOTIFY_BROADCAST_ON, 1);
+				 (void *)1, 1);
 
 	poll_idle_init(drv);
 
@@ -263,7 +262,7 @@ static void __cpuidle_unregister_driver(struct cpuidle_driver *drv)
 	if (drv->bctimer) {
 		drv->bctimer = 0;
 		on_each_cpu_mask(drv->cpumask, cpuidle_setup_broadcast_timer,
-				 (void *)CLOCK_EVT_NOTIFY_BROADCAST_OFF, 1);
+				 NULL, 1);
 	}
 
 	__cpuidle_unset_driver(drv);
diff --git a/drivers/cpuidle/sysfs.c b/drivers/cpuidle/sysfs.c
index 97c5903b4606..832a2c3f01ff 100644
--- a/drivers/cpuidle/sysfs.c
+++ b/drivers/cpuidle/sysfs.c
@@ -401,7 +401,7 @@ static int cpuidle_add_state_sysfs(struct cpuidle_device *device)
 	struct cpuidle_driver *drv = cpuidle_get_cpu_driver(device);
 
 	/* state statistics */
-	for (i = 0; i < device->state_count; i++) {
+	for (i = 0; i < drv->state_count; i++) {
 		kobj = kzalloc(sizeof(struct cpuidle_state_kobj), GFP_KERNEL);
 		if (!kobj)
 			goto error_state;
@@ -433,9 +433,10 @@ error_state:
  */
 static void cpuidle_remove_state_sysfs(struct cpuidle_device *device)
 {
+	struct cpuidle_driver *drv = cpuidle_get_cpu_driver(device);
 	int i;
 
-	for (i = 0; i < device->state_count; i++)
+	for (i = 0; i < drv->state_count; i++)
 		cpuidle_free_state_kobj(device, i);
 }
 
diff --git a/drivers/dma-buf/fence.c b/drivers/dma-buf/fence.c
index e5541117b3e9..50ef8bd8708b 100644
--- a/drivers/dma-buf/fence.c
+++ b/drivers/dma-buf/fence.c
@@ -159,6 +159,9 @@ fence_wait_timeout(struct fence *fence, bool intr, signed long timeout)
 	if (WARN_ON(timeout < 0))
 		return -EINVAL;
 
+	if (timeout == 0)
+		return fence_is_signaled(fence);
+
 	trace_fence_wait_start(fence);
 	ret = fence->ops->wait(fence, intr, timeout);
 	trace_fence_wait_end(fence);
diff --git a/drivers/dma-buf/reservation.c b/drivers/dma-buf/reservation.c
index 3c97c8fa8d02..39920d77f288 100644
--- a/drivers/dma-buf/reservation.c
+++ b/drivers/dma-buf/reservation.c
@@ -327,6 +327,9 @@ long reservation_object_wait_timeout_rcu(struct reservation_object *obj,
 	unsigned seq, shared_count, i = 0;
 	long ret = timeout;
 
+	if (!timeout)
+		return reservation_object_test_signaled_rcu(obj, wait_all);
+
 retry:
 	fence = NULL;
 	shared_count = 0;
@@ -402,8 +405,6 @@ reservation_object_test_signaled_single(struct fence *passed_fence)
 	int ret = 1;
 
 	if (!test_bit(FENCE_FLAG_SIGNALED_BIT, &lfence->flags)) {
-		int ret;
-
 		fence = fence_get_rcu(lfence);
 		if (!fence)
 			return -1;
diff --git a/drivers/dma/Kconfig b/drivers/dma/Kconfig
index a874b6ec6650..942ca541dcbd 100644
--- a/drivers/dma/Kconfig
+++ b/drivers/dma/Kconfig
@@ -51,19 +51,6 @@ config INTEL_MIC_X100_DMA
 	  OS and tools for MIC to use with this driver are available from
 	  <http://software.intel.com/en-us/mic-developer>.
 
-config INTEL_MID_DMAC
-	tristate "Intel MID DMA support for Peripheral DMA controllers"
-	depends on PCI && X86
-	select DMA_ENGINE
-	default n
-	help
-	  Enable support for the Intel(R) MID DMA engine present
-	  in Intel MID chipsets.
-
-	  Say Y here if you have such a chipset.
-
-	  If unsure, say N.
-
 config ASYNC_TX_ENABLE_CHANNEL_SWITCH
 	bool
 
diff --git a/drivers/dma/Makefile b/drivers/dma/Makefile
index f915f61ec574..539d4825bd76 100644
--- a/drivers/dma/Makefile
+++ b/drivers/dma/Makefile
@@ -6,7 +6,6 @@ obj-$(CONFIG_DMA_VIRTUAL_CHANNELS) += virt-dma.o
 obj-$(CONFIG_DMA_ACPI) += acpi-dma.o
 obj-$(CONFIG_DMA_OF) += of-dma.o
 
-obj-$(CONFIG_INTEL_MID_DMAC) += intel_mid_dma.o
 obj-$(CONFIG_DMATEST) += dmatest.o
 obj-$(CONFIG_INTEL_IOATDMA) += ioat/
 obj-$(CONFIG_INTEL_IOP_ADMA) += iop-adma.o
diff --git a/drivers/dma/amba-pl08x.c b/drivers/dma/amba-pl08x.c
index 4a5fd245014e..83aa55d6fa5d 100644
--- a/drivers/dma/amba-pl08x.c
+++ b/drivers/dma/amba-pl08x.c
@@ -97,6 +97,12 @@
 
 #define DRIVER_NAME	"pl08xdmac"
 
+#define PL80X_DMA_BUSWIDTHS \
+	BIT(DMA_SLAVE_BUSWIDTH_UNDEFINED) | \
+	BIT(DMA_SLAVE_BUSWIDTH_1_BYTE) | \
+	BIT(DMA_SLAVE_BUSWIDTH_2_BYTES) | \
+	BIT(DMA_SLAVE_BUSWIDTH_4_BYTES)
+
 static struct amba_driver pl08x_amba_driver;
 struct pl08x_driver_data;
 
@@ -2070,6 +2076,10 @@ static int pl08x_probe(struct amba_device *adev, const struct amba_id *id)
 	pl08x->memcpy.device_pause = pl08x_pause;
 	pl08x->memcpy.device_resume = pl08x_resume;
 	pl08x->memcpy.device_terminate_all = pl08x_terminate_all;
+	pl08x->memcpy.src_addr_widths = PL80X_DMA_BUSWIDTHS;
+	pl08x->memcpy.dst_addr_widths = PL80X_DMA_BUSWIDTHS;
+	pl08x->memcpy.directions = BIT(DMA_MEM_TO_MEM);
+	pl08x->memcpy.residue_granularity = DMA_RESIDUE_GRANULARITY_SEGMENT;
 
 	/* Initialize slave engine */
 	dma_cap_set(DMA_SLAVE, pl08x->slave.cap_mask);
@@ -2086,6 +2096,10 @@ static int pl08x_probe(struct amba_device *adev, const struct amba_id *id)
 	pl08x->slave.device_pause = pl08x_pause;
 	pl08x->slave.device_resume = pl08x_resume;
 	pl08x->slave.device_terminate_all = pl08x_terminate_all;
+	pl08x->slave.src_addr_widths = PL80X_DMA_BUSWIDTHS;
+	pl08x->slave.dst_addr_widths = PL80X_DMA_BUSWIDTHS;
+	pl08x->slave.directions = BIT(DMA_DEV_TO_MEM) | BIT(DMA_MEM_TO_DEV);
+	pl08x->slave.residue_granularity = DMA_RESIDUE_GRANULARITY_SEGMENT;
 
 	/* Get the platform data */
 	pl08x->pd = dev_get_platdata(&adev->dev);
diff --git a/drivers/dma/at_hdmac.c b/drivers/dma/at_hdmac.c
index 1e1a4c567542..0b4fc6fb48ce 100644
--- a/drivers/dma/at_hdmac.c
+++ b/drivers/dma/at_hdmac.c
@@ -238,93 +238,126 @@ static void atc_dostart(struct at_dma_chan *atchan, struct at_desc *first)
 }
 
 /*
- * atc_get_current_descriptors -
- * locate the descriptor which equal to physical address in DSCR
- * @atchan: the channel we want to start
- * @dscr_addr: physical descriptor address in DSCR
+ * atc_get_desc_by_cookie - get the descriptor of a cookie
+ * @atchan: the DMA channel
+ * @cookie: the cookie to get the descriptor for
  */
-static struct at_desc *atc_get_current_descriptors(struct at_dma_chan *atchan,
-							u32 dscr_addr)
+static struct at_desc *atc_get_desc_by_cookie(struct at_dma_chan *atchan,
+						dma_cookie_t cookie)
 {
-	struct at_desc  *desc, *_desc, *child, *desc_cur = NULL;
+	struct at_desc *desc, *_desc;
 
-	list_for_each_entry_safe(desc, _desc, &atchan->active_list, desc_node) {
-		if (desc->lli.dscr == dscr_addr) {
-			desc_cur = desc;
-			break;
-		}
+	list_for_each_entry_safe(desc, _desc, &atchan->queue, desc_node) {
+		if (desc->txd.cookie == cookie)
+			return desc;
+	}
 
-		list_for_each_entry(child, &desc->tx_list, desc_node) {
-			if (child->lli.dscr == dscr_addr) {
-				desc_cur = child;
-				break;
-			}
-		}
+	list_for_each_entry_safe(desc, _desc, &atchan->active_list, desc_node) {
+		if (desc->txd.cookie == cookie)
+			return desc;
 	}
 
-	return desc_cur;
+	return NULL;
 }
 
-/*
- * atc_get_bytes_left -
- * Get the number of bytes residue in dma buffer,
- * @chan: the channel we want to start
+/**
+ * atc_calc_bytes_left - calculates the number of bytes left according to the
+ * value read from CTRLA.
+ *
+ * @current_len: the number of bytes left before reading CTRLA
+ * @ctrla: the value of CTRLA
+ * @desc: the descriptor containing the transfer width
+ */
+static inline int atc_calc_bytes_left(int current_len, u32 ctrla,
+					struct at_desc *desc)
+{
+	return current_len - ((ctrla & ATC_BTSIZE_MAX) << desc->tx_width);
+}
+
+/**
+ * atc_calc_bytes_left_from_reg - calculates the number of bytes left according
+ * to the current value of CTRLA.
+ *
+ * @current_len: the number of bytes left before reading CTRLA
+ * @atchan: the channel to read CTRLA for
+ * @desc: the descriptor containing the transfer width
+ */
+static inline int atc_calc_bytes_left_from_reg(int current_len,
+			struct at_dma_chan *atchan, struct at_desc *desc)
+{
+	u32 ctrla = channel_readl(atchan, CTRLA);
+
+	return atc_calc_bytes_left(current_len, ctrla, desc);
+}
+
+/**
+ * atc_get_bytes_left - get the number of bytes residue for a cookie
+ * @chan: DMA channel
+ * @cookie: transaction identifier to check status of
  */
-static int atc_get_bytes_left(struct dma_chan *chan)
+static int atc_get_bytes_left(struct dma_chan *chan, dma_cookie_t cookie)
 {
 	struct at_dma_chan      *atchan = to_at_dma_chan(chan);
-	struct at_dma           *atdma = to_at_dma(chan->device);
-	int	chan_id = atchan->chan_common.chan_id;
 	struct at_desc *desc_first = atc_first_active(atchan);
-	struct at_desc *desc_cur;
-	int ret = 0, count = 0;
+	struct at_desc *desc;
+	int ret;
+	u32 ctrla, dscr;
 
 	/*
-	 * Initialize necessary values in the first time.
-	 * remain_desc record remain desc length.
+	 * If the cookie doesn't match to the currently running transfer then
+	 * we can return the total length of the associated DMA transfer,
+	 * because it is still queued.
 	 */
-	if (atchan->remain_desc == 0)
-		/* First descriptor embedds the transaction length */
-		atchan->remain_desc = desc_first->len;
+	desc = atc_get_desc_by_cookie(atchan, cookie);
+	if (desc == NULL)
+		return -EINVAL;
+	else if (desc != desc_first)
+		return desc->total_len;
 
-	/*
-	 * This happens when current descriptor transfer complete.
-	 * The residual buffer size should reduce current descriptor length.
-	 */
-	if (unlikely(test_bit(ATC_IS_BTC, &atchan->status))) {
-		clear_bit(ATC_IS_BTC, &atchan->status);
-		desc_cur = atc_get_current_descriptors(atchan,
-						channel_readl(atchan, DSCR));
-		if (!desc_cur) {
-			ret = -EINVAL;
-			goto out;
-		}
+	/* cookie matches to the currently running transfer */
+	ret = desc_first->total_len;
 
-		count = (desc_cur->lli.ctrla & ATC_BTSIZE_MAX)
-			<< desc_first->tx_width;
-		if (atchan->remain_desc < count) {
-			ret = -EINVAL;
-			goto out;
+	if (desc_first->lli.dscr) {
+		/* hardware linked list transfer */
+
+		/*
+		 * Calculate the residue by removing the length of the child
+		 * descriptors already transferred from the total length.
+		 * To get the current child descriptor we can use the value of
+		 * the channel's DSCR register and compare it against the value
+		 * of the hardware linked list structure of each child
+		 * descriptor.
+		 */
+
+		ctrla = channel_readl(atchan, CTRLA);
+		rmb(); /* ensure CTRLA is read before DSCR */
+		dscr = channel_readl(atchan, DSCR);
+
+		/* for the first descriptor we can be more accurate */
+		if (desc_first->lli.dscr == dscr)
+			return atc_calc_bytes_left(ret, ctrla, desc_first);
+
+		ret -= desc_first->len;
+		list_for_each_entry(desc, &desc_first->tx_list, desc_node) {
+			if (desc->lli.dscr == dscr)
+				break;
+
+			ret -= desc->len;
 		}
 
-		atchan->remain_desc -= count;
-		ret = atchan->remain_desc;
-	} else {
 		/*
-		 * Get residual bytes when current
-		 * descriptor transfer in progress.
+		 * For the last descriptor in the chain we can calculate
+		 * the remaining bytes using the channel's register.
+		 * Note that the transfer width of the first and last
+		 * descriptor may differ.
 		 */
-		count = (channel_readl(atchan, CTRLA) & ATC_BTSIZE_MAX)
-				<< (desc_first->tx_width);
-		ret = atchan->remain_desc - count;
+		if (!desc->lli.dscr)
+			ret = atc_calc_bytes_left_from_reg(ret, atchan, desc);
+	} else {
+		/* single transfer */
+		ret = atc_calc_bytes_left_from_reg(ret, atchan, desc_first);
 	}
-	/*
-	 * Check fifo empty.
-	 */
-	if (!(dma_readl(atdma, CHSR) & AT_DMA_EMPT(chan_id)))
-		atc_issue_pending(chan);
 
-out:
 	return ret;
 }
 
@@ -539,8 +572,6 @@ static irqreturn_t at_dma_interrupt(int irq, void *dev_id)
 					/* Give information to tasklet */
 					set_bit(ATC_IS_ERROR, &atchan->status);
 				}
-				if (pending & AT_DMA_BTC(i))
-					set_bit(ATC_IS_BTC, &atchan->status);
 				tasklet_schedule(&atchan->tasklet);
 				ret = IRQ_HANDLED;
 			}
@@ -653,14 +684,18 @@ atc_prep_dma_memcpy(struct dma_chan *chan, dma_addr_t dest, dma_addr_t src,
 		desc->lli.ctrlb = ctrlb;
 
 		desc->txd.cookie = 0;
+		desc->len = xfer_count << src_width;
 
 		atc_desc_chain(&first, &prev, desc);
 	}
 
 	/* First descriptor of the chain embedds additional information */
 	first->txd.cookie = -EBUSY;
-	first->len = len;
+	first->total_len = len;
+
+	/* set transfer width for the calculation of the residue */
 	first->tx_width = src_width;
+	prev->tx_width = src_width;
 
 	/* set end-of-link to the last link descriptor of list*/
 	set_desc_eol(desc);
@@ -752,6 +787,7 @@ atc_prep_slave_sg(struct dma_chan *chan, struct scatterlist *sgl,
 					| ATC_SRC_WIDTH(mem_width)
 					| len >> mem_width;
 			desc->lli.ctrlb = ctrlb;
+			desc->len = len;
 
 			atc_desc_chain(&first, &prev, desc);
 			total_len += len;
@@ -792,6 +828,7 @@ atc_prep_slave_sg(struct dma_chan *chan, struct scatterlist *sgl,
 					| ATC_DST_WIDTH(mem_width)
 					| len >> reg_width;
 			desc->lli.ctrlb = ctrlb;
+			desc->len = len;
 
 			atc_desc_chain(&first, &prev, desc);
 			total_len += len;
@@ -806,8 +843,11 @@ atc_prep_slave_sg(struct dma_chan *chan, struct scatterlist *sgl,
 
 	/* First descriptor of the chain embedds additional information */
 	first->txd.cookie = -EBUSY;
-	first->len = total_len;
+	first->total_len = total_len;
+
+	/* set transfer width for the calculation of the residue */
 	first->tx_width = reg_width;
+	prev->tx_width = reg_width;
 
 	/* first link descriptor of list is responsible of flags */
 	first->txd.flags = flags; /* client is in control of this ack */
@@ -872,6 +912,7 @@ atc_dma_cyclic_fill_desc(struct dma_chan *chan, struct at_desc *desc,
 				| ATC_FC_MEM2PER
 				| ATC_SIF(atchan->mem_if)
 				| ATC_DIF(atchan->per_if);
+		desc->len = period_len;
 		break;
 
 	case DMA_DEV_TO_MEM:
@@ -883,6 +924,7 @@ atc_dma_cyclic_fill_desc(struct dma_chan *chan, struct at_desc *desc,
 				| ATC_FC_PER2MEM
 				| ATC_SIF(atchan->per_if)
 				| ATC_DIF(atchan->mem_if);
+		desc->len = period_len;
 		break;
 
 	default:
@@ -964,7 +1006,7 @@ atc_prep_dma_cyclic(struct dma_chan *chan, dma_addr_t buf_addr, size_t buf_len,
 
 	/* First descriptor of the chain embedds additional information */
 	first->txd.cookie = -EBUSY;
-	first->len = buf_len;
+	first->total_len = buf_len;
 	first->tx_width = reg_width;
 
 	return &first->txd;
@@ -1118,7 +1160,7 @@ atc_tx_status(struct dma_chan *chan,
 	spin_lock_irqsave(&atchan->lock, flags);
 
 	/*  Get number of bytes left in the active transactions */
-	bytes = atc_get_bytes_left(chan);
+	bytes = atc_get_bytes_left(chan, cookie);
 
 	spin_unlock_irqrestore(&atchan->lock, flags);
 
@@ -1214,7 +1256,6 @@ static int atc_alloc_chan_resources(struct dma_chan *chan)
 
 	spin_lock_irqsave(&atchan->lock, flags);
 	atchan->descs_allocated = i;
-	atchan->remain_desc = 0;
 	list_splice(&tmp_list, &atchan->free_list);
 	dma_cookie_init(chan);
 	spin_unlock_irqrestore(&atchan->lock, flags);
@@ -1257,7 +1298,6 @@ static void atc_free_chan_resources(struct dma_chan *chan)
 	list_splice_init(&atchan->free_list, &list);
 	atchan->descs_allocated = 0;
 	atchan->status = 0;
-	atchan->remain_desc = 0;
 
 	dev_vdbg(chan2dev(chan), "free_chan_resources: done\n");
 }
diff --git a/drivers/dma/at_hdmac_regs.h b/drivers/dma/at_hdmac_regs.h
index d6bba6c636c2..2727ca560572 100644
--- a/drivers/dma/at_hdmac_regs.h
+++ b/drivers/dma/at_hdmac_regs.h
@@ -181,8 +181,9 @@ struct at_lli {
  * @at_lli: hardware lli structure
  * @txd: support for the async_tx api
  * @desc_node: node on the channed descriptors list
- * @len: total transaction bytecount
+ * @len: descriptor byte count
  * @tx_width: transfer width
+ * @total_len: total transaction byte count
  */
 struct at_desc {
 	/* FIRST values the hardware uses */
@@ -194,6 +195,7 @@ struct at_desc {
 	struct list_head		desc_node;
 	size_t				len;
 	u32				tx_width;
+	size_t				total_len;
 };
 
 static inline struct at_desc *
@@ -213,7 +215,6 @@ txd_to_at_desc(struct dma_async_tx_descriptor *txd)
 enum atc_status {
 	ATC_IS_ERROR = 0,
 	ATC_IS_PAUSED = 1,
-	ATC_IS_BTC = 2,
 	ATC_IS_CYCLIC = 24,
 };
 
@@ -231,7 +232,6 @@ enum atc_status {
  * @save_cfg: configuration register that is saved on suspend/resume cycle
  * @save_dscr: for cyclic operations, preserve next descriptor address in
  *             the cyclic list on suspend/resume cycle
- * @remain_desc: to save remain desc length
  * @dma_sconfig: configuration for slave transfers, passed via
  * .device_config
  * @lock: serializes enqueue/dequeue operations to descriptors lists
@@ -251,7 +251,6 @@ struct at_dma_chan {
 	struct tasklet_struct	tasklet;
 	u32			save_cfg;
 	u32			save_dscr;
-	u32			remain_desc;
 	struct dma_slave_config dma_sconfig;
 
 	spinlock_t		lock;
diff --git a/drivers/dma/at_xdmac.c b/drivers/dma/at_xdmac.c
index 09e2825a547a..d9891d3461f6 100644
--- a/drivers/dma/at_xdmac.c
+++ b/drivers/dma/at_xdmac.c
@@ -664,7 +664,6 @@ at_xdmac_prep_dma_cyclic(struct dma_chan *chan, dma_addr_t buf_addr,
 	struct at_xdmac_desc	*first = NULL, *prev = NULL;
 	unsigned int		periods = buf_len / period_len;
 	int			i;
-	u32			cfg;
 
 	dev_dbg(chan2dev(chan), "%s: buf_addr=%pad, buf_len=%zd, period_len=%zd, dir=%s, flags=0x%lx\n",
 		__func__, &buf_addr, buf_len, period_len,
@@ -700,17 +699,17 @@ at_xdmac_prep_dma_cyclic(struct dma_chan *chan, dma_addr_t buf_addr,
 		if (direction == DMA_DEV_TO_MEM) {
 			desc->lld.mbr_sa = atchan->per_src_addr;
 			desc->lld.mbr_da = buf_addr + i * period_len;
-			cfg = atchan->cfg[AT_XDMAC_DEV_TO_MEM_CFG];
+			desc->lld.mbr_cfg = atchan->cfg[AT_XDMAC_DEV_TO_MEM_CFG];
 		} else {
 			desc->lld.mbr_sa = buf_addr + i * period_len;
 			desc->lld.mbr_da = atchan->per_dst_addr;
-			cfg = atchan->cfg[AT_XDMAC_MEM_TO_DEV_CFG];
+			desc->lld.mbr_cfg = atchan->cfg[AT_XDMAC_MEM_TO_DEV_CFG];
 		}
 		desc->lld.mbr_ubc = AT_XDMAC_MBR_UBC_NDV1
 			| AT_XDMAC_MBR_UBC_NDEN
 			| AT_XDMAC_MBR_UBC_NSEN
 			| AT_XDMAC_MBR_UBC_NDE
-			| period_len >> at_xdmac_get_dwidth(cfg);
+			| period_len >> at_xdmac_get_dwidth(desc->lld.mbr_cfg);
 
 		dev_dbg(chan2dev(chan),
 			 "%s: lld: mbr_sa=%pad, mbr_da=%pad, mbr_ubc=0x%08x\n",
diff --git a/drivers/dma/bcm2835-dma.c b/drivers/dma/bcm2835-dma.c
index 0723096fb50a..c92d6a70ccf3 100644
--- a/drivers/dma/bcm2835-dma.c
+++ b/drivers/dma/bcm2835-dma.c
@@ -475,6 +475,7 @@ static int bcm2835_dma_terminate_all(struct dma_chan *chan)
 	 * c->desc is NULL and exit.)
 	 */
 	if (c->desc) {
+		bcm2835_dma_desc_free(&c->desc->vd);
 		c->desc = NULL;
 		bcm2835_dma_abort(c->chan_base);
 
diff --git a/drivers/dma/cppi41.c b/drivers/dma/cppi41.c
index 512cb8e2805e..ceedafbd23e0 100644
--- a/drivers/dma/cppi41.c
+++ b/drivers/dma/cppi41.c
@@ -903,6 +903,11 @@ static const struct cppi_glue_infos *get_glue_info(struct device *dev)
 	return of_id->data;
 }
 
+#define CPPI41_DMA_BUSWIDTHS	(BIT(DMA_SLAVE_BUSWIDTH_1_BYTE) | \
+				BIT(DMA_SLAVE_BUSWIDTH_2_BYTES) | \
+				BIT(DMA_SLAVE_BUSWIDTH_3_BYTES) | \
+				BIT(DMA_SLAVE_BUSWIDTH_4_BYTES))
+
 static int cppi41_dma_probe(struct platform_device *pdev)
 {
 	struct cppi41_dd *cdd;
@@ -926,6 +931,10 @@ static int cppi41_dma_probe(struct platform_device *pdev)
 	cdd->ddev.device_issue_pending = cppi41_dma_issue_pending;
 	cdd->ddev.device_prep_slave_sg = cppi41_dma_prep_slave_sg;
 	cdd->ddev.device_terminate_all = cppi41_stop_chan;
+	cdd->ddev.directions = BIT(DMA_DEV_TO_MEM) | BIT(DMA_MEM_TO_DEV);
+	cdd->ddev.src_addr_widths = CPPI41_DMA_BUSWIDTHS;
+	cdd->ddev.dst_addr_widths = CPPI41_DMA_BUSWIDTHS;
+	cdd->ddev.residue_granularity = DMA_RESIDUE_GRANULARITY_BURST;
 	cdd->ddev.dev = dev;
 	INIT_LIST_HEAD(&cdd->ddev.channels);
 	cpp41_dma_info.dma_cap = cdd->ddev.cap_mask;
diff --git a/drivers/dma/dma-jz4740.c b/drivers/dma/dma-jz4740.c
index 4527a3ebeac4..84884418fd30 100644
--- a/drivers/dma/dma-jz4740.c
+++ b/drivers/dma/dma-jz4740.c
@@ -511,6 +511,9 @@ static void jz4740_dma_desc_free(struct virt_dma_desc *vdesc)
 	kfree(container_of(vdesc, struct jz4740_dma_desc, vdesc));
 }
 
+#define JZ4740_DMA_BUSWIDTHS (BIT(DMA_SLAVE_BUSWIDTH_1_BYTE) | \
+	BIT(DMA_SLAVE_BUSWIDTH_2_BYTES) | BIT(DMA_SLAVE_BUSWIDTH_4_BYTES))
+
 static int jz4740_dma_probe(struct platform_device *pdev)
 {
 	struct jz4740_dmaengine_chan *chan;
@@ -548,6 +551,10 @@ static int jz4740_dma_probe(struct platform_device *pdev)
 	dd->device_prep_dma_cyclic = jz4740_dma_prep_dma_cyclic;
 	dd->device_config = jz4740_dma_slave_config;
 	dd->device_terminate_all = jz4740_dma_terminate_all;
+	dd->src_addr_widths = JZ4740_DMA_BUSWIDTHS;
+	dd->dst_addr_widths = JZ4740_DMA_BUSWIDTHS;
+	dd->directions = BIT(DMA_DEV_TO_MEM) | BIT(DMA_MEM_TO_DEV);
+	dd->residue_granularity = DMA_RESIDUE_GRANULARITY_BURST;
 	dd->dev = &pdev->dev;
 	INIT_LIST_HEAD(&dd->channels);
 
diff --git a/drivers/dma/dmaengine.c b/drivers/dma/dmaengine.c
index f15712f2fec6..ac336a961dea 100644
--- a/drivers/dma/dmaengine.c
+++ b/drivers/dma/dmaengine.c
@@ -859,9 +859,6 @@ int dma_async_device_register(struct dma_device *device)
 	BUG_ON(!device->device_issue_pending);
 	BUG_ON(!device->dev);
 
-	WARN(dma_has_cap(DMA_SLAVE, device->cap_mask) && !device->directions,
-	     "this driver doesn't support generic slave capabilities reporting\n");
-
 	/* note: this only matters in the
 	 * CONFIG_ASYNC_TX_ENABLE_CHANNEL_SWITCH=n case
 	 */
diff --git a/drivers/dma/dw/core.c b/drivers/dma/dw/core.c
index 455b7a4f1e87..a8ad05291b27 100644
--- a/drivers/dma/dw/core.c
+++ b/drivers/dma/dw/core.c
@@ -626,7 +626,7 @@ static irqreturn_t dw_dma_interrupt(int irq, void *dev_id)
 	dev_vdbg(dw->dma.dev, "%s: status=0x%x\n", __func__, status);
 
 	/* Check if we have any interrupt from the DMAC */
-	if (!status)
+	if (!status || !dw->in_use)
 		return IRQ_NONE;
 
 	/*
diff --git a/drivers/dma/dw/platform.c b/drivers/dma/dw/platform.c
index 6565a361e7e5..b2c3ae071429 100644
--- a/drivers/dma/dw/platform.c
+++ b/drivers/dma/dw/platform.c
@@ -26,6 +26,8 @@
 
 #include "internal.h"
 
+#define DRV_NAME	"dw_dmac"
+
 static struct dma_chan *dw_dma_of_xlate(struct of_phandle_args *dma_spec,
 					struct of_dma *ofdma)
 {
@@ -284,7 +286,7 @@ static struct platform_driver dw_driver = {
 	.remove		= dw_remove,
 	.shutdown       = dw_shutdown,
 	.driver = {
-		.name	= "dw_dmac",
+		.name	= DRV_NAME,
 		.pm	= &dw_dev_pm_ops,
 		.of_match_table = of_match_ptr(dw_dma_of_id_table),
 		.acpi_match_table = ACPI_PTR(dw_dma_acpi_id_table),
@@ -305,3 +307,4 @@ module_exit(dw_exit);
 
 MODULE_LICENSE("GPL v2");
 MODULE_DESCRIPTION("Synopsys DesignWare DMA Controller platform driver");
+MODULE_ALIAS("platform:" DRV_NAME);
diff --git a/drivers/dma/edma.c b/drivers/dma/edma.c
index 276157f22612..53dbd3b3384c 100644
--- a/drivers/dma/edma.c
+++ b/drivers/dma/edma.c
@@ -260,6 +260,13 @@ static int edma_terminate_all(struct dma_chan *chan)
 	 */
 	if (echan->edesc) {
 		int cyclic = echan->edesc->cyclic;
+
+		/*
+		 * free the running request descriptor
+		 * since it is not in any of the vdesc lists
+		 */
+		edma_desc_free(&echan->edesc->vdesc);
+
 		echan->edesc = NULL;
 		edma_stop(echan->ch_num);
 		/* Move the cyclic channel back to default queue */
diff --git a/drivers/dma/imx-sdma.c b/drivers/dma/imx-sdma.c
index 18c0a131e4e4..66a0efb9651d 100644
--- a/drivers/dma/imx-sdma.c
+++ b/drivers/dma/imx-sdma.c
@@ -531,6 +531,10 @@ static int sdma_run_channel0(struct sdma_engine *sdma)
 		dev_err(sdma->dev, "Timeout waiting for CH0 ready\n");
 	}
 
+	/* Set bits of CONFIG register with dynamic context switching */
+	if (readl(sdma->regs + SDMA_H_CONFIG) == 0)
+		writel_relaxed(SDMA_H_CONFIG_CSM, sdma->regs + SDMA_H_CONFIG);
+
 	return ret ? 0 : -ETIMEDOUT;
 }
 
@@ -1394,9 +1398,6 @@ static int sdma_init(struct sdma_engine *sdma)
 
 	writel_relaxed(ccb_phys, sdma->regs + SDMA_H_C0PTR);
 
-	/* Set bits of CONFIG register with given context switching mode */
-	writel_relaxed(SDMA_H_CONFIG_CSM, sdma->regs + SDMA_H_CONFIG);
-
 	/* Initializes channel's priorities */
 	sdma_set_channel_priority(&sdma->channel[0], 7);
 
diff --git a/drivers/dma/intel_mid_dma.c b/drivers/dma/intel_mid_dma.c
deleted file mode 100644
index 5aaead9b56f7..000000000000
--- a/drivers/dma/intel_mid_dma.c
+++ /dev/null
@@ -1,1447 +0,0 @@
-/*
- *  intel_mid_dma.c - Intel Langwell DMA Drivers
- *
- *  Copyright (C) 2008-10 Intel Corp
- *  Author: Vinod Koul <vinod.koul@intel.com>
- *  The driver design is based on dw_dmac driver
- *  ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
- *
- *  This program is free software; you can redistribute it and/or modify
- *  it under the terms of the GNU General Public License as published by
- *  the Free Software Foundation; version 2 of the License.
- *
- *  This program is distributed in the hope that it will be useful, but
- *  WITHOUT ANY WARRANTY; without even the implied warranty of
- *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- *  General Public License for more details.
- *
- *  You should have received a copy of the GNU General Public License along
- *  with this program; if not, write to the Free Software Foundation, Inc.,
- *  59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
- *
- * ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
- *
- *
- */
-#include <linux/pci.h>
-#include <linux/interrupt.h>
-#include <linux/pm_runtime.h>
-#include <linux/intel_mid_dma.h>
-#include <linux/module.h>
-
-#include "dmaengine.h"
-
-#define MAX_CHAN	4 /*max ch across controllers*/
-#include "intel_mid_dma_regs.h"
-
-#define INTEL_MID_DMAC1_ID		0x0814
-#define INTEL_MID_DMAC2_ID		0x0813
-#define INTEL_MID_GP_DMAC2_ID		0x0827
-#define INTEL_MFLD_DMAC1_ID		0x0830
-#define LNW_PERIPHRAL_MASK_BASE		0xFFAE8008
-#define LNW_PERIPHRAL_MASK_SIZE		0x10
-#define LNW_PERIPHRAL_STATUS		0x0
-#define LNW_PERIPHRAL_MASK		0x8
-
-struct intel_mid_dma_probe_info {
-	u8 max_chan;
-	u8 ch_base;
-	u16 block_size;
-	u32 pimr_mask;
-};
-
-#define INFO(_max_chan, _ch_base, _block_size, _pimr_mask) \
-	((kernel_ulong_t)&(struct intel_mid_dma_probe_info) {	\
-		.max_chan = (_max_chan),			\
-		.ch_base = (_ch_base),				\
-		.block_size = (_block_size),			\
-		.pimr_mask = (_pimr_mask),			\
-	})
-
-/*****************************************************************************
-Utility Functions*/
-/**
- * get_ch_index	-	convert status to channel
- * @status: status mask
- * @base: dma ch base value
- *
- * Modify the status mask and return the channel index needing
- * attention (or -1 if neither)
- */
-static int get_ch_index(int *status, unsigned int base)
-{
-	int i;
-	for (i = 0; i < MAX_CHAN; i++) {
-		if (*status & (1 << (i + base))) {
-			*status = *status & ~(1 << (i + base));
-			pr_debug("MDMA: index %d New status %x\n", i, *status);
-			return i;
-		}
-	}
-	return -1;
-}
-
-/**
- * get_block_ts	-	calculates dma transaction length
- * @len: dma transfer length
- * @tx_width: dma transfer src width
- * @block_size: dma controller max block size
- *
- * Based on src width calculate the DMA trsaction length in data items
- * return data items or FFFF if exceeds max length for block
- */
-static int get_block_ts(int len, int tx_width, int block_size)
-{
-	int byte_width = 0, block_ts = 0;
-
-	switch (tx_width) {
-	case DMA_SLAVE_BUSWIDTH_1_BYTE:
-		byte_width = 1;
-		break;
-	case DMA_SLAVE_BUSWIDTH_2_BYTES:
-		byte_width = 2;
-		break;
-	case DMA_SLAVE_BUSWIDTH_4_BYTES:
-	default:
-		byte_width = 4;
-		break;
-	}
-
-	block_ts = len/byte_width;
-	if (block_ts > block_size)
-		block_ts = 0xFFFF;
-	return block_ts;
-}
-
-/*****************************************************************************
-DMAC1 interrupt Functions*/
-
-/**
- * dmac1_mask_periphral_intr -	mask the periphral interrupt
- * @mid: dma device for which masking is required
- *
- * Masks the DMA periphral interrupt
- * this is valid for DMAC1 family controllers only
- * This controller should have periphral mask registers already mapped
- */
-static void dmac1_mask_periphral_intr(struct middma_device *mid)
-{
-	u32 pimr;
-
-	if (mid->pimr_mask) {
-		pimr = readl(mid->mask_reg + LNW_PERIPHRAL_MASK);
-		pimr |= mid->pimr_mask;
-		writel(pimr, mid->mask_reg + LNW_PERIPHRAL_MASK);
-	}
-	return;
-}
-
-/**
- * dmac1_unmask_periphral_intr -	unmask the periphral interrupt
- * @midc: dma channel for which masking is required
- *
- * UnMasks the DMA periphral interrupt,
- * this is valid for DMAC1 family controllers only
- * This controller should have periphral mask registers already mapped
- */
-static void dmac1_unmask_periphral_intr(struct intel_mid_dma_chan *midc)
-{
-	u32 pimr;
-	struct middma_device *mid = to_middma_device(midc->chan.device);
-
-	if (mid->pimr_mask) {
-		pimr = readl(mid->mask_reg + LNW_PERIPHRAL_MASK);
-		pimr &= ~mid->pimr_mask;
-		writel(pimr, mid->mask_reg + LNW_PERIPHRAL_MASK);
-	}
-	return;
-}
-
-/**
- * enable_dma_interrupt -	enable the periphral interrupt
- * @midc: dma channel for which enable interrupt is required
- *
- * Enable the DMA periphral interrupt,
- * this is valid for DMAC1 family controllers only
- * This controller should have periphral mask registers already mapped
- */
-static void enable_dma_interrupt(struct intel_mid_dma_chan *midc)
-{
-	dmac1_unmask_periphral_intr(midc);
-
-	/*en ch interrupts*/
-	iowrite32(UNMASK_INTR_REG(midc->ch_id), midc->dma_base + MASK_TFR);
-	iowrite32(UNMASK_INTR_REG(midc->ch_id), midc->dma_base + MASK_ERR);
-	return;
-}
-
-/**
- * disable_dma_interrupt -	disable the periphral interrupt
- * @midc: dma channel for which disable interrupt is required
- *
- * Disable the DMA periphral interrupt,
- * this is valid for DMAC1 family controllers only
- * This controller should have periphral mask registers already mapped
- */
-static void disable_dma_interrupt(struct intel_mid_dma_chan *midc)
-{
-	/*Check LPE PISR, make sure fwd is disabled*/
-	iowrite32(MASK_INTR_REG(midc->ch_id), midc->dma_base + MASK_BLOCK);
-	iowrite32(MASK_INTR_REG(midc->ch_id), midc->dma_base + MASK_TFR);
-	iowrite32(MASK_INTR_REG(midc->ch_id), midc->dma_base + MASK_ERR);
-	return;
-}
-
-/*****************************************************************************
-DMA channel helper Functions*/
-/**
- * mid_desc_get		-	get a descriptor
- * @midc: dma channel for which descriptor is required
- *
- * Obtain a descriptor for the channel. Returns NULL if none are free.
- * Once the descriptor is returned it is private until put on another
- * list or freed
- */
-static struct intel_mid_dma_desc *midc_desc_get(struct intel_mid_dma_chan *midc)
-{
-	struct intel_mid_dma_desc *desc, *_desc;
-	struct intel_mid_dma_desc *ret = NULL;
-
-	spin_lock_bh(&midc->lock);
-	list_for_each_entry_safe(desc, _desc, &midc->free_list, desc_node) {
-		if (async_tx_test_ack(&desc->txd)) {
-			list_del(&desc->desc_node);
-			ret = desc;
-			break;
-		}
-	}
-	spin_unlock_bh(&midc->lock);
-	return ret;
-}
-
-/**
- * mid_desc_put		-	put a descriptor
- * @midc: dma channel for which descriptor is required
- * @desc: descriptor to put
- *
- * Return a descriptor from lwn_desc_get back to the free pool
- */
-static void midc_desc_put(struct intel_mid_dma_chan *midc,
-			struct intel_mid_dma_desc *desc)
-{
-	if (desc) {
-		spin_lock_bh(&midc->lock);
-		list_add_tail(&desc->desc_node, &midc->free_list);
-		spin_unlock_bh(&midc->lock);
-	}
-}
-/**
- * midc_dostart		-		begin a DMA transaction
- * @midc: channel for which txn is to be started
- * @first: first descriptor of series
- *
- * Load a transaction into the engine. This must be called with midc->lock
- * held and bh disabled.
- */
-static void midc_dostart(struct intel_mid_dma_chan *midc,
-			struct intel_mid_dma_desc *first)
-{
-	struct middma_device *mid = to_middma_device(midc->chan.device);
-
-	/*  channel is idle */
-	if (midc->busy && test_ch_en(midc->dma_base, midc->ch_id)) {
-		/*error*/
-		pr_err("ERR_MDMA: channel is busy in start\n");
-		/* The tasklet will hopefully advance the queue... */
-		return;
-	}
-	midc->busy = true;
-	/*write registers and en*/
-	iowrite32(first->sar, midc->ch_regs + SAR);
-	iowrite32(first->dar, midc->ch_regs + DAR);
-	iowrite32(first->lli_phys, midc->ch_regs + LLP);
-	iowrite32(first->cfg_hi, midc->ch_regs + CFG_HIGH);
-	iowrite32(first->cfg_lo, midc->ch_regs + CFG_LOW);
-	iowrite32(first->ctl_lo, midc->ch_regs + CTL_LOW);
-	iowrite32(first->ctl_hi, midc->ch_regs + CTL_HIGH);
-	pr_debug("MDMA:TX SAR %x,DAR %x,CFGL %x,CFGH %x,CTLH %x, CTLL %x\n",
-		(int)first->sar, (int)first->dar, first->cfg_hi,
-		first->cfg_lo, first->ctl_hi, first->ctl_lo);
-	first->status = DMA_IN_PROGRESS;
-
-	iowrite32(ENABLE_CHANNEL(midc->ch_id), mid->dma_base + DMA_CHAN_EN);
-}
-
-/**
- * midc_descriptor_complete	-	process completed descriptor
- * @midc: channel owning the descriptor
- * @desc: the descriptor itself
- *
- * Process a completed descriptor and perform any callbacks upon
- * the completion. The completion handling drops the lock during the
- * callbacks but must be called with the lock held.
- */
-static void midc_descriptor_complete(struct intel_mid_dma_chan *midc,
-		struct intel_mid_dma_desc *desc)
-		__releases(&midc->lock) __acquires(&midc->lock)
-{
-	struct dma_async_tx_descriptor	*txd = &desc->txd;
-	dma_async_tx_callback callback_txd = NULL;
-	struct intel_mid_dma_lli	*llitem;
-	void *param_txd = NULL;
-
-	dma_cookie_complete(txd);
-	callback_txd = txd->callback;
-	param_txd = txd->callback_param;
-
-	if (desc->lli != NULL) {
-		/*clear the DONE bit of completed LLI in memory*/
-		llitem = desc->lli + desc->current_lli;
-		llitem->ctl_hi &= CLEAR_DONE;
-		if (desc->current_lli < desc->lli_length-1)
-			(desc->current_lli)++;
-		else
-			desc->current_lli = 0;
-	}
-	spin_unlock_bh(&midc->lock);
-	if (callback_txd) {
-		pr_debug("MDMA: TXD callback set ... calling\n");
-		callback_txd(param_txd);
-	}
-	if (midc->raw_tfr) {
-		desc->status = DMA_COMPLETE;
-		if (desc->lli != NULL) {
-			pci_pool_free(desc->lli_pool, desc->lli,
-						desc->lli_phys);
-			pci_pool_destroy(desc->lli_pool);
-			desc->lli = NULL;
-		}
-		list_move(&desc->desc_node, &midc->free_list);
-		midc->busy = false;
-	}
-	spin_lock_bh(&midc->lock);
-
-}
-/**
- * midc_scan_descriptors -		check the descriptors in channel
- *					mark completed when tx is completete
- * @mid: device
- * @midc: channel to scan
- *
- * Walk the descriptor chain for the device and process any entries
- * that are complete.
- */
-static void midc_scan_descriptors(struct middma_device *mid,
-				struct intel_mid_dma_chan *midc)
-{
-	struct intel_mid_dma_desc *desc = NULL, *_desc = NULL;
-
-	/*tx is complete*/
-	list_for_each_entry_safe(desc, _desc, &midc->active_list, desc_node) {
-		if (desc->status == DMA_IN_PROGRESS)
-			midc_descriptor_complete(midc, desc);
-	}
-	return;
-	}
-/**
- * midc_lli_fill_sg -		Helper function to convert
- *				SG list to Linked List Items.
- *@midc: Channel
- *@desc: DMA descriptor
- *@sglist: Pointer to SG list
- *@sglen: SG list length
- *@flags: DMA transaction flags
- *
- * Walk through the SG list and convert the SG list into Linked
- * List Items (LLI).
- */
-static int midc_lli_fill_sg(struct intel_mid_dma_chan *midc,
-				struct intel_mid_dma_desc *desc,
-				struct scatterlist *sglist,
-				unsigned int sglen,
-				unsigned int flags)
-{
-	struct intel_mid_dma_slave *mids;
-	struct scatterlist  *sg;
-	dma_addr_t lli_next, sg_phy_addr;
-	struct intel_mid_dma_lli *lli_bloc_desc;
-	union intel_mid_dma_ctl_lo ctl_lo;
-	union intel_mid_dma_ctl_hi ctl_hi;
-	int i;
-
-	pr_debug("MDMA: Entered midc_lli_fill_sg\n");
-	mids = midc->mid_slave;
-
-	lli_bloc_desc = desc->lli;
-	lli_next = desc->lli_phys;
-
-	ctl_lo.ctl_lo = desc->ctl_lo;
-	ctl_hi.ctl_hi = desc->ctl_hi;
-	for_each_sg(sglist, sg, sglen, i) {
-		/*Populate CTL_LOW and LLI values*/
-		if (i != sglen - 1) {
-			lli_next = lli_next +
-				sizeof(struct intel_mid_dma_lli);
-		} else {
-		/*Check for circular list, otherwise terminate LLI to ZERO*/
-			if (flags & DMA_PREP_CIRCULAR_LIST) {
-				pr_debug("MDMA: LLI is configured in circular mode\n");
-				lli_next = desc->lli_phys;
-			} else {
-				lli_next = 0;
-				ctl_lo.ctlx.llp_dst_en = 0;
-				ctl_lo.ctlx.llp_src_en = 0;
-			}
-		}
-		/*Populate CTL_HI values*/
-		ctl_hi.ctlx.block_ts = get_block_ts(sg_dma_len(sg),
-							desc->width,
-							midc->dma->block_size);
-		/*Populate SAR and DAR values*/
-		sg_phy_addr = sg_dma_address(sg);
-		if (desc->dirn ==  DMA_MEM_TO_DEV) {
-			lli_bloc_desc->sar  = sg_phy_addr;
-			lli_bloc_desc->dar  = mids->dma_slave.dst_addr;
-		} else if (desc->dirn ==  DMA_DEV_TO_MEM) {
-			lli_bloc_desc->sar  = mids->dma_slave.src_addr;
-			lli_bloc_desc->dar  = sg_phy_addr;
-		}
-		/*Copy values into block descriptor in system memroy*/
-		lli_bloc_desc->llp = lli_next;
-		lli_bloc_desc->ctl_lo = ctl_lo.ctl_lo;
-		lli_bloc_desc->ctl_hi = ctl_hi.ctl_hi;
-
-		lli_bloc_desc++;
-	}
-	/*Copy very first LLI values to descriptor*/
-	desc->ctl_lo = desc->lli->ctl_lo;
-	desc->ctl_hi = desc->lli->ctl_hi;
-	desc->sar = desc->lli->sar;
-	desc->dar = desc->lli->dar;
-
-	return 0;
-}
-/*****************************************************************************
-DMA engine callback Functions*/
-/**
- * intel_mid_dma_tx_submit -	callback to submit DMA transaction
- * @tx: dma engine descriptor
- *
- * Submit the DMA transaction for this descriptor, start if ch idle
- */
-static dma_cookie_t intel_mid_dma_tx_submit(struct dma_async_tx_descriptor *tx)
-{
-	struct intel_mid_dma_desc	*desc = to_intel_mid_dma_desc(tx);
-	struct intel_mid_dma_chan	*midc = to_intel_mid_dma_chan(tx->chan);
-	dma_cookie_t		cookie;
-
-	spin_lock_bh(&midc->lock);
-	cookie = dma_cookie_assign(tx);
-
-	if (list_empty(&midc->active_list))
-		list_add_tail(&desc->desc_node, &midc->active_list);
-	else
-		list_add_tail(&desc->desc_node, &midc->queue);
-
-	midc_dostart(midc, desc);
-	spin_unlock_bh(&midc->lock);
-
-	return cookie;
-}
-
-/**
- * intel_mid_dma_issue_pending -	callback to issue pending txn
- * @chan: chan where pending trascation needs to be checked and submitted
- *
- * Call for scan to issue pending descriptors
- */
-static void intel_mid_dma_issue_pending(struct dma_chan *chan)
-{
-	struct intel_mid_dma_chan	*midc = to_intel_mid_dma_chan(chan);
-
-	spin_lock_bh(&midc->lock);
-	if (!list_empty(&midc->queue))
-		midc_scan_descriptors(to_middma_device(chan->device), midc);
-	spin_unlock_bh(&midc->lock);
-}
-
-/**
- * intel_mid_dma_tx_status -	Return status of txn
- * @chan: chan for where status needs to be checked
- * @cookie: cookie for txn
- * @txstate: DMA txn state
- *
- * Return status of DMA txn
- */
-static enum dma_status intel_mid_dma_tx_status(struct dma_chan *chan,
-						dma_cookie_t cookie,
-						struct dma_tx_state *txstate)
-{
-	struct intel_mid_dma_chan *midc = to_intel_mid_dma_chan(chan);
-	enum dma_status ret;
-
-	ret = dma_cookie_status(chan, cookie, txstate);
-	if (ret != DMA_COMPLETE) {
-		spin_lock_bh(&midc->lock);
-		midc_scan_descriptors(to_middma_device(chan->device), midc);
-		spin_unlock_bh(&midc->lock);
-
-		ret = dma_cookie_status(chan, cookie, txstate);
-	}
-
-	return ret;
-}
-
-static int intel_mid_dma_config(struct dma_chan *chan,
-				struct dma_slave_config *slave)
-{
-	struct intel_mid_dma_chan	*midc = to_intel_mid_dma_chan(chan);
-	struct intel_mid_dma_slave *mid_slave;
-
-	BUG_ON(!midc);
-	BUG_ON(!slave);
-	pr_debug("MDMA: slave control called\n");
-
-	mid_slave = to_intel_mid_dma_slave(slave);
-
-	BUG_ON(!mid_slave);
-
-	midc->mid_slave = mid_slave;
-	return 0;
-}
-
-static int intel_mid_dma_terminate_all(struct dma_chan *chan)
-{
-	struct intel_mid_dma_chan	*midc = to_intel_mid_dma_chan(chan);
-	struct middma_device	*mid = to_middma_device(chan->device);
-	struct intel_mid_dma_desc	*desc, *_desc;
-	union intel_mid_dma_cfg_lo cfg_lo;
-
-	spin_lock_bh(&midc->lock);
-	if (midc->busy == false) {
-		spin_unlock_bh(&midc->lock);
-		return 0;
-	}
-	/*Suspend and disable the channel*/
-	cfg_lo.cfg_lo = ioread32(midc->ch_regs + CFG_LOW);
-	cfg_lo.cfgx.ch_susp = 1;
-	iowrite32(cfg_lo.cfg_lo, midc->ch_regs + CFG_LOW);
-	iowrite32(DISABLE_CHANNEL(midc->ch_id), mid->dma_base + DMA_CHAN_EN);
-	midc->busy = false;
-	/* Disable interrupts */
-	disable_dma_interrupt(midc);
-	midc->descs_allocated = 0;
-
-	spin_unlock_bh(&midc->lock);
-	list_for_each_entry_safe(desc, _desc, &midc->active_list, desc_node) {
-		if (desc->lli != NULL) {
-			pci_pool_free(desc->lli_pool, desc->lli,
-						desc->lli_phys);
-			pci_pool_destroy(desc->lli_pool);
-			desc->lli = NULL;
-		}
-		list_move(&desc->desc_node, &midc->free_list);
-	}
-	return 0;
-}
-
-
-/**
- * intel_mid_dma_prep_memcpy -	Prep memcpy txn
- * @chan: chan for DMA transfer
- * @dest: destn address
- * @src: src address
- * @len: DMA transfer len
- * @flags: DMA flags
- *
- * Perform a DMA memcpy. Note we support slave periphral DMA transfers only
- * The periphral txn details should be filled in slave structure properly
- * Returns the descriptor for this txn
- */
-static struct dma_async_tx_descriptor *intel_mid_dma_prep_memcpy(
-			struct dma_chan *chan, dma_addr_t dest,
-			dma_addr_t src, size_t len, unsigned long flags)
-{
-	struct intel_mid_dma_chan *midc;
-	struct intel_mid_dma_desc *desc = NULL;
-	struct intel_mid_dma_slave *mids;
-	union intel_mid_dma_ctl_lo ctl_lo;
-	union intel_mid_dma_ctl_hi ctl_hi;
-	union intel_mid_dma_cfg_lo cfg_lo;
-	union intel_mid_dma_cfg_hi cfg_hi;
-	enum dma_slave_buswidth width;
-
-	pr_debug("MDMA: Prep for memcpy\n");
-	BUG_ON(!chan);
-	if (!len)
-		return NULL;
-
-	midc = to_intel_mid_dma_chan(chan);
-	BUG_ON(!midc);
-
-	mids = midc->mid_slave;
-	BUG_ON(!mids);
-
-	pr_debug("MDMA:called for DMA %x CH %d Length %zu\n",
-				midc->dma->pci_id, midc->ch_id, len);
-	pr_debug("MDMA:Cfg passed Mode %x, Dirn %x, HS %x, Width %x\n",
-			mids->cfg_mode, mids->dma_slave.direction,
-			mids->hs_mode, mids->dma_slave.src_addr_width);
-
-	/*calculate CFG_LO*/
-	if (mids->hs_mode == LNW_DMA_SW_HS) {
-		cfg_lo.cfg_lo = 0;
-		cfg_lo.cfgx.hs_sel_dst = 1;
-		cfg_lo.cfgx.hs_sel_src = 1;
-	} else if (mids->hs_mode == LNW_DMA_HW_HS)
-		cfg_lo.cfg_lo = 0x00000;
-
-	/*calculate CFG_HI*/
-	if (mids->cfg_mode == LNW_DMA_MEM_TO_MEM) {
-		/*SW HS only*/
-		cfg_hi.cfg_hi = 0;
-	} else {
-		cfg_hi.cfg_hi = 0;
-		if (midc->dma->pimr_mask) {
-			cfg_hi.cfgx.protctl = 0x0; /*default value*/
-			cfg_hi.cfgx.fifo_mode = 1;
-			if (mids->dma_slave.direction == DMA_MEM_TO_DEV) {
-				cfg_hi.cfgx.src_per = 0;
-				if (mids->device_instance == 0)
-					cfg_hi.cfgx.dst_per = 3;
-				if (mids->device_instance == 1)
-					cfg_hi.cfgx.dst_per = 1;
-			} else if (mids->dma_slave.direction == DMA_DEV_TO_MEM) {
-				if (mids->device_instance == 0)
-					cfg_hi.cfgx.src_per = 2;
-				if (mids->device_instance == 1)
-					cfg_hi.cfgx.src_per = 0;
-				cfg_hi.cfgx.dst_per = 0;
-			}
-		} else {
-			cfg_hi.cfgx.protctl = 0x1; /*default value*/
-			cfg_hi.cfgx.src_per = cfg_hi.cfgx.dst_per =
-					midc->ch_id - midc->dma->chan_base;
-		}
-	}
-
-	/*calculate CTL_HI*/
-	ctl_hi.ctlx.reser = 0;
-	ctl_hi.ctlx.done  = 0;
-	width = mids->dma_slave.src_addr_width;
-
-	ctl_hi.ctlx.block_ts = get_block_ts(len, width, midc->dma->block_size);
-	pr_debug("MDMA:calc len %d for block size %d\n",
-				ctl_hi.ctlx.block_ts, midc->dma->block_size);
-	/*calculate CTL_LO*/
-	ctl_lo.ctl_lo = 0;
-	ctl_lo.ctlx.int_en = 1;
-	ctl_lo.ctlx.dst_msize = mids->dma_slave.src_maxburst;
-	ctl_lo.ctlx.src_msize = mids->dma_slave.dst_maxburst;
-
-	/*
-	 * Here we need some translation from "enum dma_slave_buswidth"
-	 * to the format for our dma controller
-	 *		standard	intel_mid_dmac's format
-	 *		 1 Byte			0b000
-	 *		 2 Bytes		0b001
-	 *		 4 Bytes		0b010
-	 */
-	ctl_lo.ctlx.dst_tr_width = mids->dma_slave.dst_addr_width / 2;
-	ctl_lo.ctlx.src_tr_width = mids->dma_slave.src_addr_width / 2;
-
-	if (mids->cfg_mode == LNW_DMA_MEM_TO_MEM) {
-		ctl_lo.ctlx.tt_fc = 0;
-		ctl_lo.ctlx.sinc = 0;
-		ctl_lo.ctlx.dinc = 0;
-	} else {
-		if (mids->dma_slave.direction == DMA_MEM_TO_DEV) {
-			ctl_lo.ctlx.sinc = 0;
-			ctl_lo.ctlx.dinc = 2;
-			ctl_lo.ctlx.tt_fc = 1;
-		} else if (mids->dma_slave.direction == DMA_DEV_TO_MEM) {
-			ctl_lo.ctlx.sinc = 2;
-			ctl_lo.ctlx.dinc = 0;
-			ctl_lo.ctlx.tt_fc = 2;
-		}
-	}
-
-	pr_debug("MDMA:Calc CTL LO %x, CTL HI %x, CFG LO %x, CFG HI %x\n",
-		ctl_lo.ctl_lo, ctl_hi.ctl_hi, cfg_lo.cfg_lo, cfg_hi.cfg_hi);
-
-	enable_dma_interrupt(midc);
-
-	desc = midc_desc_get(midc);
-	if (desc == NULL)
-		goto err_desc_get;
-	desc->sar = src;
-	desc->dar = dest ;
-	desc->len = len;
-	desc->cfg_hi = cfg_hi.cfg_hi;
-	desc->cfg_lo = cfg_lo.cfg_lo;
-	desc->ctl_lo = ctl_lo.ctl_lo;
-	desc->ctl_hi = ctl_hi.ctl_hi;
-	desc->width = width;
-	desc->dirn = mids->dma_slave.direction;
-	desc->lli_phys = 0;
-	desc->lli = NULL;
-	desc->lli_pool = NULL;
-	return &desc->txd;
-
-err_desc_get:
-	pr_err("ERR_MDMA: Failed to get desc\n");
-	midc_desc_put(midc, desc);
-	return NULL;
-}
-/**
- * intel_mid_dma_prep_slave_sg -	Prep slave sg txn
- * @chan: chan for DMA transfer
- * @sgl: scatter gather list
- * @sg_len: length of sg txn
- * @direction: DMA transfer dirtn
- * @flags: DMA flags
- * @context: transfer context (ignored)
- *
- * Prepares LLI based periphral transfer
- */
-static struct dma_async_tx_descriptor *intel_mid_dma_prep_slave_sg(
-			struct dma_chan *chan, struct scatterlist *sgl,
-			unsigned int sg_len, enum dma_transfer_direction direction,
-			unsigned long flags, void *context)
-{
-	struct intel_mid_dma_chan *midc = NULL;
-	struct intel_mid_dma_slave *mids = NULL;
-	struct intel_mid_dma_desc *desc = NULL;
-	struct dma_async_tx_descriptor *txd = NULL;
-	union intel_mid_dma_ctl_lo ctl_lo;
-
-	pr_debug("MDMA: Prep for slave SG\n");
-
-	if (!sg_len) {
-		pr_err("MDMA: Invalid SG length\n");
-		return NULL;
-	}
-	midc = to_intel_mid_dma_chan(chan);
-	BUG_ON(!midc);
-
-	mids = midc->mid_slave;
-	BUG_ON(!mids);
-
-	if (!midc->dma->pimr_mask) {
-		/* We can still handle sg list with only one item */
-		if (sg_len == 1) {
-			txd = intel_mid_dma_prep_memcpy(chan,
-						mids->dma_slave.dst_addr,
-						mids->dma_slave.src_addr,
-						sg_dma_len(sgl),
-						flags);
-			return txd;
-		} else {
-			pr_warn("MDMA: SG list is not supported by this controller\n");
-			return  NULL;
-		}
-	}
-
-	pr_debug("MDMA: SG Length = %d, direction = %d, Flags = %#lx\n",
-			sg_len, direction, flags);
-
-	txd = intel_mid_dma_prep_memcpy(chan, 0, 0, sg_dma_len(sgl), flags);
-	if (NULL == txd) {
-		pr_err("MDMA: Prep memcpy failed\n");
-		return NULL;
-	}
-
-	desc = to_intel_mid_dma_desc(txd);
-	desc->dirn = direction;
-	ctl_lo.ctl_lo = desc->ctl_lo;
-	ctl_lo.ctlx.llp_dst_en = 1;
-	ctl_lo.ctlx.llp_src_en = 1;
-	desc->ctl_lo = ctl_lo.ctl_lo;
-	desc->lli_length = sg_len;
-	desc->current_lli = 0;
-	/* DMA coherent memory pool for LLI descriptors*/
-	desc->lli_pool = pci_pool_create("intel_mid_dma_lli_pool",
-				midc->dma->pdev,
-				(sizeof(struct intel_mid_dma_lli)*sg_len),
-				32, 0);
-	if (NULL == desc->lli_pool) {
-		pr_err("MID_DMA:LLI pool create failed\n");
-		return NULL;
-	}
-
-	desc->lli = pci_pool_alloc(desc->lli_pool, GFP_KERNEL, &desc->lli_phys);
-	if (!desc->lli) {
-		pr_err("MID_DMA: LLI alloc failed\n");
-		pci_pool_destroy(desc->lli_pool);
-		return NULL;
-	}
-
-	midc_lli_fill_sg(midc, desc, sgl, sg_len, flags);
-	if (flags & DMA_PREP_INTERRUPT) {
-		iowrite32(UNMASK_INTR_REG(midc->ch_id),
-				midc->dma_base + MASK_BLOCK);
-		pr_debug("MDMA:Enabled Block interrupt\n");
-	}
-	return &desc->txd;
-}
-
-/**
- * intel_mid_dma_free_chan_resources -	Frees dma resources
- * @chan: chan requiring attention
- *
- * Frees the allocated resources on this DMA chan
- */
-static void intel_mid_dma_free_chan_resources(struct dma_chan *chan)
-{
-	struct intel_mid_dma_chan	*midc = to_intel_mid_dma_chan(chan);
-	struct middma_device	*mid = to_middma_device(chan->device);
-	struct intel_mid_dma_desc	*desc, *_desc;
-
-	if (true == midc->busy) {
-		/*trying to free ch in use!!!!!*/
-		pr_err("ERR_MDMA: trying to free ch in use\n");
-	}
-	spin_lock_bh(&midc->lock);
-	midc->descs_allocated = 0;
-	list_for_each_entry_safe(desc, _desc, &midc->active_list, desc_node) {
-		list_del(&desc->desc_node);
-		pci_pool_free(mid->dma_pool, desc, desc->txd.phys);
-	}
-	list_for_each_entry_safe(desc, _desc, &midc->free_list, desc_node) {
-		list_del(&desc->desc_node);
-		pci_pool_free(mid->dma_pool, desc, desc->txd.phys);
-	}
-	list_for_each_entry_safe(desc, _desc, &midc->queue, desc_node) {
-		list_del(&desc->desc_node);
-		pci_pool_free(mid->dma_pool, desc, desc->txd.phys);
-	}
-	spin_unlock_bh(&midc->lock);
-	midc->in_use = false;
-	midc->busy = false;
-	/* Disable CH interrupts */
-	iowrite32(MASK_INTR_REG(midc->ch_id), mid->dma_base + MASK_BLOCK);
-	iowrite32(MASK_INTR_REG(midc->ch_id), mid->dma_base + MASK_ERR);
-	pm_runtime_put(&mid->pdev->dev);
-}
-
-/**
- * intel_mid_dma_alloc_chan_resources -	Allocate dma resources
- * @chan: chan requiring attention
- *
- * Allocates DMA resources on this chan
- * Return the descriptors allocated
- */
-static int intel_mid_dma_alloc_chan_resources(struct dma_chan *chan)
-{
-	struct intel_mid_dma_chan	*midc = to_intel_mid_dma_chan(chan);
-	struct middma_device	*mid = to_middma_device(chan->device);
-	struct intel_mid_dma_desc	*desc;
-	dma_addr_t		phys;
-	int	i = 0;
-
-	pm_runtime_get_sync(&mid->pdev->dev);
-
-	if (mid->state == SUSPENDED) {
-		if (dma_resume(&mid->pdev->dev)) {
-			pr_err("ERR_MDMA: resume failed");
-			return -EFAULT;
-		}
-	}
-
-	/* ASSERT:  channel is idle */
-	if (test_ch_en(mid->dma_base, midc->ch_id)) {
-		/*ch is not idle*/
-		pr_err("ERR_MDMA: ch not idle\n");
-		pm_runtime_put(&mid->pdev->dev);
-		return -EIO;
-	}
-	dma_cookie_init(chan);
-
-	spin_lock_bh(&midc->lock);
-	while (midc->descs_allocated < DESCS_PER_CHANNEL) {
-		spin_unlock_bh(&midc->lock);
-		desc = pci_pool_alloc(mid->dma_pool, GFP_KERNEL, &phys);
-		if (!desc) {
-			pr_err("ERR_MDMA: desc failed\n");
-			pm_runtime_put(&mid->pdev->dev);
-			return -ENOMEM;
-			/*check*/
-		}
-		dma_async_tx_descriptor_init(&desc->txd, chan);
-		desc->txd.tx_submit = intel_mid_dma_tx_submit;
-		desc->txd.flags = DMA_CTRL_ACK;
-		desc->txd.phys = phys;
-		spin_lock_bh(&midc->lock);
-		i = ++midc->descs_allocated;
-		list_add_tail(&desc->desc_node, &midc->free_list);
-	}
-	spin_unlock_bh(&midc->lock);
-	midc->in_use = true;
-	midc->busy = false;
-	pr_debug("MID_DMA: Desc alloc done ret: %d desc\n", i);
-	return i;
-}
-
-/**
- * midc_handle_error -	Handle DMA txn error
- * @mid: controller where error occurred
- * @midc: chan where error occurred
- *
- * Scan the descriptor for error
- */
-static void midc_handle_error(struct middma_device *mid,
-		struct intel_mid_dma_chan *midc)
-{
-	midc_scan_descriptors(mid, midc);
-}
-
-/**
- * dma_tasklet -	DMA interrupt tasklet
- * @data: tasklet arg (the controller structure)
- *
- * Scan the controller for interrupts for completion/error
- * Clear the interrupt and call for handling completion/error
- */
-static void dma_tasklet(unsigned long data)
-{
-	struct middma_device *mid = NULL;
-	struct intel_mid_dma_chan *midc = NULL;
-	u32 status, raw_tfr, raw_block;
-	int i;
-
-	mid = (struct middma_device *)data;
-	if (mid == NULL) {
-		pr_err("ERR_MDMA: tasklet Null param\n");
-		return;
-	}
-	pr_debug("MDMA: in tasklet for device %x\n", mid->pci_id);
-	raw_tfr = ioread32(mid->dma_base + RAW_TFR);
-	raw_block = ioread32(mid->dma_base + RAW_BLOCK);
-	status = raw_tfr | raw_block;
-	status &= mid->intr_mask;
-	while (status) {
-		/*txn interrupt*/
-		i = get_ch_index(&status, mid->chan_base);
-		if (i < 0) {
-			pr_err("ERR_MDMA:Invalid ch index %x\n", i);
-			return;
-		}
-		midc = &mid->ch[i];
-		if (midc == NULL) {
-			pr_err("ERR_MDMA:Null param midc\n");
-			return;
-		}
-		pr_debug("MDMA:Tx complete interrupt %x, Ch No %d Index %d\n",
-				status, midc->ch_id, i);
-		midc->raw_tfr = raw_tfr;
-		midc->raw_block = raw_block;
-		spin_lock_bh(&midc->lock);
-		/*clearing this interrupts first*/
-		iowrite32((1 << midc->ch_id), mid->dma_base + CLEAR_TFR);
-		if (raw_block) {
-			iowrite32((1 << midc->ch_id),
-				mid->dma_base + CLEAR_BLOCK);
-		}
-		midc_scan_descriptors(mid, midc);
-		pr_debug("MDMA:Scan of desc... complete, unmasking\n");
-		iowrite32(UNMASK_INTR_REG(midc->ch_id),
-				mid->dma_base + MASK_TFR);
-		if (raw_block) {
-			iowrite32(UNMASK_INTR_REG(midc->ch_id),
-				mid->dma_base + MASK_BLOCK);
-		}
-		spin_unlock_bh(&midc->lock);
-	}
-
-	status = ioread32(mid->dma_base + RAW_ERR);
-	status &= mid->intr_mask;
-	while (status) {
-		/*err interrupt*/
-		i = get_ch_index(&status, mid->chan_base);
-		if (i < 0) {
-			pr_err("ERR_MDMA:Invalid ch index %x\n", i);
-			return;
-		}
-		midc = &mid->ch[i];
-		if (midc == NULL) {
-			pr_err("ERR_MDMA:Null param midc\n");
-			return;
-		}
-		pr_debug("MDMA:Tx complete interrupt %x, Ch No %d Index %d\n",
-				status, midc->ch_id, i);
-
-		iowrite32((1 << midc->ch_id), mid->dma_base + CLEAR_ERR);
-		spin_lock_bh(&midc->lock);
-		midc_handle_error(mid, midc);
-		iowrite32(UNMASK_INTR_REG(midc->ch_id),
-				mid->dma_base + MASK_ERR);
-		spin_unlock_bh(&midc->lock);
-	}
-	pr_debug("MDMA:Exiting takslet...\n");
-	return;
-}
-
-static void dma_tasklet1(unsigned long data)
-{
-	pr_debug("MDMA:in takslet1...\n");
-	return dma_tasklet(data);
-}
-
-static void dma_tasklet2(unsigned long data)
-{
-	pr_debug("MDMA:in takslet2...\n");
-	return dma_tasklet(data);
-}
-
-/**
- * intel_mid_dma_interrupt -	DMA ISR
- * @irq: IRQ where interrupt occurred
- * @data: ISR cllback data (the controller structure)
- *
- * See if this is our interrupt if so then schedule the tasklet
- * otherwise ignore
- */
-static irqreturn_t intel_mid_dma_interrupt(int irq, void *data)
-{
-	struct middma_device *mid = data;
-	u32 tfr_status, err_status;
-	int call_tasklet = 0;
-
-	tfr_status = ioread32(mid->dma_base + RAW_TFR);
-	err_status = ioread32(mid->dma_base + RAW_ERR);
-	if (!tfr_status && !err_status)
-		return IRQ_NONE;
-
-	/*DMA Interrupt*/
-	pr_debug("MDMA:Got an interrupt on irq %d\n", irq);
-	pr_debug("MDMA: Status %x, Mask %x\n", tfr_status, mid->intr_mask);
-	tfr_status &= mid->intr_mask;
-	if (tfr_status) {
-		/*need to disable intr*/
-		iowrite32((tfr_status << INT_MASK_WE), mid->dma_base + MASK_TFR);
-		iowrite32((tfr_status << INT_MASK_WE), mid->dma_base + MASK_BLOCK);
-		pr_debug("MDMA: Calling tasklet %x\n", tfr_status);
-		call_tasklet = 1;
-	}
-	err_status &= mid->intr_mask;
-	if (err_status) {
-		iowrite32((err_status << INT_MASK_WE),
-			  mid->dma_base + MASK_ERR);
-		call_tasklet = 1;
-	}
-	if (call_tasklet)
-		tasklet_schedule(&mid->tasklet);
-
-	return IRQ_HANDLED;
-}
-
-static irqreturn_t intel_mid_dma_interrupt1(int irq, void *data)
-{
-	return intel_mid_dma_interrupt(irq, data);
-}
-
-static irqreturn_t intel_mid_dma_interrupt2(int irq, void *data)
-{
-	return intel_mid_dma_interrupt(irq, data);
-}
-
-/**
- * mid_setup_dma -	Setup the DMA controller
- * @pdev: Controller PCI device structure
- *
- * Initialize the DMA controller, channels, registers with DMA engine,
- * ISR. Initialize DMA controller channels.
- */
-static int mid_setup_dma(struct pci_dev *pdev)
-{
-	struct middma_device *dma = pci_get_drvdata(pdev);
-	int err, i;
-
-	/* DMA coherent memory pool for DMA descriptor allocations */
-	dma->dma_pool = pci_pool_create("intel_mid_dma_desc_pool", pdev,
-					sizeof(struct intel_mid_dma_desc),
-					32, 0);
-	if (NULL == dma->dma_pool) {
-		pr_err("ERR_MDMA:pci_pool_create failed\n");
-		err = -ENOMEM;
-		goto err_dma_pool;
-	}
-
-	INIT_LIST_HEAD(&dma->common.channels);
-	dma->pci_id = pdev->device;
-	if (dma->pimr_mask) {
-		dma->mask_reg = ioremap(LNW_PERIPHRAL_MASK_BASE,
-					LNW_PERIPHRAL_MASK_SIZE);
-		if (dma->mask_reg == NULL) {
-			pr_err("ERR_MDMA:Can't map periphral intr space !!\n");
-			err = -ENOMEM;
-			goto err_ioremap;
-		}
-	} else
-		dma->mask_reg = NULL;
-
-	pr_debug("MDMA:Adding %d channel for this controller\n", dma->max_chan);
-	/*init CH structures*/
-	dma->intr_mask = 0;
-	dma->state = RUNNING;
-	for (i = 0; i < dma->max_chan; i++) {
-		struct intel_mid_dma_chan *midch = &dma->ch[i];
-
-		midch->chan.device = &dma->common;
-		dma_cookie_init(&midch->chan);
-		midch->ch_id = dma->chan_base + i;
-		pr_debug("MDMA:Init CH %d, ID %d\n", i, midch->ch_id);
-
-		midch->dma_base = dma->dma_base;
-		midch->ch_regs = dma->dma_base + DMA_CH_SIZE * midch->ch_id;
-		midch->dma = dma;
-		dma->intr_mask |= 1 << (dma->chan_base + i);
-		spin_lock_init(&midch->lock);
-
-		INIT_LIST_HEAD(&midch->active_list);
-		INIT_LIST_HEAD(&midch->queue);
-		INIT_LIST_HEAD(&midch->free_list);
-		/*mask interrupts*/
-		iowrite32(MASK_INTR_REG(midch->ch_id),
-			dma->dma_base + MASK_BLOCK);
-		iowrite32(MASK_INTR_REG(midch->ch_id),
-			dma->dma_base + MASK_SRC_TRAN);
-		iowrite32(MASK_INTR_REG(midch->ch_id),
-			dma->dma_base + MASK_DST_TRAN);
-		iowrite32(MASK_INTR_REG(midch->ch_id),
-			dma->dma_base + MASK_ERR);
-		iowrite32(MASK_INTR_REG(midch->ch_id),
-			dma->dma_base + MASK_TFR);
-
-		disable_dma_interrupt(midch);
-		list_add_tail(&midch->chan.device_node, &dma->common.channels);
-	}
-	pr_debug("MDMA: Calc Mask as %x for this controller\n", dma->intr_mask);
-
-	/*init dma structure*/
-	dma_cap_zero(dma->common.cap_mask);
-	dma_cap_set(DMA_MEMCPY, dma->common.cap_mask);
-	dma_cap_set(DMA_SLAVE, dma->common.cap_mask);
-	dma_cap_set(DMA_PRIVATE, dma->common.cap_mask);
-	dma->common.dev = &pdev->dev;
-
-	dma->common.device_alloc_chan_resources =
-					intel_mid_dma_alloc_chan_resources;
-	dma->common.device_free_chan_resources =
-					intel_mid_dma_free_chan_resources;
-
-	dma->common.device_tx_status = intel_mid_dma_tx_status;
-	dma->common.device_prep_dma_memcpy = intel_mid_dma_prep_memcpy;
-	dma->common.device_issue_pending = intel_mid_dma_issue_pending;
-	dma->common.device_prep_slave_sg = intel_mid_dma_prep_slave_sg;
-	dma->common.device_config = intel_mid_dma_config;
-	dma->common.device_terminate_all = intel_mid_dma_terminate_all;
-
-	/*enable dma cntrl*/
-	iowrite32(REG_BIT0, dma->dma_base + DMA_CFG);
-
-	/*register irq */
-	if (dma->pimr_mask) {
-		pr_debug("MDMA:Requesting irq shared for DMAC1\n");
-		err = request_irq(pdev->irq, intel_mid_dma_interrupt1,
-			IRQF_SHARED, "INTEL_MID_DMAC1", dma);
-		if (0 != err)
-			goto err_irq;
-	} else {
-		dma->intr_mask = 0x03;
-		pr_debug("MDMA:Requesting irq for DMAC2\n");
-		err = request_irq(pdev->irq, intel_mid_dma_interrupt2,
-			IRQF_SHARED, "INTEL_MID_DMAC2", dma);
-		if (0 != err)
-			goto err_irq;
-	}
-	/*register device w/ engine*/
-	err = dma_async_device_register(&dma->common);
-	if (0 != err) {
-		pr_err("ERR_MDMA:device_register failed: %d\n", err);
-		goto err_engine;
-	}
-	if (dma->pimr_mask) {
-		pr_debug("setting up tasklet1 for DMAC1\n");
-		tasklet_init(&dma->tasklet, dma_tasklet1, (unsigned long)dma);
-	} else {
-		pr_debug("setting up tasklet2 for DMAC2\n");
-		tasklet_init(&dma->tasklet, dma_tasklet2, (unsigned long)dma);
-	}
-	return 0;
-
-err_engine:
-	free_irq(pdev->irq, dma);
-err_irq:
-	if (dma->mask_reg)
-		iounmap(dma->mask_reg);
-err_ioremap:
-	pci_pool_destroy(dma->dma_pool);
-err_dma_pool:
-	pr_err("ERR_MDMA:setup_dma failed: %d\n", err);
-	return err;
-
-}
-
-/**
- * middma_shutdown -	Shutdown the DMA controller
- * @pdev: Controller PCI device structure
- *
- * Called by remove
- * Unregister DMa controller, clear all structures and free interrupt
- */
-static void middma_shutdown(struct pci_dev *pdev)
-{
-	struct middma_device *device = pci_get_drvdata(pdev);
-
-	dma_async_device_unregister(&device->common);
-	pci_pool_destroy(device->dma_pool);
-	if (device->mask_reg)
-		iounmap(device->mask_reg);
-	if (device->dma_base)
-		iounmap(device->dma_base);
-	free_irq(pdev->irq, device);
-	return;
-}
-
-/**
- * intel_mid_dma_probe -	PCI Probe
- * @pdev: Controller PCI device structure
- * @id: pci device id structure
- *
- * Initialize the PCI device, map BARs, query driver data.
- * Call setup_dma to complete contoller and chan initilzation
- */
-static int intel_mid_dma_probe(struct pci_dev *pdev,
-					const struct pci_device_id *id)
-{
-	struct middma_device *device;
-	u32 base_addr, bar_size;
-	struct intel_mid_dma_probe_info *info;
-	int err;
-
-	pr_debug("MDMA: probe for %x\n", pdev->device);
-	info = (void *)id->driver_data;
-	pr_debug("MDMA: CH %d, base %d, block len %d, Periphral mask %x\n",
-				info->max_chan, info->ch_base,
-				info->block_size, info->pimr_mask);
-
-	err = pci_enable_device(pdev);
-	if (err)
-		goto err_enable_device;
-
-	err = pci_request_regions(pdev, "intel_mid_dmac");
-	if (err)
-		goto err_request_regions;
-
-	err = pci_set_dma_mask(pdev, DMA_BIT_MASK(32));
-	if (err)
-		goto err_set_dma_mask;
-
-	err = pci_set_consistent_dma_mask(pdev, DMA_BIT_MASK(32));
-	if (err)
-		goto err_set_dma_mask;
-
-	device = kzalloc(sizeof(*device), GFP_KERNEL);
-	if (!device) {
-		pr_err("ERR_MDMA:kzalloc failed probe\n");
-		err = -ENOMEM;
-		goto err_kzalloc;
-	}
-	device->pdev = pci_dev_get(pdev);
-
-	base_addr = pci_resource_start(pdev, 0);
-	bar_size  = pci_resource_len(pdev, 0);
-	device->dma_base = ioremap_nocache(base_addr, DMA_REG_SIZE);
-	if (!device->dma_base) {
-		pr_err("ERR_MDMA:ioremap failed\n");
-		err = -ENOMEM;
-		goto err_ioremap;
-	}
-	pci_set_drvdata(pdev, device);
-	pci_set_master(pdev);
-	device->max_chan = info->max_chan;
-	device->chan_base = info->ch_base;
-	device->block_size = info->block_size;
-	device->pimr_mask = info->pimr_mask;
-
-	err = mid_setup_dma(pdev);
-	if (err)
-		goto err_dma;
-
-	pm_runtime_put_noidle(&pdev->dev);
-	pm_runtime_allow(&pdev->dev);
-	return 0;
-
-err_dma:
-	iounmap(device->dma_base);
-err_ioremap:
-	pci_dev_put(pdev);
-	kfree(device);
-err_kzalloc:
-err_set_dma_mask:
-	pci_release_regions(pdev);
-	pci_disable_device(pdev);
-err_request_regions:
-err_enable_device:
-	pr_err("ERR_MDMA:Probe failed %d\n", err);
-	return err;
-}
-
-/**
- * intel_mid_dma_remove -	PCI remove
- * @pdev: Controller PCI device structure
- *
- * Free up all resources and data
- * Call shutdown_dma to complete contoller and chan cleanup
- */
-static void intel_mid_dma_remove(struct pci_dev *pdev)
-{
-	struct middma_device *device = pci_get_drvdata(pdev);
-
-	pm_runtime_get_noresume(&pdev->dev);
-	pm_runtime_forbid(&pdev->dev);
-	middma_shutdown(pdev);
-	pci_dev_put(pdev);
-	kfree(device);
-	pci_release_regions(pdev);
-	pci_disable_device(pdev);
-}
-
-/* Power Management */
-/*
-* dma_suspend - PCI suspend function
-*
-* @pci: PCI device structure
-* @state: PM message
-*
-* This function is called by OS when a power event occurs
-*/
-static int dma_suspend(struct device *dev)
-{
-	struct pci_dev *pci = to_pci_dev(dev);
-	int i;
-	struct middma_device *device = pci_get_drvdata(pci);
-	pr_debug("MDMA: dma_suspend called\n");
-
-	for (i = 0; i < device->max_chan; i++) {
-		if (device->ch[i].in_use)
-			return -EAGAIN;
-	}
-	dmac1_mask_periphral_intr(device);
-	device->state = SUSPENDED;
-	pci_save_state(pci);
-	pci_disable_device(pci);
-	pci_set_power_state(pci, PCI_D3hot);
-	return 0;
-}
-
-/**
-* dma_resume - PCI resume function
-*
-* @pci:	PCI device structure
-*
-* This function is called by OS when a power event occurs
-*/
-int dma_resume(struct device *dev)
-{
-	struct pci_dev *pci = to_pci_dev(dev);
-	int ret;
-	struct middma_device *device = pci_get_drvdata(pci);
-
-	pr_debug("MDMA: dma_resume called\n");
-	pci_set_power_state(pci, PCI_D0);
-	pci_restore_state(pci);
-	ret = pci_enable_device(pci);
-	if (ret) {
-		pr_err("MDMA: device can't be enabled for %x\n", pci->device);
-		return ret;
-	}
-	device->state = RUNNING;
-	iowrite32(REG_BIT0, device->dma_base + DMA_CFG);
-	return 0;
-}
-
-static int dma_runtime_suspend(struct device *dev)
-{
-	struct pci_dev *pci_dev = to_pci_dev(dev);
-	struct middma_device *device = pci_get_drvdata(pci_dev);
-
-	device->state = SUSPENDED;
-	return 0;
-}
-
-static int dma_runtime_resume(struct device *dev)
-{
-	struct pci_dev *pci_dev = to_pci_dev(dev);
-	struct middma_device *device = pci_get_drvdata(pci_dev);
-
-	device->state = RUNNING;
-	iowrite32(REG_BIT0, device->dma_base + DMA_CFG);
-	return 0;
-}
-
-static int dma_runtime_idle(struct device *dev)
-{
-	struct pci_dev *pdev = to_pci_dev(dev);
-	struct middma_device *device = pci_get_drvdata(pdev);
-	int i;
-
-	for (i = 0; i < device->max_chan; i++) {
-		if (device->ch[i].in_use)
-			return -EAGAIN;
-	}
-
-	return 0;
-}
-
-/******************************************************************************
-* PCI stuff
-*/
-static struct pci_device_id intel_mid_dma_ids[] = {
-	{ PCI_VDEVICE(INTEL, INTEL_MID_DMAC1_ID),	INFO(2, 6, 4095, 0x200020)},
-	{ PCI_VDEVICE(INTEL, INTEL_MID_DMAC2_ID),	INFO(2, 0, 2047, 0)},
-	{ PCI_VDEVICE(INTEL, INTEL_MID_GP_DMAC2_ID),	INFO(2, 0, 2047, 0)},
-	{ PCI_VDEVICE(INTEL, INTEL_MFLD_DMAC1_ID),	INFO(4, 0, 4095, 0x400040)},
-	{ 0, }
-};
-MODULE_DEVICE_TABLE(pci, intel_mid_dma_ids);
-
-static const struct dev_pm_ops intel_mid_dma_pm = {
-	.runtime_suspend = dma_runtime_suspend,
-	.runtime_resume = dma_runtime_resume,
-	.runtime_idle = dma_runtime_idle,
-	.suspend = dma_suspend,
-	.resume = dma_resume,
-};
-
-static struct pci_driver intel_mid_dma_pci_driver = {
-	.name		=	"Intel MID DMA",
-	.id_table	=	intel_mid_dma_ids,
-	.probe		=	intel_mid_dma_probe,
-	.remove		=	intel_mid_dma_remove,
-#ifdef CONFIG_PM
-	.driver = {
-		.pm = &intel_mid_dma_pm,
-	},
-#endif
-};
-
-static int __init intel_mid_dma_init(void)
-{
-	pr_debug("INFO_MDMA: LNW DMA Driver Version %s\n",
-			INTEL_MID_DMA_DRIVER_VERSION);
-	return pci_register_driver(&intel_mid_dma_pci_driver);
-}
-fs_initcall(intel_mid_dma_init);
-
-static void __exit intel_mid_dma_exit(void)
-{
-	pci_unregister_driver(&intel_mid_dma_pci_driver);
-}
-module_exit(intel_mid_dma_exit);
-
-MODULE_AUTHOR("Vinod Koul <vinod.koul@intel.com>");
-MODULE_DESCRIPTION("Intel (R) MID DMAC Driver");
-MODULE_LICENSE("GPL v2");
-MODULE_VERSION(INTEL_MID_DMA_DRIVER_VERSION);
diff --git a/drivers/dma/intel_mid_dma_regs.h b/drivers/dma/intel_mid_dma_regs.h
deleted file mode 100644
index 17b42192ea58..000000000000
--- a/drivers/dma/intel_mid_dma_regs.h
+++ /dev/null
@@ -1,299 +0,0 @@
-/*
- *  intel_mid_dma_regs.h - Intel MID DMA Drivers
- *
- *  Copyright (C) 2008-10 Intel Corp
- *  Author: Vinod Koul <vinod.koul@intel.com>
- *  ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
- *
- *  This program is free software; you can redistribute it and/or modify
- *  it under the terms of the GNU General Public License as published by
- *  the Free Software Foundation; version 2 of the License.
- *
- *  This program is distributed in the hope that it will be useful, but
- *  WITHOUT ANY WARRANTY; without even the implied warranty of
- *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- *  General Public License for more details.
- *
- *  You should have received a copy of the GNU General Public License along
- *  with this program; if not, write to the Free Software Foundation, Inc.,
- *  59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
- *
- * ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
- *
- *
- */
-#ifndef __INTEL_MID_DMAC_REGS_H__
-#define __INTEL_MID_DMAC_REGS_H__
-
-#include <linux/dmaengine.h>
-#include <linux/dmapool.h>
-#include <linux/pci_ids.h>
-
-#define INTEL_MID_DMA_DRIVER_VERSION "1.1.0"
-
-#define	REG_BIT0		0x00000001
-#define	REG_BIT8		0x00000100
-#define INT_MASK_WE		0x8
-#define CLEAR_DONE		0xFFFFEFFF
-#define UNMASK_INTR_REG(chan_num) \
-	((REG_BIT0 << chan_num) | (REG_BIT8 << chan_num))
-#define MASK_INTR_REG(chan_num) (REG_BIT8 << chan_num)
-
-#define ENABLE_CHANNEL(chan_num) \
-	((REG_BIT0 << chan_num) | (REG_BIT8 << chan_num))
-
-#define DISABLE_CHANNEL(chan_num) \
-	(REG_BIT8 << chan_num)
-
-#define DESCS_PER_CHANNEL	16
-/*DMA Registers*/
-/*registers associated with channel programming*/
-#define DMA_REG_SIZE		0x400
-#define DMA_CH_SIZE		0x58
-
-/*CH X REG = (DMA_CH_SIZE)*CH_NO + REG*/
-#define SAR			0x00 /* Source Address Register*/
-#define DAR			0x08 /* Destination Address Register*/
-#define LLP			0x10 /* Linked List Pointer Register*/
-#define CTL_LOW			0x18 /* Control Register*/
-#define CTL_HIGH		0x1C /* Control Register*/
-#define CFG_LOW			0x40 /* Configuration Register Low*/
-#define CFG_HIGH		0x44 /* Configuration Register high*/
-
-#define STATUS_TFR		0x2E8
-#define STATUS_BLOCK		0x2F0
-#define STATUS_ERR		0x308
-
-#define RAW_TFR			0x2C0
-#define RAW_BLOCK		0x2C8
-#define RAW_ERR			0x2E0
-
-#define MASK_TFR		0x310
-#define MASK_BLOCK		0x318
-#define MASK_SRC_TRAN		0x320
-#define MASK_DST_TRAN		0x328
-#define MASK_ERR		0x330
-
-#define CLEAR_TFR		0x338
-#define CLEAR_BLOCK		0x340
-#define CLEAR_SRC_TRAN		0x348
-#define CLEAR_DST_TRAN		0x350
-#define CLEAR_ERR		0x358
-
-#define INTR_STATUS		0x360
-#define DMA_CFG			0x398
-#define DMA_CHAN_EN		0x3A0
-
-/*DMA channel control registers*/
-union intel_mid_dma_ctl_lo {
-	struct {
-		u32	int_en:1;	/*enable or disable interrupts*/
-					/*should be 0*/
-		u32	dst_tr_width:3;	/*destination transfer width*/
-					/*usually 32 bits = 010*/
-		u32	src_tr_width:3; /*source transfer width*/
-					/*usually 32 bits = 010*/
-		u32	dinc:2;		/*destination address inc/dec*/
-					/*For mem:INC=00, Periphral NoINC=11*/
-		u32	sinc:2;		/*source address inc or dec, as above*/
-		u32	dst_msize:3;	/*destination burst transaction length*/
-					/*always = 16 ie 011*/
-		u32	src_msize:3;	/*source burst transaction length*/
-					/*always = 16 ie 011*/
-		u32	reser1:3;
-		u32	tt_fc:3;	/*transfer type and flow controller*/
-					/*M-M = 000
-					  P-M = 010
-					  M-P = 001*/
-		u32	dms:2;		/*destination master select = 0*/
-		u32	sms:2;		/*source master select = 0*/
-		u32	llp_dst_en:1;	/*enable/disable destination LLP = 0*/
-		u32	llp_src_en:1;	/*enable/disable source LLP = 0*/
-		u32	reser2:3;
-	} ctlx;
-	u32	ctl_lo;
-};
-
-union intel_mid_dma_ctl_hi {
-	struct {
-		u32	block_ts:12;	/*block transfer size*/
-		u32	done:1;		/*Done - updated by DMAC*/
-		u32	reser:19;	/*configured by DMAC*/
-	} ctlx;
-	u32	ctl_hi;
-
-};
-
-/*DMA channel configuration registers*/
-union intel_mid_dma_cfg_lo {
-	struct {
-		u32	reser1:5;
-		u32	ch_prior:3;	/*channel priority = 0*/
-		u32	ch_susp:1;	/*channel suspend = 0*/
-		u32	fifo_empty:1;	/*FIFO empty or not R bit = 0*/
-		u32	hs_sel_dst:1;	/*select HW/SW destn handshaking*/
-					/*HW = 0, SW = 1*/
-		u32	hs_sel_src:1;	/*select HW/SW src handshaking*/
-		u32	reser2:6;
-		u32	dst_hs_pol:1;	/*dest HS interface polarity*/
-		u32	src_hs_pol:1;	/*src HS interface polarity*/
-		u32	max_abrst:10;	/*max AMBA burst len = 0 (no sw limit*/
-		u32	reload_src:1;	/*auto reload src addr =1 if src is P*/
-		u32	reload_dst:1;	/*AR destn addr =1 if dstn is P*/
-	} cfgx;
-	u32	cfg_lo;
-};
-
-union intel_mid_dma_cfg_hi {
-	struct {
-		u32	fcmode:1;	/*flow control mode = 1*/
-		u32	fifo_mode:1;	/*FIFO mode select = 1*/
-		u32	protctl:3;	/*protection control = 0*/
-		u32	rsvd:2;
-		u32	src_per:4;	/*src hw HS interface*/
-		u32	dst_per:4;	/*dstn hw HS interface*/
-		u32	reser2:17;
-	} cfgx;
-	u32	cfg_hi;
-};
-
-
-/**
- * struct intel_mid_dma_chan - internal mid representation of a DMA channel
- * @chan: dma_chan strcture represetation for mid chan
- * @ch_regs: MMIO register space pointer to channel register
- * @dma_base: MMIO register space DMA engine base pointer
- * @ch_id: DMA channel id
- * @lock: channel spinlock
- * @active_list: current active descriptors
- * @queue: current queued up descriptors
- * @free_list: current free descriptors
- * @slave: dma slave structure
- * @descs_allocated: total number of descriptors allocated
- * @dma: dma device structure pointer
- * @busy: bool representing if ch is busy (active txn) or not
- * @in_use: bool representing if ch is in use or not
- * @raw_tfr: raw trf interrupt received
- * @raw_block: raw block interrupt received
- */
-struct intel_mid_dma_chan {
-	struct dma_chan		chan;
-	void __iomem		*ch_regs;
-	void __iomem		*dma_base;
-	int			ch_id;
-	spinlock_t		lock;
-	struct list_head	active_list;
-	struct list_head	queue;
-	struct list_head	free_list;
-	unsigned int		descs_allocated;
-	struct middma_device	*dma;
-	bool			busy;
-	bool			in_use;
-	u32			raw_tfr;
-	u32			raw_block;
-	struct intel_mid_dma_slave *mid_slave;
-};
-
-static inline struct intel_mid_dma_chan *to_intel_mid_dma_chan(
-						struct dma_chan *chan)
-{
-	return container_of(chan, struct intel_mid_dma_chan, chan);
-}
-
-enum intel_mid_dma_state {
-	RUNNING = 0,
-	SUSPENDED,
-};
-/**
- * struct middma_device - internal representation of a DMA device
- * @pdev: PCI device
- * @dma_base: MMIO register space pointer of DMA
- * @dma_pool: for allocating DMA descriptors
- * @common: embedded struct dma_device
- * @tasklet: dma tasklet for processing interrupts
- * @ch: per channel data
- * @pci_id: DMA device PCI ID
- * @intr_mask: Interrupt mask to be used
- * @mask_reg: MMIO register for periphral mask
- * @chan_base: Base ch index (read from driver data)
- * @max_chan: max number of chs supported (from drv_data)
- * @block_size: Block size of DMA transfer supported (from drv_data)
- * @pimr_mask: MMIO register addr for periphral interrupt (from drv_data)
- * @state: dma PM device state
- */
-struct middma_device {
-	struct pci_dev		*pdev;
-	void __iomem		*dma_base;
-	struct pci_pool		*dma_pool;
-	struct dma_device	common;
-	struct tasklet_struct   tasklet;
-	struct intel_mid_dma_chan ch[MAX_CHAN];
-	unsigned int		pci_id;
-	unsigned int		intr_mask;
-	void __iomem		*mask_reg;
-	int			chan_base;
-	int			max_chan;
-	int			block_size;
-	unsigned int		pimr_mask;
-	enum intel_mid_dma_state state;
-};
-
-static inline struct middma_device *to_middma_device(struct dma_device *common)
-{
-	return container_of(common, struct middma_device, common);
-}
-
-struct intel_mid_dma_desc {
-	void __iomem			*block; /*ch ptr*/
-	struct list_head		desc_node;
-	struct dma_async_tx_descriptor	txd;
-	size_t				len;
-	dma_addr_t			sar;
-	dma_addr_t			dar;
-	u32				cfg_hi;
-	u32				cfg_lo;
-	u32				ctl_lo;
-	u32				ctl_hi;
-	struct pci_pool			*lli_pool;
-	struct intel_mid_dma_lli	*lli;
-	dma_addr_t			lli_phys;
-	unsigned int			lli_length;
-	unsigned int			current_lli;
-	dma_addr_t			next;
-	enum dma_transfer_direction		dirn;
-	enum dma_status			status;
-	enum dma_slave_buswidth		width; /*width of DMA txn*/
-	enum intel_mid_dma_mode		cfg_mode; /*mode configuration*/
-
-};
-
-struct intel_mid_dma_lli {
-	dma_addr_t			sar;
-	dma_addr_t			dar;
-	dma_addr_t			llp;
-	u32				ctl_lo;
-	u32				ctl_hi;
-} __attribute__ ((packed));
-
-static inline int test_ch_en(void __iomem *dma, u32 ch_no)
-{
-	u32 en_reg = ioread32(dma + DMA_CHAN_EN);
-	return (en_reg >> ch_no) & 0x1;
-}
-
-static inline struct intel_mid_dma_desc *to_intel_mid_dma_desc
-		(struct dma_async_tx_descriptor *txd)
-{
-	return container_of(txd, struct intel_mid_dma_desc, txd);
-}
-
-static inline struct intel_mid_dma_slave *to_intel_mid_dma_slave
-		(struct dma_slave_config *slave)
-{
-	return container_of(slave, struct intel_mid_dma_slave, dma_slave);
-}
-
-
-int dma_resume(struct device *dev);
-
-#endif /*__INTEL_MID_DMAC_REGS_H__*/
diff --git a/drivers/dma/ioat/dma_v3.c b/drivers/dma/ioat/dma_v3.c
index 77a6dcf25b98..194ec20c9408 100644
--- a/drivers/dma/ioat/dma_v3.c
+++ b/drivers/dma/ioat/dma_v3.c
@@ -230,6 +230,10 @@ static bool is_bwd_noraid(struct pci_dev *pdev)
 	switch (pdev->device) {
 	case PCI_DEVICE_ID_INTEL_IOAT_BWD2:
 	case PCI_DEVICE_ID_INTEL_IOAT_BWD3:
+	case PCI_DEVICE_ID_INTEL_IOAT_BDXDE0:
+	case PCI_DEVICE_ID_INTEL_IOAT_BDXDE1:
+	case PCI_DEVICE_ID_INTEL_IOAT_BDXDE2:
+	case PCI_DEVICE_ID_INTEL_IOAT_BDXDE3:
 		return true;
 	default:
 		return false;
diff --git a/drivers/dma/mmp_pdma.c b/drivers/dma/mmp_pdma.c
index 8926f271904e..eb410044e1af 100644
--- a/drivers/dma/mmp_pdma.c
+++ b/drivers/dma/mmp_pdma.c
@@ -219,6 +219,9 @@ static irqreturn_t mmp_pdma_int_handler(int irq, void *dev_id)
 
 	while (dint) {
 		i = __ffs(dint);
+		/* only handle interrupts belonging to pdma driver*/
+		if (i >= pdev->dma_channels)
+			break;
 		dint &= (dint - 1);
 		phy = &pdev->phy[i];
 		ret = mmp_pdma_chan_handler(irq, phy);
@@ -999,6 +1002,9 @@ static int mmp_pdma_probe(struct platform_device *op)
 	struct resource *iores;
 	int i, ret, irq = 0;
 	int dma_channels = 0, irq_num = 0;
+	const enum dma_slave_buswidth widths =
+		DMA_SLAVE_BUSWIDTH_1_BYTE   | DMA_SLAVE_BUSWIDTH_2_BYTES |
+		DMA_SLAVE_BUSWIDTH_4_BYTES;
 
 	pdev = devm_kzalloc(&op->dev, sizeof(*pdev), GFP_KERNEL);
 	if (!pdev)
@@ -1066,6 +1072,10 @@ static int mmp_pdma_probe(struct platform_device *op)
 	pdev->device.device_config = mmp_pdma_config;
 	pdev->device.device_terminate_all = mmp_pdma_terminate_all;
 	pdev->device.copy_align = PDMA_ALIGNMENT;
+	pdev->device.src_addr_widths = widths;
+	pdev->device.dst_addr_widths = widths;
+	pdev->device.directions = BIT(DMA_MEM_TO_DEV) | BIT(DMA_DEV_TO_MEM);
+	pdev->device.residue_granularity = DMA_RESIDUE_GRANULARITY_DESCRIPTOR;
 
 	if (pdev->dev->coherent_dma_mask)
 		dma_set_mask(pdev->dev, pdev->dev->coherent_dma_mask);
diff --git a/drivers/dma/mmp_tdma.c b/drivers/dma/mmp_tdma.c
index 70c2fa9963cd..b6f4e1fc9c78 100644
--- a/drivers/dma/mmp_tdma.c
+++ b/drivers/dma/mmp_tdma.c
@@ -110,7 +110,7 @@ struct mmp_tdma_chan {
 	struct tasklet_struct		tasklet;
 
 	struct mmp_tdma_desc		*desc_arr;
-	phys_addr_t			desc_arr_phys;
+	dma_addr_t			desc_arr_phys;
 	int				desc_num;
 	enum dma_transfer_direction	dir;
 	dma_addr_t			dev_addr;
@@ -166,9 +166,12 @@ static void mmp_tdma_enable_chan(struct mmp_tdma_chan *tdmac)
 static int mmp_tdma_disable_chan(struct dma_chan *chan)
 {
 	struct mmp_tdma_chan *tdmac = to_mmp_tdma_chan(chan);
+	u32 tdcr;
 
-	writel(readl(tdmac->reg_base + TDCR) & ~TDCR_CHANEN,
-					tdmac->reg_base + TDCR);
+	tdcr = readl(tdmac->reg_base + TDCR);
+	tdcr |= TDCR_ABR;
+	tdcr &= ~TDCR_CHANEN;
+	writel(tdcr, tdmac->reg_base + TDCR);
 
 	tdmac->status = DMA_COMPLETE;
 
@@ -296,12 +299,27 @@ static int mmp_tdma_clear_chan_irq(struct mmp_tdma_chan *tdmac)
 	return -EAGAIN;
 }
 
+static size_t mmp_tdma_get_pos(struct mmp_tdma_chan *tdmac)
+{
+	size_t reg;
+
+	if (tdmac->idx == 0) {
+		reg = __raw_readl(tdmac->reg_base + TDSAR);
+		reg -= tdmac->desc_arr[0].src_addr;
+	} else if (tdmac->idx == 1) {
+		reg = __raw_readl(tdmac->reg_base + TDDAR);
+		reg -= tdmac->desc_arr[0].dst_addr;
+	} else
+		return -EINVAL;
+
+	return reg;
+}
+
 static irqreturn_t mmp_tdma_chan_handler(int irq, void *dev_id)
 {
 	struct mmp_tdma_chan *tdmac = dev_id;
 
 	if (mmp_tdma_clear_chan_irq(tdmac) == 0) {
-		tdmac->pos = (tdmac->pos + tdmac->period_len) % tdmac->buf_len;
 		tasklet_schedule(&tdmac->tasklet);
 		return IRQ_HANDLED;
 	} else
@@ -343,7 +361,7 @@ static void mmp_tdma_free_descriptor(struct mmp_tdma_chan *tdmac)
 	int size = tdmac->desc_num * sizeof(struct mmp_tdma_desc);
 
 	gpool = tdmac->pool;
-	if (tdmac->desc_arr)
+	if (gpool && tdmac->desc_arr)
 		gen_pool_free(gpool, (unsigned long)tdmac->desc_arr,
 				size);
 	tdmac->desc_arr = NULL;
@@ -499,6 +517,7 @@ static enum dma_status mmp_tdma_tx_status(struct dma_chan *chan,
 {
 	struct mmp_tdma_chan *tdmac = to_mmp_tdma_chan(chan);
 
+	tdmac->pos = mmp_tdma_get_pos(tdmac);
 	dma_set_tx_state(txstate, chan->completed_cookie, chan->cookie,
 			 tdmac->buf_len - tdmac->pos);
 
@@ -610,7 +629,7 @@ static int mmp_tdma_probe(struct platform_device *pdev)
 	int i, ret;
 	int irq = 0, irq_num = 0;
 	int chan_num = TDMA_CHANNEL_NUM;
-	struct gen_pool *pool;
+	struct gen_pool *pool = NULL;
 
 	of_id = of_match_device(mmp_tdma_dt_ids, &pdev->dev);
 	if (of_id)
diff --git a/drivers/dma/moxart-dma.c b/drivers/dma/moxart-dma.c
index 15cab7d79525..b4634109e010 100644
--- a/drivers/dma/moxart-dma.c
+++ b/drivers/dma/moxart-dma.c
@@ -193,8 +193,10 @@ static int moxart_terminate_all(struct dma_chan *chan)
 
 	spin_lock_irqsave(&ch->vc.lock, flags);
 
-	if (ch->desc)
+	if (ch->desc) {
+		moxart_dma_desc_free(&ch->desc->vd);
 		ch->desc = NULL;
+	}
 
 	ctrl = readl(ch->base + REG_OFF_CTRL);
 	ctrl &= ~(APB_DMA_ENABLE | APB_DMA_FIN_INT_EN | APB_DMA_ERR_INT_EN);
diff --git a/drivers/dma/omap-dma.c b/drivers/dma/omap-dma.c
index 7dd6dd121681..167dbaf65742 100644
--- a/drivers/dma/omap-dma.c
+++ b/drivers/dma/omap-dma.c
@@ -981,6 +981,7 @@ static int omap_dma_terminate_all(struct dma_chan *chan)
 	 * c->desc is NULL and exit.)
 	 */
 	if (c->desc) {
+		omap_dma_desc_free(&c->desc->vd);
 		c->desc = NULL;
 		/* Avoid stopping the dma twice */
 		if (!c->paused)
diff --git a/drivers/dma/qcom_bam_dma.c b/drivers/dma/qcom_bam_dma.c
index d7a33b3ac466..9c914d625906 100644
--- a/drivers/dma/qcom_bam_dma.c
+++ b/drivers/dma/qcom_bam_dma.c
@@ -162,9 +162,9 @@ static const struct reg_offset_data bam_v1_4_reg_info[] = {
 	[BAM_P_IRQ_STTS]	= { 0x1010, 0x1000, 0x00, 0x00 },
 	[BAM_P_IRQ_CLR]		= { 0x1014, 0x1000, 0x00, 0x00 },
 	[BAM_P_IRQ_EN]		= { 0x1018, 0x1000, 0x00, 0x00 },
-	[BAM_P_EVNT_DEST_ADDR]	= { 0x102C, 0x00, 0x1000, 0x00 },
-	[BAM_P_EVNT_REG]	= { 0x1018, 0x00, 0x1000, 0x00 },
-	[BAM_P_SW_OFSTS]	= { 0x1000, 0x00, 0x1000, 0x00 },
+	[BAM_P_EVNT_DEST_ADDR]	= { 0x182C, 0x00, 0x1000, 0x00 },
+	[BAM_P_EVNT_REG]	= { 0x1818, 0x00, 0x1000, 0x00 },
+	[BAM_P_SW_OFSTS]	= { 0x1800, 0x00, 0x1000, 0x00 },
 	[BAM_P_DATA_FIFO_ADDR]	= { 0x1824, 0x00, 0x1000, 0x00 },
 	[BAM_P_DESC_FIFO_ADDR]	= { 0x181C, 0x00, 0x1000, 0x00 },
 	[BAM_P_EVNT_GEN_TRSHLD]	= { 0x1828, 0x00, 0x1000, 0x00 },
@@ -1143,6 +1143,10 @@ static int bam_dma_probe(struct platform_device *pdev)
 	dma_cap_set(DMA_SLAVE, bdev->common.cap_mask);
 
 	/* initialize dmaengine apis */
+	bdev->common.directions = BIT(DMA_DEV_TO_MEM) | BIT(DMA_MEM_TO_DEV);
+	bdev->common.residue_granularity = DMA_RESIDUE_GRANULARITY_SEGMENT;
+	bdev->common.src_addr_widths = DMA_SLAVE_BUSWIDTH_4_BYTES;
+	bdev->common.dst_addr_widths = DMA_SLAVE_BUSWIDTH_4_BYTES;
 	bdev->common.device_alloc_chan_resources = bam_alloc_chan;
 	bdev->common.device_free_chan_resources = bam_free_chan;
 	bdev->common.device_prep_slave_sg = bam_prep_slave_sg;
diff --git a/drivers/dma/sh/shdmac.c b/drivers/dma/sh/shdmac.c
index b2431aa30033..9f1d4c7dbab8 100644
--- a/drivers/dma/sh/shdmac.c
+++ b/drivers/dma/sh/shdmac.c
@@ -582,15 +582,12 @@ static void sh_dmae_chan_remove(struct sh_dmae_device *shdev)
 	}
 }
 
-static void sh_dmae_shutdown(struct platform_device *pdev)
-{
-	struct sh_dmae_device *shdev = platform_get_drvdata(pdev);
-	sh_dmae_ctl_stop(shdev);
-}
-
 #ifdef CONFIG_PM
 static int sh_dmae_runtime_suspend(struct device *dev)
 {
+	struct sh_dmae_device *shdev = dev_get_drvdata(dev);
+
+	sh_dmae_ctl_stop(shdev);
 	return 0;
 }
 
@@ -605,6 +602,9 @@ static int sh_dmae_runtime_resume(struct device *dev)
 #ifdef CONFIG_PM_SLEEP
 static int sh_dmae_suspend(struct device *dev)
 {
+	struct sh_dmae_device *shdev = dev_get_drvdata(dev);
+
+	sh_dmae_ctl_stop(shdev);
 	return 0;
 }
 
@@ -929,13 +929,12 @@ static int sh_dmae_remove(struct platform_device *pdev)
 }
 
 static struct platform_driver sh_dmae_driver = {
-	.driver 	= {
+	.driver		= {
 		.pm	= &sh_dmae_pm,
 		.name	= SH_DMAE_DRV_NAME,
 		.of_match_table = sh_dmae_of_match,
 	},
 	.remove		= sh_dmae_remove,
-	.shutdown	= sh_dmae_shutdown,
 };
 
 static int __init sh_dmae_init(void)
diff --git a/drivers/dma/ste_dma40.c b/drivers/dma/ste_dma40.c
index 68aca3334a17..1332b1d4d541 100644
--- a/drivers/dma/ste_dma40.c
+++ b/drivers/dma/ste_dma40.c
@@ -3548,7 +3548,7 @@ static int __init d40_probe(struct platform_device *pdev)
 
 	if (!plat_data) {
 		if (np) {
-			if(d40_of_probe(pdev, np)) {
+			if (d40_of_probe(pdev, np)) {
 				ret = -ENOMEM;
 				goto failure;
 			}
diff --git a/drivers/edac/amd64_edac.c b/drivers/edac/amd64_edac.c
index 5907c1718f8c..92772fffc52f 100644
--- a/drivers/edac/amd64_edac.c
+++ b/drivers/edac/amd64_edac.c
@@ -20,8 +20,7 @@ static struct msr __percpu *msrs;
  */
 static atomic_t drv_instances = ATOMIC_INIT(0);
 
-/* Per-node driver instances */
-static struct mem_ctl_info **mcis;
+/* Per-node stuff */
 static struct ecc_settings **ecc_stngs;
 
 /*
@@ -903,9 +902,17 @@ static int k8_early_channel_count(struct amd64_pvt *pvt)
 /* On F10h and later ErrAddr is MC4_ADDR[47:1] */
 static u64 get_error_address(struct amd64_pvt *pvt, struct mce *m)
 {
-	u64 addr;
+	u16 mce_nid = amd_get_nb_id(m->extcpu);
+	struct mem_ctl_info *mci;
 	u8 start_bit = 1;
 	u8 end_bit   = 47;
+	u64 addr;
+
+	mci = edac_mc_find(mce_nid);
+	if (!mci)
+		return 0;
+
+	pvt = mci->pvt_info;
 
 	if (pvt->fam == 0xf) {
 		start_bit = 3;
@@ -918,17 +925,13 @@ static u64 get_error_address(struct amd64_pvt *pvt, struct mce *m)
 	 * Erratum 637 workaround
 	 */
 	if (pvt->fam == 0x15) {
-		struct amd64_pvt *pvt;
 		u64 cc6_base, tmp_addr;
 		u32 tmp;
-		u16 mce_nid;
 		u8 intlv_en;
 
 		if ((addr & GENMASK_ULL(47, 24)) >> 24 != 0x00fdf7)
 			return addr;
 
-		mce_nid	= amd_get_nb_id(m->extcpu);
-		pvt	= mcis[mce_nid]->pvt_info;
 
 		amd64_read_pci_cfg(pvt->F1, DRAM_LOCAL_NODE_LIM, &tmp);
 		intlv_en = tmp >> 21 & 0x7;
@@ -1511,7 +1514,7 @@ static int f1x_lookup_addr_in_dct(u64 in_addr, u8 nid, u8 dct)
 	int cs_found = -EINVAL;
 	int csrow;
 
-	mci = mcis[nid];
+	mci = edac_mc_find(nid);
 	if (!mci)
 		return cs_found;
 
@@ -2663,34 +2666,6 @@ static bool ecc_enabled(struct pci_dev *F3, u16 nid)
 	return true;
 }
 
-static int set_mc_sysfs_attrs(struct mem_ctl_info *mci)
-{
-	struct amd64_pvt *pvt = mci->pvt_info;
-	int rc;
-
-	rc = amd64_create_sysfs_dbg_files(mci);
-	if (rc < 0)
-		return rc;
-
-	if (pvt->fam >= 0x10) {
-		rc = amd64_create_sysfs_inject_files(mci);
-		if (rc < 0)
-			return rc;
-	}
-
-	return 0;
-}
-
-static void del_mc_sysfs_attrs(struct mem_ctl_info *mci)
-{
-	struct amd64_pvt *pvt = mci->pvt_info;
-
-	amd64_remove_sysfs_dbg_files(mci);
-
-	if (pvt->fam >= 0x10)
-		amd64_remove_sysfs_inject_files(mci);
-}
-
 static void setup_mci_misc_attrs(struct mem_ctl_info *mci,
 				 struct amd64_family_type *fam)
 {
@@ -2778,6 +2753,16 @@ static struct amd64_family_type *per_family_init(struct amd64_pvt *pvt)
 	return fam_type;
 }
 
+static const struct attribute_group *amd64_edac_attr_groups[] = {
+#ifdef CONFIG_EDAC_DEBUG
+	&amd64_edac_dbg_group,
+#endif
+#ifdef CONFIG_EDAC_AMD64_ERROR_INJECTION
+	&amd64_edac_inj_group,
+#endif
+	NULL
+};
+
 static int init_one_instance(struct pci_dev *F2)
 {
 	struct amd64_pvt *pvt = NULL;
@@ -2844,14 +2829,10 @@ static int init_one_instance(struct pci_dev *F2)
 		mci->edac_cap = EDAC_FLAG_NONE;
 
 	ret = -ENODEV;
-	if (edac_mc_add_mc(mci)) {
+	if (edac_mc_add_mc_with_groups(mci, amd64_edac_attr_groups)) {
 		edac_dbg(1, "failed edac_mc_add_mc()\n");
 		goto err_add_mc;
 	}
-	if (set_mc_sysfs_attrs(mci)) {
-		edac_dbg(1, "failed edac_mc_add_mc()\n");
-		goto err_add_sysfs;
-	}
 
 	/* register stuff with EDAC MCE */
 	if (report_gart_errors)
@@ -2859,14 +2840,10 @@ static int init_one_instance(struct pci_dev *F2)
 
 	amd_register_ecc_decoder(decode_bus_error);
 
-	mcis[nid] = mci;
-
 	atomic_inc(&drv_instances);
 
 	return 0;
 
-err_add_sysfs:
-	edac_mc_del_mc(mci->pdev);
 err_add_mc:
 	edac_mc_free(mci);
 
@@ -2940,7 +2917,6 @@ static void remove_one_instance(struct pci_dev *pdev)
 	mci = find_mci_by_dev(&pdev->dev);
 	WARN_ON(!mci);
 
-	del_mc_sysfs_attrs(mci);
 	/* Remove from EDAC CORE tracking list */
 	mci = edac_mc_del_mc(&pdev->dev);
 	if (!mci)
@@ -2961,7 +2937,6 @@ static void remove_one_instance(struct pci_dev *pdev)
 
 	/* Free the EDAC CORE resources */
 	mci->pvt_info = NULL;
-	mcis[nid] = NULL;
 
 	kfree(pvt);
 	edac_mc_free(mci);
@@ -2999,7 +2974,7 @@ static void setup_pci_device(void)
 	if (pci_ctl)
 		return;
 
-	mci = mcis[0];
+	mci = edac_mc_find(0);
 	if (!mci)
 		return;
 
@@ -3023,9 +2998,8 @@ static int __init amd64_edac_init(void)
 		goto err_ret;
 
 	err = -ENOMEM;
-	mcis	  = kzalloc(amd_nb_num() * sizeof(mcis[0]), GFP_KERNEL);
 	ecc_stngs = kzalloc(amd_nb_num() * sizeof(ecc_stngs[0]), GFP_KERNEL);
-	if (!(mcis && ecc_stngs))
+	if (!ecc_stngs)
 		goto err_free;
 
 	msrs = msrs_alloc();
@@ -3056,9 +3030,6 @@ err_pci:
 	msrs = NULL;
 
 err_free:
-	kfree(mcis);
-	mcis = NULL;
-
 	kfree(ecc_stngs);
 	ecc_stngs = NULL;
 
@@ -3076,9 +3047,6 @@ static void __exit amd64_edac_exit(void)
 	kfree(ecc_stngs);
 	ecc_stngs = NULL;
 
-	kfree(mcis);
-	mcis = NULL;
-
 	msrs_free(msrs);
 	msrs = NULL;
 }
diff --git a/drivers/edac/amd64_edac.h b/drivers/edac/amd64_edac.h
index d8468c667925..4bdec752d330 100644
--- a/drivers/edac/amd64_edac.h
+++ b/drivers/edac/amd64_edac.h
@@ -453,31 +453,11 @@ struct ecc_settings {
 };
 
 #ifdef CONFIG_EDAC_DEBUG
-int amd64_create_sysfs_dbg_files(struct mem_ctl_info *mci);
-void amd64_remove_sysfs_dbg_files(struct mem_ctl_info *mci);
-
-#else
-static inline int amd64_create_sysfs_dbg_files(struct mem_ctl_info *mci)
-{
-	return 0;
-}
-static void inline amd64_remove_sysfs_dbg_files(struct mem_ctl_info *mci)
-{
-}
+extern const struct attribute_group amd64_edac_dbg_group;
 #endif
 
 #ifdef CONFIG_EDAC_AMD64_ERROR_INJECTION
-int amd64_create_sysfs_inject_files(struct mem_ctl_info *mci);
-void amd64_remove_sysfs_inject_files(struct mem_ctl_info *mci);
-
-#else
-static inline int amd64_create_sysfs_inject_files(struct mem_ctl_info *mci)
-{
-	return 0;
-}
-static inline void amd64_remove_sysfs_inject_files(struct mem_ctl_info *mci)
-{
-}
+extern const struct attribute_group amd64_edac_inj_group;
 #endif
 
 /*
diff --git a/drivers/edac/amd64_edac_dbg.c b/drivers/edac/amd64_edac_dbg.c
index 2c1bbf740605..4709c6079848 100644
--- a/drivers/edac/amd64_edac_dbg.c
+++ b/drivers/edac/amd64_edac_dbg.c
@@ -40,34 +40,15 @@ static DEVICE_ATTR(topmem, S_IRUGO, amd64_top_mem_show, NULL);
 static DEVICE_ATTR(topmem2, S_IRUGO, amd64_top_mem2_show, NULL);
 static DEVICE_ATTR(dram_hole, S_IRUGO, amd64_hole_show, NULL);
 
-int amd64_create_sysfs_dbg_files(struct mem_ctl_info *mci)
-{
-	int rc;
-
-	rc = device_create_file(&mci->dev, &dev_attr_dhar);
-	if (rc < 0)
-		return rc;
-	rc = device_create_file(&mci->dev, &dev_attr_dbam);
-	if (rc < 0)
-		return rc;
-	rc = device_create_file(&mci->dev, &dev_attr_topmem);
-	if (rc < 0)
-		return rc;
-	rc = device_create_file(&mci->dev, &dev_attr_topmem2);
-	if (rc < 0)
-		return rc;
-	rc = device_create_file(&mci->dev, &dev_attr_dram_hole);
-	if (rc < 0)
-		return rc;
-
-	return 0;
-}
-
-void amd64_remove_sysfs_dbg_files(struct mem_ctl_info *mci)
-{
-	device_remove_file(&mci->dev, &dev_attr_dhar);
-	device_remove_file(&mci->dev, &dev_attr_dbam);
-	device_remove_file(&mci->dev, &dev_attr_topmem);
-	device_remove_file(&mci->dev, &dev_attr_topmem2);
-	device_remove_file(&mci->dev, &dev_attr_dram_hole);
-}
+static struct attribute *amd64_edac_dbg_attrs[] = {
+	&dev_attr_dhar.attr,
+	&dev_attr_dbam.attr,
+	&dev_attr_topmem.attr,
+	&dev_attr_topmem2.attr,
+	&dev_attr_dram_hole.attr,
+	NULL
+};
+
+const struct attribute_group amd64_edac_dbg_group = {
+	.attrs = amd64_edac_dbg_attrs,
+};
diff --git a/drivers/edac/amd64_edac_inj.c b/drivers/edac/amd64_edac_inj.c
index 0d66ae68d468..e14977ff95db 100644
--- a/drivers/edac/amd64_edac_inj.c
+++ b/drivers/edac/amd64_edac_inj.c
@@ -207,35 +207,28 @@ static DEVICE_ATTR(inject_write, S_IWUSR,
 static DEVICE_ATTR(inject_read,  S_IWUSR,
 		   NULL, amd64_inject_read_store);
 
-
-int amd64_create_sysfs_inject_files(struct mem_ctl_info *mci)
+static struct attribute *amd64_edac_inj_attrs[] = {
+	&dev_attr_inject_section.attr,
+	&dev_attr_inject_word.attr,
+	&dev_attr_inject_ecc_vector.attr,
+	&dev_attr_inject_write.attr,
+	&dev_attr_inject_read.attr,
+	NULL
+};
+
+static umode_t amd64_edac_inj_is_visible(struct kobject *kobj,
+					 struct attribute *attr, int idx)
 {
-	int rc;
-
-	rc = device_create_file(&mci->dev, &dev_attr_inject_section);
-	if (rc < 0)
-		return rc;
-	rc = device_create_file(&mci->dev, &dev_attr_inject_word);
-	if (rc < 0)
-		return rc;
-	rc = device_create_file(&mci->dev, &dev_attr_inject_ecc_vector);
-	if (rc < 0)
-		return rc;
-	rc = device_create_file(&mci->dev, &dev_attr_inject_write);
-	if (rc < 0)
-		return rc;
-	rc = device_create_file(&mci->dev, &dev_attr_inject_read);
-	if (rc < 0)
-		return rc;
-
-	return 0;
-}
+	struct device *dev = kobj_to_dev(kobj);
+	struct mem_ctl_info *mci = container_of(dev, struct mem_ctl_info, dev);
+	struct amd64_pvt *pvt = mci->pvt_info;
 
-void amd64_remove_sysfs_inject_files(struct mem_ctl_info *mci)
-{
-	device_remove_file(&mci->dev, &dev_attr_inject_section);
-	device_remove_file(&mci->dev, &dev_attr_inject_word);
-	device_remove_file(&mci->dev, &dev_attr_inject_ecc_vector);
-	device_remove_file(&mci->dev, &dev_attr_inject_write);
-	device_remove_file(&mci->dev, &dev_attr_inject_read);
+	if (pvt->fam < 0x10)
+		return 0;
+	return attr->mode;
 }
+
+const struct attribute_group amd64_edac_inj_group = {
+	.attrs = amd64_edac_inj_attrs,
+	.is_visible = amd64_edac_inj_is_visible,
+};
diff --git a/drivers/edac/edac_core.h b/drivers/edac/edac_core.h
index 6c9f381e8fe6..ad42587c3f4d 100644
--- a/drivers/edac/edac_core.h
+++ b/drivers/edac/edac_core.h
@@ -446,7 +446,9 @@ struct mem_ctl_info *edac_mc_alloc(unsigned mc_num,
 				   unsigned n_layers,
 				   struct edac_mc_layer *layers,
 				   unsigned sz_pvt);
-extern int edac_mc_add_mc(struct mem_ctl_info *mci);
+extern int edac_mc_add_mc_with_groups(struct mem_ctl_info *mci,
+				      const struct attribute_group **groups);
+#define edac_mc_add_mc(mci)	edac_mc_add_mc_with_groups(mci, NULL)
 extern void edac_mc_free(struct mem_ctl_info *mci);
 extern struct mem_ctl_info *edac_mc_find(int idx);
 extern struct mem_ctl_info *find_mci_by_dev(struct device *dev);
diff --git a/drivers/edac/edac_mc.c b/drivers/edac/edac_mc.c
index 1747906f10ce..af3be1914dbb 100644
--- a/drivers/edac/edac_mc.c
+++ b/drivers/edac/edac_mc.c
@@ -710,9 +710,10 @@ struct mem_ctl_info *edac_mc_find(int idx)
 EXPORT_SYMBOL(edac_mc_find);
 
 /**
- * edac_mc_add_mc: Insert the 'mci' structure into the mci global list and
- *                 create sysfs entries associated with mci structure
+ * edac_mc_add_mc_with_groups: Insert the 'mci' structure into the mci
+ *	global list and create sysfs entries associated with mci structure
  * @mci: pointer to the mci structure to be added to the list
+ * @groups: optional attribute groups for the driver-specific sysfs entries
  *
  * Return:
  *	0	Success
@@ -720,7 +721,8 @@ EXPORT_SYMBOL(edac_mc_find);
  */
 
 /* FIXME - should a warning be printed if no error detection? correction? */
-int edac_mc_add_mc(struct mem_ctl_info *mci)
+int edac_mc_add_mc_with_groups(struct mem_ctl_info *mci,
+			       const struct attribute_group **groups)
 {
 	int ret = -EINVAL;
 	edac_dbg(0, "\n");
@@ -771,7 +773,7 @@ int edac_mc_add_mc(struct mem_ctl_info *mci)
 
 	mci->bus = &mc_bus[mci->mc_idx];
 
-	if (edac_create_sysfs_mci_device(mci)) {
+	if (edac_create_sysfs_mci_device(mci, groups)) {
 		edac_mc_printk(mci, KERN_WARNING,
 			"failed to create sysfs device\n");
 		goto fail1;
@@ -805,7 +807,7 @@ fail0:
 	mutex_unlock(&mem_ctls_mutex);
 	return ret;
 }
-EXPORT_SYMBOL_GPL(edac_mc_add_mc);
+EXPORT_SYMBOL_GPL(edac_mc_add_mc_with_groups);
 
 /**
  * edac_mc_del_mc: Remove sysfs entries for specified mci structure and
diff --git a/drivers/edac/edac_mc_sysfs.c b/drivers/edac/edac_mc_sysfs.c
index c84eecb191ef..112d63ad1154 100644
--- a/drivers/edac/edac_mc_sysfs.c
+++ b/drivers/edac/edac_mc_sysfs.c
@@ -323,13 +323,14 @@ DEVICE_CHANNEL(ch5_dimm_label, S_IRUGO | S_IWUSR,
 	channel_dimm_label_show, channel_dimm_label_store, 5);
 
 /* Total possible dynamic DIMM Label attribute file table */
-static struct device_attribute *dynamic_csrow_dimm_attr[] = {
-	&dev_attr_legacy_ch0_dimm_label.attr,
-	&dev_attr_legacy_ch1_dimm_label.attr,
-	&dev_attr_legacy_ch2_dimm_label.attr,
-	&dev_attr_legacy_ch3_dimm_label.attr,
-	&dev_attr_legacy_ch4_dimm_label.attr,
-	&dev_attr_legacy_ch5_dimm_label.attr
+static struct attribute *dynamic_csrow_dimm_attr[] = {
+	&dev_attr_legacy_ch0_dimm_label.attr.attr,
+	&dev_attr_legacy_ch1_dimm_label.attr.attr,
+	&dev_attr_legacy_ch2_dimm_label.attr.attr,
+	&dev_attr_legacy_ch3_dimm_label.attr.attr,
+	&dev_attr_legacy_ch4_dimm_label.attr.attr,
+	&dev_attr_legacy_ch5_dimm_label.attr.attr,
+	NULL
 };
 
 /* possible dynamic channel ce_count attribute files */
@@ -347,13 +348,45 @@ DEVICE_CHANNEL(ch5_ce_count, S_IRUGO,
 		   channel_ce_count_show, NULL, 5);
 
 /* Total possible dynamic ce_count attribute file table */
-static struct device_attribute *dynamic_csrow_ce_count_attr[] = {
-	&dev_attr_legacy_ch0_ce_count.attr,
-	&dev_attr_legacy_ch1_ce_count.attr,
-	&dev_attr_legacy_ch2_ce_count.attr,
-	&dev_attr_legacy_ch3_ce_count.attr,
-	&dev_attr_legacy_ch4_ce_count.attr,
-	&dev_attr_legacy_ch5_ce_count.attr
+static struct attribute *dynamic_csrow_ce_count_attr[] = {
+	&dev_attr_legacy_ch0_ce_count.attr.attr,
+	&dev_attr_legacy_ch1_ce_count.attr.attr,
+	&dev_attr_legacy_ch2_ce_count.attr.attr,
+	&dev_attr_legacy_ch3_ce_count.attr.attr,
+	&dev_attr_legacy_ch4_ce_count.attr.attr,
+	&dev_attr_legacy_ch5_ce_count.attr.attr,
+	NULL
+};
+
+static umode_t csrow_dev_is_visible(struct kobject *kobj,
+				    struct attribute *attr, int idx)
+{
+	struct device *dev = kobj_to_dev(kobj);
+	struct csrow_info *csrow = container_of(dev, struct csrow_info, dev);
+
+	if (idx >= csrow->nr_channels)
+		return 0;
+	/* Only expose populated DIMMs */
+	if (!csrow->channels[idx]->dimm->nr_pages)
+		return 0;
+	return attr->mode;
+}
+
+
+static const struct attribute_group csrow_dev_dimm_group = {
+	.attrs = dynamic_csrow_dimm_attr,
+	.is_visible = csrow_dev_is_visible,
+};
+
+static const struct attribute_group csrow_dev_ce_count_group = {
+	.attrs = dynamic_csrow_ce_count_attr,
+	.is_visible = csrow_dev_is_visible,
+};
+
+static const struct attribute_group *csrow_dev_groups[] = {
+	&csrow_dev_dimm_group,
+	&csrow_dev_ce_count_group,
+	NULL
 };
 
 static inline int nr_pages_per_csrow(struct csrow_info *csrow)
@@ -370,13 +403,12 @@ static inline int nr_pages_per_csrow(struct csrow_info *csrow)
 static int edac_create_csrow_object(struct mem_ctl_info *mci,
 				    struct csrow_info *csrow, int index)
 {
-	int err, chan;
-
 	if (csrow->nr_channels > EDAC_NR_CHANNELS)
 		return -ENODEV;
 
 	csrow->dev.type = &csrow_attr_type;
 	csrow->dev.bus = mci->bus;
+	csrow->dev.groups = csrow_dev_groups;
 	device_initialize(&csrow->dev);
 	csrow->dev.parent = &mci->dev;
 	csrow->mci = mci;
@@ -386,45 +418,13 @@ static int edac_create_csrow_object(struct mem_ctl_info *mci,
 	edac_dbg(0, "creating (virtual) csrow node %s\n",
 		 dev_name(&csrow->dev));
 
-	err = device_add(&csrow->dev);
-	if (err < 0)
-		return err;
-
-	for (chan = 0; chan < csrow->nr_channels; chan++) {
-		/* Only expose populated DIMMs */
-		if (!csrow->channels[chan]->dimm->nr_pages)
-			continue;
-		err = device_create_file(&csrow->dev,
-					 dynamic_csrow_dimm_attr[chan]);
-		if (err < 0)
-			goto error;
-		err = device_create_file(&csrow->dev,
-					 dynamic_csrow_ce_count_attr[chan]);
-		if (err < 0) {
-			device_remove_file(&csrow->dev,
-					   dynamic_csrow_dimm_attr[chan]);
-			goto error;
-		}
-	}
-
-	return 0;
-
-error:
-	for (--chan; chan >= 0; chan--) {
-		device_remove_file(&csrow->dev,
-					dynamic_csrow_dimm_attr[chan]);
-		device_remove_file(&csrow->dev,
-					   dynamic_csrow_ce_count_attr[chan]);
-	}
-	put_device(&csrow->dev);
-
-	return err;
+	return device_add(&csrow->dev);
 }
 
 /* Create a CSROW object under specifed edac_mc_device */
 static int edac_create_csrow_objects(struct mem_ctl_info *mci)
 {
-	int err, i, chan;
+	int err, i;
 	struct csrow_info *csrow;
 
 	for (i = 0; i < mci->nr_csrows; i++) {
@@ -446,14 +446,6 @@ error:
 		csrow = mci->csrows[i];
 		if (!nr_pages_per_csrow(csrow))
 			continue;
-		for (chan = csrow->nr_channels - 1; chan >= 0; chan--) {
-			if (!csrow->channels[chan]->dimm->nr_pages)
-				continue;
-			device_remove_file(&csrow->dev,
-						dynamic_csrow_dimm_attr[chan]);
-			device_remove_file(&csrow->dev,
-						dynamic_csrow_ce_count_attr[chan]);
-		}
 		put_device(&mci->csrows[i]->dev);
 	}
 
@@ -462,23 +454,13 @@ error:
 
 static void edac_delete_csrow_objects(struct mem_ctl_info *mci)
 {
-	int i, chan;
+	int i;
 	struct csrow_info *csrow;
 
 	for (i = mci->nr_csrows - 1; i >= 0; i--) {
 		csrow = mci->csrows[i];
 		if (!nr_pages_per_csrow(csrow))
 			continue;
-		for (chan = csrow->nr_channels - 1; chan >= 0; chan--) {
-			if (!csrow->channels[chan]->dimm->nr_pages)
-				continue;
-			edac_dbg(1, "Removing csrow %d channel %d sysfs nodes\n",
-				 i, chan);
-			device_remove_file(&csrow->dev,
-						dynamic_csrow_dimm_attr[chan]);
-			device_remove_file(&csrow->dev,
-						dynamic_csrow_ce_count_attr[chan]);
-		}
 		device_unregister(&mci->csrows[i]->dev);
 	}
 }
@@ -863,7 +845,8 @@ static DEVICE_ATTR(ce_count, S_IRUGO, mci_ce_count_show, NULL);
 static DEVICE_ATTR(max_location, S_IRUGO, mci_max_location_show, NULL);
 
 /* memory scrubber attribute file */
-static DEVICE_ATTR(sdram_scrub_rate, 0, NULL, NULL);
+DEVICE_ATTR(sdram_scrub_rate, 0, mci_sdram_scrub_rate_show,
+	    mci_sdram_scrub_rate_store); /* umode set later in is_visible */
 
 static struct attribute *mci_attrs[] = {
 	&dev_attr_reset_counters.attr,
@@ -875,11 +858,29 @@ static struct attribute *mci_attrs[] = {
 	&dev_attr_ue_count.attr,
 	&dev_attr_ce_count.attr,
 	&dev_attr_max_location.attr,
+	&dev_attr_sdram_scrub_rate.attr,
 	NULL
 };
 
+static umode_t mci_attr_is_visible(struct kobject *kobj,
+				   struct attribute *attr, int idx)
+{
+	struct device *dev = kobj_to_dev(kobj);
+	struct mem_ctl_info *mci = to_mci(dev);
+	umode_t mode = 0;
+
+	if (attr != &dev_attr_sdram_scrub_rate.attr)
+		return attr->mode;
+	if (mci->get_sdram_scrub_rate)
+		mode |= S_IRUGO;
+	if (mci->set_sdram_scrub_rate)
+		mode |= S_IWUSR;
+	return mode;
+}
+
 static struct attribute_group mci_attr_grp = {
 	.attrs	= mci_attrs,
+	.is_visible = mci_attr_is_visible,
 };
 
 static const struct attribute_group *mci_attr_groups[] = {
@@ -913,7 +914,7 @@ int __init edac_debugfs_init(void)
 	return 0;
 }
 
-void __exit edac_debugfs_exit(void)
+void edac_debugfs_exit(void)
 {
 	debugfs_remove(edac_debugfs);
 }
@@ -973,7 +974,8 @@ nomem:
  *	0	Success
  *	!0	Failure
  */
-int edac_create_sysfs_mci_device(struct mem_ctl_info *mci)
+int edac_create_sysfs_mci_device(struct mem_ctl_info *mci,
+				 const struct attribute_group **groups)
 {
 	int i, err;
 
@@ -997,6 +999,7 @@ int edac_create_sysfs_mci_device(struct mem_ctl_info *mci)
 
 	mci->dev.parent = mci_pdev;
 	mci->dev.bus = mci->bus;
+	mci->dev.groups = groups;
 	dev_set_name(&mci->dev, "mc%d", mci->mc_idx);
 	dev_set_drvdata(&mci->dev, mci);
 	pm_runtime_forbid(&mci->dev);
@@ -1008,23 +1011,6 @@ int edac_create_sysfs_mci_device(struct mem_ctl_info *mci)
 		goto fail_unregister_bus;
 	}
 
-	if (mci->set_sdram_scrub_rate || mci->get_sdram_scrub_rate) {
-		if (mci->get_sdram_scrub_rate) {
-			dev_attr_sdram_scrub_rate.attr.mode |= S_IRUGO;
-			dev_attr_sdram_scrub_rate.show = &mci_sdram_scrub_rate_show;
-		}
-
-		if (mci->set_sdram_scrub_rate) {
-			dev_attr_sdram_scrub_rate.attr.mode |= S_IWUSR;
-			dev_attr_sdram_scrub_rate.store = &mci_sdram_scrub_rate_store;
-		}
-
-		err = device_create_file(&mci->dev, &dev_attr_sdram_scrub_rate);
-		if (err) {
-			edac_dbg(1, "failure: create sdram_scrub_rate\n");
-			goto fail_unregister_dev;
-		}
-	}
 	/*
 	 * Create the dimm/rank devices
 	 */
@@ -1071,7 +1057,6 @@ fail_unregister_dimm:
 
 		device_unregister(&dimm->dev);
 	}
-fail_unregister_dev:
 	device_unregister(&mci->dev);
 fail_unregister_bus:
 	bus_unregister(mci->bus);
@@ -1170,7 +1155,7 @@ int __init edac_mc_sysfs_init(void)
 	return err;
 }
 
-void __exit edac_mc_sysfs_exit(void)
+void edac_mc_sysfs_exit(void)
 {
 	device_unregister(mci_pdev);
 	edac_put_sysfs_subsys();
diff --git a/drivers/edac/edac_module.c b/drivers/edac/edac_module.c
index e6d1691dfa45..9cb082a19d8a 100644
--- a/drivers/edac/edac_module.c
+++ b/drivers/edac/edac_module.c
@@ -112,20 +112,23 @@ static int __init edac_init(void)
 
 	err = edac_mc_sysfs_init();
 	if (err)
-		goto error;
+		goto err_sysfs;
 
 	edac_debugfs_init();
 
-	/* Setup/Initialize the workq for this core */
 	err = edac_workqueue_setup();
 	if (err) {
-		edac_printk(KERN_ERR, EDAC_MC, "init WorkQueue failure\n");
-		goto error;
+		edac_printk(KERN_ERR, EDAC_MC, "Failure initializing workqueue\n");
+		goto err_wq;
 	}
 
 	return 0;
 
-error:
+err_wq:
+	edac_debugfs_exit();
+	edac_mc_sysfs_exit();
+
+err_sysfs:
 	return err;
 }
 
diff --git a/drivers/edac/edac_module.h b/drivers/edac/edac_module.h
index f2118bfcf8df..26ecc52e073d 100644
--- a/drivers/edac/edac_module.h
+++ b/drivers/edac/edac_module.h
@@ -22,7 +22,8 @@
 	/* on edac_mc_sysfs.c */
 int edac_mc_sysfs_init(void);
 void edac_mc_sysfs_exit(void);
-extern int edac_create_sysfs_mci_device(struct mem_ctl_info *mci);
+extern int edac_create_sysfs_mci_device(struct mem_ctl_info *mci,
+					const struct attribute_group **groups);
 extern void edac_remove_sysfs_mci_device(struct mem_ctl_info *mci);
 void edac_unregister_sysfs(struct mem_ctl_info *mci);
 extern int edac_get_log_ue(void);
diff --git a/drivers/edac/highbank_mc_edac.c b/drivers/edac/highbank_mc_edac.c
index f784de1dc793..11260cc3360e 100644
--- a/drivers/edac/highbank_mc_edac.c
+++ b/drivers/edac/highbank_mc_edac.c
@@ -124,6 +124,13 @@ static ssize_t highbank_mc_inject_ctrl(struct device *dev,
 
 static DEVICE_ATTR(inject_ctrl, S_IWUSR, NULL, highbank_mc_inject_ctrl);
 
+static struct attribute *highbank_dev_attrs[] = {
+	&dev_attr_inject_ctrl.attr,
+	NULL
+};
+
+ATTRIBUTE_GROUPS(highbank_dev);
+
 struct hb_mc_settings {
 	int	err_offset;
 	int	int_offset;
@@ -139,7 +146,7 @@ static struct hb_mc_settings mw_settings = {
 	.int_offset = MW_DDR_ECC_INT_BASE,
 };
 
-static struct of_device_id hb_ddr_ctrl_of_match[] = {
+static const struct of_device_id hb_ddr_ctrl_of_match[] = {
 	{ .compatible = "calxeda,hb-ddr-ctrl",		.data = &hb_settings },
 	{ .compatible = "calxeda,ecx-2000-ddr-ctrl",	.data = &mw_settings },
 	{},
@@ -231,7 +238,7 @@ static int highbank_mc_probe(struct platform_device *pdev)
 	dimm->mtype = MEM_DDR3;
 	dimm->edac_mode = EDAC_SECDED;
 
-	res = edac_mc_add_mc(mci);
+	res = edac_mc_add_mc_with_groups(mci, highbank_dev_groups);
 	if (res < 0)
 		goto err;
 
@@ -243,8 +250,6 @@ static int highbank_mc_probe(struct platform_device *pdev)
 		goto err2;
 	}
 
-	device_create_file(&mci->dev, &dev_attr_inject_ctrl);
-
 	devres_close_group(&pdev->dev, NULL);
 	return 0;
 err2:
@@ -259,7 +264,6 @@ static int highbank_mc_remove(struct platform_device *pdev)
 {
 	struct mem_ctl_info *mci = platform_get_drvdata(pdev);
 
-	device_remove_file(&mci->dev, &dev_attr_inject_ctrl);
 	edac_mc_del_mc(&pdev->dev);
 	edac_mc_free(mci);
 	return 0;
diff --git a/drivers/edac/i7core_edac.c b/drivers/edac/i7core_edac.c
index 9cd0b301f81b..01087a38da22 100644
--- a/drivers/edac/i7core_edac.c
+++ b/drivers/edac/i7core_edac.c
@@ -1157,27 +1157,24 @@ static DEVICE_ATTR(inject_eccmask, S_IRUGO | S_IWUSR,
 static DEVICE_ATTR(inject_enable, S_IRUGO | S_IWUSR,
 		   i7core_inject_enable_show, i7core_inject_enable_store);
 
+static struct attribute *i7core_dev_attrs[] = {
+	&dev_attr_inject_section.attr,
+	&dev_attr_inject_type.attr,
+	&dev_attr_inject_eccmask.attr,
+	&dev_attr_inject_enable.attr,
+	NULL
+};
+
+ATTRIBUTE_GROUPS(i7core_dev);
+
 static int i7core_create_sysfs_devices(struct mem_ctl_info *mci)
 {
 	struct i7core_pvt *pvt = mci->pvt_info;
 	int rc;
 
-	rc = device_create_file(&mci->dev, &dev_attr_inject_section);
-	if (rc < 0)
-		return rc;
-	rc = device_create_file(&mci->dev, &dev_attr_inject_type);
-	if (rc < 0)
-		return rc;
-	rc = device_create_file(&mci->dev, &dev_attr_inject_eccmask);
-	if (rc < 0)
-		return rc;
-	rc = device_create_file(&mci->dev, &dev_attr_inject_enable);
-	if (rc < 0)
-		return rc;
-
 	pvt->addrmatch_dev = kzalloc(sizeof(*pvt->addrmatch_dev), GFP_KERNEL);
 	if (!pvt->addrmatch_dev)
-		return rc;
+		return -ENOMEM;
 
 	pvt->addrmatch_dev->type = &addrmatch_type;
 	pvt->addrmatch_dev->bus = mci->dev.bus;
@@ -1198,7 +1195,7 @@ static int i7core_create_sysfs_devices(struct mem_ctl_info *mci)
 		if (!pvt->chancounts_dev) {
 			put_device(pvt->addrmatch_dev);
 			device_del(pvt->addrmatch_dev);
-			return rc;
+			return -ENOMEM;
 		}
 
 		pvt->chancounts_dev->type = &all_channel_counts_type;
@@ -1223,11 +1220,6 @@ static void i7core_delete_sysfs_devices(struct mem_ctl_info *mci)
 
 	edac_dbg(1, "\n");
 
-	device_remove_file(&mci->dev, &dev_attr_inject_section);
-	device_remove_file(&mci->dev, &dev_attr_inject_type);
-	device_remove_file(&mci->dev, &dev_attr_inject_eccmask);
-	device_remove_file(&mci->dev, &dev_attr_inject_enable);
-
 	if (!pvt->is_registered) {
 		put_device(pvt->chancounts_dev);
 		device_del(pvt->chancounts_dev);
@@ -2259,7 +2251,7 @@ static int i7core_register_mci(struct i7core_dev *i7core_dev)
 		enable_sdram_scrub_setting(mci);
 
 	/* add this new MC control structure to EDAC's list of MCs */
-	if (unlikely(edac_mc_add_mc(mci))) {
+	if (unlikely(edac_mc_add_mc_with_groups(mci, i7core_dev_groups))) {
 		edac_dbg(0, "MC: failed edac_mc_add_mc()\n");
 		/* FIXME: perhaps some code should go here that disables error
 		 * reporting if we just enabled it
diff --git a/drivers/edac/i82443bxgx_edac.c b/drivers/edac/i82443bxgx_edac.c
index b4705d9366bf..4d4110364f02 100644
--- a/drivers/edac/i82443bxgx_edac.c
+++ b/drivers/edac/i82443bxgx_edac.c
@@ -350,8 +350,6 @@ fail:
 	return -ENODEV;
 }
 
-EXPORT_SYMBOL_GPL(i82443bxgx_edacmc_probe1);
-
 /* returns count (>= 0), or negative on error */
 static int i82443bxgx_edacmc_init_one(struct pci_dev *pdev,
 				      const struct pci_device_id *ent)
@@ -384,8 +382,6 @@ static void i82443bxgx_edacmc_remove_one(struct pci_dev *pdev)
 	edac_mc_free(mci);
 }
 
-EXPORT_SYMBOL_GPL(i82443bxgx_edacmc_remove_one);
-
 static const struct pci_device_id i82443bxgx_pci_tbl[] = {
 	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, PCI_DEVICE_ID_INTEL_82443BX_0)},
 	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, PCI_DEVICE_ID_INTEL_82443BX_2)},
@@ -445,9 +441,7 @@ fail1:
 	pci_unregister_driver(&i82443bxgx_edacmc_driver);
 
 fail0:
-	if (mci_pdev != NULL)
-		pci_dev_put(mci_pdev);
-
+	pci_dev_put(mci_pdev);
 	return pci_rc;
 }
 
diff --git a/drivers/edac/i82860_edac.c b/drivers/edac/i82860_edac.c
index 4382343a7c60..ee1078cd3b96 100644
--- a/drivers/edac/i82860_edac.c
+++ b/drivers/edac/i82860_edac.c
@@ -343,20 +343,15 @@ fail1:
 	pci_unregister_driver(&i82860_driver);
 
 fail0:
-	if (mci_pdev != NULL)
-		pci_dev_put(mci_pdev);
-
+	pci_dev_put(mci_pdev);
 	return pci_rc;
 }
 
 static void __exit i82860_exit(void)
 {
 	edac_dbg(3, "\n");
-
 	pci_unregister_driver(&i82860_driver);
-
-	if (mci_pdev != NULL)
-		pci_dev_put(mci_pdev);
+	pci_dev_put(mci_pdev);
 }
 
 module_init(i82860_init);
diff --git a/drivers/edac/i82875p_edac.c b/drivers/edac/i82875p_edac.c
index 64b68320249f..c26a513f8869 100644
--- a/drivers/edac/i82875p_edac.c
+++ b/drivers/edac/i82875p_edac.c
@@ -576,9 +576,7 @@ fail1:
 	pci_unregister_driver(&i82875p_driver);
 
 fail0:
-	if (mci_pdev != NULL)
-		pci_dev_put(mci_pdev);
-
+	pci_dev_put(mci_pdev);
 	return pci_rc;
 }
 
diff --git a/drivers/edac/i82975x_edac.c b/drivers/edac/i82975x_edac.c
index 10b10521f62e..35ab66c623a3 100644
--- a/drivers/edac/i82975x_edac.c
+++ b/drivers/edac/i82975x_edac.c
@@ -685,9 +685,7 @@ fail1:
 	pci_unregister_driver(&i82975x_driver);
 
 fail0:
-	if (mci_pdev != NULL)
-		pci_dev_put(mci_pdev);
-
+	pci_dev_put(mci_pdev);
 	return pci_rc;
 }
 
diff --git a/drivers/edac/mpc85xx_edac.c b/drivers/edac/mpc85xx_edac.c
index 1fa76a588af3..68bf234bdfe6 100644
--- a/drivers/edac/mpc85xx_edac.c
+++ b/drivers/edac/mpc85xx_edac.c
@@ -134,29 +134,14 @@ DEVICE_ATTR(inject_data_lo, S_IRUGO | S_IWUSR,
 DEVICE_ATTR(inject_ctrl, S_IRUGO | S_IWUSR,
 	    mpc85xx_mc_inject_ctrl_show, mpc85xx_mc_inject_ctrl_store);
 
-static int mpc85xx_create_sysfs_attributes(struct mem_ctl_info *mci)
-{
-	int rc;
-
-	rc = device_create_file(&mci->dev, &dev_attr_inject_data_hi);
-	if (rc < 0)
-		return rc;
-	rc = device_create_file(&mci->dev, &dev_attr_inject_data_lo);
-	if (rc < 0)
-		return rc;
-	rc = device_create_file(&mci->dev, &dev_attr_inject_ctrl);
-	if (rc < 0)
-		return rc;
+static struct attribute *mpc85xx_dev_attrs[] = {
+	&dev_attr_inject_data_hi.attr,
+	&dev_attr_inject_data_lo.attr,
+	&dev_attr_inject_ctrl.attr,
+	NULL
+};
 
-	return 0;
-}
-
-static void mpc85xx_remove_sysfs_attributes(struct mem_ctl_info *mci)
-{
-	device_remove_file(&mci->dev, &dev_attr_inject_data_hi);
-	device_remove_file(&mci->dev, &dev_attr_inject_data_lo);
-	device_remove_file(&mci->dev, &dev_attr_inject_ctrl);
-}
+ATTRIBUTE_GROUPS(mpc85xx_dev);
 
 /**************************** PCI Err device ***************************/
 #ifdef CONFIG_PCI
@@ -685,7 +670,7 @@ static int mpc85xx_l2_err_remove(struct platform_device *op)
 	return 0;
 }
 
-static struct of_device_id mpc85xx_l2_err_of_match[] = {
+static const struct of_device_id mpc85xx_l2_err_of_match[] = {
 /* deprecate the fsl,85.. forms in the future, 2.6.30? */
 	{ .compatible = "fsl,8540-l2-cache-controller", },
 	{ .compatible = "fsl,8541-l2-cache-controller", },
@@ -1106,13 +1091,7 @@ static int mpc85xx_mc_err_probe(struct platform_device *op)
 	/* clear all error bits */
 	out_be32(pdata->mc_vbase + MPC85XX_MC_ERR_DETECT, ~0);
 
-	if (edac_mc_add_mc(mci)) {
-		edac_dbg(3, "failed edac_mc_add_mc()\n");
-		goto err;
-	}
-
-	if (mpc85xx_create_sysfs_attributes(mci)) {
-		edac_mc_del_mc(mci->pdev);
+	if (edac_mc_add_mc_with_groups(mci, mpc85xx_dev_groups)) {
 		edac_dbg(3, "failed edac_mc_add_mc()\n");
 		goto err;
 	}
@@ -1176,13 +1155,12 @@ static int mpc85xx_mc_err_remove(struct platform_device *op)
 		 orig_ddr_err_disable);
 	out_be32(pdata->mc_vbase + MPC85XX_MC_ERR_SBE, orig_ddr_err_sbe);
 
-	mpc85xx_remove_sysfs_attributes(mci);
 	edac_mc_del_mc(&op->dev);
 	edac_mc_free(mci);
 	return 0;
 }
 
-static struct of_device_id mpc85xx_mc_err_of_match[] = {
+static const struct of_device_id mpc85xx_mc_err_of_match[] = {
 /* deprecate the fsl,85.. forms in the future, 2.6.30? */
 	{ .compatible = "fsl,8540-memory-controller", },
 	{ .compatible = "fsl,8541-memory-controller", },
diff --git a/drivers/edac/octeon_edac-lmc.c b/drivers/edac/octeon_edac-lmc.c
index 4bd10f94f068..bb19e0732681 100644
--- a/drivers/edac/octeon_edac-lmc.c
+++ b/drivers/edac/octeon_edac-lmc.c
@@ -209,35 +209,18 @@ static DEVICE_ATTR(row, S_IRUGO | S_IWUSR,
 static DEVICE_ATTR(col, S_IRUGO | S_IWUSR,
 		   octeon_mc_inject_col_show, octeon_mc_inject_col_store);
 
+static struct attribute *octeon_dev_attrs[] = {
+	&dev_attr_inject.attr,
+	&dev_attr_error_type.attr,
+	&dev_attr_dimm.attr,
+	&dev_attr_rank.attr,
+	&dev_attr_bank.attr,
+	&dev_attr_row.attr,
+	&dev_attr_col.attr,
+	NULL
+};
 
-static int octeon_set_mc_sysfs_attributes(struct mem_ctl_info *mci)
-{
-	int rc;
-
-	rc = device_create_file(&mci->dev, &dev_attr_inject);
-	if (rc < 0)
-		return rc;
-	rc = device_create_file(&mci->dev, &dev_attr_error_type);
-	if (rc < 0)
-		return rc;
-	rc = device_create_file(&mci->dev, &dev_attr_dimm);
-	if (rc < 0)
-		return rc;
-	rc = device_create_file(&mci->dev, &dev_attr_rank);
-	if (rc < 0)
-		return rc;
-	rc = device_create_file(&mci->dev, &dev_attr_bank);
-	if (rc < 0)
-		return rc;
-	rc = device_create_file(&mci->dev, &dev_attr_row);
-	if (rc < 0)
-		return rc;
-	rc = device_create_file(&mci->dev, &dev_attr_col);
-	if (rc < 0)
-		return rc;
-
-	return 0;
-}
+ATTRIBUTE_GROUPS(octeon_dev);
 
 static int octeon_lmc_edac_probe(struct platform_device *pdev)
 {
@@ -271,18 +254,12 @@ static int octeon_lmc_edac_probe(struct platform_device *pdev)
 		mci->ctl_name = "octeon-lmc-err";
 		mci->edac_check = octeon_lmc_edac_poll;
 
-		if (edac_mc_add_mc(mci)) {
+		if (edac_mc_add_mc_with_groups(mci, octeon_dev_groups)) {
 			dev_err(&pdev->dev, "edac_mc_add_mc() failed\n");
 			edac_mc_free(mci);
 			return -ENXIO;
 		}
 
-		if (octeon_set_mc_sysfs_attributes(mci)) {
-			dev_err(&pdev->dev, "octeon_set_mc_sysfs_attributes() failed\n");
-			return -ENXIO;
-		}
-
-
 		cfg0.u64 = cvmx_read_csr(CVMX_LMCX_MEM_CFG0(mc));
 		cfg0.s.intr_ded_ena = 0;	/* We poll */
 		cfg0.s.intr_sec_ena = 0;
@@ -309,18 +286,12 @@ static int octeon_lmc_edac_probe(struct platform_device *pdev)
 		mci->ctl_name = "co_lmc_err";
 		mci->edac_check = octeon_lmc_edac_poll_o2;
 
-		if (edac_mc_add_mc(mci)) {
+		if (edac_mc_add_mc_with_groups(mci, octeon_dev_groups)) {
 			dev_err(&pdev->dev, "edac_mc_add_mc() failed\n");
 			edac_mc_free(mci);
 			return -ENXIO;
 		}
 
-		if (octeon_set_mc_sysfs_attributes(mci)) {
-			dev_err(&pdev->dev, "octeon_set_mc_sysfs_attributes() failed\n");
-			return -ENXIO;
-		}
-
-
 		en.u64 = cvmx_read_csr(CVMX_LMCX_MEM_CFG0(mc));
 		en.s.intr_ded_ena = 0;	/* We poll */
 		en.s.intr_sec_ena = 0;
diff --git a/drivers/edac/ppc4xx_edac.c b/drivers/edac/ppc4xx_edac.c
index 1b64fd060821..3515b381c131 100644
--- a/drivers/edac/ppc4xx_edac.c
+++ b/drivers/edac/ppc4xx_edac.c
@@ -193,7 +193,7 @@ static int ppc4xx_edac_remove(struct platform_device *device);
  * Device tree node type and compatible tuples this driver can match
  * on.
  */
-static struct of_device_id ppc4xx_edac_match[] = {
+static const struct of_device_id ppc4xx_edac_match[] = {
 	{
 		.compatible	= "ibm,sdram-4xx-ddr2"
 	},
diff --git a/drivers/edac/synopsys_edac.c b/drivers/edac/synopsys_edac.c
index 1c9691535e13..fc153aea2f6c 100644
--- a/drivers/edac/synopsys_edac.c
+++ b/drivers/edac/synopsys_edac.c
@@ -512,7 +512,7 @@ static int synps_edac_mc_remove(struct platform_device *pdev)
 	return 0;
 }
 
-static struct of_device_id synps_edac_match[] = {
+static const struct of_device_id synps_edac_match[] = {
 	{ .compatible = "xlnx,zynq-ddrc-a05", },
 	{ /* end of table */ }
 };
diff --git a/drivers/firmware/dmi_scan.c b/drivers/firmware/dmi_scan.c
index c5f7b4e9eb6c..6e45a43ffe84 100644
--- a/drivers/firmware/dmi_scan.c
+++ b/drivers/firmware/dmi_scan.c
@@ -17,7 +17,9 @@
  */
 static const char dmi_empty_string[] = "        ";
 
-static u16 __initdata dmi_ver;
+static u32 dmi_ver __initdata;
+static u32 dmi_len;
+static u16 dmi_num;
 /*
  * Catch too early calls to dmi_check_system():
  */
@@ -78,7 +80,7 @@ static const char * __init dmi_string(const struct dmi_header *dm, u8 s)
  *	We have to be cautious here. We have seen BIOSes with DMI pointers
  *	pointing to completely the wrong place for example
  */
-static void dmi_table(u8 *buf, int len, int num,
+static void dmi_table(u8 *buf,
 		      void (*decode)(const struct dmi_header *, void *),
 		      void *private_data)
 {
@@ -86,36 +88,38 @@ static void dmi_table(u8 *buf, int len, int num,
 	int i = 0;
 
 	/*
-	 *	Stop when we see all the items the table claimed to have
-	 *	OR we run off the end of the table (also happens)
+	 * Stop when we have seen all the items the table claimed to have
+	 * (SMBIOS < 3.0 only) OR we reach an end-of-table marker OR we run
+	 * off the end of the table (should never happen but sometimes does
+	 * on bogus implementations.)
 	 */
-	while ((i < num) && (data - buf + sizeof(struct dmi_header)) <= len) {
+	while ((!dmi_num || i < dmi_num) &&
+	       (data - buf + sizeof(struct dmi_header)) <= dmi_len) {
 		const struct dmi_header *dm = (const struct dmi_header *)data;
 
 		/*
-		 * 7.45 End-of-Table (Type 127) [SMBIOS reference spec v3.0.0]
-		 */
-		if (dm->type == DMI_ENTRY_END_OF_TABLE)
-			break;
-
-		/*
 		 *  We want to know the total length (formatted area and
 		 *  strings) before decoding to make sure we won't run off the
 		 *  table in dmi_decode or dmi_string
 		 */
 		data += dm->length;
-		while ((data - buf < len - 1) && (data[0] || data[1]))
+		while ((data - buf < dmi_len - 1) && (data[0] || data[1]))
 			data++;
-		if (data - buf < len - 1)
+		if (data - buf < dmi_len - 1)
 			decode(dm, private_data);
+
+		/*
+		 * 7.45 End-of-Table (Type 127) [SMBIOS reference spec v3.0.0]
+		 */
+		if (dm->type == DMI_ENTRY_END_OF_TABLE)
+			break;
+
 		data += 2;
 		i++;
 	}
 }
 
 static phys_addr_t dmi_base;
-static u16 dmi_len;
-static u16 dmi_num;
 
 static int __init dmi_walk_early(void (*decode)(const struct dmi_header *,
 		void *))
@@ -126,7 +130,7 @@ static int __init dmi_walk_early(void (*decode)(const struct dmi_header *,
 	if (buf == NULL)
 		return -1;
 
-	dmi_table(buf, dmi_len, dmi_num, decode, NULL);
+	dmi_table(buf, decode, NULL);
 
 	add_device_randomness(buf, dmi_len);
 
@@ -197,7 +201,7 @@ static void __init dmi_save_uuid(const struct dmi_header *dm, int slot,
 	 * the UUID are supposed to be little-endian encoded.  The specification
 	 * says that this is the defacto standard.
 	 */
-	if (dmi_ver >= 0x0206)
+	if (dmi_ver >= 0x020600)
 		sprintf(s, "%pUL", d);
 	else
 		sprintf(s, "%pUB", d);
@@ -469,7 +473,7 @@ static void __init dmi_format_ids(char *buf, size_t len)
  */
 static int __init dmi_present(const u8 *buf)
 {
-	int smbios_ver;
+	u32 smbios_ver;
 
 	if (memcmp(buf, "_SM_", 4) == 0 &&
 	    buf[5] < 32 && dmi_checksum(buf, buf[5])) {
@@ -502,14 +506,16 @@ static int __init dmi_present(const u8 *buf)
 		if (dmi_walk_early(dmi_decode) == 0) {
 			if (smbios_ver) {
 				dmi_ver = smbios_ver;
-				pr_info("SMBIOS %d.%d present.\n",
-				       dmi_ver >> 8, dmi_ver & 0xFF);
+				pr_info("SMBIOS %d.%d%s present.\n",
+					dmi_ver >> 8, dmi_ver & 0xFF,
+					(dmi_ver < 0x0300) ? "" : ".x");
 			} else {
 				dmi_ver = (buf[14] & 0xF0) << 4 |
 					   (buf[14] & 0x0F);
 				pr_info("Legacy DMI %d.%d present.\n",
 				       dmi_ver >> 8, dmi_ver & 0xFF);
 			}
+			dmi_ver <<= 8;
 			dmi_format_ids(dmi_ids_string, sizeof(dmi_ids_string));
 			printk(KERN_DEBUG "DMI: %s\n", dmi_ids_string);
 			return 0;
@@ -527,25 +533,16 @@ static int __init dmi_smbios3_present(const u8 *buf)
 {
 	if (memcmp(buf, "_SM3_", 5) == 0 &&
 	    buf[6] < 32 && dmi_checksum(buf, buf[6])) {
-		dmi_ver = get_unaligned_be16(buf + 7);
+		dmi_ver = get_unaligned_be32(buf + 6);
+		dmi_ver &= 0xFFFFFF;
+		dmi_num = 0;			/* No longer specified */
 		dmi_len = get_unaligned_le32(buf + 12);
 		dmi_base = get_unaligned_le64(buf + 16);
 
-		/*
-		 * The 64-bit SMBIOS 3.0 entry point no longer has a field
-		 * containing the number of structures present in the table.
-		 * Instead, it defines the table size as a maximum size, and
-		 * relies on the end-of-table structure type (#127) to be used
-		 * to signal the end of the table.
-		 * So let's define dmi_num as an upper bound as well: each
-		 * structure has a 4 byte header, so dmi_len / 4 is an upper
-		 * bound for the number of structures in the table.
-		 */
-		dmi_num = dmi_len / 4;
-
 		if (dmi_walk_early(dmi_decode) == 0) {
-			pr_info("SMBIOS %d.%d present.\n",
-				dmi_ver >> 8, dmi_ver & 0xFF);
+			pr_info("SMBIOS %d.%d.%d present.\n",
+				dmi_ver >> 16, (dmi_ver >> 8) & 0xFF,
+				dmi_ver & 0xFF);
 			dmi_format_ids(dmi_ids_string, sizeof(dmi_ids_string));
 			pr_debug("DMI: %s\n", dmi_ids_string);
 			return 0;
@@ -900,7 +897,7 @@ int dmi_walk(void (*decode)(const struct dmi_header *, void *),
 	if (buf == NULL)
 		return -1;
 
-	dmi_table(buf, dmi_len, dmi_num, decode, private_data);
+	dmi_table(buf, decode, private_data);
 
 	dmi_unmap(buf);
 	return 0;
diff --git a/drivers/firmware/efi/libstub/arm-stub.c b/drivers/firmware/efi/libstub/arm-stub.c
index dcae482a9a17..e29560e6b40b 100644
--- a/drivers/firmware/efi/libstub/arm-stub.c
+++ b/drivers/firmware/efi/libstub/arm-stub.c
@@ -175,7 +175,7 @@ unsigned long efi_entry(void *handle, efi_system_table_t *sys_table,
 	unsigned long initrd_addr;
 	u64 initrd_size = 0;
 	unsigned long fdt_addr = 0;  /* Original DTB */
-	u64 fdt_size = 0;  /* We don't get size from configuration table */
+	unsigned long fdt_size = 0;
 	char *cmdline_ptr = NULL;
 	int cmdline_size = 0;
 	unsigned long new_fdt_addr;
@@ -239,8 +239,7 @@ unsigned long efi_entry(void *handle, efi_system_table_t *sys_table,
 	} else {
 		status = handle_cmdline_files(sys_table, image, cmdline_ptr,
 					      "dtb=",
-					      ~0UL, (unsigned long *)&fdt_addr,
-					      (unsigned long *)&fdt_size);
+					      ~0UL, &fdt_addr, &fdt_size);
 
 		if (status != EFI_SUCCESS) {
 			pr_efi_err(sys_table, "Failed to load device tree!\n");
@@ -252,7 +251,7 @@ unsigned long efi_entry(void *handle, efi_system_table_t *sys_table,
 		pr_efi(sys_table, "Using DTB from command line\n");
 	} else {
 		/* Look for a device tree configuration table entry. */
-		fdt_addr = (uintptr_t)get_fdt(sys_table);
+		fdt_addr = (uintptr_t)get_fdt(sys_table, &fdt_size);
 		if (fdt_addr)
 			pr_efi(sys_table, "Using DTB from configuration table\n");
 	}
diff --git a/drivers/firmware/efi/libstub/efi-stub-helper.c b/drivers/firmware/efi/libstub/efi-stub-helper.c
index 2fe195002021..f07d4a67fa76 100644
--- a/drivers/firmware/efi/libstub/efi-stub-helper.c
+++ b/drivers/firmware/efi/libstub/efi-stub-helper.c
@@ -179,12 +179,12 @@ again:
 		start = desc->phys_addr;
 		end = start + desc->num_pages * (1UL << EFI_PAGE_SHIFT);
 
-		if ((start + size) > end || (start + size) > max)
-			continue;
-
-		if (end - size > max)
+		if (end > max)
 			end = max;
 
+		if ((start + size) > end)
+			continue;
+
 		if (round_down(end - size, align) < start)
 			continue;
 
diff --git a/drivers/firmware/efi/libstub/efistub.h b/drivers/firmware/efi/libstub/efistub.h
index 47437b16b186..e334a01cf92f 100644
--- a/drivers/firmware/efi/libstub/efistub.h
+++ b/drivers/firmware/efi/libstub/efistub.h
@@ -41,7 +41,7 @@ efi_status_t allocate_new_fdt_and_exit_boot(efi_system_table_t *sys_table,
 					    unsigned long fdt_addr,
 					    unsigned long fdt_size);
 
-void *get_fdt(efi_system_table_t *sys_table);
+void *get_fdt(efi_system_table_t *sys_table, unsigned long *fdt_size);
 
 void efi_get_virtmap(efi_memory_desc_t *memory_map, unsigned long map_size,
 		     unsigned long desc_size, efi_memory_desc_t *runtime_map,
diff --git a/drivers/firmware/efi/libstub/fdt.c b/drivers/firmware/efi/libstub/fdt.c
index 91da56c4fd54..ef5d764e2a27 100644
--- a/drivers/firmware/efi/libstub/fdt.c
+++ b/drivers/firmware/efi/libstub/fdt.c
@@ -323,7 +323,7 @@ fail:
 	return EFI_LOAD_ERROR;
 }
 
-void *get_fdt(efi_system_table_t *sys_table)
+void *get_fdt(efi_system_table_t *sys_table, unsigned long *fdt_size)
 {
 	efi_guid_t fdt_guid = DEVICE_TREE_GUID;
 	efi_config_table_t *tables;
@@ -336,6 +336,11 @@ void *get_fdt(efi_system_table_t *sys_table)
 	for (i = 0; i < sys_table->nr_tables; i++)
 		if (efi_guidcmp(tables[i].guid, fdt_guid) == 0) {
 			fdt = (void *) tables[i].table;
+			if (fdt_check_header(fdt) != 0) {
+				pr_efi_err(sys_table, "Invalid header detected on UEFI supplied FDT, ignoring ...\n");
+				return NULL;
+			}
+			*fdt_size = fdt_totalsize(fdt);
 			break;
 	 }
 
diff --git a/drivers/gpio/gpio-mpc8xxx.c b/drivers/gpio/gpio-mpc8xxx.c
index a6952ba343a8..a65b75161aa4 100644
--- a/drivers/gpio/gpio-mpc8xxx.c
+++ b/drivers/gpio/gpio-mpc8xxx.c
@@ -334,7 +334,7 @@ static struct irq_domain_ops mpc8xxx_gpio_irq_ops = {
 	.xlate	= irq_domain_xlate_twocell,
 };
 
-static struct of_device_id mpc8xxx_gpio_ids[] __initdata = {
+static struct of_device_id mpc8xxx_gpio_ids[] = {
 	{ .compatible = "fsl,mpc8349-gpio", },
 	{ .compatible = "fsl,mpc8572-gpio", },
 	{ .compatible = "fsl,mpc8610-gpio", },
diff --git a/drivers/gpio/gpio-syscon.c b/drivers/gpio/gpio-syscon.c
index 257e2989215c..045a952576c7 100644
--- a/drivers/gpio/gpio-syscon.c
+++ b/drivers/gpio/gpio-syscon.c
@@ -219,7 +219,7 @@ static int syscon_gpio_probe(struct platform_device *pdev)
 		ret = of_property_read_u32_index(np, "gpio,syscon-dev", 2,
 						 &priv->dir_reg_offset);
 		if (ret)
-			dev_err(dev, "can't read the dir register offset!\n");
+			dev_dbg(dev, "can't read the dir register offset!\n");
 
 		priv->dir_reg_offset <<= 3;
 	}
diff --git a/drivers/gpio/gpio-tps65912.c b/drivers/gpio/gpio-tps65912.c
index 472fb5b8779f..9cdbc0c9cb2d 100644
--- a/drivers/gpio/gpio-tps65912.c
+++ b/drivers/gpio/gpio-tps65912.c
@@ -26,9 +26,12 @@ struct tps65912_gpio_data {
 	struct gpio_chip gpio_chip;
 };
 
+#define to_tgd(gc) container_of(gc, struct tps65912_gpio_data, gpio_chip)
+
 static int tps65912_gpio_get(struct gpio_chip *gc, unsigned offset)
 {
-	struct tps65912 *tps65912 = container_of(gc, struct tps65912, gpio);
+	struct tps65912_gpio_data *tps65912_gpio = to_tgd(gc);
+	struct tps65912 *tps65912 = tps65912_gpio->tps65912;
 	int val;
 
 	val = tps65912_reg_read(tps65912, TPS65912_GPIO1 + offset);
@@ -42,7 +45,8 @@ static int tps65912_gpio_get(struct gpio_chip *gc, unsigned offset)
 static void tps65912_gpio_set(struct gpio_chip *gc, unsigned offset,
 			      int value)
 {
-	struct tps65912 *tps65912 = container_of(gc, struct tps65912, gpio);
+	struct tps65912_gpio_data *tps65912_gpio = to_tgd(gc);
+	struct tps65912 *tps65912 = tps65912_gpio->tps65912;
 
 	if (value)
 		tps65912_set_bits(tps65912, TPS65912_GPIO1 + offset,
@@ -55,7 +59,8 @@ static void tps65912_gpio_set(struct gpio_chip *gc, unsigned offset,
 static int tps65912_gpio_output(struct gpio_chip *gc, unsigned offset,
 				int value)
 {
-	struct tps65912 *tps65912 = container_of(gc, struct tps65912, gpio);
+	struct tps65912_gpio_data *tps65912_gpio = to_tgd(gc);
+	struct tps65912 *tps65912 = tps65912_gpio->tps65912;
 
 	/* Set the initial value */
 	tps65912_gpio_set(gc, offset, value);
@@ -66,7 +71,8 @@ static int tps65912_gpio_output(struct gpio_chip *gc, unsigned offset,
 
 static int tps65912_gpio_input(struct gpio_chip *gc, unsigned offset)
 {
-	struct tps65912 *tps65912 = container_of(gc, struct tps65912, gpio);
+	struct tps65912_gpio_data *tps65912_gpio = to_tgd(gc);
+	struct tps65912 *tps65912 = tps65912_gpio->tps65912;
 
 	return tps65912_clear_bits(tps65912, TPS65912_GPIO1 + offset,
 								GPIO_CFG_MASK);
diff --git a/drivers/gpio/gpiolib-acpi.c b/drivers/gpio/gpiolib-acpi.c
index c0929d938ced..df990f29757a 100644
--- a/drivers/gpio/gpiolib-acpi.c
+++ b/drivers/gpio/gpiolib-acpi.c
@@ -201,6 +201,10 @@ static acpi_status acpi_gpiochip_request_interrupt(struct acpi_resource *ares,
 	if (!handler)
 		return AE_BAD_PARAMETER;
 
+	pin = acpi_gpiochip_pin_to_gpio_offset(chip, pin);
+	if (pin < 0)
+		return AE_BAD_PARAMETER;
+
 	desc = gpiochip_request_own_desc(chip, pin, "ACPI:Event");
 	if (IS_ERR(desc)) {
 		dev_err(chip->dev, "Failed to request GPIO\n");
@@ -551,6 +555,12 @@ acpi_gpio_adr_space_handler(u32 function, acpi_physical_address address,
 		struct gpio_desc *desc;
 		bool found;
 
+		pin = acpi_gpiochip_pin_to_gpio_offset(chip, pin);
+		if (pin < 0) {
+			status = AE_BAD_PARAMETER;
+			goto out;
+		}
+
 		mutex_lock(&achip->conn_lock);
 
 		found = false;
diff --git a/drivers/gpio/gpiolib-of.c b/drivers/gpio/gpiolib-of.c
index 8cad8e400b44..4650bf830d6b 100644
--- a/drivers/gpio/gpiolib-of.c
+++ b/drivers/gpio/gpiolib-of.c
@@ -46,12 +46,13 @@ static int of_gpiochip_find_and_xlate(struct gpio_chip *gc, void *data)
 
 	ret = gc->of_xlate(gc, &gg_data->gpiospec, gg_data->flags);
 	if (ret < 0) {
-		/* We've found the gpio chip, but the translation failed.
-		 * Return true to stop looking and return the translation
-		 * error via out_gpio
+		/* We've found a gpio chip, but the translation failed.
+		 * Store translation error in out_gpio.
+		 * Return false to keep looking, as more than one gpio chip
+		 * could be registered per of-node.
 		 */
 		gg_data->out_gpio = ERR_PTR(ret);
-		return true;
+		return false;
 	 }
 
 	gg_data->out_gpio = gpiochip_get_desc(gc, ret);
diff --git a/drivers/gpu/drm/amd/amdkfd/kfd_device_queue_manager.c b/drivers/gpu/drm/amd/amdkfd/kfd_device_queue_manager.c
index b3589d0e39b9..d8135adb2238 100644
--- a/drivers/gpu/drm/amd/amdkfd/kfd_device_queue_manager.c
+++ b/drivers/gpu/drm/amd/amdkfd/kfd_device_queue_manager.c
@@ -62,12 +62,18 @@ enum KFD_MQD_TYPE get_mqd_type_from_queue_type(enum kfd_queue_type type)
 	return KFD_MQD_TYPE_CP;
 }
 
-static inline unsigned int get_first_pipe(struct device_queue_manager *dqm)
+unsigned int get_first_pipe(struct device_queue_manager *dqm)
 {
-	BUG_ON(!dqm);
+	BUG_ON(!dqm || !dqm->dev);
 	return dqm->dev->shared_resources.first_compute_pipe;
 }
 
+unsigned int get_pipes_num(struct device_queue_manager *dqm)
+{
+	BUG_ON(!dqm || !dqm->dev);
+	return dqm->dev->shared_resources.compute_pipe_count;
+}
+
 static inline unsigned int get_pipes_num_cpsch(void)
 {
 	return PIPE_PER_ME_CP_SCHEDULING;
@@ -639,6 +645,7 @@ static int create_sdma_queue_nocpsch(struct device_queue_manager *dqm,
 	pr_debug("     sdma queue id: %d\n", q->properties.sdma_queue_id);
 	pr_debug("     sdma engine id: %d\n", q->properties.sdma_engine_id);
 
+	init_sdma_vm(dqm, q, qpd);
 	retval = mqd->init_mqd(mqd, &q->mqd, &q->mqd_mem_obj,
 				&q->gart_mqd_addr, &q->properties);
 	if (retval != 0) {
@@ -646,7 +653,14 @@ static int create_sdma_queue_nocpsch(struct device_queue_manager *dqm,
 		return retval;
 	}
 
-	init_sdma_vm(dqm, q, qpd);
+	retval = mqd->load_mqd(mqd, q->mqd, 0,
+				0, NULL);
+	if (retval != 0) {
+		deallocate_sdma_queue(dqm, q->sdma_id);
+		mqd->uninit_mqd(mqd, q->mqd, q->mqd_mem_obj);
+		return retval;
+	}
+
 	return 0;
 }
 
diff --git a/drivers/gpu/drm/amd/amdkfd/kfd_device_queue_manager.h b/drivers/gpu/drm/amd/amdkfd/kfd_device_queue_manager.h
index d64f86cda34f..488f51d19427 100644
--- a/drivers/gpu/drm/amd/amdkfd/kfd_device_queue_manager.h
+++ b/drivers/gpu/drm/amd/amdkfd/kfd_device_queue_manager.h
@@ -163,6 +163,8 @@ void program_sh_mem_settings(struct device_queue_manager *dqm,
 					struct qcm_process_device *qpd);
 int init_pipelines(struct device_queue_manager *dqm,
 		unsigned int pipes_num, unsigned int first_pipe);
+unsigned int get_first_pipe(struct device_queue_manager *dqm);
+unsigned int get_pipes_num(struct device_queue_manager *dqm);
 
 extern inline unsigned int get_sh_mem_bases_32(struct kfd_process_device *pdd)
 {
@@ -175,10 +177,4 @@ get_sh_mem_bases_nybble_64(struct kfd_process_device *pdd)
 	return (pdd->lds_base >> 60) & 0x0E;
 }
 
-extern inline unsigned int get_pipes_num(struct device_queue_manager *dqm)
-{
-	BUG_ON(!dqm || !dqm->dev);
-	return dqm->dev->shared_resources.compute_pipe_count;
-}
-
 #endif /* KFD_DEVICE_QUEUE_MANAGER_H_ */
diff --git a/drivers/gpu/drm/amd/amdkfd/kfd_device_queue_manager_cik.c b/drivers/gpu/drm/amd/amdkfd/kfd_device_queue_manager_cik.c
index 6b072466e2a6..5469efe0523e 100644
--- a/drivers/gpu/drm/amd/amdkfd/kfd_device_queue_manager_cik.c
+++ b/drivers/gpu/drm/amd/amdkfd/kfd_device_queue_manager_cik.c
@@ -131,5 +131,5 @@ static int register_process_cik(struct device_queue_manager *dqm,
 
 static int initialize_cpsch_cik(struct device_queue_manager *dqm)
 {
-	return init_pipelines(dqm, get_pipes_num(dqm), 0);
+	return init_pipelines(dqm, get_pipes_num(dqm), get_first_pipe(dqm));
 }
diff --git a/drivers/gpu/drm/amd/amdkfd/kfd_kernel_queue.c b/drivers/gpu/drm/amd/amdkfd/kfd_kernel_queue.c
index e415a2a9207e..c7d298e62c96 100644
--- a/drivers/gpu/drm/amd/amdkfd/kfd_kernel_queue.c
+++ b/drivers/gpu/drm/amd/amdkfd/kfd_kernel_queue.c
@@ -44,7 +44,7 @@ static bool initialize(struct kernel_queue *kq, struct kfd_dev *dev,
 	BUG_ON(!kq || !dev);
 	BUG_ON(type != KFD_QUEUE_TYPE_DIQ && type != KFD_QUEUE_TYPE_HIQ);
 
-	pr_debug("kfd: In func %s initializing queue type %d size %d\n",
+	pr_debug("amdkfd: In func %s initializing queue type %d size %d\n",
 			__func__, KFD_QUEUE_TYPE_HIQ, queue_size);
 
 	nop.opcode = IT_NOP;
@@ -69,12 +69,16 @@ static bool initialize(struct kernel_queue *kq, struct kfd_dev *dev,
 
 	prop.doorbell_ptr = kfd_get_kernel_doorbell(dev, &prop.doorbell_off);
 
-	if (prop.doorbell_ptr == NULL)
+	if (prop.doorbell_ptr == NULL) {
+		pr_err("amdkfd: error init doorbell");
 		goto err_get_kernel_doorbell;
+	}
 
 	retval = kfd_gtt_sa_allocate(dev, queue_size, &kq->pq);
-	if (retval != 0)
+	if (retval != 0) {
+		pr_err("amdkfd: error init pq queues size (%d)\n", queue_size);
 		goto err_pq_allocate_vidmem;
+	}
 
 	kq->pq_kernel_addr = kq->pq->cpu_ptr;
 	kq->pq_gpu_addr = kq->pq->gpu_addr;
@@ -165,10 +169,8 @@ err_rptr_allocate_vidmem:
 err_eop_allocate_vidmem:
 	kfd_gtt_sa_free(dev, kq->pq);
 err_pq_allocate_vidmem:
-	pr_err("kfd: error init pq\n");
 	kfd_release_kernel_doorbell(dev, prop.doorbell_ptr);
 err_get_kernel_doorbell:
-	pr_err("kfd: error init doorbell");
 	return false;
 
 }
@@ -187,6 +189,8 @@ static void uninitialize(struct kernel_queue *kq)
 	else if (kq->queue->properties.type == KFD_QUEUE_TYPE_DIQ)
 		kfd_gtt_sa_free(kq->dev, kq->fence_mem_obj);
 
+	kq->mqd->uninit_mqd(kq->mqd, kq->queue->mqd, kq->queue->mqd_mem_obj);
+
 	kfd_gtt_sa_free(kq->dev, kq->rptr_mem);
 	kfd_gtt_sa_free(kq->dev, kq->wptr_mem);
 	kq->ops_asic_specific.uninitialize(kq);
@@ -211,7 +215,7 @@ static int acquire_packet_buffer(struct kernel_queue *kq,
 	queue_address = (unsigned int *)kq->pq_kernel_addr;
 	queue_size_dwords = kq->queue->properties.queue_size / sizeof(uint32_t);
 
-	pr_debug("kfd: In func %s\nrptr: %d\nwptr: %d\nqueue_address 0x%p\n",
+	pr_debug("amdkfd: In func %s\nrptr: %d\nwptr: %d\nqueue_address 0x%p\n",
 			__func__, rptr, wptr, queue_address);
 
 	available_size = (rptr - 1 - wptr + queue_size_dwords) %
@@ -296,7 +300,7 @@ struct kernel_queue *kernel_queue_init(struct kfd_dev *dev,
 	}
 
 	if (kq->ops.initialize(kq, dev, type, KFD_KERNEL_QUEUE_SIZE) == false) {
-		pr_err("kfd: failed to init kernel queue\n");
+		pr_err("amdkfd: failed to init kernel queue\n");
 		kfree(kq);
 		return NULL;
 	}
@@ -319,7 +323,7 @@ static __attribute__((unused)) void test_kq(struct kfd_dev *dev)
 
 	BUG_ON(!dev);
 
-	pr_err("kfd: starting kernel queue test\n");
+	pr_err("amdkfd: starting kernel queue test\n");
 
 	kq = kernel_queue_init(dev, KFD_QUEUE_TYPE_HIQ);
 	BUG_ON(!kq);
@@ -330,7 +334,7 @@ static __attribute__((unused)) void test_kq(struct kfd_dev *dev)
 		buffer[i] = kq->nop_packet;
 	kq->ops.submit_packet(kq);
 
-	pr_err("kfd: ending kernel queue test\n");
+	pr_err("amdkfd: ending kernel queue test\n");
 }
 
 
diff --git a/drivers/gpu/drm/atmel-hlcdc/atmel_hlcdc_crtc.c b/drivers/gpu/drm/atmel-hlcdc/atmel_hlcdc_crtc.c
index 0409b907de5d..b3e3068c6ec0 100644
--- a/drivers/gpu/drm/atmel-hlcdc/atmel_hlcdc_crtc.c
+++ b/drivers/gpu/drm/atmel-hlcdc/atmel_hlcdc_crtc.c
@@ -153,7 +153,7 @@ static int atmel_hlcdc_crtc_mode_set(struct drm_crtc *c,
 		     (adj->crtc_hdisplay - 1) |
 		     ((adj->crtc_vdisplay - 1) << 16));
 
-	cfg = ATMEL_HLCDC_CLKPOL;
+	cfg = 0;
 
 	prate = clk_get_rate(crtc->dc->hlcdc->sys_clk);
 	mode_rate = mode->crtc_clock * 1000;
diff --git a/drivers/gpu/drm/atmel-hlcdc/atmel_hlcdc_dc.c b/drivers/gpu/drm/atmel-hlcdc/atmel_hlcdc_dc.c
index 7320a6c6613f..c1cb17493e0d 100644
--- a/drivers/gpu/drm/atmel-hlcdc/atmel_hlcdc_dc.c
+++ b/drivers/gpu/drm/atmel-hlcdc/atmel_hlcdc_dc.c
@@ -311,8 +311,6 @@ static int atmel_hlcdc_dc_load(struct drm_device *dev)
 
 	pm_runtime_enable(dev->dev);
 
-	pm_runtime_put_sync(dev->dev);
-
 	ret = atmel_hlcdc_dc_modeset_init(dev);
 	if (ret < 0) {
 		dev_err(dev->dev, "failed to initialize mode setting\n");
diff --git a/drivers/gpu/drm/atmel-hlcdc/atmel_hlcdc_layer.c b/drivers/gpu/drm/atmel-hlcdc/atmel_hlcdc_layer.c
index 063d2a7b941f..e79bd9ba474b 100644
--- a/drivers/gpu/drm/atmel-hlcdc/atmel_hlcdc_layer.c
+++ b/drivers/gpu/drm/atmel-hlcdc/atmel_hlcdc_layer.c
@@ -311,7 +311,8 @@ int atmel_hlcdc_layer_disable(struct atmel_hlcdc_layer *layer)
 
 	/* Disable the layer */
 	regmap_write(regmap, desc->regs_offset + ATMEL_HLCDC_LAYER_CHDR,
-		     ATMEL_HLCDC_LAYER_RST);
+		     ATMEL_HLCDC_LAYER_RST | ATMEL_HLCDC_LAYER_A2Q |
+		     ATMEL_HLCDC_LAYER_UPDATE);
 
 	/* Clear all pending interrupts */
 	regmap_read(regmap, desc->regs_offset + ATMEL_HLCDC_LAYER_ISR, &isr);
diff --git a/drivers/gpu/drm/drm_crtc.c b/drivers/gpu/drm/drm_crtc.c
index 6b00173d1be4..b6f076b213bc 100644
--- a/drivers/gpu/drm/drm_crtc.c
+++ b/drivers/gpu/drm/drm_crtc.c
@@ -43,9 +43,10 @@
 #include "drm_crtc_internal.h"
 #include "drm_internal.h"
 
-static struct drm_framebuffer *add_framebuffer_internal(struct drm_device *dev,
-							struct drm_mode_fb_cmd2 *r,
-							struct drm_file *file_priv);
+static struct drm_framebuffer *
+internal_framebuffer_create(struct drm_device *dev,
+			    struct drm_mode_fb_cmd2 *r,
+			    struct drm_file *file_priv);
 
 /* Avoid boilerplate.  I'm tired of typing. */
 #define DRM_ENUM_NAME_FN(fnname, list)				\
@@ -524,17 +525,6 @@ void drm_framebuffer_reference(struct drm_framebuffer *fb)
 }
 EXPORT_SYMBOL(drm_framebuffer_reference);
 
-static void drm_framebuffer_free_bug(struct kref *kref)
-{
-	BUG();
-}
-
-static void __drm_framebuffer_unreference(struct drm_framebuffer *fb)
-{
-	DRM_DEBUG("%p: FB ID: %d (%d)\n", fb, fb->base.id, atomic_read(&fb->refcount.refcount));
-	kref_put(&fb->refcount, drm_framebuffer_free_bug);
-}
-
 /**
  * drm_framebuffer_unregister_private - unregister a private fb from the lookup idr
  * @fb: fb to unregister
@@ -1319,7 +1309,7 @@ void drm_plane_force_disable(struct drm_plane *plane)
 		return;
 	}
 	/* disconnect the plane from the fb and crtc: */
-	__drm_framebuffer_unreference(plane->old_fb);
+	drm_framebuffer_unreference(plane->old_fb);
 	plane->old_fb = NULL;
 	plane->fb = NULL;
 	plane->crtc = NULL;
@@ -2127,12 +2117,11 @@ int drm_mode_getconnector(struct drm_device *dev, void *data,
 	DRM_DEBUG_KMS("[CONNECTOR:%d:?]\n", out_resp->connector_id);
 
 	mutex_lock(&dev->mode_config.mutex);
-	drm_modeset_lock(&dev->mode_config.connection_mutex, NULL);
 
 	connector = drm_connector_find(dev, out_resp->connector_id);
 	if (!connector) {
 		ret = -ENOENT;
-		goto out;
+		goto out_unlock;
 	}
 
 	for (i = 0; i < DRM_CONNECTOR_MAX_ENCODER; i++)
@@ -2157,6 +2146,8 @@ int drm_mode_getconnector(struct drm_device *dev, void *data,
 	out_resp->mm_height = connector->display_info.height_mm;
 	out_resp->subpixel = connector->display_info.subpixel_order;
 	out_resp->connection = connector->status;
+
+	drm_modeset_lock(&dev->mode_config.connection_mutex, NULL);
 	encoder = drm_connector_get_encoder(connector);
 	if (encoder)
 		out_resp->encoder_id = encoder->base.id;
@@ -2210,6 +2201,8 @@ int drm_mode_getconnector(struct drm_device *dev, void *data,
 
 out:
 	drm_modeset_unlock(&dev->mode_config.connection_mutex);
+
+out_unlock:
 	mutex_unlock(&dev->mode_config.mutex);
 
 	return ret;
@@ -2907,13 +2900,11 @@ static int drm_mode_cursor_universal(struct drm_crtc *crtc,
 	 */
 	if (req->flags & DRM_MODE_CURSOR_BO) {
 		if (req->handle) {
-			fb = add_framebuffer_internal(dev, &fbreq, file_priv);
+			fb = internal_framebuffer_create(dev, &fbreq, file_priv);
 			if (IS_ERR(fb)) {
 				DRM_DEBUG_KMS("failed to wrap cursor buffer in drm framebuffer\n");
 				return PTR_ERR(fb);
 			}
-
-			drm_framebuffer_reference(fb);
 		} else {
 			fb = NULL;
 		}
@@ -3266,9 +3257,10 @@ static int framebuffer_check(const struct drm_mode_fb_cmd2 *r)
 	return 0;
 }
 
-static struct drm_framebuffer *add_framebuffer_internal(struct drm_device *dev,
-							struct drm_mode_fb_cmd2 *r,
-							struct drm_file *file_priv)
+static struct drm_framebuffer *
+internal_framebuffer_create(struct drm_device *dev,
+			    struct drm_mode_fb_cmd2 *r,
+			    struct drm_file *file_priv)
 {
 	struct drm_mode_config *config = &dev->mode_config;
 	struct drm_framebuffer *fb;
@@ -3300,12 +3292,6 @@ static struct drm_framebuffer *add_framebuffer_internal(struct drm_device *dev,
 		return fb;
 	}
 
-	mutex_lock(&file_priv->fbs_lock);
-	r->fb_id = fb->base.id;
-	list_add(&fb->filp_head, &file_priv->fbs);
-	DRM_DEBUG_KMS("[FB:%d]\n", fb->base.id);
-	mutex_unlock(&file_priv->fbs_lock);
-
 	return fb;
 }
 
@@ -3327,15 +3313,24 @@ static struct drm_framebuffer *add_framebuffer_internal(struct drm_device *dev,
 int drm_mode_addfb2(struct drm_device *dev,
 		    void *data, struct drm_file *file_priv)
 {
+	struct drm_mode_fb_cmd2 *r = data;
 	struct drm_framebuffer *fb;
 
 	if (!drm_core_check_feature(dev, DRIVER_MODESET))
 		return -EINVAL;
 
-	fb = add_framebuffer_internal(dev, data, file_priv);
+	fb = internal_framebuffer_create(dev, r, file_priv);
 	if (IS_ERR(fb))
 		return PTR_ERR(fb);
 
+	/* Transfer ownership to the filp for reaping on close */
+
+	DRM_DEBUG_KMS("[FB:%d]\n", fb->base.id);
+	mutex_lock(&file_priv->fbs_lock);
+	r->fb_id = fb->base.id;
+	list_add(&fb->filp_head, &file_priv->fbs);
+	mutex_unlock(&file_priv->fbs_lock);
+
 	return 0;
 }
 
diff --git a/drivers/gpu/drm/drm_dp_mst_topology.c b/drivers/gpu/drm/drm_dp_mst_topology.c
index 9a5b68717ec8..379ab4555756 100644
--- a/drivers/gpu/drm/drm_dp_mst_topology.c
+++ b/drivers/gpu/drm/drm_dp_mst_topology.c
@@ -733,10 +733,14 @@ static bool check_txmsg_state(struct drm_dp_mst_topology_mgr *mgr,
 			      struct drm_dp_sideband_msg_tx *txmsg)
 {
 	bool ret;
-	mutex_lock(&mgr->qlock);
+
+	/*
+	 * All updates to txmsg->state are protected by mgr->qlock, and the two
+	 * cases we check here are terminal states. For those the barriers
+	 * provided by the wake_up/wait_event pair are enough.
+	 */
 	ret = (txmsg->state == DRM_DP_SIDEBAND_TX_RX ||
 	       txmsg->state == DRM_DP_SIDEBAND_TX_TIMEOUT);
-	mutex_unlock(&mgr->qlock);
 	return ret;
 }
 
@@ -1363,12 +1367,13 @@ static int process_single_tx_qlock(struct drm_dp_mst_topology_mgr *mgr,
 	return 0;
 }
 
-/* must be called holding qlock */
 static void process_single_down_tx_qlock(struct drm_dp_mst_topology_mgr *mgr)
 {
 	struct drm_dp_sideband_msg_tx *txmsg;
 	int ret;
 
+	WARN_ON(!mutex_is_locked(&mgr->qlock));
+
 	/* construct a chunk from the first msg in the tx_msg queue */
 	if (list_empty(&mgr->tx_msg_downq)) {
 		mgr->tx_down_in_progress = false;
diff --git a/drivers/gpu/drm/drm_edid_load.c b/drivers/gpu/drm/drm_edid_load.c
index 732cb6f8e653..4c0aa97aaf03 100644
--- a/drivers/gpu/drm/drm_edid_load.c
+++ b/drivers/gpu/drm/drm_edid_load.c
@@ -287,6 +287,7 @@ int drm_load_edid_firmware(struct drm_connector *connector)
 
 	drm_mode_connector_update_edid_property(connector, edid);
 	ret = drm_add_edid_modes(connector, edid);
+	drm_edid_to_eld(connector, edid);
 	kfree(edid);
 
 	return ret;
diff --git a/drivers/gpu/drm/drm_mm.c b/drivers/gpu/drm/drm_mm.c
index 04a209e2b66d..1134526286c8 100644
--- a/drivers/gpu/drm/drm_mm.c
+++ b/drivers/gpu/drm/drm_mm.c
@@ -91,29 +91,29 @@
  */
 
 static struct drm_mm_node *drm_mm_search_free_generic(const struct drm_mm *mm,
-						unsigned long size,
+						u64 size,
 						unsigned alignment,
 						unsigned long color,
 						enum drm_mm_search_flags flags);
 static struct drm_mm_node *drm_mm_search_free_in_range_generic(const struct drm_mm *mm,
-						unsigned long size,
+						u64 size,
 						unsigned alignment,
 						unsigned long color,
-						unsigned long start,
-						unsigned long end,
+						u64 start,
+						u64 end,
 						enum drm_mm_search_flags flags);
 
 static void drm_mm_insert_helper(struct drm_mm_node *hole_node,
 				 struct drm_mm_node *node,
-				 unsigned long size, unsigned alignment,
+				 u64 size, unsigned alignment,
 				 unsigned long color,
 				 enum drm_mm_allocator_flags flags)
 {
 	struct drm_mm *mm = hole_node->mm;
-	unsigned long hole_start = drm_mm_hole_node_start(hole_node);
-	unsigned long hole_end = drm_mm_hole_node_end(hole_node);
-	unsigned long adj_start = hole_start;
-	unsigned long adj_end = hole_end;
+	u64 hole_start = drm_mm_hole_node_start(hole_node);
+	u64 hole_end = drm_mm_hole_node_end(hole_node);
+	u64 adj_start = hole_start;
+	u64 adj_end = hole_end;
 
 	BUG_ON(node->allocated);
 
@@ -124,12 +124,15 @@ static void drm_mm_insert_helper(struct drm_mm_node *hole_node,
 		adj_start = adj_end - size;
 
 	if (alignment) {
-		unsigned tmp = adj_start % alignment;
-		if (tmp) {
+		u64 tmp = adj_start;
+		unsigned rem;
+
+		rem = do_div(tmp, alignment);
+		if (rem) {
 			if (flags & DRM_MM_CREATE_TOP)
-				adj_start -= tmp;
+				adj_start -= rem;
 			else
-				adj_start += alignment - tmp;
+				adj_start += alignment - rem;
 		}
 	}
 
@@ -176,9 +179,9 @@ static void drm_mm_insert_helper(struct drm_mm_node *hole_node,
 int drm_mm_reserve_node(struct drm_mm *mm, struct drm_mm_node *node)
 {
 	struct drm_mm_node *hole;
-	unsigned long end = node->start + node->size;
-	unsigned long hole_start;
-	unsigned long hole_end;
+	u64 end = node->start + node->size;
+	u64 hole_start;
+	u64 hole_end;
 
 	BUG_ON(node == NULL);
 
@@ -227,7 +230,7 @@ EXPORT_SYMBOL(drm_mm_reserve_node);
  * 0 on success, -ENOSPC if there's no suitable hole.
  */
 int drm_mm_insert_node_generic(struct drm_mm *mm, struct drm_mm_node *node,
-			       unsigned long size, unsigned alignment,
+			       u64 size, unsigned alignment,
 			       unsigned long color,
 			       enum drm_mm_search_flags sflags,
 			       enum drm_mm_allocator_flags aflags)
@@ -246,16 +249,16 @@ EXPORT_SYMBOL(drm_mm_insert_node_generic);
 
 static void drm_mm_insert_helper_range(struct drm_mm_node *hole_node,
 				       struct drm_mm_node *node,
-				       unsigned long size, unsigned alignment,
+				       u64 size, unsigned alignment,
 				       unsigned long color,
-				       unsigned long start, unsigned long end,
+				       u64 start, u64 end,
 				       enum drm_mm_allocator_flags flags)
 {
 	struct drm_mm *mm = hole_node->mm;
-	unsigned long hole_start = drm_mm_hole_node_start(hole_node);
-	unsigned long hole_end = drm_mm_hole_node_end(hole_node);
-	unsigned long adj_start = hole_start;
-	unsigned long adj_end = hole_end;
+	u64 hole_start = drm_mm_hole_node_start(hole_node);
+	u64 hole_end = drm_mm_hole_node_end(hole_node);
+	u64 adj_start = hole_start;
+	u64 adj_end = hole_end;
 
 	BUG_ON(!hole_node->hole_follows || node->allocated);
 
@@ -271,12 +274,15 @@ static void drm_mm_insert_helper_range(struct drm_mm_node *hole_node,
 		mm->color_adjust(hole_node, color, &adj_start, &adj_end);
 
 	if (alignment) {
-		unsigned tmp = adj_start % alignment;
-		if (tmp) {
+		u64 tmp = adj_start;
+		unsigned rem;
+
+		rem = do_div(tmp, alignment);
+		if (rem) {
 			if (flags & DRM_MM_CREATE_TOP)
-				adj_start -= tmp;
+				adj_start -= rem;
 			else
-				adj_start += alignment - tmp;
+				adj_start += alignment - rem;
 		}
 	}
 
@@ -324,9 +330,9 @@ static void drm_mm_insert_helper_range(struct drm_mm_node *hole_node,
  * 0 on success, -ENOSPC if there's no suitable hole.
  */
 int drm_mm_insert_node_in_range_generic(struct drm_mm *mm, struct drm_mm_node *node,
-					unsigned long size, unsigned alignment,
+					u64 size, unsigned alignment,
 					unsigned long color,
-					unsigned long start, unsigned long end,
+					u64 start, u64 end,
 					enum drm_mm_search_flags sflags,
 					enum drm_mm_allocator_flags aflags)
 {
@@ -387,32 +393,34 @@ void drm_mm_remove_node(struct drm_mm_node *node)
 }
 EXPORT_SYMBOL(drm_mm_remove_node);
 
-static int check_free_hole(unsigned long start, unsigned long end,
-			   unsigned long size, unsigned alignment)
+static int check_free_hole(u64 start, u64 end, u64 size, unsigned alignment)
 {
 	if (end - start < size)
 		return 0;
 
 	if (alignment) {
-		unsigned tmp = start % alignment;
-		if (tmp)
-			start += alignment - tmp;
+		u64 tmp = start;
+		unsigned rem;
+
+		rem = do_div(tmp, alignment);
+		if (rem)
+			start += alignment - rem;
 	}
 
 	return end >= start + size;
 }
 
 static struct drm_mm_node *drm_mm_search_free_generic(const struct drm_mm *mm,
-						      unsigned long size,
+						      u64 size,
 						      unsigned alignment,
 						      unsigned long color,
 						      enum drm_mm_search_flags flags)
 {
 	struct drm_mm_node *entry;
 	struct drm_mm_node *best;
-	unsigned long adj_start;
-	unsigned long adj_end;
-	unsigned long best_size;
+	u64 adj_start;
+	u64 adj_end;
+	u64 best_size;
 
 	BUG_ON(mm->scanned_blocks);
 
@@ -421,7 +429,7 @@ static struct drm_mm_node *drm_mm_search_free_generic(const struct drm_mm *mm,
 
 	__drm_mm_for_each_hole(entry, mm, adj_start, adj_end,
 			       flags & DRM_MM_SEARCH_BELOW) {
-		unsigned long hole_size = adj_end - adj_start;
+		u64 hole_size = adj_end - adj_start;
 
 		if (mm->color_adjust) {
 			mm->color_adjust(entry, color, &adj_start, &adj_end);
@@ -445,18 +453,18 @@ static struct drm_mm_node *drm_mm_search_free_generic(const struct drm_mm *mm,
 }
 
 static struct drm_mm_node *drm_mm_search_free_in_range_generic(const struct drm_mm *mm,
-							unsigned long size,
+							u64 size,
 							unsigned alignment,
 							unsigned long color,
-							unsigned long start,
-							unsigned long end,
+							u64 start,
+							u64 end,
 							enum drm_mm_search_flags flags)
 {
 	struct drm_mm_node *entry;
 	struct drm_mm_node *best;
-	unsigned long adj_start;
-	unsigned long adj_end;
-	unsigned long best_size;
+	u64 adj_start;
+	u64 adj_end;
+	u64 best_size;
 
 	BUG_ON(mm->scanned_blocks);
 
@@ -465,7 +473,7 @@ static struct drm_mm_node *drm_mm_search_free_in_range_generic(const struct drm_
 
 	__drm_mm_for_each_hole(entry, mm, adj_start, adj_end,
 			       flags & DRM_MM_SEARCH_BELOW) {
-		unsigned long hole_size = adj_end - adj_start;
+		u64 hole_size = adj_end - adj_start;
 
 		if (adj_start < start)
 			adj_start = start;
@@ -561,7 +569,7 @@ EXPORT_SYMBOL(drm_mm_replace_node);
  * adding/removing nodes to/from the scan list are allowed.
  */
 void drm_mm_init_scan(struct drm_mm *mm,
-		      unsigned long size,
+		      u64 size,
 		      unsigned alignment,
 		      unsigned long color)
 {
@@ -594,11 +602,11 @@ EXPORT_SYMBOL(drm_mm_init_scan);
  * adding/removing nodes to/from the scan list are allowed.
  */
 void drm_mm_init_scan_with_range(struct drm_mm *mm,
-				 unsigned long size,
+				 u64 size,
 				 unsigned alignment,
 				 unsigned long color,
-				 unsigned long start,
-				 unsigned long end)
+				 u64 start,
+				 u64 end)
 {
 	mm->scan_color = color;
 	mm->scan_alignment = alignment;
@@ -627,8 +635,8 @@ bool drm_mm_scan_add_block(struct drm_mm_node *node)
 {
 	struct drm_mm *mm = node->mm;
 	struct drm_mm_node *prev_node;
-	unsigned long hole_start, hole_end;
-	unsigned long adj_start, adj_end;
+	u64 hole_start, hole_end;
+	u64 adj_start, adj_end;
 
 	mm->scanned_blocks++;
 
@@ -731,7 +739,7 @@ EXPORT_SYMBOL(drm_mm_clean);
  *
  * Note that @mm must be cleared to 0 before calling this function.
  */
-void drm_mm_init(struct drm_mm * mm, unsigned long start, unsigned long size)
+void drm_mm_init(struct drm_mm * mm, u64 start, u64 size)
 {
 	INIT_LIST_HEAD(&mm->hole_stack);
 	mm->scanned_blocks = 0;
@@ -766,18 +774,17 @@ void drm_mm_takedown(struct drm_mm * mm)
 }
 EXPORT_SYMBOL(drm_mm_takedown);
 
-static unsigned long drm_mm_debug_hole(struct drm_mm_node *entry,
-				       const char *prefix)
+static u64 drm_mm_debug_hole(struct drm_mm_node *entry,
+				     const char *prefix)
 {
-	unsigned long hole_start, hole_end, hole_size;
+	u64 hole_start, hole_end, hole_size;
 
 	if (entry->hole_follows) {
 		hole_start = drm_mm_hole_node_start(entry);
 		hole_end = drm_mm_hole_node_end(entry);
 		hole_size = hole_end - hole_start;
-		printk(KERN_DEBUG "%s 0x%08lx-0x%08lx: %8lu: free\n",
-			prefix, hole_start, hole_end,
-			hole_size);
+		pr_debug("%s %#llx-%#llx: %llu: free\n", prefix, hole_start,
+			 hole_end, hole_size);
 		return hole_size;
 	}
 
@@ -792,35 +799,34 @@ static unsigned long drm_mm_debug_hole(struct drm_mm_node *entry,
 void drm_mm_debug_table(struct drm_mm *mm, const char *prefix)
 {
 	struct drm_mm_node *entry;
-	unsigned long total_used = 0, total_free = 0, total = 0;
+	u64 total_used = 0, total_free = 0, total = 0;
 
 	total_free += drm_mm_debug_hole(&mm->head_node, prefix);
 
 	drm_mm_for_each_node(entry, mm) {
-		printk(KERN_DEBUG "%s 0x%08lx-0x%08lx: %8lu: used\n",
-			prefix, entry->start, entry->start + entry->size,
-			entry->size);
+		pr_debug("%s %#llx-%#llx: %llu: used\n", prefix, entry->start,
+			 entry->start + entry->size, entry->size);
 		total_used += entry->size;
 		total_free += drm_mm_debug_hole(entry, prefix);
 	}
 	total = total_free + total_used;
 
-	printk(KERN_DEBUG "%s total: %lu, used %lu free %lu\n", prefix, total,
-		total_used, total_free);
+	pr_debug("%s total: %llu, used %llu free %llu\n", prefix, total,
+		 total_used, total_free);
 }
 EXPORT_SYMBOL(drm_mm_debug_table);
 
 #if defined(CONFIG_DEBUG_FS)
-static unsigned long drm_mm_dump_hole(struct seq_file *m, struct drm_mm_node *entry)
+static u64 drm_mm_dump_hole(struct seq_file *m, struct drm_mm_node *entry)
 {
-	unsigned long hole_start, hole_end, hole_size;
+	u64 hole_start, hole_end, hole_size;
 
 	if (entry->hole_follows) {
 		hole_start = drm_mm_hole_node_start(entry);
 		hole_end = drm_mm_hole_node_end(entry);
 		hole_size = hole_end - hole_start;
-		seq_printf(m, "0x%08lx-0x%08lx: 0x%08lx: free\n",
-				hole_start, hole_end, hole_size);
+		seq_printf(m, "%#llx-%#llx: %llu: free\n", hole_start,
+			   hole_end, hole_size);
 		return hole_size;
 	}
 
@@ -835,20 +841,20 @@ static unsigned long drm_mm_dump_hole(struct seq_file *m, struct drm_mm_node *en
 int drm_mm_dump_table(struct seq_file *m, struct drm_mm *mm)
 {
 	struct drm_mm_node *entry;
-	unsigned long total_used = 0, total_free = 0, total = 0;
+	u64 total_used = 0, total_free = 0, total = 0;
 
 	total_free += drm_mm_dump_hole(m, &mm->head_node);
 
 	drm_mm_for_each_node(entry, mm) {
-		seq_printf(m, "0x%08lx-0x%08lx: 0x%08lx: used\n",
-				entry->start, entry->start + entry->size,
-				entry->size);
+		seq_printf(m, "%#016llx-%#016llx: %llu: used\n", entry->start,
+			   entry->start + entry->size, entry->size);
 		total_used += entry->size;
 		total_free += drm_mm_dump_hole(m, entry);
 	}
 	total = total_free + total_used;
 
-	seq_printf(m, "total: %lu, used %lu free %lu\n", total, total_used, total_free);
+	seq_printf(m, "total: %llu, used %llu free %llu\n", total,
+		   total_used, total_free);
 	return 0;
 }
 EXPORT_SYMBOL(drm_mm_dump_table);
diff --git a/drivers/gpu/drm/drm_probe_helper.c b/drivers/gpu/drm/drm_probe_helper.c
index 6591d48c1b9d..3fee587bc284 100644
--- a/drivers/gpu/drm/drm_probe_helper.c
+++ b/drivers/gpu/drm/drm_probe_helper.c
@@ -174,6 +174,7 @@ static int drm_helper_probe_single_connector_modes_merge_bits(struct drm_connect
 			struct edid *edid = (struct edid *) connector->edid_blob_ptr->data;
 
 			count = drm_add_edid_modes(connector, edid);
+			drm_edid_to_eld(connector, edid);
 		} else
 			count = (*connector_funcs->get_modes)(connector);
 	}
diff --git a/drivers/gpu/drm/exynos/Kconfig b/drivers/gpu/drm/exynos/Kconfig
index a5e74612100e..0a6780367d28 100644
--- a/drivers/gpu/drm/exynos/Kconfig
+++ b/drivers/gpu/drm/exynos/Kconfig
@@ -50,7 +50,7 @@ config DRM_EXYNOS_DSI
 
 config DRM_EXYNOS_DP
 	bool "EXYNOS DRM DP driver support"
-	depends on (DRM_EXYNOS_FIMD || DRM_EXYNOS7DECON) && ARCH_EXYNOS && (DRM_PTN3460=n || DRM_PTN3460=y || DRM_PTN3460=DRM_EXYNOS)
+	depends on (DRM_EXYNOS_FIMD || DRM_EXYNOS7_DECON) && ARCH_EXYNOS && (DRM_PTN3460=n || DRM_PTN3460=y || DRM_PTN3460=DRM_EXYNOS)
 	default DRM_EXYNOS
 	select DRM_PANEL
 	help
diff --git a/drivers/gpu/drm/exynos/exynos7_drm_decon.c b/drivers/gpu/drm/exynos/exynos7_drm_decon.c
index 63f02e2380ae..970046199608 100644
--- a/drivers/gpu/drm/exynos/exynos7_drm_decon.c
+++ b/drivers/gpu/drm/exynos/exynos7_drm_decon.c
@@ -888,8 +888,8 @@ static int decon_probe(struct platform_device *pdev)
 	of_node_put(i80_if_timings);
 
 	ctx->regs = of_iomap(dev->of_node, 0);
-	if (IS_ERR(ctx->regs)) {
-		ret = PTR_ERR(ctx->regs);
+	if (!ctx->regs) {
+		ret = -ENOMEM;
 		goto err_del_component;
 	}
 
diff --git a/drivers/gpu/drm/exynos/exynos_drm_connector.c b/drivers/gpu/drm/exynos/exynos_drm_connector.c
deleted file mode 100644
index ba9b3d5ed672..000000000000
--- a/drivers/gpu/drm/exynos/exynos_drm_connector.c
+++ /dev/null
@@ -1,245 +0,0 @@
-/*
- * Copyright (c) 2011 Samsung Electronics Co., Ltd.
- * Authors:
- *	Inki Dae <inki.dae@samsung.com>
- *	Joonyoung Shim <jy0922.shim@samsung.com>
- *	Seung-Woo Kim <sw0312.kim@samsung.com>
- *
- * This program is free software; you can redistribute  it and/or modify it
- * under  the terms of  the GNU General  Public License as published by the
- * Free Software Foundation;  either version 2 of the  License, or (at your
- * option) any later version.
- */
-
-#include <drm/drmP.h>
-#include <drm/drm_crtc_helper.h>
-
-#include <drm/exynos_drm.h>
-#include "exynos_drm_drv.h"
-#include "exynos_drm_encoder.h"
-#include "exynos_drm_connector.h"
-
-#define to_exynos_connector(x)	container_of(x, struct exynos_drm_connector,\
-				drm_connector)
-
-struct exynos_drm_connector {
-	struct drm_connector		drm_connector;
-	uint32_t			encoder_id;
-	struct exynos_drm_display	*display;
-};
-
-static int exynos_drm_connector_get_modes(struct drm_connector *connector)
-{
-	struct exynos_drm_connector *exynos_connector =
-					to_exynos_connector(connector);
-	struct exynos_drm_display *display = exynos_connector->display;
-	struct edid *edid = NULL;
-	unsigned int count = 0;
-	int ret;
-
-	/*
-	 * if get_edid() exists then get_edid() callback of hdmi side
-	 * is called to get edid data through i2c interface else
-	 * get timing from the FIMD driver(display controller).
-	 *
-	 * P.S. in case of lcd panel, count is always 1 if success
-	 * because lcd panel has only one mode.
-	 */
-	if (display->ops->get_edid) {
-		edid = display->ops->get_edid(display, connector);
-		if (IS_ERR_OR_NULL(edid)) {
-			ret = PTR_ERR(edid);
-			edid = NULL;
-			DRM_ERROR("Panel operation get_edid failed %d\n", ret);
-			goto out;
-		}
-
-		count = drm_add_edid_modes(connector, edid);
-		if (!count) {
-			DRM_ERROR("Add edid modes failed %d\n", count);
-			goto out;
-		}
-
-		drm_mode_connector_update_edid_property(connector, edid);
-	} else {
-		struct exynos_drm_panel_info *panel;
-		struct drm_display_mode *mode = drm_mode_create(connector->dev);
-		if (!mode) {
-			DRM_ERROR("failed to create a new display mode.\n");
-			return 0;
-		}
-
-		if (display->ops->get_panel)
-			panel = display->ops->get_panel(display);
-		else {
-			drm_mode_destroy(connector->dev, mode);
-			return 0;
-		}
-
-		drm_display_mode_from_videomode(&panel->vm, mode);
-		mode->width_mm = panel->width_mm;
-		mode->height_mm = panel->height_mm;
-		connector->display_info.width_mm = mode->width_mm;
-		connector->display_info.height_mm = mode->height_mm;
-
-		mode->type = DRM_MODE_TYPE_DRIVER | DRM_MODE_TYPE_PREFERRED;
-		drm_mode_set_name(mode);
-		drm_mode_probed_add(connector, mode);
-
-		count = 1;
-	}
-
-out:
-	kfree(edid);
-	return count;
-}
-
-static int exynos_drm_connector_mode_valid(struct drm_connector *connector,
-					    struct drm_display_mode *mode)
-{
-	struct exynos_drm_connector *exynos_connector =
-					to_exynos_connector(connector);
-	struct exynos_drm_display *display = exynos_connector->display;
-	int ret = MODE_BAD;
-
-	DRM_DEBUG_KMS("%s\n", __FILE__);
-
-	if (display->ops->check_mode)
-		if (!display->ops->check_mode(display, mode))
-			ret = MODE_OK;
-
-	return ret;
-}
-
-static struct drm_encoder *exynos_drm_best_encoder(
-		struct drm_connector *connector)
-{
-	struct drm_device *dev = connector->dev;
-	struct exynos_drm_connector *exynos_connector =
-					to_exynos_connector(connector);
-	return drm_encoder_find(dev, exynos_connector->encoder_id);
-}
-
-static struct drm_connector_helper_funcs exynos_connector_helper_funcs = {
-	.get_modes	= exynos_drm_connector_get_modes,
-	.mode_valid	= exynos_drm_connector_mode_valid,
-	.best_encoder	= exynos_drm_best_encoder,
-};
-
-static int exynos_drm_connector_fill_modes(struct drm_connector *connector,
-				unsigned int max_width, unsigned int max_height)
-{
-	struct exynos_drm_connector *exynos_connector =
-					to_exynos_connector(connector);
-	struct exynos_drm_display *display = exynos_connector->display;
-	unsigned int width, height;
-
-	width = max_width;
-	height = max_height;
-
-	/*
-	 * if specific driver want to find desired_mode using maxmum
-	 * resolution then get max width and height from that driver.
-	 */
-	if (display->ops->get_max_resol)
-		display->ops->get_max_resol(display, &width, &height);
-
-	return drm_helper_probe_single_connector_modes(connector, width,
-							height);
-}
-
-/* get detection status of display device. */
-static enum drm_connector_status
-exynos_drm_connector_detect(struct drm_connector *connector, bool force)
-{
-	struct exynos_drm_connector *exynos_connector =
-					to_exynos_connector(connector);
-	struct exynos_drm_display *display = exynos_connector->display;
-	enum drm_connector_status status = connector_status_disconnected;
-
-	if (display->ops->is_connected) {
-		if (display->ops->is_connected(display))
-			status = connector_status_connected;
-		else
-			status = connector_status_disconnected;
-	}
-
-	return status;
-}
-
-static void exynos_drm_connector_destroy(struct drm_connector *connector)
-{
-	struct exynos_drm_connector *exynos_connector =
-		to_exynos_connector(connector);
-
-	drm_connector_unregister(connector);
-	drm_connector_cleanup(connector);
-	kfree(exynos_connector);
-}
-
-static struct drm_connector_funcs exynos_connector_funcs = {
-	.dpms		= drm_helper_connector_dpms,
-	.fill_modes	= exynos_drm_connector_fill_modes,
-	.detect		= exynos_drm_connector_detect,
-	.destroy	= exynos_drm_connector_destroy,
-};
-
-struct drm_connector *exynos_drm_connector_create(struct drm_device *dev,
-						   struct drm_encoder *encoder)
-{
-	struct exynos_drm_connector *exynos_connector;
-	struct exynos_drm_display *display = exynos_drm_get_display(encoder);
-	struct drm_connector *connector;
-	int type;
-	int err;
-
-	exynos_connector = kzalloc(sizeof(*exynos_connector), GFP_KERNEL);
-	if (!exynos_connector)
-		return NULL;
-
-	connector = &exynos_connector->drm_connector;
-
-	switch (display->type) {
-	case EXYNOS_DISPLAY_TYPE_HDMI:
-		type = DRM_MODE_CONNECTOR_HDMIA;
-		connector->interlace_allowed = true;
-		connector->polled = DRM_CONNECTOR_POLL_HPD;
-		break;
-	case EXYNOS_DISPLAY_TYPE_VIDI:
-		type = DRM_MODE_CONNECTOR_VIRTUAL;
-		connector->polled = DRM_CONNECTOR_POLL_HPD;
-		break;
-	default:
-		type = DRM_MODE_CONNECTOR_Unknown;
-		break;
-	}
-
-	drm_connector_init(dev, connector, &exynos_connector_funcs, type);
-	drm_connector_helper_add(connector, &exynos_connector_helper_funcs);
-
-	err = drm_connector_register(connector);
-	if (err)
-		goto err_connector;
-
-	exynos_connector->encoder_id = encoder->base.id;
-	exynos_connector->display = display;
-	connector->dpms = DRM_MODE_DPMS_OFF;
-	connector->encoder = encoder;
-
-	err = drm_mode_connector_attach_encoder(connector, encoder);
-	if (err) {
-		DRM_ERROR("failed to attach a connector to a encoder\n");
-		goto err_sysfs;
-	}
-
-	DRM_DEBUG_KMS("connector has been created\n");
-
-	return connector;
-
-err_sysfs:
-	drm_connector_unregister(connector);
-err_connector:
-	drm_connector_cleanup(connector);
-	kfree(exynos_connector);
-	return NULL;
-}
diff --git a/drivers/gpu/drm/exynos/exynos_drm_connector.h b/drivers/gpu/drm/exynos/exynos_drm_connector.h
deleted file mode 100644
index 4eb20d78379a..000000000000
--- a/drivers/gpu/drm/exynos/exynos_drm_connector.h
+++ /dev/null
@@ -1,20 +0,0 @@
-/*
- * Copyright (c) 2011 Samsung Electronics Co., Ltd.
- * Authors:
- *	Inki Dae <inki.dae@samsung.com>
- *	Joonyoung Shim <jy0922.shim@samsung.com>
- *	Seung-Woo Kim <sw0312.kim@samsung.com>
- *
- * This program is free software; you can redistribute  it and/or modify it
- * under  the terms of  the GNU General  Public License as published by the
- * Free Software Foundation;  either version 2 of the  License, or (at your
- * option) any later version.
- */
-
-#ifndef _EXYNOS_DRM_CONNECTOR_H_
-#define _EXYNOS_DRM_CONNECTOR_H_
-
-struct drm_connector *exynos_drm_connector_create(struct drm_device *dev,
-						   struct drm_encoder *encoder);
-
-#endif
diff --git a/drivers/gpu/drm/exynos/exynos_drm_fimd.c b/drivers/gpu/drm/exynos/exynos_drm_fimd.c
index 925fc69af1a0..33a10ce967ea 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_fimd.c
+++ b/drivers/gpu/drm/exynos/exynos_drm_fimd.c
@@ -147,6 +147,7 @@ struct fimd_win_data {
 	unsigned int		ovl_height;
 	unsigned int		fb_width;
 	unsigned int		fb_height;
+	unsigned int		fb_pitch;
 	unsigned int		bpp;
 	unsigned int		pixel_format;
 	dma_addr_t		dma_addr;
@@ -284,14 +285,9 @@ static void fimd_clear_channel(struct fimd_context *ctx)
 	}
 }
 
-static int fimd_ctx_initialize(struct fimd_context *ctx,
+static int fimd_iommu_attach_devices(struct fimd_context *ctx,
 			struct drm_device *drm_dev)
 {
-	struct exynos_drm_private *priv;
-	priv = drm_dev->dev_private;
-
-	ctx->drm_dev = drm_dev;
-	ctx->pipe = priv->pipe++;
 
 	/* attach this sub driver to iommu mapping if supported. */
 	if (is_drm_iommu_supported(ctx->drm_dev)) {
@@ -313,7 +309,7 @@ static int fimd_ctx_initialize(struct fimd_context *ctx,
 	return 0;
 }
 
-static void fimd_ctx_remove(struct fimd_context *ctx)
+static void fimd_iommu_detach_devices(struct fimd_context *ctx)
 {
 	/* detach this sub driver from iommu mapping if supported. */
 	if (is_drm_iommu_supported(ctx->drm_dev))
@@ -537,13 +533,14 @@ static void fimd_win_mode_set(struct exynos_drm_crtc *crtc,
 	win_data->offset_y = plane->crtc_y;
 	win_data->ovl_width = plane->crtc_width;
 	win_data->ovl_height = plane->crtc_height;
+	win_data->fb_pitch = plane->pitch;
 	win_data->fb_width = plane->fb_width;
 	win_data->fb_height = plane->fb_height;
 	win_data->dma_addr = plane->dma_addr[0] + offset;
 	win_data->bpp = plane->bpp;
 	win_data->pixel_format = plane->pixel_format;
-	win_data->buf_offsize = (plane->fb_width - plane->crtc_width) *
-				(plane->bpp >> 3);
+	win_data->buf_offsize =
+		plane->pitch - (plane->crtc_width * (plane->bpp >> 3));
 	win_data->line_size = plane->crtc_width * (plane->bpp >> 3);
 
 	DRM_DEBUG_KMS("offset_x = %d, offset_y = %d\n",
@@ -709,7 +706,7 @@ static void fimd_win_commit(struct exynos_drm_crtc *crtc, int zpos)
 	writel(val, ctx->regs + VIDWx_BUF_START(win, 0));
 
 	/* buffer end address */
-	size = win_data->fb_width * win_data->ovl_height * (win_data->bpp >> 3);
+	size = win_data->fb_pitch * win_data->ovl_height * (win_data->bpp >> 3);
 	val = (unsigned long)(win_data->dma_addr + size);
 	writel(val, ctx->regs + VIDWx_BUF_END(win, 0));
 
@@ -1056,25 +1053,23 @@ static int fimd_bind(struct device *dev, struct device *master, void *data)
 {
 	struct fimd_context *ctx = dev_get_drvdata(dev);
 	struct drm_device *drm_dev = data;
+	struct exynos_drm_private *priv = drm_dev->dev_private;
 	int ret;
 
-	ret = fimd_ctx_initialize(ctx, drm_dev);
-	if (ret) {
-		DRM_ERROR("fimd_ctx_initialize failed.\n");
-		return ret;
-	}
+	ctx->drm_dev = drm_dev;
+	ctx->pipe = priv->pipe++;
 
 	ctx->crtc = exynos_drm_crtc_create(drm_dev, ctx->pipe,
 					   EXYNOS_DISPLAY_TYPE_LCD,
 					   &fimd_crtc_ops, ctx);
-	if (IS_ERR(ctx->crtc)) {
-		fimd_ctx_remove(ctx);
-		return PTR_ERR(ctx->crtc);
-	}
 
 	if (ctx->display)
 		exynos_drm_create_enc_conn(drm_dev, ctx->display);
 
+	ret = fimd_iommu_attach_devices(ctx, drm_dev);
+	if (ret)
+		return ret;
+
 	return 0;
 
 }
@@ -1086,10 +1081,10 @@ static void fimd_unbind(struct device *dev, struct device *master,
 
 	fimd_dpms(ctx->crtc, DRM_MODE_DPMS_OFF);
 
+	fimd_iommu_detach_devices(ctx);
+
 	if (ctx->display)
 		exynos_dpi_remove(ctx->display);
-
-	fimd_ctx_remove(ctx);
 }
 
 static const struct component_ops fimd_component_ops = {
diff --git a/drivers/gpu/drm/exynos/exynos_drm_plane.c b/drivers/gpu/drm/exynos/exynos_drm_plane.c
index a5616872eee7..8ad5b7294eb4 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_plane.c
+++ b/drivers/gpu/drm/exynos/exynos_drm_plane.c
@@ -175,7 +175,7 @@ static int exynos_disable_plane(struct drm_plane *plane)
 	struct exynos_drm_plane *exynos_plane = to_exynos_plane(plane);
 	struct exynos_drm_crtc *exynos_crtc = to_exynos_crtc(plane->crtc);
 
-	if (exynos_crtc->ops->win_disable)
+	if (exynos_crtc && exynos_crtc->ops->win_disable)
 		exynos_crtc->ops->win_disable(exynos_crtc,
 					      exynos_plane->zpos);
 
diff --git a/drivers/gpu/drm/exynos/exynos_mixer.c b/drivers/gpu/drm/exynos/exynos_mixer.c
index 3518bc4654c5..2e3bc57ea50e 100644
--- a/drivers/gpu/drm/exynos/exynos_mixer.c
+++ b/drivers/gpu/drm/exynos/exynos_mixer.c
@@ -55,6 +55,7 @@ struct hdmi_win_data {
 	unsigned int		fb_x;
 	unsigned int		fb_y;
 	unsigned int		fb_width;
+	unsigned int		fb_pitch;
 	unsigned int		fb_height;
 	unsigned int		src_width;
 	unsigned int		src_height;
@@ -438,7 +439,7 @@ static void vp_video_buffer(struct mixer_context *ctx, int win)
 	} else {
 		luma_addr[0] = win_data->dma_addr;
 		chroma_addr[0] = win_data->dma_addr
-			+ (win_data->fb_width * win_data->fb_height);
+			+ (win_data->fb_pitch * win_data->fb_height);
 	}
 
 	if (win_data->scan_flags & DRM_MODE_FLAG_INTERLACE) {
@@ -447,8 +448,8 @@ static void vp_video_buffer(struct mixer_context *ctx, int win)
 			luma_addr[1] = luma_addr[0] + 0x40;
 			chroma_addr[1] = chroma_addr[0] + 0x40;
 		} else {
-			luma_addr[1] = luma_addr[0] + win_data->fb_width;
-			chroma_addr[1] = chroma_addr[0] + win_data->fb_width;
+			luma_addr[1] = luma_addr[0] + win_data->fb_pitch;
+			chroma_addr[1] = chroma_addr[0] + win_data->fb_pitch;
 		}
 	} else {
 		ctx->interlace = false;
@@ -469,10 +470,10 @@ static void vp_video_buffer(struct mixer_context *ctx, int win)
 	vp_reg_writemask(res, VP_MODE, val, VP_MODE_FMT_MASK);
 
 	/* setting size of input image */
-	vp_reg_write(res, VP_IMG_SIZE_Y, VP_IMG_HSIZE(win_data->fb_width) |
+	vp_reg_write(res, VP_IMG_SIZE_Y, VP_IMG_HSIZE(win_data->fb_pitch) |
 		VP_IMG_VSIZE(win_data->fb_height));
 	/* chroma height has to reduced by 2 to avoid chroma distorions */
-	vp_reg_write(res, VP_IMG_SIZE_C, VP_IMG_HSIZE(win_data->fb_width) |
+	vp_reg_write(res, VP_IMG_SIZE_C, VP_IMG_HSIZE(win_data->fb_pitch) |
 		VP_IMG_VSIZE(win_data->fb_height / 2));
 
 	vp_reg_write(res, VP_SRC_WIDTH, win_data->src_width);
@@ -559,7 +560,7 @@ static void mixer_graph_buffer(struct mixer_context *ctx, int win)
 	/* converting dma address base and source offset */
 	dma_addr = win_data->dma_addr
 		+ (win_data->fb_x * win_data->bpp >> 3)
-		+ (win_data->fb_y * win_data->fb_width * win_data->bpp >> 3);
+		+ (win_data->fb_y * win_data->fb_pitch);
 	src_x_offset = 0;
 	src_y_offset = 0;
 
@@ -576,7 +577,8 @@ static void mixer_graph_buffer(struct mixer_context *ctx, int win)
 		MXR_GRP_CFG_FORMAT_VAL(fmt), MXR_GRP_CFG_FORMAT_MASK);
 
 	/* setup geometry */
-	mixer_reg_write(res, MXR_GRAPHIC_SPAN(win), win_data->fb_width);
+	mixer_reg_write(res, MXR_GRAPHIC_SPAN(win),
+			win_data->fb_pitch / (win_data->bpp >> 3));
 
 	/* setup display size */
 	if (ctx->mxr_ver == MXR_VER_128_0_0_184 &&
@@ -961,6 +963,7 @@ static void mixer_win_mode_set(struct exynos_drm_crtc *crtc,
 	win_data->fb_y = plane->fb_y;
 	win_data->fb_width = plane->fb_width;
 	win_data->fb_height = plane->fb_height;
+	win_data->fb_pitch = plane->pitch;
 	win_data->src_width = plane->src_width;
 	win_data->src_height = plane->src_height;
 
diff --git a/drivers/gpu/drm/i915/i915_debugfs.c b/drivers/gpu/drm/i915/i915_debugfs.c
index 96e811fe24ca..e8b18e542da4 100644
--- a/drivers/gpu/drm/i915/i915_debugfs.c
+++ b/drivers/gpu/drm/i915/i915_debugfs.c
@@ -152,12 +152,12 @@ describe_obj(struct seq_file *m, struct drm_i915_gem_object *obj)
 			seq_puts(m, " (pp");
 		else
 			seq_puts(m, " (g");
-		seq_printf(m, "gtt offset: %08lx, size: %08lx, type: %u)",
+		seq_printf(m, "gtt offset: %08llx, size: %08llx, type: %u)",
 			   vma->node.start, vma->node.size,
 			   vma->ggtt_view.type);
 	}
 	if (obj->stolen)
-		seq_printf(m, " (stolen: %08lx)", obj->stolen->start);
+		seq_printf(m, " (stolen: %08llx)", obj->stolen->start);
 	if (obj->pin_mappable || obj->fault_mappable) {
 		char s[3], *t = s;
 		if (obj->pin_mappable)
diff --git a/drivers/gpu/drm/i915/i915_drv.c b/drivers/gpu/drm/i915/i915_drv.c
index 8039cec71fc2..5c66b568bb81 100644
--- a/drivers/gpu/drm/i915/i915_drv.c
+++ b/drivers/gpu/drm/i915/i915_drv.c
@@ -622,7 +622,7 @@ static int i915_drm_suspend(struct drm_device *dev)
 	return 0;
 }
 
-static int i915_drm_suspend_late(struct drm_device *drm_dev)
+static int i915_drm_suspend_late(struct drm_device *drm_dev, bool hibernation)
 {
 	struct drm_i915_private *dev_priv = drm_dev->dev_private;
 	int ret;
@@ -636,7 +636,17 @@ static int i915_drm_suspend_late(struct drm_device *drm_dev)
 	}
 
 	pci_disable_device(drm_dev->pdev);
-	pci_set_power_state(drm_dev->pdev, PCI_D3hot);
+	/*
+	 * During hibernation on some GEN4 platforms the BIOS may try to access
+	 * the device even though it's already in D3 and hang the machine. So
+	 * leave the device in D0 on those platforms and hope the BIOS will
+	 * power down the device properly. Platforms where this was seen:
+	 * Lenovo Thinkpad X301, X61s
+	 */
+	if (!(hibernation &&
+	      drm_dev->pdev->subsystem_vendor == PCI_VENDOR_ID_LENOVO &&
+	      INTEL_INFO(dev_priv)->gen == 4))
+		pci_set_power_state(drm_dev->pdev, PCI_D3hot);
 
 	return 0;
 }
@@ -662,7 +672,7 @@ int i915_suspend_legacy(struct drm_device *dev, pm_message_t state)
 	if (error)
 		return error;
 
-	return i915_drm_suspend_late(dev);
+	return i915_drm_suspend_late(dev, false);
 }
 
 static int i915_drm_resume(struct drm_device *dev)
@@ -950,7 +960,17 @@ static int i915_pm_suspend_late(struct device *dev)
 	if (drm_dev->switch_power_state == DRM_SWITCH_POWER_OFF)
 		return 0;
 
-	return i915_drm_suspend_late(drm_dev);
+	return i915_drm_suspend_late(drm_dev, false);
+}
+
+static int i915_pm_poweroff_late(struct device *dev)
+{
+	struct drm_device *drm_dev = dev_to_i915(dev)->dev;
+
+	if (drm_dev->switch_power_state == DRM_SWITCH_POWER_OFF)
+		return 0;
+
+	return i915_drm_suspend_late(drm_dev, true);
 }
 
 static int i915_pm_resume_early(struct device *dev)
@@ -1075,6 +1095,7 @@ static void vlv_save_gunit_s0ix_state(struct drm_i915_private *dev_priv)
 	/* Gunit-Display CZ domain, 0x182028-0x1821CF */
 	s->gu_ctl0		= I915_READ(VLV_GU_CTL0);
 	s->gu_ctl1		= I915_READ(VLV_GU_CTL1);
+	s->pcbr			= I915_READ(VLV_PCBR);
 	s->clock_gate_dis2	= I915_READ(VLV_GUNIT_CLOCK_GATE2);
 
 	/*
@@ -1169,6 +1190,7 @@ static void vlv_restore_gunit_s0ix_state(struct drm_i915_private *dev_priv)
 	/* Gunit-Display CZ domain, 0x182028-0x1821CF */
 	I915_WRITE(VLV_GU_CTL0,			s->gu_ctl0);
 	I915_WRITE(VLV_GU_CTL1,			s->gu_ctl1);
+	I915_WRITE(VLV_PCBR,			s->pcbr);
 	I915_WRITE(VLV_GUNIT_CLOCK_GATE2,	s->clock_gate_dis2);
 }
 
@@ -1177,19 +1199,7 @@ int vlv_force_gfx_clock(struct drm_i915_private *dev_priv, bool force_on)
 	u32 val;
 	int err;
 
-	val = I915_READ(VLV_GTLC_SURVIVABILITY_REG);
-	WARN_ON(!!(val & VLV_GFX_CLK_FORCE_ON_BIT) == force_on);
-
 #define COND (I915_READ(VLV_GTLC_SURVIVABILITY_REG) & VLV_GFX_CLK_STATUS_BIT)
-	/* Wait for a previous force-off to settle */
-	if (force_on) {
-		err = wait_for(!COND, 20);
-		if (err) {
-			DRM_ERROR("timeout waiting for GFX clock force-off (%08x)\n",
-				  I915_READ(VLV_GTLC_SURVIVABILITY_REG));
-			return err;
-		}
-	}
 
 	val = I915_READ(VLV_GTLC_SURVIVABILITY_REG);
 	val &= ~VLV_GFX_CLK_FORCE_ON_BIT;
@@ -1520,7 +1530,7 @@ static const struct dev_pm_ops i915_pm_ops = {
 	.thaw_early = i915_pm_resume_early,
 	.thaw = i915_pm_resume,
 	.poweroff = i915_pm_suspend,
-	.poweroff_late = i915_pm_suspend_late,
+	.poweroff_late = i915_pm_poweroff_late,
 	.restore_early = i915_pm_resume_early,
 	.restore = i915_pm_resume,
 
diff --git a/drivers/gpu/drm/i915/i915_drv.h b/drivers/gpu/drm/i915/i915_drv.h
index f2a825e39646..b4faa2df9d3d 100644
--- a/drivers/gpu/drm/i915/i915_drv.h
+++ b/drivers/gpu/drm/i915/i915_drv.h
@@ -1094,6 +1094,7 @@ struct vlv_s0ix_state {
 	/* Display 2 CZ domain */
 	u32 gu_ctl0;
 	u32 gu_ctl1;
+	u32 pcbr;
 	u32 clock_gate_dis2;
 };
 
@@ -2114,6 +2115,9 @@ void i915_gem_track_fb(struct drm_i915_gem_object *old,
  * number comparisons on buffer last_read|write_seqno. It also allows an
  * emission time to be associated with the request for tracking how far ahead
  * of the GPU the submission is.
+ *
+ * The requests are reference counted, so upon creation they should have an
+ * initial reference taken using kref_init
  */
 struct drm_i915_gem_request {
 	struct kref ref;
@@ -2137,7 +2141,16 @@ struct drm_i915_gem_request {
 	/** Position in the ringbuffer of the end of the whole request */
 	u32 tail;
 
-	/** Context related to this request */
+	/**
+	 * Context related to this request
+	 * Contexts are refcounted, so when this request is associated with a
+	 * context, we must increment the context's refcount, to guarantee that
+	 * it persists while any request is linked to it. Requests themselves
+	 * are also refcounted, so the request will only be freed when the last
+	 * reference to it is dismissed, and the code in
+	 * i915_gem_request_free() will then decrement the refcount on the
+	 * context.
+	 */
 	struct intel_context *ctx;
 
 	/** Batch buffer related to this request if any */
@@ -2374,6 +2387,7 @@ struct drm_i915_cmd_table {
 				 (INTEL_DEVID(dev) & 0xFF00) == 0x0C00)
 #define IS_BDW_ULT(dev)		(IS_BROADWELL(dev) && \
 				 ((INTEL_DEVID(dev) & 0xf) == 0x6 ||	\
+				 (INTEL_DEVID(dev) & 0xf) == 0xb ||	\
 				 (INTEL_DEVID(dev) & 0xf) == 0xe))
 #define IS_BDW_GT3(dev)		(IS_BROADWELL(dev) && \
 				 (INTEL_DEVID(dev) & 0x00F0) == 0x0020)
diff --git a/drivers/gpu/drm/i915/i915_gem.c b/drivers/gpu/drm/i915/i915_gem.c
index c26d36cc4b31..27ea6bdebce7 100644
--- a/drivers/gpu/drm/i915/i915_gem.c
+++ b/drivers/gpu/drm/i915/i915_gem.c
@@ -2659,8 +2659,7 @@ static void i915_gem_reset_ring_cleanup(struct drm_i915_private *dev_priv,
 		if (submit_req->ctx != ring->default_context)
 			intel_lr_context_unpin(ring, submit_req->ctx);
 
-		i915_gem_context_unreference(submit_req->ctx);
-		kfree(submit_req);
+		i915_gem_request_unreference(submit_req);
 	}
 
 	/*
@@ -2738,24 +2737,11 @@ i915_gem_retire_requests_ring(struct intel_engine_cs *ring)
 
 	WARN_ON(i915_verify_lists(ring->dev));
 
-	/* Move any buffers on the active list that are no longer referenced
-	 * by the ringbuffer to the flushing/inactive lists as appropriate,
-	 * before we free the context associated with the requests.
+	/* Retire requests first as we use it above for the early return.
+	 * If we retire requests last, we may use a later seqno and so clear
+	 * the requests lists without clearing the active list, leading to
+	 * confusion.
 	 */
-	while (!list_empty(&ring->active_list)) {
-		struct drm_i915_gem_object *obj;
-
-		obj = list_first_entry(&ring->active_list,
-				      struct drm_i915_gem_object,
-				      ring_list);
-
-		if (!i915_gem_request_completed(obj->last_read_req, true))
-			break;
-
-		i915_gem_object_move_to_inactive(obj);
-	}
-
-
 	while (!list_empty(&ring->request_list)) {
 		struct drm_i915_gem_request *request;
 		struct intel_ringbuffer *ringbuf;
@@ -2790,6 +2776,23 @@ i915_gem_retire_requests_ring(struct intel_engine_cs *ring)
 		i915_gem_free_request(request);
 	}
 
+	/* Move any buffers on the active list that are no longer referenced
+	 * by the ringbuffer to the flushing/inactive lists as appropriate,
+	 * before we free the context associated with the requests.
+	 */
+	while (!list_empty(&ring->active_list)) {
+		struct drm_i915_gem_object *obj;
+
+		obj = list_first_entry(&ring->active_list,
+				      struct drm_i915_gem_object,
+				      ring_list);
+
+		if (!i915_gem_request_completed(obj->last_read_req, true))
+			break;
+
+		i915_gem_object_move_to_inactive(obj);
+	}
+
 	if (unlikely(ring->trace_irq_req &&
 		     i915_gem_request_completed(ring->trace_irq_req, true))) {
 		ring->irq_put(ring);
@@ -2937,9 +2940,9 @@ i915_gem_wait_ioctl(struct drm_device *dev, void *data, struct drm_file *file)
 	req = obj->last_read_req;
 
 	/* Do this after OLR check to make sure we make forward progress polling
-	 * on this IOCTL with a timeout <=0 (like busy ioctl)
+	 * on this IOCTL with a timeout == 0 (like busy ioctl)
 	 */
-	if (args->timeout_ns <= 0) {
+	if (args->timeout_ns == 0) {
 		ret = -ETIME;
 		goto out;
 	}
@@ -2949,7 +2952,8 @@ i915_gem_wait_ioctl(struct drm_device *dev, void *data, struct drm_file *file)
 	i915_gem_request_reference(req);
 	mutex_unlock(&dev->struct_mutex);
 
-	ret = __i915_wait_request(req, reset_counter, true, &args->timeout_ns,
+	ret = __i915_wait_request(req, reset_counter, true,
+				  args->timeout_ns > 0 ? &args->timeout_ns : NULL,
 				  file->driver_priv);
 	mutex_lock(&dev->struct_mutex);
 	i915_gem_request_unreference(req);
@@ -4793,6 +4797,9 @@ i915_gem_init_hw(struct drm_device *dev)
 	if (INTEL_INFO(dev)->gen < 6 && !intel_enable_gtt())
 		return -EIO;
 
+	/* Double layer security blanket, see i915_gem_init() */
+	intel_uncore_forcewake_get(dev_priv, FORCEWAKE_ALL);
+
 	if (dev_priv->ellc_size)
 		I915_WRITE(HSW_IDICR, I915_READ(HSW_IDICR) | IDIHASHMSK(0xf));
 
@@ -4825,7 +4832,7 @@ i915_gem_init_hw(struct drm_device *dev)
 	for_each_ring(ring, dev_priv, i) {
 		ret = ring->init_hw(ring);
 		if (ret)
-			return ret;
+			goto out;
 	}
 
 	for (i = 0; i < NUM_L3_SLICES(dev); i++)
@@ -4842,9 +4849,11 @@ i915_gem_init_hw(struct drm_device *dev)
 		DRM_ERROR("Context enable failed %d\n", ret);
 		i915_gem_cleanup_ringbuffer(dev);
 
-		return ret;
+		goto out;
 	}
 
+out:
+	intel_uncore_forcewake_put(dev_priv, FORCEWAKE_ALL);
 	return ret;
 }
 
@@ -4878,6 +4887,14 @@ int i915_gem_init(struct drm_device *dev)
 		dev_priv->gt.stop_ring = intel_logical_ring_stop;
 	}
 
+	/* This is just a security blanket to placate dragons.
+	 * On some systems, we very sporadically observe that the first TLBs
+	 * used by the CS may be stale, despite us poking the TLB reset. If
+	 * we hold the forcewake during initialisation these problems
+	 * just magically go away.
+	 */
+	intel_uncore_forcewake_get(dev_priv, FORCEWAKE_ALL);
+
 	ret = i915_gem_init_userptr(dev);
 	if (ret)
 		goto out_unlock;
@@ -4904,6 +4921,7 @@ int i915_gem_init(struct drm_device *dev)
 	}
 
 out_unlock:
+	intel_uncore_forcewake_put(dev_priv, FORCEWAKE_ALL);
 	mutex_unlock(&dev->struct_mutex);
 
 	return ret;
diff --git a/drivers/gpu/drm/i915/i915_gem_execbuffer.c b/drivers/gpu/drm/i915/i915_gem_execbuffer.c
index b773368fc62c..38a742532c4f 100644
--- a/drivers/gpu/drm/i915/i915_gem_execbuffer.c
+++ b/drivers/gpu/drm/i915/i915_gem_execbuffer.c
@@ -1487,7 +1487,7 @@ i915_gem_do_execbuffer(struct drm_device *dev, void *data,
 		goto err;
 	}
 
-	if (i915_needs_cmd_parser(ring)) {
+	if (i915_needs_cmd_parser(ring) && args->batch_len) {
 		batch_obj = i915_gem_execbuffer_parse(ring,
 						      &shadow_exec_entry,
 						      eb,
diff --git a/drivers/gpu/drm/i915/i915_gem_gtt.c b/drivers/gpu/drm/i915/i915_gem_gtt.c
index 746f77fb57a3..dccdc8aad2e2 100644
--- a/drivers/gpu/drm/i915/i915_gem_gtt.c
+++ b/drivers/gpu/drm/i915/i915_gem_gtt.c
@@ -1145,7 +1145,7 @@ static int gen6_ppgtt_init(struct i915_hw_ppgtt *ppgtt)
 
 	ppgtt->base.clear_range(&ppgtt->base, 0, ppgtt->base.total, true);
 
-	DRM_DEBUG_DRIVER("Allocated pde space (%ldM) at GTT entry: %lx\n",
+	DRM_DEBUG_DRIVER("Allocated pde space (%lldM) at GTT entry: %llx\n",
 			 ppgtt->node.size >> 20,
 			 ppgtt->node.start / PAGE_SIZE);
 
@@ -1713,8 +1713,8 @@ void i915_gem_gtt_finish_object(struct drm_i915_gem_object *obj)
 
 static void i915_gtt_color_adjust(struct drm_mm_node *node,
 				  unsigned long color,
-				  unsigned long *start,
-				  unsigned long *end)
+				  u64 *start,
+				  u64 *end)
 {
 	if (node->color != color)
 		*start += 4096;
diff --git a/drivers/gpu/drm/i915/i915_gem_stolen.c b/drivers/gpu/drm/i915/i915_gem_stolen.c
index a2045848bd1a..9c6f93ec886b 100644
--- a/drivers/gpu/drm/i915/i915_gem_stolen.c
+++ b/drivers/gpu/drm/i915/i915_gem_stolen.c
@@ -485,10 +485,8 @@ i915_gem_object_create_stolen_for_preallocated(struct drm_device *dev,
 			stolen_offset, gtt_offset, size);
 
 	/* KISS and expect everything to be page-aligned */
-	BUG_ON(stolen_offset & 4095);
-	BUG_ON(size & 4095);
-
-	if (WARN_ON(size == 0))
+	if (WARN_ON(size == 0) || WARN_ON(size & 4095) ||
+	    WARN_ON(stolen_offset & 4095))
 		return NULL;
 
 	stolen = kzalloc(sizeof(*stolen), GFP_KERNEL);
diff --git a/drivers/gpu/drm/i915/i915_gem_tiling.c b/drivers/gpu/drm/i915/i915_gem_tiling.c
index 7a24bd1a51f6..6377b22269ad 100644
--- a/drivers/gpu/drm/i915/i915_gem_tiling.c
+++ b/drivers/gpu/drm/i915/i915_gem_tiling.c
@@ -335,9 +335,10 @@ i915_gem_set_tiling(struct drm_device *dev, void *data,
 		return -EINVAL;
 	}
 
+	mutex_lock(&dev->struct_mutex);
 	if (i915_gem_obj_is_pinned(obj) || obj->framebuffer_references) {
-		drm_gem_object_unreference_unlocked(&obj->base);
-		return -EBUSY;
+		ret = -EBUSY;
+		goto err;
 	}
 
 	if (args->tiling_mode == I915_TILING_NONE) {
@@ -369,7 +370,6 @@ i915_gem_set_tiling(struct drm_device *dev, void *data,
 		}
 	}
 
-	mutex_lock(&dev->struct_mutex);
 	if (args->tiling_mode != obj->tiling_mode ||
 	    args->stride != obj->stride) {
 		/* We need to rebind the object if its current allocation
@@ -424,6 +424,7 @@ i915_gem_set_tiling(struct drm_device *dev, void *data,
 		obj->bit_17 = NULL;
 	}
 
+err:
 	drm_gem_object_unreference(&obj->base);
 	mutex_unlock(&dev->struct_mutex);
 
diff --git a/drivers/gpu/drm/i915/i915_irq.c b/drivers/gpu/drm/i915/i915_irq.c
index 4145d95902f5..ede5bbbd8a08 100644
--- a/drivers/gpu/drm/i915/i915_irq.c
+++ b/drivers/gpu/drm/i915/i915_irq.c
@@ -1892,6 +1892,9 @@ static irqreturn_t valleyview_irq_handler(int irq, void *arg)
 	u32 iir, gt_iir, pm_iir;
 	irqreturn_t ret = IRQ_NONE;
 
+	if (!intel_irqs_enabled(dev_priv))
+		return IRQ_NONE;
+
 	while (true) {
 		/* Find, clear, then process each source of interrupt */
 
@@ -1936,6 +1939,9 @@ static irqreturn_t cherryview_irq_handler(int irq, void *arg)
 	u32 master_ctl, iir;
 	irqreturn_t ret = IRQ_NONE;
 
+	if (!intel_irqs_enabled(dev_priv))
+		return IRQ_NONE;
+
 	for (;;) {
 		master_ctl = I915_READ(GEN8_MASTER_IRQ) & ~GEN8_MASTER_IRQ_CONTROL;
 		iir = I915_READ(VLV_IIR);
@@ -2208,6 +2214,9 @@ static irqreturn_t ironlake_irq_handler(int irq, void *arg)
 	u32 de_iir, gt_iir, de_ier, sde_ier = 0;
 	irqreturn_t ret = IRQ_NONE;
 
+	if (!intel_irqs_enabled(dev_priv))
+		return IRQ_NONE;
+
 	/* We get interrupts on unclaimed registers, so check for this before we
 	 * do any I915_{READ,WRITE}. */
 	intel_uncore_check_errors(dev);
@@ -2279,6 +2288,9 @@ static irqreturn_t gen8_irq_handler(int irq, void *arg)
 	enum pipe pipe;
 	u32 aux_mask = GEN8_AUX_CHANNEL_A;
 
+	if (!intel_irqs_enabled(dev_priv))
+		return IRQ_NONE;
+
 	if (IS_GEN9(dev))
 		aux_mask |=  GEN9_AUX_CHANNEL_B | GEN9_AUX_CHANNEL_C |
 			GEN9_AUX_CHANNEL_D;
@@ -3771,6 +3783,9 @@ static irqreturn_t i8xx_irq_handler(int irq, void *arg)
 		I915_DISPLAY_PLANE_A_FLIP_PENDING_INTERRUPT |
 		I915_DISPLAY_PLANE_B_FLIP_PENDING_INTERRUPT;
 
+	if (!intel_irqs_enabled(dev_priv))
+		return IRQ_NONE;
+
 	iir = I915_READ16(IIR);
 	if (iir == 0)
 		return IRQ_NONE;
@@ -3951,6 +3966,9 @@ static irqreturn_t i915_irq_handler(int irq, void *arg)
 		I915_DISPLAY_PLANE_B_FLIP_PENDING_INTERRUPT;
 	int pipe, ret = IRQ_NONE;
 
+	if (!intel_irqs_enabled(dev_priv))
+		return IRQ_NONE;
+
 	iir = I915_READ(IIR);
 	do {
 		bool irq_received = (iir & ~flip_mask) != 0;
@@ -4171,6 +4189,9 @@ static irqreturn_t i965_irq_handler(int irq, void *arg)
 		I915_DISPLAY_PLANE_A_FLIP_PENDING_INTERRUPT |
 		I915_DISPLAY_PLANE_B_FLIP_PENDING_INTERRUPT;
 
+	if (!intel_irqs_enabled(dev_priv))
+		return IRQ_NONE;
+
 	iir = I915_READ(IIR);
 
 	for (;;) {
@@ -4520,6 +4541,7 @@ void intel_runtime_pm_disable_interrupts(struct drm_i915_private *dev_priv)
 {
 	dev_priv->dev->driver->irq_uninstall(dev_priv->dev);
 	dev_priv->pm.irqs_enabled = false;
+	synchronize_irq(dev_priv->dev->irq);
 }
 
 /**
diff --git a/drivers/gpu/drm/i915/intel_display.c b/drivers/gpu/drm/i915/intel_display.c
index 3d220a67f865..f75173c20f47 100644
--- a/drivers/gpu/drm/i915/intel_display.c
+++ b/drivers/gpu/drm/i915/intel_display.c
@@ -37,6 +37,7 @@
 #include <drm/i915_drm.h>
 #include "i915_drv.h"
 #include "i915_trace.h"
+#include <drm/drm_atomic.h>
 #include <drm/drm_atomic_helper.h>
 #include <drm/drm_dp_helper.h>
 #include <drm/drm_crtc_helper.h>
@@ -2371,13 +2372,19 @@ intel_alloc_plane_obj(struct intel_crtc *crtc,
 	struct drm_device *dev = crtc->base.dev;
 	struct drm_i915_gem_object *obj = NULL;
 	struct drm_mode_fb_cmd2 mode_cmd = { 0 };
-	u32 base = plane_config->base;
+	u32 base_aligned = round_down(plane_config->base, PAGE_SIZE);
+	u32 size_aligned = round_up(plane_config->base + plane_config->size,
+				    PAGE_SIZE);
+
+	size_aligned -= base_aligned;
 
 	if (plane_config->size == 0)
 		return false;
 
-	obj = i915_gem_object_create_stolen_for_preallocated(dev, base, base,
-							     plane_config->size);
+	obj = i915_gem_object_create_stolen_for_preallocated(dev,
+							     base_aligned,
+							     base_aligned,
+							     size_aligned);
 	if (!obj)
 		return false;
 
@@ -2410,6 +2417,14 @@ out_unref_obj:
 	return false;
 }
 
+/* Update plane->state->fb to match plane->fb after driver-internal updates */
+static void
+update_state_fb(struct drm_plane *plane)
+{
+	if (plane->fb != plane->state->fb)
+		drm_atomic_set_fb_for_plane(plane->state, plane->fb);
+}
+
 static void
 intel_find_plane_obj(struct intel_crtc *intel_crtc,
 		     struct intel_initial_plane_config *plane_config)
@@ -2423,8 +2438,15 @@ intel_find_plane_obj(struct intel_crtc *intel_crtc,
 	if (!intel_crtc->base.primary->fb)
 		return;
 
-	if (intel_alloc_plane_obj(intel_crtc, plane_config))
+	if (intel_alloc_plane_obj(intel_crtc, plane_config)) {
+		struct drm_plane *primary = intel_crtc->base.primary;
+
+		primary->state->crtc = &intel_crtc->base;
+		primary->crtc = &intel_crtc->base;
+		update_state_fb(primary);
+
 		return;
+	}
 
 	kfree(intel_crtc->base.primary->fb);
 	intel_crtc->base.primary->fb = NULL;
@@ -2447,15 +2469,21 @@ intel_find_plane_obj(struct intel_crtc *intel_crtc,
 			continue;
 
 		if (i915_gem_obj_ggtt_offset(obj) == plane_config->base) {
+			struct drm_plane *primary = intel_crtc->base.primary;
+
 			if (obj->tiling_mode != I915_TILING_NONE)
 				dev_priv->preserve_bios_swizzle = true;
 
 			drm_framebuffer_reference(c->primary->fb);
-			intel_crtc->base.primary->fb = c->primary->fb;
+			primary->fb = c->primary->fb;
+			primary->state->crtc = &intel_crtc->base;
+			primary->crtc = &intel_crtc->base;
 			obj->frontbuffer_bits |= INTEL_FRONTBUFFER_PRIMARY(intel_crtc->pipe);
 			break;
 		}
 	}
+
+	update_state_fb(intel_crtc->base.primary);
 }
 
 static void i9xx_update_primary_plane(struct drm_crtc *crtc,
@@ -2725,10 +2753,19 @@ static void skylake_update_primary_plane(struct drm_crtc *crtc,
 	case DRM_FORMAT_XRGB8888:
 		plane_ctl |= PLANE_CTL_FORMAT_XRGB_8888;
 		break;
+	case DRM_FORMAT_ARGB8888:
+		plane_ctl |= PLANE_CTL_FORMAT_XRGB_8888;
+		plane_ctl |= PLANE_CTL_ALPHA_SW_PREMULTIPLY;
+		break;
 	case DRM_FORMAT_XBGR8888:
 		plane_ctl |= PLANE_CTL_ORDER_RGBX;
 		plane_ctl |= PLANE_CTL_FORMAT_XRGB_8888;
 		break;
+	case DRM_FORMAT_ABGR8888:
+		plane_ctl |= PLANE_CTL_ORDER_RGBX;
+		plane_ctl |= PLANE_CTL_FORMAT_XRGB_8888;
+		plane_ctl |= PLANE_CTL_ALPHA_SW_PREMULTIPLY;
+		break;
 	case DRM_FORMAT_XRGB2101010:
 		plane_ctl |= PLANE_CTL_FORMAT_XRGB_2101010;
 		break;
@@ -6587,6 +6624,10 @@ i9xx_get_initial_plane_config(struct intel_crtc *crtc,
 	struct drm_framebuffer *fb;
 	struct intel_framebuffer *intel_fb;
 
+	val = I915_READ(DSPCNTR(plane));
+	if (!(val & DISPLAY_PLANE_ENABLE))
+		return;
+
 	intel_fb = kzalloc(sizeof(*intel_fb), GFP_KERNEL);
 	if (!intel_fb) {
 		DRM_DEBUG_KMS("failed to alloc fb\n");
@@ -6595,8 +6636,6 @@ i9xx_get_initial_plane_config(struct intel_crtc *crtc,
 
 	fb = &intel_fb->base;
 
-	val = I915_READ(DSPCNTR(plane));
-
 	if (INTEL_INFO(dev)->gen >= 4)
 		if (val & DISPPLANE_TILED)
 			plane_config->tiling = I915_TILING_X;
@@ -6627,7 +6666,7 @@ i9xx_get_initial_plane_config(struct intel_crtc *crtc,
 	aligned_height = intel_fb_align_height(dev, fb->height,
 					       plane_config->tiling);
 
-	plane_config->size = PAGE_ALIGN(fb->pitches[0] * aligned_height);
+	plane_config->size = fb->pitches[0] * aligned_height;
 
 	DRM_DEBUG_KMS("pipe/plane %c/%d with fb: size=%dx%d@%d, offset=%x, pitch %d, size 0x%x\n",
 		      pipe_name(pipe), plane, fb->width, fb->height,
@@ -7628,6 +7667,9 @@ skylake_get_initial_plane_config(struct intel_crtc *crtc,
 	fb = &intel_fb->base;
 
 	val = I915_READ(PLANE_CTL(pipe, 0));
+	if (!(val & PLANE_CTL_ENABLE))
+		goto error;
+
 	if (val & PLANE_CTL_TILED_MASK)
 		plane_config->tiling = I915_TILING_X;
 
@@ -7664,7 +7706,7 @@ skylake_get_initial_plane_config(struct intel_crtc *crtc,
 	aligned_height = intel_fb_align_height(dev, fb->height,
 					       plane_config->tiling);
 
-	plane_config->size = ALIGN(fb->pitches[0] * aligned_height, PAGE_SIZE);
+	plane_config->size = fb->pitches[0] * aligned_height;
 
 	DRM_DEBUG_KMS("pipe %c with fb: size=%dx%d@%d, offset=%x, pitch %d, size 0x%x\n",
 		      pipe_name(pipe), fb->width, fb->height,
@@ -7715,6 +7757,10 @@ ironlake_get_initial_plane_config(struct intel_crtc *crtc,
 	struct drm_framebuffer *fb;
 	struct intel_framebuffer *intel_fb;
 
+	val = I915_READ(DSPCNTR(pipe));
+	if (!(val & DISPLAY_PLANE_ENABLE))
+		return;
+
 	intel_fb = kzalloc(sizeof(*intel_fb), GFP_KERNEL);
 	if (!intel_fb) {
 		DRM_DEBUG_KMS("failed to alloc fb\n");
@@ -7723,8 +7769,6 @@ ironlake_get_initial_plane_config(struct intel_crtc *crtc,
 
 	fb = &intel_fb->base;
 
-	val = I915_READ(DSPCNTR(pipe));
-
 	if (INTEL_INFO(dev)->gen >= 4)
 		if (val & DISPPLANE_TILED)
 			plane_config->tiling = I915_TILING_X;
@@ -7755,7 +7799,7 @@ ironlake_get_initial_plane_config(struct intel_crtc *crtc,
 	aligned_height = intel_fb_align_height(dev, fb->height,
 					       plane_config->tiling);
 
-	plane_config->size = PAGE_ALIGN(fb->pitches[0] * aligned_height);
+	plane_config->size = fb->pitches[0] * aligned_height;
 
 	DRM_DEBUG_KMS("pipe %c with fb: size=%dx%d@%d, offset=%x, pitch %d, size 0x%x\n",
 		      pipe_name(pipe), fb->width, fb->height,
@@ -8698,6 +8742,7 @@ retry:
 			old->release_fb->funcs->destroy(old->release_fb);
 		goto fail;
 	}
+	crtc->primary->crtc = crtc;
 
 	/* let the connector get through one full cycle before testing */
 	intel_wait_for_vblank(dev, intel_crtc->pipe);
@@ -9700,7 +9745,7 @@ void intel_check_page_flip(struct drm_device *dev, int pipe)
 	struct drm_crtc *crtc = dev_priv->pipe_to_crtc_mapping[pipe];
 	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
 
-	WARN_ON(!in_irq());
+	WARN_ON(!in_interrupt());
 
 	if (crtc == NULL)
 		return;
@@ -9800,6 +9845,7 @@ static int intel_crtc_page_flip(struct drm_crtc *crtc,
 	drm_gem_object_reference(&obj->base);
 
 	crtc->primary->fb = fb;
+	update_state_fb(crtc->primary);
 
 	work->pending_flip_obj = obj;
 
@@ -9868,6 +9914,7 @@ cleanup_unpin:
 cleanup_pending:
 	atomic_dec(&intel_crtc->unpin_work_count);
 	crtc->primary->fb = old_fb;
+	update_state_fb(crtc->primary);
 	drm_gem_object_unreference(&work->old_fb_obj->base);
 	drm_gem_object_unreference(&obj->base);
 	mutex_unlock(&dev->struct_mutex);
@@ -12182,9 +12229,6 @@ intel_check_cursor_plane(struct drm_plane *plane,
 		return -ENOMEM;
 	}
 
-	if (fb == crtc->cursor->fb)
-		return 0;
-
 	/* we only need to pin inside GTT if cursor is non-phy */
 	mutex_lock(&dev->struct_mutex);
 	if (!INTEL_INFO(dev)->cursor_needs_physical && obj->tiling_mode) {
@@ -13096,6 +13140,9 @@ static struct intel_quirk intel_quirks[] = {
 
 	/* HP Chromebook 14 (Celeron 2955U) */
 	{ 0x0a06, 0x103c, 0x21ed, quirk_backlight_present },
+
+	/* Dell Chromebook 11 */
+	{ 0x0a06, 0x1028, 0x0a35, quirk_backlight_present },
 };
 
 static void intel_init_quirks(struct drm_device *dev)
@@ -13702,6 +13749,7 @@ void intel_modeset_gem_init(struct drm_device *dev)
 				  to_intel_crtc(c)->pipe);
 			drm_framebuffer_unreference(c->primary->fb);
 			c->primary->fb = NULL;
+			update_state_fb(c->primary);
 		}
 	}
 	mutex_unlock(&dev->struct_mutex);
diff --git a/drivers/gpu/drm/i915/intel_fifo_underrun.c b/drivers/gpu/drm/i915/intel_fifo_underrun.c
index 04e248dd2259..54daa66c6970 100644
--- a/drivers/gpu/drm/i915/intel_fifo_underrun.c
+++ b/drivers/gpu/drm/i915/intel_fifo_underrun.c
@@ -282,16 +282,6 @@ bool intel_set_cpu_fifo_underrun_reporting(struct drm_i915_private *dev_priv,
 	return ret;
 }
 
-static bool
-__cpu_fifo_underrun_reporting_enabled(struct drm_i915_private *dev_priv,
-				      enum pipe pipe)
-{
-	struct drm_crtc *crtc = dev_priv->pipe_to_crtc_mapping[pipe];
-	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
-
-	return !intel_crtc->cpu_fifo_underrun_disabled;
-}
-
 /**
  * intel_set_pch_fifo_underrun_reporting - set PCH fifo underrun reporting state
  * @dev_priv: i915 device instance
@@ -352,9 +342,15 @@ bool intel_set_pch_fifo_underrun_reporting(struct drm_i915_private *dev_priv,
 void intel_cpu_fifo_underrun_irq_handler(struct drm_i915_private *dev_priv,
 					 enum pipe pipe)
 {
+	struct drm_crtc *crtc = dev_priv->pipe_to_crtc_mapping[pipe];
+
+	/* We may be called too early in init, thanks BIOS! */
+	if (crtc == NULL)
+		return;
+
 	/* GMCH can't disable fifo underruns, filter them. */
 	if (HAS_GMCH_DISPLAY(dev_priv->dev) &&
-	    !__cpu_fifo_underrun_reporting_enabled(dev_priv, pipe))
+	    to_intel_crtc(crtc)->cpu_fifo_underrun_disabled)
 		return;
 
 	if (intel_set_cpu_fifo_underrun_reporting(dev_priv, pipe, false))
diff --git a/drivers/gpu/drm/i915/intel_lrc.c b/drivers/gpu/drm/i915/intel_lrc.c
index 0f358c5999ec..e8d3da9f3373 100644
--- a/drivers/gpu/drm/i915/intel_lrc.c
+++ b/drivers/gpu/drm/i915/intel_lrc.c
@@ -503,18 +503,19 @@ static int execlists_context_queue(struct intel_engine_cs *ring,
 		 * If there isn't a request associated with this submission,
 		 * create one as a temporary holder.
 		 */
-		WARN(1, "execlist context submission without request");
 		request = kzalloc(sizeof(*request), GFP_KERNEL);
 		if (request == NULL)
 			return -ENOMEM;
 		request->ring = ring;
 		request->ctx = to;
+		kref_init(&request->ref);
+		request->uniq = dev_priv->request_uniq++;
+		i915_gem_context_reference(request->ctx);
 	} else {
+		i915_gem_request_reference(request);
 		WARN_ON(to != request->ctx);
 	}
 	request->tail = tail;
-	i915_gem_request_reference(request);
-	i915_gem_context_reference(request->ctx);
 
 	intel_runtime_pm_get(dev_priv);
 
@@ -731,7 +732,6 @@ void intel_execlists_retire_requests(struct intel_engine_cs *ring)
 		if (ctx_obj && (ctx != ring->default_context))
 			intel_lr_context_unpin(ring, ctx);
 		intel_runtime_pm_put(dev_priv);
-		i915_gem_context_unreference(ctx);
 		list_del(&req->execlist_link);
 		i915_gem_request_unreference(req);
 	}
diff --git a/drivers/gpu/drm/i915/intel_sprite.c b/drivers/gpu/drm/i915/intel_sprite.c
index 0a52c44ad03d..9c5451c97942 100644
--- a/drivers/gpu/drm/i915/intel_sprite.c
+++ b/drivers/gpu/drm/i915/intel_sprite.c
@@ -1322,7 +1322,7 @@ int intel_sprite_set_colorkey(struct drm_device *dev, void *data,
 	drm_modeset_lock_all(dev);
 
 	plane = drm_plane_find(dev, set->plane_id);
-	if (!plane) {
+	if (!plane || plane->type != DRM_PLANE_TYPE_OVERLAY) {
 		ret = -ENOENT;
 		goto out_unlock;
 	}
@@ -1349,7 +1349,7 @@ int intel_sprite_get_colorkey(struct drm_device *dev, void *data,
 	drm_modeset_lock_all(dev);
 
 	plane = drm_plane_find(dev, get->plane_id);
-	if (!plane) {
+	if (!plane || plane->type != DRM_PLANE_TYPE_OVERLAY) {
 		ret = -ENOENT;
 		goto out_unlock;
 	}
diff --git a/drivers/gpu/drm/i915/intel_uncore.c b/drivers/gpu/drm/i915/intel_uncore.c
index c47a3baa53d5..4e8fb891d4ea 100644
--- a/drivers/gpu/drm/i915/intel_uncore.c
+++ b/drivers/gpu/drm/i915/intel_uncore.c
@@ -1048,8 +1048,14 @@ static void intel_uncore_fw_domains_init(struct drm_device *dev)
 
 		/* We need to init first for ECOBUS access and then
 		 * determine later if we want to reinit, in case of MT access is
-		 * not working
+		 * not working. In this stage we don't know which flavour this
+		 * ivb is, so it is better to reset also the gen6 fw registers
+		 * before the ecobus check.
 		 */
+
+		__raw_i915_write32(dev_priv, FORCEWAKE, 0);
+		__raw_posting_read(dev_priv, ECOBUS);
+
 		fw_domain_init(dev_priv, FW_DOMAIN_ID_RENDER,
 			       FORCEWAKE_MT, FORCEWAKE_MT_ACK);
 
diff --git a/drivers/gpu/drm/imx/dw_hdmi-imx.c b/drivers/gpu/drm/imx/dw_hdmi-imx.c
index 121d30ca2d44..87fe8ed92ebe 100644
--- a/drivers/gpu/drm/imx/dw_hdmi-imx.c
+++ b/drivers/gpu/drm/imx/dw_hdmi-imx.c
@@ -70,7 +70,9 @@ static const struct dw_hdmi_curr_ctrl imx_cur_ctr[] = {
 		118800000, { 0x091c, 0x091c, 0x06dc },
 	}, {
 		216000000, { 0x06dc, 0x0b5c, 0x091c },
-	}
+	}, {
+		~0UL, { 0x0000, 0x0000, 0x0000 },
+	},
 };
 
 static const struct dw_hdmi_sym_term imx_sym_term[] = {
@@ -136,11 +138,34 @@ static struct drm_encoder_funcs dw_hdmi_imx_encoder_funcs = {
 	.destroy = drm_encoder_cleanup,
 };
 
+static enum drm_mode_status imx6q_hdmi_mode_valid(struct drm_connector *con,
+						  struct drm_display_mode *mode)
+{
+	if (mode->clock < 13500)
+		return MODE_CLOCK_LOW;
+	if (mode->clock > 266000)
+		return MODE_CLOCK_HIGH;
+
+	return MODE_OK;
+}
+
+static enum drm_mode_status imx6dl_hdmi_mode_valid(struct drm_connector *con,
+						   struct drm_display_mode *mode)
+{
+	if (mode->clock < 13500)
+		return MODE_CLOCK_LOW;
+	if (mode->clock > 270000)
+		return MODE_CLOCK_HIGH;
+
+	return MODE_OK;
+}
+
 static struct dw_hdmi_plat_data imx6q_hdmi_drv_data = {
-	.mpll_cfg = imx_mpll_cfg,
-	.cur_ctr  = imx_cur_ctr,
-	.sym_term = imx_sym_term,
-	.dev_type = IMX6Q_HDMI,
+	.mpll_cfg   = imx_mpll_cfg,
+	.cur_ctr    = imx_cur_ctr,
+	.sym_term   = imx_sym_term,
+	.dev_type   = IMX6Q_HDMI,
+	.mode_valid = imx6q_hdmi_mode_valid,
 };
 
 static struct dw_hdmi_plat_data imx6dl_hdmi_drv_data = {
@@ -148,6 +173,7 @@ static struct dw_hdmi_plat_data imx6dl_hdmi_drv_data = {
 	.cur_ctr  = imx_cur_ctr,
 	.sym_term = imx_sym_term,
 	.dev_type = IMX6DL_HDMI,
+	.mode_valid = imx6dl_hdmi_mode_valid,
 };
 
 static const struct of_device_id dw_hdmi_imx_dt_ids[] = {
diff --git a/drivers/gpu/drm/imx/imx-ldb.c b/drivers/gpu/drm/imx/imx-ldb.c
index 1b86aac0b341..2d6dc94e1e64 100644
--- a/drivers/gpu/drm/imx/imx-ldb.c
+++ b/drivers/gpu/drm/imx/imx-ldb.c
@@ -163,22 +163,7 @@ static void imx_ldb_encoder_prepare(struct drm_encoder *encoder)
 {
 	struct imx_ldb_channel *imx_ldb_ch = enc_to_imx_ldb_ch(encoder);
 	struct imx_ldb *ldb = imx_ldb_ch->ldb;
-	struct drm_display_mode *mode = &encoder->crtc->hwmode;
 	u32 pixel_fmt;
-	unsigned long serial_clk;
-	unsigned long di_clk = mode->clock * 1000;
-	int mux = imx_drm_encoder_get_mux_id(imx_ldb_ch->child, encoder);
-
-	if (ldb->ldb_ctrl & LDB_SPLIT_MODE_EN) {
-		/* dual channel LVDS mode */
-		serial_clk = 3500UL * mode->clock;
-		imx_ldb_set_clock(ldb, mux, 0, serial_clk, di_clk);
-		imx_ldb_set_clock(ldb, mux, 1, serial_clk, di_clk);
-	} else {
-		serial_clk = 7000UL * mode->clock;
-		imx_ldb_set_clock(ldb, mux, imx_ldb_ch->chno, serial_clk,
-				di_clk);
-	}
 
 	switch (imx_ldb_ch->chno) {
 	case 0:
@@ -247,6 +232,9 @@ static void imx_ldb_encoder_mode_set(struct drm_encoder *encoder,
 	struct imx_ldb_channel *imx_ldb_ch = enc_to_imx_ldb_ch(encoder);
 	struct imx_ldb *ldb = imx_ldb_ch->ldb;
 	int dual = ldb->ldb_ctrl & LDB_SPLIT_MODE_EN;
+	unsigned long serial_clk;
+	unsigned long di_clk = mode->clock * 1000;
+	int mux = imx_drm_encoder_get_mux_id(imx_ldb_ch->child, encoder);
 
 	if (mode->clock > 170000) {
 		dev_warn(ldb->dev,
@@ -257,6 +245,16 @@ static void imx_ldb_encoder_mode_set(struct drm_encoder *encoder,
 			 "%s: mode exceeds 85 MHz pixel clock\n", __func__);
 	}
 
+	if (dual) {
+		serial_clk = 3500UL * mode->clock;
+		imx_ldb_set_clock(ldb, mux, 0, serial_clk, di_clk);
+		imx_ldb_set_clock(ldb, mux, 1, serial_clk, di_clk);
+	} else {
+		serial_clk = 7000UL * mode->clock;
+		imx_ldb_set_clock(ldb, mux, imx_ldb_ch->chno, serial_clk,
+				  di_clk);
+	}
+
 	/* FIXME - assumes straight connections DI0 --> CH0, DI1 --> CH1 */
 	if (imx_ldb_ch == &ldb->channel[0]) {
 		if (mode->flags & DRM_MODE_FLAG_NVSYNC)
diff --git a/drivers/gpu/drm/imx/parallel-display.c b/drivers/gpu/drm/imx/parallel-display.c
index 5e83e007080f..900dda6a8e71 100644
--- a/drivers/gpu/drm/imx/parallel-display.c
+++ b/drivers/gpu/drm/imx/parallel-display.c
@@ -236,8 +236,11 @@ static int imx_pd_bind(struct device *dev, struct device *master, void *data)
 	}
 
 	panel_node = of_parse_phandle(np, "fsl,panel", 0);
-	if (panel_node)
+	if (panel_node) {
 		imxpd->panel = of_drm_find_panel(panel_node);
+		if (!imxpd->panel)
+			return -EPROBE_DEFER;
+	}
 
 	imxpd->dev = dev;
 
diff --git a/drivers/gpu/drm/msm/edp/edp_ctrl.c b/drivers/gpu/drm/msm/edp/edp_ctrl.c
index 3e246210c46f..0ec5abdba5c4 100644
--- a/drivers/gpu/drm/msm/edp/edp_ctrl.c
+++ b/drivers/gpu/drm/msm/edp/edp_ctrl.c
@@ -332,7 +332,7 @@ static int edp_regulator_enable(struct edp_ctrl *ctrl)
 		goto vdda_set_fail;
 	}
 
-	ret = regulator_set_optimum_mode(ctrl->vdda_vreg, VDDA_UA_ON_LOAD);
+	ret = regulator_set_load(ctrl->vdda_vreg, VDDA_UA_ON_LOAD);
 	if (ret < 0) {
 		pr_err("%s: vdda_vreg set regulator mode failed.\n", __func__);
 		goto vdda_set_fail;
@@ -356,7 +356,7 @@ static int edp_regulator_enable(struct edp_ctrl *ctrl)
 lvl_enable_fail:
 	regulator_disable(ctrl->vdda_vreg);
 vdda_enable_fail:
-	regulator_set_optimum_mode(ctrl->vdda_vreg, VDDA_UA_OFF_LOAD);
+	regulator_set_load(ctrl->vdda_vreg, VDDA_UA_OFF_LOAD);
 vdda_set_fail:
 	return ret;
 }
@@ -365,7 +365,7 @@ static void edp_regulator_disable(struct edp_ctrl *ctrl)
 {
 	regulator_disable(ctrl->lvl_vreg);
 	regulator_disable(ctrl->vdda_vreg);
-	regulator_set_optimum_mode(ctrl->vdda_vreg, VDDA_UA_OFF_LOAD);
+	regulator_set_load(ctrl->vdda_vreg, VDDA_UA_OFF_LOAD);
 }
 
 static int edp_gpio_config(struct edp_ctrl *ctrl)
diff --git a/drivers/gpu/drm/msm/mdp/mdp4/mdp4_irq.c b/drivers/gpu/drm/msm/mdp/mdp4/mdp4_irq.c
index 8edd531cb621..7369ee7f0c55 100644
--- a/drivers/gpu/drm/msm/mdp/mdp4/mdp4_irq.c
+++ b/drivers/gpu/drm/msm/mdp/mdp4/mdp4_irq.c
@@ -32,7 +32,10 @@ static void mdp4_irq_error_handler(struct mdp_irq *irq, uint32_t irqstatus)
 void mdp4_irq_preinstall(struct msm_kms *kms)
 {
 	struct mdp4_kms *mdp4_kms = to_mdp4_kms(to_mdp_kms(kms));
+	mdp4_enable(mdp4_kms);
 	mdp4_write(mdp4_kms, REG_MDP4_INTR_CLEAR, 0xffffffff);
+	mdp4_write(mdp4_kms, REG_MDP4_INTR_ENABLE, 0x00000000);
+	mdp4_disable(mdp4_kms);
 }
 
 int mdp4_irq_postinstall(struct msm_kms *kms)
@@ -53,7 +56,9 @@ int mdp4_irq_postinstall(struct msm_kms *kms)
 void mdp4_irq_uninstall(struct msm_kms *kms)
 {
 	struct mdp4_kms *mdp4_kms = to_mdp4_kms(to_mdp_kms(kms));
+	mdp4_enable(mdp4_kms);
 	mdp4_write(mdp4_kms, REG_MDP4_INTR_ENABLE, 0x00000000);
+	mdp4_disable(mdp4_kms);
 }
 
 irqreturn_t mdp4_irq(struct msm_kms *kms)
diff --git a/drivers/gpu/drm/msm/mdp/mdp5/mdp5.xml.h b/drivers/gpu/drm/msm/mdp/mdp5/mdp5.xml.h
index 09b4a25eb553..c276624290af 100644
--- a/drivers/gpu/drm/msm/mdp/mdp5/mdp5.xml.h
+++ b/drivers/gpu/drm/msm/mdp/mdp5/mdp5.xml.h
@@ -8,17 +8,9 @@ http://github.com/freedreno/envytools/
 git clone https://github.com/freedreno/envytools.git
 
 The rules-ng-ng source files this header was generated from are:
-- /home/robclark/src/freedreno/envytools/rnndb/msm.xml                 (    676 bytes, from 2014-12-05 15:34:49)
-- /home/robclark/src/freedreno/envytools/rnndb/freedreno_copyright.xml (   1453 bytes, from 2013-03-31 16:51:27)
-- /home/robclark/src/freedreno/envytools/rnndb/mdp/mdp4.xml            (  20908 bytes, from 2014-12-08 16:13:00)
-- /home/robclark/src/freedreno/envytools/rnndb/mdp/mdp_common.xml      (   2357 bytes, from 2014-12-08 16:13:00)
-- /home/robclark/src/freedreno/envytools/rnndb/mdp/mdp5.xml            (  27208 bytes, from 2015-01-13 23:56:11)
-- /home/robclark/src/freedreno/envytools/rnndb/dsi/dsi.xml             (  11712 bytes, from 2013-08-17 17:13:43)
-- /home/robclark/src/freedreno/envytools/rnndb/dsi/sfpb.xml            (    344 bytes, from 2013-08-11 19:26:32)
-- /home/robclark/src/freedreno/envytools/rnndb/dsi/mmss_cc.xml         (   1686 bytes, from 2014-10-31 16:48:57)
-- /home/robclark/src/freedreno/envytools/rnndb/hdmi/qfprom.xml         (    600 bytes, from 2013-07-05 19:21:12)
-- /home/robclark/src/freedreno/envytools/rnndb/hdmi/hdmi.xml           (  26848 bytes, from 2015-01-13 23:55:57)
-- /home/robclark/src/freedreno/envytools/rnndb/edp/edp.xml             (   8253 bytes, from 2014-12-08 16:13:00)
+- /local/mnt2/workspace2/sviau/envytools/rnndb/mdp/mdp5.xml            (  27229 bytes, from 2015-02-10 17:00:41)
+- /local/mnt2/workspace2/sviau/envytools/rnndb/freedreno_copyright.xml (   1453 bytes, from 2014-06-02 18:31:15)
+- /local/mnt2/workspace2/sviau/envytools/rnndb/mdp/mdp_common.xml      (   2357 bytes, from 2015-01-23 16:20:19)
 
 Copyright (C) 2013-2015 by the following authors:
 - Rob Clark <robdclark@gmail.com> (robclark)
@@ -910,6 +902,7 @@ static inline uint32_t __offset_LM(uint32_t idx)
 		case 2: return (mdp5_cfg->lm.base[2]);
 		case 3: return (mdp5_cfg->lm.base[3]);
 		case 4: return (mdp5_cfg->lm.base[4]);
+		case 5: return (mdp5_cfg->lm.base[5]);
 		default: return INVALID_IDX(idx);
 	}
 }
diff --git a/drivers/gpu/drm/msm/mdp/mdp5/mdp5_crtc.c b/drivers/gpu/drm/msm/mdp/mdp5/mdp5_crtc.c
index 46fac545dc2b..2f2863cf8b45 100644
--- a/drivers/gpu/drm/msm/mdp/mdp5/mdp5_crtc.c
+++ b/drivers/gpu/drm/msm/mdp/mdp5/mdp5_crtc.c
@@ -62,8 +62,8 @@ struct mdp5_crtc {
 
 		/* current cursor being scanned out: */
 		struct drm_gem_object *scanout_bo;
-		uint32_t width;
-		uint32_t height;
+		uint32_t width, height;
+		uint32_t x, y;
 	} cursor;
 };
 #define to_mdp5_crtc(x) container_of(x, struct mdp5_crtc, base)
@@ -103,8 +103,8 @@ static void crtc_flush_all(struct drm_crtc *crtc)
 	struct drm_plane *plane;
 	uint32_t flush_mask = 0;
 
-	/* we could have already released CTL in the disable path: */
-	if (!mdp5_crtc->ctl)
+	/* this should not happen: */
+	if (WARN_ON(!mdp5_crtc->ctl))
 		return;
 
 	drm_atomic_crtc_for_each_plane(plane, crtc) {
@@ -143,6 +143,11 @@ static void complete_flip(struct drm_crtc *crtc, struct drm_file *file)
 	drm_atomic_crtc_for_each_plane(plane, crtc) {
 		mdp5_plane_complete_flip(plane);
 	}
+
+	if (mdp5_crtc->ctl && !crtc->state->enable) {
+		mdp5_ctl_release(mdp5_crtc->ctl);
+		mdp5_crtc->ctl = NULL;
+	}
 }
 
 static void unref_cursor_worker(struct drm_flip_work *work, void *val)
@@ -386,14 +391,17 @@ static void mdp5_crtc_atomic_flush(struct drm_crtc *crtc)
 	mdp5_crtc->event = crtc->state->event;
 	spin_unlock_irqrestore(&dev->event_lock, flags);
 
+	/*
+	 * If no CTL has been allocated in mdp5_crtc_atomic_check(),
+	 * it means we are trying to flush a CRTC whose state is disabled:
+	 * nothing else needs to be done.
+	 */
+	if (unlikely(!mdp5_crtc->ctl))
+		return;
+
 	blend_setup(crtc);
 	crtc_flush_all(crtc);
 	request_pending(crtc, PENDING_FLIP);
-
-	if (mdp5_crtc->ctl && !crtc->state->enable) {
-		mdp5_ctl_release(mdp5_crtc->ctl);
-		mdp5_crtc->ctl = NULL;
-	}
 }
 
 static int mdp5_crtc_set_property(struct drm_crtc *crtc,
@@ -403,6 +411,32 @@ static int mdp5_crtc_set_property(struct drm_crtc *crtc,
 	return -EINVAL;
 }
 
+static void get_roi(struct drm_crtc *crtc, uint32_t *roi_w, uint32_t *roi_h)
+{
+	struct mdp5_crtc *mdp5_crtc = to_mdp5_crtc(crtc);
+	uint32_t xres = crtc->mode.hdisplay;
+	uint32_t yres = crtc->mode.vdisplay;
+
+	/*
+	 * Cursor Region Of Interest (ROI) is a plane read from cursor
+	 * buffer to render. The ROI region is determined by the visibility of
+	 * the cursor point. In the default Cursor image the cursor point will
+	 * be at the top left of the cursor image, unless it is specified
+	 * otherwise using hotspot feature.
+	 *
+	 * If the cursor point reaches the right (xres - x < cursor.width) or
+	 * bottom (yres - y < cursor.height) boundary of the screen, then ROI
+	 * width and ROI height need to be evaluated to crop the cursor image
+	 * accordingly.
+	 * (xres-x) will be new cursor width when x > (xres - cursor.width)
+	 * (yres-y) will be new cursor height when y > (yres - cursor.height)
+	 */
+	*roi_w = min(mdp5_crtc->cursor.width, xres -
+			mdp5_crtc->cursor.x);
+	*roi_h = min(mdp5_crtc->cursor.height, yres -
+			mdp5_crtc->cursor.y);
+}
+
 static int mdp5_crtc_cursor_set(struct drm_crtc *crtc,
 		struct drm_file *file, uint32_t handle,
 		uint32_t width, uint32_t height)
@@ -416,6 +450,7 @@ static int mdp5_crtc_cursor_set(struct drm_crtc *crtc,
 	unsigned int depth;
 	enum mdp5_cursor_alpha cur_alpha = CURSOR_ALPHA_PER_PIXEL;
 	uint32_t flush_mask = mdp_ctl_flush_mask_cursor(0);
+	uint32_t roi_w, roi_h;
 	unsigned long flags;
 
 	if ((width > CURSOR_WIDTH) || (height > CURSOR_HEIGHT)) {
@@ -446,6 +481,12 @@ static int mdp5_crtc_cursor_set(struct drm_crtc *crtc,
 	spin_lock_irqsave(&mdp5_crtc->cursor.lock, flags);
 	old_bo = mdp5_crtc->cursor.scanout_bo;
 
+	mdp5_crtc->cursor.scanout_bo = cursor_bo;
+	mdp5_crtc->cursor.width = width;
+	mdp5_crtc->cursor.height = height;
+
+	get_roi(crtc, &roi_w, &roi_h);
+
 	mdp5_write(mdp5_kms, REG_MDP5_LM_CURSOR_STRIDE(lm), stride);
 	mdp5_write(mdp5_kms, REG_MDP5_LM_CURSOR_FORMAT(lm),
 			MDP5_LM_CURSOR_FORMAT_FORMAT(CURSOR_FMT_ARGB8888));
@@ -453,19 +494,14 @@ static int mdp5_crtc_cursor_set(struct drm_crtc *crtc,
 			MDP5_LM_CURSOR_IMG_SIZE_SRC_H(height) |
 			MDP5_LM_CURSOR_IMG_SIZE_SRC_W(width));
 	mdp5_write(mdp5_kms, REG_MDP5_LM_CURSOR_SIZE(lm),
-			MDP5_LM_CURSOR_SIZE_ROI_H(height) |
-			MDP5_LM_CURSOR_SIZE_ROI_W(width));
+			MDP5_LM_CURSOR_SIZE_ROI_H(roi_h) |
+			MDP5_LM_CURSOR_SIZE_ROI_W(roi_w));
 	mdp5_write(mdp5_kms, REG_MDP5_LM_CURSOR_BASE_ADDR(lm), cursor_addr);
 
-
 	blendcfg = MDP5_LM_CURSOR_BLEND_CONFIG_BLEND_EN;
-	blendcfg |= MDP5_LM_CURSOR_BLEND_CONFIG_BLEND_TRANSP_EN;
 	blendcfg |= MDP5_LM_CURSOR_BLEND_CONFIG_BLEND_ALPHA_SEL(cur_alpha);
 	mdp5_write(mdp5_kms, REG_MDP5_LM_CURSOR_BLEND_CONFIG(lm), blendcfg);
 
-	mdp5_crtc->cursor.scanout_bo = cursor_bo;
-	mdp5_crtc->cursor.width = width;
-	mdp5_crtc->cursor.height = height;
 	spin_unlock_irqrestore(&mdp5_crtc->cursor.lock, flags);
 
 	ret = mdp5_ctl_set_cursor(mdp5_crtc->ctl, true);
@@ -489,31 +525,18 @@ static int mdp5_crtc_cursor_move(struct drm_crtc *crtc, int x, int y)
 	struct mdp5_kms *mdp5_kms = get_kms(crtc);
 	struct mdp5_crtc *mdp5_crtc = to_mdp5_crtc(crtc);
 	uint32_t flush_mask = mdp_ctl_flush_mask_cursor(0);
-	uint32_t xres = crtc->mode.hdisplay;
-	uint32_t yres = crtc->mode.vdisplay;
 	uint32_t roi_w;
 	uint32_t roi_h;
 	unsigned long flags;
 
-	x = (x > 0) ? x : 0;
-	y = (y > 0) ? y : 0;
+	/* In case the CRTC is disabled, just drop the cursor update */
+	if (unlikely(!crtc->state->enable))
+		return 0;
 
-	/*
-	 * Cursor Region Of Interest (ROI) is a plane read from cursor
-	 * buffer to render. The ROI region is determined by the visiblity of
-	 * the cursor point. In the default Cursor image the cursor point will
-	 * be at the top left of the cursor image, unless it is specified
-	 * otherwise using hotspot feature.
-	 *
-	 * If the cursor point reaches the right (xres - x < cursor.width) or
-	 * bottom (yres - y < cursor.height) boundary of the screen, then ROI
-	 * width and ROI height need to be evaluated to crop the cursor image
-	 * accordingly.
-	 * (xres-x) will be new cursor width when x > (xres - cursor.width)
-	 * (yres-y) will be new cursor height when y > (yres - cursor.height)
-	 */
-	roi_w = min(mdp5_crtc->cursor.width, xres - x);
-	roi_h = min(mdp5_crtc->cursor.height, yres - y);
+	mdp5_crtc->cursor.x = x = max(x, 0);
+	mdp5_crtc->cursor.y = y = max(y, 0);
+
+	get_roi(crtc, &roi_w, &roi_h);
 
 	spin_lock_irqsave(&mdp5_crtc->cursor.lock, flags);
 	mdp5_write(mdp5_kms, REG_MDP5_LM_CURSOR_SIZE(mdp5_crtc->lm),
@@ -544,8 +567,8 @@ static const struct drm_crtc_funcs mdp5_crtc_funcs = {
 static const struct drm_crtc_helper_funcs mdp5_crtc_helper_funcs = {
 	.mode_fixup = mdp5_crtc_mode_fixup,
 	.mode_set_nofb = mdp5_crtc_mode_set_nofb,
-	.prepare = mdp5_crtc_disable,
-	.commit = mdp5_crtc_enable,
+	.disable = mdp5_crtc_disable,
+	.enable = mdp5_crtc_enable,
 	.atomic_check = mdp5_crtc_atomic_check,
 	.atomic_begin = mdp5_crtc_atomic_begin,
 	.atomic_flush = mdp5_crtc_atomic_flush,
diff --git a/drivers/gpu/drm/msm/mdp/mdp5/mdp5_encoder.c b/drivers/gpu/drm/msm/mdp/mdp5/mdp5_encoder.c
index d6a14bb99988..af0e02fa4f48 100644
--- a/drivers/gpu/drm/msm/mdp/mdp5/mdp5_encoder.c
+++ b/drivers/gpu/drm/msm/mdp/mdp5/mdp5_encoder.c
@@ -267,14 +267,14 @@ static void mdp5_encoder_enable(struct drm_encoder *encoder)
 	mdp5_write(mdp5_kms, REG_MDP5_INTF_TIMING_ENGINE_EN(intf), 1);
 	spin_unlock_irqrestore(&mdp5_encoder->intf_lock, flags);
 
-	mdp5_encoder->enabled = false;
+	mdp5_encoder->enabled = true;
 }
 
 static const struct drm_encoder_helper_funcs mdp5_encoder_helper_funcs = {
 	.mode_fixup = mdp5_encoder_mode_fixup,
 	.mode_set = mdp5_encoder_mode_set,
-	.prepare = mdp5_encoder_disable,
-	.commit = mdp5_encoder_enable,
+	.disable = mdp5_encoder_disable,
+	.enable = mdp5_encoder_enable,
 };
 
 /* initialize encoder */
diff --git a/drivers/gpu/drm/msm/mdp/mdp5/mdp5_irq.c b/drivers/gpu/drm/msm/mdp/mdp5/mdp5_irq.c
index 70ac81edd40f..a9407105b9b7 100644
--- a/drivers/gpu/drm/msm/mdp/mdp5/mdp5_irq.c
+++ b/drivers/gpu/drm/msm/mdp/mdp5/mdp5_irq.c
@@ -34,7 +34,10 @@ static void mdp5_irq_error_handler(struct mdp_irq *irq, uint32_t irqstatus)
 void mdp5_irq_preinstall(struct msm_kms *kms)
 {
 	struct mdp5_kms *mdp5_kms = to_mdp5_kms(to_mdp_kms(kms));
+	mdp5_enable(mdp5_kms);
 	mdp5_write(mdp5_kms, REG_MDP5_INTR_CLEAR, 0xffffffff);
+	mdp5_write(mdp5_kms, REG_MDP5_INTR_EN, 0x00000000);
+	mdp5_disable(mdp5_kms);
 }
 
 int mdp5_irq_postinstall(struct msm_kms *kms)
@@ -57,7 +60,9 @@ int mdp5_irq_postinstall(struct msm_kms *kms)
 void mdp5_irq_uninstall(struct msm_kms *kms)
 {
 	struct mdp5_kms *mdp5_kms = to_mdp5_kms(to_mdp_kms(kms));
+	mdp5_enable(mdp5_kms);
 	mdp5_write(mdp5_kms, REG_MDP5_INTR_EN, 0x00000000);
+	mdp5_disable(mdp5_kms);
 }
 
 static void mdp5_irq_mdp(struct mdp_kms *mdp_kms)
diff --git a/drivers/gpu/drm/msm/msm_atomic.c b/drivers/gpu/drm/msm/msm_atomic.c
index 871aa2108dc6..18fd643b6e69 100644
--- a/drivers/gpu/drm/msm/msm_atomic.c
+++ b/drivers/gpu/drm/msm/msm_atomic.c
@@ -219,8 +219,10 @@ int msm_atomic_commit(struct drm_device *dev,
 	 * mark our set of crtc's as busy:
 	 */
 	ret = start_atomic(dev->dev_private, c->crtc_mask);
-	if (ret)
+	if (ret) {
+		kfree(c);
 		return ret;
+	}
 
 	/*
 	 * This is the point of no return - everything below never fails except
diff --git a/drivers/gpu/drm/nouveau/nouveau_fbcon.c b/drivers/gpu/drm/nouveau/nouveau_fbcon.c
index 79924e4b1b49..6751553abe4a 100644
--- a/drivers/gpu/drm/nouveau/nouveau_fbcon.c
+++ b/drivers/gpu/drm/nouveau/nouveau_fbcon.c
@@ -418,7 +418,7 @@ nouveau_fbcon_create(struct drm_fb_helper *helper,
 	nouveau_fbcon_zfill(dev, fbcon);
 
 	/* To allow resizeing without swapping buffers */
-	NV_INFO(drm, "allocated %dx%d fb: 0x%lx, bo %p\n",
+	NV_INFO(drm, "allocated %dx%d fb: 0x%llx, bo %p\n",
 		nouveau_fb->base.width, nouveau_fb->base.height,
 		nvbo->bo.offset, nvbo);
 
diff --git a/drivers/gpu/drm/nouveau/nvkm/engine/device/base.c b/drivers/gpu/drm/nouveau/nvkm/engine/device/base.c
index 29bd539af183..6efa8f38ff54 100644
--- a/drivers/gpu/drm/nouveau/nvkm/engine/device/base.c
+++ b/drivers/gpu/drm/nouveau/nvkm/engine/device/base.c
@@ -340,11 +340,13 @@ nvkm_devobj_ctor(struct nvkm_object *parent, struct nvkm_object *engine,
 
 		/* switch mmio to cpu's native endianness */
 #ifndef __BIG_ENDIAN
-		if (ioread32_native(map + 0x000004) != 0x00000000)
+		if (ioread32_native(map + 0x000004) != 0x00000000) {
 #else
-		if (ioread32_native(map + 0x000004) == 0x00000000)
+		if (ioread32_native(map + 0x000004) == 0x00000000) {
 #endif
 			iowrite32_native(0x01000001, map + 0x000004);
+			ioread32_native(map);
+		}
 
 		/* read boot0 and strapping information */
 		boot0 = ioread32_native(map + 0x000000);
diff --git a/drivers/gpu/drm/nouveau/nvkm/engine/device/gm100.c b/drivers/gpu/drm/nouveau/nvkm/engine/device/gm100.c
index 539561ed3281..108d048da764 100644
--- a/drivers/gpu/drm/nouveau/nvkm/engine/device/gm100.c
+++ b/drivers/gpu/drm/nouveau/nvkm/engine/device/gm100.c
@@ -142,6 +142,49 @@ gm100_identify(struct nvkm_device *device)
 		device->oclass[NVDEV_ENGINE_MSPPP  ] = &gf100_msppp_oclass;
 #endif
 		break;
+	case 0x126:
+		device->cname = "GM206";
+		device->oclass[NVDEV_SUBDEV_VBIOS  ] = &nvkm_bios_oclass;
+		device->oclass[NVDEV_SUBDEV_GPIO   ] =  gk104_gpio_oclass;
+		device->oclass[NVDEV_SUBDEV_I2C    ] =  gm204_i2c_oclass;
+		device->oclass[NVDEV_SUBDEV_FUSE   ] = &gm107_fuse_oclass;
+#if 0
+		/* looks to be some non-trivial changes */
+		device->oclass[NVDEV_SUBDEV_CLK    ] = &gk104_clk_oclass;
+		/* priv ring says no to 0x10eb14 writes */
+		device->oclass[NVDEV_SUBDEV_THERM  ] = &gm107_therm_oclass;
+#endif
+		device->oclass[NVDEV_SUBDEV_MXM    ] = &nv50_mxm_oclass;
+		device->oclass[NVDEV_SUBDEV_DEVINIT] =  gm204_devinit_oclass;
+		device->oclass[NVDEV_SUBDEV_MC     ] =  gk20a_mc_oclass;
+		device->oclass[NVDEV_SUBDEV_BUS    ] =  gf100_bus_oclass;
+		device->oclass[NVDEV_SUBDEV_TIMER  ] = &gk20a_timer_oclass;
+		device->oclass[NVDEV_SUBDEV_FB     ] =  gm107_fb_oclass;
+		device->oclass[NVDEV_SUBDEV_LTC    ] =  gm107_ltc_oclass;
+		device->oclass[NVDEV_SUBDEV_IBUS   ] = &gk104_ibus_oclass;
+		device->oclass[NVDEV_SUBDEV_INSTMEM] =  nv50_instmem_oclass;
+		device->oclass[NVDEV_SUBDEV_MMU    ] = &gf100_mmu_oclass;
+		device->oclass[NVDEV_SUBDEV_BAR    ] = &gf100_bar_oclass;
+		device->oclass[NVDEV_SUBDEV_PMU    ] =  gk208_pmu_oclass;
+#if 0
+		device->oclass[NVDEV_SUBDEV_VOLT   ] = &nv40_volt_oclass;
+#endif
+		device->oclass[NVDEV_ENGINE_DMAOBJ ] =  gf110_dmaeng_oclass;
+#if 0
+		device->oclass[NVDEV_ENGINE_FIFO   ] =  gk208_fifo_oclass;
+		device->oclass[NVDEV_ENGINE_SW     ] =  gf100_sw_oclass;
+		device->oclass[NVDEV_ENGINE_GR     ] =  gm107_gr_oclass;
+#endif
+		device->oclass[NVDEV_ENGINE_DISP   ] =  gm204_disp_oclass;
+#if 0
+		device->oclass[NVDEV_ENGINE_CE0    ] = &gm204_ce0_oclass;
+		device->oclass[NVDEV_ENGINE_CE1    ] = &gm204_ce1_oclass;
+		device->oclass[NVDEV_ENGINE_CE2    ] = &gm204_ce2_oclass;
+		device->oclass[NVDEV_ENGINE_MSVLD  ] = &gk104_msvld_oclass;
+		device->oclass[NVDEV_ENGINE_MSPDEC ] = &gk104_mspdec_oclass;
+		device->oclass[NVDEV_ENGINE_MSPPP  ] = &gf100_msppp_oclass;
+#endif
+		break;
 	default:
 		nv_fatal(device, "unknown Maxwell chipset\n");
 		return -EINVAL;
diff --git a/drivers/gpu/drm/nouveau/nvkm/engine/fifo/nv04.c b/drivers/gpu/drm/nouveau/nvkm/engine/fifo/nv04.c
index b038b6eb51db..043e4296084c 100644
--- a/drivers/gpu/drm/nouveau/nvkm/engine/fifo/nv04.c
+++ b/drivers/gpu/drm/nouveau/nvkm/engine/fifo/nv04.c
@@ -502,72 +502,57 @@ nv04_fifo_intr(struct nvkm_subdev *subdev)
 {
 	struct nvkm_device *device = nv_device(subdev);
 	struct nv04_fifo_priv *priv = (void *)subdev;
-	uint32_t status, reassign;
-	int cnt = 0;
+	u32 mask = nv_rd32(priv, NV03_PFIFO_INTR_EN_0);
+	u32 stat = nv_rd32(priv, NV03_PFIFO_INTR_0) & mask;
+	u32 reassign, chid, get, sem;
 
 	reassign = nv_rd32(priv, NV03_PFIFO_CACHES) & 1;
-	while ((status = nv_rd32(priv, NV03_PFIFO_INTR_0)) && (cnt++ < 100)) {
-		uint32_t chid, get;
-
-		nv_wr32(priv, NV03_PFIFO_CACHES, 0);
-
-		chid = nv_rd32(priv, NV03_PFIFO_CACHE1_PUSH1) & priv->base.max;
-		get  = nv_rd32(priv, NV03_PFIFO_CACHE1_GET);
+	nv_wr32(priv, NV03_PFIFO_CACHES, 0);
 
-		if (status & NV_PFIFO_INTR_CACHE_ERROR) {
-			nv04_fifo_cache_error(device, priv, chid, get);
-			status &= ~NV_PFIFO_INTR_CACHE_ERROR;
-		}
+	chid = nv_rd32(priv, NV03_PFIFO_CACHE1_PUSH1) & priv->base.max;
+	get  = nv_rd32(priv, NV03_PFIFO_CACHE1_GET);
 
-		if (status & NV_PFIFO_INTR_DMA_PUSHER) {
-			nv04_fifo_dma_pusher(device, priv, chid);
-			status &= ~NV_PFIFO_INTR_DMA_PUSHER;
-		}
+	if (stat & NV_PFIFO_INTR_CACHE_ERROR) {
+		nv04_fifo_cache_error(device, priv, chid, get);
+		stat &= ~NV_PFIFO_INTR_CACHE_ERROR;
+	}
 
-		if (status & NV_PFIFO_INTR_SEMAPHORE) {
-			uint32_t sem;
+	if (stat & NV_PFIFO_INTR_DMA_PUSHER) {
+		nv04_fifo_dma_pusher(device, priv, chid);
+		stat &= ~NV_PFIFO_INTR_DMA_PUSHER;
+	}
 
-			status &= ~NV_PFIFO_INTR_SEMAPHORE;
-			nv_wr32(priv, NV03_PFIFO_INTR_0,
-				NV_PFIFO_INTR_SEMAPHORE);
+	if (stat & NV_PFIFO_INTR_SEMAPHORE) {
+		stat &= ~NV_PFIFO_INTR_SEMAPHORE;
+		nv_wr32(priv, NV03_PFIFO_INTR_0, NV_PFIFO_INTR_SEMAPHORE);
 
-			sem = nv_rd32(priv, NV10_PFIFO_CACHE1_SEMAPHORE);
-			nv_wr32(priv, NV10_PFIFO_CACHE1_SEMAPHORE, sem | 0x1);
+		sem = nv_rd32(priv, NV10_PFIFO_CACHE1_SEMAPHORE);
+		nv_wr32(priv, NV10_PFIFO_CACHE1_SEMAPHORE, sem | 0x1);
 
-			nv_wr32(priv, NV03_PFIFO_CACHE1_GET, get + 4);
-			nv_wr32(priv, NV04_PFIFO_CACHE1_PULL0, 1);
-		}
+		nv_wr32(priv, NV03_PFIFO_CACHE1_GET, get + 4);
+		nv_wr32(priv, NV04_PFIFO_CACHE1_PULL0, 1);
+	}
 
-		if (device->card_type == NV_50) {
-			if (status & 0x00000010) {
-				status &= ~0x00000010;
-				nv_wr32(priv, 0x002100, 0x00000010);
-			}
-
-			if (status & 0x40000000) {
-				nv_wr32(priv, 0x002100, 0x40000000);
-				nvkm_fifo_uevent(&priv->base);
-				status &= ~0x40000000;
-			}
+	if (device->card_type == NV_50) {
+		if (stat & 0x00000010) {
+			stat &= ~0x00000010;
+			nv_wr32(priv, 0x002100, 0x00000010);
 		}
 
-		if (status) {
-			nv_warn(priv, "unknown intr 0x%08x, ch %d\n",
-				status, chid);
-			nv_wr32(priv, NV03_PFIFO_INTR_0, status);
-			status = 0;
+		if (stat & 0x40000000) {
+			nv_wr32(priv, 0x002100, 0x40000000);
+			nvkm_fifo_uevent(&priv->base);
+			stat &= ~0x40000000;
 		}
-
-		nv_wr32(priv, NV03_PFIFO_CACHES, reassign);
 	}
 
-	if (status) {
-		nv_error(priv, "still angry after %d spins, halt\n", cnt);
-		nv_wr32(priv, 0x002140, 0);
-		nv_wr32(priv, 0x000140, 0);
+	if (stat) {
+		nv_warn(priv, "unknown intr 0x%08x\n", stat);
+		nv_mask(priv, NV03_PFIFO_INTR_EN_0, stat, 0x00000000);
+		nv_wr32(priv, NV03_PFIFO_INTR_0, stat);
 	}
 
-	nv_wr32(priv, 0x000100, 0x00000100);
+	nv_wr32(priv, NV03_PFIFO_CACHES, reassign);
 }
 
 static int
diff --git a/drivers/gpu/drm/nouveau/nvkm/engine/gr/ctxgf100.c b/drivers/gpu/drm/nouveau/nvkm/engine/gr/ctxgf100.c
index 2e7ec389eea7..57e2c5b13123 100644
--- a/drivers/gpu/drm/nouveau/nvkm/engine/gr/ctxgf100.c
+++ b/drivers/gpu/drm/nouveau/nvkm/engine/gr/ctxgf100.c
@@ -1032,9 +1032,9 @@ gf100_grctx_generate_bundle(struct gf100_grctx *info)
 	const int s = 8;
 	const int b = mmio_vram(info, impl->bundle_size, (1 << s), access);
 	mmio_refn(info, 0x408004, 0x00000000, s, b);
-	mmio_refn(info, 0x408008, 0x80000000 | (impl->bundle_size >> s), 0, b);
+	mmio_wr32(info, 0x408008, 0x80000000 | (impl->bundle_size >> s));
 	mmio_refn(info, 0x418808, 0x00000000, s, b);
-	mmio_refn(info, 0x41880c, 0x80000000 | (impl->bundle_size >> s), 0, b);
+	mmio_wr32(info, 0x41880c, 0x80000000 | (impl->bundle_size >> s));
 }
 
 void
diff --git a/drivers/gpu/drm/nouveau/nvkm/engine/gr/ctxgk104.c b/drivers/gpu/drm/nouveau/nvkm/engine/gr/ctxgk104.c
index b52300d8861a..5e9454ba158f 100644
--- a/drivers/gpu/drm/nouveau/nvkm/engine/gr/ctxgk104.c
+++ b/drivers/gpu/drm/nouveau/nvkm/engine/gr/ctxgk104.c
@@ -851,9 +851,9 @@ gk104_grctx_generate_bundle(struct gf100_grctx *info)
 	const int s = 8;
 	const int b = mmio_vram(info, impl->bundle_size, (1 << s), access);
 	mmio_refn(info, 0x408004, 0x00000000, s, b);
-	mmio_refn(info, 0x408008, 0x80000000 | (impl->bundle_size >> s), 0, b);
+	mmio_wr32(info, 0x408008, 0x80000000 | (impl->bundle_size >> s));
 	mmio_refn(info, 0x418808, 0x00000000, s, b);
-	mmio_refn(info, 0x41880c, 0x80000000 | (impl->bundle_size >> s), 0, b);
+	mmio_wr32(info, 0x41880c, 0x80000000 | (impl->bundle_size >> s));
 	mmio_wr32(info, 0x4064c8, (state_limit << 16) | token_limit);
 }
 
diff --git a/drivers/gpu/drm/nouveau/nvkm/engine/gr/ctxgm107.c b/drivers/gpu/drm/nouveau/nvkm/engine/gr/ctxgm107.c
index 956f4dce960c..b2fae6e389e2 100644
--- a/drivers/gpu/drm/nouveau/nvkm/engine/gr/ctxgm107.c
+++ b/drivers/gpu/drm/nouveau/nvkm/engine/gr/ctxgm107.c
@@ -871,9 +871,9 @@ gm107_grctx_generate_bundle(struct gf100_grctx *info)
 	const int s = 8;
 	const int b = mmio_vram(info, impl->bundle_size, (1 << s), access);
 	mmio_refn(info, 0x408004, 0x00000000, s, b);
-	mmio_refn(info, 0x408008, 0x80000000 | (impl->bundle_size >> s), 0, b);
+	mmio_wr32(info, 0x408008, 0x80000000 | (impl->bundle_size >> s));
 	mmio_refn(info, 0x418e24, 0x00000000, s, b);
-	mmio_refn(info, 0x418e28, 0x80000000 | (impl->bundle_size >> s), 0, b);
+	mmio_wr32(info, 0x418e28, 0x80000000 | (impl->bundle_size >> s));
 	mmio_wr32(info, 0x4064c8, (state_limit << 16) | token_limit);
 }
 
diff --git a/drivers/gpu/drm/nouveau/nvkm/subdev/bios/i2c.c b/drivers/gpu/drm/nouveau/nvkm/subdev/bios/i2c.c
index d1a89b2bd5c1..c4e1f085ee10 100644
--- a/drivers/gpu/drm/nouveau/nvkm/subdev/bios/i2c.c
+++ b/drivers/gpu/drm/nouveau/nvkm/subdev/bios/i2c.c
@@ -74,7 +74,11 @@ dcb_i2c_parse(struct nvkm_bios *bios, u8 idx, struct dcb_i2c_entry *info)
 	u16 ent = dcb_i2c_entry(bios, idx, &ver, &len);
 	if (ent) {
 		if (ver >= 0x41) {
-			if (!(nv_ro32(bios, ent) & 0x80000000))
+			u32 ent_value = nv_ro32(bios, ent);
+			u8 i2c_port = (ent_value >> 27) & 0x1f;
+			u8 dpaux_port = (ent_value >> 22) & 0x1f;
+			/* value 0x1f means unused according to DCB 4.x spec */
+			if (i2c_port == 0x1f && dpaux_port == 0x1f)
 				info->type = DCB_I2C_UNUSED;
 			else
 				info->type = DCB_I2C_PMGR;
diff --git a/drivers/gpu/drm/radeon/atombios_crtc.c b/drivers/gpu/drm/radeon/atombios_crtc.c
index ed644a4f6f57..86807ee91bd1 100644
--- a/drivers/gpu/drm/radeon/atombios_crtc.c
+++ b/drivers/gpu/drm/radeon/atombios_crtc.c
@@ -1405,6 +1405,9 @@ static int dce4_crtc_do_set_base(struct drm_crtc *crtc,
 	       (x << 16) | y);
 	viewport_w = crtc->mode.hdisplay;
 	viewport_h = (crtc->mode.vdisplay + 1) & ~1;
+	if ((rdev->family >= CHIP_BONAIRE) &&
+	    (crtc->mode.flags & DRM_MODE_FLAG_INTERLACE))
+		viewport_h *= 2;
 	WREG32(EVERGREEN_VIEWPORT_SIZE + radeon_crtc->crtc_offset,
 	       (viewport_w << 16) | viewport_h);
 
diff --git a/drivers/gpu/drm/radeon/atombios_dp.c b/drivers/gpu/drm/radeon/atombios_dp.c
index 5bf825dfaa09..8d74de82456e 100644
--- a/drivers/gpu/drm/radeon/atombios_dp.c
+++ b/drivers/gpu/drm/radeon/atombios_dp.c
@@ -178,6 +178,13 @@ radeon_dp_aux_transfer(struct drm_dp_aux *aux, struct drm_dp_aux_msg *msg)
 	switch (msg->request & ~DP_AUX_I2C_MOT) {
 	case DP_AUX_NATIVE_WRITE:
 	case DP_AUX_I2C_WRITE:
+		/* The atom implementation only supports writes with a max payload of
+		 * 12 bytes since it uses 4 bits for the total count (header + payload)
+		 * in the parameter space.  The atom interface supports 16 byte
+		 * payloads for reads. The hw itself supports up to 16 bytes of payload.
+		 */
+		if (WARN_ON_ONCE(msg->size > 12))
+			return -E2BIG;
 		/* tx_size needs to be 4 even for bare address packets since the atom
 		 * table needs the info in tx_buf[3].
 		 */
diff --git a/drivers/gpu/drm/radeon/atombios_encoders.c b/drivers/gpu/drm/radeon/atombios_encoders.c
index 7c9df1eac065..c39c1d0d9d4e 100644
--- a/drivers/gpu/drm/radeon/atombios_encoders.c
+++ b/drivers/gpu/drm/radeon/atombios_encoders.c
@@ -731,7 +731,9 @@ atombios_get_encoder_mode(struct drm_encoder *encoder)
 		dig_connector = radeon_connector->con_priv;
 		if ((dig_connector->dp_sink_type == CONNECTOR_OBJECT_ID_DISPLAYPORT) ||
 		    (dig_connector->dp_sink_type == CONNECTOR_OBJECT_ID_eDP)) {
-			if (radeon_audio != 0 && ASIC_IS_DCE4(rdev) && !ASIC_IS_DCE5(rdev))
+			if (radeon_audio != 0 &&
+			    drm_detect_monitor_audio(radeon_connector_edid(connector)) &&
+			    ASIC_IS_DCE4(rdev) && !ASIC_IS_DCE5(rdev))
 				return ATOM_ENCODER_MODE_DP_AUDIO;
 			return ATOM_ENCODER_MODE_DP;
 		} else if (radeon_audio != 0) {
@@ -747,7 +749,9 @@ atombios_get_encoder_mode(struct drm_encoder *encoder)
 		}
 		break;
 	case DRM_MODE_CONNECTOR_eDP:
-		if (radeon_audio != 0 && ASIC_IS_DCE4(rdev) && !ASIC_IS_DCE5(rdev))
+		if (radeon_audio != 0 &&
+		    drm_detect_monitor_audio(radeon_connector_edid(connector)) &&
+		    ASIC_IS_DCE4(rdev) && !ASIC_IS_DCE5(rdev))
 			return ATOM_ENCODER_MODE_DP_AUDIO;
 		return ATOM_ENCODER_MODE_DP;
 	case DRM_MODE_CONNECTOR_DVIA:
@@ -1622,7 +1626,6 @@ radeon_atom_encoder_dpms_dig(struct drm_encoder *encoder, int mode)
 	struct radeon_connector *radeon_connector = NULL;
 	struct radeon_connector_atom_dig *radeon_dig_connector = NULL;
 	bool travis_quirk = false;
-	int encoder_mode;
 
 	if (connector) {
 		radeon_connector = to_radeon_connector(connector);
@@ -1718,11 +1721,6 @@ radeon_atom_encoder_dpms_dig(struct drm_encoder *encoder, int mode)
 		}
 		break;
 	}
-
-	encoder_mode = atombios_get_encoder_mode(encoder);
-	if (radeon_audio != 0 &&
-		(encoder_mode == ATOM_ENCODER_MODE_HDMI || ENCODER_MODE_IS_DP(encoder_mode)))
-		radeon_audio_dpms(encoder, mode);
 }
 
 static void
@@ -1731,10 +1729,19 @@ radeon_atom_encoder_dpms(struct drm_encoder *encoder, int mode)
 	struct drm_device *dev = encoder->dev;
 	struct radeon_device *rdev = dev->dev_private;
 	struct radeon_encoder *radeon_encoder = to_radeon_encoder(encoder);
+	struct drm_connector *connector = radeon_get_connector_for_encoder(encoder);
+	int encoder_mode = atombios_get_encoder_mode(encoder);
 
 	DRM_DEBUG_KMS("encoder dpms %d to mode %d, devices %08x, active_devices %08x\n",
 		  radeon_encoder->encoder_id, mode, radeon_encoder->devices,
 		  radeon_encoder->active_device);
+
+	if (connector && (radeon_audio != 0) &&
+	    ((encoder_mode == ATOM_ENCODER_MODE_HDMI) ||
+	     (ENCODER_MODE_IS_DP(encoder_mode) &&
+	      drm_detect_monitor_audio(radeon_connector_edid(connector)))))
+		radeon_audio_dpms(encoder, mode);
+
 	switch (radeon_encoder->encoder_id) {
 	case ENCODER_OBJECT_ID_INTERNAL_TMDS1:
 	case ENCODER_OBJECT_ID_INTERNAL_KLDSCP_TMDS1:
@@ -2136,6 +2143,7 @@ radeon_atom_encoder_mode_set(struct drm_encoder *encoder,
 	struct drm_device *dev = encoder->dev;
 	struct radeon_device *rdev = dev->dev_private;
 	struct radeon_encoder *radeon_encoder = to_radeon_encoder(encoder);
+	struct drm_connector *connector = radeon_get_connector_for_encoder(encoder);
 	int encoder_mode;
 
 	radeon_encoder->pixel_clock = adjusted_mode->clock;
@@ -2163,10 +2171,6 @@ radeon_atom_encoder_mode_set(struct drm_encoder *encoder,
 	case ENCODER_OBJECT_ID_INTERNAL_UNIPHY3:
 	case ENCODER_OBJECT_ID_INTERNAL_KLDSCP_LVTMA:
 		/* handled in dpms */
-		encoder_mode = atombios_get_encoder_mode(encoder);
-		if (radeon_audio != 0 &&
-			(encoder_mode == ATOM_ENCODER_MODE_HDMI || ENCODER_MODE_IS_DP(encoder_mode)))
-			radeon_audio_mode_set(encoder, adjusted_mode);
 		break;
 	case ENCODER_OBJECT_ID_INTERNAL_DDI:
 	case ENCODER_OBJECT_ID_INTERNAL_DVO1:
@@ -2188,6 +2192,13 @@ radeon_atom_encoder_mode_set(struct drm_encoder *encoder,
 	}
 
 	atombios_apply_encoder_quirks(encoder, adjusted_mode);
+
+	encoder_mode = atombios_get_encoder_mode(encoder);
+	if (connector && (radeon_audio != 0) &&
+	    ((encoder_mode == ATOM_ENCODER_MODE_HDMI) ||
+	     (ENCODER_MODE_IS_DP(encoder_mode) &&
+	      drm_detect_monitor_audio(radeon_connector_edid(connector)))))
+		radeon_audio_mode_set(encoder, adjusted_mode);
 }
 
 static bool
diff --git a/drivers/gpu/drm/radeon/cik.c b/drivers/gpu/drm/radeon/cik.c
index e6a4ba236c70..3e670d344a20 100644
--- a/drivers/gpu/drm/radeon/cik.c
+++ b/drivers/gpu/drm/radeon/cik.c
@@ -3613,6 +3613,8 @@ static void cik_gpu_init(struct radeon_device *rdev)
 	}
 
 	WREG32(GRBM_CNTL, GRBM_READ_TIMEOUT(0xff));
+	WREG32(SRBM_INT_CNTL, 0x1);
+	WREG32(SRBM_INT_ACK, 0x1);
 
 	WREG32(BIF_FB_EN, FB_READ_EN | FB_WRITE_EN);
 
@@ -7230,6 +7232,8 @@ static void cik_disable_interrupt_state(struct radeon_device *rdev)
 	WREG32(CP_ME2_PIPE3_INT_CNTL, 0);
 	/* grbm */
 	WREG32(GRBM_INT_CNTL, 0);
+	/* SRBM */
+	WREG32(SRBM_INT_CNTL, 0);
 	/* vline/vblank, etc. */
 	WREG32(LB_INTERRUPT_MASK + EVERGREEN_CRTC0_REGISTER_OFFSET, 0);
 	WREG32(LB_INTERRUPT_MASK + EVERGREEN_CRTC1_REGISTER_OFFSET, 0);
@@ -7551,6 +7555,9 @@ int cik_irq_set(struct radeon_device *rdev)
 	WREG32(DC_HPD5_INT_CONTROL, hpd5);
 	WREG32(DC_HPD6_INT_CONTROL, hpd6);
 
+	/* posting read */
+	RREG32(SRBM_STATUS);
+
 	return 0;
 }
 
@@ -8046,6 +8053,10 @@ restart_ih:
 				break;
 			}
 			break;
+		case 96:
+			DRM_ERROR("SRBM_READ_ERROR: 0x%x\n", RREG32(SRBM_READ_ERROR));
+			WREG32(SRBM_INT_ACK, 0x1);
+			break;
 		case 124: /* UVD */
 			DRM_DEBUG("IH: UVD int: 0x%08x\n", src_data);
 			radeon_fence_process(rdev, R600_RING_TYPE_UVD_INDEX);
diff --git a/drivers/gpu/drm/radeon/cikd.h b/drivers/gpu/drm/radeon/cikd.h
index 03003f8a6de6..243a36c93b8f 100644
--- a/drivers/gpu/drm/radeon/cikd.h
+++ b/drivers/gpu/drm/radeon/cikd.h
@@ -482,6 +482,10 @@
 #define		SOFT_RESET_ORB				(1 << 23)
 #define		SOFT_RESET_VCE				(1 << 24)
 
+#define SRBM_READ_ERROR					0xE98
+#define SRBM_INT_CNTL					0xEA0
+#define SRBM_INT_ACK					0xEA8
+
 #define VM_L2_CNTL					0x1400
 #define		ENABLE_L2_CACHE					(1 << 0)
 #define		ENABLE_L2_FRAGMENT_PROCESSING			(1 << 1)
@@ -2125,6 +2129,7 @@
 #define VCE_UENC_REG_CLOCK_GATING	0x207c0
 #define VCE_SYS_INT_EN			0x21300
 #	define VCE_SYS_INT_TRAP_INTERRUPT_EN	(1 << 3)
+#define VCE_LMI_VCPU_CACHE_40BIT_BAR	0x2145c
 #define VCE_LMI_CTRL2			0x21474
 #define VCE_LMI_CTRL			0x21498
 #define VCE_LMI_VM_CTRL			0x214a0
diff --git a/drivers/gpu/drm/radeon/dce6_afmt.c b/drivers/gpu/drm/radeon/dce6_afmt.c
index 192c80389151..3adc2afe32aa 100644
--- a/drivers/gpu/drm/radeon/dce6_afmt.c
+++ b/drivers/gpu/drm/radeon/dce6_afmt.c
@@ -26,6 +26,9 @@
 #include "radeon_audio.h"
 #include "sid.h"
 
+#define DCE8_DCCG_AUDIO_DTO1_PHASE	0x05b8
+#define DCE8_DCCG_AUDIO_DTO1_MODULE	0x05bc
+
 u32 dce6_endpoint_rreg(struct radeon_device *rdev,
 			      u32 block_offset, u32 reg)
 {
@@ -252,72 +255,67 @@ void dce6_audio_enable(struct radeon_device *rdev,
 void dce6_hdmi_audio_set_dto(struct radeon_device *rdev,
 	struct radeon_crtc *crtc, unsigned int clock)
 {
-    /* Two dtos; generally use dto0 for HDMI */
+	/* Two dtos; generally use dto0 for HDMI */
 	u32 value = 0;
 
-    if (crtc)
+	if (crtc)
 		value |= DCCG_AUDIO_DTO0_SOURCE_SEL(crtc->crtc_id);
 
 	WREG32(DCCG_AUDIO_DTO_SOURCE, value);
 
-    /* Express [24MHz / target pixel clock] as an exact rational
-     * number (coefficient of two integer numbers.  DCCG_AUDIO_DTOx_PHASE
-     * is the numerator, DCCG_AUDIO_DTOx_MODULE is the denominator
-     */
-    WREG32(DCCG_AUDIO_DTO0_PHASE, 24000);
-    WREG32(DCCG_AUDIO_DTO0_MODULE, clock);
+	/* Express [24MHz / target pixel clock] as an exact rational
+	 * number (coefficient of two integer numbers.  DCCG_AUDIO_DTOx_PHASE
+	 * is the numerator, DCCG_AUDIO_DTOx_MODULE is the denominator
+	 */
+	WREG32(DCCG_AUDIO_DTO0_PHASE, 24000);
+	WREG32(DCCG_AUDIO_DTO0_MODULE, clock);
 }
 
 void dce6_dp_audio_set_dto(struct radeon_device *rdev,
 	struct radeon_crtc *crtc, unsigned int clock)
 {
-    /* Two dtos; generally use dto1 for DP */
+	/* Two dtos; generally use dto1 for DP */
 	u32 value = 0;
 	value |= DCCG_AUDIO_DTO_SEL;
 
-    if (crtc)
+	if (crtc)
 		value |= DCCG_AUDIO_DTO0_SOURCE_SEL(crtc->crtc_id);
 
 	WREG32(DCCG_AUDIO_DTO_SOURCE, value);
 
-    /* Express [24MHz / target pixel clock] as an exact rational
-     * number (coefficient of two integer numbers.  DCCG_AUDIO_DTOx_PHASE
-     * is the numerator, DCCG_AUDIO_DTOx_MODULE is the denominator
-     */
-    WREG32(DCCG_AUDIO_DTO1_PHASE, 24000);
-    WREG32(DCCG_AUDIO_DTO1_MODULE, clock);
+	/* Express [24MHz / target pixel clock] as an exact rational
+	 * number (coefficient of two integer numbers.  DCCG_AUDIO_DTOx_PHASE
+	 * is the numerator, DCCG_AUDIO_DTOx_MODULE is the denominator
+	 */
+	if (ASIC_IS_DCE8(rdev)) {
+		WREG32(DCE8_DCCG_AUDIO_DTO1_PHASE, 24000);
+		WREG32(DCE8_DCCG_AUDIO_DTO1_MODULE, clock);
+	} else {
+		WREG32(DCCG_AUDIO_DTO1_PHASE, 24000);
+		WREG32(DCCG_AUDIO_DTO1_MODULE, clock);
+	}
 }
 
-void dce6_enable_dp_audio_packets(struct drm_encoder *encoder, bool enable)
+void dce6_dp_enable(struct drm_encoder *encoder, bool enable)
 {
 	struct drm_device *dev = encoder->dev;
 	struct radeon_device *rdev = dev->dev_private;
 	struct radeon_encoder *radeon_encoder = to_radeon_encoder(encoder);
 	struct radeon_encoder_atom_dig *dig = radeon_encoder->enc_priv;
-	uint32_t offset;
 
 	if (!dig || !dig->afmt)
 		return;
 
-	offset = dig->afmt->offset;
-
 	if (enable) {
-        if (dig->afmt->enabled)
-            return;
-
-		WREG32(EVERGREEN_DP_SEC_TIMESTAMP + offset, EVERGREEN_DP_SEC_TIMESTAMP_MODE(1));
-		WREG32(EVERGREEN_DP_SEC_CNTL + offset,
-			EVERGREEN_DP_SEC_ASP_ENABLE |		/* Audio packet transmission */
-			EVERGREEN_DP_SEC_ATP_ENABLE |		/* Audio timestamp packet transmission */
-			EVERGREEN_DP_SEC_AIP_ENABLE |		/* Audio infoframe packet transmission */
-			EVERGREEN_DP_SEC_STREAM_ENABLE);	/* Master enable for secondary stream engine */
-		radeon_audio_enable(rdev, dig->afmt->pin, true);
+		WREG32(EVERGREEN_DP_SEC_TIMESTAMP + dig->afmt->offset,
+		       EVERGREEN_DP_SEC_TIMESTAMP_MODE(1));
+		WREG32(EVERGREEN_DP_SEC_CNTL + dig->afmt->offset,
+		       EVERGREEN_DP_SEC_ASP_ENABLE |		/* Audio packet transmission */
+		       EVERGREEN_DP_SEC_ATP_ENABLE |		/* Audio timestamp packet transmission */
+		       EVERGREEN_DP_SEC_AIP_ENABLE |		/* Audio infoframe packet transmission */
+		       EVERGREEN_DP_SEC_STREAM_ENABLE);	/* Master enable for secondary stream engine */
 	} else {
-		if (!dig->afmt->enabled)
-			return;
-
-		WREG32(EVERGREEN_DP_SEC_CNTL + offset, 0);
-		radeon_audio_enable(rdev, dig->afmt->pin, false);
+		WREG32(EVERGREEN_DP_SEC_CNTL + dig->afmt->offset, 0);
 	}
 
 	dig->afmt->enabled = enable;
diff --git a/drivers/gpu/drm/radeon/evergreen.c b/drivers/gpu/drm/radeon/evergreen.c
index 78600f534c80..973df064c14f 100644
--- a/drivers/gpu/drm/radeon/evergreen.c
+++ b/drivers/gpu/drm/radeon/evergreen.c
@@ -3253,6 +3253,8 @@ static void evergreen_gpu_init(struct radeon_device *rdev)
 	}
 
 	WREG32(GRBM_CNTL, GRBM_READ_TIMEOUT(0xff));
+	WREG32(SRBM_INT_CNTL, 0x1);
+	WREG32(SRBM_INT_ACK, 0x1);
 
 	evergreen_fix_pci_max_read_req_size(rdev);
 
@@ -4324,6 +4326,7 @@ void evergreen_disable_interrupt_state(struct radeon_device *rdev)
 	tmp = RREG32(DMA_CNTL) & ~TRAP_ENABLE;
 	WREG32(DMA_CNTL, tmp);
 	WREG32(GRBM_INT_CNTL, 0);
+	WREG32(SRBM_INT_CNTL, 0);
 	WREG32(INT_MASK + EVERGREEN_CRTC0_REGISTER_OFFSET, 0);
 	WREG32(INT_MASK + EVERGREEN_CRTC1_REGISTER_OFFSET, 0);
 	if (rdev->num_crtc >= 4) {
@@ -4590,6 +4593,9 @@ int evergreen_irq_set(struct radeon_device *rdev)
 	WREG32(AFMT_AUDIO_PACKET_CONTROL + EVERGREEN_CRTC4_REGISTER_OFFSET, afmt5);
 	WREG32(AFMT_AUDIO_PACKET_CONTROL + EVERGREEN_CRTC5_REGISTER_OFFSET, afmt6);
 
+	/* posting read */
+	RREG32(SRBM_STATUS);
+
 	return 0;
 }
 
@@ -5066,6 +5072,10 @@ restart_ih:
 				DRM_ERROR("Unhandled interrupt: %d %d\n", src_id, src_data);
 				break;
 			}
+		case 96:
+			DRM_ERROR("SRBM_READ_ERROR: 0x%x\n", RREG32(SRBM_READ_ERROR));
+			WREG32(SRBM_INT_ACK, 0x1);
+			break;
 		case 124: /* UVD */
 			DRM_DEBUG("IH: UVD int: 0x%08x\n", src_data);
 			radeon_fence_process(rdev, R600_RING_TYPE_UVD_INDEX);
diff --git a/drivers/gpu/drm/radeon/evergreen_hdmi.c b/drivers/gpu/drm/radeon/evergreen_hdmi.c
index 1d9aebc79595..c18d4ecbd95d 100644
--- a/drivers/gpu/drm/radeon/evergreen_hdmi.c
+++ b/drivers/gpu/drm/radeon/evergreen_hdmi.c
@@ -272,7 +272,7 @@ void dce4_hdmi_audio_set_dto(struct radeon_device *rdev,
 }
 
 void dce4_dp_audio_set_dto(struct radeon_device *rdev,
-	struct radeon_crtc *crtc, unsigned int clock)
+			   struct radeon_crtc *crtc, unsigned int clock)
 {
 	u32 value;
 
@@ -294,7 +294,7 @@ void dce4_dp_audio_set_dto(struct radeon_device *rdev,
 	 * is the numerator, DCCG_AUDIO_DTOx_MODULE is the denominator
 	 */
 	WREG32(DCCG_AUDIO_DTO1_PHASE, 24000);
-	WREG32(DCCG_AUDIO_DTO1_MODULE, rdev->clock.max_pixel_clock * 10);
+	WREG32(DCCG_AUDIO_DTO1_MODULE, clock);
 }
 
 void dce4_set_vbi_packet(struct drm_encoder *encoder, u32 offset)
@@ -350,20 +350,9 @@ void dce4_set_audio_packet(struct drm_encoder *encoder, u32 offset)
 	struct drm_device *dev = encoder->dev;
 	struct radeon_device *rdev = dev->dev_private;
 
-	WREG32(HDMI_INFOFRAME_CONTROL0 + offset,
-		HDMI_AUDIO_INFO_SEND | /* enable audio info frames (frames won't be set until audio is enabled) */
-		HDMI_AUDIO_INFO_CONT); /* required for audio info values to be updated */
-
 	WREG32(AFMT_INFOFRAME_CONTROL0 + offset,
 		AFMT_AUDIO_INFO_UPDATE); /* required for audio info values to be updated */
 
-	WREG32(HDMI_INFOFRAME_CONTROL1 + offset,
-		HDMI_AUDIO_INFO_LINE(2)); /* anything other than 0 */
-
-	WREG32(HDMI_AUDIO_PACKET_CONTROL + offset,
-		HDMI_AUDIO_DELAY_EN(1) | /* set the default audio delay */
-		HDMI_AUDIO_PACKETS_PER_LINE(3)); /* should be suffient for all audio modes and small enough for all hblanks */
-
 	WREG32(AFMT_60958_0 + offset,
 		AFMT_60958_CS_CHANNEL_NUMBER_L(1));
 
@@ -408,15 +397,19 @@ void evergreen_hdmi_enable(struct drm_encoder *encoder, bool enable)
 	if (!dig || !dig->afmt)
 		return;
 
-	/* Silent, r600_hdmi_enable will raise WARN for us */
-	if (enable && dig->afmt->enabled)
-		return;
-	if (!enable && !dig->afmt->enabled)
-		return;
+	if (enable) {
+		WREG32(HDMI_INFOFRAME_CONTROL1 + dig->afmt->offset,
+		       HDMI_AUDIO_INFO_LINE(2)); /* anything other than 0 */
+
+		WREG32(HDMI_AUDIO_PACKET_CONTROL + dig->afmt->offset,
+		       HDMI_AUDIO_DELAY_EN(1) | /* set the default audio delay */
+		       HDMI_AUDIO_PACKETS_PER_LINE(3)); /* should be suffient for all audio modes and small enough for all hblanks */
 
-	if (!enable && dig->afmt->pin) {
-		radeon_audio_enable(rdev, dig->afmt->pin, 0);
-		dig->afmt->pin = NULL;
+		WREG32(HDMI_INFOFRAME_CONTROL0 + dig->afmt->offset,
+		       HDMI_AUDIO_INFO_SEND | /* enable audio info frames (frames won't be set until audio is enabled) */
+		       HDMI_AUDIO_INFO_CONT); /* required for audio info values to be updated */
+	} else {
+		WREG32(HDMI_INFOFRAME_CONTROL0 + dig->afmt->offset, 0);
 	}
 
 	dig->afmt->enabled = enable;
@@ -425,33 +418,28 @@ void evergreen_hdmi_enable(struct drm_encoder *encoder, bool enable)
 		  enable ? "En" : "Dis", dig->afmt->offset, radeon_encoder->encoder_id);
 }
 
-void evergreen_enable_dp_audio_packets(struct drm_encoder *encoder, bool enable)
+void evergreen_dp_enable(struct drm_encoder *encoder, bool enable)
 {
 	struct drm_device *dev = encoder->dev;
 	struct radeon_device *rdev = dev->dev_private;
 	struct radeon_encoder *radeon_encoder = to_radeon_encoder(encoder);
 	struct radeon_encoder_atom_dig *dig = radeon_encoder->enc_priv;
-	uint32_t offset;
 
 	if (!dig || !dig->afmt)
 		return;
 
-	offset = dig->afmt->offset;
-
 	if (enable) {
 		struct drm_connector *connector = radeon_get_connector_for_encoder(encoder);
 		struct radeon_connector *radeon_connector = to_radeon_connector(connector);
 		struct radeon_connector_atom_dig *dig_connector;
 		uint32_t val;
 
-		if (dig->afmt->enabled)
-			return;
-
-		WREG32(EVERGREEN_DP_SEC_TIMESTAMP + offset, EVERGREEN_DP_SEC_TIMESTAMP_MODE(1));
+		WREG32(EVERGREEN_DP_SEC_TIMESTAMP + dig->afmt->offset,
+		       EVERGREEN_DP_SEC_TIMESTAMP_MODE(1));
 
 		if (radeon_connector->con_priv) {
 			dig_connector = radeon_connector->con_priv;
-			val = RREG32(EVERGREEN_DP_SEC_AUD_N + offset);
+			val = RREG32(EVERGREEN_DP_SEC_AUD_N + dig->afmt->offset);
 			val &= ~EVERGREEN_DP_SEC_N_BASE_MULTIPLE(0xf);
 
 			if (dig_connector->dp_clock == 162000)
@@ -459,21 +447,16 @@ void evergreen_enable_dp_audio_packets(struct drm_encoder *encoder, bool enable)
 			else
 				val |= EVERGREEN_DP_SEC_N_BASE_MULTIPLE(5);
 
-			WREG32(EVERGREEN_DP_SEC_AUD_N + offset, val);
+			WREG32(EVERGREEN_DP_SEC_AUD_N + dig->afmt->offset, val);
 		}
 
-		WREG32(EVERGREEN_DP_SEC_CNTL + offset,
+		WREG32(EVERGREEN_DP_SEC_CNTL + dig->afmt->offset,
 			EVERGREEN_DP_SEC_ASP_ENABLE |		/* Audio packet transmission */
 			EVERGREEN_DP_SEC_ATP_ENABLE |		/* Audio timestamp packet transmission */
 			EVERGREEN_DP_SEC_AIP_ENABLE |		/* Audio infoframe packet transmission */
 			EVERGREEN_DP_SEC_STREAM_ENABLE);	/* Master enable for secondary stream engine */
-		radeon_audio_enable(rdev, dig->afmt->pin, 0xf);
 	} else {
-		if (!dig->afmt->enabled)
-			return;
-
-		WREG32(EVERGREEN_DP_SEC_CNTL + offset, 0);
-		radeon_audio_enable(rdev, dig->afmt->pin, 0);
+		WREG32(EVERGREEN_DP_SEC_CNTL + dig->afmt->offset, 0);
 	}
 
 	dig->afmt->enabled = enable;
diff --git a/drivers/gpu/drm/radeon/evergreend.h b/drivers/gpu/drm/radeon/evergreend.h
index ee83d2a88750..a8d1d5240fcb 100644
--- a/drivers/gpu/drm/radeon/evergreend.h
+++ b/drivers/gpu/drm/radeon/evergreend.h
@@ -1191,6 +1191,10 @@
 #define		SOFT_RESET_REGBB			(1 << 22)
 #define		SOFT_RESET_ORB				(1 << 23)
 
+#define SRBM_READ_ERROR					0xE98
+#define SRBM_INT_CNTL					0xEA0
+#define SRBM_INT_ACK					0xEA8
+
 /* display watermarks */
 #define	DC_LB_MEMORY_SPLIT				  0x6b0c
 #define	PRIORITY_A_CNT			                  0x6b18
diff --git a/drivers/gpu/drm/radeon/ni.c b/drivers/gpu/drm/radeon/ni.c
index 24242a7f0ac3..dab00812abaa 100644
--- a/drivers/gpu/drm/radeon/ni.c
+++ b/drivers/gpu/drm/radeon/ni.c
@@ -962,6 +962,8 @@ static void cayman_gpu_init(struct radeon_device *rdev)
 	}
 
 	WREG32(GRBM_CNTL, GRBM_READ_TIMEOUT(0xff));
+	WREG32(SRBM_INT_CNTL, 0x1);
+	WREG32(SRBM_INT_ACK, 0x1);
 
 	evergreen_fix_pci_max_read_req_size(rdev);
 
@@ -1086,12 +1088,12 @@ static void cayman_gpu_init(struct radeon_device *rdev)
 
 	if ((rdev->config.cayman.max_backends_per_se == 1) &&
 	    (rdev->flags & RADEON_IS_IGP)) {
-		if ((disabled_rb_mask & 3) == 1) {
-			/* RB0 disabled, RB1 enabled */
-			tmp = 0x11111111;
-		} else {
+		if ((disabled_rb_mask & 3) == 2) {
 			/* RB1 disabled, RB0 enabled */
 			tmp = 0x00000000;
+		} else {
+			/* RB0 disabled, RB1 enabled */
+			tmp = 0x11111111;
 		}
 	} else {
 		tmp = gb_addr_config & NUM_PIPES_MASK;
diff --git a/drivers/gpu/drm/radeon/nid.h b/drivers/gpu/drm/radeon/nid.h
index ad7125486894..6b44580440d0 100644
--- a/drivers/gpu/drm/radeon/nid.h
+++ b/drivers/gpu/drm/radeon/nid.h
@@ -82,6 +82,10 @@
 #define		SOFT_RESET_REGBB			(1 << 22)
 #define		SOFT_RESET_ORB				(1 << 23)
 
+#define SRBM_READ_ERROR					0xE98
+#define SRBM_INT_CNTL					0xEA0
+#define SRBM_INT_ACK					0xEA8
+
 #define	SRBM_STATUS2				        0x0EC4
 #define		DMA_BUSY 				(1 << 5)
 #define		DMA1_BUSY 				(1 << 6)
diff --git a/drivers/gpu/drm/radeon/r100.c b/drivers/gpu/drm/radeon/r100.c
index 279801ca5110..04f2514f7564 100644
--- a/drivers/gpu/drm/radeon/r100.c
+++ b/drivers/gpu/drm/radeon/r100.c
@@ -728,6 +728,10 @@ int r100_irq_set(struct radeon_device *rdev)
 		tmp |= RADEON_FP2_DETECT_MASK;
 	}
 	WREG32(RADEON_GEN_INT_CNTL, tmp);
+
+	/* read back to post the write */
+	RREG32(RADEON_GEN_INT_CNTL);
+
 	return 0;
 }
 
diff --git a/drivers/gpu/drm/radeon/r600.c b/drivers/gpu/drm/radeon/r600.c
index 07a71a2488c9..2fcad344492f 100644
--- a/drivers/gpu/drm/radeon/r600.c
+++ b/drivers/gpu/drm/radeon/r600.c
@@ -3784,6 +3784,9 @@ int r600_irq_set(struct radeon_device *rdev)
 		WREG32(RV770_CG_THERMAL_INT, thermal_int);
 	}
 
+	/* posting read */
+	RREG32(R_000E50_SRBM_STATUS);
+
 	return 0;
 }
 
diff --git a/drivers/gpu/drm/radeon/r600_dpm.c b/drivers/gpu/drm/radeon/r600_dpm.c
index 843b65f46ece..fa2154493cf1 100644
--- a/drivers/gpu/drm/radeon/r600_dpm.c
+++ b/drivers/gpu/drm/radeon/r600_dpm.c
@@ -188,7 +188,7 @@ u32 r600_dpm_get_vrefresh(struct radeon_device *rdev)
 		list_for_each_entry(crtc, &dev->mode_config.crtc_list, head) {
 			radeon_crtc = to_radeon_crtc(crtc);
 			if (crtc->enabled && radeon_crtc->enabled && radeon_crtc->hw_mode.clock) {
-				vrefresh = radeon_crtc->hw_mode.vrefresh;
+				vrefresh = drm_mode_vrefresh(&radeon_crtc->hw_mode);
 				break;
 			}
 		}
diff --git a/drivers/gpu/drm/radeon/r600_hdmi.c b/drivers/gpu/drm/radeon/r600_hdmi.c
index 62c91ed669ce..dd6606b8e23c 100644
--- a/drivers/gpu/drm/radeon/r600_hdmi.c
+++ b/drivers/gpu/drm/radeon/r600_hdmi.c
@@ -476,17 +476,6 @@ void r600_hdmi_enable(struct drm_encoder *encoder, bool enable)
 	if (!dig || !dig->afmt)
 		return;
 
-	/* Silent, r600_hdmi_enable will raise WARN for us */
-	if (enable && dig->afmt->enabled)
-		return;
-	if (!enable && !dig->afmt->enabled)
-		return;
-
-	if (!enable && dig->afmt->pin) {
-		radeon_audio_enable(rdev, dig->afmt->pin, 0);
-		dig->afmt->pin = NULL;
-	}
-
 	/* Older chipsets require setting HDMI and routing manually */
 	if (!ASIC_IS_DCE3(rdev)) {
 		if (enable)
diff --git a/drivers/gpu/drm/radeon/radeon.h b/drivers/gpu/drm/radeon/radeon.h
index 5587603b4a89..33d5a4f4eebd 100644
--- a/drivers/gpu/drm/radeon/radeon.h
+++ b/drivers/gpu/drm/radeon/radeon.h
@@ -1565,6 +1565,7 @@ struct radeon_dpm {
 	int			new_active_crtc_count;
 	u32			current_active_crtcs;
 	int			current_active_crtc_count;
+	bool single_display;
 	struct radeon_dpm_dynamic_state dyn_state;
 	struct radeon_dpm_fan fan;
 	u32 tdp_limit;
diff --git a/drivers/gpu/drm/radeon/radeon_audio.c b/drivers/gpu/drm/radeon/radeon_audio.c
index a3ceef6d9632..b21ef69a34ac 100644
--- a/drivers/gpu/drm/radeon/radeon_audio.c
+++ b/drivers/gpu/drm/radeon/radeon_audio.c
@@ -101,8 +101,8 @@ static void radeon_audio_dp_mode_set(struct drm_encoder *encoder,
 	struct drm_display_mode *mode);
 void r600_hdmi_enable(struct drm_encoder *encoder, bool enable);
 void evergreen_hdmi_enable(struct drm_encoder *encoder, bool enable);
-void evergreen_enable_dp_audio_packets(struct drm_encoder *encoder, bool enable);
-void dce6_enable_dp_audio_packets(struct drm_encoder *encoder, bool enable);
+void evergreen_dp_enable(struct drm_encoder *encoder, bool enable);
+void dce6_dp_enable(struct drm_encoder *encoder, bool enable);
 
 static const u32 pin_offsets[7] =
 {
@@ -210,7 +210,7 @@ static struct radeon_audio_funcs dce4_dp_funcs = {
 	.set_avi_packet = evergreen_set_avi_packet,
 	.set_audio_packet = dce4_set_audio_packet,
 	.mode_set = radeon_audio_dp_mode_set,
-	.dpms = evergreen_enable_dp_audio_packets,
+	.dpms = evergreen_dp_enable,
 };
 
 static struct radeon_audio_funcs dce6_hdmi_funcs = {
@@ -240,7 +240,7 @@ static struct radeon_audio_funcs dce6_dp_funcs = {
 	.set_avi_packet = evergreen_set_avi_packet,
 	.set_audio_packet = dce4_set_audio_packet,
 	.mode_set = radeon_audio_dp_mode_set,
-	.dpms = dce6_enable_dp_audio_packets,
+	.dpms = dce6_dp_enable,
 };
 
 static void radeon_audio_interface_init(struct radeon_device *rdev)
@@ -452,7 +452,7 @@ void radeon_audio_enable(struct radeon_device *rdev,
 }
 
 void radeon_audio_detect(struct drm_connector *connector,
-	enum drm_connector_status status)
+			 enum drm_connector_status status)
 {
 	struct radeon_device *rdev;
 	struct radeon_encoder *radeon_encoder;
@@ -483,14 +483,11 @@ void radeon_audio_detect(struct drm_connector *connector,
 		else
 			radeon_encoder->audio = rdev->audio.hdmi_funcs;
 
-		radeon_audio_write_speaker_allocation(connector->encoder);
-		radeon_audio_write_sad_regs(connector->encoder);
-		if (connector->encoder->crtc)
-			radeon_audio_write_latency_fields(connector->encoder,
-				&connector->encoder->crtc->mode);
+		dig->afmt->pin = radeon_audio_get_pin(connector->encoder);
 		radeon_audio_enable(rdev, dig->afmt->pin, 0xf);
 	} else {
 		radeon_audio_enable(rdev, dig->afmt->pin, 0);
+		dig->afmt->pin = NULL;
 	}
 }
 
@@ -694,23 +691,22 @@ static void radeon_audio_set_mute(struct drm_encoder *encoder, bool mute)
  * update the info frames with the data from the current display mode
  */
 static void radeon_audio_hdmi_mode_set(struct drm_encoder *encoder,
-	struct drm_display_mode *mode)
+				       struct drm_display_mode *mode)
 {
-	struct radeon_device *rdev = encoder->dev->dev_private;
 	struct radeon_encoder *radeon_encoder = to_radeon_encoder(encoder);
 	struct radeon_encoder_atom_dig *dig = radeon_encoder->enc_priv;
 
 	if (!dig || !dig->afmt)
 		return;
 
-	/* disable audio prior to setting up hw */
-	dig->afmt->pin = radeon_audio_get_pin(encoder);
-	radeon_audio_enable(rdev, dig->afmt->pin, 0);
+	radeon_audio_set_mute(encoder, true);
 
+	radeon_audio_write_speaker_allocation(encoder);
+	radeon_audio_write_sad_regs(encoder);
+	radeon_audio_write_latency_fields(encoder, mode);
 	radeon_audio_set_dto(encoder, mode->clock);
 	radeon_audio_set_vbi_packet(encoder);
 	radeon_hdmi_set_color_depth(encoder);
-	radeon_audio_set_mute(encoder, false);
 	radeon_audio_update_acr(encoder, mode->clock);
 	radeon_audio_set_audio_packet(encoder);
 	radeon_audio_select_pin(encoder);
@@ -718,8 +714,7 @@ static void radeon_audio_hdmi_mode_set(struct drm_encoder *encoder,
 	if (radeon_audio_set_avi_packet(encoder, mode) < 0)
 		return;
 
-	/* enable audio after to setting up hw */
-	radeon_audio_enable(rdev, dig->afmt->pin, 0xf);
+	radeon_audio_set_mute(encoder, false);
 }
 
 static void radeon_audio_dp_mode_set(struct drm_encoder *encoder,
@@ -729,23 +724,26 @@ static void radeon_audio_dp_mode_set(struct drm_encoder *encoder,
 	struct radeon_device *rdev = dev->dev_private;
 	struct radeon_encoder *radeon_encoder = to_radeon_encoder(encoder);
 	struct radeon_encoder_atom_dig *dig = radeon_encoder->enc_priv;
+	struct drm_connector *connector = radeon_get_connector_for_encoder(encoder);
+	struct radeon_connector *radeon_connector = to_radeon_connector(connector);
+	struct radeon_connector_atom_dig *dig_connector =
+		radeon_connector->con_priv;
 
 	if (!dig || !dig->afmt)
 		return;
 
-	/* disable audio prior to setting up hw */
-	dig->afmt->pin = radeon_audio_get_pin(encoder);
-	radeon_audio_enable(rdev, dig->afmt->pin, 0);
-
-	radeon_audio_set_dto(encoder, rdev->clock.default_dispclk * 10);
+	radeon_audio_write_speaker_allocation(encoder);
+	radeon_audio_write_sad_regs(encoder);
+	radeon_audio_write_latency_fields(encoder, mode);
+	if (rdev->clock.dp_extclk || ASIC_IS_DCE5(rdev))
+		radeon_audio_set_dto(encoder, rdev->clock.default_dispclk * 10);
+	else
+		radeon_audio_set_dto(encoder, dig_connector->dp_clock);
 	radeon_audio_set_audio_packet(encoder);
 	radeon_audio_select_pin(encoder);
 
 	if (radeon_audio_set_avi_packet(encoder, mode) < 0)
 		return;
-
-	/* enable audio after to setting up hw */
-	radeon_audio_enable(rdev, dig->afmt->pin, 0xf);
 }
 
 void radeon_audio_mode_set(struct drm_encoder *encoder,
diff --git a/drivers/gpu/drm/radeon/radeon_bios.c b/drivers/gpu/drm/radeon/radeon_bios.c
index 63ccb8fa799c..d27e4ccb848c 100644
--- a/drivers/gpu/drm/radeon/radeon_bios.c
+++ b/drivers/gpu/drm/radeon/radeon_bios.c
@@ -76,7 +76,7 @@ static bool igp_read_bios_from_vram(struct radeon_device *rdev)
 
 static bool radeon_read_bios(struct radeon_device *rdev)
 {
-	uint8_t __iomem *bios;
+	uint8_t __iomem *bios, val1, val2;
 	size_t size;
 
 	rdev->bios = NULL;
@@ -86,15 +86,19 @@ static bool radeon_read_bios(struct radeon_device *rdev)
 		return false;
 	}
 
-	if (size == 0 || bios[0] != 0x55 || bios[1] != 0xaa) {
+	val1 = readb(&bios[0]);
+	val2 = readb(&bios[1]);
+
+	if (size == 0 || val1 != 0x55 || val2 != 0xaa) {
 		pci_unmap_rom(rdev->pdev, bios);
 		return false;
 	}
-	rdev->bios = kmemdup(bios, size, GFP_KERNEL);
+	rdev->bios = kzalloc(size, GFP_KERNEL);
 	if (rdev->bios == NULL) {
 		pci_unmap_rom(rdev->pdev, bios);
 		return false;
 	}
+	memcpy_fromio(rdev->bios, bios, size);
 	pci_unmap_rom(rdev->pdev, bios);
 	return true;
 }
diff --git a/drivers/gpu/drm/radeon/radeon_cs.c b/drivers/gpu/drm/radeon/radeon_cs.c
index c830863bc98a..4d0f96cc3da4 100644
--- a/drivers/gpu/drm/radeon/radeon_cs.c
+++ b/drivers/gpu/drm/radeon/radeon_cs.c
@@ -256,11 +256,13 @@ int radeon_cs_parser_init(struct radeon_cs_parser *p, void *data)
 	u32 ring = RADEON_CS_RING_GFX;
 	s32 priority = 0;
 
+	INIT_LIST_HEAD(&p->validated);
+
 	if (!cs->num_chunks) {
 		return 0;
 	}
+
 	/* get chunks */
-	INIT_LIST_HEAD(&p->validated);
 	p->idx = 0;
 	p->ib.sa_bo = NULL;
 	p->const_ib.sa_bo = NULL;
@@ -715,6 +717,7 @@ int radeon_cs_packet_parse(struct radeon_cs_parser *p,
 	struct radeon_cs_chunk *ib_chunk = p->chunk_ib;
 	struct radeon_device *rdev = p->rdev;
 	uint32_t header;
+	int ret = 0, i;
 
 	if (idx >= ib_chunk->length_dw) {
 		DRM_ERROR("Can not parse packet at %d after CS end %d !\n",
@@ -743,14 +746,25 @@ int radeon_cs_packet_parse(struct radeon_cs_parser *p,
 		break;
 	default:
 		DRM_ERROR("Unknown packet type %d at %d !\n", pkt->type, idx);
-		return -EINVAL;
+		ret = -EINVAL;
+		goto dump_ib;
 	}
 	if ((pkt->count + 1 + pkt->idx) >= ib_chunk->length_dw) {
 		DRM_ERROR("Packet (%d:%d:%d) end after CS buffer (%d) !\n",
 			  pkt->idx, pkt->type, pkt->count, ib_chunk->length_dw);
-		return -EINVAL;
+		ret = -EINVAL;
+		goto dump_ib;
 	}
 	return 0;
+
+dump_ib:
+	for (i = 0; i < ib_chunk->length_dw; i++) {
+		if (i == idx)
+			printk("\t0x%08x <---\n", radeon_get_ib_value(p, i));
+		else
+			printk("\t0x%08x\n", radeon_get_ib_value(p, i));
+	}
+	return ret;
 }
 
 /**
diff --git a/drivers/gpu/drm/radeon/radeon_encoders.c b/drivers/gpu/drm/radeon/radeon_encoders.c
index 6b670b0bc47b..3a297037cc17 100644
--- a/drivers/gpu/drm/radeon/radeon_encoders.c
+++ b/drivers/gpu/drm/radeon/radeon_encoders.c
@@ -179,9 +179,12 @@ static void radeon_encoder_add_backlight(struct radeon_encoder *radeon_encoder,
 		    (rdev->pdev->subsystem_vendor == 0x1734) &&
 		    (rdev->pdev->subsystem_device == 0x1107))
 			use_bl = false;
+/* Older PPC macs use on-GPU backlight controller */
+#ifndef CONFIG_PPC_PMAC
 		/* disable native backlight control on older asics */
 		else if (rdev->family < CHIP_R600)
 			use_bl = false;
+#endif
 		else
 			use_bl = true;
 	}
diff --git a/drivers/gpu/drm/radeon/radeon_fence.c b/drivers/gpu/drm/radeon/radeon_fence.c
index d13d1b5a859f..df09ca7c4889 100644
--- a/drivers/gpu/drm/radeon/radeon_fence.c
+++ b/drivers/gpu/drm/radeon/radeon_fence.c
@@ -1030,37 +1030,59 @@ static inline bool radeon_test_signaled(struct radeon_fence *fence)
 	return test_bit(FENCE_FLAG_SIGNALED_BIT, &fence->base.flags);
 }
 
+struct radeon_wait_cb {
+	struct fence_cb base;
+	struct task_struct *task;
+};
+
+static void
+radeon_fence_wait_cb(struct fence *fence, struct fence_cb *cb)
+{
+	struct radeon_wait_cb *wait =
+		container_of(cb, struct radeon_wait_cb, base);
+
+	wake_up_process(wait->task);
+}
+
 static signed long radeon_fence_default_wait(struct fence *f, bool intr,
 					     signed long t)
 {
 	struct radeon_fence *fence = to_radeon_fence(f);
 	struct radeon_device *rdev = fence->rdev;
-	bool signaled;
+	struct radeon_wait_cb cb;
 
-	fence_enable_sw_signaling(&fence->base);
+	cb.task = current;
 
-	/*
-	 * This function has to return -EDEADLK, but cannot hold
-	 * exclusive_lock during the wait because some callers
-	 * may already hold it. This means checking needs_reset without
-	 * lock, and not fiddling with any gpu internals.
-	 *
-	 * The callback installed with fence_enable_sw_signaling will
-	 * run before our wait_event_*timeout call, so we will see
-	 * both the signaled fence and the changes to needs_reset.
-	 */
+	if (fence_add_callback(f, &cb.base, radeon_fence_wait_cb))
+		return t;
+
+	while (t > 0) {
+		if (intr)
+			set_current_state(TASK_INTERRUPTIBLE);
+		else
+			set_current_state(TASK_UNINTERRUPTIBLE);
+
+		/*
+		 * radeon_test_signaled must be called after
+		 * set_current_state to prevent a race with wake_up_process
+		 */
+		if (radeon_test_signaled(fence))
+			break;
+
+		if (rdev->needs_reset) {
+			t = -EDEADLK;
+			break;
+		}
+
+		t = schedule_timeout(t);
+
+		if (t > 0 && intr && signal_pending(current))
+			t = -ERESTARTSYS;
+	}
+
+	__set_current_state(TASK_RUNNING);
+	fence_remove_callback(f, &cb.base);
 
-	if (intr)
-		t = wait_event_interruptible_timeout(rdev->fence_queue,
-			((signaled = radeon_test_signaled(fence)) ||
-			 rdev->needs_reset), t);
-	else
-		t = wait_event_timeout(rdev->fence_queue,
-			((signaled = radeon_test_signaled(fence)) ||
-			 rdev->needs_reset), t);
-
-	if (t > 0 && !signaled)
-		return -EDEADLK;
 	return t;
 }
 
diff --git a/drivers/gpu/drm/radeon/radeon_kfd.c b/drivers/gpu/drm/radeon/radeon_kfd.c
index 061eaa9c19c7..122eb5693ba1 100644
--- a/drivers/gpu/drm/radeon/radeon_kfd.c
+++ b/drivers/gpu/drm/radeon/radeon_kfd.c
@@ -153,7 +153,7 @@ void radeon_kfd_device_init(struct radeon_device *rdev)
 			.compute_vmid_bitmap = 0xFF00,
 
 			.first_compute_pipe = 1,
-			.compute_pipe_count = 8 - 1,
+			.compute_pipe_count = 4 - 1,
 		};
 
 		radeon_doorbell_get_kfd_info(rdev,
diff --git a/drivers/gpu/drm/radeon/radeon_mn.c b/drivers/gpu/drm/radeon/radeon_mn.c
index a69bd441dd2d..572b4dbec186 100644
--- a/drivers/gpu/drm/radeon/radeon_mn.c
+++ b/drivers/gpu/drm/radeon/radeon_mn.c
@@ -122,7 +122,6 @@ static void radeon_mn_invalidate_range_start(struct mmu_notifier *mn,
 	it = interval_tree_iter_first(&rmn->objects, start, end);
 	while (it) {
 		struct radeon_bo *bo;
-		struct fence *fence;
 		int r;
 
 		bo = container_of(it, struct radeon_bo, mn_it);
@@ -134,12 +133,10 @@ static void radeon_mn_invalidate_range_start(struct mmu_notifier *mn,
 			continue;
 		}
 
-		fence = reservation_object_get_excl(bo->tbo.resv);
-		if (fence) {
-			r = radeon_fence_wait((struct radeon_fence *)fence, false);
-			if (r)
-				DRM_ERROR("(%d) failed to wait for user bo\n", r);
-		}
+		r = reservation_object_wait_timeout_rcu(bo->tbo.resv, true,
+			false, MAX_SCHEDULE_TIMEOUT);
+		if (r)
+			DRM_ERROR("(%d) failed to wait for user bo\n", r);
 
 		radeon_ttm_placement_from_domain(bo, RADEON_GEM_DOMAIN_CPU);
 		r = ttm_bo_validate(&bo->tbo, &bo->placement, false, false);
diff --git a/drivers/gpu/drm/radeon/radeon_object.c b/drivers/gpu/drm/radeon/radeon_object.c
index 43e09942823e..318165d4855c 100644
--- a/drivers/gpu/drm/radeon/radeon_object.c
+++ b/drivers/gpu/drm/radeon/radeon_object.c
@@ -173,17 +173,6 @@ void radeon_ttm_placement_from_domain(struct radeon_bo *rbo, u32 domain)
 		else
 			rbo->placements[i].lpfn = 0;
 	}
-
-	/*
-	 * Use two-ended allocation depending on the buffer size to
-	 * improve fragmentation quality.
-	 * 512kb was measured as the most optimal number.
-	 */
-	if (rbo->tbo.mem.size > 512 * 1024) {
-		for (i = 0; i < c; i++) {
-			rbo->placements[i].flags |= TTM_PL_FLAG_TOPDOWN;
-		}
-	}
 }
 
 int radeon_bo_create(struct radeon_device *rdev,
diff --git a/drivers/gpu/drm/radeon/radeon_pm.c b/drivers/gpu/drm/radeon/radeon_pm.c
index 9f758d39420d..c1ba83a8dd8c 100644
--- a/drivers/gpu/drm/radeon/radeon_pm.c
+++ b/drivers/gpu/drm/radeon/radeon_pm.c
@@ -837,12 +837,8 @@ static void radeon_dpm_thermal_work_handler(struct work_struct *work)
 	radeon_pm_compute_clocks(rdev);
 }
 
-static struct radeon_ps *radeon_dpm_pick_power_state(struct radeon_device *rdev,
-						     enum radeon_pm_state_type dpm_state)
+static bool radeon_dpm_single_display(struct radeon_device *rdev)
 {
-	int i;
-	struct radeon_ps *ps;
-	u32 ui_class;
 	bool single_display = (rdev->pm.dpm.new_active_crtc_count < 2) ?
 		true : false;
 
@@ -852,6 +848,23 @@ static struct radeon_ps *radeon_dpm_pick_power_state(struct radeon_device *rdev,
 			single_display = false;
 	}
 
+	/* 120hz tends to be problematic even if they are under the
+	 * vblank limit.
+	 */
+	if (single_display && (r600_dpm_get_vrefresh(rdev) >= 120))
+		single_display = false;
+
+	return single_display;
+}
+
+static struct radeon_ps *radeon_dpm_pick_power_state(struct radeon_device *rdev,
+						     enum radeon_pm_state_type dpm_state)
+{
+	int i;
+	struct radeon_ps *ps;
+	u32 ui_class;
+	bool single_display = radeon_dpm_single_display(rdev);
+
 	/* certain older asics have a separare 3D performance state,
 	 * so try that first if the user selected performance
 	 */
@@ -977,6 +990,7 @@ static void radeon_dpm_change_power_state_locked(struct radeon_device *rdev)
 	struct radeon_ps *ps;
 	enum radeon_pm_state_type dpm_state;
 	int ret;
+	bool single_display = radeon_dpm_single_display(rdev);
 
 	/* if dpm init failed */
 	if (!rdev->pm.dpm_enabled)
@@ -1001,6 +1015,9 @@ static void radeon_dpm_change_power_state_locked(struct radeon_device *rdev)
 		/* vce just modifies an existing state so force a change */
 		if (ps->vce_active != rdev->pm.dpm.vce_active)
 			goto force;
+		/* user has made a display change (such as timing) */
+		if (rdev->pm.dpm.single_display != single_display)
+			goto force;
 		if ((rdev->family < CHIP_BARTS) || (rdev->flags & RADEON_IS_IGP)) {
 			/* for pre-BTC and APUs if the num crtcs changed but state is the same,
 			 * all we need to do is update the display configuration.
@@ -1063,6 +1080,7 @@ force:
 
 	rdev->pm.dpm.current_active_crtcs = rdev->pm.dpm.new_active_crtcs;
 	rdev->pm.dpm.current_active_crtc_count = rdev->pm.dpm.new_active_crtc_count;
+	rdev->pm.dpm.single_display = single_display;
 
 	/* wait for the rings to drain */
 	for (i = 0; i < RADEON_NUM_RINGS; i++) {
diff --git a/drivers/gpu/drm/radeon/radeon_ring.c b/drivers/gpu/drm/radeon/radeon_ring.c
index 2456f69efd23..8c7872339c2a 100644
--- a/drivers/gpu/drm/radeon/radeon_ring.c
+++ b/drivers/gpu/drm/radeon/radeon_ring.c
@@ -495,7 +495,7 @@ static int radeon_debugfs_ring_info(struct seq_file *m, void *data)
 	seq_printf(m, "%u free dwords in ring\n", ring->ring_free_dw);
 	seq_printf(m, "%u dwords in ring\n", count);
 
-	if (!ring->ready)
+	if (!ring->ring)
 		return 0;
 
 	/* print 8 dw before current rptr as often it's the last executed
diff --git a/drivers/gpu/drm/radeon/radeon_ttm.c b/drivers/gpu/drm/radeon/radeon_ttm.c
index d02aa1d0f588..b292aca0f342 100644
--- a/drivers/gpu/drm/radeon/radeon_ttm.c
+++ b/drivers/gpu/drm/radeon/radeon_ttm.c
@@ -598,6 +598,10 @@ static void radeon_ttm_tt_unpin_userptr(struct ttm_tt *ttm)
 	enum dma_data_direction direction = write ?
 		DMA_BIDIRECTIONAL : DMA_TO_DEVICE;
 
+	/* double check that we don't free the table twice */
+	if (!ttm->sg->sgl)
+		return;
+
 	/* free the sg table and pages again */
 	dma_unmap_sg(rdev->dev, ttm->sg->sgl, ttm->sg->nents, direction);
 
diff --git a/drivers/gpu/drm/radeon/rs600.c b/drivers/gpu/drm/radeon/rs600.c
index d81182ad53ec..97a904835759 100644
--- a/drivers/gpu/drm/radeon/rs600.c
+++ b/drivers/gpu/drm/radeon/rs600.c
@@ -694,6 +694,10 @@ int rs600_irq_set(struct radeon_device *rdev)
 	WREG32(R_007D18_DC_HOT_PLUG_DETECT2_INT_CONTROL, hpd2);
 	if (ASIC_IS_DCE2(rdev))
 		WREG32(R_007408_HDMI0_AUDIO_PACKET_CONTROL, hdmi0);
+
+	/* posting read */
+	RREG32(R_000040_GEN_INT_CNTL);
+
 	return 0;
 }
 
diff --git a/drivers/gpu/drm/radeon/si.c b/drivers/gpu/drm/radeon/si.c
index 73107fe9e46f..a7fb2735d4a9 100644
--- a/drivers/gpu/drm/radeon/si.c
+++ b/drivers/gpu/drm/radeon/si.c
@@ -3162,6 +3162,8 @@ static void si_gpu_init(struct radeon_device *rdev)
 	}
 
 	WREG32(GRBM_CNTL, GRBM_READ_TIMEOUT(0xff));
+	WREG32(SRBM_INT_CNTL, 1);
+	WREG32(SRBM_INT_ACK, 1);
 
 	evergreen_fix_pci_max_read_req_size(rdev);
 
@@ -4699,12 +4701,6 @@ int si_ib_parse(struct radeon_device *rdev, struct radeon_ib *ib)
 		switch (pkt.type) {
 		case RADEON_PACKET_TYPE0:
 			dev_err(rdev->dev, "Packet0 not allowed!\n");
-			for (i = 0; i < ib->length_dw; i++) {
-				if (i == idx)
-					printk("\t0x%08x <---\n", ib->ptr[i]);
-				else
-					printk("\t0x%08x\n", ib->ptr[i]);
-			}
 			ret = -EINVAL;
 			break;
 		case RADEON_PACKET_TYPE2:
@@ -4736,8 +4732,15 @@ int si_ib_parse(struct radeon_device *rdev, struct radeon_ib *ib)
 			ret = -EINVAL;
 			break;
 		}
-		if (ret)
+		if (ret) {
+			for (i = 0; i < ib->length_dw; i++) {
+				if (i == idx)
+					printk("\t0x%08x <---\n", ib->ptr[i]);
+				else
+					printk("\t0x%08x\n", ib->ptr[i]);
+			}
 			break;
+		}
 	} while (idx < ib->length_dw);
 
 	return ret;
@@ -5910,6 +5913,7 @@ static void si_disable_interrupt_state(struct radeon_device *rdev)
 	tmp = RREG32(DMA_CNTL + DMA1_REGISTER_OFFSET) & ~TRAP_ENABLE;
 	WREG32(DMA_CNTL + DMA1_REGISTER_OFFSET, tmp);
 	WREG32(GRBM_INT_CNTL, 0);
+	WREG32(SRBM_INT_CNTL, 0);
 	if (rdev->num_crtc >= 2) {
 		WREG32(INT_MASK + EVERGREEN_CRTC0_REGISTER_OFFSET, 0);
 		WREG32(INT_MASK + EVERGREEN_CRTC1_REGISTER_OFFSET, 0);
@@ -6199,6 +6203,9 @@ int si_irq_set(struct radeon_device *rdev)
 
 	WREG32(CG_THERMAL_INT, thermal_int);
 
+	/* posting read */
+	RREG32(SRBM_STATUS);
+
 	return 0;
 }
 
@@ -6609,6 +6616,10 @@ restart_ih:
 				break;
 			}
 			break;
+		case 96:
+			DRM_ERROR("SRBM_READ_ERROR: 0x%x\n", RREG32(SRBM_READ_ERROR));
+			WREG32(SRBM_INT_ACK, 0x1);
+			break;
 		case 124: /* UVD */
 			DRM_DEBUG("IH: UVD int: 0x%08x\n", src_data);
 			radeon_fence_process(rdev, R600_RING_TYPE_UVD_INDEX);
@@ -7119,8 +7130,7 @@ int si_set_uvd_clocks(struct radeon_device *rdev, u32 vclk, u32 dclk)
 	WREG32_P(CG_UPLL_FUNC_CNTL, UPLL_BYPASS_EN_MASK, ~UPLL_BYPASS_EN_MASK);
 
 	if (!vclk || !dclk) {
-		/* keep the Bypass mode, put PLL to sleep */
-		WREG32_P(CG_UPLL_FUNC_CNTL, UPLL_SLEEP_MASK, ~UPLL_SLEEP_MASK);
+		/* keep the Bypass mode */
 		return 0;
 	}
 
@@ -7136,8 +7146,7 @@ int si_set_uvd_clocks(struct radeon_device *rdev, u32 vclk, u32 dclk)
 	/* set VCO_MODE to 1 */
 	WREG32_P(CG_UPLL_FUNC_CNTL, UPLL_VCO_MODE_MASK, ~UPLL_VCO_MODE_MASK);
 
-	/* toggle UPLL_SLEEP to 1 then back to 0 */
-	WREG32_P(CG_UPLL_FUNC_CNTL, UPLL_SLEEP_MASK, ~UPLL_SLEEP_MASK);
+	/* disable sleep mode */
 	WREG32_P(CG_UPLL_FUNC_CNTL, 0, ~UPLL_SLEEP_MASK);
 
 	/* deassert UPLL_RESET */
diff --git a/drivers/gpu/drm/radeon/sid.h b/drivers/gpu/drm/radeon/sid.h
index cbd91d226f3c..99a9835c9f61 100644
--- a/drivers/gpu/drm/radeon/sid.h
+++ b/drivers/gpu/drm/radeon/sid.h
@@ -358,6 +358,10 @@
 #define	CC_SYS_RB_BACKEND_DISABLE			0xe80
 #define	GC_USER_SYS_RB_BACKEND_DISABLE			0xe84
 
+#define SRBM_READ_ERROR					0xE98
+#define SRBM_INT_CNTL					0xEA0
+#define SRBM_INT_ACK					0xEA8
+
 #define	SRBM_STATUS2				        0x0EC4
 #define		DMA_BUSY 				(1 << 5)
 #define		DMA1_BUSY 				(1 << 6)
@@ -908,8 +912,8 @@
 
 #define DCCG_AUDIO_DTO0_PHASE                           0x05b0
 #define DCCG_AUDIO_DTO0_MODULE                          0x05b4
-#define DCCG_AUDIO_DTO1_PHASE                           0x05b8
-#define DCCG_AUDIO_DTO1_MODULE                          0x05bc
+#define DCCG_AUDIO_DTO1_PHASE                           0x05c0
+#define DCCG_AUDIO_DTO1_MODULE                          0x05c4
 
 #define AFMT_AUDIO_SRC_CONTROL                          0x713c
 #define		AFMT_AUDIO_SRC_SELECT(x)		(((x) & 7) << 0)
diff --git a/drivers/gpu/drm/radeon/vce_v2_0.c b/drivers/gpu/drm/radeon/vce_v2_0.c
index 1ac7bb825a1b..fbbe78fbd087 100644
--- a/drivers/gpu/drm/radeon/vce_v2_0.c
+++ b/drivers/gpu/drm/radeon/vce_v2_0.c
@@ -156,6 +156,9 @@ int vce_v2_0_resume(struct radeon_device *rdev)
 	WREG32(VCE_LMI_SWAP_CNTL1, 0);
 	WREG32(VCE_LMI_VM_CTRL, 0);
 
+	WREG32(VCE_LMI_VCPU_CACHE_40BIT_BAR, addr >> 8);
+
+	addr &= 0xff;
 	size = RADEON_GPU_PAGE_ALIGN(rdev->vce_fw->size);
 	WREG32(VCE_VCPU_CACHE_OFFSET0, addr & 0x7fffffff);
 	WREG32(VCE_VCPU_CACHE_SIZE0, size);
diff --git a/drivers/gpu/drm/tegra/dc.c b/drivers/gpu/drm/tegra/dc.c
index 3aaa84ae2681..1a52522f5da7 100644
--- a/drivers/gpu/drm/tegra/dc.c
+++ b/drivers/gpu/drm/tegra/dc.c
@@ -997,8 +997,10 @@ static void tegra_crtc_reset(struct drm_crtc *crtc)
 	crtc->state = NULL;
 
 	state = kzalloc(sizeof(*state), GFP_KERNEL);
-	if (state)
+	if (state) {
 		crtc->state = &state->base;
+		crtc->state->crtc = crtc;
+	}
 }
 
 static struct drm_crtc_state *
@@ -1012,6 +1014,7 @@ tegra_crtc_atomic_duplicate_state(struct drm_crtc *crtc)
 		return NULL;
 
 	copy->base.mode_changed = false;
+	copy->base.active_changed = false;
 	copy->base.planes_changed = false;
 	copy->base.event = NULL;
 
@@ -1227,9 +1230,6 @@ static void tegra_crtc_mode_set_nofb(struct drm_crtc *crtc)
 	/* program display mode */
 	tegra_dc_set_timings(dc, mode);
 
-	if (dc->soc->supports_border_color)
-		tegra_dc_writel(dc, 0, DC_DISP_BORDER_COLOR);
-
 	/* interlacing isn't supported yet, so disable it */
 	if (dc->soc->supports_interlacing) {
 		value = tegra_dc_readl(dc, DC_DISP_INTERLACE_CONTROL);
@@ -1252,42 +1252,7 @@ static void tegra_crtc_mode_set_nofb(struct drm_crtc *crtc)
 
 static void tegra_crtc_prepare(struct drm_crtc *crtc)
 {
-	struct tegra_dc *dc = to_tegra_dc(crtc);
-	unsigned int syncpt;
-	unsigned long value;
-
 	drm_crtc_vblank_off(crtc);
-
-	if (dc->pipe)
-		syncpt = SYNCPT_VBLANK1;
-	else
-		syncpt = SYNCPT_VBLANK0;
-
-	/* initialize display controller */
-	tegra_dc_writel(dc, 0x00000100, DC_CMD_GENERAL_INCR_SYNCPT_CNTRL);
-	tegra_dc_writel(dc, 0x100 | syncpt, DC_CMD_CONT_SYNCPT_VSYNC);
-
-	value = WIN_A_UF_INT | WIN_B_UF_INT | WIN_C_UF_INT | WIN_A_OF_INT;
-	tegra_dc_writel(dc, value, DC_CMD_INT_TYPE);
-
-	value = WIN_A_UF_INT | WIN_B_UF_INT | WIN_C_UF_INT |
-		WIN_A_OF_INT | WIN_B_OF_INT | WIN_C_OF_INT;
-	tegra_dc_writel(dc, value, DC_CMD_INT_POLARITY);
-
-	/* initialize timer */
-	value = CURSOR_THRESHOLD(0) | WINDOW_A_THRESHOLD(0x20) |
-		WINDOW_B_THRESHOLD(0x20) | WINDOW_C_THRESHOLD(0x20);
-	tegra_dc_writel(dc, value, DC_DISP_DISP_MEM_HIGH_PRIORITY);
-
-	value = CURSOR_THRESHOLD(0) | WINDOW_A_THRESHOLD(1) |
-		WINDOW_B_THRESHOLD(1) | WINDOW_C_THRESHOLD(1);
-	tegra_dc_writel(dc, value, DC_DISP_DISP_MEM_HIGH_PRIORITY_TIMER);
-
-	value = VBLANK_INT | WIN_A_UF_INT | WIN_B_UF_INT | WIN_C_UF_INT;
-	tegra_dc_writel(dc, value, DC_CMD_INT_ENABLE);
-
-	value = WIN_A_UF_INT | WIN_B_UF_INT | WIN_C_UF_INT;
-	tegra_dc_writel(dc, value, DC_CMD_INT_MASK);
 }
 
 static void tegra_crtc_commit(struct drm_crtc *crtc)
@@ -1664,6 +1629,8 @@ static int tegra_dc_init(struct host1x_client *client)
 	struct tegra_drm *tegra = drm->dev_private;
 	struct drm_plane *primary = NULL;
 	struct drm_plane *cursor = NULL;
+	unsigned int syncpt;
+	u32 value;
 	int err;
 
 	if (tegra->domain) {
@@ -1730,6 +1697,40 @@ static int tegra_dc_init(struct host1x_client *client)
 		goto cleanup;
 	}
 
+	/* initialize display controller */
+	if (dc->pipe)
+		syncpt = SYNCPT_VBLANK1;
+	else
+		syncpt = SYNCPT_VBLANK0;
+
+	tegra_dc_writel(dc, 0x00000100, DC_CMD_GENERAL_INCR_SYNCPT_CNTRL);
+	tegra_dc_writel(dc, 0x100 | syncpt, DC_CMD_CONT_SYNCPT_VSYNC);
+
+	value = WIN_A_UF_INT | WIN_B_UF_INT | WIN_C_UF_INT | WIN_A_OF_INT;
+	tegra_dc_writel(dc, value, DC_CMD_INT_TYPE);
+
+	value = WIN_A_UF_INT | WIN_B_UF_INT | WIN_C_UF_INT |
+		WIN_A_OF_INT | WIN_B_OF_INT | WIN_C_OF_INT;
+	tegra_dc_writel(dc, value, DC_CMD_INT_POLARITY);
+
+	/* initialize timer */
+	value = CURSOR_THRESHOLD(0) | WINDOW_A_THRESHOLD(0x20) |
+		WINDOW_B_THRESHOLD(0x20) | WINDOW_C_THRESHOLD(0x20);
+	tegra_dc_writel(dc, value, DC_DISP_DISP_MEM_HIGH_PRIORITY);
+
+	value = CURSOR_THRESHOLD(0) | WINDOW_A_THRESHOLD(1) |
+		WINDOW_B_THRESHOLD(1) | WINDOW_C_THRESHOLD(1);
+	tegra_dc_writel(dc, value, DC_DISP_DISP_MEM_HIGH_PRIORITY_TIMER);
+
+	value = VBLANK_INT | WIN_A_UF_INT | WIN_B_UF_INT | WIN_C_UF_INT;
+	tegra_dc_writel(dc, value, DC_CMD_INT_ENABLE);
+
+	value = WIN_A_UF_INT | WIN_B_UF_INT | WIN_C_UF_INT;
+	tegra_dc_writel(dc, value, DC_CMD_INT_MASK);
+
+	if (dc->soc->supports_border_color)
+		tegra_dc_writel(dc, 0, DC_DISP_BORDER_COLOR);
+
 	return 0;
 
 cleanup:
diff --git a/drivers/gpu/drm/tegra/hdmi.c b/drivers/gpu/drm/tegra/hdmi.c
index 7e06657ae58b..7eaaee74a039 100644
--- a/drivers/gpu/drm/tegra/hdmi.c
+++ b/drivers/gpu/drm/tegra/hdmi.c
@@ -851,6 +851,14 @@ static void tegra_hdmi_encoder_mode_set(struct drm_encoder *encoder,
 	h_back_porch = mode->htotal - mode->hsync_end;
 	h_front_porch = mode->hsync_start - mode->hdisplay;
 
+	err = clk_set_rate(hdmi->clk, pclk);
+	if (err < 0) {
+		dev_err(hdmi->dev, "failed to set HDMI clock frequency: %d\n",
+			err);
+	}
+
+	DRM_DEBUG_KMS("HDMI clock rate: %lu Hz\n", clk_get_rate(hdmi->clk));
+
 	/* power up sequence */
 	value = tegra_hdmi_readl(hdmi, HDMI_NV_PDISP_SOR_PLL0);
 	value &= ~SOR_PLL_PDBG;
diff --git a/drivers/gpu/drm/ttm/ttm_bo.c b/drivers/gpu/drm/ttm/ttm_bo.c
index d395b0bef73b..8d9b7de25613 100644
--- a/drivers/gpu/drm/ttm/ttm_bo.c
+++ b/drivers/gpu/drm/ttm/ttm_bo.c
@@ -74,7 +74,7 @@ static void ttm_mem_type_debug(struct ttm_bo_device *bdev, int mem_type)
 	pr_err("    has_type: %d\n", man->has_type);
 	pr_err("    use_type: %d\n", man->use_type);
 	pr_err("    flags: 0x%08X\n", man->flags);
-	pr_err("    gpu_offset: 0x%08lX\n", man->gpu_offset);
+	pr_err("    gpu_offset: 0x%08llX\n", man->gpu_offset);
 	pr_err("    size: %llu\n", man->size);
 	pr_err("    available_caching: 0x%08X\n", man->available_caching);
 	pr_err("    default_caching: 0x%08X\n", man->default_caching);
diff --git a/drivers/gpu/drm/vmwgfx/vmwgfx_drv.c b/drivers/gpu/drm/vmwgfx/vmwgfx_drv.c
index 6c6b655defcf..e13b9cbc304e 100644
--- a/drivers/gpu/drm/vmwgfx/vmwgfx_drv.c
+++ b/drivers/gpu/drm/vmwgfx/vmwgfx_drv.c
@@ -725,32 +725,6 @@ static int vmw_driver_load(struct drm_device *dev, unsigned long chipset)
 		goto out_err1;
 	}
 
-	ret = ttm_bo_init_mm(&dev_priv->bdev, TTM_PL_VRAM,
-			     (dev_priv->vram_size >> PAGE_SHIFT));
-	if (unlikely(ret != 0)) {
-		DRM_ERROR("Failed initializing memory manager for VRAM.\n");
-		goto out_err2;
-	}
-
-	dev_priv->has_gmr = true;
-	if (((dev_priv->capabilities & (SVGA_CAP_GMR | SVGA_CAP_GMR2)) == 0) ||
-	    refuse_dma || ttm_bo_init_mm(&dev_priv->bdev, VMW_PL_GMR,
-					 VMW_PL_GMR) != 0) {
-		DRM_INFO("No GMR memory available. "
-			 "Graphics memory resources are very limited.\n");
-		dev_priv->has_gmr = false;
-	}
-
-	if (dev_priv->capabilities & SVGA_CAP_GBOBJECTS) {
-		dev_priv->has_mob = true;
-		if (ttm_bo_init_mm(&dev_priv->bdev, VMW_PL_MOB,
-				   VMW_PL_MOB) != 0) {
-			DRM_INFO("No MOB memory available. "
-				 "3D will be disabled.\n");
-			dev_priv->has_mob = false;
-		}
-	}
-
 	dev_priv->mmio_mtrr = arch_phys_wc_add(dev_priv->mmio_start,
 					       dev_priv->mmio_size);
 
@@ -813,6 +787,33 @@ static int vmw_driver_load(struct drm_device *dev, unsigned long chipset)
 		goto out_no_fman;
 	}
 
+
+	ret = ttm_bo_init_mm(&dev_priv->bdev, TTM_PL_VRAM,
+			     (dev_priv->vram_size >> PAGE_SHIFT));
+	if (unlikely(ret != 0)) {
+		DRM_ERROR("Failed initializing memory manager for VRAM.\n");
+		goto out_no_vram;
+	}
+
+	dev_priv->has_gmr = true;
+	if (((dev_priv->capabilities & (SVGA_CAP_GMR | SVGA_CAP_GMR2)) == 0) ||
+	    refuse_dma || ttm_bo_init_mm(&dev_priv->bdev, VMW_PL_GMR,
+					 VMW_PL_GMR) != 0) {
+		DRM_INFO("No GMR memory available. "
+			 "Graphics memory resources are very limited.\n");
+		dev_priv->has_gmr = false;
+	}
+
+	if (dev_priv->capabilities & SVGA_CAP_GBOBJECTS) {
+		dev_priv->has_mob = true;
+		if (ttm_bo_init_mm(&dev_priv->bdev, VMW_PL_MOB,
+				   VMW_PL_MOB) != 0) {
+			DRM_INFO("No MOB memory available. "
+				 "3D will be disabled.\n");
+			dev_priv->has_mob = false;
+		}
+	}
+
 	vmw_kms_save_vga(dev_priv);
 
 	/* Start kms and overlay systems, needs fifo. */
@@ -838,6 +839,12 @@ out_no_fifo:
 	vmw_kms_close(dev_priv);
 out_no_kms:
 	vmw_kms_restore_vga(dev_priv);
+	if (dev_priv->has_mob)
+		(void) ttm_bo_clean_mm(&dev_priv->bdev, VMW_PL_MOB);
+	if (dev_priv->has_gmr)
+		(void) ttm_bo_clean_mm(&dev_priv->bdev, VMW_PL_GMR);
+	(void)ttm_bo_clean_mm(&dev_priv->bdev, TTM_PL_VRAM);
+out_no_vram:
 	vmw_fence_manager_takedown(dev_priv->fman);
 out_no_fman:
 	if (dev_priv->capabilities & SVGA_CAP_IRQMASK)
@@ -853,12 +860,6 @@ out_err4:
 	iounmap(dev_priv->mmio_virt);
 out_err3:
 	arch_phys_wc_del(dev_priv->mmio_mtrr);
-	if (dev_priv->has_mob)
-		(void) ttm_bo_clean_mm(&dev_priv->bdev, VMW_PL_MOB);
-	if (dev_priv->has_gmr)
-		(void) ttm_bo_clean_mm(&dev_priv->bdev, VMW_PL_GMR);
-	(void)ttm_bo_clean_mm(&dev_priv->bdev, TTM_PL_VRAM);
-out_err2:
 	(void)ttm_bo_device_release(&dev_priv->bdev);
 out_err1:
 	vmw_ttm_global_release(dev_priv);
@@ -887,6 +888,13 @@ static int vmw_driver_unload(struct drm_device *dev)
 	}
 	vmw_kms_close(dev_priv);
 	vmw_overlay_close(dev_priv);
+
+	if (dev_priv->has_mob)
+		(void) ttm_bo_clean_mm(&dev_priv->bdev, VMW_PL_MOB);
+	if (dev_priv->has_gmr)
+		(void)ttm_bo_clean_mm(&dev_priv->bdev, VMW_PL_GMR);
+	(void)ttm_bo_clean_mm(&dev_priv->bdev, TTM_PL_VRAM);
+
 	vmw_fence_manager_takedown(dev_priv->fman);
 	if (dev_priv->capabilities & SVGA_CAP_IRQMASK)
 		drm_irq_uninstall(dev_priv->dev);
@@ -898,11 +906,6 @@ static int vmw_driver_unload(struct drm_device *dev)
 	ttm_object_device_release(&dev_priv->tdev);
 	iounmap(dev_priv->mmio_virt);
 	arch_phys_wc_del(dev_priv->mmio_mtrr);
-	if (dev_priv->has_mob)
-		(void) ttm_bo_clean_mm(&dev_priv->bdev, VMW_PL_MOB);
-	if (dev_priv->has_gmr)
-		(void)ttm_bo_clean_mm(&dev_priv->bdev, VMW_PL_GMR);
-	(void)ttm_bo_clean_mm(&dev_priv->bdev, TTM_PL_VRAM);
 	(void)ttm_bo_device_release(&dev_priv->bdev);
 	vmw_ttm_global_release(dev_priv);
 
@@ -1235,6 +1238,7 @@ static void vmw_remove(struct pci_dev *pdev)
 {
 	struct drm_device *dev = pci_get_drvdata(pdev);
 
+	pci_disable_device(pdev);
 	drm_put_dev(dev);
 }
 
diff --git a/drivers/gpu/drm/vmwgfx/vmwgfx_execbuf.c b/drivers/gpu/drm/vmwgfx/vmwgfx_execbuf.c
index 33176d05db35..654c8daeb5ab 100644
--- a/drivers/gpu/drm/vmwgfx/vmwgfx_execbuf.c
+++ b/drivers/gpu/drm/vmwgfx/vmwgfx_execbuf.c
@@ -890,7 +890,8 @@ static int vmw_translate_mob_ptr(struct vmw_private *dev_priv,
 	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;
+		ret = -EINVAL;
+		goto out_no_reloc;
 	}
 	bo = &vmw_bo->base;
 
@@ -914,7 +915,7 @@ static int vmw_translate_mob_ptr(struct vmw_private *dev_priv,
 
 out_no_reloc:
 	vmw_dmabuf_unreference(&vmw_bo);
-	vmw_bo_p = NULL;
+	*vmw_bo_p = NULL;
 	return ret;
 }
 
@@ -951,7 +952,8 @@ static int vmw_translate_guest_ptr(struct vmw_private *dev_priv,
 	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;
+		ret = -EINVAL;
+		goto out_no_reloc;
 	}
 	bo = &vmw_bo->base;
 
@@ -974,7 +976,7 @@ static int vmw_translate_guest_ptr(struct vmw_private *dev_priv,
 
 out_no_reloc:
 	vmw_dmabuf_unreference(&vmw_bo);
-	vmw_bo_p = NULL;
+	*vmw_bo_p = NULL;
 	return ret;
 }
 
@@ -2780,13 +2782,11 @@ int vmw_execbuf_ioctl(struct drm_device *dev, void *data,
 				  NULL, arg->command_size, arg->throttle_us,
 				  (void __user *)(unsigned long)arg->fence_rep,
 				  NULL);
-
+	ttm_read_unlock(&dev_priv->reservation_sem);
 	if (unlikely(ret != 0))
-		goto out_unlock;
+		return ret;
 
 	vmw_kms_cursor_post_execbuf(dev_priv);
 
-out_unlock:
-	ttm_read_unlock(&dev_priv->reservation_sem);
-	return ret;
+	return 0;
 }
diff --git a/drivers/gpu/drm/vmwgfx/vmwgfx_kms.c b/drivers/gpu/drm/vmwgfx/vmwgfx_kms.c
index 8725b79e7847..07cda8cbbddb 100644
--- a/drivers/gpu/drm/vmwgfx/vmwgfx_kms.c
+++ b/drivers/gpu/drm/vmwgfx/vmwgfx_kms.c
@@ -2033,23 +2033,17 @@ int vmw_kms_update_layout_ioctl(struct drm_device *dev, void *data,
 	int i;
 	struct drm_mode_config *mode_config = &dev->mode_config;
 
-	ret = ttm_read_lock(&dev_priv->reservation_sem, true);
-	if (unlikely(ret != 0))
-		return ret;
-
 	if (!arg->num_outputs) {
 		struct drm_vmw_rect def_rect = {0, 0, 800, 600};
 		vmw_du_update_layout(dev_priv, 1, &def_rect);
-		goto out_unlock;
+		return 0;
 	}
 
 	rects_size = arg->num_outputs * sizeof(struct drm_vmw_rect);
 	rects = kcalloc(arg->num_outputs, sizeof(struct drm_vmw_rect),
 			GFP_KERNEL);
-	if (unlikely(!rects)) {
-		ret = -ENOMEM;
-		goto out_unlock;
-	}
+	if (unlikely(!rects))
+		return -ENOMEM;
 
 	user_rects = (void __user *)(unsigned long)arg->rects;
 	ret = copy_from_user(rects, user_rects, rects_size);
@@ -2074,7 +2068,5 @@ int vmw_kms_update_layout_ioctl(struct drm_device *dev, void *data,
 
 out_free:
 	kfree(rects);
-out_unlock:
-	ttm_read_unlock(&dev_priv->reservation_sem);
 	return ret;
 }
diff --git a/drivers/gpu/ipu-v3/ipu-di.c b/drivers/gpu/ipu-v3/ipu-di.c
index b61d6be97602..3ddfb3d0b64d 100644
--- a/drivers/gpu/ipu-v3/ipu-di.c
+++ b/drivers/gpu/ipu-v3/ipu-di.c
@@ -459,6 +459,8 @@ static void ipu_di_config_clock(struct ipu_di *di,
 
 		clkrate = clk_get_rate(di->clk_ipu);
 		div = DIV_ROUND_CLOSEST(clkrate, sig->mode.pixelclock);
+		if (div == 0)
+			div = 1;
 		rate = clkrate / div;
 
 		error = rate / (sig->mode.pixelclock / 1000);
diff --git a/drivers/hid/hid-input.c b/drivers/hid/hid-input.c
index 19603efc8fa2..008e89bf6f3c 100644
--- a/drivers/hid/hid-input.c
+++ b/drivers/hid/hid-input.c
@@ -339,7 +339,7 @@ static int hidinput_get_battery_property(struct power_supply *psy,
 					 enum power_supply_property prop,
 					 union power_supply_propval *val)
 {
-	struct hid_device *dev = container_of(psy, struct hid_device, battery);
+	struct hid_device *dev = power_supply_get_drvdata(psy);
 	int ret = 0;
 	__u8 *buf;
 
@@ -397,26 +397,32 @@ static int hidinput_get_battery_property(struct power_supply *psy,
 
 static bool hidinput_setup_battery(struct hid_device *dev, unsigned report_type, struct hid_field *field)
 {
-	struct power_supply *battery = &dev->battery;
-	int ret;
+	struct power_supply_desc *psy_desc = NULL;
+	struct power_supply_config psy_cfg = { .drv_data = dev, };
 	unsigned quirks;
 	s32 min, max;
 
 	if (field->usage->hid != HID_DC_BATTERYSTRENGTH)
 		return false;	/* no match */
 
-	if (battery->name != NULL)
+	if (dev->battery != NULL)
 		goto out;	/* already initialized? */
 
-	battery->name = kasprintf(GFP_KERNEL, "hid-%s-battery", dev->uniq);
-	if (battery->name == NULL)
+	psy_desc = kzalloc(sizeof(*psy_desc), GFP_KERNEL);
+	if (psy_desc == NULL)
 		goto out;
 
-	battery->type = POWER_SUPPLY_TYPE_BATTERY;
-	battery->properties = hidinput_battery_props;
-	battery->num_properties = ARRAY_SIZE(hidinput_battery_props);
-	battery->use_for_apm = 0;
-	battery->get_property = hidinput_get_battery_property;
+	psy_desc->name = kasprintf(GFP_KERNEL, "hid-%s-battery", dev->uniq);
+	if (psy_desc->name == NULL) {
+		kfree(psy_desc);
+		goto out;
+	}
+
+	psy_desc->type = POWER_SUPPLY_TYPE_BATTERY;
+	psy_desc->properties = hidinput_battery_props;
+	psy_desc->num_properties = ARRAY_SIZE(hidinput_battery_props);
+	psy_desc->use_for_apm = 0;
+	psy_desc->get_property = hidinput_get_battery_property;
 
 	quirks = find_battery_quirk(dev);
 
@@ -439,27 +445,30 @@ static bool hidinput_setup_battery(struct hid_device *dev, unsigned report_type,
 	dev->battery_report_type = report_type;
 	dev->battery_report_id = field->report->id;
 
-	ret = power_supply_register(&dev->dev, battery);
-	if (ret != 0) {
-		hid_warn(dev, "can't register power supply: %d\n", ret);
-		kfree(battery->name);
-		battery->name = NULL;
+	dev->battery = power_supply_register(&dev->dev, psy_desc, &psy_cfg);
+	if (IS_ERR(dev->battery)) {
+		hid_warn(dev, "can't register power supply: %ld\n",
+				PTR_ERR(dev->battery));
+		kfree(psy_desc->name);
+		kfree(psy_desc);
+		dev->battery = NULL;
+	} else {
+		power_supply_powers(dev->battery, &dev->dev);
 	}
 
-	power_supply_powers(battery, &dev->dev);
-
 out:
 	return true;
 }
 
 static void hidinput_cleanup_battery(struct hid_device *dev)
 {
-	if (!dev->battery.name)
+	if (!dev->battery)
 		return;
 
-	power_supply_unregister(&dev->battery);
-	kfree(dev->battery.name);
-	dev->battery.name = NULL;
+	power_supply_unregister(dev->battery);
+	kfree(dev->battery->desc->name);
+	kfree(dev->battery->desc);
+	dev->battery = NULL;
 }
 #else  /* !CONFIG_HID_BATTERY_STRENGTH */
 static bool hidinput_setup_battery(struct hid_device *dev, unsigned report_type,
diff --git a/drivers/hid/hid-sony.c b/drivers/hid/hid-sony.c
index 27f67e7ebf10..6ca96cebb44c 100644
--- a/drivers/hid/hid-sony.c
+++ b/drivers/hid/hid-sony.c
@@ -816,7 +816,8 @@ struct sony_sc {
 	struct led_classdev *leds[MAX_LEDS];
 	unsigned long quirks;
 	struct work_struct state_worker;
-	struct power_supply battery;
+	struct power_supply *battery;
+	struct power_supply_desc battery_desc;
 	int device_id;
 	__u8 *output_report_dmabuf;
 
@@ -1681,7 +1682,7 @@ static int sony_battery_get_property(struct power_supply *psy,
 				     enum power_supply_property psp,
 				     union power_supply_propval *val)
 {
-	struct sony_sc *sc = container_of(psy, struct sony_sc, battery);
+	struct sony_sc *sc = power_supply_get_drvdata(psy);
 	unsigned long flags;
 	int ret = 0;
 	u8 battery_charging, battery_capacity, cable_state;
@@ -1720,6 +1721,7 @@ static int sony_battery_get_property(struct power_supply *psy,
 
 static int sony_battery_probe(struct sony_sc *sc)
 {
+	struct power_supply_config psy_cfg = { .drv_data = sc, };
 	struct hid_device *hdev = sc->hdev;
 	int ret;
 
@@ -1729,39 +1731,42 @@ static int sony_battery_probe(struct sony_sc *sc)
 	 */
 	sc->battery_capacity = 100;
 
-	sc->battery.properties = sony_battery_props;
-	sc->battery.num_properties = ARRAY_SIZE(sony_battery_props);
-	sc->battery.get_property = sony_battery_get_property;
-	sc->battery.type = POWER_SUPPLY_TYPE_BATTERY;
-	sc->battery.use_for_apm = 0;
-	sc->battery.name = kasprintf(GFP_KERNEL, "sony_controller_battery_%pMR",
-				     sc->mac_address);
-	if (!sc->battery.name)
+	sc->battery_desc.properties = sony_battery_props;
+	sc->battery_desc.num_properties = ARRAY_SIZE(sony_battery_props);
+	sc->battery_desc.get_property = sony_battery_get_property;
+	sc->battery_desc.type = POWER_SUPPLY_TYPE_BATTERY;
+	sc->battery_desc.use_for_apm = 0;
+	sc->battery_desc.name = kasprintf(GFP_KERNEL,
+					  "sony_controller_battery_%pMR",
+					  sc->mac_address);
+	if (!sc->battery_desc.name)
 		return -ENOMEM;
 
-	ret = power_supply_register(&hdev->dev, &sc->battery);
-	if (ret) {
+	sc->battery = power_supply_register(&hdev->dev, &sc->battery_desc,
+					    &psy_cfg);
+	if (IS_ERR(sc->battery)) {
+		ret = PTR_ERR(sc->battery);
 		hid_err(hdev, "Unable to register battery device\n");
 		goto err_free;
 	}
 
-	power_supply_powers(&sc->battery, &hdev->dev);
+	power_supply_powers(sc->battery, &hdev->dev);
 	return 0;
 
 err_free:
-	kfree(sc->battery.name);
-	sc->battery.name = NULL;
+	kfree(sc->battery_desc.name);
+	sc->battery_desc.name = NULL;
 	return ret;
 }
 
 static void sony_battery_remove(struct sony_sc *sc)
 {
-	if (!sc->battery.name)
+	if (!sc->battery_desc.name)
 		return;
 
-	power_supply_unregister(&sc->battery);
-	kfree(sc->battery.name);
-	sc->battery.name = NULL;
+	power_supply_unregister(sc->battery);
+	kfree(sc->battery_desc.name);
+	sc->battery_desc.name = NULL;
 }
 
 /*
diff --git a/drivers/hid/hid-wiimote-modules.c b/drivers/hid/hid-wiimote-modules.c
index 6b61f01e01e7..05e23c417d50 100644
--- a/drivers/hid/hid-wiimote-modules.c
+++ b/drivers/hid/hid-wiimote-modules.c
@@ -203,8 +203,7 @@ static int wiimod_battery_get_property(struct power_supply *psy,
 				       enum power_supply_property psp,
 				       union power_supply_propval *val)
 {
-	struct wiimote_data *wdata = container_of(psy, struct wiimote_data,
-						  battery);
+	struct wiimote_data *wdata = power_supply_get_drvdata(psy);
 	int ret = 0, state;
 	unsigned long flags;
 
@@ -238,42 +237,46 @@ static int wiimod_battery_get_property(struct power_supply *psy,
 static int wiimod_battery_probe(const struct wiimod_ops *ops,
 				struct wiimote_data *wdata)
 {
+	struct power_supply_config psy_cfg = { .drv_data = wdata, };
 	int ret;
 
-	wdata->battery.properties = wiimod_battery_props;
-	wdata->battery.num_properties = ARRAY_SIZE(wiimod_battery_props);
-	wdata->battery.get_property = wiimod_battery_get_property;
-	wdata->battery.type = POWER_SUPPLY_TYPE_BATTERY;
-	wdata->battery.use_for_apm = 0;
-	wdata->battery.name = kasprintf(GFP_KERNEL, "wiimote_battery_%s",
-					wdata->hdev->uniq);
-	if (!wdata->battery.name)
+	wdata->battery_desc.properties = wiimod_battery_props;
+	wdata->battery_desc.num_properties = ARRAY_SIZE(wiimod_battery_props);
+	wdata->battery_desc.get_property = wiimod_battery_get_property;
+	wdata->battery_desc.type = POWER_SUPPLY_TYPE_BATTERY;
+	wdata->battery_desc.use_for_apm = 0;
+	wdata->battery_desc.name = kasprintf(GFP_KERNEL, "wiimote_battery_%s",
+					     wdata->hdev->uniq);
+	if (!wdata->battery_desc.name)
 		return -ENOMEM;
 
-	ret = power_supply_register(&wdata->hdev->dev, &wdata->battery);
-	if (ret) {
+	wdata->battery = power_supply_register(&wdata->hdev->dev,
+					       &wdata->battery_desc,
+					       &psy_cfg);
+	if (IS_ERR(wdata->battery)) {
 		hid_err(wdata->hdev, "cannot register battery device\n");
+		ret = PTR_ERR(wdata->battery);
 		goto err_free;
 	}
 
-	power_supply_powers(&wdata->battery, &wdata->hdev->dev);
+	power_supply_powers(wdata->battery, &wdata->hdev->dev);
 	return 0;
 
 err_free:
-	kfree(wdata->battery.name);
-	wdata->battery.name = NULL;
+	kfree(wdata->battery_desc.name);
+	wdata->battery_desc.name = NULL;
 	return ret;
 }
 
 static void wiimod_battery_remove(const struct wiimod_ops *ops,
 				  struct wiimote_data *wdata)
 {
-	if (!wdata->battery.name)
+	if (!wdata->battery_desc.name)
 		return;
 
-	power_supply_unregister(&wdata->battery);
-	kfree(wdata->battery.name);
-	wdata->battery.name = NULL;
+	power_supply_unregister(wdata->battery);
+	kfree(wdata->battery_desc.name);
+	wdata->battery_desc.name = NULL;
 }
 
 static const struct wiimod_ops wiimod_battery = {
diff --git a/drivers/hid/hid-wiimote.h b/drivers/hid/hid-wiimote.h
index 10934aa129fb..875694d43e4d 100644
--- a/drivers/hid/hid-wiimote.h
+++ b/drivers/hid/hid-wiimote.h
@@ -147,7 +147,8 @@ struct wiimote_data {
 	struct led_classdev *leds[4];
 	struct input_dev *accel;
 	struct input_dev *ir;
-	struct power_supply battery;
+	struct power_supply *battery;
+	struct power_supply_desc battery_desc;
 	struct input_dev *mp;
 	struct timer_list timer;
 	struct wiimote_debug *debug;
diff --git a/drivers/hid/wacom.h b/drivers/hid/wacom.h
index ad7318db1dfe..024f4d89d579 100644
--- a/drivers/hid/wacom.h
+++ b/drivers/hid/wacom.h
@@ -119,8 +119,10 @@ struct wacom {
 		u8 img_lum;   /* OLED matrix display brightness */
 	} led;
 	bool led_initialized;
-	struct power_supply battery;
-	struct power_supply ac;
+	struct power_supply *battery;
+	struct power_supply *ac;
+	struct power_supply_desc battery_desc;
+	struct power_supply_desc ac_desc;
 };
 
 static inline void wacom_schedule_work(struct wacom_wac *wacom_wac)
diff --git a/drivers/hid/wacom_sys.c b/drivers/hid/wacom_sys.c
index 1b00d8d4466c..e8607d096138 100644
--- a/drivers/hid/wacom_sys.c
+++ b/drivers/hid/wacom_sys.c
@@ -961,7 +961,7 @@ static int wacom_battery_get_property(struct power_supply *psy,
 				      enum power_supply_property psp,
 				      union power_supply_propval *val)
 {
-	struct wacom *wacom = container_of(psy, struct wacom, battery);
+	struct wacom *wacom = power_supply_get_drvdata(psy);
 	int ret = 0;
 
 	switch (psp) {
@@ -998,7 +998,7 @@ static int wacom_ac_get_property(struct power_supply *psy,
 				enum power_supply_property psp,
 				union power_supply_propval *val)
 {
-	struct wacom *wacom = container_of(psy, struct wacom, ac);
+	struct wacom *wacom = power_supply_get_drvdata(psy);
 	int ret = 0;
 
 	switch (psp) {
@@ -1020,42 +1020,46 @@ static int wacom_ac_get_property(struct power_supply *psy,
 static int wacom_initialize_battery(struct wacom *wacom)
 {
 	static atomic_t battery_no = ATOMIC_INIT(0);
-	int error;
+	struct power_supply_config psy_cfg = { .drv_data = wacom, };
 	unsigned long n;
 
 	if (wacom->wacom_wac.features.quirks & WACOM_QUIRK_BATTERY) {
+		struct power_supply_desc *bat_desc = &wacom->battery_desc;
+		struct power_supply_desc *ac_desc = &wacom->ac_desc;
 		n = atomic_inc_return(&battery_no) - 1;
 
-		wacom->battery.properties = wacom_battery_props;
-		wacom->battery.num_properties = ARRAY_SIZE(wacom_battery_props);
-		wacom->battery.get_property = wacom_battery_get_property;
+		bat_desc->properties = wacom_battery_props;
+		bat_desc->num_properties = ARRAY_SIZE(wacom_battery_props);
+		bat_desc->get_property = wacom_battery_get_property;
 		sprintf(wacom->wacom_wac.bat_name, "wacom_battery_%ld", n);
-		wacom->battery.name = wacom->wacom_wac.bat_name;
-		wacom->battery.type = POWER_SUPPLY_TYPE_BATTERY;
-		wacom->battery.use_for_apm = 0;
+		bat_desc->name = wacom->wacom_wac.bat_name;
+		bat_desc->type = POWER_SUPPLY_TYPE_BATTERY;
+		bat_desc->use_for_apm = 0;
 
-		wacom->ac.properties = wacom_ac_props;
-		wacom->ac.num_properties = ARRAY_SIZE(wacom_ac_props);
-		wacom->ac.get_property = wacom_ac_get_property;
+		ac_desc->properties = wacom_ac_props;
+		ac_desc->num_properties = ARRAY_SIZE(wacom_ac_props);
+		ac_desc->get_property = wacom_ac_get_property;
 		sprintf(wacom->wacom_wac.ac_name, "wacom_ac_%ld", n);
-		wacom->ac.name = wacom->wacom_wac.ac_name;
-		wacom->ac.type = POWER_SUPPLY_TYPE_MAINS;
-		wacom->ac.use_for_apm = 0;
-
-		error = power_supply_register(&wacom->hdev->dev,
-					      &wacom->battery);
-		if (error)
-			return error;
-
-		power_supply_powers(&wacom->battery, &wacom->hdev->dev);
-
-		error = power_supply_register(&wacom->hdev->dev, &wacom->ac);
-		if (error) {
-			power_supply_unregister(&wacom->battery);
-			return error;
+		ac_desc->name = wacom->wacom_wac.ac_name;
+		ac_desc->type = POWER_SUPPLY_TYPE_MAINS;
+		ac_desc->use_for_apm = 0;
+
+		wacom->battery = power_supply_register(&wacom->hdev->dev,
+					      &wacom->battery_desc, &psy_cfg);
+		if (IS_ERR(wacom->battery))
+			return PTR_ERR(wacom->battery);
+
+		power_supply_powers(wacom->battery, &wacom->hdev->dev);
+
+		wacom->ac = power_supply_register(&wacom->hdev->dev,
+						  &wacom->ac_desc,
+						  &psy_cfg);
+		if (IS_ERR(wacom->ac)) {
+			power_supply_unregister(wacom->battery);
+			return PTR_ERR(wacom->ac);
 		}
 
-		power_supply_powers(&wacom->ac, &wacom->hdev->dev);
+		power_supply_powers(wacom->ac, &wacom->hdev->dev);
 	}
 
 	return 0;
@@ -1063,11 +1067,11 @@ static int wacom_initialize_battery(struct wacom *wacom)
 
 static void wacom_destroy_battery(struct wacom *wacom)
 {
-	if (wacom->battery.dev) {
-		power_supply_unregister(&wacom->battery);
-		wacom->battery.dev = NULL;
-		power_supply_unregister(&wacom->ac);
-		wacom->ac.dev = NULL;
+	if (wacom->battery) {
+		power_supply_unregister(wacom->battery);
+		wacom->battery = NULL;
+		power_supply_unregister(wacom->ac);
+		wacom->ac = NULL;
 	}
 }
 
@@ -1339,11 +1343,11 @@ void wacom_battery_work(struct work_struct *work)
 	struct wacom *wacom = container_of(work, struct wacom, work);
 
 	if ((wacom->wacom_wac.features.quirks & WACOM_QUIRK_BATTERY) &&
-	     !wacom->battery.dev) {
+	     !wacom->battery) {
 		wacom_initialize_battery(wacom);
 	}
 	else if (!(wacom->wacom_wac.features.quirks & WACOM_QUIRK_BATTERY) &&
-		 wacom->battery.dev) {
+		 wacom->battery) {
 		wacom_destroy_battery(wacom);
 	}
 }
diff --git a/drivers/hid/wacom_wac.c b/drivers/hid/wacom_wac.c
index 69c7df78281d..fa54d3290659 100644
--- a/drivers/hid/wacom_wac.c
+++ b/drivers/hid/wacom_wac.c
@@ -61,8 +61,8 @@ static void wacom_notify_battery(struct wacom_wac *wacom_wac,
 		wacom_wac->bat_connected = bat_connected;
 		wacom_wac->ps_connected = ps_connected;
 
-		if (wacom->battery.dev)
-			power_supply_changed(&wacom->battery);
+		if (wacom->battery)
+			power_supply_changed(wacom->battery);
 	}
 }
 
@@ -1991,7 +1991,7 @@ static int wacom_status_irq(struct wacom_wac *wacom_wac, size_t len)
 		wacom_notify_battery(wacom_wac, battery, charging,
 				     battery || charging, 1);
 
-		if (!wacom->battery.dev &&
+		if (!wacom->battery &&
 		    !(features->quirks & WACOM_QUIRK_BATTERY)) {
 			features->quirks |= WACOM_QUIRK_BATTERY;
 			INIT_WORK(&wacom->work, wacom_battery_work);
@@ -1999,7 +1999,7 @@ static int wacom_status_irq(struct wacom_wac *wacom_wac, size_t len)
 		}
 	}
 	else if ((features->quirks & WACOM_QUIRK_BATTERY) &&
-		 wacom->battery.dev) {
+		 wacom->battery) {
 		features->quirks &= ~WACOM_QUIRK_BATTERY;
 		INIT_WORK(&wacom->work, wacom_battery_work);
 		wacom_schedule_work(wacom_wac);
diff --git a/drivers/hsi/clients/Kconfig b/drivers/hsi/clients/Kconfig
index bc60dec3f586..d6126200361f 100644
--- a/drivers/hsi/clients/Kconfig
+++ b/drivers/hsi/clients/Kconfig
@@ -6,13 +6,23 @@ comment "HSI clients"
 
 config NOKIA_MODEM
 	tristate "Nokia Modem"
-	depends on HSI && SSI_PROTOCOL
+	depends on HSI && SSI_PROTOCOL && CMT_SPEECH
 	help
 	Say Y here if you want to add support for the modem on Nokia
 	N900 (Nokia RX-51) hardware.
 
 	If unsure, say N.
 
+config CMT_SPEECH
+	tristate "CMT speech"
+	depends on HSI && SSI_PROTOCOL
+	help
+	If you say Y here, you will enable the CMT speech protocol used
+	by Nokia modems. If you say M the protocol will be available as
+	module named cmt_speech.
+
+	If unsure, say N.
+
 config SSI_PROTOCOL
 	tristate "SSI protocol"
 	depends on HSI && PHONET && OMAP_SSI
diff --git a/drivers/hsi/clients/Makefile b/drivers/hsi/clients/Makefile
index 4d5bc0e0b27b..260723266407 100644
--- a/drivers/hsi/clients/Makefile
+++ b/drivers/hsi/clients/Makefile
@@ -4,4 +4,5 @@
 
 obj-$(CONFIG_NOKIA_MODEM)	+= nokia-modem.o
 obj-$(CONFIG_SSI_PROTOCOL)	+= ssi_protocol.o
+obj-$(CONFIG_CMT_SPEECH)	+= cmt_speech.o
 obj-$(CONFIG_HSI_CHAR)		+= hsi_char.o
diff --git a/drivers/hsi/clients/cmt_speech.c b/drivers/hsi/clients/cmt_speech.c
new file mode 100644
index 000000000000..4983529a9c6c
--- /dev/null
+++ b/drivers/hsi/clients/cmt_speech.c
@@ -0,0 +1,1457 @@
+/*
+ * cmt_speech.c - HSI CMT speech driver
+ *
+ * Copyright (C) 2008,2009,2010 Nokia Corporation. All rights reserved.
+ *
+ * Contact: Kai Vehmanen <kai.vehmanen@nokia.com>
+ * Original author: Peter Ujfalusi <peter.ujfalusi@nokia.com>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
+ * 02110-1301 USA
+ */
+
+#include <linux/errno.h>
+#include <linux/module.h>
+#include <linux/types.h>
+#include <linux/init.h>
+#include <linux/device.h>
+#include <linux/miscdevice.h>
+#include <linux/mm.h>
+#include <linux/slab.h>
+#include <linux/fs.h>
+#include <linux/poll.h>
+#include <linux/sched.h>
+#include <linux/ioctl.h>
+#include <linux/uaccess.h>
+#include <linux/pm_qos.h>
+#include <linux/hsi/hsi.h>
+#include <linux/hsi/ssi_protocol.h>
+#include <linux/hsi/cs-protocol.h>
+
+#define CS_MMAP_SIZE	PAGE_SIZE
+
+struct char_queue {
+	struct list_head	list;
+	u32			msg;
+};
+
+struct cs_char {
+	unsigned int		opened;
+	struct hsi_client	*cl;
+	struct cs_hsi_iface	*hi;
+	struct list_head	chardev_queue;
+	struct list_head	dataind_queue;
+	int			dataind_pending;
+	/* mmap things */
+	unsigned long		mmap_base;
+	unsigned long		mmap_size;
+	spinlock_t		lock;
+	struct fasync_struct	*async_queue;
+	wait_queue_head_t	wait;
+	/* hsi channel ids */
+	int                     channel_id_cmd;
+	int                     channel_id_data;
+};
+
+#define SSI_CHANNEL_STATE_READING	1
+#define SSI_CHANNEL_STATE_WRITING	(1 << 1)
+#define SSI_CHANNEL_STATE_POLL		(1 << 2)
+#define SSI_CHANNEL_STATE_ERROR		(1 << 3)
+
+#define TARGET_MASK			0xf000000
+#define TARGET_REMOTE			(1 << CS_DOMAIN_SHIFT)
+#define TARGET_LOCAL			0
+
+/* Number of pre-allocated commands buffers */
+#define CS_MAX_CMDS		        4
+
+/*
+ * During data transfers, transactions must be handled
+ * within 20ms (fixed value in cmtspeech HSI protocol)
+ */
+#define CS_QOS_LATENCY_FOR_DATA_USEC	20000
+
+/* Timeout to wait for pending HSI transfers to complete */
+#define CS_HSI_TRANSFER_TIMEOUT_MS      500
+
+
+#define RX_PTR_BOUNDARY_SHIFT		8
+#define RX_PTR_MAX_SHIFT		(RX_PTR_BOUNDARY_SHIFT + \
+						CS_MAX_BUFFERS_SHIFT)
+struct cs_hsi_iface {
+	struct hsi_client		*cl;
+	struct hsi_client		*master;
+
+	unsigned int			iface_state;
+	unsigned int			wakeline_state;
+	unsigned int			control_state;
+	unsigned int			data_state;
+
+	/* state exposed to application */
+	struct cs_mmap_config_block	*mmap_cfg;
+
+	unsigned long			mmap_base;
+	unsigned long			mmap_size;
+
+	unsigned int			rx_slot;
+	unsigned int			tx_slot;
+
+	/* note: for security reasons, we do not trust the contents of
+	 * mmap_cfg, but instead duplicate the variables here */
+	unsigned int			buf_size;
+	unsigned int			rx_bufs;
+	unsigned int			tx_bufs;
+	unsigned int			rx_ptr_boundary;
+	unsigned int			rx_offsets[CS_MAX_BUFFERS];
+	unsigned int			tx_offsets[CS_MAX_BUFFERS];
+
+	/* size of aligned memory blocks */
+	unsigned int			slot_size;
+	unsigned int			flags;
+
+	struct list_head		cmdqueue;
+
+	struct hsi_msg			*data_rx_msg;
+	struct hsi_msg			*data_tx_msg;
+	wait_queue_head_t		datawait;
+
+	struct pm_qos_request           pm_qos_req;
+
+	spinlock_t			lock;
+};
+
+static struct cs_char cs_char_data;
+
+static void cs_hsi_read_on_control(struct cs_hsi_iface *hi);
+static void cs_hsi_read_on_data(struct cs_hsi_iface *hi);
+
+static inline void rx_ptr_shift_too_big(void)
+{
+	BUILD_BUG_ON((1LLU << RX_PTR_MAX_SHIFT) > UINT_MAX);
+}
+
+static void cs_notify(u32 message, struct list_head *head)
+{
+	struct char_queue *entry;
+
+	spin_lock(&cs_char_data.lock);
+
+	if (!cs_char_data.opened) {
+		spin_unlock(&cs_char_data.lock);
+		goto out;
+	}
+
+	entry = kmalloc(sizeof(*entry), GFP_ATOMIC);
+	if (!entry) {
+		dev_err(&cs_char_data.cl->device,
+			"Can't allocate new entry for the queue.\n");
+		spin_unlock(&cs_char_data.lock);
+		goto out;
+	}
+
+	entry->msg = message;
+	list_add_tail(&entry->list, head);
+
+	spin_unlock(&cs_char_data.lock);
+
+	wake_up_interruptible(&cs_char_data.wait);
+	kill_fasync(&cs_char_data.async_queue, SIGIO, POLL_IN);
+
+out:
+	return;
+}
+
+static u32 cs_pop_entry(struct list_head *head)
+{
+	struct char_queue *entry;
+	u32 data;
+
+	entry = list_entry(head->next, struct char_queue, list);
+	data = entry->msg;
+	list_del(&entry->list);
+	kfree(entry);
+
+	return data;
+}
+
+static void cs_notify_control(u32 message)
+{
+	cs_notify(message, &cs_char_data.chardev_queue);
+}
+
+static void cs_notify_data(u32 message, int maxlength)
+{
+	cs_notify(message, &cs_char_data.dataind_queue);
+
+	spin_lock(&cs_char_data.lock);
+	cs_char_data.dataind_pending++;
+	while (cs_char_data.dataind_pending > maxlength &&
+				!list_empty(&cs_char_data.dataind_queue)) {
+		dev_dbg(&cs_char_data.cl->device, "data notification "
+		"queue overrun (%u entries)\n", cs_char_data.dataind_pending);
+
+		cs_pop_entry(&cs_char_data.dataind_queue);
+		cs_char_data.dataind_pending--;
+	}
+	spin_unlock(&cs_char_data.lock);
+}
+
+static inline void cs_set_cmd(struct hsi_msg *msg, u32 cmd)
+{
+	u32 *data = sg_virt(msg->sgt.sgl);
+	*data = cmd;
+}
+
+static inline u32 cs_get_cmd(struct hsi_msg *msg)
+{
+	u32 *data = sg_virt(msg->sgt.sgl);
+	return *data;
+}
+
+static void cs_release_cmd(struct hsi_msg *msg)
+{
+	struct cs_hsi_iface *hi = msg->context;
+
+	list_add_tail(&msg->link, &hi->cmdqueue);
+}
+
+static void cs_cmd_destructor(struct hsi_msg *msg)
+{
+	struct cs_hsi_iface *hi = msg->context;
+
+	spin_lock(&hi->lock);
+
+	dev_dbg(&cs_char_data.cl->device, "control cmd destructor\n");
+
+	if (hi->iface_state != CS_STATE_CLOSED)
+		dev_err(&hi->cl->device, "Cmd flushed while driver active\n");
+
+	if (msg->ttype == HSI_MSG_READ)
+		hi->control_state &=
+			~(SSI_CHANNEL_STATE_POLL | SSI_CHANNEL_STATE_READING);
+	else if (msg->ttype == HSI_MSG_WRITE &&
+			hi->control_state & SSI_CHANNEL_STATE_WRITING)
+		hi->control_state &= ~SSI_CHANNEL_STATE_WRITING;
+
+	cs_release_cmd(msg);
+
+	spin_unlock(&hi->lock);
+}
+
+static struct hsi_msg *cs_claim_cmd(struct cs_hsi_iface* ssi)
+{
+	struct hsi_msg *msg;
+
+	BUG_ON(list_empty(&ssi->cmdqueue));
+
+	msg = list_first_entry(&ssi->cmdqueue, struct hsi_msg, link);
+	list_del(&msg->link);
+	msg->destructor = cs_cmd_destructor;
+
+	return msg;
+}
+
+static void cs_free_cmds(struct cs_hsi_iface *ssi)
+{
+	struct hsi_msg *msg, *tmp;
+
+	list_for_each_entry_safe(msg, tmp, &ssi->cmdqueue, link) {
+		list_del(&msg->link);
+		msg->destructor = NULL;
+		kfree(sg_virt(msg->sgt.sgl));
+		hsi_free_msg(msg);
+	}
+}
+
+static int cs_alloc_cmds(struct cs_hsi_iface *hi)
+{
+	struct hsi_msg *msg;
+	u32 *buf;
+	unsigned int i;
+
+	INIT_LIST_HEAD(&hi->cmdqueue);
+
+	for (i = 0; i < CS_MAX_CMDS; i++) {
+		msg = hsi_alloc_msg(1, GFP_KERNEL);
+		if (!msg)
+			goto out;
+		buf = kmalloc(sizeof(*buf), GFP_KERNEL);
+		if (!buf) {
+			hsi_free_msg(msg);
+			goto out;
+		}
+		sg_init_one(msg->sgt.sgl, buf, sizeof(*buf));
+		msg->channel = cs_char_data.channel_id_cmd;
+		msg->context = hi;
+		list_add_tail(&msg->link, &hi->cmdqueue);
+	}
+
+	return 0;
+
+out:
+	cs_free_cmds(hi);
+	return -ENOMEM;
+}
+
+static void cs_hsi_data_destructor(struct hsi_msg *msg)
+{
+	struct cs_hsi_iface *hi = msg->context;
+	const char *dir = (msg->ttype == HSI_MSG_READ) ? "TX" : "RX";
+
+	dev_dbg(&cs_char_data.cl->device, "Freeing data %s message\n", dir);
+
+	spin_lock(&hi->lock);
+	if (hi->iface_state != CS_STATE_CLOSED)
+		dev_err(&cs_char_data.cl->device,
+				"Data %s flush while device active\n", dir);
+	if (msg->ttype == HSI_MSG_READ)
+		hi->data_state &=
+			~(SSI_CHANNEL_STATE_POLL | SSI_CHANNEL_STATE_READING);
+	else
+		hi->data_state &= ~SSI_CHANNEL_STATE_WRITING;
+
+	msg->status = HSI_STATUS_COMPLETED;
+	if (unlikely(waitqueue_active(&hi->datawait)))
+		wake_up_interruptible(&hi->datawait);
+
+	spin_unlock(&hi->lock);
+}
+
+static int cs_hsi_alloc_data(struct cs_hsi_iface *hi)
+{
+	struct hsi_msg *txmsg, *rxmsg;
+	int res = 0;
+
+	rxmsg = hsi_alloc_msg(1, GFP_KERNEL);
+	if (!rxmsg) {
+		res = -ENOMEM;
+		goto out1;
+	}
+	rxmsg->channel = cs_char_data.channel_id_data;
+	rxmsg->destructor = cs_hsi_data_destructor;
+	rxmsg->context = hi;
+
+	txmsg = hsi_alloc_msg(1, GFP_KERNEL);
+	if (!txmsg) {
+		res = -ENOMEM;
+		goto out2;
+	}
+	txmsg->channel = cs_char_data.channel_id_data;
+	txmsg->destructor = cs_hsi_data_destructor;
+	txmsg->context = hi;
+
+	hi->data_rx_msg = rxmsg;
+	hi->data_tx_msg = txmsg;
+
+	return 0;
+
+out2:
+	hsi_free_msg(rxmsg);
+out1:
+	return res;
+}
+
+static void cs_hsi_free_data_msg(struct hsi_msg *msg)
+{
+	WARN_ON(msg->status != HSI_STATUS_COMPLETED &&
+					msg->status != HSI_STATUS_ERROR);
+	hsi_free_msg(msg);
+}
+
+static void cs_hsi_free_data(struct cs_hsi_iface *hi)
+{
+	cs_hsi_free_data_msg(hi->data_rx_msg);
+	cs_hsi_free_data_msg(hi->data_tx_msg);
+}
+
+static inline void __cs_hsi_error_pre(struct cs_hsi_iface *hi,
+					struct hsi_msg *msg, const char *info,
+					unsigned int *state)
+{
+	spin_lock(&hi->lock);
+	dev_err(&hi->cl->device, "HSI %s error, msg %d, state %u\n",
+		info, msg->status, *state);
+}
+
+static inline void __cs_hsi_error_post(struct cs_hsi_iface *hi)
+{
+	spin_unlock(&hi->lock);
+}
+
+static inline void __cs_hsi_error_read_bits(unsigned int *state)
+{
+	*state |= SSI_CHANNEL_STATE_ERROR;
+	*state &= ~(SSI_CHANNEL_STATE_READING | SSI_CHANNEL_STATE_POLL);
+}
+
+static inline void __cs_hsi_error_write_bits(unsigned int *state)
+{
+	*state |= SSI_CHANNEL_STATE_ERROR;
+	*state &= ~SSI_CHANNEL_STATE_WRITING;
+}
+
+static void cs_hsi_control_read_error(struct cs_hsi_iface *hi,
+							struct hsi_msg *msg)
+{
+	__cs_hsi_error_pre(hi, msg, "control read", &hi->control_state);
+	cs_release_cmd(msg);
+	__cs_hsi_error_read_bits(&hi->control_state);
+	__cs_hsi_error_post(hi);
+}
+
+static void cs_hsi_control_write_error(struct cs_hsi_iface *hi,
+							struct hsi_msg *msg)
+{
+	__cs_hsi_error_pre(hi, msg, "control write", &hi->control_state);
+	cs_release_cmd(msg);
+	__cs_hsi_error_write_bits(&hi->control_state);
+	__cs_hsi_error_post(hi);
+
+}
+
+static void cs_hsi_data_read_error(struct cs_hsi_iface *hi, struct hsi_msg *msg)
+{
+	__cs_hsi_error_pre(hi, msg, "data read", &hi->data_state);
+	__cs_hsi_error_read_bits(&hi->data_state);
+	__cs_hsi_error_post(hi);
+}
+
+static void cs_hsi_data_write_error(struct cs_hsi_iface *hi,
+							struct hsi_msg *msg)
+{
+	__cs_hsi_error_pre(hi, msg, "data write", &hi->data_state);
+	__cs_hsi_error_write_bits(&hi->data_state);
+	__cs_hsi_error_post(hi);
+}
+
+static void cs_hsi_read_on_control_complete(struct hsi_msg *msg)
+{
+	u32 cmd = cs_get_cmd(msg);
+	struct cs_hsi_iface *hi = msg->context;
+
+	spin_lock(&hi->lock);
+	hi->control_state &= ~SSI_CHANNEL_STATE_READING;
+	if (msg->status == HSI_STATUS_ERROR) {
+		dev_err(&hi->cl->device, "Control RX error detected\n");
+		cs_hsi_control_read_error(hi, msg);
+		spin_unlock(&hi->lock);
+		goto out;
+	}
+	dev_dbg(&hi->cl->device, "Read on control: %08X\n", cmd);
+	cs_release_cmd(msg);
+	if (hi->flags & CS_FEAT_TSTAMP_RX_CTRL) {
+		struct timespec *tstamp =
+			&hi->mmap_cfg->tstamp_rx_ctrl;
+		do_posix_clock_monotonic_gettime(tstamp);
+	}
+	spin_unlock(&hi->lock);
+
+	cs_notify_control(cmd);
+
+out:
+	cs_hsi_read_on_control(hi);
+}
+
+static void cs_hsi_peek_on_control_complete(struct hsi_msg *msg)
+{
+	struct cs_hsi_iface *hi = msg->context;
+	int ret;
+
+	if (msg->status == HSI_STATUS_ERROR) {
+		dev_err(&hi->cl->device, "Control peek RX error detected\n");
+		cs_hsi_control_read_error(hi, msg);
+		return;
+	}
+
+	WARN_ON(!(hi->control_state & SSI_CHANNEL_STATE_READING));
+
+	dev_dbg(&hi->cl->device, "Peek on control complete, reading\n");
+	msg->sgt.nents = 1;
+	msg->complete = cs_hsi_read_on_control_complete;
+	ret = hsi_async_read(hi->cl, msg);
+	if (ret)
+		cs_hsi_control_read_error(hi, msg);
+}
+
+static void cs_hsi_read_on_control(struct cs_hsi_iface *hi)
+{
+	struct hsi_msg *msg;
+	int ret;
+
+	spin_lock(&hi->lock);
+	if (hi->control_state & SSI_CHANNEL_STATE_READING) {
+		dev_err(&hi->cl->device, "Control read already pending (%d)\n",
+			hi->control_state);
+		spin_unlock(&hi->lock);
+		return;
+	}
+	if (hi->control_state & SSI_CHANNEL_STATE_ERROR) {
+		dev_err(&hi->cl->device, "Control read error (%d)\n",
+			hi->control_state);
+		spin_unlock(&hi->lock);
+		return;
+	}
+	hi->control_state |= SSI_CHANNEL_STATE_READING;
+	dev_dbg(&hi->cl->device, "Issuing RX on control\n");
+	msg = cs_claim_cmd(hi);
+	spin_unlock(&hi->lock);
+
+	msg->sgt.nents = 0;
+	msg->complete = cs_hsi_peek_on_control_complete;
+	ret = hsi_async_read(hi->cl, msg);
+	if (ret)
+		cs_hsi_control_read_error(hi, msg);
+}
+
+static void cs_hsi_write_on_control_complete(struct hsi_msg *msg)
+{
+	struct cs_hsi_iface *hi = msg->context;
+	if (msg->status == HSI_STATUS_COMPLETED) {
+		spin_lock(&hi->lock);
+		hi->control_state &= ~SSI_CHANNEL_STATE_WRITING;
+		cs_release_cmd(msg);
+		spin_unlock(&hi->lock);
+	} else if (msg->status == HSI_STATUS_ERROR) {
+		cs_hsi_control_write_error(hi, msg);
+	} else {
+		dev_err(&hi->cl->device,
+			"unexpected status in control write callback %d\n",
+			msg->status);
+	}
+}
+
+static int cs_hsi_write_on_control(struct cs_hsi_iface *hi, u32 message)
+{
+	struct hsi_msg *msg;
+	int ret;
+
+	spin_lock(&hi->lock);
+	if (hi->control_state & SSI_CHANNEL_STATE_ERROR) {
+		spin_unlock(&hi->lock);
+		return -EIO;
+	}
+	if (hi->control_state & SSI_CHANNEL_STATE_WRITING) {
+		dev_err(&hi->cl->device,
+			"Write still pending on control channel.\n");
+		spin_unlock(&hi->lock);
+		return -EBUSY;
+	}
+	hi->control_state |= SSI_CHANNEL_STATE_WRITING;
+	msg = cs_claim_cmd(hi);
+	spin_unlock(&hi->lock);
+
+	cs_set_cmd(msg, message);
+	msg->sgt.nents = 1;
+	msg->complete = cs_hsi_write_on_control_complete;
+	dev_dbg(&hi->cl->device,
+		"Sending control message %08X\n", message);
+	ret = hsi_async_write(hi->cl, msg);
+	if (ret) {
+		dev_err(&hi->cl->device,
+			"async_write failed with %d\n", ret);
+		cs_hsi_control_write_error(hi, msg);
+	}
+
+	/*
+	 * Make sure control read is always pending when issuing
+	 * new control writes. This is needed as the controller
+	 * may flush our messages if e.g. the peer device reboots
+	 * unexpectedly (and we cannot directly resubmit a new read from
+	 * the message destructor; see cs_cmd_destructor()).
+	 */
+	if (!(hi->control_state & SSI_CHANNEL_STATE_READING)) {
+		dev_err(&hi->cl->device, "Restarting control reads\n");
+		cs_hsi_read_on_control(hi);
+	}
+
+	return 0;
+}
+
+static void cs_hsi_read_on_data_complete(struct hsi_msg *msg)
+{
+	struct cs_hsi_iface *hi = msg->context;
+	u32 payload;
+
+	if (unlikely(msg->status == HSI_STATUS_ERROR)) {
+		cs_hsi_data_read_error(hi, msg);
+		return;
+	}
+
+	spin_lock(&hi->lock);
+	WARN_ON(!(hi->data_state & SSI_CHANNEL_STATE_READING));
+	hi->data_state &= ~SSI_CHANNEL_STATE_READING;
+	payload = CS_RX_DATA_RECEIVED;
+	payload |= hi->rx_slot;
+	hi->rx_slot++;
+	hi->rx_slot %= hi->rx_ptr_boundary;
+	/* expose current rx ptr in mmap area */
+	hi->mmap_cfg->rx_ptr = hi->rx_slot;
+	if (unlikely(waitqueue_active(&hi->datawait)))
+		wake_up_interruptible(&hi->datawait);
+	spin_unlock(&hi->lock);
+
+	cs_notify_data(payload, hi->rx_bufs);
+	cs_hsi_read_on_data(hi);
+}
+
+static void cs_hsi_peek_on_data_complete(struct hsi_msg *msg)
+{
+	struct cs_hsi_iface *hi = msg->context;
+	u32 *address;
+	int ret;
+
+	if (unlikely(msg->status == HSI_STATUS_ERROR)) {
+		cs_hsi_data_read_error(hi, msg);
+		return;
+	}
+	if (unlikely(hi->iface_state != CS_STATE_CONFIGURED)) {
+		dev_err(&hi->cl->device, "Data received in invalid state\n");
+		cs_hsi_data_read_error(hi, msg);
+		return;
+	}
+
+	spin_lock(&hi->lock);
+	WARN_ON(!(hi->data_state & SSI_CHANNEL_STATE_POLL));
+	hi->data_state &= ~SSI_CHANNEL_STATE_POLL;
+	hi->data_state |= SSI_CHANNEL_STATE_READING;
+	spin_unlock(&hi->lock);
+
+	address = (u32 *)(hi->mmap_base +
+				hi->rx_offsets[hi->rx_slot % hi->rx_bufs]);
+	sg_init_one(msg->sgt.sgl, address, hi->buf_size);
+	msg->sgt.nents = 1;
+	msg->complete = cs_hsi_read_on_data_complete;
+	ret = hsi_async_read(hi->cl, msg);
+	if (ret)
+		cs_hsi_data_read_error(hi, msg);
+}
+
+/*
+ * Read/write transaction is ongoing. Returns false if in
+ * SSI_CHANNEL_STATE_POLL state.
+ */
+static inline int cs_state_xfer_active(unsigned int state)
+{
+	return (state & SSI_CHANNEL_STATE_WRITING) ||
+		(state & SSI_CHANNEL_STATE_READING);
+}
+
+/*
+ * No pending read/writes
+ */
+static inline int cs_state_idle(unsigned int state)
+{
+	return !(state & ~SSI_CHANNEL_STATE_ERROR);
+}
+
+static void cs_hsi_read_on_data(struct cs_hsi_iface *hi)
+{
+	struct hsi_msg *rxmsg;
+	int ret;
+
+	spin_lock(&hi->lock);
+	if (hi->data_state &
+		(SSI_CHANNEL_STATE_READING | SSI_CHANNEL_STATE_POLL)) {
+		dev_dbg(&hi->cl->device, "Data read already pending (%u)\n",
+			hi->data_state);
+		spin_unlock(&hi->lock);
+		return;
+	}
+	hi->data_state |= SSI_CHANNEL_STATE_POLL;
+	spin_unlock(&hi->lock);
+
+	rxmsg = hi->data_rx_msg;
+	sg_init_one(rxmsg->sgt.sgl, (void *)hi->mmap_base, 0);
+	rxmsg->sgt.nents = 0;
+	rxmsg->complete = cs_hsi_peek_on_data_complete;
+
+	ret = hsi_async_read(hi->cl, rxmsg);
+	if (ret)
+		cs_hsi_data_read_error(hi, rxmsg);
+}
+
+static void cs_hsi_write_on_data_complete(struct hsi_msg *msg)
+{
+	struct cs_hsi_iface *hi = msg->context;
+
+	if (msg->status == HSI_STATUS_COMPLETED) {
+		spin_lock(&hi->lock);
+		hi->data_state &= ~SSI_CHANNEL_STATE_WRITING;
+		if (unlikely(waitqueue_active(&hi->datawait)))
+			wake_up_interruptible(&hi->datawait);
+		spin_unlock(&hi->lock);
+	} else {
+		cs_hsi_data_write_error(hi, msg);
+	}
+}
+
+static int cs_hsi_write_on_data(struct cs_hsi_iface *hi, unsigned int slot)
+{
+	u32 *address;
+	struct hsi_msg *txmsg;
+	int ret;
+
+	spin_lock(&hi->lock);
+	if (hi->iface_state != CS_STATE_CONFIGURED) {
+		dev_err(&hi->cl->device, "Not configured, aborting\n");
+		ret = -EINVAL;
+		goto error;
+	}
+	if (hi->data_state & SSI_CHANNEL_STATE_ERROR) {
+		dev_err(&hi->cl->device, "HSI error, aborting\n");
+		ret = -EIO;
+		goto error;
+	}
+	if (hi->data_state & SSI_CHANNEL_STATE_WRITING) {
+		dev_err(&hi->cl->device, "Write pending on data channel.\n");
+		ret = -EBUSY;
+		goto error;
+	}
+	hi->data_state |= SSI_CHANNEL_STATE_WRITING;
+	spin_unlock(&hi->lock);
+
+	hi->tx_slot = slot;
+	address = (u32 *)(hi->mmap_base + hi->tx_offsets[hi->tx_slot]);
+	txmsg = hi->data_tx_msg;
+	sg_init_one(txmsg->sgt.sgl, address, hi->buf_size);
+	txmsg->complete = cs_hsi_write_on_data_complete;
+	ret = hsi_async_write(hi->cl, txmsg);
+	if (ret)
+		cs_hsi_data_write_error(hi, txmsg);
+
+	return ret;
+
+error:
+	spin_unlock(&hi->lock);
+	if (ret == -EIO)
+		cs_hsi_data_write_error(hi, hi->data_tx_msg);
+
+	return ret;
+}
+
+static unsigned int cs_hsi_get_state(struct cs_hsi_iface *hi)
+{
+	return hi->iface_state;
+}
+
+static int cs_hsi_command(struct cs_hsi_iface *hi, u32 cmd)
+{
+	int ret = 0;
+
+	local_bh_disable();
+	switch (cmd & TARGET_MASK) {
+	case TARGET_REMOTE:
+		ret = cs_hsi_write_on_control(hi, cmd);
+		break;
+	case TARGET_LOCAL:
+		if ((cmd & CS_CMD_MASK) == CS_TX_DATA_READY)
+			ret = cs_hsi_write_on_data(hi, cmd & CS_PARAM_MASK);
+		else
+			ret = -EINVAL;
+		break;
+	default:
+		ret = -EINVAL;
+		break;
+	}
+	local_bh_enable();
+
+	return ret;
+}
+
+static void cs_hsi_set_wakeline(struct cs_hsi_iface *hi, bool new_state)
+{
+	int change = 0;
+
+	spin_lock_bh(&hi->lock);
+	if (hi->wakeline_state != new_state) {
+		hi->wakeline_state = new_state;
+		change = 1;
+		dev_dbg(&hi->cl->device, "setting wake line to %d (%p)\n",
+			new_state, hi->cl);
+	}
+	spin_unlock_bh(&hi->lock);
+
+	if (change) {
+		if (new_state)
+			ssip_slave_start_tx(hi->master);
+		else
+			ssip_slave_stop_tx(hi->master);
+	}
+
+	dev_dbg(&hi->cl->device, "wake line set to %d (%p)\n",
+		new_state, hi->cl);
+}
+
+static void set_buffer_sizes(struct cs_hsi_iface *hi, int rx_bufs, int tx_bufs)
+{
+	hi->rx_bufs = rx_bufs;
+	hi->tx_bufs = tx_bufs;
+	hi->mmap_cfg->rx_bufs = rx_bufs;
+	hi->mmap_cfg->tx_bufs = tx_bufs;
+
+	if (hi->flags & CS_FEAT_ROLLING_RX_COUNTER) {
+		/*
+		 * For more robust overrun detection, let the rx
+		 * pointer run in range 0..'boundary-1'. Boundary
+		 * is a multiple of rx_bufs, and limited in max size
+		 * by RX_PTR_MAX_SHIFT to allow for fast ptr-diff
+		 * calculation.
+		 */
+		hi->rx_ptr_boundary = (rx_bufs << RX_PTR_BOUNDARY_SHIFT);
+		hi->mmap_cfg->rx_ptr_boundary = hi->rx_ptr_boundary;
+	} else {
+		hi->rx_ptr_boundary = hi->rx_bufs;
+	}
+}
+
+static int check_buf_params(struct cs_hsi_iface *hi,
+					const struct cs_buffer_config *buf_cfg)
+{
+	size_t buf_size_aligned = L1_CACHE_ALIGN(buf_cfg->buf_size) *
+					(buf_cfg->rx_bufs + buf_cfg->tx_bufs);
+	size_t ctrl_size_aligned = L1_CACHE_ALIGN(sizeof(*hi->mmap_cfg));
+	int r = 0;
+
+	if (buf_cfg->rx_bufs > CS_MAX_BUFFERS ||
+					buf_cfg->tx_bufs > CS_MAX_BUFFERS) {
+		r = -EINVAL;
+	} else if ((buf_size_aligned + ctrl_size_aligned) >= hi->mmap_size) {
+		dev_err(&hi->cl->device, "No space for the requested buffer "
+			"configuration\n");
+		r = -ENOBUFS;
+	}
+
+	return r;
+}
+
+/**
+ * Block until pending data transfers have completed.
+ */
+static int cs_hsi_data_sync(struct cs_hsi_iface *hi)
+{
+	int r = 0;
+
+	spin_lock_bh(&hi->lock);
+
+	if (!cs_state_xfer_active(hi->data_state)) {
+		dev_dbg(&hi->cl->device, "hsi_data_sync break, idle\n");
+		goto out;
+	}
+
+	for (;;) {
+		int s;
+		DEFINE_WAIT(wait);
+		if (!cs_state_xfer_active(hi->data_state))
+			goto out;
+		if (signal_pending(current)) {
+			r = -ERESTARTSYS;
+			goto out;
+		}
+		/**
+		 * prepare_to_wait must be called with hi->lock held
+		 * so that callbacks can check for waitqueue_active()
+		 */
+		prepare_to_wait(&hi->datawait, &wait, TASK_INTERRUPTIBLE);
+		spin_unlock_bh(&hi->lock);
+		s = schedule_timeout(
+			msecs_to_jiffies(CS_HSI_TRANSFER_TIMEOUT_MS));
+		spin_lock_bh(&hi->lock);
+		finish_wait(&hi->datawait, &wait);
+		if (!s) {
+			dev_dbg(&hi->cl->device,
+				"hsi_data_sync timeout after %d ms\n",
+				CS_HSI_TRANSFER_TIMEOUT_MS);
+			r = -EIO;
+			goto out;
+		}
+	}
+
+out:
+	spin_unlock_bh(&hi->lock);
+	dev_dbg(&hi->cl->device, "hsi_data_sync done with res %d\n", r);
+
+	return r;
+}
+
+static void cs_hsi_data_enable(struct cs_hsi_iface *hi,
+					struct cs_buffer_config *buf_cfg)
+{
+	unsigned int data_start, i;
+
+	BUG_ON(hi->buf_size == 0);
+
+	set_buffer_sizes(hi, buf_cfg->rx_bufs, buf_cfg->tx_bufs);
+
+	hi->slot_size = L1_CACHE_ALIGN(hi->buf_size);
+	dev_dbg(&hi->cl->device,
+			"setting slot size to %u, buf size %u, align %u\n",
+			hi->slot_size, hi->buf_size, L1_CACHE_BYTES);
+
+	data_start = L1_CACHE_ALIGN(sizeof(*hi->mmap_cfg));
+	dev_dbg(&hi->cl->device,
+			"setting data start at %u, cfg block %u, align %u\n",
+			data_start, sizeof(*hi->mmap_cfg), L1_CACHE_BYTES);
+
+	for (i = 0; i < hi->mmap_cfg->rx_bufs; i++) {
+		hi->rx_offsets[i] = data_start + i * hi->slot_size;
+		hi->mmap_cfg->rx_offsets[i] = hi->rx_offsets[i];
+		dev_dbg(&hi->cl->device, "DL buf #%u at %u\n",
+					i, hi->rx_offsets[i]);
+	}
+	for (i = 0; i < hi->mmap_cfg->tx_bufs; i++) {
+		hi->tx_offsets[i] = data_start +
+			(i + hi->mmap_cfg->rx_bufs) * hi->slot_size;
+		hi->mmap_cfg->tx_offsets[i] = hi->tx_offsets[i];
+		dev_dbg(&hi->cl->device, "UL buf #%u at %u\n",
+					i, hi->rx_offsets[i]);
+	}
+
+	hi->iface_state = CS_STATE_CONFIGURED;
+}
+
+static void cs_hsi_data_disable(struct cs_hsi_iface *hi, int old_state)
+{
+	if (old_state == CS_STATE_CONFIGURED) {
+		dev_dbg(&hi->cl->device,
+			"closing data channel with slot size 0\n");
+		hi->iface_state = CS_STATE_OPENED;
+	}
+}
+
+static int cs_hsi_buf_config(struct cs_hsi_iface *hi,
+					struct cs_buffer_config *buf_cfg)
+{
+	int r = 0;
+	unsigned int old_state = hi->iface_state;
+
+	spin_lock_bh(&hi->lock);
+	/* Prevent new transactions during buffer reconfig */
+	if (old_state == CS_STATE_CONFIGURED)
+		hi->iface_state = CS_STATE_OPENED;
+	spin_unlock_bh(&hi->lock);
+
+	/*
+	 * make sure that no non-zero data reads are ongoing before
+	 * proceeding to change the buffer layout
+	 */
+	r = cs_hsi_data_sync(hi);
+	if (r < 0)
+		return r;
+
+	WARN_ON(cs_state_xfer_active(hi->data_state));
+
+	spin_lock_bh(&hi->lock);
+	r = check_buf_params(hi, buf_cfg);
+	if (r < 0)
+		goto error;
+
+	hi->buf_size = buf_cfg->buf_size;
+	hi->mmap_cfg->buf_size = hi->buf_size;
+	hi->flags = buf_cfg->flags;
+
+	hi->rx_slot = 0;
+	hi->tx_slot = 0;
+	hi->slot_size = 0;
+
+	if (hi->buf_size)
+		cs_hsi_data_enable(hi, buf_cfg);
+	else
+		cs_hsi_data_disable(hi, old_state);
+
+	spin_unlock_bh(&hi->lock);
+
+	if (old_state != hi->iface_state) {
+		if (hi->iface_state == CS_STATE_CONFIGURED) {
+			pm_qos_add_request(&hi->pm_qos_req,
+				PM_QOS_CPU_DMA_LATENCY,
+				CS_QOS_LATENCY_FOR_DATA_USEC);
+			local_bh_disable();
+			cs_hsi_read_on_data(hi);
+			local_bh_enable();
+		} else if (old_state == CS_STATE_CONFIGURED) {
+			pm_qos_remove_request(&hi->pm_qos_req);
+		}
+	}
+	return r;
+
+error:
+	spin_unlock_bh(&hi->lock);
+	return r;
+}
+
+static int cs_hsi_start(struct cs_hsi_iface **hi, struct hsi_client *cl,
+			unsigned long mmap_base, unsigned long mmap_size)
+{
+	int err = 0;
+	struct cs_hsi_iface *hsi_if = kzalloc(sizeof(*hsi_if), GFP_KERNEL);
+
+	dev_dbg(&cl->device, "cs_hsi_start\n");
+
+	if (!hsi_if) {
+		err = -ENOMEM;
+		goto leave0;
+	}
+	spin_lock_init(&hsi_if->lock);
+	hsi_if->cl = cl;
+	hsi_if->iface_state = CS_STATE_CLOSED;
+	hsi_if->mmap_cfg = (struct cs_mmap_config_block *)mmap_base;
+	hsi_if->mmap_base = mmap_base;
+	hsi_if->mmap_size = mmap_size;
+	memset(hsi_if->mmap_cfg, 0, sizeof(*hsi_if->mmap_cfg));
+	init_waitqueue_head(&hsi_if->datawait);
+	err = cs_alloc_cmds(hsi_if);
+	if (err < 0) {
+		dev_err(&cl->device, "Unable to alloc HSI messages\n");
+		goto leave1;
+	}
+	err = cs_hsi_alloc_data(hsi_if);
+	if (err < 0) {
+		dev_err(&cl->device, "Unable to alloc HSI messages for data\n");
+		goto leave2;
+	}
+	err = hsi_claim_port(cl, 1);
+	if (err < 0) {
+		dev_err(&cl->device,
+				"Could not open, HSI port already claimed\n");
+		goto leave3;
+	}
+	hsi_if->master = ssip_slave_get_master(cl);
+	if (IS_ERR(hsi_if->master)) {
+		err = PTR_ERR(hsi_if->master);
+		dev_err(&cl->device, "Could not get HSI master client\n");
+		goto leave4;
+	}
+	if (!ssip_slave_running(hsi_if->master)) {
+		err = -ENODEV;
+		dev_err(&cl->device,
+				"HSI port not initialized\n");
+		goto leave4;
+	}
+
+	hsi_if->iface_state = CS_STATE_OPENED;
+	local_bh_disable();
+	cs_hsi_read_on_control(hsi_if);
+	local_bh_enable();
+
+	dev_dbg(&cl->device, "cs_hsi_start...done\n");
+
+	BUG_ON(!hi);
+	*hi = hsi_if;
+
+	return 0;
+
+leave4:
+	hsi_release_port(cl);
+leave3:
+	cs_hsi_free_data(hsi_if);
+leave2:
+	cs_free_cmds(hsi_if);
+leave1:
+	kfree(hsi_if);
+leave0:
+	dev_dbg(&cl->device, "cs_hsi_start...done/error\n\n");
+
+	return err;
+}
+
+static void cs_hsi_stop(struct cs_hsi_iface *hi)
+{
+	dev_dbg(&hi->cl->device, "cs_hsi_stop\n");
+	cs_hsi_set_wakeline(hi, 0);
+	ssip_slave_put_master(hi->master);
+
+	/* hsi_release_port() needs to be called with CS_STATE_CLOSED */
+	hi->iface_state = CS_STATE_CLOSED;
+	hsi_release_port(hi->cl);
+
+	/*
+	 * hsi_release_port() should flush out all the pending
+	 * messages, so cs_state_idle() should be true for both
+	 * control and data channels.
+	 */
+	WARN_ON(!cs_state_idle(hi->control_state));
+	WARN_ON(!cs_state_idle(hi->data_state));
+
+	if (pm_qos_request_active(&hi->pm_qos_req))
+		pm_qos_remove_request(&hi->pm_qos_req);
+
+	spin_lock_bh(&hi->lock);
+	cs_hsi_free_data(hi);
+	cs_free_cmds(hi);
+	spin_unlock_bh(&hi->lock);
+	kfree(hi);
+}
+
+static int cs_char_vma_fault(struct vm_area_struct *vma, struct vm_fault *vmf)
+{
+	struct cs_char *csdata = vma->vm_private_data;
+	struct page *page;
+
+	page = virt_to_page(csdata->mmap_base);
+	get_page(page);
+	vmf->page = page;
+
+	return 0;
+}
+
+static struct vm_operations_struct cs_char_vm_ops = {
+	.fault	= cs_char_vma_fault,
+};
+
+static int cs_char_fasync(int fd, struct file *file, int on)
+{
+	struct cs_char *csdata = file->private_data;
+
+	if (fasync_helper(fd, file, on, &csdata->async_queue) < 0)
+		return -EIO;
+
+	return 0;
+}
+
+static unsigned int cs_char_poll(struct file *file, poll_table *wait)
+{
+	struct cs_char *csdata = file->private_data;
+	unsigned int ret = 0;
+
+	poll_wait(file, &cs_char_data.wait, wait);
+	spin_lock_bh(&csdata->lock);
+	if (!list_empty(&csdata->chardev_queue))
+		ret = POLLIN | POLLRDNORM;
+	else if (!list_empty(&csdata->dataind_queue))
+		ret = POLLIN | POLLRDNORM;
+	spin_unlock_bh(&csdata->lock);
+
+	return ret;
+}
+
+static ssize_t cs_char_read(struct file *file, char __user *buf, size_t count,
+								loff_t *unused)
+{
+	struct cs_char *csdata = file->private_data;
+	u32 data;
+	ssize_t retval;
+
+	if (count < sizeof(data))
+		return -EINVAL;
+
+	for (;;) {
+		DEFINE_WAIT(wait);
+
+		spin_lock_bh(&csdata->lock);
+		if (!list_empty(&csdata->chardev_queue)) {
+			data = cs_pop_entry(&csdata->chardev_queue);
+		} else if (!list_empty(&csdata->dataind_queue)) {
+			data = cs_pop_entry(&csdata->dataind_queue);
+			csdata->dataind_pending--;
+		} else {
+			data = 0;
+		}
+		spin_unlock_bh(&csdata->lock);
+
+		if (data)
+			break;
+		if (file->f_flags & O_NONBLOCK) {
+			retval = -EAGAIN;
+			goto out;
+		} else if (signal_pending(current)) {
+			retval = -ERESTARTSYS;
+			goto out;
+		}
+		prepare_to_wait_exclusive(&csdata->wait, &wait,
+						TASK_INTERRUPTIBLE);
+		schedule();
+		finish_wait(&csdata->wait, &wait);
+	}
+
+	retval = put_user(data, (u32 __user *)buf);
+	if (!retval)
+		retval = sizeof(data);
+
+out:
+	return retval;
+}
+
+static ssize_t cs_char_write(struct file *file, const char __user *buf,
+						size_t count, loff_t *unused)
+{
+	struct cs_char *csdata = file->private_data;
+	u32 data;
+	int err;
+	ssize_t	retval;
+
+	if (count < sizeof(data))
+		return -EINVAL;
+
+	if (get_user(data, (u32 __user *)buf))
+		retval = -EFAULT;
+	else
+		retval = count;
+
+	err = cs_hsi_command(csdata->hi, data);
+	if (err < 0)
+		retval = err;
+
+	return retval;
+}
+
+static long cs_char_ioctl(struct file *file, unsigned int cmd,
+				unsigned long arg)
+{
+	struct cs_char *csdata = file->private_data;
+	int r = 0;
+
+	switch (cmd) {
+	case CS_GET_STATE: {
+		unsigned int state;
+
+		state = cs_hsi_get_state(csdata->hi);
+		if (copy_to_user((void __user *)arg, &state, sizeof(state)))
+			r = -EFAULT;
+
+		break;
+	}
+	case CS_SET_WAKELINE: {
+		unsigned int state;
+
+		if (copy_from_user(&state, (void __user *)arg, sizeof(state))) {
+			r = -EFAULT;
+			break;
+		}
+
+		if (state > 1) {
+			r = -EINVAL;
+			break;
+		}
+
+		cs_hsi_set_wakeline(csdata->hi, !!state);
+
+		break;
+	}
+	case CS_GET_IF_VERSION: {
+		unsigned int ifver = CS_IF_VERSION;
+
+		if (copy_to_user((void __user *)arg, &ifver, sizeof(ifver)))
+			r = -EFAULT;
+
+		break;
+	}
+	case CS_CONFIG_BUFS: {
+		struct cs_buffer_config buf_cfg;
+
+		if (copy_from_user(&buf_cfg, (void __user *)arg,
+							sizeof(buf_cfg)))
+			r = -EFAULT;
+		else
+			r = cs_hsi_buf_config(csdata->hi, &buf_cfg);
+
+		break;
+	}
+	default:
+		r = -ENOTTY;
+		break;
+	}
+
+	return r;
+}
+
+static int cs_char_mmap(struct file *file, struct vm_area_struct *vma)
+{
+	if (vma->vm_end < vma->vm_start)
+		return -EINVAL;
+
+	if (((vma->vm_end - vma->vm_start) >> PAGE_SHIFT) != 1)
+		return -EINVAL;
+
+	vma->vm_flags |= VM_IO | VM_DONTDUMP | VM_DONTEXPAND;
+	vma->vm_ops = &cs_char_vm_ops;
+	vma->vm_private_data = file->private_data;
+
+	return 0;
+}
+
+static int cs_char_open(struct inode *unused, struct file *file)
+{
+	int ret = 0;
+	unsigned long p;
+
+	spin_lock_bh(&cs_char_data.lock);
+	if (cs_char_data.opened) {
+		ret = -EBUSY;
+		spin_unlock_bh(&cs_char_data.lock);
+		goto out1;
+	}
+	cs_char_data.opened = 1;
+	cs_char_data.dataind_pending = 0;
+	spin_unlock_bh(&cs_char_data.lock);
+
+	p = get_zeroed_page(GFP_KERNEL);
+	if (!p) {
+		ret = -ENOMEM;
+		goto out2;
+	}
+
+	ret = cs_hsi_start(&cs_char_data.hi, cs_char_data.cl, p, CS_MMAP_SIZE);
+	if (ret) {
+		dev_err(&cs_char_data.cl->device, "Unable to initialize HSI\n");
+		goto out3;
+	}
+
+	/* these are only used in release so lock not needed */
+	cs_char_data.mmap_base = p;
+	cs_char_data.mmap_size = CS_MMAP_SIZE;
+
+	file->private_data = &cs_char_data;
+
+	return 0;
+
+out3:
+	free_page(p);
+out2:
+	spin_lock_bh(&cs_char_data.lock);
+	cs_char_data.opened = 0;
+	spin_unlock_bh(&cs_char_data.lock);
+out1:
+	return ret;
+}
+
+static void cs_free_char_queue(struct list_head *head)
+{
+	struct char_queue *entry;
+	struct list_head *cursor, *next;
+
+	if (!list_empty(head)) {
+		list_for_each_safe(cursor, next, head) {
+			entry = list_entry(cursor, struct char_queue, list);
+			list_del(&entry->list);
+			kfree(entry);
+		}
+	}
+
+}
+
+static int cs_char_release(struct inode *unused, struct file *file)
+{
+	struct cs_char *csdata = file->private_data;
+
+	cs_hsi_stop(csdata->hi);
+	spin_lock_bh(&csdata->lock);
+	csdata->hi = NULL;
+	free_page(csdata->mmap_base);
+	cs_free_char_queue(&csdata->chardev_queue);
+	cs_free_char_queue(&csdata->dataind_queue);
+	csdata->opened = 0;
+	spin_unlock_bh(&csdata->lock);
+
+	return 0;
+}
+
+static const struct file_operations cs_char_fops = {
+	.owner		= THIS_MODULE,
+	.read		= cs_char_read,
+	.write		= cs_char_write,
+	.poll		= cs_char_poll,
+	.unlocked_ioctl	= cs_char_ioctl,
+	.mmap		= cs_char_mmap,
+	.open		= cs_char_open,
+	.release	= cs_char_release,
+	.fasync		= cs_char_fasync,
+};
+
+static struct miscdevice cs_char_miscdev = {
+	.minor	= MISC_DYNAMIC_MINOR,
+	.name	= "cmt_speech",
+	.fops	= &cs_char_fops
+};
+
+static int cs_hsi_client_probe(struct device *dev)
+{
+	int err = 0;
+	struct hsi_client *cl = to_hsi_client(dev);
+
+	dev_dbg(dev, "hsi_client_probe\n");
+	init_waitqueue_head(&cs_char_data.wait);
+	spin_lock_init(&cs_char_data.lock);
+	cs_char_data.opened = 0;
+	cs_char_data.cl = cl;
+	cs_char_data.hi = NULL;
+	INIT_LIST_HEAD(&cs_char_data.chardev_queue);
+	INIT_LIST_HEAD(&cs_char_data.dataind_queue);
+
+	cs_char_data.channel_id_cmd = hsi_get_channel_id_by_name(cl,
+		"speech-control");
+	if (cs_char_data.channel_id_cmd < 0) {
+		err = cs_char_data.channel_id_cmd;
+		dev_err(dev, "Could not get cmd channel (%d)\n", err);
+		return err;
+	}
+
+	cs_char_data.channel_id_data = hsi_get_channel_id_by_name(cl,
+		"speech-data");
+	if (cs_char_data.channel_id_data < 0) {
+		err = cs_char_data.channel_id_data;
+		dev_err(dev, "Could not get data channel (%d)\n", err);
+		return err;
+	}
+
+	err = misc_register(&cs_char_miscdev);
+	if (err)
+		dev_err(dev, "Failed to register: %d\n", err);
+
+	return err;
+}
+
+static int cs_hsi_client_remove(struct device *dev)
+{
+	struct cs_hsi_iface *hi;
+
+	dev_dbg(dev, "hsi_client_remove\n");
+	misc_deregister(&cs_char_miscdev);
+	spin_lock_bh(&cs_char_data.lock);
+	hi = cs_char_data.hi;
+	cs_char_data.hi = NULL;
+	spin_unlock_bh(&cs_char_data.lock);
+	if (hi)
+		cs_hsi_stop(hi);
+
+	return 0;
+}
+
+static struct hsi_client_driver cs_hsi_driver = {
+	.driver = {
+		.name	= "cmt-speech",
+		.owner	= THIS_MODULE,
+		.probe	= cs_hsi_client_probe,
+		.remove	= cs_hsi_client_remove,
+	},
+};
+
+static int __init cs_char_init(void)
+{
+	pr_info("CMT speech driver added\n");
+	return hsi_register_client_driver(&cs_hsi_driver);
+}
+module_init(cs_char_init);
+
+static void __exit cs_char_exit(void)
+{
+	hsi_unregister_client_driver(&cs_hsi_driver);
+	pr_info("CMT speech driver removed\n");
+}
+module_exit(cs_char_exit);
+
+MODULE_ALIAS("hsi:cmt-speech");
+MODULE_AUTHOR("Kai Vehmanen <kai.vehmanen@nokia.com>");
+MODULE_AUTHOR("Peter Ujfalusi <peter.ujfalusi@nokia.com>");
+MODULE_DESCRIPTION("CMT speech driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/hsi/clients/nokia-modem.c b/drivers/hsi/clients/nokia-modem.c
index eb4dc63dbc93..bbb19231fa82 100644
--- a/drivers/hsi/clients/nokia-modem.c
+++ b/drivers/hsi/clients/nokia-modem.c
@@ -46,6 +46,7 @@ struct nokia_modem_device {
 	struct nokia_modem_gpio	*gpios;
 	int			gpio_amount;
 	struct hsi_client	*ssi_protocol;
+	struct hsi_client	*cmt_speech;
 };
 
 static void do_nokia_modem_rst_ind_tasklet(unsigned long data)
@@ -149,6 +150,7 @@ static int nokia_modem_probe(struct device *dev)
 	struct hsi_port *port = hsi_get_port(cl);
 	int irq, pflags, err;
 	struct hsi_board_info ssip;
+	struct hsi_board_info cmtspeech;
 
 	np = dev->of_node;
 	if (!np) {
@@ -200,6 +202,7 @@ static int nokia_modem_probe(struct device *dev)
 	modem->ssi_protocol = hsi_new_client(port, &ssip);
 	if (!modem->ssi_protocol) {
 		dev_err(dev, "Could not register ssi-protocol device\n");
+		err = -ENOMEM;
 		goto error2;
 	}
 
@@ -213,12 +216,35 @@ static int nokia_modem_probe(struct device *dev)
 		goto error3;
 	}
 
-	/* TODO: register cmt-speech hsi client */
+	cmtspeech.name = "cmt-speech";
+	cmtspeech.tx_cfg = cl->tx_cfg;
+	cmtspeech.rx_cfg = cl->rx_cfg;
+	cmtspeech.platform_data = NULL;
+	cmtspeech.archdata = NULL;
+
+	modem->cmt_speech = hsi_new_client(port, &cmtspeech);
+	if (!modem->cmt_speech) {
+		dev_err(dev, "Could not register cmt-speech device\n");
+		err = -ENOMEM;
+		goto error3;
+	}
+
+	err = device_attach(&modem->cmt_speech->device);
+	if (err == 0) {
+		dev_err(dev, "Missing cmt-speech driver\n");
+		err = -EPROBE_DEFER;
+		goto error4;
+	} else if (err < 0) {
+		dev_err(dev, "Could not load cmt-speech driver (%d)\n", err);
+		goto error4;
+	}
 
 	dev_info(dev, "Registered Nokia HSI modem\n");
 
 	return 0;
 
+error4:
+	hsi_remove_client(&modem->cmt_speech->device, NULL);
 error3:
 	hsi_remove_client(&modem->ssi_protocol->device, NULL);
 error2:
@@ -237,6 +263,11 @@ static int nokia_modem_remove(struct device *dev)
 	if (!modem)
 		return 0;
 
+	if (modem->cmt_speech) {
+		hsi_remove_client(&modem->cmt_speech->device, NULL);
+		modem->cmt_speech = NULL;
+	}
+
 	if (modem->ssi_protocol) {
 		hsi_remove_client(&modem->ssi_protocol->device, NULL);
 		modem->ssi_protocol = NULL;
diff --git a/drivers/hwmon/Kconfig b/drivers/hwmon/Kconfig
index 110fade9cb74..25d9e72627e9 100644
--- a/drivers/hwmon/Kconfig
+++ b/drivers/hwmon/Kconfig
@@ -510,6 +510,7 @@ config SENSORS_G762
 config SENSORS_GPIO_FAN
 	tristate "GPIO fan"
 	depends on GPIOLIB
+	depends on THERMAL || THERMAL=n
 	help
 	  If you say yes here you get support for fans connected to GPIO lines.
 
@@ -599,8 +600,8 @@ config SENSORS_IT87
 	help
 	  If you say yes here you get support for ITE IT8705F, IT8712F,
 	  IT8716F, IT8718F, IT8720F, IT8721F, IT8726F, IT8728F, IT8758E,
-	  IT8771E, IT8772E, IT8782F, IT8783E/F and IT8603E sensor chips,
-	  and the SiS950 clone.
+	  IT8771E, IT8772E, IT8781F, IT8782F, IT8783E/F, IT8786E, IT8790E,
+	  IT8603E, IT8620E, and IT8623E sensor chips, and the SiS950 clone.
 
 	  This driver can also be built as a module.  If so, the module
 	  will be called it87.
@@ -624,7 +625,7 @@ config SENSORS_JC42
 	  mobile devices and servers.  Support will include, but not be limited
 	  to, ADT7408, AT30TS00, CAT34TS02, CAT6095, MAX6604, MCP9804, MCP9805,
 	  MCP98242, MCP98243, MCP98244, MCP9843, SE97, SE98, STTS424(E),
-	  STTS2002, STTS3000, TSE2002B3, TSE2002GB2, TS3000B3, and TS3000GB2.
+	  STTS2002, STTS3000, TSE2002, TSE2004, TS3000, and TS3001.
 
 	  This driver can also be built as a module.  If so, the module
 	  will be called jc42.
@@ -1145,6 +1146,16 @@ config SENSORS_NCT7802
 	  This driver can also be built as a module.  If so, the module
 	  will be called nct7802.
 
+config SENSORS_NCT7904
+	tristate "Nuvoton NCT7904"
+	depends on I2C
+	help
+	  If you say yes here you get support for the Nuvoton NCT7904
+	  hardware monitoring chip, including manual fan speed control.
+
+	  This driver can also be built as a module.  If so, the module
+	  will be called nct7904.
+
 config SENSORS_PCF8591
 	tristate "Philips PCF8591 ADC/DAC"
 	depends on I2C
@@ -1164,6 +1175,7 @@ source drivers/hwmon/pmbus/Kconfig
 config SENSORS_PWM_FAN
 	tristate "PWM fan"
 	depends on (PWM && OF) || COMPILE_TEST
+	depends on THERMAL || THERMAL=n
 	help
 	  If you say yes here you get support for fans connected to PWM lines.
 	  The driver uses the generic PWM interface, thus it will work on a
diff --git a/drivers/hwmon/Makefile b/drivers/hwmon/Makefile
index 6c941472e707..b4a40f17e2aa 100644
--- a/drivers/hwmon/Makefile
+++ b/drivers/hwmon/Makefile
@@ -120,6 +120,7 @@ obj-$(CONFIG_SENSORS_MENF21BMC_HWMON) += menf21bmc_hwmon.o
 obj-$(CONFIG_SENSORS_NCT6683)	+= nct6683.o
 obj-$(CONFIG_SENSORS_NCT6775)	+= nct6775.o
 obj-$(CONFIG_SENSORS_NCT7802)	+= nct7802.o
+obj-$(CONFIG_SENSORS_NCT7904)	+= nct7904.o
 obj-$(CONFIG_SENSORS_NTC_THERMISTOR)	+= ntc_thermistor.o
 obj-$(CONFIG_SENSORS_PC87360)	+= pc87360.o
 obj-$(CONFIG_SENSORS_PC87427)	+= pc87427.o
diff --git a/drivers/hwmon/ads7828.c b/drivers/hwmon/ads7828.c
index bce4e9ff21bf..6c99ee7bafa3 100644
--- a/drivers/hwmon/ads7828.c
+++ b/drivers/hwmon/ads7828.c
@@ -147,6 +147,9 @@ static int ads7828_probe(struct i2c_client *client,
 						    &ads2830_regmap_config);
 	}
 
+	if (IS_ERR(data->regmap))
+		return PTR_ERR(data->regmap);
+
 	data->cmd_byte = ext_vref ? ADS7828_CMD_PD1 : ADS7828_CMD_PD3;
 	if (!diff_input)
 		data->cmd_byte |= ADS7828_CMD_SD_SE;
diff --git a/drivers/hwmon/coretemp.c b/drivers/hwmon/coretemp.c
index 5b7fec824f10..ed303ba3a593 100644
--- a/drivers/hwmon/coretemp.c
+++ b/drivers/hwmon/coretemp.c
@@ -397,14 +397,13 @@ static int create_core_attrs(struct temp_data *tdata, struct device *dev,
 			struct device_attribute *devattr, char *buf) = {
 			show_label, show_crit_alarm, show_temp, show_tjmax,
 			show_ttarget };
-	static const char *const names[TOTAL_ATTRS] = {
-					"temp%d_label", "temp%d_crit_alarm",
-					"temp%d_input", "temp%d_crit",
-					"temp%d_max" };
+	static const char *const suffixes[TOTAL_ATTRS] = {
+		"label", "crit_alarm", "input", "crit", "max"
+	};
 
 	for (i = 0; i < tdata->attr_size; i++) {
-		snprintf(tdata->attr_name[i], CORETEMP_NAME_LENGTH, names[i],
-			attr_no);
+		snprintf(tdata->attr_name[i], CORETEMP_NAME_LENGTH,
+			 "temp%d_%s", attr_no, suffixes[i]);
 		sysfs_attr_init(&tdata->sd_attrs[i].dev_attr.attr);
 		tdata->sd_attrs[i].dev_attr.attr.name = tdata->attr_name[i];
 		tdata->sd_attrs[i].dev_attr.attr.mode = S_IRUGO;
diff --git a/drivers/hwmon/gpio-fan.c b/drivers/hwmon/gpio-fan.c
index 36abf814b8c7..a3dae6d0082a 100644
--- a/drivers/hwmon/gpio-fan.c
+++ b/drivers/hwmon/gpio-fan.c
@@ -34,10 +34,13 @@
 #include <linux/of.h>
 #include <linux/of_platform.h>
 #include <linux/of_gpio.h>
+#include <linux/thermal.h>
 
 struct gpio_fan_data {
 	struct platform_device	*pdev;
 	struct device		*hwmon_dev;
+	/* Cooling device if any */
+	struct thermal_cooling_device *cdev;
 	struct mutex		lock; /* lock GPIOs operations. */
 	int			num_ctrl;
 	unsigned		*ctrl;
@@ -387,6 +390,53 @@ static int fan_ctrl_init(struct gpio_fan_data *fan_data,
 	return 0;
 }
 
+static int gpio_fan_get_max_state(struct thermal_cooling_device *cdev,
+				  unsigned long *state)
+{
+	struct gpio_fan_data *fan_data = cdev->devdata;
+
+	if (!fan_data)
+		return -EINVAL;
+
+	*state = fan_data->num_speed - 1;
+	return 0;
+}
+
+static int gpio_fan_get_cur_state(struct thermal_cooling_device *cdev,
+				  unsigned long *state)
+{
+	struct gpio_fan_data *fan_data = cdev->devdata;
+	int r;
+
+	if (!fan_data)
+		return -EINVAL;
+
+	r = get_fan_speed_index(fan_data);
+	if (r < 0)
+		return r;
+
+	*state = r;
+	return 0;
+}
+
+static int gpio_fan_set_cur_state(struct thermal_cooling_device *cdev,
+				  unsigned long state)
+{
+	struct gpio_fan_data *fan_data = cdev->devdata;
+
+	if (!fan_data)
+		return -EINVAL;
+
+	set_fan_speed(fan_data, state);
+	return 0;
+}
+
+static const struct thermal_cooling_device_ops gpio_fan_cool_ops = {
+	.get_max_state = gpio_fan_get_max_state,
+	.get_cur_state = gpio_fan_get_cur_state,
+	.set_cur_state = gpio_fan_set_cur_state,
+};
+
 #ifdef CONFIG_OF_GPIO
 /*
  * Translate OpenFirmware node properties into platform_data
@@ -404,10 +454,32 @@ static int gpio_fan_get_of_pdata(struct device *dev,
 
 	node = dev->of_node;
 
+	/* Alarm GPIO if one exists */
+	if (of_gpio_named_count(node, "alarm-gpios") > 0) {
+		struct gpio_fan_alarm *alarm;
+		int val;
+		enum of_gpio_flags flags;
+
+		alarm = devm_kzalloc(dev, sizeof(struct gpio_fan_alarm),
+					GFP_KERNEL);
+		if (!alarm)
+			return -ENOMEM;
+
+		val = of_get_named_gpio_flags(node, "alarm-gpios", 0, &flags);
+		if (val < 0)
+			return val;
+		alarm->gpio = val;
+		alarm->active_low = flags & OF_GPIO_ACTIVE_LOW;
+
+		pdata->alarm = alarm;
+	}
+
 	/* Fill GPIO pin array */
 	pdata->num_ctrl = of_gpio_count(node);
 	if (pdata->num_ctrl <= 0) {
-		dev_err(dev, "gpios DT property empty / missing");
+		if (pdata->alarm)
+			return 0;
+		dev_err(dev, "DT properties empty / missing");
 		return -ENODEV;
 	}
 	ctrl = devm_kzalloc(dev, pdata->num_ctrl * sizeof(unsigned),
@@ -460,26 +532,6 @@ static int gpio_fan_get_of_pdata(struct device *dev,
 	}
 	pdata->speed = speed;
 
-	/* Alarm GPIO if one exists */
-	if (of_gpio_named_count(node, "alarm-gpios") > 0) {
-		struct gpio_fan_alarm *alarm;
-		int val;
-		enum of_gpio_flags flags;
-
-		alarm = devm_kzalloc(dev, sizeof(struct gpio_fan_alarm),
-					GFP_KERNEL);
-		if (!alarm)
-			return -ENOMEM;
-
-		val = of_get_named_gpio_flags(node, "alarm-gpios", 0, &flags);
-		if (val < 0)
-			return val;
-		alarm->gpio = val;
-		alarm->active_low = flags & OF_GPIO_ACTIVE_LOW;
-
-		pdata->alarm = alarm;
-	}
-
 	return 0;
 }
 
@@ -495,6 +547,11 @@ static int gpio_fan_probe(struct platform_device *pdev)
 	struct gpio_fan_data *fan_data;
 	struct gpio_fan_platform_data *pdata = dev_get_platdata(&pdev->dev);
 
+	fan_data = devm_kzalloc(&pdev->dev, sizeof(struct gpio_fan_data),
+				GFP_KERNEL);
+	if (!fan_data)
+		return -ENOMEM;
+
 #ifdef CONFIG_OF_GPIO
 	if (!pdata) {
 		pdata = devm_kzalloc(&pdev->dev,
@@ -512,11 +569,6 @@ static int gpio_fan_probe(struct platform_device *pdev)
 		return -EINVAL;
 #endif /* CONFIG_OF_GPIO */
 
-	fan_data = devm_kzalloc(&pdev->dev, sizeof(struct gpio_fan_data),
-				GFP_KERNEL);
-	if (!fan_data)
-		return -ENOMEM;
-
 	fan_data->pdev = pdev;
 	platform_set_drvdata(pdev, fan_data);
 	mutex_init(&fan_data->lock);
@@ -544,18 +596,39 @@ static int gpio_fan_probe(struct platform_device *pdev)
 						       gpio_fan_groups);
 	if (IS_ERR(fan_data->hwmon_dev))
 		return PTR_ERR(fan_data->hwmon_dev);
+#ifdef CONFIG_OF_GPIO
+	/* Optional cooling device register for Device tree platforms */
+	fan_data->cdev = thermal_of_cooling_device_register(pdev->dev.of_node,
+							    "gpio-fan",
+							    fan_data,
+							    &gpio_fan_cool_ops);
+#else /* CONFIG_OF_GPIO */
+	/* Optional cooling device register for non Device tree platforms */
+	fan_data->cdev = thermal_cooling_device_register("gpio-fan", fan_data,
+							 &gpio_fan_cool_ops);
+#endif /* CONFIG_OF_GPIO */
 
 	dev_info(&pdev->dev, "GPIO fan initialized\n");
 
 	return 0;
 }
 
-static void gpio_fan_shutdown(struct platform_device *pdev)
+static int gpio_fan_remove(struct platform_device *pdev)
 {
-	struct gpio_fan_data *fan_data = dev_get_drvdata(&pdev->dev);
+	struct gpio_fan_data *fan_data = platform_get_drvdata(pdev);
+
+	if (!IS_ERR(fan_data->cdev))
+		thermal_cooling_device_unregister(fan_data->cdev);
 
 	if (fan_data->ctrl)
 		set_fan_speed(fan_data, 0);
+
+	return 0;
+}
+
+static void gpio_fan_shutdown(struct platform_device *pdev)
+{
+	gpio_fan_remove(pdev);
 }
 
 #ifdef CONFIG_PM_SLEEP
@@ -589,6 +662,7 @@ static SIMPLE_DEV_PM_OPS(gpio_fan_pm, gpio_fan_suspend, gpio_fan_resume);
 
 static struct platform_driver gpio_fan_driver = {
 	.probe		= gpio_fan_probe,
+	.remove		= gpio_fan_remove,
 	.shutdown	= gpio_fan_shutdown,
 	.driver	= {
 		.name	= "gpio-fan",
diff --git a/drivers/hwmon/ibmpex.c b/drivers/hwmon/ibmpex.c
index 030e7ff589be..21b9c72f16bd 100644
--- a/drivers/hwmon/ibmpex.c
+++ b/drivers/hwmon/ibmpex.c
@@ -56,15 +56,10 @@ static u8 const temp_sensor_sig[]  = {0x74, 0x65, 0x6D};
 static u8 const watt_sensor_sig[]  = {0x41, 0x43};
 
 #define PEX_NUM_SENSOR_FUNCS	3
-static char const * const power_sensor_name_templates[] = {
-	"%s%d_average",
-	"%s%d_average_lowest",
-	"%s%d_average_highest"
-};
-static char const * const temp_sensor_name_templates[] = {
-	"%s%d_input",
-	"%s%d_input_lowest",
-	"%s%d_input_highest"
+static const char * const sensor_name_suffixes[] = {
+	"",
+	"_lowest",
+	"_highest"
 };
 
 static void ibmpex_msg_handler(struct ipmi_recv_msg *msg, void *user_msg_data);
@@ -355,9 +350,11 @@ static int create_sensor(struct ibmpex_bmc_data *data, int type,
 		return -ENOMEM;
 
 	if (type == TEMP_SENSOR)
-		sprintf(n, temp_sensor_name_templates[func], "temp", counter);
+		sprintf(n, "temp%d_input%s",
+			counter, sensor_name_suffixes[func]);
 	else if (type == POWER_SENSOR)
-		sprintf(n, power_sensor_name_templates[func], "power", counter);
+		sprintf(n, "power%d_average%s",
+			counter, sensor_name_suffixes[func]);
 
 	sysfs_attr_init(&data->sensors[sensor].attr[func].dev_attr.attr);
 	data->sensors[sensor].attr[func].dev_attr.attr.name = n;
diff --git a/drivers/hwmon/ibmpowernv.c b/drivers/hwmon/ibmpowernv.c
index febe8175d36c..4255514b2c72 100644
--- a/drivers/hwmon/ibmpowernv.c
+++ b/drivers/hwmon/ibmpowernv.c
@@ -30,8 +30,11 @@
 #include <linux/platform_device.h>
 #include <asm/opal.h>
 #include <linux/err.h>
+#include <asm/cputhreads.h>
+#include <asm/smp.h>
 
 #define MAX_ATTR_LEN	32
+#define MAX_LABEL_LEN	64
 
 /* Sensor suffix name from DT */
 #define DT_FAULT_ATTR_SUFFIX		"faulted"
@@ -44,17 +47,20 @@
  */
 enum sensors {
 	FAN,
-	AMBIENT_TEMP,
+	TEMP,
 	POWER_SUPPLY,
 	POWER_INPUT,
 	MAX_SENSOR_TYPE,
 };
 
+#define INVALID_INDEX (-1U)
+
 static struct sensor_group {
 	const char *name;
 	const char *compatible;
 	struct attribute_group group;
 	u32 attr_count;
+	u32 hwmon_index;
 } sensor_groups[] = {
 	{"fan", "ibm,opal-sensor-cooling-fan"},
 	{"temp", "ibm,opal-sensor-amb-temp"},
@@ -64,7 +70,10 @@ static struct sensor_group {
 
 struct sensor_data {
 	u32 id; /* An opaque id of the firmware for each sensor */
+	u32 hwmon_index;
+	u32 opal_index;
 	enum sensors type;
+	char label[MAX_LABEL_LEN];
 	char name[MAX_ATTR_LEN];
 	struct device_attribute dev_attr;
 };
@@ -87,7 +96,7 @@ static ssize_t show_sensor(struct device *dev, struct device_attribute *devattr,
 		return ret;
 
 	/* Convert temperature to milli-degrees */
-	if (sdata->type == AMBIENT_TEMP)
+	if (sdata->type == TEMP)
 		x *= 1000;
 	/* Convert power to micro-watts */
 	else if (sdata->type == POWER_INPUT)
@@ -96,8 +105,65 @@ static ssize_t show_sensor(struct device *dev, struct device_attribute *devattr,
 	return sprintf(buf, "%u\n", x);
 }
 
-static int get_sensor_index_attr(const char *name, u32 *index,
-					char *attr)
+static ssize_t show_label(struct device *dev, struct device_attribute *devattr,
+			  char *buf)
+{
+	struct sensor_data *sdata = container_of(devattr, struct sensor_data,
+						 dev_attr);
+
+	return sprintf(buf, "%s\n", sdata->label);
+}
+
+static int __init get_logical_cpu(int hwcpu)
+{
+	int cpu;
+
+	for_each_possible_cpu(cpu)
+		if (get_hard_smp_processor_id(cpu) == hwcpu)
+			return cpu;
+
+	return -ENOENT;
+}
+
+static void __init make_sensor_label(struct device_node *np,
+				     struct sensor_data *sdata,
+				     const char *label)
+{
+	u32 id;
+	size_t n;
+
+	n = snprintf(sdata->label, sizeof(sdata->label), "%s", label);
+
+	/*
+	 * Core temp pretty print
+	 */
+	if (!of_property_read_u32(np, "ibm,pir", &id)) {
+		int cpuid = get_logical_cpu(id);
+
+		if (cpuid >= 0)
+			/*
+			 * The digital thermal sensors are associated
+			 * with a core. Let's print out the range of
+			 * cpu ids corresponding to the hardware
+			 * threads of the core.
+			 */
+			n += snprintf(sdata->label + n,
+				      sizeof(sdata->label) - n, " %d-%d",
+				      cpuid, cpuid + threads_per_core - 1);
+		else
+			n += snprintf(sdata->label + n,
+				      sizeof(sdata->label) - n, " phy%d", id);
+	}
+
+	/*
+	 * Membuffer pretty print
+	 */
+	if (!of_property_read_u32(np, "ibm,chip-id", &id))
+		n += snprintf(sdata->label + n, sizeof(sdata->label) - n,
+			      " %d", id & 0xffff);
+}
+
+static int get_sensor_index_attr(const char *name, u32 *index, char *attr)
 {
 	char *hash_pos = strchr(name, '#');
 	char buf[8] = { 0 };
@@ -127,46 +193,90 @@ static int get_sensor_index_attr(const char *name, u32 *index,
 	return 0;
 }
 
+static const char *convert_opal_attr_name(enum sensors type,
+					  const char *opal_attr)
+{
+	const char *attr_name = NULL;
+
+	if (!strcmp(opal_attr, DT_FAULT_ATTR_SUFFIX)) {
+		attr_name = "fault";
+	} else if (!strcmp(opal_attr, DT_DATA_ATTR_SUFFIX)) {
+		attr_name = "input";
+	} else if (!strcmp(opal_attr, DT_THRESHOLD_ATTR_SUFFIX)) {
+		if (type == TEMP)
+			attr_name = "max";
+		else if (type == FAN)
+			attr_name = "min";
+	}
+
+	return attr_name;
+}
+
 /*
  * This function translates the DT node name into the 'hwmon' attribute name.
  * IBMPOWERNV device node appear like cooling-fan#2-data, amb-temp#1-thrs etc.
  * which need to be mapped as fan2_input, temp1_max respectively before
  * populating them inside hwmon device class.
  */
-static int create_hwmon_attr_name(struct device *dev, enum sensors type,
-					 const char *node_name,
-					 char *hwmon_attr_name)
+static const char *parse_opal_node_name(const char *node_name,
+					enum sensors type, u32 *index)
 {
 	char attr_suffix[MAX_ATTR_LEN];
-	char *attr_name;
-	u32 index;
+	const char *attr_name;
 	int err;
 
-	err = get_sensor_index_attr(node_name, &index, attr_suffix);
-	if (err) {
-		dev_err(dev, "Sensor device node name '%s' is invalid\n",
-			node_name);
-		return err;
-	}
+	err = get_sensor_index_attr(node_name, index, attr_suffix);
+	if (err)
+		return ERR_PTR(err);
 
-	if (!strcmp(attr_suffix, DT_FAULT_ATTR_SUFFIX)) {
-		attr_name = "fault";
-	} else if (!strcmp(attr_suffix, DT_DATA_ATTR_SUFFIX)) {
-		attr_name = "input";
-	} else if (!strcmp(attr_suffix, DT_THRESHOLD_ATTR_SUFFIX)) {
-		if (type == AMBIENT_TEMP)
-			attr_name = "max";
-		else if (type == FAN)
-			attr_name = "min";
-		else
-			return -ENOENT;
-	} else {
-		return -ENOENT;
+	attr_name = convert_opal_attr_name(type, attr_suffix);
+	if (!attr_name)
+		return ERR_PTR(-ENOENT);
+
+	return attr_name;
+}
+
+static int get_sensor_type(struct device_node *np)
+{
+	enum sensors type;
+	const char *str;
+
+	for (type = 0; type < MAX_SENSOR_TYPE; type++) {
+		if (of_device_is_compatible(np, sensor_groups[type].compatible))
+			return type;
 	}
 
-	snprintf(hwmon_attr_name, MAX_ATTR_LEN, "%s%d_%s",
-		 sensor_groups[type].name, index, attr_name);
-	return 0;
+	/*
+	 * Let's check if we have a newer device tree
+	 */
+	if (!of_device_is_compatible(np, "ibm,opal-sensor"))
+		return MAX_SENSOR_TYPE;
+
+	if (of_property_read_string(np, "sensor-type", &str))
+		return MAX_SENSOR_TYPE;
+
+	for (type = 0; type < MAX_SENSOR_TYPE; type++)
+		if (!strcmp(str, sensor_groups[type].name))
+			return type;
+
+	return MAX_SENSOR_TYPE;
+}
+
+static u32 get_sensor_hwmon_index(struct sensor_data *sdata,
+				  struct sensor_data *sdata_table, int count)
+{
+	int i;
+
+	/*
+	 * We don't use the OPAL index on newer device trees
+	 */
+	if (sdata->opal_index != INVALID_INDEX) {
+		for (i = 0; i < count; i++)
+			if (sdata_table[i].opal_index == sdata->opal_index &&
+			    sdata_table[i].type == sdata->type)
+				return sdata_table[i].hwmon_index;
+	}
+	return ++sensor_groups[sdata->type].hwmon_index;
 }
 
 static int populate_attr_groups(struct platform_device *pdev)
@@ -178,15 +288,22 @@ static int populate_attr_groups(struct platform_device *pdev)
 
 	opal = of_find_node_by_path("/ibm,opal/sensors");
 	for_each_child_of_node(opal, np) {
+		const char *label;
+
 		if (np->name == NULL)
 			continue;
 
-		for (type = 0; type < MAX_SENSOR_TYPE; type++)
-			if (of_device_is_compatible(np,
-					sensor_groups[type].compatible)) {
-				sensor_groups[type].attr_count++;
-				break;
-			}
+		type = get_sensor_type(np);
+		if (type == MAX_SENSOR_TYPE)
+			continue;
+
+		sensor_groups[type].attr_count++;
+
+		/*
+		 * add a new attribute for labels
+		 */
+		if (!of_property_read_string(np, "label", &label))
+			sensor_groups[type].attr_count++;
 	}
 
 	of_node_put(opal);
@@ -207,6 +324,21 @@ static int populate_attr_groups(struct platform_device *pdev)
 	return 0;
 }
 
+static void create_hwmon_attr(struct sensor_data *sdata, const char *attr_name,
+			      ssize_t (*show)(struct device *dev,
+					      struct device_attribute *attr,
+					      char *buf))
+{
+	snprintf(sdata->name, MAX_ATTR_LEN, "%s%d_%s",
+		 sensor_groups[sdata->type].name, sdata->hwmon_index,
+		 attr_name);
+
+	sysfs_attr_init(&sdata->dev_attr.attr);
+	sdata->dev_attr.attr.name = sdata->name;
+	sdata->dev_attr.attr.mode = S_IRUGO;
+	sdata->dev_attr.show = show;
+}
+
 /*
  * Iterate through the device tree for each child of 'sensors' node, create
  * a sysfs attribute file, the file is named by translating the DT node name
@@ -233,18 +365,23 @@ static int create_device_attrs(struct platform_device *pdev)
 	}
 
 	for_each_child_of_node(opal, np) {
+		const char *attr_name;
+		u32 opal_index;
+		const char *label;
+
 		if (np->name == NULL)
 			continue;
 
-		for (type = 0; type < MAX_SENSOR_TYPE; type++)
-			if (of_device_is_compatible(np,
-					sensor_groups[type].compatible))
-				break;
-
+		type = get_sensor_type(np);
 		if (type == MAX_SENSOR_TYPE)
 			continue;
 
-		if (of_property_read_u32(np, "sensor-id", &sensor_id)) {
+		/*
+		 * Newer device trees use a "sensor-data" property
+		 * name for input.
+		 */
+		if (of_property_read_u32(np, "sensor-id", &sensor_id) &&
+		    of_property_read_u32(np, "sensor-data", &sensor_id)) {
 			dev_info(&pdev->dev,
 				 "'sensor-id' missing in the node '%s'\n",
 				 np->name);
@@ -253,18 +390,46 @@ static int create_device_attrs(struct platform_device *pdev)
 
 		sdata[count].id = sensor_id;
 		sdata[count].type = type;
-		err = create_hwmon_attr_name(&pdev->dev, type, np->name,
-					     sdata[count].name);
-		if (err)
-			goto exit_put_node;
 
-		sysfs_attr_init(&sdata[count].dev_attr.attr);
-		sdata[count].dev_attr.attr.name = sdata[count].name;
-		sdata[count].dev_attr.attr.mode = S_IRUGO;
-		sdata[count].dev_attr.show = show_sensor;
+		/*
+		 * If we can not parse the node name, it means we are
+		 * running on a newer device tree. We can just forget
+		 * about the OPAL index and use a defaut value for the
+		 * hwmon attribute name
+		 */
+		attr_name = parse_opal_node_name(np->name, type, &opal_index);
+		if (IS_ERR(attr_name)) {
+			attr_name = "input";
+			opal_index = INVALID_INDEX;
+		}
+
+		sdata[count].opal_index = opal_index;
+		sdata[count].hwmon_index =
+			get_sensor_hwmon_index(&sdata[count], sdata, count);
+
+		create_hwmon_attr(&sdata[count], attr_name, show_sensor);
 
 		pgroups[type]->attrs[sensor_groups[type].attr_count++] =
 				&sdata[count++].dev_attr.attr;
+
+		if (!of_property_read_string(np, "label", &label)) {
+			/*
+			 * For the label attribute, we can reuse the
+			 * "properties" of the previous "input"
+			 * attribute. They are related to the same
+			 * sensor.
+			 */
+			sdata[count].type = type;
+			sdata[count].opal_index = sdata[count - 1].opal_index;
+			sdata[count].hwmon_index = sdata[count - 1].hwmon_index;
+
+			make_sensor_label(np, &sdata[count], label);
+
+			create_hwmon_attr(&sdata[count], "label", show_label);
+
+			pgroups[type]->attrs[sensor_groups[type].attr_count++] =
+				&sdata[count++].dev_attr.attr;
+		}
 	}
 
 exit_put_node:
diff --git a/drivers/hwmon/it87.c b/drivers/hwmon/it87.c
index 409116c52cc5..d0ee556e8ce0 100644
--- a/drivers/hwmon/it87.c
+++ b/drivers/hwmon/it87.c
@@ -11,6 +11,7 @@
  *  similar parts.  The other devices are supported by different drivers.
  *
  *  Supports: IT8603E  Super I/O chip w/LPC interface
+ *            IT8620E  Super I/O chip w/LPC interface
  *            IT8623E  Super I/O chip w/LPC interface
  *            IT8705F  Super I/O chip w/LPC interface
  *            IT8712F  Super I/O chip w/LPC interface
@@ -23,8 +24,11 @@
  *            IT8758E  Super I/O chip w/LPC interface
  *            IT8771E  Super I/O chip w/LPC interface
  *            IT8772E  Super I/O chip w/LPC interface
+ *            IT8781F  Super I/O chip w/LPC interface
  *            IT8782F  Super I/O chip w/LPC interface
  *            IT8783E/F Super I/O chip w/LPC interface
+ *            IT8786E  Super I/O chip w/LPC interface
+ *            IT8790E  Super I/O chip w/LPC interface
  *            Sis950   A clone of the IT8705F
  *
  *  Copyright (C) 2001 Chris Gauthron
@@ -66,7 +70,7 @@
 #define DRVNAME "it87"
 
 enum chips { it87, it8712, it8716, it8718, it8720, it8721, it8728, it8771,
-	     it8772, it8782, it8783, it8603 };
+	     it8772, it8781, it8782, it8783, it8786, it8790, it8603, it8620 };
 
 static unsigned short force_id;
 module_param(force_id, ushort, 0);
@@ -146,15 +150,20 @@ static inline void superio_exit(void)
 #define IT8728F_DEVID 0x8728
 #define IT8771E_DEVID 0x8771
 #define IT8772E_DEVID 0x8772
+#define IT8781F_DEVID 0x8781
 #define IT8782F_DEVID 0x8782
 #define IT8783E_DEVID 0x8783
+#define IT8786E_DEVID 0x8786
+#define IT8790E_DEVID 0x8790
 #define IT8603E_DEVID 0x8603
+#define IT8620E_DEVID 0x8620
 #define IT8623E_DEVID 0x8623
 #define IT87_ACT_REG  0x30
 #define IT87_BASE_REG 0x60
 
 /* Logical device 7 registers (IT8712F and later) */
 #define IT87_SIO_GPIO1_REG	0x25
+#define IT87_SIO_GPIO2_REG	0x26
 #define IT87_SIO_GPIO3_REG	0x27
 #define IT87_SIO_GPIO5_REG	0x29
 #define IT87_SIO_PINX1_REG	0x2a	/* Pin selection */
@@ -207,11 +216,11 @@ static bool fix_pwm_polarity;
 
 /* Monitors: 9 voltage (0 to 7, battery), 3 temp (1 to 3), 3 fan (1 to 3) */
 
-static const u8 IT87_REG_FAN[]		= { 0x0d, 0x0e, 0x0f, 0x80, 0x82 };
-static const u8 IT87_REG_FAN_MIN[]	= { 0x10, 0x11, 0x12, 0x84, 0x86 };
-static const u8 IT87_REG_FANX[]		= { 0x18, 0x19, 0x1a, 0x81, 0x83 };
-static const u8 IT87_REG_FANX_MIN[]	= { 0x1b, 0x1c, 0x1d, 0x85, 0x87 };
-static const u8 IT87_REG_TEMP_OFFSET[]	= { 0x56, 0x57, 0x59 };
+static const u8 IT87_REG_FAN[]         = { 0x0d, 0x0e, 0x0f, 0x80, 0x82, 0x4c };
+static const u8 IT87_REG_FAN_MIN[]     = { 0x10, 0x11, 0x12, 0x84, 0x86, 0x4e };
+static const u8 IT87_REG_FANX[]        = { 0x18, 0x19, 0x1a, 0x81, 0x83, 0x4d };
+static const u8 IT87_REG_FANX_MIN[]    = { 0x1b, 0x1c, 0x1d, 0x85, 0x87, 0x4f };
+static const u8 IT87_REG_TEMP_OFFSET[] = { 0x56, 0x57, 0x59 };
 
 #define IT87_REG_FAN_MAIN_CTRL 0x13
 #define IT87_REG_FAN_CTL       0x14
@@ -238,6 +247,7 @@ static const u8 IT87_REG_TEMP_OFFSET[]	= { 0x56, 0x57, 0x59 };
 
 struct it87_devices {
 	const char *name;
+	const char * const suffix;
 	u16 features;
 	u8 peci_mask;
 	u8 old_peci_mask;
@@ -250,79 +260,131 @@ struct it87_devices {
 #define FEAT_TEMP_OFFSET	(1 << 4)
 #define FEAT_TEMP_PECI		(1 << 5)
 #define FEAT_TEMP_OLD_PECI	(1 << 6)
+#define FEAT_FAN16_CONFIG	(1 << 7)	/* Need to enable 16-bit fans */
+#define FEAT_FIVE_FANS		(1 << 8)	/* Supports five fans */
+#define FEAT_VID		(1 << 9)	/* Set if chip supports VID */
+#define FEAT_IN7_INTERNAL	(1 << 10)	/* Set if in7 is internal */
+#define FEAT_SIX_FANS		(1 << 11)	/* Supports six fans */
 
 static const struct it87_devices it87_devices[] = {
 	[it87] = {
 		.name = "it87",
+		.suffix = "F",
 		.features = FEAT_OLD_AUTOPWM,	/* may need to overwrite */
 	},
 	[it8712] = {
 		.name = "it8712",
-		.features = FEAT_OLD_AUTOPWM,	/* may need to overwrite */
+		.suffix = "F",
+		.features = FEAT_OLD_AUTOPWM | FEAT_VID,
+						/* may need to overwrite */
 	},
 	[it8716] = {
 		.name = "it8716",
-		.features = FEAT_16BIT_FANS | FEAT_TEMP_OFFSET,
+		.suffix = "F",
+		.features = FEAT_16BIT_FANS | FEAT_TEMP_OFFSET | FEAT_VID
+		  | FEAT_FAN16_CONFIG | FEAT_FIVE_FANS,
 	},
 	[it8718] = {
 		.name = "it8718",
-		.features = FEAT_16BIT_FANS | FEAT_TEMP_OFFSET
-		  | FEAT_TEMP_OLD_PECI,
+		.suffix = "F",
+		.features = FEAT_16BIT_FANS | FEAT_TEMP_OFFSET | FEAT_VID
+		  | FEAT_TEMP_OLD_PECI | FEAT_FAN16_CONFIG | FEAT_FIVE_FANS,
 		.old_peci_mask = 0x4,
 	},
 	[it8720] = {
 		.name = "it8720",
-		.features = FEAT_16BIT_FANS | FEAT_TEMP_OFFSET
-		  | FEAT_TEMP_OLD_PECI,
+		.suffix = "F",
+		.features = FEAT_16BIT_FANS | FEAT_TEMP_OFFSET | FEAT_VID
+		  | FEAT_TEMP_OLD_PECI | FEAT_FAN16_CONFIG | FEAT_FIVE_FANS,
 		.old_peci_mask = 0x4,
 	},
 	[it8721] = {
 		.name = "it8721",
+		.suffix = "F",
 		.features = FEAT_NEWER_AUTOPWM | FEAT_12MV_ADC | FEAT_16BIT_FANS
-		  | FEAT_TEMP_OFFSET | FEAT_TEMP_OLD_PECI | FEAT_TEMP_PECI,
+		  | FEAT_TEMP_OFFSET | FEAT_TEMP_OLD_PECI | FEAT_TEMP_PECI
+		  | FEAT_FAN16_CONFIG | FEAT_FIVE_FANS | FEAT_IN7_INTERNAL,
 		.peci_mask = 0x05,
 		.old_peci_mask = 0x02,	/* Actually reports PCH */
 	},
 	[it8728] = {
 		.name = "it8728",
+		.suffix = "F",
 		.features = FEAT_NEWER_AUTOPWM | FEAT_12MV_ADC | FEAT_16BIT_FANS
-		  | FEAT_TEMP_OFFSET | FEAT_TEMP_PECI,
+		  | FEAT_TEMP_OFFSET | FEAT_TEMP_PECI | FEAT_FIVE_FANS
+		  | FEAT_IN7_INTERNAL,
 		.peci_mask = 0x07,
 	},
 	[it8771] = {
 		.name = "it8771",
+		.suffix = "E",
 		.features = FEAT_NEWER_AUTOPWM | FEAT_12MV_ADC | FEAT_16BIT_FANS
-		  | FEAT_TEMP_OFFSET | FEAT_TEMP_PECI,
-					/* PECI: guesswork */
-					/* 12mV ADC (OHM) */
-					/* 16 bit fans (OHM) */
+		  | FEAT_TEMP_OFFSET | FEAT_TEMP_PECI | FEAT_IN7_INTERNAL,
+				/* PECI: guesswork */
+				/* 12mV ADC (OHM) */
+				/* 16 bit fans (OHM) */
+				/* three fans, always 16 bit (guesswork) */
 		.peci_mask = 0x07,
 	},
 	[it8772] = {
 		.name = "it8772",
+		.suffix = "E",
 		.features = FEAT_NEWER_AUTOPWM | FEAT_12MV_ADC | FEAT_16BIT_FANS
-		  | FEAT_TEMP_OFFSET | FEAT_TEMP_PECI,
-					/* PECI (coreboot) */
-					/* 12mV ADC (HWSensors4, OHM) */
-					/* 16 bit fans (HWSensors4, OHM) */
+		  | FEAT_TEMP_OFFSET | FEAT_TEMP_PECI | FEAT_IN7_INTERNAL,
+				/* PECI (coreboot) */
+				/* 12mV ADC (HWSensors4, OHM) */
+				/* 16 bit fans (HWSensors4, OHM) */
+				/* three fans, always 16 bit (datasheet) */
 		.peci_mask = 0x07,
 	},
+	[it8781] = {
+		.name = "it8781",
+		.suffix = "F",
+		.features = FEAT_16BIT_FANS | FEAT_TEMP_OFFSET
+		  | FEAT_TEMP_OLD_PECI | FEAT_FAN16_CONFIG,
+		.old_peci_mask = 0x4,
+	},
 	[it8782] = {
 		.name = "it8782",
+		.suffix = "F",
 		.features = FEAT_16BIT_FANS | FEAT_TEMP_OFFSET
-		  | FEAT_TEMP_OLD_PECI,
+		  | FEAT_TEMP_OLD_PECI | FEAT_FAN16_CONFIG,
 		.old_peci_mask = 0x4,
 	},
 	[it8783] = {
 		.name = "it8783",
+		.suffix = "E/F",
 		.features = FEAT_16BIT_FANS | FEAT_TEMP_OFFSET
-		  | FEAT_TEMP_OLD_PECI,
+		  | FEAT_TEMP_OLD_PECI | FEAT_FAN16_CONFIG,
 		.old_peci_mask = 0x4,
 	},
+	[it8786] = {
+		.name = "it8786",
+		.suffix = "E",
+		.features = FEAT_NEWER_AUTOPWM | FEAT_12MV_ADC | FEAT_16BIT_FANS
+		  | FEAT_TEMP_OFFSET | FEAT_TEMP_PECI | FEAT_IN7_INTERNAL,
+		.peci_mask = 0x07,
+	},
+	[it8790] = {
+		.name = "it8790",
+		.suffix = "E",
+		.features = FEAT_NEWER_AUTOPWM | FEAT_12MV_ADC | FEAT_16BIT_FANS
+		  | FEAT_TEMP_OFFSET | FEAT_TEMP_PECI | FEAT_IN7_INTERNAL,
+		.peci_mask = 0x07,
+	},
 	[it8603] = {
 		.name = "it8603",
+		.suffix = "E",
 		.features = FEAT_NEWER_AUTOPWM | FEAT_12MV_ADC | FEAT_16BIT_FANS
-		  | FEAT_TEMP_OFFSET | FEAT_TEMP_PECI,
+		  | FEAT_TEMP_OFFSET | FEAT_TEMP_PECI | FEAT_IN7_INTERNAL,
+		.peci_mask = 0x07,
+	},
+	[it8620] = {
+		.name = "it8620",
+		.suffix = "E",
+		.features = FEAT_NEWER_AUTOPWM | FEAT_12MV_ADC | FEAT_16BIT_FANS
+		  | FEAT_TEMP_OFFSET | FEAT_TEMP_PECI | FEAT_SIX_FANS
+		  | FEAT_IN7_INTERNAL,
 		.peci_mask = 0x07,
 	},
 };
@@ -337,6 +399,12 @@ static const struct it87_devices it87_devices[] = {
 #define has_temp_old_peci(data, nr) \
 				(((data)->features & FEAT_TEMP_OLD_PECI) && \
 				 ((data)->old_peci_mask & (1 << nr)))
+#define has_fan16_config(data)	((data)->features & FEAT_FAN16_CONFIG)
+#define has_five_fans(data)	((data)->features & (FEAT_FIVE_FANS | \
+						     FEAT_SIX_FANS))
+#define has_vid(data)		((data)->features & FEAT_VID)
+#define has_in7_internal(data)	((data)->features & FEAT_IN7_INTERNAL)
+#define has_six_fans(data)	((data)->features & FEAT_SIX_FANS)
 
 struct it87_sio_data {
 	enum chips type;
@@ -373,7 +441,7 @@ struct it87_data {
 	u16 in_scaled;		/* Internal voltage sensors are scaled */
 	u8 in[10][3];		/* [nr][0]=in, [1]=min, [2]=max */
 	u8 has_fan;		/* Bitfield, fans enabled */
-	u16 fan[5][2];		/* Register values, [nr][0]=fan, [1]=min */
+	u16 fan[6][2];		/* Register values, [nr][0]=fan, [1]=min */
 	u8 has_temp;		/* Bitfield, temp sensors enabled */
 	s8 temp[3][4];		/* [nr][0]=temp, [1]=min, [2]=max, [3]=offset */
 	u8 sensor;		/* Register value (IT87_REG_TEMP_ENABLE) */
@@ -475,15 +543,25 @@ static int DIV_TO_REG(int val)
 }
 #define DIV_FROM_REG(val) (1 << (val))
 
+/*
+ * PWM base frequencies. The frequency has to be divided by either 128 or 256,
+ * depending on the chip type, to calculate the actual PWM frequency.
+ *
+ * Some of the chip datasheets suggest a base frequency of 51 kHz instead
+ * of 750 kHz for the slowest base frequency, resulting in a PWM frequency
+ * of 200 Hz. Sometimes both PWM frequency select registers are affected,
+ * sometimes just one. It is unknown if this is a datasheet error or real,
+ * so this is ignored for now.
+ */
 static const unsigned int pwm_freq[8] = {
-	48000000 / 128,
-	24000000 / 128,
-	12000000 / 128,
-	8000000 / 128,
-	6000000 / 128,
-	3000000 / 128,
-	1500000 / 128,
-	750000 / 128,
+	48000000,
+	24000000,
+	12000000,
+	8000000,
+	6000000,
+	3000000,
+	1500000,
+	750000,
 };
 
 static int it87_probe(struct platform_device *pdev);
@@ -801,8 +879,11 @@ static ssize_t show_pwm_freq(struct device *dev, struct device_attribute *attr,
 {
 	struct it87_data *data = it87_update_device(dev);
 	int index = (data->fan_ctl >> 4) & 0x07;
+	unsigned int freq;
 
-	return sprintf(buf, "%u\n", pwm_freq[index]);
+	freq = pwm_freq[index] / (has_newer_autopwm(data) ? 256 : 128);
+
+	return sprintf(buf, "%u\n", freq);
 }
 
 static ssize_t set_fan(struct device *dev, struct device_attribute *attr,
@@ -1024,6 +1105,9 @@ static ssize_t set_pwm_freq(struct device *dev,
 	if (kstrtoul(buf, 10, &val) < 0)
 		return -EINVAL;
 
+	val = clamp_val(val, 0, 1000000);
+	val *= has_newer_autopwm(data) ? 256 : 128;
+
 	/* Search for the nearest available frequency */
 	for (i = 0; i < 7; i++) {
 		if (val > (pwm_freq[i] + pwm_freq[i+1]) / 2)
@@ -1196,6 +1280,10 @@ static SENSOR_DEVICE_ATTR_2(fan5_input, S_IRUGO, show_fan, NULL, 4, 0);
 static SENSOR_DEVICE_ATTR_2(fan5_min, S_IRUGO | S_IWUSR, show_fan, set_fan,
 			    4, 1);
 
+static SENSOR_DEVICE_ATTR_2(fan6_input, S_IRUGO, show_fan, NULL, 5, 0);
+static SENSOR_DEVICE_ATTR_2(fan6_min, S_IRUGO | S_IWUSR, show_fan, set_fan,
+			    5, 1);
+
 static SENSOR_DEVICE_ATTR(pwm1_enable, S_IRUGO | S_IWUSR,
 			  show_pwm_enable, set_pwm_enable, 0);
 static SENSOR_DEVICE_ATTR(pwm1, S_IRUGO | S_IWUSR, show_pwm, set_pwm, 0);
@@ -1326,6 +1414,7 @@ static SENSOR_DEVICE_ATTR(fan2_alarm, S_IRUGO, show_alarm, NULL, 1);
 static SENSOR_DEVICE_ATTR(fan3_alarm, S_IRUGO, show_alarm, NULL, 2);
 static SENSOR_DEVICE_ATTR(fan4_alarm, S_IRUGO, show_alarm, NULL, 3);
 static SENSOR_DEVICE_ATTR(fan5_alarm, S_IRUGO, show_alarm, NULL, 6);
+static SENSOR_DEVICE_ATTR(fan6_alarm, S_IRUGO, show_alarm, NULL, 7);
 static SENSOR_DEVICE_ATTR(temp1_alarm, S_IRUGO, show_alarm, NULL, 16);
 static SENSOR_DEVICE_ATTR(temp2_alarm, S_IRUGO, show_alarm, NULL, 17);
 static SENSOR_DEVICE_ATTR(temp3_alarm, S_IRUGO, show_alarm, NULL, 18);
@@ -1376,6 +1465,7 @@ static SENSOR_DEVICE_ATTR(fan2_beep, S_IRUGO, show_beep, set_beep, 0);
 static SENSOR_DEVICE_ATTR(fan3_beep, S_IRUGO, show_beep, set_beep, 0);
 static SENSOR_DEVICE_ATTR(fan4_beep, S_IRUGO, show_beep, set_beep, 0);
 static SENSOR_DEVICE_ATTR(fan5_beep, S_IRUGO, show_beep, set_beep, 0);
+static SENSOR_DEVICE_ATTR(fan6_beep, S_IRUGO, show_beep, set_beep, 0);
 static SENSOR_DEVICE_ATTR(temp1_beep, S_IRUGO | S_IWUSR,
 			  show_beep, set_beep, 2);
 static SENSOR_DEVICE_ATTR(temp2_beep, S_IRUGO, show_beep, NULL, 2);
@@ -1579,7 +1669,7 @@ static struct attribute *it87_attributes_temp_beep[] = {
 	&sensor_dev_attr_temp3_beep.dev_attr.attr,
 };
 
-static struct attribute *it87_attributes_fan[5][3+1] = { {
+static struct attribute *it87_attributes_fan[6][3+1] = { {
 	&sensor_dev_attr_fan1_input.dev_attr.attr,
 	&sensor_dev_attr_fan1_min.dev_attr.attr,
 	&sensor_dev_attr_fan1_alarm.dev_attr.attr,
@@ -1604,14 +1694,20 @@ static struct attribute *it87_attributes_fan[5][3+1] = { {
 	&sensor_dev_attr_fan5_min.dev_attr.attr,
 	&sensor_dev_attr_fan5_alarm.dev_attr.attr,
 	NULL
+}, {
+	&sensor_dev_attr_fan6_input.dev_attr.attr,
+	&sensor_dev_attr_fan6_min.dev_attr.attr,
+	&sensor_dev_attr_fan6_alarm.dev_attr.attr,
+	NULL
 } };
 
-static const struct attribute_group it87_group_fan[5] = {
+static const struct attribute_group it87_group_fan[6] = {
 	{ .attrs = it87_attributes_fan[0] },
 	{ .attrs = it87_attributes_fan[1] },
 	{ .attrs = it87_attributes_fan[2] },
 	{ .attrs = it87_attributes_fan[3] },
 	{ .attrs = it87_attributes_fan[4] },
+	{ .attrs = it87_attributes_fan[5] },
 };
 
 static const struct attribute *it87_attributes_fan_div[] = {
@@ -1693,6 +1789,7 @@ static struct attribute *it87_attributes_fan_beep[] = {
 	&sensor_dev_attr_fan3_beep.dev_attr.attr,
 	&sensor_dev_attr_fan4_beep.dev_attr.attr,
 	&sensor_dev_attr_fan5_beep.dev_attr.attr,
+	&sensor_dev_attr_fan6_beep.dev_attr.attr,
 };
 
 static struct attribute *it87_attributes_vid[] = {
@@ -1724,6 +1821,7 @@ static int __init it87_find(unsigned short *address,
 	int err;
 	u16 chip_type;
 	const char *board_vendor, *board_name;
+	const struct it87_devices *config;
 
 	err = superio_enter();
 	if (err)
@@ -1761,16 +1859,28 @@ static int __init it87_find(unsigned short *address,
 	case IT8772E_DEVID:
 		sio_data->type = it8772;
 		break;
+	case IT8781F_DEVID:
+		sio_data->type = it8781;
+		break;
 	case IT8782F_DEVID:
 		sio_data->type = it8782;
 		break;
 	case IT8783E_DEVID:
 		sio_data->type = it8783;
 		break;
+	case IT8786E_DEVID:
+		sio_data->type = it8786;
+		break;
+	case IT8790E_DEVID:
+		sio_data->type = it8790;
+		break;
 	case IT8603E_DEVID:
 	case IT8623E_DEVID:
 		sio_data->type = it8603;
 		break;
+	case IT8620E_DEVID:
+		sio_data->type = it8620;
+		break;
 	case 0xffff:	/* No device at all */
 		goto exit;
 	default:
@@ -1792,30 +1902,34 @@ static int __init it87_find(unsigned short *address,
 
 	err = 0;
 	sio_data->revision = superio_inb(DEVREV) & 0x0f;
-	pr_info("Found IT%04x%c chip at 0x%x, revision %d\n", chip_type,
-		chip_type == 0x8771 || chip_type == 0x8772 ||
-		chip_type == 0x8603 ? 'E' : 'F', *address,
-		sio_data->revision);
+	pr_info("Found IT%04x%s chip at 0x%x, revision %d\n", chip_type,
+		it87_devices[sio_data->type].suffix,
+		*address, sio_data->revision);
+
+	config = &it87_devices[sio_data->type];
+
+	/* in7 (VSB or VCCH5V) is always internal on some chips */
+	if (has_in7_internal(config))
+		sio_data->internal |= (1 << 1);
 
 	/* in8 (Vbat) is always internal */
-	sio_data->internal = (1 << 2);
+	sio_data->internal |= (1 << 2);
+
 	/* Only the IT8603E has in9 */
 	if (sio_data->type != it8603)
 		sio_data->skip_in |= (1 << 9);
 
-	/* Read GPIO config and VID value from LDN 7 (GPIO) */
-	if (sio_data->type == it87) {
-		/* The IT8705F doesn't have VID pins at all */
+	if (!has_vid(config))
 		sio_data->skip_vid = 1;
 
+	/* Read GPIO config and VID value from LDN 7 (GPIO) */
+	if (sio_data->type == it87) {
 		/* The IT8705F has a different LD number for GPIO */
 		superio_select(5);
 		sio_data->beep_pin = superio_inb(IT87_SIO_BEEP_PIN_REG) & 0x3f;
 	} else if (sio_data->type == it8783) {
 		int reg25, reg27, reg2a, reg2c, regef;
 
-		sio_data->skip_vid = 1;	/* No VID */
-
 		superio_select(GPIO);
 
 		reg25 = superio_inb(IT87_SIO_GPIO1_REG);
@@ -1881,7 +1995,6 @@ static int __init it87_find(unsigned short *address,
 	} else if (sio_data->type == it8603) {
 		int reg27, reg29;
 
-		sio_data->skip_vid = 1;	/* No VID */
 		superio_select(GPIO);
 
 		reg27 = superio_inb(IT87_SIO_GPIO3_REG);
@@ -1902,14 +2015,36 @@ static int __init it87_find(unsigned short *address,
 		sio_data->skip_in |= (1 << 5); /* No VIN5 */
 		sio_data->skip_in |= (1 << 6); /* No VIN6 */
 
-		/* no fan4 */
-		sio_data->skip_pwm |= (1 << 3);
-		sio_data->skip_fan |= (1 << 3);
-
-		sio_data->internal |= (1 << 1); /* in7 is VSB */
 		sio_data->internal |= (1 << 3); /* in9 is AVCC */
 
 		sio_data->beep_pin = superio_inb(IT87_SIO_BEEP_PIN_REG) & 0x3f;
+	} else if (sio_data->type == it8620) {
+		int reg;
+
+		superio_select(GPIO);
+
+		/* Check for fan4, fan5 */
+		reg = superio_inb(IT87_SIO_GPIO2_REG);
+		if (!(reg & (1 << 5)))
+			sio_data->skip_fan |= (1 << 3);
+		if (!(reg & (1 << 4)))
+			sio_data->skip_fan |= (1 << 4);
+
+		/* Check for pwm3, fan3 */
+		reg = superio_inb(IT87_SIO_GPIO3_REG);
+		if (reg & (1 << 6))
+			sio_data->skip_pwm |= (1 << 2);
+		if (reg & (1 << 7))
+			sio_data->skip_fan |= (1 << 2);
+
+		/* Check for pwm2, fan2 */
+		reg = superio_inb(IT87_SIO_GPIO5_REG);
+		if (reg & (1 << 1))
+			sio_data->skip_pwm |= (1 << 1);
+		if (reg & (1 << 2))
+			sio_data->skip_fan |= (1 << 1);
+
+		sio_data->beep_pin = superio_inb(IT87_SIO_BEEP_PIN_REG) & 0x3f;
 	} else {
 		int reg;
 		bool uart6;
@@ -1917,15 +2052,7 @@ static int __init it87_find(unsigned short *address,
 		superio_select(GPIO);
 
 		reg = superio_inb(IT87_SIO_GPIO3_REG);
-		if (sio_data->type == it8721 || sio_data->type == it8728 ||
-		    sio_data->type == it8771 || sio_data->type == it8772 ||
-		    sio_data->type == it8782) {
-			/*
-			 * IT8721F/IT8758E, and IT8782F don't have VID pins
-			 * at all, not sure about the IT8728F and compatibles.
-			 */
-			sio_data->skip_vid = 1;
-		} else {
+		if (!sio_data->skip_vid) {
 			/* We need at least 4 VID pins */
 			if (reg & 0x0f) {
 				pr_info("VID is disabled (pins used for GPIO)\n");
@@ -1975,10 +2102,7 @@ static int __init it87_find(unsigned short *address,
 		}
 		if (reg & (1 << 0))
 			sio_data->internal |= (1 << 0);
-		if ((reg & (1 << 1)) || sio_data->type == it8721 ||
-		    sio_data->type == it8728 ||
-		    sio_data->type == it8771 ||
-		    sio_data->type == it8772)
+		if (reg & (1 << 1))
 			sio_data->internal |= (1 << 1);
 
 		/*
@@ -2050,7 +2174,7 @@ static void it87_remove_files(struct device *dev)
 			sysfs_remove_file(&dev->kobj,
 					  it87_attributes_temp_beep[i]);
 	}
-	for (i = 0; i < 5; i++) {
+	for (i = 0; i < 6; i++) {
 		if (!(data->has_fan & (1 << i)))
 			continue;
 		sysfs_remove_group(&dev->kobj, &it87_group_fan[i]);
@@ -2062,7 +2186,7 @@ static void it87_remove_files(struct device *dev)
 					  it87_attributes_fan_div[i]);
 	}
 	for (i = 0; i < 3; i++) {
-		if (sio_data->skip_pwm & (1 << 0))
+		if (sio_data->skip_pwm & (1 << i))
 			continue;
 		sysfs_remove_group(&dev->kobj, &it87_group_pwm[i]);
 		if (has_old_autopwm(data))
@@ -2112,13 +2236,14 @@ static int it87_probe(struct platform_device *pdev)
 	case it87:
 		if (sio_data->revision >= 0x03) {
 			data->features &= ~FEAT_OLD_AUTOPWM;
-			data->features |= FEAT_16BIT_FANS;
+			data->features |= FEAT_FAN16_CONFIG | FEAT_16BIT_FANS;
 		}
 		break;
 	case it8712:
 		if (sio_data->revision >= 0x08) {
 			data->features &= ~FEAT_OLD_AUTOPWM;
-			data->features |= FEAT_16BIT_FANS;
+			data->features |= FEAT_FAN16_CONFIG | FEAT_16BIT_FANS |
+					  FEAT_FIVE_FANS;
 		}
 		break;
 	default:
@@ -2147,7 +2272,8 @@ static int it87_probe(struct platform_device *pdev)
 			data->in_scaled |= (1 << 8);	/* in8 is Vbat */
 		if (sio_data->internal & (1 << 3))
 			data->in_scaled |= (1 << 9);	/* in9 is AVCC */
-	} else if (sio_data->type == it8782 || sio_data->type == it8783) {
+	} else if (sio_data->type == it8781 || sio_data->type == it8782 ||
+		   sio_data->type == it8783) {
 		if (sio_data->internal & (1 << 0))
 			data->in_scaled |= (1 << 3);	/* in3 is VCC5V */
 		if (sio_data->internal & (1 << 1))
@@ -2205,7 +2331,7 @@ static int it87_probe(struct platform_device *pdev)
 
 	/* Do not create fan files for disabled fans */
 	fan_beep_need_rw = 1;
-	for (i = 0; i < 5; i++) {
+	for (i = 0; i < 6; i++) {
 		if (!(data->has_fan & (1 << i)))
 			continue;
 		err = sysfs_create_group(&dev->kobj, &it87_group_fan[i]);
@@ -2450,24 +2576,26 @@ static void it87_init_device(struct platform_device *pdev)
 	}
 	data->has_fan = (data->fan_main_ctrl >> 4) & 0x07;
 
-	/* Set tachometers to 16-bit mode if needed, IT8603E (and IT8728F?)
-	 * has it by default */
-	if (has_16bit_fans(data) && data->type != it8603) {
-		tmp = it87_read_value(data, IT87_REG_FAN_16BIT);
+	tmp = it87_read_value(data, IT87_REG_FAN_16BIT);
+
+	/* Set tachometers to 16-bit mode if needed */
+	if (has_fan16_config(data)) {
 		if (~tmp & 0x07 & data->has_fan) {
 			dev_dbg(&pdev->dev,
 				"Setting fan1-3 to 16-bit mode\n");
 			it87_write_value(data, IT87_REG_FAN_16BIT,
 					 tmp | 0x07);
 		}
-		/* IT8705F, IT8782F, and IT8783E/F only support three fans. */
-		if (data->type != it87 && data->type != it8782 &&
-		    data->type != it8783) {
-			if (tmp & (1 << 4))
-				data->has_fan |= (1 << 3); /* fan4 enabled */
-			if (tmp & (1 << 5))
-				data->has_fan |= (1 << 4); /* fan5 enabled */
-		}
+	}
+
+	/* Check for additional fans */
+	if (has_five_fans(data)) {
+		if (tmp & (1 << 4))
+			data->has_fan |= (1 << 3); /* fan4 enabled */
+		if (tmp & (1 << 5))
+			data->has_fan |= (1 << 4); /* fan5 enabled */
+		if (has_six_fans(data) && (tmp & (1 << 2)))
+			data->has_fan |= (1 << 5); /* fan6 enabled */
 	}
 
 	/* Fan input pins may be used for alternative functions */
@@ -2535,7 +2663,7 @@ static struct it87_data *it87_update_device(struct device *dev)
 		if (data->type == it8603)
 			data->in[9][0] = it87_read_value(data, 0x2f);
 
-		for (i = 0; i < 5; i++) {
+		for (i = 0; i < 6; i++) {
 			/* Skip disabled fans */
 			if (!(data->has_fan & (1 << i)))
 				continue;
diff --git a/drivers/hwmon/jc42.c b/drivers/hwmon/jc42.c
index 996bdfd5cf25..9887d3224a86 100644
--- a/drivers/hwmon/jc42.c
+++ b/drivers/hwmon/jc42.c
@@ -87,11 +87,14 @@ static const unsigned short normal_i2c[] = {
 #define AT30TSE004_DEVID_MASK	0xffff
 
 /* IDT */
-#define TS3000B3_DEVID		0x2903  /* Also matches TSE2002B3 */
-#define TS3000B3_DEVID_MASK	0xffff
+#define TSE2004_DEVID		0x2200
+#define TSE2004_DEVID_MASK	0xff00
 
-#define TS3000GB2_DEVID		0x2912  /* Also matches TSE2002GB2 */
-#define TS3000GB2_DEVID_MASK	0xffff
+#define TS3000_DEVID		0x2900  /* Also matches TSE2002 */
+#define TS3000_DEVID_MASK	0xff00
+
+#define TS3001_DEVID		0x3000
+#define TS3001_DEVID_MASK	0xff00
 
 /* Maxim */
 #define MAX6604_DEVID		0x3e00
@@ -152,8 +155,9 @@ static struct jc42_chips jc42_chips[] = {
 	{ ADT_MANID, ADT7408_DEVID, ADT7408_DEVID_MASK },
 	{ ATMEL_MANID, AT30TS00_DEVID, AT30TS00_DEVID_MASK },
 	{ ATMEL_MANID2, AT30TSE004_DEVID, AT30TSE004_DEVID_MASK },
-	{ IDT_MANID, TS3000B3_DEVID, TS3000B3_DEVID_MASK },
-	{ IDT_MANID, TS3000GB2_DEVID, TS3000GB2_DEVID_MASK },
+	{ IDT_MANID, TSE2004_DEVID, TSE2004_DEVID_MASK },
+	{ IDT_MANID, TS3000_DEVID, TS3000_DEVID_MASK },
+	{ IDT_MANID, TS3001_DEVID, TS3001_DEVID_MASK },
 	{ MAX_MANID, MAX6604_DEVID, MAX6604_DEVID_MASK },
 	{ MCP_MANID, MCP9804_DEVID, MCP9804_DEVID_MASK },
 	{ MCP_MANID, MCP98242_DEVID, MCP98242_DEVID_MASK },
diff --git a/drivers/hwmon/nct6775.c b/drivers/hwmon/nct6775.c
index 1be41177b620..4fcb48103299 100644
--- a/drivers/hwmon/nct6775.c
+++ b/drivers/hwmon/nct6775.c
@@ -57,6 +57,7 @@
 #include <linux/err.h>
 #include <linux/mutex.h>
 #include <linux/acpi.h>
+#include <linux/dmi.h>
 #include <linux/io.h>
 #include "lm75.h"
 
@@ -880,12 +881,12 @@ struct nct6775_data {
 	u16 have_temp;
 	u16 have_temp_fixed;
 	u16 have_in;
-#ifdef CONFIG_PM
+
 	/* Remember extra register values over suspend/resume */
 	u8 vbat;
 	u8 fandiv1;
 	u8 fandiv2;
-#endif
+	u8 sio_reg_enable;
 };
 
 struct nct6775_sio_data {
@@ -3178,6 +3179,10 @@ nct6775_check_fan_inputs(struct nct6775_data *data)
 	int sioreg = data->sioreg;
 	int regval;
 
+	/* Store SIO_REG_ENABLE for use during resume */
+	superio_select(sioreg, NCT6775_LD_HWM);
+	data->sio_reg_enable = superio_inb(sioreg, SIO_REG_ENABLE);
+
 	/* fan4 and fan5 share some pins with the GPIO and serial flash */
 	if (data->kind == nct6775) {
 		regval = superio_inb(sioreg, 0x2c);
@@ -3195,21 +3200,38 @@ nct6775_check_fan_inputs(struct nct6775_data *data)
 		pwm6pin = false;
 	} else if (data->kind == nct6776) {
 		bool gpok = superio_inb(sioreg, 0x27) & 0x80;
+		const char *board_vendor, *board_name;
 
-		superio_select(sioreg, NCT6775_LD_HWM);
-		regval = superio_inb(sioreg, SIO_REG_ENABLE);
+		board_vendor = dmi_get_system_info(DMI_BOARD_VENDOR);
+		board_name = dmi_get_system_info(DMI_BOARD_NAME);
+
+		if (board_name && board_vendor &&
+		    !strcmp(board_vendor, "ASRock")) {
+			/*
+			 * Auxiliary fan monitoring is not enabled on ASRock
+			 * Z77 Pro4-M if booted in UEFI Ultra-FastBoot mode.
+			 * Observed with BIOS version 2.00.
+			 */
+			if (!strcmp(board_name, "Z77 Pro4-M")) {
+				if ((data->sio_reg_enable & 0xe0) != 0xe0) {
+					data->sio_reg_enable |= 0xe0;
+					superio_outb(sioreg, SIO_REG_ENABLE,
+						     data->sio_reg_enable);
+				}
+			}
+		}
 
-		if (regval & 0x80)
+		if (data->sio_reg_enable & 0x80)
 			fan3pin = gpok;
 		else
 			fan3pin = !(superio_inb(sioreg, 0x24) & 0x40);
 
-		if (regval & 0x40)
+		if (data->sio_reg_enable & 0x40)
 			fan4pin = gpok;
 		else
 			fan4pin = superio_inb(sioreg, 0x1C) & 0x01;
 
-		if (regval & 0x20)
+		if (data->sio_reg_enable & 0x20)
 			fan5pin = gpok;
 		else
 			fan5pin = superio_inb(sioreg, 0x1C) & 0x02;
@@ -3989,8 +4011,7 @@ static void nct6791_enable_io_mapping(int sioaddr)
 	}
 }
 
-#ifdef CONFIG_PM
-static int nct6775_suspend(struct device *dev)
+static int __maybe_unused nct6775_suspend(struct device *dev)
 {
 	struct nct6775_data *data = nct6775_update_device(dev);
 
@@ -4005,22 +4026,29 @@ static int nct6775_suspend(struct device *dev)
 	return 0;
 }
 
-static int nct6775_resume(struct device *dev)
+static int __maybe_unused nct6775_resume(struct device *dev)
 {
 	struct nct6775_data *data = dev_get_drvdata(dev);
+	int sioreg = data->sioreg;
 	int i, j, err = 0;
+	u8 reg;
 
 	mutex_lock(&data->update_lock);
 	data->bank = 0xff;		/* Force initial bank selection */
 
-	if (data->kind == nct6791 || data->kind == nct6792) {
-		err = superio_enter(data->sioreg);
-		if (err)
-			goto abort;
+	err = superio_enter(sioreg);
+	if (err)
+		goto abort;
 
-		nct6791_enable_io_mapping(data->sioreg);
-		superio_exit(data->sioreg);
-	}
+	superio_select(sioreg, NCT6775_LD_HWM);
+	reg = superio_inb(sioreg, SIO_REG_ENABLE);
+	if (reg != data->sio_reg_enable)
+		superio_outb(sioreg, SIO_REG_ENABLE, data->sio_reg_enable);
+
+	if (data->kind == nct6791 || data->kind == nct6792)
+		nct6791_enable_io_mapping(sioreg);
+
+	superio_exit(sioreg);
 
 	/* Restore limits */
 	for (i = 0; i < data->in_num; i++) {
@@ -4066,22 +4094,12 @@ abort:
 	return err;
 }
 
-static const struct dev_pm_ops nct6775_dev_pm_ops = {
-	.suspend = nct6775_suspend,
-	.resume = nct6775_resume,
-	.freeze = nct6775_suspend,
-	.restore = nct6775_resume,
-};
-
-#define NCT6775_DEV_PM_OPS	(&nct6775_dev_pm_ops)
-#else
-#define NCT6775_DEV_PM_OPS	NULL
-#endif /* CONFIG_PM */
+static SIMPLE_DEV_PM_OPS(nct6775_dev_pm_ops, nct6775_suspend, nct6775_resume);
 
 static struct platform_driver nct6775_driver = {
 	.driver = {
 		.name	= DRVNAME,
-		.pm	= NCT6775_DEV_PM_OPS,
+		.pm	= &nct6775_dev_pm_ops,
 	},
 	.probe		= nct6775_probe,
 };
diff --git a/drivers/hwmon/nct7904.c b/drivers/hwmon/nct7904.c
new file mode 100644
index 000000000000..b77b82f24480
--- /dev/null
+++ b/drivers/hwmon/nct7904.c
@@ -0,0 +1,593 @@
+/*
+ * nct7904.c - driver for Nuvoton NCT7904D.
+ *
+ * Copyright (c) 2015 Kontron
+ * Author: Vadim V. Vlasov <vvlasov@dev.rtsoft.ru>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/module.h>
+#include <linux/device.h>
+#include <linux/init.h>
+#include <linux/i2c.h>
+#include <linux/mutex.h>
+#include <linux/hwmon.h>
+#include <linux/hwmon-sysfs.h>
+
+#define VENDOR_ID_REG		0x7A	/* Any bank */
+#define NUVOTON_ID		0x50
+#define CHIP_ID_REG		0x7B	/* Any bank */
+#define NCT7904_ID		0xC5
+#define DEVICE_ID_REG		0x7C	/* Any bank */
+
+#define BANK_SEL_REG		0xFF
+#define BANK_0			0x00
+#define BANK_1			0x01
+#define BANK_2			0x02
+#define BANK_3			0x03
+#define BANK_4			0x04
+#define BANK_MAX		0x04
+
+#define FANIN_MAX		12	/* Counted from 1 */
+#define VSEN_MAX		21	/* VSEN1..14, 3VDD, VBAT, V3VSB,
+					   LTD (not a voltage), VSEN17..19 */
+#define FANCTL_MAX		4	/* Counted from 1 */
+#define TCPU_MAX		8	/* Counted from 1 */
+#define TEMP_MAX		4	/* Counted from 1 */
+
+#define VT_ADC_CTRL0_REG	0x20	/* Bank 0 */
+#define VT_ADC_CTRL1_REG	0x21	/* Bank 0 */
+#define VT_ADC_CTRL2_REG	0x22	/* Bank 0 */
+#define FANIN_CTRL0_REG		0x24
+#define FANIN_CTRL1_REG		0x25
+#define DTS_T_CTRL0_REG		0x26
+#define DTS_T_CTRL1_REG		0x27
+#define VT_ADC_MD_REG		0x2E
+
+#define VSEN1_HV_REG		0x40	/* Bank 0; 2 regs (HV/LV) per sensor */
+#define TEMP_CH1_HV_REG		0x42	/* Bank 0; same as VSEN2_HV */
+#define LTD_HV_REG		0x62	/* Bank 0; 2 regs in VSEN range */
+#define FANIN1_HV_REG		0x80	/* Bank 0; 2 regs (HV/LV) per sensor */
+#define T_CPU1_HV_REG		0xA0	/* Bank 0; 2 regs (HV/LV) per sensor */
+
+#define PRTS_REG		0x03	/* Bank 2 */
+#define FANCTL1_FMR_REG		0x00	/* Bank 3; 1 reg per channel */
+#define FANCTL1_OUT_REG		0x10	/* Bank 3; 1 reg per channel */
+
+static const unsigned short normal_i2c[] = {
+	0x2d, 0x2e, I2C_CLIENT_END
+};
+
+struct nct7904_data {
+	struct i2c_client *client;
+	struct mutex bank_lock;
+	int bank_sel;
+	u32 fanin_mask;
+	u32 vsen_mask;
+	u32 tcpu_mask;
+	u8 fan_mode[FANCTL_MAX];
+};
+
+/* Access functions */
+static int nct7904_bank_lock(struct nct7904_data *data, unsigned bank)
+{
+	int ret;
+
+	mutex_lock(&data->bank_lock);
+	if (data->bank_sel == bank)
+		return 0;
+	ret = i2c_smbus_write_byte_data(data->client, BANK_SEL_REG, bank);
+	if (ret == 0)
+		data->bank_sel = bank;
+	else
+		data->bank_sel = -1;
+	return ret;
+}
+
+static inline void nct7904_bank_release(struct nct7904_data *data)
+{
+	mutex_unlock(&data->bank_lock);
+}
+
+/* Read 1-byte register. Returns unsigned reg or -ERRNO on error. */
+static int nct7904_read_reg(struct nct7904_data *data,
+			    unsigned bank, unsigned reg)
+{
+	struct i2c_client *client = data->client;
+	int ret;
+
+	ret = nct7904_bank_lock(data, bank);
+	if (ret == 0)
+		ret = i2c_smbus_read_byte_data(client, reg);
+
+	nct7904_bank_release(data);
+	return ret;
+}
+
+/*
+ * Read 2-byte register. Returns register in big-endian format or
+ * -ERRNO on error.
+ */
+static int nct7904_read_reg16(struct nct7904_data *data,
+			      unsigned bank, unsigned reg)
+{
+	struct i2c_client *client = data->client;
+	int ret, hi;
+
+	ret = nct7904_bank_lock(data, bank);
+	if (ret == 0) {
+		ret = i2c_smbus_read_byte_data(client, reg);
+		if (ret >= 0) {
+			hi = ret;
+			ret = i2c_smbus_read_byte_data(client, reg + 1);
+			if (ret >= 0)
+				ret |= hi << 8;
+		}
+	}
+
+	nct7904_bank_release(data);
+	return ret;
+}
+
+/* Write 1-byte register. Returns 0 or -ERRNO on error. */
+static int nct7904_write_reg(struct nct7904_data *data,
+			     unsigned bank, unsigned reg, u8 val)
+{
+	struct i2c_client *client = data->client;
+	int ret;
+
+	ret = nct7904_bank_lock(data, bank);
+	if (ret == 0)
+		ret = i2c_smbus_write_byte_data(client, reg, val);
+
+	nct7904_bank_release(data);
+	return ret;
+}
+
+/* FANIN ATTR */
+static ssize_t show_fan(struct device *dev,
+			struct device_attribute *devattr, char *buf)
+{
+	int index = to_sensor_dev_attr(devattr)->index;
+	struct nct7904_data *data = dev_get_drvdata(dev);
+	int ret;
+	unsigned cnt, rpm;
+
+	ret = nct7904_read_reg16(data, BANK_0, FANIN1_HV_REG + index * 2);
+	if (ret < 0)
+		return ret;
+	cnt = ((ret & 0xff00) >> 3) | (ret & 0x1f);
+	if (cnt == 0x1fff)
+		rpm = 0;
+	else
+		rpm = 1350000 / cnt;
+	return sprintf(buf, "%u\n", rpm);
+}
+
+static umode_t nct7904_fanin_is_visible(struct kobject *kobj,
+					struct attribute *a, int n)
+{
+	struct device *dev = container_of(kobj, struct device, kobj);
+	struct nct7904_data *data = dev_get_drvdata(dev);
+
+	if (data->fanin_mask & (1 << n))
+		return a->mode;
+	return 0;
+}
+
+static SENSOR_DEVICE_ATTR(fan1_input, S_IRUGO, show_fan, NULL, 0);
+static SENSOR_DEVICE_ATTR(fan2_input, S_IRUGO, show_fan, NULL, 1);
+static SENSOR_DEVICE_ATTR(fan3_input, S_IRUGO, show_fan, NULL, 2);
+static SENSOR_DEVICE_ATTR(fan4_input, S_IRUGO, show_fan, NULL, 3);
+static SENSOR_DEVICE_ATTR(fan5_input, S_IRUGO, show_fan, NULL, 4);
+static SENSOR_DEVICE_ATTR(fan6_input, S_IRUGO, show_fan, NULL, 5);
+static SENSOR_DEVICE_ATTR(fan7_input, S_IRUGO, show_fan, NULL, 6);
+static SENSOR_DEVICE_ATTR(fan8_input, S_IRUGO, show_fan, NULL, 7);
+static SENSOR_DEVICE_ATTR(fan9_input, S_IRUGO, show_fan, NULL, 8);
+static SENSOR_DEVICE_ATTR(fan10_input, S_IRUGO, show_fan, NULL, 9);
+static SENSOR_DEVICE_ATTR(fan11_input, S_IRUGO, show_fan, NULL, 10);
+static SENSOR_DEVICE_ATTR(fan12_input, S_IRUGO, show_fan, NULL, 11);
+
+static struct attribute *nct7904_fanin_attrs[] = {
+	&sensor_dev_attr_fan1_input.dev_attr.attr,
+	&sensor_dev_attr_fan2_input.dev_attr.attr,
+	&sensor_dev_attr_fan3_input.dev_attr.attr,
+	&sensor_dev_attr_fan4_input.dev_attr.attr,
+	&sensor_dev_attr_fan5_input.dev_attr.attr,
+	&sensor_dev_attr_fan6_input.dev_attr.attr,
+	&sensor_dev_attr_fan7_input.dev_attr.attr,
+	&sensor_dev_attr_fan8_input.dev_attr.attr,
+	&sensor_dev_attr_fan9_input.dev_attr.attr,
+	&sensor_dev_attr_fan10_input.dev_attr.attr,
+	&sensor_dev_attr_fan11_input.dev_attr.attr,
+	&sensor_dev_attr_fan12_input.dev_attr.attr,
+	NULL
+};
+
+static const struct attribute_group nct7904_fanin_group = {
+	.attrs = nct7904_fanin_attrs,
+	.is_visible = nct7904_fanin_is_visible,
+};
+
+/* VSEN ATTR */
+static ssize_t show_voltage(struct device *dev,
+			    struct device_attribute *devattr, char *buf)
+{
+	int index = to_sensor_dev_attr(devattr)->index;
+	struct nct7904_data *data = dev_get_drvdata(dev);
+	int ret;
+	int volt;
+
+	ret = nct7904_read_reg16(data, BANK_0, VSEN1_HV_REG + index * 2);
+	if (ret < 0)
+		return ret;
+	volt = ((ret & 0xff00) >> 5) | (ret & 0x7);
+	if (index < 14)
+		volt *= 2; /* 0.002V scale */
+	else
+		volt *= 6; /* 0.006V scale */
+
+	return sprintf(buf, "%d\n", volt);
+}
+
+static ssize_t show_ltemp(struct device *dev,
+			  struct device_attribute *devattr, char *buf)
+{
+	struct nct7904_data *data = dev_get_drvdata(dev);
+	int ret;
+	int temp;
+
+	ret = nct7904_read_reg16(data, BANK_0, LTD_HV_REG);
+	if (ret < 0)
+		return ret;
+	temp = ((ret & 0xff00) >> 5) | (ret & 0x7);
+	temp = sign_extend32(temp, 10) * 125;
+
+	return sprintf(buf, "%d\n", temp);
+}
+
+static umode_t nct7904_vsen_is_visible(struct kobject *kobj,
+				       struct attribute *a, int n)
+{
+	struct device *dev = container_of(kobj, struct device, kobj);
+	struct nct7904_data *data = dev_get_drvdata(dev);
+
+	if (data->vsen_mask & (1 << n))
+		return a->mode;
+	return 0;
+}
+
+static SENSOR_DEVICE_ATTR(in1_input, S_IRUGO, show_voltage, NULL, 0);
+static SENSOR_DEVICE_ATTR(in2_input, S_IRUGO, show_voltage, NULL, 1);
+static SENSOR_DEVICE_ATTR(in3_input, S_IRUGO, show_voltage, NULL, 2);
+static SENSOR_DEVICE_ATTR(in4_input, S_IRUGO, show_voltage, NULL, 3);
+static SENSOR_DEVICE_ATTR(in5_input, S_IRUGO, show_voltage, NULL, 4);
+static SENSOR_DEVICE_ATTR(in6_input, S_IRUGO, show_voltage, NULL, 5);
+static SENSOR_DEVICE_ATTR(in7_input, S_IRUGO, show_voltage, NULL, 6);
+static SENSOR_DEVICE_ATTR(in8_input, S_IRUGO, show_voltage, NULL, 7);
+static SENSOR_DEVICE_ATTR(in9_input, S_IRUGO, show_voltage, NULL, 8);
+static SENSOR_DEVICE_ATTR(in10_input, S_IRUGO, show_voltage, NULL, 9);
+static SENSOR_DEVICE_ATTR(in11_input, S_IRUGO, show_voltage, NULL, 10);
+static SENSOR_DEVICE_ATTR(in12_input, S_IRUGO, show_voltage, NULL, 11);
+static SENSOR_DEVICE_ATTR(in13_input, S_IRUGO, show_voltage, NULL, 12);
+static SENSOR_DEVICE_ATTR(in14_input, S_IRUGO, show_voltage, NULL, 13);
+/*
+ * Next 3 voltage sensors have specific names in the Nuvoton doc
+ * (3VDD, VBAT, 3VSB) but we use vacant numbers for them.
+ */
+static SENSOR_DEVICE_ATTR(in15_input, S_IRUGO, show_voltage, NULL, 14);
+static SENSOR_DEVICE_ATTR(in16_input, S_IRUGO, show_voltage, NULL, 15);
+static SENSOR_DEVICE_ATTR(in20_input, S_IRUGO, show_voltage, NULL, 16);
+/* This is not a voltage, but a local temperature sensor. */
+static SENSOR_DEVICE_ATTR(temp1_input, S_IRUGO, show_ltemp, NULL, 0);
+static SENSOR_DEVICE_ATTR(in17_input, S_IRUGO, show_voltage, NULL, 18);
+static SENSOR_DEVICE_ATTR(in18_input, S_IRUGO, show_voltage, NULL, 19);
+static SENSOR_DEVICE_ATTR(in19_input, S_IRUGO, show_voltage, NULL, 20);
+
+static struct attribute *nct7904_vsen_attrs[] = {
+	&sensor_dev_attr_in1_input.dev_attr.attr,
+	&sensor_dev_attr_in2_input.dev_attr.attr,
+	&sensor_dev_attr_in3_input.dev_attr.attr,
+	&sensor_dev_attr_in4_input.dev_attr.attr,
+	&sensor_dev_attr_in5_input.dev_attr.attr,
+	&sensor_dev_attr_in6_input.dev_attr.attr,
+	&sensor_dev_attr_in7_input.dev_attr.attr,
+	&sensor_dev_attr_in8_input.dev_attr.attr,
+	&sensor_dev_attr_in9_input.dev_attr.attr,
+	&sensor_dev_attr_in10_input.dev_attr.attr,
+	&sensor_dev_attr_in11_input.dev_attr.attr,
+	&sensor_dev_attr_in12_input.dev_attr.attr,
+	&sensor_dev_attr_in13_input.dev_attr.attr,
+	&sensor_dev_attr_in14_input.dev_attr.attr,
+	&sensor_dev_attr_in15_input.dev_attr.attr,
+	&sensor_dev_attr_in16_input.dev_attr.attr,
+	&sensor_dev_attr_in20_input.dev_attr.attr,
+	&sensor_dev_attr_temp1_input.dev_attr.attr,
+	&sensor_dev_attr_in17_input.dev_attr.attr,
+	&sensor_dev_attr_in18_input.dev_attr.attr,
+	&sensor_dev_attr_in19_input.dev_attr.attr,
+	NULL
+};
+
+static const struct attribute_group nct7904_vsen_group = {
+	.attrs = nct7904_vsen_attrs,
+	.is_visible = nct7904_vsen_is_visible,
+};
+
+/* CPU_TEMP ATTR */
+static ssize_t show_tcpu(struct device *dev,
+			 struct device_attribute *devattr, char *buf)
+{
+	int index = to_sensor_dev_attr(devattr)->index;
+	struct nct7904_data *data = dev_get_drvdata(dev);
+	int ret;
+	int temp;
+
+	ret = nct7904_read_reg16(data, BANK_0, T_CPU1_HV_REG + index * 2);
+	if (ret < 0)
+		return ret;
+
+	temp = ((ret & 0xff00) >> 5) | (ret & 0x7);
+	temp = sign_extend32(temp, 10) * 125;
+	return sprintf(buf, "%d\n", temp);
+}
+
+static umode_t nct7904_tcpu_is_visible(struct kobject *kobj,
+				       struct attribute *a, int n)
+{
+	struct device *dev = container_of(kobj, struct device, kobj);
+	struct nct7904_data *data = dev_get_drvdata(dev);
+
+	if (data->tcpu_mask & (1 << n))
+		return a->mode;
+	return 0;
+}
+
+/* "temp1_input" reserved for local temp */
+static SENSOR_DEVICE_ATTR(temp2_input, S_IRUGO, show_tcpu, NULL, 0);
+static SENSOR_DEVICE_ATTR(temp3_input, S_IRUGO, show_tcpu, NULL, 1);
+static SENSOR_DEVICE_ATTR(temp4_input, S_IRUGO, show_tcpu, NULL, 2);
+static SENSOR_DEVICE_ATTR(temp5_input, S_IRUGO, show_tcpu, NULL, 3);
+static SENSOR_DEVICE_ATTR(temp6_input, S_IRUGO, show_tcpu, NULL, 4);
+static SENSOR_DEVICE_ATTR(temp7_input, S_IRUGO, show_tcpu, NULL, 5);
+static SENSOR_DEVICE_ATTR(temp8_input, S_IRUGO, show_tcpu, NULL, 6);
+static SENSOR_DEVICE_ATTR(temp9_input, S_IRUGO, show_tcpu, NULL, 7);
+
+static struct attribute *nct7904_tcpu_attrs[] = {
+	&sensor_dev_attr_temp2_input.dev_attr.attr,
+	&sensor_dev_attr_temp3_input.dev_attr.attr,
+	&sensor_dev_attr_temp4_input.dev_attr.attr,
+	&sensor_dev_attr_temp5_input.dev_attr.attr,
+	&sensor_dev_attr_temp6_input.dev_attr.attr,
+	&sensor_dev_attr_temp7_input.dev_attr.attr,
+	&sensor_dev_attr_temp8_input.dev_attr.attr,
+	&sensor_dev_attr_temp9_input.dev_attr.attr,
+	NULL
+};
+
+static const struct attribute_group nct7904_tcpu_group = {
+	.attrs = nct7904_tcpu_attrs,
+	.is_visible = nct7904_tcpu_is_visible,
+};
+
+/* PWM ATTR */
+static ssize_t store_pwm(struct device *dev, struct device_attribute *devattr,
+			 const char *buf, size_t count)
+{
+	int index = to_sensor_dev_attr(devattr)->index;
+	struct nct7904_data *data = dev_get_drvdata(dev);
+	unsigned long val;
+	int ret;
+
+	if (kstrtoul(buf, 10, &val) < 0)
+		return -EINVAL;
+	if (val > 255)
+		return -EINVAL;
+
+	ret = nct7904_write_reg(data, BANK_3, FANCTL1_OUT_REG + index, val);
+
+	return ret ? ret : count;
+}
+
+static ssize_t show_pwm(struct device *dev,
+			struct device_attribute *devattr, char *buf)
+{
+	int index = to_sensor_dev_attr(devattr)->index;
+	struct nct7904_data *data = dev_get_drvdata(dev);
+	int val;
+
+	val = nct7904_read_reg(data, BANK_3, FANCTL1_OUT_REG + index);
+	if (val < 0)
+		return val;
+
+	return sprintf(buf, "%d\n", val);
+}
+
+static ssize_t store_mode(struct device *dev, struct device_attribute *devattr,
+			  const char *buf, size_t count)
+{
+	int index = to_sensor_dev_attr(devattr)->index;
+	struct nct7904_data *data = dev_get_drvdata(dev);
+	unsigned long val;
+	int ret;
+
+	if (kstrtoul(buf, 10, &val) < 0)
+		return -EINVAL;
+	if (val > 1 || (val && !data->fan_mode[index]))
+		return -EINVAL;
+
+	ret = nct7904_write_reg(data, BANK_3, FANCTL1_FMR_REG + index,
+				val ? data->fan_mode[index] : 0);
+
+	return ret ? ret : count;
+}
+
+/* Return 0 for manual mode or 1 for SmartFan mode */
+static ssize_t show_mode(struct device *dev,
+			 struct device_attribute *devattr, char *buf)
+{
+	int index = to_sensor_dev_attr(devattr)->index;
+	struct nct7904_data *data = dev_get_drvdata(dev);
+	int val;
+
+	val = nct7904_read_reg(data, BANK_3, FANCTL1_FMR_REG + index);
+	if (val < 0)
+		return val;
+
+	return sprintf(buf, "%d\n", val ? 1 : 0);
+}
+
+/* 2 attributes per channel: pwm and mode */
+static SENSOR_DEVICE_ATTR(fan1_pwm, S_IRUGO | S_IWUSR,
+			show_pwm, store_pwm, 0);
+static SENSOR_DEVICE_ATTR(fan1_mode, S_IRUGO | S_IWUSR,
+			show_mode, store_mode, 0);
+static SENSOR_DEVICE_ATTR(fan2_pwm, S_IRUGO | S_IWUSR,
+			show_pwm, store_pwm, 1);
+static SENSOR_DEVICE_ATTR(fan2_mode, S_IRUGO | S_IWUSR,
+			show_mode, store_mode, 1);
+static SENSOR_DEVICE_ATTR(fan3_pwm, S_IRUGO | S_IWUSR,
+			show_pwm, store_pwm, 2);
+static SENSOR_DEVICE_ATTR(fan3_mode, S_IRUGO | S_IWUSR,
+			show_mode, store_mode, 2);
+static SENSOR_DEVICE_ATTR(fan4_pwm, S_IRUGO | S_IWUSR,
+			show_pwm, store_pwm, 3);
+static SENSOR_DEVICE_ATTR(fan4_mode, S_IRUGO | S_IWUSR,
+			show_mode, store_mode, 3);
+
+static struct attribute *nct7904_fanctl_attrs[] = {
+	&sensor_dev_attr_fan1_pwm.dev_attr.attr,
+	&sensor_dev_attr_fan1_mode.dev_attr.attr,
+	&sensor_dev_attr_fan2_pwm.dev_attr.attr,
+	&sensor_dev_attr_fan2_mode.dev_attr.attr,
+	&sensor_dev_attr_fan3_pwm.dev_attr.attr,
+	&sensor_dev_attr_fan3_mode.dev_attr.attr,
+	&sensor_dev_attr_fan4_pwm.dev_attr.attr,
+	&sensor_dev_attr_fan4_mode.dev_attr.attr,
+	NULL
+};
+
+static const struct attribute_group nct7904_fanctl_group = {
+	.attrs = nct7904_fanctl_attrs,
+};
+
+static const struct attribute_group *nct7904_groups[] = {
+	&nct7904_fanin_group,
+	&nct7904_vsen_group,
+	&nct7904_tcpu_group,
+	&nct7904_fanctl_group,
+	NULL
+};
+
+/* Return 0 if detection is successful, -ENODEV otherwise */
+static int nct7904_detect(struct i2c_client *client,
+			  struct i2c_board_info *info)
+{
+	struct i2c_adapter *adapter = client->adapter;
+
+	if (!i2c_check_functionality(adapter,
+				     I2C_FUNC_SMBUS_READ_BYTE |
+				     I2C_FUNC_SMBUS_WRITE_BYTE_DATA))
+		return -ENODEV;
+
+	/* Determine the chip type. */
+	if (i2c_smbus_read_byte_data(client, VENDOR_ID_REG) != NUVOTON_ID ||
+	    i2c_smbus_read_byte_data(client, CHIP_ID_REG) != NCT7904_ID ||
+	    (i2c_smbus_read_byte_data(client, DEVICE_ID_REG) & 0xf0) != 0x50 ||
+	    (i2c_smbus_read_byte_data(client, BANK_SEL_REG) & 0xf8) != 0x00)
+		return -ENODEV;
+
+	strlcpy(info->type, "nct7904", I2C_NAME_SIZE);
+
+	return 0;
+}
+
+static int nct7904_probe(struct i2c_client *client,
+			 const struct i2c_device_id *id)
+{
+	struct nct7904_data *data;
+	struct device *hwmon_dev;
+	struct device *dev = &client->dev;
+	int ret, i;
+	u32 mask;
+
+	data = devm_kzalloc(dev, sizeof(struct nct7904_data), GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
+
+	data->client = client;
+	mutex_init(&data->bank_lock);
+	data->bank_sel = -1;
+
+	/* Setup sensor groups. */
+	/* FANIN attributes */
+	ret = nct7904_read_reg16(data, BANK_0, FANIN_CTRL0_REG);
+	if (ret < 0)
+		return ret;
+	data->fanin_mask = (ret >> 8) | ((ret & 0xff) << 8);
+
+	/*
+	 * VSEN attributes
+	 *
+	 * Note: voltage sensors overlap with external temperature
+	 * sensors. So, if we ever decide to support the latter
+	 * we will have to adjust 'vsen_mask' accordingly.
+	 */
+	mask = 0;
+	ret = nct7904_read_reg16(data, BANK_0, VT_ADC_CTRL0_REG);
+	if (ret >= 0)
+		mask = (ret >> 8) | ((ret & 0xff) << 8);
+	ret = nct7904_read_reg(data, BANK_0, VT_ADC_CTRL2_REG);
+	if (ret >= 0)
+		mask |= (ret << 16);
+	data->vsen_mask = mask;
+
+	/* CPU_TEMP attributes */
+	ret = nct7904_read_reg16(data, BANK_0, DTS_T_CTRL0_REG);
+	if (ret < 0)
+		return ret;
+	data->tcpu_mask = ((ret >> 8) & 0xf) | ((ret & 0xf) << 4);
+
+	for (i = 0; i < FANCTL_MAX; i++) {
+		ret = nct7904_read_reg(data, BANK_3, FANCTL1_FMR_REG + i);
+		if (ret < 0)
+			return ret;
+		data->fan_mode[i] = ret;
+	}
+
+	hwmon_dev =
+		devm_hwmon_device_register_with_groups(dev, client->name, data,
+						       nct7904_groups);
+	return PTR_ERR_OR_ZERO(hwmon_dev);
+}
+
+static const struct i2c_device_id nct7904_id[] = {
+	{"nct7904", 0},
+	{}
+};
+
+static struct i2c_driver nct7904_driver = {
+	.class = I2C_CLASS_HWMON,
+	.driver = {
+		.name = "nct7904",
+	},
+	.probe = nct7904_probe,
+	.id_table = nct7904_id,
+	.detect = nct7904_detect,
+	.address_list = normal_i2c,
+};
+
+module_i2c_driver(nct7904_driver);
+
+MODULE_AUTHOR("Vadim V. Vlasov <vvlasov@dev.rtsoft.ru>");
+MODULE_DESCRIPTION("Hwmon driver for NUVOTON NCT7904");
+MODULE_LICENSE("GPL");
diff --git a/drivers/hwmon/pwm-fan.c b/drivers/hwmon/pwm-fan.c
index 1991d9032c38..2d9a712699ff 100644
--- a/drivers/hwmon/pwm-fan.c
+++ b/drivers/hwmon/pwm-fan.c
@@ -24,55 +24,78 @@
 #include <linux/platform_device.h>
 #include <linux/pwm.h>
 #include <linux/sysfs.h>
+#include <linux/thermal.h>
 
 #define MAX_PWM 255
 
 struct pwm_fan_ctx {
 	struct mutex lock;
 	struct pwm_device *pwm;
-	unsigned char pwm_value;
+	unsigned int pwm_value;
+	unsigned int pwm_fan_state;
+	unsigned int pwm_fan_max_state;
+	unsigned int *pwm_fan_cooling_levels;
+	struct thermal_cooling_device *cdev;
 };
 
-static ssize_t set_pwm(struct device *dev, struct device_attribute *attr,
-		       const char *buf, size_t count)
+static int  __set_pwm(struct pwm_fan_ctx *ctx, unsigned long pwm)
 {
-	struct pwm_fan_ctx *ctx = dev_get_drvdata(dev);
-	unsigned long pwm, duty;
-	ssize_t ret;
-
-	if (kstrtoul(buf, 10, &pwm) || pwm > MAX_PWM)
-		return -EINVAL;
+	unsigned long duty;
+	int ret = 0;
 
 	mutex_lock(&ctx->lock);
-
 	if (ctx->pwm_value == pwm)
-		goto exit_set_pwm_no_change;
-
-	if (pwm == 0) {
-		pwm_disable(ctx->pwm);
-		goto exit_set_pwm;
-	}
+		goto exit_set_pwm_err;
 
 	duty = DIV_ROUND_UP(pwm * (ctx->pwm->period - 1), MAX_PWM);
 	ret = pwm_config(ctx->pwm, duty, ctx->pwm->period);
 	if (ret)
 		goto exit_set_pwm_err;
 
+	if (pwm == 0)
+		pwm_disable(ctx->pwm);
+
 	if (ctx->pwm_value == 0) {
 		ret = pwm_enable(ctx->pwm);
 		if (ret)
 			goto exit_set_pwm_err;
 	}
 
-exit_set_pwm:
 	ctx->pwm_value = pwm;
-exit_set_pwm_no_change:
-	ret = count;
 exit_set_pwm_err:
 	mutex_unlock(&ctx->lock);
 	return ret;
 }
 
+static void pwm_fan_update_state(struct pwm_fan_ctx *ctx, unsigned long pwm)
+{
+	int i;
+
+	for (i = 0; i < ctx->pwm_fan_max_state; ++i)
+		if (pwm < ctx->pwm_fan_cooling_levels[i + 1])
+			break;
+
+	ctx->pwm_fan_state = i;
+}
+
+static ssize_t set_pwm(struct device *dev, struct device_attribute *attr,
+		       const char *buf, size_t count)
+{
+	struct pwm_fan_ctx *ctx = dev_get_drvdata(dev);
+	unsigned long pwm;
+	int ret;
+
+	if (kstrtoul(buf, 10, &pwm) || pwm > MAX_PWM)
+		return -EINVAL;
+
+	ret = __set_pwm(ctx, pwm);
+	if (ret)
+		return ret;
+
+	pwm_fan_update_state(ctx, pwm);
+	return count;
+}
+
 static ssize_t show_pwm(struct device *dev,
 			struct device_attribute *attr, char *buf)
 {
@@ -91,10 +114,108 @@ static struct attribute *pwm_fan_attrs[] = {
 
 ATTRIBUTE_GROUPS(pwm_fan);
 
+/* thermal cooling device callbacks */
+static int pwm_fan_get_max_state(struct thermal_cooling_device *cdev,
+				 unsigned long *state)
+{
+	struct pwm_fan_ctx *ctx = cdev->devdata;
+
+	if (!ctx)
+		return -EINVAL;
+
+	*state = ctx->pwm_fan_max_state;
+
+	return 0;
+}
+
+static int pwm_fan_get_cur_state(struct thermal_cooling_device *cdev,
+				 unsigned long *state)
+{
+	struct pwm_fan_ctx *ctx = cdev->devdata;
+
+	if (!ctx)
+		return -EINVAL;
+
+	*state = ctx->pwm_fan_state;
+
+	return 0;
+}
+
+static int
+pwm_fan_set_cur_state(struct thermal_cooling_device *cdev, unsigned long state)
+{
+	struct pwm_fan_ctx *ctx = cdev->devdata;
+	int ret;
+
+	if (!ctx || (state > ctx->pwm_fan_max_state))
+		return -EINVAL;
+
+	if (state == ctx->pwm_fan_state)
+		return 0;
+
+	ret = __set_pwm(ctx, ctx->pwm_fan_cooling_levels[state]);
+	if (ret) {
+		dev_err(&cdev->device, "Cannot set pwm!\n");
+		return ret;
+	}
+
+	ctx->pwm_fan_state = state;
+
+	return ret;
+}
+
+static const struct thermal_cooling_device_ops pwm_fan_cooling_ops = {
+	.get_max_state = pwm_fan_get_max_state,
+	.get_cur_state = pwm_fan_get_cur_state,
+	.set_cur_state = pwm_fan_set_cur_state,
+};
+
+static int pwm_fan_of_get_cooling_data(struct device *dev,
+				       struct pwm_fan_ctx *ctx)
+{
+	struct device_node *np = dev->of_node;
+	int num, i, ret;
+
+	if (!of_find_property(np, "cooling-levels", NULL))
+		return 0;
+
+	ret = of_property_count_u32_elems(np, "cooling-levels");
+	if (ret <= 0) {
+		dev_err(dev, "Wrong data!\n");
+		return ret ? : -EINVAL;
+	}
+
+	num = ret;
+	ctx->pwm_fan_cooling_levels = devm_kzalloc(dev, num * sizeof(u32),
+						   GFP_KERNEL);
+	if (!ctx->pwm_fan_cooling_levels)
+		return -ENOMEM;
+
+	ret = of_property_read_u32_array(np, "cooling-levels",
+					 ctx->pwm_fan_cooling_levels, num);
+	if (ret) {
+		dev_err(dev, "Property 'cooling-levels' cannot be read!\n");
+		return ret;
+	}
+
+	for (i = 0; i < num; i++) {
+		if (ctx->pwm_fan_cooling_levels[i] > MAX_PWM) {
+			dev_err(dev, "PWM fan state[%d]:%d > %d\n", i,
+				ctx->pwm_fan_cooling_levels[i], MAX_PWM);
+			return -EINVAL;
+		}
+	}
+
+	ctx->pwm_fan_max_state = num - 1;
+
+	return 0;
+}
+
 static int pwm_fan_probe(struct platform_device *pdev)
 {
-	struct device *hwmon;
+	struct thermal_cooling_device *cdev;
 	struct pwm_fan_ctx *ctx;
+	struct device *hwmon;
 	int duty_cycle;
 	int ret;
 
@@ -136,6 +257,26 @@ static int pwm_fan_probe(struct platform_device *pdev)
 		pwm_disable(ctx->pwm);
 		return PTR_ERR(hwmon);
 	}
+
+	ret = pwm_fan_of_get_cooling_data(&pdev->dev, ctx);
+	if (ret)
+		return ret;
+
+	ctx->pwm_fan_state = ctx->pwm_fan_max_state;
+	if (IS_ENABLED(CONFIG_THERMAL)) {
+		cdev = thermal_of_cooling_device_register(pdev->dev.of_node,
+							  "pwm-fan", ctx,
+							  &pwm_fan_cooling_ops);
+		if (IS_ERR(cdev)) {
+			dev_err(&pdev->dev,
+				"Failed to register pwm-fan as cooling device");
+			pwm_disable(ctx->pwm);
+			return PTR_ERR(cdev);
+		}
+		ctx->cdev = cdev;
+		thermal_cdev_update(cdev);
+	}
+
 	return 0;
 }
 
@@ -143,6 +284,7 @@ static int pwm_fan_remove(struct platform_device *pdev)
 {
 	struct pwm_fan_ctx *ctx = platform_get_drvdata(pdev);
 
+	thermal_cooling_device_unregister(ctx->cdev);
 	if (ctx->pwm_value)
 		pwm_disable(ctx->pwm);
 	return 0;
@@ -177,7 +319,7 @@ static int pwm_fan_resume(struct device *dev)
 
 static SIMPLE_DEV_PM_OPS(pwm_fan_pm, pwm_fan_suspend, pwm_fan_resume);
 
-static struct of_device_id of_pwm_fan_match[] = {
+static const struct of_device_id of_pwm_fan_match[] = {
 	{ .compatible = "pwm-fan", },
 	{},
 };
diff --git a/drivers/hwmon/vexpress.c b/drivers/hwmon/vexpress.c
index cf1848b8fb32..8ba419d343f8 100644
--- a/drivers/hwmon/vexpress.c
+++ b/drivers/hwmon/vexpress.c
@@ -193,7 +193,7 @@ static struct vexpress_hwmon_type vexpress_hwmon_energy = {
 	},
 };
 
-static struct of_device_id vexpress_hwmon_of_match[] = {
+static const struct of_device_id vexpress_hwmon_of_match[] = {
 #if !defined(CONFIG_REGULATOR_VEXPRESS)
 	{
 		.compatible = "arm,vexpress-volt",
diff --git a/drivers/i2c/busses/i2c-designware-baytrail.c b/drivers/i2c/busses/i2c-designware-baytrail.c
index 5f1ff4cc5c34..7d7ae97476e2 100644
--- a/drivers/i2c/busses/i2c-designware-baytrail.c
+++ b/drivers/i2c/busses/i2c-designware-baytrail.c
@@ -17,27 +17,31 @@
 #include <linux/acpi.h>
 #include <linux/i2c.h>
 #include <linux/interrupt.h>
+
 #include <asm/iosf_mbi.h>
+
 #include "i2c-designware-core.h"
 
 #define SEMAPHORE_TIMEOUT	100
 #define PUNIT_SEMAPHORE		0x7
+#define PUNIT_SEMAPHORE_BIT	BIT(0)
+#define PUNIT_SEMAPHORE_ACQUIRE	BIT(1)
 
 static unsigned long acquired;
 
 static int get_sem(struct device *dev, u32 *sem)
 {
-	u32 reg_val;
+	u32 data;
 	int ret;
 
 	ret = iosf_mbi_read(BT_MBI_UNIT_PMC, BT_MBI_BUNIT_READ, PUNIT_SEMAPHORE,
-			    &reg_val);
+				&data);
 	if (ret) {
 		dev_err(dev, "iosf failed to read punit semaphore\n");
 		return ret;
 	}
 
-	*sem = reg_val & 0x1;
+	*sem = data & PUNIT_SEMAPHORE_BIT;
 
 	return 0;
 }
@@ -52,27 +56,29 @@ static void reset_semaphore(struct device *dev)
 		return;
 	}
 
-	data = data & 0xfffffffe;
+	data &= ~PUNIT_SEMAPHORE_BIT;
 	if (iosf_mbi_write(BT_MBI_UNIT_PMC, BT_MBI_BUNIT_WRITE,
-				 PUNIT_SEMAPHORE, data))
+				PUNIT_SEMAPHORE, data))
 		dev_err(dev, "iosf failed to reset punit semaphore during write\n");
 }
 
-int baytrail_i2c_acquire(struct dw_i2c_dev *dev)
+static int baytrail_i2c_acquire(struct dw_i2c_dev *dev)
 {
-	u32 sem = 0;
+	u32 sem;
 	int ret;
 	unsigned long start, end;
 
+	might_sleep();
+
 	if (!dev || !dev->dev)
 		return -ENODEV;
 
-	if (!dev->acquire_lock)
+	if (!dev->release_lock)
 		return 0;
 
-	/* host driver writes 0x2 to side band semaphore register */
+	/* host driver writes to side band semaphore register */
 	ret = iosf_mbi_write(BT_MBI_UNIT_PMC, BT_MBI_BUNIT_WRITE,
-				 PUNIT_SEMAPHORE, 0x2);
+				PUNIT_SEMAPHORE, PUNIT_SEMAPHORE_ACQUIRE);
 	if (ret) {
 		dev_err(dev->dev, "iosf punit semaphore request failed\n");
 		return ret;
@@ -81,7 +87,7 @@ int baytrail_i2c_acquire(struct dw_i2c_dev *dev)
 	/* host driver waits for bit 0 to be set in semaphore register */
 	start = jiffies;
 	end = start + msecs_to_jiffies(SEMAPHORE_TIMEOUT);
-	while (!time_after(jiffies, end)) {
+	do {
 		ret = get_sem(dev->dev, &sem);
 		if (!ret && sem) {
 			acquired = jiffies;
@@ -91,14 +97,14 @@ int baytrail_i2c_acquire(struct dw_i2c_dev *dev)
 		}
 
 		usleep_range(1000, 2000);
-	}
+	} while (time_before(jiffies, end));
 
 	dev_err(dev->dev, "punit semaphore timed out, resetting\n");
 	reset_semaphore(dev->dev);
 
 	ret = iosf_mbi_read(BT_MBI_UNIT_PMC, BT_MBI_BUNIT_READ,
-		PUNIT_SEMAPHORE, &sem);
-	if (!ret)
+				PUNIT_SEMAPHORE, &sem);
+	if (ret)
 		dev_err(dev->dev, "iosf failed to read punit semaphore\n");
 	else
 		dev_err(dev->dev, "PUNIT SEM: %d\n", sem);
@@ -107,9 +113,8 @@ int baytrail_i2c_acquire(struct dw_i2c_dev *dev)
 
 	return -ETIMEDOUT;
 }
-EXPORT_SYMBOL(baytrail_i2c_acquire);
 
-void baytrail_i2c_release(struct dw_i2c_dev *dev)
+static void baytrail_i2c_release(struct dw_i2c_dev *dev)
 {
 	if (!dev || !dev->dev)
 		return;
@@ -121,7 +126,6 @@ void baytrail_i2c_release(struct dw_i2c_dev *dev)
 	dev_dbg(dev->dev, "punit semaphore held for %ums\n",
 		jiffies_to_msecs(jiffies - acquired));
 }
-EXPORT_SYMBOL(baytrail_i2c_release);
 
 int i2c_dw_eval_lock_support(struct dw_i2c_dev *dev)
 {
@@ -137,7 +141,6 @@ int i2c_dw_eval_lock_support(struct dw_i2c_dev *dev)
 		return 0;
 
 	status = acpi_evaluate_integer(handle, "_SEM", NULL, &shared_host);
-
 	if (ACPI_FAILURE(status))
 		return 0;
 
@@ -153,7 +156,6 @@ int i2c_dw_eval_lock_support(struct dw_i2c_dev *dev)
 
 	return 0;
 }
-EXPORT_SYMBOL(i2c_dw_eval_lock_support);
 
 MODULE_AUTHOR("David E. Box <david.e.box@linux.intel.com>");
 MODULE_DESCRIPTION("Baytrail I2C Semaphore driver");
diff --git a/drivers/i2c/i2c-core.c b/drivers/i2c/i2c-core.c
index 210cf4874cb7..edf274cabe81 100644
--- a/drivers/i2c/i2c-core.c
+++ b/drivers/i2c/i2c-core.c
@@ -679,9 +679,6 @@ static int i2c_device_remove(struct device *dev)
 		status = driver->remove(client);
 	}
 
-	if (dev->of_node)
-		irq_dispose_mapping(client->irq);
-
 	dev_pm_domain_detach(&client->dev, true);
 	return status;
 }
diff --git a/drivers/ide/ide-lib.c b/drivers/ide/ide-lib.c
index d9c9829c8b20..e1180fa46196 100644
--- a/drivers/ide/ide-lib.c
+++ b/drivers/ide/ide-lib.c
@@ -148,8 +148,8 @@ u8 ide_dump_status(ide_drive_t *drive, const char *msg, u8 stat)
 			printk(KERN_CONT "DataRequest ");
 		if (stat & ATA_CORR)
 			printk(KERN_CONT "CorrectedError ");
-		if (stat & ATA_IDX)
-			printk(KERN_CONT "Index ");
+		if (stat & ATA_SENSE)
+			printk(KERN_CONT "Sense ");
 		if (stat & ATA_ERR)
 			printk(KERN_CONT "Error ");
 	}
diff --git a/drivers/ide/ide-probe.c b/drivers/ide/ide-probe.c
index a3d3b1733c49..0b63facd1d87 100644
--- a/drivers/ide/ide-probe.c
+++ b/drivers/ide/ide-probe.c
@@ -273,7 +273,7 @@ int ide_dev_read_id(ide_drive_t *drive, u8 cmd, u16 *id, int irq_ctx)
 	    (hwif->host_flags & IDE_HFLAG_BROKEN_ALTSTATUS) == 0) {
 		a = tp_ops->read_altstatus(hwif);
 		s = tp_ops->read_status(hwif);
-		if ((a ^ s) & ~ATA_IDX)
+		if ((a ^ s) & ~ATA_SENSE)
 			/* ancient Seagate drives, broken interfaces */
 			printk(KERN_INFO "%s: probing with STATUS(0x%02x) "
 					 "instead of ALTSTATUS(0x%02x)\n",
diff --git a/drivers/ide/ide-tape.c b/drivers/ide/ide-tape.c
index 1793aea4a7d2..6eb738ca6d2f 100644
--- a/drivers/ide/ide-tape.c
+++ b/drivers/ide/ide-tape.c
@@ -1793,11 +1793,11 @@ static void idetape_setup(ide_drive_t *drive, idetape_tape_t *tape, int minor)
 	tape->best_dsc_rw_freq = clamp_t(unsigned long, t, IDETAPE_DSC_RW_MIN,
 					 IDETAPE_DSC_RW_MAX);
 	printk(KERN_INFO "ide-tape: %s <-> %s: %dKBps, %d*%dkB buffer, "
-		"%lums tDSC%s\n",
+		"%ums tDSC%s\n",
 		drive->name, tape->name, *(u16 *)&tape->caps[14],
 		(*(u16 *)&tape->caps[16] * 512) / tape->buffer_size,
 		tape->buffer_size / 1024,
-		tape->best_dsc_rw_freq * 1000 / HZ,
+		jiffies_to_msecs(tape->best_dsc_rw_freq),
 		(drive->dev_flags & IDE_DFLAG_USING_DMA) ? ", DMA" : "");
 
 	ide_proc_register_driver(drive, tape->driver);
diff --git a/drivers/idle/intel_idle.c b/drivers/idle/intel_idle.c
index b0e58522780d..5c979d0667a2 100644
--- a/drivers/idle/intel_idle.c
+++ b/drivers/idle/intel_idle.c
@@ -55,7 +55,7 @@
 
 #include <linux/kernel.h>
 #include <linux/cpuidle.h>
-#include <linux/clockchips.h>
+#include <linux/tick.h>
 #include <trace/events/power.h>
 #include <linux/sched.h>
 #include <linux/notifier.h>
@@ -638,12 +638,12 @@ static int intel_idle(struct cpuidle_device *dev,
 		leave_mm(cpu);
 
 	if (!(lapic_timer_reliable_states & (1 << (cstate))))
-		clockevents_notify(CLOCK_EVT_NOTIFY_BROADCAST_ENTER, &cpu);
+		tick_broadcast_enter();
 
 	mwait_idle_with_hints(eax, ecx);
 
 	if (!(lapic_timer_reliable_states & (1 << (cstate))))
-		clockevents_notify(CLOCK_EVT_NOTIFY_BROADCAST_EXIT, &cpu);
+		tick_broadcast_exit();
 
 	return index;
 }
@@ -665,13 +665,12 @@ static void intel_idle_freeze(struct cpuidle_device *dev,
 
 static void __setup_broadcast_timer(void *arg)
 {
-	unsigned long reason = (unsigned long)arg;
-	int cpu = smp_processor_id();
-
-	reason = reason ?
-		CLOCK_EVT_NOTIFY_BROADCAST_ON : CLOCK_EVT_NOTIFY_BROADCAST_OFF;
+	unsigned long on = (unsigned long)arg;
 
-	clockevents_notify(reason, &cpu);
+	if (on)
+		tick_broadcast_enable();
+	else
+		tick_broadcast_disable();
 }
 
 static int cpu_hotplug_notify(struct notifier_block *n,
diff --git a/drivers/iio/accel/bma180.c b/drivers/iio/accel/bma180.c
index 1096da327130..75c6d2103e07 100644
--- a/drivers/iio/accel/bma180.c
+++ b/drivers/iio/accel/bma180.c
@@ -659,7 +659,7 @@ static irqreturn_t bma180_trigger_handler(int irq, void *p)
 
 	mutex_lock(&data->mutex);
 
-	for_each_set_bit(bit, indio_dev->buffer->scan_mask,
+	for_each_set_bit(bit, indio_dev->active_scan_mask,
 			 indio_dev->masklength) {
 		ret = bma180_get_data_reg(data, bit);
 		if (ret < 0) {
diff --git a/drivers/iio/accel/bmc150-accel.c b/drivers/iio/accel/bmc150-accel.c
index 066d0c04072c..73e87739d219 100644
--- a/drivers/iio/accel/bmc150-accel.c
+++ b/drivers/iio/accel/bmc150-accel.c
@@ -70,7 +70,9 @@
 #define BMC150_ACCEL_INT_MAP_0_BIT_SLOPE	BIT(2)
 
 #define BMC150_ACCEL_REG_INT_MAP_1		0x1A
-#define BMC150_ACCEL_INT_MAP_1_BIT_DATA	BIT(0)
+#define BMC150_ACCEL_INT_MAP_1_BIT_DATA		BIT(0)
+#define BMC150_ACCEL_INT_MAP_1_BIT_FWM		BIT(1)
+#define BMC150_ACCEL_INT_MAP_1_BIT_FFULL	BIT(2)
 
 #define BMC150_ACCEL_REG_INT_RST_LATCH		0x21
 #define BMC150_ACCEL_INT_MODE_LATCH_RESET	0x80
@@ -83,7 +85,9 @@
 #define BMC150_ACCEL_INT_EN_BIT_SLP_Z		BIT(2)
 
 #define BMC150_ACCEL_REG_INT_EN_1		0x17
-#define BMC150_ACCEL_INT_EN_BIT_DATA_EN	BIT(4)
+#define BMC150_ACCEL_INT_EN_BIT_DATA_EN		BIT(4)
+#define BMC150_ACCEL_INT_EN_BIT_FFULL_EN	BIT(5)
+#define BMC150_ACCEL_INT_EN_BIT_FWM_EN		BIT(6)
 
 #define BMC150_ACCEL_REG_INT_OUT_CTRL		0x20
 #define BMC150_ACCEL_INT_OUT_CTRL_INT1_LVL	BIT(0)
@@ -122,6 +126,12 @@
 #define BMC150_ACCEL_AXIS_TO_REG(axis)	(BMC150_ACCEL_REG_XOUT_L + (axis * 2))
 #define BMC150_AUTO_SUSPEND_DELAY_MS		2000
 
+#define BMC150_ACCEL_REG_FIFO_STATUS		0x0E
+#define BMC150_ACCEL_REG_FIFO_CONFIG0		0x30
+#define BMC150_ACCEL_REG_FIFO_CONFIG1		0x3E
+#define BMC150_ACCEL_REG_FIFO_DATA		0x3F
+#define BMC150_ACCEL_FIFO_LENGTH		32
+
 enum bmc150_accel_axis {
 	AXIS_X,
 	AXIS_Y,
@@ -147,20 +157,46 @@ struct bmc150_accel_chip_info {
 	const struct bmc150_scale_info scale_table[4];
 };
 
+struct bmc150_accel_interrupt {
+	const struct bmc150_accel_interrupt_info *info;
+	atomic_t users;
+};
+
+struct bmc150_accel_trigger {
+	struct bmc150_accel_data *data;
+	struct iio_trigger *indio_trig;
+	int (*setup)(struct bmc150_accel_trigger *t, bool state);
+	int intr;
+	bool enabled;
+};
+
+enum bmc150_accel_interrupt_id {
+	BMC150_ACCEL_INT_DATA_READY,
+	BMC150_ACCEL_INT_ANY_MOTION,
+	BMC150_ACCEL_INT_WATERMARK,
+	BMC150_ACCEL_INTERRUPTS,
+};
+
+enum bmc150_accel_trigger_id {
+	BMC150_ACCEL_TRIGGER_DATA_READY,
+	BMC150_ACCEL_TRIGGER_ANY_MOTION,
+	BMC150_ACCEL_TRIGGERS,
+};
+
 struct bmc150_accel_data {
 	struct i2c_client *client;
-	struct iio_trigger *dready_trig;
-	struct iio_trigger *motion_trig;
+	struct bmc150_accel_interrupt interrupts[BMC150_ACCEL_INTERRUPTS];
+	atomic_t active_intr;
+	struct bmc150_accel_trigger triggers[BMC150_ACCEL_TRIGGERS];
 	struct mutex mutex;
+	u8 fifo_mode, watermark;
 	s16 buffer[8];
 	u8 bw_bits;
 	u32 slope_dur;
 	u32 slope_thres;
 	u32 range;
 	int ev_enable_state;
-	bool dready_trigger_on;
-	bool motion_trigger_on;
-	int64_t timestamp;
+	int64_t timestamp, old_timestamp;
 	const struct bmc150_accel_chip_info *chip_info;
 };
 
@@ -168,14 +204,14 @@ static const struct {
 	int val;
 	int val2;
 	u8 bw_bits;
-} bmc150_accel_samp_freq_table[] = { {7, 810000, 0x08},
-				     {15, 630000, 0x09},
-				     {31, 250000, 0x0A},
-				     {62, 500000, 0x0B},
-				     {125, 0, 0x0C},
-				     {250, 0, 0x0D},
-				     {500, 0, 0x0E},
-				     {1000, 0, 0x0F} };
+} bmc150_accel_samp_freq_table[] = { {15, 620000, 0x08},
+				     {31, 260000, 0x09},
+				     {62, 500000, 0x0A},
+				     {125, 0, 0x0B},
+				     {250, 0, 0x0C},
+				     {500, 0, 0x0D},
+				     {1000, 0, 0x0E},
+				     {2000, 0, 0x0F} };
 
 static const struct {
 	int bw_bits;
@@ -269,6 +305,46 @@ static int bmc150_accel_set_bw(struct bmc150_accel_data *data, int val,
 	return -EINVAL;
 }
 
+static int bmc150_accel_update_slope(struct bmc150_accel_data *data)
+{
+	int ret, val;
+
+	ret = i2c_smbus_write_byte_data(data->client, BMC150_ACCEL_REG_INT_6,
+					data->slope_thres);
+	if (ret < 0) {
+		dev_err(&data->client->dev, "Error writing reg_int_6\n");
+		return ret;
+	}
+
+	ret = i2c_smbus_read_byte_data(data->client, BMC150_ACCEL_REG_INT_5);
+	if (ret < 0) {
+		dev_err(&data->client->dev, "Error reading reg_int_5\n");
+		return ret;
+	}
+
+	val = (ret & ~BMC150_ACCEL_SLOPE_DUR_MASK) | data->slope_dur;
+	ret = i2c_smbus_write_byte_data(data->client, BMC150_ACCEL_REG_INT_5,
+					val);
+	if (ret < 0) {
+		dev_err(&data->client->dev, "Error write reg_int_5\n");
+		return ret;
+	}
+
+	dev_dbg(&data->client->dev, "%s: %x %x\n", __func__, data->slope_thres,
+		data->slope_dur);
+
+	return ret;
+}
+
+static int bmc150_accel_any_motion_setup(struct bmc150_accel_trigger *t,
+					 bool state)
+{
+	if (state)
+		return bmc150_accel_update_slope(t->data);
+
+	return 0;
+}
+
 static int bmc150_accel_chip_init(struct bmc150_accel_data *data)
 {
 	int ret;
@@ -307,32 +383,12 @@ static int bmc150_accel_chip_init(struct bmc150_accel_data *data)
 
 	data->range = BMC150_ACCEL_DEF_RANGE_4G;
 
-	/* Set default slope duration */
-	ret = i2c_smbus_read_byte_data(data->client, BMC150_ACCEL_REG_INT_5);
-	if (ret < 0) {
-		dev_err(&data->client->dev, "Error reading reg_int_5\n");
-		return ret;
-	}
-	data->slope_dur |= BMC150_ACCEL_DEF_SLOPE_DURATION;
-	ret = i2c_smbus_write_byte_data(data->client,
-					BMC150_ACCEL_REG_INT_5,
-					data->slope_dur);
-	if (ret < 0) {
-		dev_err(&data->client->dev, "Error writing reg_int_5\n");
-		return ret;
-	}
-	dev_dbg(&data->client->dev, "slope_dur %x\n", data->slope_dur);
-
-	/* Set default slope thresholds */
-	ret = i2c_smbus_write_byte_data(data->client,
-					BMC150_ACCEL_REG_INT_6,
-					BMC150_ACCEL_DEF_SLOPE_THRESHOLD);
-	if (ret < 0) {
-		dev_err(&data->client->dev, "Error writing reg_int_6\n");
-		return ret;
-	}
+	/* Set default slope duration and thresholds */
 	data->slope_thres = BMC150_ACCEL_DEF_SLOPE_THRESHOLD;
-	dev_dbg(&data->client->dev, "slope_thres %x\n", data->slope_thres);
+	data->slope_dur = BMC150_ACCEL_DEF_SLOPE_DURATION;
+	ret = bmc150_accel_update_slope(data);
+	if (ret < 0)
+		return ret;
 
 	/* Set default as latched interrupts */
 	ret = i2c_smbus_write_byte_data(data->client,
@@ -348,155 +404,6 @@ static int bmc150_accel_chip_init(struct bmc150_accel_data *data)
 	return 0;
 }
 
-static int bmc150_accel_setup_any_motion_interrupt(
-					struct bmc150_accel_data *data,
-					bool status)
-{
-	int ret;
-
-	/* Enable/Disable INT1 mapping */
-	ret = i2c_smbus_read_byte_data(data->client,
-				       BMC150_ACCEL_REG_INT_MAP_0);
-	if (ret < 0) {
-		dev_err(&data->client->dev, "Error reading reg_int_map_0\n");
-		return ret;
-	}
-	if (status)
-		ret |= BMC150_ACCEL_INT_MAP_0_BIT_SLOPE;
-	else
-		ret &= ~BMC150_ACCEL_INT_MAP_0_BIT_SLOPE;
-
-	ret = i2c_smbus_write_byte_data(data->client,
-					BMC150_ACCEL_REG_INT_MAP_0,
-					ret);
-	if (ret < 0) {
-		dev_err(&data->client->dev, "Error writing reg_int_map_0\n");
-		return ret;
-	}
-
-	if (status) {
-		/* Set slope duration (no of samples) */
-		ret = i2c_smbus_write_byte_data(data->client,
-						BMC150_ACCEL_REG_INT_5,
-						data->slope_dur);
-		if (ret < 0) {
-			dev_err(&data->client->dev, "Error write reg_int_5\n");
-			return ret;
-		}
-
-		/* Set slope thresholds */
-		ret = i2c_smbus_write_byte_data(data->client,
-						BMC150_ACCEL_REG_INT_6,
-						data->slope_thres);
-		if (ret < 0) {
-			dev_err(&data->client->dev, "Error write reg_int_6\n");
-			return ret;
-		}
-
-		/*
-		 * New data interrupt is always non-latched,
-		 * which will have higher priority, so no need
-		 * to set latched mode, we will be flooded anyway with INTR
-		 */
-		if (!data->dready_trigger_on) {
-			ret = i2c_smbus_write_byte_data(data->client,
-					BMC150_ACCEL_REG_INT_RST_LATCH,
-					BMC150_ACCEL_INT_MODE_LATCH_INT |
-					BMC150_ACCEL_INT_MODE_LATCH_RESET);
-			if (ret < 0) {
-				dev_err(&data->client->dev,
-					"Error writing reg_int_rst_latch\n");
-				return ret;
-			}
-		}
-
-		ret = i2c_smbus_write_byte_data(data->client,
-						BMC150_ACCEL_REG_INT_EN_0,
-						BMC150_ACCEL_INT_EN_BIT_SLP_X |
-						BMC150_ACCEL_INT_EN_BIT_SLP_Y |
-						BMC150_ACCEL_INT_EN_BIT_SLP_Z);
-	} else
-		ret = i2c_smbus_write_byte_data(data->client,
-						BMC150_ACCEL_REG_INT_EN_0,
-						0);
-
-	if (ret < 0) {
-		dev_err(&data->client->dev, "Error writing reg_int_en_0\n");
-		return ret;
-	}
-
-	return 0;
-}
-
-static int bmc150_accel_setup_new_data_interrupt(struct bmc150_accel_data *data,
-					   bool status)
-{
-	int ret;
-
-	/* Enable/Disable INT1 mapping */
-	ret = i2c_smbus_read_byte_data(data->client,
-				       BMC150_ACCEL_REG_INT_MAP_1);
-	if (ret < 0) {
-		dev_err(&data->client->dev, "Error reading reg_int_map_1\n");
-		return ret;
-	}
-	if (status)
-		ret |= BMC150_ACCEL_INT_MAP_1_BIT_DATA;
-	else
-		ret &= ~BMC150_ACCEL_INT_MAP_1_BIT_DATA;
-
-	ret = i2c_smbus_write_byte_data(data->client,
-					BMC150_ACCEL_REG_INT_MAP_1,
-					ret);
-	if (ret < 0) {
-		dev_err(&data->client->dev, "Error writing reg_int_map_1\n");
-		return ret;
-	}
-
-	if (status) {
-		/*
-		 * Set non latched mode interrupt and clear any latched
-		 * interrupt
-		 */
-		ret = i2c_smbus_write_byte_data(data->client,
-					BMC150_ACCEL_REG_INT_RST_LATCH,
-					BMC150_ACCEL_INT_MODE_NON_LATCH_INT |
-					BMC150_ACCEL_INT_MODE_LATCH_RESET);
-		if (ret < 0) {
-			dev_err(&data->client->dev,
-				"Error writing reg_int_rst_latch\n");
-			return ret;
-		}
-
-		ret = i2c_smbus_write_byte_data(data->client,
-					BMC150_ACCEL_REG_INT_EN_1,
-					BMC150_ACCEL_INT_EN_BIT_DATA_EN);
-
-	} else {
-		/* Restore default interrupt mode */
-		ret = i2c_smbus_write_byte_data(data->client,
-					BMC150_ACCEL_REG_INT_RST_LATCH,
-					BMC150_ACCEL_INT_MODE_LATCH_INT |
-					BMC150_ACCEL_INT_MODE_LATCH_RESET);
-		if (ret < 0) {
-			dev_err(&data->client->dev,
-				"Error writing reg_int_rst_latch\n");
-			return ret;
-		}
-
-		ret = i2c_smbus_write_byte_data(data->client,
-						BMC150_ACCEL_REG_INT_EN_1,
-						0);
-	}
-
-	if (ret < 0) {
-		dev_err(&data->client->dev, "Error writing reg_int_en_1\n");
-		return ret;
-	}
-
-	return 0;
-}
-
 static int bmc150_accel_get_bw(struct bmc150_accel_data *data, int *val,
 			       int *val2)
 {
@@ -554,6 +461,120 @@ static int bmc150_accel_set_power_state(struct bmc150_accel_data *data, bool on)
 }
 #endif
 
+static const struct bmc150_accel_interrupt_info {
+	u8 map_reg;
+	u8 map_bitmask;
+	u8 en_reg;
+	u8 en_bitmask;
+} bmc150_accel_interrupts[BMC150_ACCEL_INTERRUPTS] = {
+	{ /* data ready interrupt */
+		.map_reg = BMC150_ACCEL_REG_INT_MAP_1,
+		.map_bitmask = BMC150_ACCEL_INT_MAP_1_BIT_DATA,
+		.en_reg = BMC150_ACCEL_REG_INT_EN_1,
+		.en_bitmask = BMC150_ACCEL_INT_EN_BIT_DATA_EN,
+	},
+	{  /* motion interrupt */
+		.map_reg = BMC150_ACCEL_REG_INT_MAP_0,
+		.map_bitmask = BMC150_ACCEL_INT_MAP_0_BIT_SLOPE,
+		.en_reg = BMC150_ACCEL_REG_INT_EN_0,
+		.en_bitmask =  BMC150_ACCEL_INT_EN_BIT_SLP_X |
+			BMC150_ACCEL_INT_EN_BIT_SLP_Y |
+			BMC150_ACCEL_INT_EN_BIT_SLP_Z
+	},
+	{ /* fifo watermark interrupt */
+		.map_reg = BMC150_ACCEL_REG_INT_MAP_1,
+		.map_bitmask = BMC150_ACCEL_INT_MAP_1_BIT_FWM,
+		.en_reg = BMC150_ACCEL_REG_INT_EN_1,
+		.en_bitmask = BMC150_ACCEL_INT_EN_BIT_FWM_EN,
+	},
+};
+
+static void bmc150_accel_interrupts_setup(struct iio_dev *indio_dev,
+					  struct bmc150_accel_data *data)
+{
+	int i;
+
+	for (i = 0; i < BMC150_ACCEL_INTERRUPTS; i++)
+		data->interrupts[i].info = &bmc150_accel_interrupts[i];
+}
+
+static int bmc150_accel_set_interrupt(struct bmc150_accel_data *data, int i,
+				      bool state)
+{
+	struct bmc150_accel_interrupt *intr = &data->interrupts[i];
+	const struct bmc150_accel_interrupt_info *info = intr->info;
+	int ret;
+
+	if (state) {
+		if (atomic_inc_return(&intr->users) > 1)
+			return 0;
+	} else {
+		if (atomic_dec_return(&intr->users) > 0)
+			return 0;
+	}
+
+	/*
+	 * We will expect the enable and disable to do operation in
+	 * in reverse order. This will happen here anyway as our
+	 * resume operation uses sync mode runtime pm calls, the
+	 * suspend operation will be delayed by autosuspend delay
+	 * So the disable operation will still happen in reverse of
+	 * enable operation. When runtime pm is disabled the mode
+	 * is always on so sequence doesn't matter
+	 */
+	ret = bmc150_accel_set_power_state(data, state);
+	if (ret < 0)
+		return ret;
+
+	/* map the interrupt to the appropriate pins */
+	ret = i2c_smbus_read_byte_data(data->client, info->map_reg);
+	if (ret < 0) {
+		dev_err(&data->client->dev, "Error reading reg_int_map\n");
+		goto out_fix_power_state;
+	}
+	if (state)
+		ret |= info->map_bitmask;
+	else
+		ret &= ~info->map_bitmask;
+
+	ret = i2c_smbus_write_byte_data(data->client, info->map_reg,
+					ret);
+	if (ret < 0) {
+		dev_err(&data->client->dev, "Error writing reg_int_map\n");
+		goto out_fix_power_state;
+	}
+
+	/* enable/disable the interrupt */
+	ret = i2c_smbus_read_byte_data(data->client, info->en_reg);
+	if (ret < 0) {
+		dev_err(&data->client->dev, "Error reading reg_int_en\n");
+		goto out_fix_power_state;
+	}
+
+	if (state)
+		ret |= info->en_bitmask;
+	else
+		ret &= ~info->en_bitmask;
+
+	ret = i2c_smbus_write_byte_data(data->client, info->en_reg, ret);
+	if (ret < 0) {
+		dev_err(&data->client->dev, "Error writing reg_int_en\n");
+		goto out_fix_power_state;
+	}
+
+	if (state)
+		atomic_inc(&data->active_intr);
+	else
+		atomic_dec(&data->active_intr);
+
+	return 0;
+
+out_fix_power_state:
+	bmc150_accel_set_power_state(data, false);
+	return ret;
+}
+
+
 static int bmc150_accel_set_scale(struct bmc150_accel_data *data, int val)
 {
 	int ret, i;
@@ -732,7 +753,7 @@ static int bmc150_accel_read_event(struct iio_dev *indio_dev,
 		*val = data->slope_thres;
 		break;
 	case IIO_EV_INFO_PERIOD:
-		*val = data->slope_dur & BMC150_ACCEL_SLOPE_DUR_MASK;
+		*val = data->slope_dur;
 		break;
 	default:
 		return -EINVAL;
@@ -755,11 +776,10 @@ static int bmc150_accel_write_event(struct iio_dev *indio_dev,
 
 	switch (info) {
 	case IIO_EV_INFO_VALUE:
-		data->slope_thres = val;
+		data->slope_thres = val & 0xFF;
 		break;
 	case IIO_EV_INFO_PERIOD:
-		data->slope_dur &= ~BMC150_ACCEL_SLOPE_DUR_MASK;
-		data->slope_dur |= val & BMC150_ACCEL_SLOPE_DUR_MASK;
+		data->slope_dur = val & BMC150_ACCEL_SLOPE_DUR_MASK;
 		break;
 	default:
 		return -EINVAL;
@@ -788,36 +808,14 @@ static int bmc150_accel_write_event_config(struct iio_dev *indio_dev,
 	struct bmc150_accel_data *data = iio_priv(indio_dev);
 	int ret;
 
-	if (state && data->ev_enable_state)
+	if (state == data->ev_enable_state)
 		return 0;
 
 	mutex_lock(&data->mutex);
 
-	if (!state && data->motion_trigger_on) {
-		data->ev_enable_state = 0;
-		mutex_unlock(&data->mutex);
-		return 0;
-	}
-
-	/*
-	 * We will expect the enable and disable to do operation in
-	 * in reverse order. This will happen here anyway as our
-	 * resume operation uses sync mode runtime pm calls, the
-	 * suspend operation will be delayed by autosuspend delay
-	 * So the disable operation will still happen in reverse of
-	 * enable operation. When runtime pm is disabled the mode
-	 * is always on so sequence doesn't matter
-	 */
-
-	ret = bmc150_accel_set_power_state(data, state);
-	if (ret < 0) {
-		mutex_unlock(&data->mutex);
-		return ret;
-	}
-
-	ret =  bmc150_accel_setup_any_motion_interrupt(data, state);
+	ret = bmc150_accel_set_interrupt(data, BMC150_ACCEL_INT_ANY_MOTION,
+					 state);
 	if (ret < 0) {
-		bmc150_accel_set_power_state(data, false);
 		mutex_unlock(&data->mutex);
 		return ret;
 	}
@@ -832,15 +830,226 @@ static int bmc150_accel_validate_trigger(struct iio_dev *indio_dev,
 				   struct iio_trigger *trig)
 {
 	struct bmc150_accel_data *data = iio_priv(indio_dev);
+	int i;
 
-	if (data->dready_trig != trig && data->motion_trig != trig)
-		return -EINVAL;
+	for (i = 0; i < BMC150_ACCEL_TRIGGERS; i++) {
+		if (data->triggers[i].indio_trig == trig)
+			return 0;
+	}
+
+	return -EINVAL;
+}
+
+static ssize_t bmc150_accel_get_fifo_watermark(struct device *dev,
+					       struct device_attribute *attr,
+					       char *buf)
+{
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
+	struct bmc150_accel_data *data = iio_priv(indio_dev);
+	int wm;
+
+	mutex_lock(&data->mutex);
+	wm = data->watermark;
+	mutex_unlock(&data->mutex);
+
+	return sprintf(buf, "%d\n", wm);
+}
+
+static ssize_t bmc150_accel_get_fifo_state(struct device *dev,
+					   struct device_attribute *attr,
+					   char *buf)
+{
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
+	struct bmc150_accel_data *data = iio_priv(indio_dev);
+	bool state;
+
+	mutex_lock(&data->mutex);
+	state = data->fifo_mode;
+	mutex_unlock(&data->mutex);
+
+	return sprintf(buf, "%d\n", state);
+}
+
+static IIO_CONST_ATTR(hwfifo_watermark_min, "1");
+static IIO_CONST_ATTR(hwfifo_watermark_max,
+		      __stringify(BMC150_ACCEL_FIFO_LENGTH));
+static IIO_DEVICE_ATTR(hwfifo_enabled, S_IRUGO,
+		       bmc150_accel_get_fifo_state, NULL, 0);
+static IIO_DEVICE_ATTR(hwfifo_watermark, S_IRUGO,
+		       bmc150_accel_get_fifo_watermark, NULL, 0);
+
+static const struct attribute *bmc150_accel_fifo_attributes[] = {
+	&iio_const_attr_hwfifo_watermark_min.dev_attr.attr,
+	&iio_const_attr_hwfifo_watermark_max.dev_attr.attr,
+	&iio_dev_attr_hwfifo_watermark.dev_attr.attr,
+	&iio_dev_attr_hwfifo_enabled.dev_attr.attr,
+	NULL,
+};
+
+static int bmc150_accel_set_watermark(struct iio_dev *indio_dev, unsigned val)
+{
+	struct bmc150_accel_data *data = iio_priv(indio_dev);
+
+	if (val > BMC150_ACCEL_FIFO_LENGTH)
+		val = BMC150_ACCEL_FIFO_LENGTH;
+
+	mutex_lock(&data->mutex);
+	data->watermark = val;
+	mutex_unlock(&data->mutex);
 
 	return 0;
 }
 
+/*
+ * We must read at least one full frame in one burst, otherwise the rest of the
+ * frame data is discarded.
+ */
+static int bmc150_accel_fifo_transfer(const struct i2c_client *client,
+				      char *buffer, int samples)
+{
+	int sample_length = 3 * 2;
+	u8 reg_fifo_data = BMC150_ACCEL_REG_FIFO_DATA;
+	int ret = -EIO;
+
+	if (i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
+		struct i2c_msg msg[2] = {
+			{
+				.addr = client->addr,
+				.flags = 0,
+				.buf = &reg_fifo_data,
+				.len = sizeof(reg_fifo_data),
+			},
+			{
+				.addr = client->addr,
+				.flags = I2C_M_RD,
+				.buf = (u8 *)buffer,
+				.len = samples * sample_length,
+			}
+		};
+
+		ret = i2c_transfer(client->adapter, msg, 2);
+		if (ret != 2)
+			ret = -EIO;
+		else
+			ret = 0;
+	} else {
+		int i, step = I2C_SMBUS_BLOCK_MAX / sample_length;
+
+		for (i = 0; i < samples * sample_length; i += step) {
+			ret = i2c_smbus_read_i2c_block_data(client,
+							    reg_fifo_data, step,
+							    &buffer[i]);
+			if (ret != step) {
+				ret = -EIO;
+				break;
+			}
+
+			ret = 0;
+		}
+	}
+
+	if (ret)
+		dev_err(&client->dev, "Error transferring data from fifo\n");
+
+	return ret;
+}
+
+static int __bmc150_accel_fifo_flush(struct iio_dev *indio_dev,
+				     unsigned samples, bool irq)
+{
+	struct bmc150_accel_data *data = iio_priv(indio_dev);
+	int ret, i;
+	u8 count;
+	u16 buffer[BMC150_ACCEL_FIFO_LENGTH * 3];
+	int64_t tstamp;
+	uint64_t sample_period;
+	ret = i2c_smbus_read_byte_data(data->client,
+				       BMC150_ACCEL_REG_FIFO_STATUS);
+	if (ret < 0) {
+		dev_err(&data->client->dev, "Error reading reg_fifo_status\n");
+		return ret;
+	}
+
+	count = ret & 0x7F;
+
+	if (!count)
+		return 0;
+
+	/*
+	 * If we getting called from IRQ handler we know the stored timestamp is
+	 * fairly accurate for the last stored sample. Otherwise, if we are
+	 * called as a result of a read operation from userspace and hence
+	 * before the watermark interrupt was triggered, take a timestamp
+	 * now. We can fall anywhere in between two samples so the error in this
+	 * case is at most one sample period.
+	 */
+	if (!irq) {
+		data->old_timestamp = data->timestamp;
+		data->timestamp = iio_get_time_ns();
+	}
+
+	/*
+	 * Approximate timestamps for each of the sample based on the sampling
+	 * frequency, timestamp for last sample and number of samples.
+	 *
+	 * Note that we can't use the current bandwidth settings to compute the
+	 * sample period because the sample rate varies with the device
+	 * (e.g. between 31.70ms to 32.20ms for a bandwidth of 15.63HZ). That
+	 * small variation adds when we store a large number of samples and
+	 * creates significant jitter between the last and first samples in
+	 * different batches (e.g. 32ms vs 21ms).
+	 *
+	 * To avoid this issue we compute the actual sample period ourselves
+	 * based on the timestamp delta between the last two flush operations.
+	 */
+	sample_period = (data->timestamp - data->old_timestamp);
+	do_div(sample_period, count);
+	tstamp = data->timestamp - (count - 1) * sample_period;
+
+	if (samples && count > samples)
+		count = samples;
+
+	ret = bmc150_accel_fifo_transfer(data->client, (u8 *)buffer, count);
+	if (ret)
+		return ret;
+
+	/*
+	 * Ideally we want the IIO core to handle the demux when running in fifo
+	 * mode but not when running in triggered buffer mode. Unfortunately
+	 * this does not seem to be possible, so stick with driver demux for
+	 * now.
+	 */
+	for (i = 0; i < count; i++) {
+		u16 sample[8];
+		int j, bit;
+
+		j = 0;
+		for_each_set_bit(bit, indio_dev->active_scan_mask,
+				 indio_dev->masklength)
+			memcpy(&sample[j++], &buffer[i * 3 + bit], 2);
+
+		iio_push_to_buffers_with_timestamp(indio_dev, sample, tstamp);
+
+		tstamp += sample_period;
+	}
+
+	return count;
+}
+
+static int bmc150_accel_fifo_flush(struct iio_dev *indio_dev, unsigned samples)
+{
+	struct bmc150_accel_data *data = iio_priv(indio_dev);
+	int ret;
+
+	mutex_lock(&data->mutex);
+	ret = __bmc150_accel_fifo_flush(indio_dev, samples, false);
+	mutex_unlock(&data->mutex);
+
+	return ret;
+}
+
 static IIO_CONST_ATTR_SAMP_FREQ_AVAIL(
-		"7.810000 15.630000 31.250000 62.500000 125 250 500 1000");
+		"15.620000 31.260000 62.50000 125 250 500 1000 2000");
 
 static struct attribute *bmc150_accel_attributes[] = {
 	&iio_const_attr_sampling_frequency_available.dev_attr.attr,
@@ -978,6 +1187,20 @@ static const struct iio_info bmc150_accel_info = {
 	.driver_module		= THIS_MODULE,
 };
 
+static const struct iio_info bmc150_accel_info_fifo = {
+	.attrs			= &bmc150_accel_attrs_group,
+	.read_raw		= bmc150_accel_read_raw,
+	.write_raw		= bmc150_accel_write_raw,
+	.read_event_value	= bmc150_accel_read_event,
+	.write_event_value	= bmc150_accel_write_event,
+	.write_event_config	= bmc150_accel_write_event_config,
+	.read_event_config	= bmc150_accel_read_event_config,
+	.validate_trigger	= bmc150_accel_validate_trigger,
+	.hwfifo_set_watermark	= bmc150_accel_set_watermark,
+	.hwfifo_flush_to_buffer	= bmc150_accel_fifo_flush,
+	.driver_module		= THIS_MODULE,
+};
+
 static irqreturn_t bmc150_accel_trigger_handler(int irq, void *p)
 {
 	struct iio_poll_func *pf = p;
@@ -986,7 +1209,7 @@ static irqreturn_t bmc150_accel_trigger_handler(int irq, void *p)
 	int bit, ret, i = 0;
 
 	mutex_lock(&data->mutex);
-	for_each_set_bit(bit, indio_dev->buffer->scan_mask,
+	for_each_set_bit(bit, indio_dev->active_scan_mask,
 			 indio_dev->masklength) {
 		ret = i2c_smbus_read_word_data(data->client,
 					       BMC150_ACCEL_AXIS_TO_REG(bit));
@@ -1008,12 +1231,12 @@ err_read:
 
 static int bmc150_accel_trig_try_reen(struct iio_trigger *trig)
 {
-	struct iio_dev *indio_dev = iio_trigger_get_drvdata(trig);
-	struct bmc150_accel_data *data = iio_priv(indio_dev);
+	struct bmc150_accel_trigger *t = iio_trigger_get_drvdata(trig);
+	struct bmc150_accel_data *data = t->data;
 	int ret;
 
 	/* new data interrupts don't need ack */
-	if (data->dready_trigger_on)
+	if (t == &t->data->triggers[BMC150_ACCEL_TRIGGER_DATA_READY])
 		return 0;
 
 	mutex_lock(&data->mutex);
@@ -1032,43 +1255,35 @@ static int bmc150_accel_trig_try_reen(struct iio_trigger *trig)
 	return 0;
 }
 
-static int bmc150_accel_data_rdy_trigger_set_state(struct iio_trigger *trig,
+static int bmc150_accel_trigger_set_state(struct iio_trigger *trig,
 						   bool state)
 {
-	struct iio_dev *indio_dev = iio_trigger_get_drvdata(trig);
-	struct bmc150_accel_data *data = iio_priv(indio_dev);
+	struct bmc150_accel_trigger *t = iio_trigger_get_drvdata(trig);
+	struct bmc150_accel_data *data = t->data;
 	int ret;
 
 	mutex_lock(&data->mutex);
 
-	if (!state && data->ev_enable_state && data->motion_trigger_on) {
-		data->motion_trigger_on = false;
+	if (t->enabled == state) {
 		mutex_unlock(&data->mutex);
 		return 0;
 	}
 
-	/*
-	 * Refer to comment in bmc150_accel_write_event_config for
-	 * enable/disable operation order
-	 */
-	ret = bmc150_accel_set_power_state(data, state);
-	if (ret < 0) {
-		mutex_unlock(&data->mutex);
-		return ret;
+	if (t->setup) {
+		ret = t->setup(t, state);
+		if (ret < 0) {
+			mutex_unlock(&data->mutex);
+			return ret;
+		}
 	}
-	if (data->motion_trig == trig)
-		ret =  bmc150_accel_setup_any_motion_interrupt(data, state);
-	else
-		ret = bmc150_accel_setup_new_data_interrupt(data, state);
+
+	ret = bmc150_accel_set_interrupt(data, t->intr, state);
 	if (ret < 0) {
-		bmc150_accel_set_power_state(data, false);
 		mutex_unlock(&data->mutex);
 		return ret;
 	}
-	if (data->motion_trig == trig)
-		data->motion_trigger_on = state;
-	else
-		data->dready_trigger_on = state;
+
+	t->enabled = state;
 
 	mutex_unlock(&data->mutex);
 
@@ -1076,23 +1291,22 @@ static int bmc150_accel_data_rdy_trigger_set_state(struct iio_trigger *trig,
 }
 
 static const struct iio_trigger_ops bmc150_accel_trigger_ops = {
-	.set_trigger_state = bmc150_accel_data_rdy_trigger_set_state,
+	.set_trigger_state = bmc150_accel_trigger_set_state,
 	.try_reenable = bmc150_accel_trig_try_reen,
 	.owner = THIS_MODULE,
 };
 
-static irqreturn_t bmc150_accel_event_handler(int irq, void *private)
+static int bmc150_accel_handle_roc_event(struct iio_dev *indio_dev)
 {
-	struct iio_dev *indio_dev = private;
 	struct bmc150_accel_data *data = iio_priv(indio_dev);
-	int ret;
 	int dir;
+	int ret;
 
 	ret = i2c_smbus_read_byte_data(data->client,
 				       BMC150_ACCEL_REG_INT_STATUS_2);
 	if (ret < 0) {
 		dev_err(&data->client->dev, "Error reading reg_int_status_2\n");
-		goto ack_intr_status;
+		return ret;
 	}
 
 	if (ret & BMC150_ACCEL_ANY_MOTION_BIT_SIGN)
@@ -1121,32 +1335,73 @@ static irqreturn_t bmc150_accel_event_handler(int irq, void *private)
 							IIO_EV_TYPE_ROC,
 							dir),
 							data->timestamp);
-ack_intr_status:
-	if (!data->dready_trigger_on)
+	return ret;
+}
+
+static irqreturn_t bmc150_accel_irq_thread_handler(int irq, void *private)
+{
+	struct iio_dev *indio_dev = private;
+	struct bmc150_accel_data *data = iio_priv(indio_dev);
+	bool ack = false;
+	int ret;
+
+	mutex_lock(&data->mutex);
+
+	if (data->fifo_mode) {
+		ret = __bmc150_accel_fifo_flush(indio_dev,
+						BMC150_ACCEL_FIFO_LENGTH, true);
+		if (ret > 0)
+			ack = true;
+	}
+
+	if (data->ev_enable_state) {
+		ret = bmc150_accel_handle_roc_event(indio_dev);
+		if (ret > 0)
+			ack = true;
+	}
+
+	if (ack) {
 		ret = i2c_smbus_write_byte_data(data->client,
 					BMC150_ACCEL_REG_INT_RST_LATCH,
 					BMC150_ACCEL_INT_MODE_LATCH_INT |
 					BMC150_ACCEL_INT_MODE_LATCH_RESET);
+		if (ret)
+			dev_err(&data->client->dev, "Error writing reg_int_rst_latch\n");
+		ret = IRQ_HANDLED;
+	} else {
+		ret = IRQ_NONE;
+	}
 
-	return IRQ_HANDLED;
+	mutex_unlock(&data->mutex);
+
+	return ret;
 }
 
-static irqreturn_t bmc150_accel_data_rdy_trig_poll(int irq, void *private)
+static irqreturn_t bmc150_accel_irq_handler(int irq, void *private)
 {
 	struct iio_dev *indio_dev = private;
 	struct bmc150_accel_data *data = iio_priv(indio_dev);
+	bool ack = false;
+	int i;
 
+	data->old_timestamp = data->timestamp;
 	data->timestamp = iio_get_time_ns();
 
-	if (data->dready_trigger_on)
-		iio_trigger_poll(data->dready_trig);
-	else if (data->motion_trigger_on)
-		iio_trigger_poll(data->motion_trig);
+	for (i = 0; i < BMC150_ACCEL_TRIGGERS; i++) {
+		if (data->triggers[i].enabled) {
+			iio_trigger_poll(data->triggers[i].indio_trig);
+			ack = true;
+			break;
+		}
+	}
 
-	if (data->ev_enable_state)
+	if (data->ev_enable_state || data->fifo_mode)
 		return IRQ_WAKE_THREAD;
-	else
+
+	if (ack)
 		return IRQ_HANDLED;
+
+	return IRQ_NONE;
 }
 
 static const char *bmc150_accel_match_acpi_device(struct device *dev, int *data)
@@ -1176,23 +1431,171 @@ static int bmc150_accel_gpio_probe(struct i2c_client *client,
 	dev = &client->dev;
 
 	/* data ready gpio interrupt pin */
-	gpio = devm_gpiod_get_index(dev, BMC150_ACCEL_GPIO_NAME, 0);
+	gpio = devm_gpiod_get_index(dev, BMC150_ACCEL_GPIO_NAME, 0, GPIOD_IN);
 	if (IS_ERR(gpio)) {
 		dev_err(dev, "Failed: gpio get index\n");
 		return PTR_ERR(gpio);
 	}
 
-	ret = gpiod_direction_input(gpio);
+	ret = gpiod_to_irq(gpio);
+
+	dev_dbg(dev, "GPIO resource, no:%d irq:%d\n", desc_to_gpio(gpio), ret);
+
+	return ret;
+}
+
+static const struct {
+	int intr;
+	const char *name;
+	int (*setup)(struct bmc150_accel_trigger *t, bool state);
+} bmc150_accel_triggers[BMC150_ACCEL_TRIGGERS] = {
+	{
+		.intr = 0,
+		.name = "%s-dev%d",
+	},
+	{
+		.intr = 1,
+		.name = "%s-any-motion-dev%d",
+		.setup = bmc150_accel_any_motion_setup,
+	},
+};
+
+static void bmc150_accel_unregister_triggers(struct bmc150_accel_data *data,
+					     int from)
+{
+	int i;
+
+	for (i = from; i >= 0; i++) {
+		if (data->triggers[i].indio_trig) {
+			iio_trigger_unregister(data->triggers[i].indio_trig);
+			data->triggers[i].indio_trig = NULL;
+		}
+	}
+}
+
+static int bmc150_accel_triggers_setup(struct iio_dev *indio_dev,
+				       struct bmc150_accel_data *data)
+{
+	int i, ret;
+
+	for (i = 0; i < BMC150_ACCEL_TRIGGERS; i++) {
+		struct bmc150_accel_trigger *t = &data->triggers[i];
+
+		t->indio_trig = devm_iio_trigger_alloc(&data->client->dev,
+					       bmc150_accel_triggers[i].name,
+						       indio_dev->name,
+						       indio_dev->id);
+		if (!t->indio_trig) {
+			ret = -ENOMEM;
+			break;
+		}
+
+		t->indio_trig->dev.parent = &data->client->dev;
+		t->indio_trig->ops = &bmc150_accel_trigger_ops;
+		t->intr = bmc150_accel_triggers[i].intr;
+		t->data = data;
+		t->setup = bmc150_accel_triggers[i].setup;
+		iio_trigger_set_drvdata(t->indio_trig, t);
+
+		ret = iio_trigger_register(t->indio_trig);
+		if (ret)
+			break;
+	}
+
 	if (ret)
+		bmc150_accel_unregister_triggers(data, i - 1);
+
+	return ret;
+}
+
+#define BMC150_ACCEL_FIFO_MODE_STREAM          0x80
+#define BMC150_ACCEL_FIFO_MODE_FIFO            0x40
+#define BMC150_ACCEL_FIFO_MODE_BYPASS          0x00
+
+static int bmc150_accel_fifo_set_mode(struct bmc150_accel_data *data)
+{
+	u8 reg = BMC150_ACCEL_REG_FIFO_CONFIG1;
+	int ret;
+
+	ret = i2c_smbus_write_byte_data(data->client, reg, data->fifo_mode);
+	if (ret < 0) {
+		dev_err(&data->client->dev, "Error writing reg_fifo_config1\n");
 		return ret;
+	}
 
-	ret = gpiod_to_irq(gpio);
+	if (!data->fifo_mode)
+		return 0;
 
-	dev_dbg(dev, "GPIO resource, no:%d irq:%d\n", desc_to_gpio(gpio), ret);
+	ret = i2c_smbus_write_byte_data(data->client,
+					BMC150_ACCEL_REG_FIFO_CONFIG0,
+					data->watermark);
+	if (ret < 0)
+		dev_err(&data->client->dev, "Error writing reg_fifo_config0\n");
 
 	return ret;
 }
 
+static int bmc150_accel_buffer_postenable(struct iio_dev *indio_dev)
+{
+	struct bmc150_accel_data *data = iio_priv(indio_dev);
+	int ret = 0;
+
+	if (indio_dev->currentmode == INDIO_BUFFER_TRIGGERED)
+		return iio_triggered_buffer_postenable(indio_dev);
+
+	mutex_lock(&data->mutex);
+
+	if (!data->watermark)
+		goto out;
+
+	ret = bmc150_accel_set_interrupt(data, BMC150_ACCEL_INT_WATERMARK,
+					 true);
+	if (ret)
+		goto out;
+
+	data->fifo_mode = BMC150_ACCEL_FIFO_MODE_FIFO;
+
+	ret = bmc150_accel_fifo_set_mode(data);
+	if (ret) {
+		data->fifo_mode = 0;
+		bmc150_accel_set_interrupt(data, BMC150_ACCEL_INT_WATERMARK,
+					   false);
+	}
+
+out:
+	mutex_unlock(&data->mutex);
+
+	return ret;
+}
+
+static int bmc150_accel_buffer_predisable(struct iio_dev *indio_dev)
+{
+	struct bmc150_accel_data *data = iio_priv(indio_dev);
+
+	if (indio_dev->currentmode == INDIO_BUFFER_TRIGGERED)
+		return iio_triggered_buffer_predisable(indio_dev);
+
+	mutex_lock(&data->mutex);
+
+	if (!data->fifo_mode)
+		goto out;
+
+	bmc150_accel_set_interrupt(data, BMC150_ACCEL_INT_WATERMARK, false);
+	__bmc150_accel_fifo_flush(indio_dev, BMC150_ACCEL_FIFO_LENGTH, false);
+	data->fifo_mode = 0;
+	bmc150_accel_fifo_set_mode(data);
+
+out:
+	mutex_unlock(&data->mutex);
+
+	return 0;
+}
+
+static const struct iio_buffer_setup_ops bmc150_accel_buffer_ops = {
+	.postenable = bmc150_accel_buffer_postenable,
+	.predisable = bmc150_accel_buffer_predisable,
+};
+
 static int bmc150_accel_probe(struct i2c_client *client,
 			      const struct i2c_device_id *id)
 {
@@ -1239,53 +1642,51 @@ static int bmc150_accel_probe(struct i2c_client *client,
 	if (client->irq >= 0) {
 		ret = devm_request_threaded_irq(
 						&client->dev, client->irq,
-						bmc150_accel_data_rdy_trig_poll,
-						bmc150_accel_event_handler,
+						bmc150_accel_irq_handler,
+						bmc150_accel_irq_thread_handler,
 						IRQF_TRIGGER_RISING,
 						BMC150_ACCEL_IRQ_NAME,
 						indio_dev);
 		if (ret)
 			return ret;
 
-		data->dready_trig = devm_iio_trigger_alloc(&client->dev,
-							   "%s-dev%d",
-							   indio_dev->name,
-							   indio_dev->id);
-		if (!data->dready_trig)
-			return -ENOMEM;
-
-		data->motion_trig = devm_iio_trigger_alloc(&client->dev,
-							  "%s-any-motion-dev%d",
-							  indio_dev->name,
-							  indio_dev->id);
-		if (!data->motion_trig)
-			return -ENOMEM;
-
-		data->dready_trig->dev.parent = &client->dev;
-		data->dready_trig->ops = &bmc150_accel_trigger_ops;
-		iio_trigger_set_drvdata(data->dready_trig, indio_dev);
-		ret = iio_trigger_register(data->dready_trig);
-		if (ret)
+		/*
+		 * Set latched mode interrupt. While certain interrupts are
+		 * non-latched regardless of this settings (e.g. new data) we
+		 * want to use latch mode when we can to prevent interrupt
+		 * flooding.
+		 */
+		ret = i2c_smbus_write_byte_data(data->client,
+						BMC150_ACCEL_REG_INT_RST_LATCH,
+					     BMC150_ACCEL_INT_MODE_LATCH_RESET);
+		if (ret < 0) {
+			dev_err(&data->client->dev, "Error writing reg_int_rst_latch\n");
 			return ret;
-
-		data->motion_trig->dev.parent = &client->dev;
-		data->motion_trig->ops = &bmc150_accel_trigger_ops;
-		iio_trigger_set_drvdata(data->motion_trig, indio_dev);
-		ret = iio_trigger_register(data->motion_trig);
-		if (ret) {
-			data->motion_trig = NULL;
-			goto err_trigger_unregister;
 		}
 
+		bmc150_accel_interrupts_setup(indio_dev, data);
+
+		ret = bmc150_accel_triggers_setup(indio_dev, data);
+		if (ret)
+			return ret;
+
 		ret = iio_triggered_buffer_setup(indio_dev,
 						 &iio_pollfunc_store_time,
 						 bmc150_accel_trigger_handler,
-						 NULL);
+						 &bmc150_accel_buffer_ops);
 		if (ret < 0) {
 			dev_err(&client->dev,
 				"Failed: iio triggered buffer setup\n");
 			goto err_trigger_unregister;
 		}
+
+		if (i2c_check_functionality(client->adapter, I2C_FUNC_I2C) ||
+		    i2c_check_functionality(client->adapter,
+					    I2C_FUNC_SMBUS_READ_I2C_BLOCK)) {
+			indio_dev->modes |= INDIO_BUFFER_SOFTWARE;
+			indio_dev->info = &bmc150_accel_info_fifo;
+			indio_dev->buffer->attrs = bmc150_accel_fifo_attributes;
+		}
 	}
 
 	ret = iio_device_register(indio_dev);
@@ -1308,13 +1709,10 @@ static int bmc150_accel_probe(struct i2c_client *client,
 err_iio_unregister:
 	iio_device_unregister(indio_dev);
 err_buffer_cleanup:
-	if (data->dready_trig)
+	if (indio_dev->pollfunc)
 		iio_triggered_buffer_cleanup(indio_dev);
 err_trigger_unregister:
-	if (data->dready_trig)
-		iio_trigger_unregister(data->dready_trig);
-	if (data->motion_trig)
-		iio_trigger_unregister(data->motion_trig);
+	bmc150_accel_unregister_triggers(data, BMC150_ACCEL_TRIGGERS - 1);
 
 	return ret;
 }
@@ -1330,11 +1728,7 @@ static int bmc150_accel_remove(struct i2c_client *client)
 
 	iio_device_unregister(indio_dev);
 
-	if (data->dready_trig) {
-		iio_triggered_buffer_cleanup(indio_dev);
-		iio_trigger_unregister(data->dready_trig);
-		iio_trigger_unregister(data->motion_trig);
-	}
+	bmc150_accel_unregister_triggers(data, BMC150_ACCEL_TRIGGERS - 1);
 
 	mutex_lock(&data->mutex);
 	bmc150_accel_set_mode(data, BMC150_ACCEL_SLEEP_MODE_DEEP_SUSPEND, 0);
@@ -1362,9 +1756,9 @@ static int bmc150_accel_resume(struct device *dev)
 	struct bmc150_accel_data *data = iio_priv(indio_dev);
 
 	mutex_lock(&data->mutex);
-	if (data->dready_trigger_on || data->motion_trigger_on ||
-							data->ev_enable_state)
+	if (atomic_read(&data->active_intr))
 		bmc150_accel_set_mode(data, BMC150_ACCEL_SLEEP_MODE_NORMAL, 0);
+	bmc150_accel_fifo_set_mode(data);
 	mutex_unlock(&data->mutex);
 
 	return 0;
@@ -1398,6 +1792,9 @@ static int bmc150_accel_runtime_resume(struct device *dev)
 	ret = bmc150_accel_set_mode(data, BMC150_ACCEL_SLEEP_MODE_NORMAL, 0);
 	if (ret < 0)
 		return ret;
+	ret = bmc150_accel_fifo_set_mode(data);
+	if (ret < 0)
+		return ret;
 
 	sleep_val = bmc150_accel_get_startup_times(data);
 	if (sleep_val < 20)
diff --git a/drivers/iio/accel/kxcjk-1013.c b/drivers/iio/accel/kxcjk-1013.c
index 567de269cc00..51da3692d561 100644
--- a/drivers/iio/accel/kxcjk-1013.c
+++ b/drivers/iio/accel/kxcjk-1013.c
@@ -956,7 +956,7 @@ static irqreturn_t kxcjk1013_trigger_handler(int irq, void *p)
 
 	mutex_lock(&data->mutex);
 
-	for_each_set_bit(bit, indio_dev->buffer->scan_mask,
+	for_each_set_bit(bit, indio_dev->active_scan_mask,
 			 indio_dev->masklength) {
 		ret = kxcjk1013_get_acc_reg(data, bit);
 		if (ret < 0) {
@@ -1169,16 +1169,12 @@ static int kxcjk1013_gpio_probe(struct i2c_client *client,
 	dev = &client->dev;
 
 	/* data ready gpio interrupt pin */
-	gpio = devm_gpiod_get_index(dev, "kxcjk1013_int", 0);
+	gpio = devm_gpiod_get_index(dev, "kxcjk1013_int", 0, GPIOD_IN);
 	if (IS_ERR(gpio)) {
 		dev_err(dev, "acpi gpio get index failed\n");
 		return PTR_ERR(gpio);
 	}
 
-	ret = gpiod_direction_input(gpio);
-	if (ret)
-		return ret;
-
 	ret = gpiod_to_irq(gpio);
 
 	dev_dbg(dev, "GPIO resource, no:%d irq:%d\n", desc_to_gpio(gpio), ret);
diff --git a/drivers/iio/accel/mma9551.c b/drivers/iio/accel/mma9551.c
index 46c38351c6a3..7db7cc0bf362 100644
--- a/drivers/iio/accel/mma9551.c
+++ b/drivers/iio/accel/mma9551.c
@@ -418,17 +418,18 @@ static int mma9551_gpio_probe(struct iio_dev *indio_dev)
 	struct device *dev = &data->client->dev;
 
 	for (i = 0; i < MMA9551_GPIO_COUNT; i++) {
-		gpio = devm_gpiod_get_index(dev, MMA9551_GPIO_NAME, i);
+		gpio = devm_gpiod_get_index(dev, MMA9551_GPIO_NAME, i,
+					    GPIOD_IN);
 		if (IS_ERR(gpio)) {
 			dev_err(dev, "acpi gpio get index failed\n");
 			return PTR_ERR(gpio);
 		}
 
-		ret = gpiod_direction_input(gpio);
-		if (ret)
+		ret = gpiod_to_irq(gpio);
+		if (ret < 0)
 			return ret;
 
-		data->irqs[i] = gpiod_to_irq(gpio);
+		data->irqs[i] = ret;
 		ret = devm_request_threaded_irq(dev, data->irqs[i],
 				NULL, mma9551_event_handler,
 				IRQF_TRIGGER_RISING | IRQF_ONESHOT,
diff --git a/drivers/iio/accel/mma9553.c b/drivers/iio/accel/mma9553.c
index d23ebf192f63..2df1af7d43fc 100644
--- a/drivers/iio/accel/mma9553.c
+++ b/drivers/iio/accel/mma9553.c
@@ -1109,16 +1109,12 @@ static int mma9553_gpio_probe(struct i2c_client *client)
 	dev = &client->dev;
 
 	/* data ready gpio interrupt pin */
-	gpio = devm_gpiod_get_index(dev, MMA9553_GPIO_NAME, 0);
+	gpio = devm_gpiod_get_index(dev, MMA9553_GPIO_NAME, 0, GPIOD_IN);
 	if (IS_ERR(gpio)) {
 		dev_err(dev, "acpi gpio get index failed\n");
 		return PTR_ERR(gpio);
 	}
 
-	ret = gpiod_direction_input(gpio);
-	if (ret)
-		return ret;
-
 	ret = gpiod_to_irq(gpio);
 
 	dev_dbg(dev, "gpio resource, no:%d irq:%d\n", desc_to_gpio(gpio), ret);
diff --git a/drivers/iio/accel/st_accel.h b/drivers/iio/accel/st_accel.h
index fa9646034305..7ee9724b1428 100644
--- a/drivers/iio/accel/st_accel.h
+++ b/drivers/iio/accel/st_accel.h
@@ -14,6 +14,7 @@
 #include <linux/types.h>
 #include <linux/iio/common/st_sensors.h>
 
+#define LIS3LV02DL_ACCEL_DEV_NAME	"lis3lv02dl_accel"
 #define LSM303DLHC_ACCEL_DEV_NAME	"lsm303dlhc_accel"
 #define LIS3DH_ACCEL_DEV_NAME		"lis3dh"
 #define LSM330D_ACCEL_DEV_NAME		"lsm330d_accel"
diff --git a/drivers/iio/accel/st_accel_core.c b/drivers/iio/accel/st_accel_core.c
index 53f32629283a..58d1d13d552a 100644
--- a/drivers/iio/accel/st_accel_core.c
+++ b/drivers/iio/accel/st_accel_core.c
@@ -129,6 +129,30 @@
 #define ST_ACCEL_3_IG1_EN_MASK			0x08
 #define ST_ACCEL_3_MULTIREAD_BIT		false
 
+/* CUSTOM VALUES FOR SENSOR 4 */
+#define ST_ACCEL_4_WAI_EXP			0x3a
+#define ST_ACCEL_4_ODR_ADDR			0x20
+#define ST_ACCEL_4_ODR_MASK			0x30 /* DF1 and DF0 */
+#define ST_ACCEL_4_ODR_AVL_40HZ_VAL		0x00
+#define ST_ACCEL_4_ODR_AVL_160HZ_VAL		0x01
+#define ST_ACCEL_4_ODR_AVL_640HZ_VAL		0x02
+#define ST_ACCEL_4_ODR_AVL_2560HZ_VAL		0x03
+#define ST_ACCEL_4_PW_ADDR			0x20
+#define ST_ACCEL_4_PW_MASK			0xc0
+#define ST_ACCEL_4_FS_ADDR			0x21
+#define ST_ACCEL_4_FS_MASK			0x80
+#define ST_ACCEL_4_FS_AVL_2_VAL			0X00
+#define ST_ACCEL_4_FS_AVL_6_VAL			0X01
+#define ST_ACCEL_4_FS_AVL_2_GAIN		IIO_G_TO_M_S_2(1024)
+#define ST_ACCEL_4_FS_AVL_6_GAIN		IIO_G_TO_M_S_2(340)
+#define ST_ACCEL_4_BDU_ADDR			0x21
+#define ST_ACCEL_4_BDU_MASK			0x40
+#define ST_ACCEL_4_DRDY_IRQ_ADDR		0x21
+#define ST_ACCEL_4_DRDY_IRQ_INT1_MASK		0x04
+#define ST_ACCEL_4_IG1_EN_ADDR			0x21
+#define ST_ACCEL_4_IG1_EN_MASK			0x08
+#define ST_ACCEL_4_MULTIREAD_BIT		true
+
 static const struct iio_chan_spec st_accel_12bit_channels[] = {
 	ST_SENSORS_LSM_CHANNELS(IIO_ACCEL,
 			BIT(IIO_CHAN_INFO_RAW) | BIT(IIO_CHAN_INFO_SCALE),
@@ -373,6 +397,63 @@ static const struct st_sensor_settings st_accel_sensors_settings[] = {
 		.multi_read_bit = ST_ACCEL_3_MULTIREAD_BIT,
 		.bootime = 2,
 	},
+	{
+		.wai = ST_ACCEL_4_WAI_EXP,
+		.sensors_supported = {
+			[0] = LIS3LV02DL_ACCEL_DEV_NAME,
+		},
+		.ch = (struct iio_chan_spec *)st_accel_12bit_channels,
+		.odr = {
+			.addr = ST_ACCEL_4_ODR_ADDR,
+			.mask = ST_ACCEL_4_ODR_MASK,
+			.odr_avl = {
+				{ 40, ST_ACCEL_4_ODR_AVL_40HZ_VAL },
+				{ 160, ST_ACCEL_4_ODR_AVL_160HZ_VAL, },
+				{ 640, ST_ACCEL_4_ODR_AVL_640HZ_VAL, },
+				{ 2560, ST_ACCEL_4_ODR_AVL_2560HZ_VAL, },
+			},
+		},
+		.pw = {
+			.addr = ST_ACCEL_4_PW_ADDR,
+			.mask = ST_ACCEL_4_PW_MASK,
+			.value_on = ST_SENSORS_DEFAULT_POWER_ON_VALUE,
+			.value_off = ST_SENSORS_DEFAULT_POWER_OFF_VALUE,
+		},
+		.enable_axis = {
+			.addr = ST_SENSORS_DEFAULT_AXIS_ADDR,
+			.mask = ST_SENSORS_DEFAULT_AXIS_MASK,
+		},
+		.fs = {
+			.addr = ST_ACCEL_4_FS_ADDR,
+			.mask = ST_ACCEL_4_FS_MASK,
+			.fs_avl = {
+				[0] = {
+					.num = ST_ACCEL_FS_AVL_2G,
+					.value = ST_ACCEL_4_FS_AVL_2_VAL,
+					.gain = ST_ACCEL_4_FS_AVL_2_GAIN,
+				},
+				[1] = {
+					.num = ST_ACCEL_FS_AVL_6G,
+					.value = ST_ACCEL_4_FS_AVL_6_VAL,
+					.gain = ST_ACCEL_4_FS_AVL_6_GAIN,
+				},
+			},
+		},
+		.bdu = {
+			.addr = ST_ACCEL_4_BDU_ADDR,
+			.mask = ST_ACCEL_4_BDU_MASK,
+		},
+		.drdy_irq = {
+			.addr = ST_ACCEL_4_DRDY_IRQ_ADDR,
+			.mask_int1 = ST_ACCEL_4_DRDY_IRQ_INT1_MASK,
+			.ig1 = {
+				.en_addr = ST_ACCEL_4_IG1_EN_ADDR,
+				.en_mask = ST_ACCEL_4_IG1_EN_MASK,
+			},
+		},
+		.multi_read_bit = ST_ACCEL_4_MULTIREAD_BIT,
+		.bootime = 2, /* guess */
+	},
 };
 
 static int st_accel_read_raw(struct iio_dev *indio_dev,
diff --git a/drivers/iio/accel/st_accel_i2c.c b/drivers/iio/accel/st_accel_i2c.c
index c7246bdd30b9..6b720c190b2d 100644
--- a/drivers/iio/accel/st_accel_i2c.c
+++ b/drivers/iio/accel/st_accel_i2c.c
@@ -21,6 +21,10 @@
 #ifdef CONFIG_OF
 static const struct of_device_id st_accel_of_match[] = {
 	{
+		.compatible = "st,lis3lv02dl-accel",
+		.data = LIS3LV02DL_ACCEL_DEV_NAME,
+	},
+	{
 		.compatible = "st,lsm303dlh-accel",
 		.data = LSM303DLH_ACCEL_DEV_NAME,
 	},
diff --git a/drivers/iio/adc/Kconfig b/drivers/iio/adc/Kconfig
index 202daf889be2..e36a73e7c3a8 100644
--- a/drivers/iio/adc/Kconfig
+++ b/drivers/iio/adc/Kconfig
@@ -135,9 +135,19 @@ config AXP288_ADC
 	  device. Depending on platform configuration, this general purpose ADC can
 	  be used for sampling sensors such as thermal resistors.
 
+config DA9150_GPADC
+	tristate "Dialog DA9150 GPADC driver support"
+	depends on MFD_DA9150
+	help
+	  Say yes here to build support for Dialog DA9150 GPADC.
+
+	  This driver can also be built as a module. If chosen, the module name
+	  will be da9150-gpadc.
+
 config CC10001_ADC
 	tristate "Cosmic Circuits 10001 ADC driver"
-	depends on HAS_IOMEM || HAVE_CLK || REGULATOR
+	depends on HAVE_CLK || REGULATOR
+	depends on HAS_IOMEM
 	select IIO_BUFFER
 	select IIO_TRIGGERED_BUFFER
 	help
@@ -186,10 +196,11 @@ config MAX1363
 	  data via the iio dev interface.
 
 config MCP320X
-	tristate "Microchip Technology MCP3204/08"
+	tristate "Microchip Technology MCP3x01/02/04/08"
 	depends on SPI
 	help
-	  Say yes here to build support for Microchip Technology's MCP3204 or
+	  Say yes here to build support for Microchip Technology's
+	  MCP3001, MCP3002, MCP3004, MCP3008, MCP3201, MCP3202, MCP3204 or
 	  MCP3208 analog to digital converter.
 
 	  This driver can also be built as a module. If so, the module will be
diff --git a/drivers/iio/adc/Makefile b/drivers/iio/adc/Makefile
index 0315af640866..3930e63e84bc 100644
--- a/drivers/iio/adc/Makefile
+++ b/drivers/iio/adc/Makefile
@@ -15,6 +15,7 @@ obj-$(CONFIG_AD7887) += ad7887.o
 obj-$(CONFIG_AD799X) += ad799x.o
 obj-$(CONFIG_AT91_ADC) += at91_adc.o
 obj-$(CONFIG_AXP288_ADC) += axp288_adc.o
+obj-$(CONFIG_DA9150_GPADC) += da9150-gpadc.o
 obj-$(CONFIG_CC10001_ADC) += cc10001_adc.o
 obj-$(CONFIG_EXYNOS_ADC) += exynos_adc.o
 obj-$(CONFIG_LP8788_ADC) += lp8788_adc.o
diff --git a/drivers/iio/adc/ad7793.c b/drivers/iio/adc/ad7793.c
index 4dddeabdfbb0..b84922a4b32e 100644
--- a/drivers/iio/adc/ad7793.c
+++ b/drivers/iio/adc/ad7793.c
@@ -861,5 +861,5 @@ static struct spi_driver ad7793_driver = {
 module_spi_driver(ad7793_driver);
 
 MODULE_AUTHOR("Michael Hennerich <hennerich@blackfin.uclinux.org>");
-MODULE_DESCRIPTION("Analog Devices AD7793 and simialr ADCs");
+MODULE_DESCRIPTION("Analog Devices AD7793 and similar ADCs");
 MODULE_LICENSE("GPL v2");
diff --git a/drivers/iio/adc/at91_adc.c b/drivers/iio/adc/at91_adc.c
index ff61ae55dd3f..8a0eb4a04fb5 100644
--- a/drivers/iio/adc/at91_adc.c
+++ b/drivers/iio/adc/at91_adc.c
@@ -544,7 +544,6 @@ static int at91_adc_configure_trigger(struct iio_trigger *trig, bool state)
 {
 	struct iio_dev *idev = iio_trigger_get_drvdata(trig);
 	struct at91_adc_state *st = iio_priv(idev);
-	struct iio_buffer *buffer = idev->buffer;
 	struct at91_adc_reg_desc *reg = st->registers;
 	u32 status = at91_adc_readl(st, reg->trigger_register);
 	int value;
@@ -564,7 +563,7 @@ static int at91_adc_configure_trigger(struct iio_trigger *trig, bool state)
 		at91_adc_writel(st, reg->trigger_register,
 				status | value);
 
-		for_each_set_bit(bit, buffer->scan_mask,
+		for_each_set_bit(bit, idev->active_scan_mask,
 				 st->num_channels) {
 			struct iio_chan_spec const *chan = idev->channels + bit;
 			at91_adc_writel(st, AT91_ADC_CHER,
@@ -579,7 +578,7 @@ static int at91_adc_configure_trigger(struct iio_trigger *trig, bool state)
 		at91_adc_writel(st, reg->trigger_register,
 				status & ~value);
 
-		for_each_set_bit(bit, buffer->scan_mask,
+		for_each_set_bit(bit, idev->active_scan_mask,
 				 st->num_channels) {
 			struct iio_chan_spec const *chan = idev->channels + bit;
 			at91_adc_writel(st, AT91_ADC_CHDR,
diff --git a/drivers/iio/adc/da9150-gpadc.c b/drivers/iio/adc/da9150-gpadc.c
new file mode 100644
index 000000000000..3445107e10b7
--- /dev/null
+++ b/drivers/iio/adc/da9150-gpadc.c
@@ -0,0 +1,407 @@
+/*
+ * DA9150 GPADC Driver
+ *
+ * Copyright (c) 2014 Dialog Semiconductor
+ *
+ * Author: Adam Thomson <Adam.Thomson.Opensource@diasemi.com>
+ *
+ * This program is free software; you can redistribute  it and/or modify it
+ * under  the terms of  the GNU General  Public License as published by the
+ * Free Software Foundation;  either version 2 of the  License, or (at your
+ * option) any later version.
+ */
+
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/interrupt.h>
+#include <linux/mutex.h>
+#include <linux/completion.h>
+#include <linux/iio/iio.h>
+#include <linux/iio/machine.h>
+#include <linux/iio/driver.h>
+#include <linux/mfd/da9150/core.h>
+#include <linux/mfd/da9150/registers.h>
+
+/* Channels */
+enum da9150_gpadc_hw_channel {
+	DA9150_GPADC_HW_CHAN_GPIOA_2V = 0,
+	DA9150_GPADC_HW_CHAN_GPIOA_2V_,
+	DA9150_GPADC_HW_CHAN_GPIOB_2V,
+	DA9150_GPADC_HW_CHAN_GPIOB_2V_,
+	DA9150_GPADC_HW_CHAN_GPIOC_2V,
+	DA9150_GPADC_HW_CHAN_GPIOC_2V_,
+	DA9150_GPADC_HW_CHAN_GPIOD_2V,
+	DA9150_GPADC_HW_CHAN_GPIOD_2V_,
+	DA9150_GPADC_HW_CHAN_IBUS_SENSE,
+	DA9150_GPADC_HW_CHAN_IBUS_SENSE_,
+	DA9150_GPADC_HW_CHAN_VBUS_DIV,
+	DA9150_GPADC_HW_CHAN_VBUS_DIV_,
+	DA9150_GPADC_HW_CHAN_ID,
+	DA9150_GPADC_HW_CHAN_ID_,
+	DA9150_GPADC_HW_CHAN_VSYS,
+	DA9150_GPADC_HW_CHAN_VSYS_,
+	DA9150_GPADC_HW_CHAN_GPIOA_6V,
+	DA9150_GPADC_HW_CHAN_GPIOA_6V_,
+	DA9150_GPADC_HW_CHAN_GPIOB_6V,
+	DA9150_GPADC_HW_CHAN_GPIOB_6V_,
+	DA9150_GPADC_HW_CHAN_GPIOC_6V,
+	DA9150_GPADC_HW_CHAN_GPIOC_6V_,
+	DA9150_GPADC_HW_CHAN_GPIOD_6V,
+	DA9150_GPADC_HW_CHAN_GPIOD_6V_,
+	DA9150_GPADC_HW_CHAN_VBAT,
+	DA9150_GPADC_HW_CHAN_VBAT_,
+	DA9150_GPADC_HW_CHAN_TBAT,
+	DA9150_GPADC_HW_CHAN_TBAT_,
+	DA9150_GPADC_HW_CHAN_TJUNC_CORE,
+	DA9150_GPADC_HW_CHAN_TJUNC_CORE_,
+	DA9150_GPADC_HW_CHAN_TJUNC_OVP,
+	DA9150_GPADC_HW_CHAN_TJUNC_OVP_,
+};
+
+enum da9150_gpadc_channel {
+	DA9150_GPADC_CHAN_GPIOA = 0,
+	DA9150_GPADC_CHAN_GPIOB,
+	DA9150_GPADC_CHAN_GPIOC,
+	DA9150_GPADC_CHAN_GPIOD,
+	DA9150_GPADC_CHAN_IBUS,
+	DA9150_GPADC_CHAN_VBUS,
+	DA9150_GPADC_CHAN_VSYS,
+	DA9150_GPADC_CHAN_VBAT,
+	DA9150_GPADC_CHAN_TBAT,
+	DA9150_GPADC_CHAN_TJUNC_CORE,
+	DA9150_GPADC_CHAN_TJUNC_OVP,
+};
+
+/* Private data */
+struct da9150_gpadc {
+	struct da9150 *da9150;
+	struct device *dev;
+
+	struct mutex lock;
+	struct completion complete;
+};
+
+
+static irqreturn_t da9150_gpadc_irq(int irq, void *data)
+{
+
+	struct da9150_gpadc *gpadc = data;
+
+	complete(&gpadc->complete);
+
+	return IRQ_HANDLED;
+}
+
+static int da9150_gpadc_read_adc(struct da9150_gpadc *gpadc, int hw_chan)
+{
+	u8 result_regs[2];
+	int result;
+
+	mutex_lock(&gpadc->lock);
+
+	/* Set channel & enable measurement */
+	da9150_reg_write(gpadc->da9150, DA9150_GPADC_MAN,
+			 (DA9150_GPADC_EN_MASK |
+			  hw_chan << DA9150_GPADC_MUX_SHIFT));
+
+	/* Consume left-over completion from a previous timeout */
+	try_wait_for_completion(&gpadc->complete);
+
+	/* Check for actual completion */
+	wait_for_completion_timeout(&gpadc->complete, msecs_to_jiffies(5));
+
+	/* Read result and status from device */
+	da9150_bulk_read(gpadc->da9150, DA9150_GPADC_RES_A, 2, result_regs);
+
+	mutex_unlock(&gpadc->lock);
+
+	/* Check to make sure device really has completed reading */
+	if (result_regs[1] & DA9150_GPADC_RUN_MASK) {
+		dev_err(gpadc->dev, "Timeout on channel %d of GPADC\n",
+			hw_chan);
+		return -ETIMEDOUT;
+	}
+
+	/* LSBs - 2 bits */
+	result = (result_regs[1] & DA9150_GPADC_RES_L_MASK) >>
+		 DA9150_GPADC_RES_L_SHIFT;
+	/* MSBs - 8 bits */
+	result |= result_regs[0] << DA9150_GPADC_RES_L_BITS;
+
+	return result;
+}
+
+static inline int da9150_gpadc_gpio_6v_voltage_now(int raw_val)
+{
+	/* Convert to mV */
+	return (6 * ((raw_val * 1000) + 500)) / 1024;
+}
+
+static inline int da9150_gpadc_ibus_current_avg(int raw_val)
+{
+	/* Convert to mA */
+	return (4 * ((raw_val * 1000) + 500)) / 2048;
+}
+
+static inline int da9150_gpadc_vbus_21v_voltage_now(int raw_val)
+{
+	/* Convert to mV */
+	return (21 * ((raw_val * 1000) + 500)) / 1024;
+}
+
+static inline int da9150_gpadc_vsys_6v_voltage_now(int raw_val)
+{
+	/* Convert to mV */
+	return (3 * ((raw_val * 1000) + 500)) / 512;
+}
+
+static int da9150_gpadc_read_processed(struct da9150_gpadc *gpadc, int channel,
+				       int hw_chan, int *val)
+{
+	int raw_val;
+
+	raw_val = da9150_gpadc_read_adc(gpadc, hw_chan);
+	if (raw_val < 0)
+		return raw_val;
+
+	switch (channel) {
+	case DA9150_GPADC_CHAN_GPIOA:
+	case DA9150_GPADC_CHAN_GPIOB:
+	case DA9150_GPADC_CHAN_GPIOC:
+	case DA9150_GPADC_CHAN_GPIOD:
+		*val = da9150_gpadc_gpio_6v_voltage_now(raw_val);
+		break;
+	case DA9150_GPADC_CHAN_IBUS:
+		*val = da9150_gpadc_ibus_current_avg(raw_val);
+		break;
+	case DA9150_GPADC_CHAN_VBUS:
+		*val = da9150_gpadc_vbus_21v_voltage_now(raw_val);
+		break;
+	case DA9150_GPADC_CHAN_VSYS:
+		*val = da9150_gpadc_vsys_6v_voltage_now(raw_val);
+		break;
+	default:
+		/* No processing for other channels so return raw value */
+		*val = raw_val;
+		break;
+	}
+
+	return IIO_VAL_INT;
+}
+
+static int da9150_gpadc_read_scale(int channel, int *val, int *val2)
+{
+	switch (channel) {
+	case DA9150_GPADC_CHAN_VBAT:
+		*val = 2932;
+		*val2 = 1000;
+		return IIO_VAL_FRACTIONAL;
+	case DA9150_GPADC_CHAN_TJUNC_CORE:
+	case DA9150_GPADC_CHAN_TJUNC_OVP:
+		*val = 1000000;
+		*val2 = 4420;
+		return IIO_VAL_FRACTIONAL;
+	default:
+		return -EINVAL;
+	}
+}
+
+static int da9150_gpadc_read_offset(int channel, int *val)
+{
+	switch (channel) {
+	case DA9150_GPADC_CHAN_VBAT:
+		*val = 1500000 / 2932;
+		return IIO_VAL_INT;
+	case DA9150_GPADC_CHAN_TJUNC_CORE:
+	case DA9150_GPADC_CHAN_TJUNC_OVP:
+		*val = -144;
+		return IIO_VAL_INT;
+	default:
+		return -EINVAL;
+	}
+}
+
+static int da9150_gpadc_read_raw(struct iio_dev *indio_dev,
+				 struct iio_chan_spec const *chan,
+				 int *val, int *val2, long mask)
+{
+	struct da9150_gpadc *gpadc = iio_priv(indio_dev);
+
+	if ((chan->channel < DA9150_GPADC_CHAN_GPIOA) ||
+	    (chan->channel > DA9150_GPADC_CHAN_TJUNC_OVP))
+		return -EINVAL;
+
+	switch (mask) {
+	case IIO_CHAN_INFO_RAW:
+	case IIO_CHAN_INFO_PROCESSED:
+		return da9150_gpadc_read_processed(gpadc, chan->channel,
+						   chan->address, val);
+	case IIO_CHAN_INFO_SCALE:
+		return da9150_gpadc_read_scale(chan->channel, val, val2);
+	case IIO_CHAN_INFO_OFFSET:
+		return da9150_gpadc_read_offset(chan->channel, val);
+	default:
+		return -EINVAL;
+	}
+}
+
+static const struct iio_info da9150_gpadc_info = {
+	.read_raw = &da9150_gpadc_read_raw,
+	.driver_module = THIS_MODULE,
+};
+
+#define DA9150_GPADC_CHANNEL(_id, _hw_id, _type, chan_info,	\
+			     _ext_name) {			\
+	.type = _type,						\
+	.indexed = 1,						\
+	.channel = DA9150_GPADC_CHAN_##_id,			\
+	.address = DA9150_GPADC_HW_CHAN_##_hw_id,		\
+	.info_mask_separate = chan_info,			\
+	.extend_name = _ext_name,				\
+	.datasheet_name = #_id,					\
+}
+
+#define DA9150_GPADC_CHANNEL_RAW(_id, _hw_id, _type, _ext_name)	\
+	DA9150_GPADC_CHANNEL(_id, _hw_id, _type,		\
+			     BIT(IIO_CHAN_INFO_RAW), _ext_name)
+
+#define DA9150_GPADC_CHANNEL_SCALED(_id, _hw_id, _type, _ext_name)	\
+	DA9150_GPADC_CHANNEL(_id, _hw_id, _type,			\
+			     BIT(IIO_CHAN_INFO_RAW) |			\
+			     BIT(IIO_CHAN_INFO_SCALE) |			\
+			     BIT(IIO_CHAN_INFO_OFFSET),			\
+			     _ext_name)
+
+#define DA9150_GPADC_CHANNEL_PROCESSED(_id, _hw_id, _type, _ext_name)	\
+	DA9150_GPADC_CHANNEL(_id, _hw_id, _type,			\
+			     BIT(IIO_CHAN_INFO_PROCESSED), _ext_name)
+
+/* Supported channels */
+static const struct iio_chan_spec da9150_gpadc_channels[] = {
+	DA9150_GPADC_CHANNEL_PROCESSED(GPIOA, GPIOA_6V, IIO_VOLTAGE, NULL),
+	DA9150_GPADC_CHANNEL_PROCESSED(GPIOB, GPIOB_6V, IIO_VOLTAGE, NULL),
+	DA9150_GPADC_CHANNEL_PROCESSED(GPIOC, GPIOC_6V, IIO_VOLTAGE, NULL),
+	DA9150_GPADC_CHANNEL_PROCESSED(GPIOD, GPIOD_6V, IIO_VOLTAGE, NULL),
+	DA9150_GPADC_CHANNEL_PROCESSED(IBUS, IBUS_SENSE, IIO_CURRENT, "ibus"),
+	DA9150_GPADC_CHANNEL_PROCESSED(VBUS, VBUS_DIV_, IIO_VOLTAGE, "vbus"),
+	DA9150_GPADC_CHANNEL_PROCESSED(VSYS, VSYS, IIO_VOLTAGE, "vsys"),
+	DA9150_GPADC_CHANNEL_SCALED(VBAT, VBAT, IIO_VOLTAGE, "vbat"),
+	DA9150_GPADC_CHANNEL_RAW(TBAT, TBAT, IIO_VOLTAGE, "tbat"),
+	DA9150_GPADC_CHANNEL_SCALED(TJUNC_CORE, TJUNC_CORE, IIO_TEMP,
+				    "tjunc_core"),
+	DA9150_GPADC_CHANNEL_SCALED(TJUNC_OVP, TJUNC_OVP, IIO_TEMP,
+				    "tjunc_ovp"),
+};
+
+/* Default maps used by da9150-charger */
+static struct iio_map da9150_gpadc_default_maps[] = {
+	{
+		.consumer_dev_name = "da9150-charger",
+		.consumer_channel = "CHAN_IBUS",
+		.adc_channel_label = "IBUS",
+	},
+	{
+		.consumer_dev_name = "da9150-charger",
+		.consumer_channel = "CHAN_VBUS",
+		.adc_channel_label = "VBUS",
+	},
+	{
+		.consumer_dev_name = "da9150-charger",
+		.consumer_channel = "CHAN_TJUNC",
+		.adc_channel_label = "TJUNC_CORE",
+	},
+	{
+		.consumer_dev_name = "da9150-charger",
+		.consumer_channel = "CHAN_VBAT",
+		.adc_channel_label = "VBAT",
+	},
+	{},
+};
+
+static int da9150_gpadc_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct da9150 *da9150 = dev_get_drvdata(dev->parent);
+	struct da9150_gpadc *gpadc;
+	struct iio_dev *indio_dev;
+	int irq, ret;
+
+	indio_dev = devm_iio_device_alloc(dev, sizeof(*gpadc));
+	if (!indio_dev) {
+		dev_err(&pdev->dev, "Failed to allocate IIO device\n");
+		return -ENOMEM;
+	}
+	gpadc = iio_priv(indio_dev);
+
+	platform_set_drvdata(pdev, indio_dev);
+	gpadc->da9150 = da9150;
+	gpadc->dev = dev;
+	mutex_init(&gpadc->lock);
+	init_completion(&gpadc->complete);
+
+	irq = platform_get_irq_byname(pdev, "GPADC");
+	if (irq < 0) {
+		dev_err(dev, "Failed to get IRQ: %d\n", irq);
+		return irq;
+	}
+
+	ret = devm_request_threaded_irq(dev, irq, NULL, da9150_gpadc_irq,
+					IRQF_ONESHOT, "GPADC", gpadc);
+	if (ret) {
+		dev_err(dev, "Failed to request IRQ %d: %d\n", irq, ret);
+		return ret;
+	}
+
+	ret = iio_map_array_register(indio_dev, da9150_gpadc_default_maps);
+	if (ret) {
+		dev_err(dev, "Failed to register IIO maps: %d\n", ret);
+		return ret;
+	}
+
+	indio_dev->name = dev_name(dev);
+	indio_dev->dev.parent = dev;
+	indio_dev->dev.of_node = pdev->dev.of_node;
+	indio_dev->info = &da9150_gpadc_info;
+	indio_dev->modes = INDIO_DIRECT_MODE;
+	indio_dev->channels = da9150_gpadc_channels;
+	indio_dev->num_channels = ARRAY_SIZE(da9150_gpadc_channels);
+
+	ret = iio_device_register(indio_dev);
+	if (ret) {
+		dev_err(dev, "Failed to register IIO device: %d\n", ret);
+		goto iio_map_unreg;
+	}
+
+	return 0;
+
+iio_map_unreg:
+	iio_map_array_unregister(indio_dev);
+
+	return ret;
+}
+
+static int da9150_gpadc_remove(struct platform_device *pdev)
+{
+	struct iio_dev *indio_dev = platform_get_drvdata(pdev);
+
+	iio_device_unregister(indio_dev);
+	iio_map_array_unregister(indio_dev);
+
+	return 0;
+}
+
+static struct platform_driver da9150_gpadc_driver = {
+	.driver = {
+		.name = "da9150-gpadc",
+	},
+	.probe = da9150_gpadc_probe,
+	.remove = da9150_gpadc_remove,
+};
+
+module_platform_driver(da9150_gpadc_driver);
+
+MODULE_DESCRIPTION("GPADC Driver for DA9150");
+MODULE_AUTHOR("Adam Thomson <Adam.Thomson.Opensource@diasemi.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/iio/adc/mcp3422.c b/drivers/iio/adc/mcp3422.c
index 51672256072b..b96c636470ef 100644
--- a/drivers/iio/adc/mcp3422.c
+++ b/drivers/iio/adc/mcp3422.c
@@ -58,20 +58,11 @@
 		.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SAMP_FREQ), \
 	}
 
-/* LSB is in nV to eliminate floating point */
-static const u32 rates_to_lsb[] = {1000000, 250000, 62500, 15625};
-
-/*
- *  scales calculated as:
- *  rates_to_lsb[sample_rate] / (1 << pga);
- *  pga is 1 for 0, 2
- */
-
 static const int mcp3422_scales[4][4] = {
-	{ 1000000, 250000, 62500, 15625 },
-	{ 500000 , 125000, 31250, 7812 },
-	{ 250000 , 62500 , 15625, 3906 },
-	{ 125000 , 31250 , 7812 , 1953 } };
+	{ 1000000, 500000, 250000, 125000 },
+	{ 250000 , 125000, 62500 , 31250  },
+	{ 62500  , 31250 , 15625 , 7812   },
+	{ 15625  , 7812  , 3906  , 1953   } };
 
 /* Constant msleep times for data acquisitions */
 static const int mcp3422_read_times[4] = {
diff --git a/drivers/iio/adc/qcom-spmi-iadc.c b/drivers/iio/adc/qcom-spmi-iadc.c
index b9666f2f5e51..fabd24edc2a1 100644
--- a/drivers/iio/adc/qcom-spmi-iadc.c
+++ b/drivers/iio/adc/qcom-spmi-iadc.c
@@ -296,7 +296,8 @@ static int iadc_do_conversion(struct iadc_chip *iadc, int chan, u16 *data)
 	if (iadc->poll_eoc) {
 		ret = iadc_poll_wait_eoc(iadc, wait);
 	} else {
-		ret = wait_for_completion_timeout(&iadc->complete, wait);
+		ret = wait_for_completion_timeout(&iadc->complete,
+			usecs_to_jiffies(wait));
 		if (!ret)
 			ret = -ETIMEDOUT;
 		else
diff --git a/drivers/iio/adc/ti_am335x_adc.c b/drivers/iio/adc/ti_am335x_adc.c
index 2e5cc4409f78..a0e7161f040c 100644
--- a/drivers/iio/adc/ti_am335x_adc.c
+++ b/drivers/iio/adc/ti_am335x_adc.c
@@ -188,12 +188,11 @@ static int tiadc_buffer_preenable(struct iio_dev *indio_dev)
 static int tiadc_buffer_postenable(struct iio_dev *indio_dev)
 {
 	struct tiadc_device *adc_dev = iio_priv(indio_dev);
-	struct iio_buffer *buffer = indio_dev->buffer;
 	unsigned int enb = 0;
 	u8 bit;
 
 	tiadc_step_config(indio_dev);
-	for_each_set_bit(bit, buffer->scan_mask, adc_dev->channels)
+	for_each_set_bit(bit, indio_dev->active_scan_mask, adc_dev->channels)
 		enb |= (get_adc_step_bit(adc_dev, bit) << 1);
 	adc_dev->buffer_en_ch_steps = enb;
 
diff --git a/drivers/iio/adc/vf610_adc.c b/drivers/iio/adc/vf610_adc.c
index 8ec353c01d98..56292ae4538d 100644
--- a/drivers/iio/adc/vf610_adc.c
+++ b/drivers/iio/adc/vf610_adc.c
@@ -141,9 +141,13 @@ struct vf610_adc {
 	struct regulator *vref;
 	struct vf610_adc_feature adc_feature;
 
+	u32 sample_freq_avail[5];
+
 	struct completion completion;
 };
 
+static const u32 vf610_hw_avgs[] = { 1, 4, 8, 16, 32 };
+
 #define VF610_ADC_CHAN(_idx, _chan_type) {			\
 	.type = (_chan_type),					\
 	.indexed = 1,						\
@@ -180,35 +184,47 @@ static const struct iio_chan_spec vf610_adc_iio_channels[] = {
 	/* sentinel */
 };
 
-/*
- * ADC sample frequency, unit is ADCK cycles.
- * ADC clk source is ipg clock, which is the same as bus clock.
- *
- * ADC conversion time = SFCAdder + AverageNum x (BCT + LSTAdder)
- * SFCAdder: fixed to 6 ADCK cycles
- * AverageNum: 1, 4, 8, 16, 32 samples for hardware average.
- * BCT (Base Conversion Time): fixed to 25 ADCK cycles for 12 bit mode
- * LSTAdder(Long Sample Time): fixed to 3 ADCK cycles
- *
- * By default, enable 12 bit resolution mode, clock source
- * set to ipg clock, So get below frequency group:
- */
-static const u32 vf610_sample_freq_avail[5] =
-{1941176, 559332, 286957, 145374, 73171};
+static inline void vf610_adc_calculate_rates(struct vf610_adc *info)
+{
+	unsigned long adck_rate, ipg_rate = clk_get_rate(info->clk);
+	int i;
+
+	/*
+	 * Calculate ADC sample frequencies
+	 * Sample time unit is ADCK cycles. ADCK clk source is ipg clock,
+	 * which is the same as bus clock.
+	 *
+	 * ADC conversion time = SFCAdder + AverageNum x (BCT + LSTAdder)
+	 * SFCAdder: fixed to 6 ADCK cycles
+	 * AverageNum: 1, 4, 8, 16, 32 samples for hardware average.
+	 * BCT (Base Conversion Time): fixed to 25 ADCK cycles for 12 bit mode
+	 * LSTAdder(Long Sample Time): fixed to 3 ADCK cycles
+	 */
+	adck_rate = ipg_rate / info->adc_feature.clk_div;
+	for (i = 0; i < ARRAY_SIZE(vf610_hw_avgs); i++)
+		info->sample_freq_avail[i] =
+			adck_rate / (6 + vf610_hw_avgs[i] * (25 + 3));
+}
 
 static inline void vf610_adc_cfg_init(struct vf610_adc *info)
 {
+	struct vf610_adc_feature *adc_feature = &info->adc_feature;
+
 	/* set default Configuration for ADC controller */
-	info->adc_feature.clk_sel = VF610_ADCIOC_BUSCLK_SET;
-	info->adc_feature.vol_ref = VF610_ADCIOC_VR_VREF_SET;
+	adc_feature->clk_sel = VF610_ADCIOC_BUSCLK_SET;
+	adc_feature->vol_ref = VF610_ADCIOC_VR_VREF_SET;
+
+	adc_feature->calibration = true;
+	adc_feature->ovwren = true;
+
+	adc_feature->res_mode = 12;
+	adc_feature->sample_rate = 1;
+	adc_feature->lpm = true;
 
-	info->adc_feature.calibration = true;
-	info->adc_feature.ovwren = true;
+	/* Use a save ADCK which is below 20MHz on all devices */
+	adc_feature->clk_div = 8;
 
-	info->adc_feature.clk_div = 1;
-	info->adc_feature.res_mode = 12;
-	info->adc_feature.sample_rate = 1;
-	info->adc_feature.lpm = true;
+	vf610_adc_calculate_rates(info);
 }
 
 static void vf610_adc_cfg_post_set(struct vf610_adc *info)
@@ -259,7 +275,6 @@ static void vf610_adc_cfg_post_set(struct vf610_adc *info)
 static void vf610_adc_calibration(struct vf610_adc *info)
 {
 	int adc_gc, hc_cfg;
-	int timeout;
 
 	if (!info->adc_feature.calibration)
 		return;
@@ -271,9 +286,7 @@ static void vf610_adc_calibration(struct vf610_adc *info)
 	adc_gc = readl(info->regs + VF610_REG_ADC_GC);
 	writel(adc_gc | VF610_ADC_CAL, info->regs + VF610_REG_ADC_GC);
 
-	timeout = wait_for_completion_timeout
-			(&info->completion, VF610_ADC_TIMEOUT);
-	if (timeout == 0)
+	if (!wait_for_completion_timeout(&info->completion, VF610_ADC_TIMEOUT))
 		dev_err(info->dev, "Timeout for adc calibration\n");
 
 	adc_gc = readl(info->regs + VF610_REG_ADC_GS);
@@ -290,12 +303,10 @@ static void vf610_adc_cfg_set(struct vf610_adc *info)
 
 	cfg_data = readl(info->regs + VF610_REG_ADC_CFG);
 
-	/* low power configuration */
 	cfg_data &= ~VF610_ADC_ADLPC_EN;
 	if (adc_feature->lpm)
 		cfg_data |= VF610_ADC_ADLPC_EN;
 
-	/* disable high speed */
 	cfg_data &= ~VF610_ADC_ADHSC_EN;
 
 	writel(cfg_data, info->regs + VF610_REG_ADC_CFG);
@@ -435,10 +446,27 @@ static irqreturn_t vf610_adc_isr(int irq, void *dev_id)
 	return IRQ_HANDLED;
 }
 
-static IIO_CONST_ATTR_SAMP_FREQ_AVAIL("1941176, 559332, 286957, 145374, 73171");
+static ssize_t vf610_show_samp_freq_avail(struct device *dev,
+				struct device_attribute *attr, char *buf)
+{
+	struct vf610_adc *info = iio_priv(dev_to_iio_dev(dev));
+	size_t len = 0;
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(info->sample_freq_avail); i++)
+		len += scnprintf(buf + len, PAGE_SIZE - len,
+			"%u ", info->sample_freq_avail[i]);
+
+	/* replace trailing space by newline */
+	buf[len - 1] = '\n';
+
+	return len;
+}
+
+static IIO_DEV_ATTR_SAMP_FREQ_AVAIL(vf610_show_samp_freq_avail);
 
 static struct attribute *vf610_attributes[] = {
-	&iio_const_attr_sampling_frequency_available.dev_attr.attr,
+	&iio_dev_attr_sampling_frequency_available.dev_attr.attr,
 	NULL
 };
 
@@ -502,7 +530,7 @@ static int vf610_read_raw(struct iio_dev *indio_dev,
 		return IIO_VAL_FRACTIONAL_LOG2;
 
 	case IIO_CHAN_INFO_SAMP_FREQ:
-		*val = vf610_sample_freq_avail[info->adc_feature.sample_rate];
+		*val = info->sample_freq_avail[info->adc_feature.sample_rate];
 		*val2 = 0;
 		return IIO_VAL_INT;
 
@@ -525,9 +553,9 @@ static int vf610_write_raw(struct iio_dev *indio_dev,
 	switch (mask) {
 		case IIO_CHAN_INFO_SAMP_FREQ:
 			for (i = 0;
-				i < ARRAY_SIZE(vf610_sample_freq_avail);
+				i < ARRAY_SIZE(info->sample_freq_avail);
 				i++)
-				if (val == vf610_sample_freq_avail[i]) {
+				if (val == info->sample_freq_avail[i]) {
 					info->adc_feature.sample_rate = i;
 					vf610_adc_sample_set(info);
 					return 0;
diff --git a/drivers/iio/common/ssp_sensors/ssp_dev.c b/drivers/iio/common/ssp_sensors/ssp_dev.c
index 52d70435f5a1..9a40097e7cf8 100644
--- a/drivers/iio/common/ssp_sensors/ssp_dev.c
+++ b/drivers/iio/common/ssp_sensors/ssp_dev.c
@@ -437,7 +437,7 @@ int ssp_queue_ssp_refresh_task(struct ssp_data *data, unsigned int delay)
 }
 
 #ifdef CONFIG_OF
-static struct of_device_id ssp_of_match[] = {
+static const struct of_device_id ssp_of_match[] = {
 	{
 		.compatible	= "samsung,sensorhub-rinato",
 		.data		= &ssp_rinato_info,
@@ -640,6 +640,7 @@ static int ssp_remove(struct spi_device *spi)
 	return 0;
 }
 
+#ifdef CONFIG_PM_SLEEP
 static int ssp_suspend(struct device *dev)
 {
 	int ret;
@@ -688,6 +689,7 @@ static int ssp_resume(struct device *dev)
 
 	return 0;
 }
+#endif /* CONFIG_PM_SLEEP */
 
 static const struct dev_pm_ops ssp_pm_ops = {
 	SET_SYSTEM_SLEEP_PM_OPS(ssp_suspend, ssp_resume)
diff --git a/drivers/iio/dac/Kconfig b/drivers/iio/dac/Kconfig
index 2236ea22f98a..13471a76e5bf 100644
--- a/drivers/iio/dac/Kconfig
+++ b/drivers/iio/dac/Kconfig
@@ -143,11 +143,16 @@ config AD7303
 	  ad7303.
 
 config MAX517
-	tristate "Maxim MAX517/518/519 DAC driver"
+	tristate "Maxim MAX517/518/519/520/521 DAC driver"
 	depends on I2C
 	help
-	  If you say yes here you get support for the Maxim chips MAX517,
-	  MAX518 and MAX519 (I2C 8-Bit DACs with rail-to-rail outputs).
+	  If you say yes here you get support for the following Maxim chips
+	  (I2C 8-Bit DACs with rail-to-rail outputs):
+	  MAX517 - Single channel, single reference
+	  MAX518 - Dual channel, ref=Vdd
+	  MAX519 - Dual channel, dual reference
+	  MAX520 - Quad channel, quad reference
+	  MAX521 - Octal channel, independent ref for ch0-3, shared ref for ch4-7
 
 	  This driver can also be built as a module.  If so, the module
 	  will be called max517.
diff --git a/drivers/iio/dac/ad5686.c b/drivers/iio/dac/ad5686.c
index f57562aa396f..15c73e20272d 100644
--- a/drivers/iio/dac/ad5686.c
+++ b/drivers/iio/dac/ad5686.c
@@ -322,7 +322,7 @@ static int ad5686_probe(struct spi_device *spi)
 	st = iio_priv(indio_dev);
 	spi_set_drvdata(spi, indio_dev);
 
-	st->reg = devm_regulator_get(&spi->dev, "vcc");
+	st->reg = devm_regulator_get_optional(&spi->dev, "vcc");
 	if (!IS_ERR(st->reg)) {
 		ret = regulator_enable(st->reg);
 		if (ret)
diff --git a/drivers/iio/dac/max517.c b/drivers/iio/dac/max517.c
index 9a82a7255ebb..5507b3970b4b 100644
--- a/drivers/iio/dac/max517.c
+++ b/drivers/iio/dac/max517.c
@@ -39,11 +39,13 @@ enum max517_device_ids {
 	ID_MAX517,
 	ID_MAX518,
 	ID_MAX519,
+	ID_MAX520,
+	ID_MAX521,
 };
 
 struct max517_data {
 	struct i2c_client	*client;
-	unsigned short		vref_mv[2];
+	unsigned short		vref_mv[8];
 };
 
 /*
@@ -149,7 +151,13 @@ static const struct iio_info max517_info = {
 
 static const struct iio_chan_spec max517_channels[] = {
 	MAX517_CHANNEL(0),
-	MAX517_CHANNEL(1)
+	MAX517_CHANNEL(1),
+	MAX517_CHANNEL(2),
+	MAX517_CHANNEL(3),
+	MAX517_CHANNEL(4),
+	MAX517_CHANNEL(5),
+	MAX517_CHANNEL(6),
+	MAX517_CHANNEL(7),
 };
 
 static int max517_probe(struct i2c_client *client,
@@ -158,6 +166,7 @@ static int max517_probe(struct i2c_client *client,
 	struct max517_data *data;
 	struct iio_dev *indio_dev;
 	struct max517_platform_data *platform_data = client->dev.platform_data;
+	int chan;
 
 	indio_dev = devm_iio_device_alloc(&client->dev, sizeof(*data));
 	if (!indio_dev)
@@ -169,11 +178,21 @@ static int max517_probe(struct i2c_client *client,
 	/* establish that the iio_dev is a child of the i2c device */
 	indio_dev->dev.parent = &client->dev;
 
-	/* reduced channel set for MAX517 */
-	if (id->driver_data == ID_MAX517)
-		indio_dev->num_channels = 1;
-	else
+	switch (id->driver_data) {
+	case ID_MAX521:
+		indio_dev->num_channels = 8;
+		break;
+	case ID_MAX520:
+		indio_dev->num_channels = 4;
+		break;
+	case ID_MAX519:
+	case ID_MAX518:
 		indio_dev->num_channels = 2;
+		break;
+	default:  /* single channel for MAX517 */
+		indio_dev->num_channels = 1;
+		break;
+	}
 	indio_dev->channels = max517_channels;
 	indio_dev->modes = INDIO_DIRECT_MODE;
 	indio_dev->info = &max517_info;
@@ -182,11 +201,11 @@ static int max517_probe(struct i2c_client *client,
 	 * Reference voltage on MAX518 and default is 5V, else take vref_mv
 	 * from platform_data
 	 */
-	if (id->driver_data == ID_MAX518 || !platform_data) {
-		data->vref_mv[0] = data->vref_mv[1] = 5000; /* mV */
-	} else {
-		data->vref_mv[0] = platform_data->vref_mv[0];
-		data->vref_mv[1] = platform_data->vref_mv[1];
+	for (chan = 0; chan < indio_dev->num_channels; chan++) {
+		if (id->driver_data == ID_MAX518 || !platform_data)
+			data->vref_mv[chan] = 5000; /* mV */
+		else
+			data->vref_mv[chan] = platform_data->vref_mv[chan];
 	}
 
 	return iio_device_register(indio_dev);
@@ -202,6 +221,8 @@ static const struct i2c_device_id max517_id[] = {
 	{ "max517", ID_MAX517 },
 	{ "max518", ID_MAX518 },
 	{ "max519", ID_MAX519 },
+	{ "max520", ID_MAX520 },
+	{ "max521", ID_MAX521 },
 	{ }
 };
 MODULE_DEVICE_TABLE(i2c, max517_id);
@@ -218,5 +239,5 @@ static struct i2c_driver max517_driver = {
 module_i2c_driver(max517_driver);
 
 MODULE_AUTHOR("Roland Stigge <stigge@antcom.de>");
-MODULE_DESCRIPTION("MAX517/MAX518/MAX519 8-bit DAC");
+MODULE_DESCRIPTION("MAX517/518/519/520/521 8-bit DAC");
 MODULE_LICENSE("GPL");
diff --git a/drivers/iio/gyro/bmg160.c b/drivers/iio/gyro/bmg160.c
index 60451b328242..4415f55d26b6 100644
--- a/drivers/iio/gyro/bmg160.c
+++ b/drivers/iio/gyro/bmg160.c
@@ -822,7 +822,7 @@ static irqreturn_t bmg160_trigger_handler(int irq, void *p)
 	int bit, ret, i = 0;
 
 	mutex_lock(&data->mutex);
-	for_each_set_bit(bit, indio_dev->buffer->scan_mask,
+	for_each_set_bit(bit, indio_dev->active_scan_mask,
 			 indio_dev->masklength) {
 		ret = i2c_smbus_read_word_data(data->client,
 					       BMG160_AXIS_TO_REG(bit));
@@ -1001,16 +1001,12 @@ static int bmg160_gpio_probe(struct i2c_client *client,
 	dev = &client->dev;
 
 	/* data ready gpio interrupt pin */
-	gpio = devm_gpiod_get_index(dev, BMG160_GPIO_NAME, 0);
+	gpio = devm_gpiod_get_index(dev, BMG160_GPIO_NAME, 0, GPIOD_IN);
 	if (IS_ERR(gpio)) {
 		dev_err(dev, "acpi gpio get index failed\n");
 		return PTR_ERR(gpio);
 	}
 
-	ret = gpiod_direction_input(gpio);
-	if (ret)
-		return ret;
-
 	ret = gpiod_to_irq(gpio);
 
 	dev_dbg(dev, "GPIO resource, no:%d irq:%d\n", desc_to_gpio(gpio), ret);
diff --git a/drivers/iio/gyro/itg3200_core.c b/drivers/iio/gyro/itg3200_core.c
index 6a8020d48140..f0fd94055d88 100644
--- a/drivers/iio/gyro/itg3200_core.c
+++ b/drivers/iio/gyro/itg3200_core.c
@@ -223,6 +223,10 @@ static int itg3200_initial_setup(struct iio_dev *indio_dev)
 	int ret;
 	u8 val;
 
+	ret = itg3200_reset(indio_dev);
+	if (ret)
+		goto err_ret;
+
 	ret = itg3200_read_reg_8(indio_dev, ITG3200_REG_ADDRESS, &val);
 	if (ret)
 		goto err_ret;
@@ -233,10 +237,6 @@ static int itg3200_initial_setup(struct iio_dev *indio_dev)
 		goto err_ret;
 	}
 
-	ret = itg3200_reset(indio_dev);
-	if (ret)
-		goto err_ret;
-
 	ret = itg3200_enable_full_scale(indio_dev);
 err_ret:
 	return ret;
@@ -351,6 +351,26 @@ static int itg3200_remove(struct i2c_client *client)
 	return 0;
 }
 
+static int __maybe_unused itg3200_suspend(struct device *dev)
+{
+	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct itg3200 *st = iio_priv(indio_dev);
+
+	dev_dbg(&st->i2c->dev, "suspend device");
+
+	return itg3200_write_reg_8(indio_dev, ITG3200_REG_POWER_MANAGEMENT,
+				   ITG3200_SLEEP);
+}
+
+static int __maybe_unused itg3200_resume(struct device *dev)
+{
+	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+
+	return itg3200_initial_setup(indio_dev);
+}
+
+static SIMPLE_DEV_PM_OPS(itg3200_pm_ops, itg3200_suspend, itg3200_resume);
+
 static const struct i2c_device_id itg3200_id[] = {
 	{ "itg3200", 0 },
 	{ }
@@ -361,6 +381,7 @@ static struct i2c_driver itg3200_driver = {
 	.driver = {
 		.owner  = THIS_MODULE,
 		.name	= "itg3200",
+		.pm	= &itg3200_pm_ops,
 	},
 	.id_table	= itg3200_id,
 	.probe		= itg3200_probe,
diff --git a/drivers/iio/gyro/st_gyro_core.c b/drivers/iio/gyro/st_gyro_core.c
index f07a2336f7dc..21395f26d227 100644
--- a/drivers/iio/gyro/st_gyro_core.c
+++ b/drivers/iio/gyro/st_gyro_core.c
@@ -87,6 +87,31 @@
 #define ST_GYRO_2_DRDY_IRQ_INT2_MASK		0x08
 #define ST_GYRO_2_MULTIREAD_BIT			true
 
+/* CUSTOM VALUES FOR SENSOR 3 */
+#define ST_GYRO_3_WAI_EXP			0xd7
+#define ST_GYRO_3_ODR_ADDR			0x20
+#define ST_GYRO_3_ODR_MASK			0xc0
+#define ST_GYRO_3_ODR_AVL_95HZ_VAL		0x00
+#define ST_GYRO_3_ODR_AVL_190HZ_VAL		0x01
+#define ST_GYRO_3_ODR_AVL_380HZ_VAL		0x02
+#define ST_GYRO_3_ODR_AVL_760HZ_VAL		0x03
+#define ST_GYRO_3_PW_ADDR			0x20
+#define ST_GYRO_3_PW_MASK			0x08
+#define ST_GYRO_3_FS_ADDR			0x23
+#define ST_GYRO_3_FS_MASK			0x30
+#define ST_GYRO_3_FS_AVL_250_VAL		0x00
+#define ST_GYRO_3_FS_AVL_500_VAL		0x01
+#define ST_GYRO_3_FS_AVL_2000_VAL		0x02
+#define ST_GYRO_3_FS_AVL_250_GAIN		IIO_DEGREE_TO_RAD(8750)
+#define ST_GYRO_3_FS_AVL_500_GAIN		IIO_DEGREE_TO_RAD(17500)
+#define ST_GYRO_3_FS_AVL_2000_GAIN		IIO_DEGREE_TO_RAD(70000)
+#define ST_GYRO_3_BDU_ADDR			0x23
+#define ST_GYRO_3_BDU_MASK			0x80
+#define ST_GYRO_3_DRDY_IRQ_ADDR			0x22
+#define ST_GYRO_3_DRDY_IRQ_INT2_MASK		0x08
+#define ST_GYRO_3_MULTIREAD_BIT			true
+
+
 static const struct iio_chan_spec st_gyro_16bit_channels[] = {
 	ST_SENSORS_LSM_CHANNELS(IIO_ANGL_VEL,
 			BIT(IIO_CHAN_INFO_RAW) | BIT(IIO_CHAN_INFO_SCALE),
@@ -225,6 +250,64 @@ static const struct st_sensor_settings st_gyro_sensors_settings[] = {
 		.multi_read_bit = ST_GYRO_2_MULTIREAD_BIT,
 		.bootime = 2,
 	},
+	{
+		.wai = ST_GYRO_3_WAI_EXP,
+		.sensors_supported = {
+			[0] = L3GD20_GYRO_DEV_NAME,
+		},
+		.ch = (struct iio_chan_spec *)st_gyro_16bit_channels,
+		.odr = {
+			.addr = ST_GYRO_3_ODR_ADDR,
+			.mask = ST_GYRO_3_ODR_MASK,
+			.odr_avl = {
+				{ 95, ST_GYRO_3_ODR_AVL_95HZ_VAL, },
+				{ 190, ST_GYRO_3_ODR_AVL_190HZ_VAL, },
+				{ 380, ST_GYRO_3_ODR_AVL_380HZ_VAL, },
+				{ 760, ST_GYRO_3_ODR_AVL_760HZ_VAL, },
+			},
+		},
+		.pw = {
+			.addr = ST_GYRO_3_PW_ADDR,
+			.mask = ST_GYRO_3_PW_MASK,
+			.value_on = ST_SENSORS_DEFAULT_POWER_ON_VALUE,
+			.value_off = ST_SENSORS_DEFAULT_POWER_OFF_VALUE,
+		},
+		.enable_axis = {
+			.addr = ST_SENSORS_DEFAULT_AXIS_ADDR,
+			.mask = ST_SENSORS_DEFAULT_AXIS_MASK,
+		},
+		.fs = {
+			.addr = ST_GYRO_3_FS_ADDR,
+			.mask = ST_GYRO_3_FS_MASK,
+			.fs_avl = {
+				[0] = {
+					.num = ST_GYRO_FS_AVL_250DPS,
+					.value = ST_GYRO_3_FS_AVL_250_VAL,
+					.gain = ST_GYRO_3_FS_AVL_250_GAIN,
+				},
+				[1] = {
+					.num = ST_GYRO_FS_AVL_500DPS,
+					.value = ST_GYRO_3_FS_AVL_500_VAL,
+					.gain = ST_GYRO_3_FS_AVL_500_GAIN,
+				},
+				[2] = {
+					.num = ST_GYRO_FS_AVL_2000DPS,
+					.value = ST_GYRO_3_FS_AVL_2000_VAL,
+					.gain = ST_GYRO_3_FS_AVL_2000_GAIN,
+				},
+			},
+		},
+		.bdu = {
+			.addr = ST_GYRO_3_BDU_ADDR,
+			.mask = ST_GYRO_3_BDU_MASK,
+		},
+		.drdy_irq = {
+			.addr = ST_GYRO_3_DRDY_IRQ_ADDR,
+			.mask_int2 = ST_GYRO_3_DRDY_IRQ_INT2_MASK,
+		},
+		.multi_read_bit = ST_GYRO_3_MULTIREAD_BIT,
+		.bootime = 2,
+	},
 };
 
 static int st_gyro_read_raw(struct iio_dev *indio_dev,
diff --git a/drivers/iio/humidity/dht11.c b/drivers/iio/humidity/dht11.c
index 623c145d8a97..7d79a1ac5f5f 100644
--- a/drivers/iio/humidity/dht11.c
+++ b/drivers/iio/humidity/dht11.c
@@ -29,6 +29,7 @@
 #include <linux/wait.h>
 #include <linux/bitops.h>
 #include <linux/completion.h>
+#include <linux/mutex.h>
 #include <linux/delay.h>
 #include <linux/gpio.h>
 #include <linux/of_gpio.h>
@@ -39,8 +40,12 @@
 
 #define DHT11_DATA_VALID_TIME	2000000000  /* 2s in ns */
 
-#define DHT11_EDGES_PREAMBLE 4
+#define DHT11_EDGES_PREAMBLE 2
 #define DHT11_BITS_PER_READ 40
+/*
+ * Note that when reading the sensor actually 84 edges are detected, but
+ * since the last edge is not significant, we only store 83:
+ */
 #define DHT11_EDGES_PER_READ (2*DHT11_BITS_PER_READ + DHT11_EDGES_PREAMBLE + 1)
 
 /* Data transmission timing (nano seconds) */
@@ -57,6 +62,7 @@ struct dht11 {
 	int				irq;
 
 	struct completion		completion;
+	struct mutex			lock;
 
 	s64				timestamp;
 	int				temperature;
@@ -88,7 +94,7 @@ static int dht11_decode(struct dht11 *dht11, int offset)
 	unsigned char temp_int, temp_dec, hum_int, hum_dec, checksum;
 
 	/* Calculate timestamp resolution */
-	for (i = 0; i < dht11->num_edges; ++i) {
+	for (i = 1; i < dht11->num_edges; ++i) {
 		t = dht11->edges[i].ts - dht11->edges[i-1].ts;
 		if (t > 0 && t < timeres)
 			timeres = t;
@@ -138,6 +144,27 @@ static int dht11_decode(struct dht11 *dht11, int offset)
 	return 0;
 }
 
+/*
+ * IRQ handler called on GPIO edges
+ */
+static irqreturn_t dht11_handle_irq(int irq, void *data)
+{
+	struct iio_dev *iio = data;
+	struct dht11 *dht11 = iio_priv(iio);
+
+	/* TODO: Consider making the handler safe for IRQ sharing */
+	if (dht11->num_edges < DHT11_EDGES_PER_READ && dht11->num_edges >= 0) {
+		dht11->edges[dht11->num_edges].ts = iio_get_time_ns();
+		dht11->edges[dht11->num_edges++].value =
+						gpio_get_value(dht11->gpio);
+
+		if (dht11->num_edges >= DHT11_EDGES_PER_READ)
+			complete(&dht11->completion);
+	}
+
+	return IRQ_HANDLED;
+}
+
 static int dht11_read_raw(struct iio_dev *iio_dev,
 			const struct iio_chan_spec *chan,
 			int *val, int *val2, long m)
@@ -145,6 +172,7 @@ static int dht11_read_raw(struct iio_dev *iio_dev,
 	struct dht11 *dht11 = iio_priv(iio_dev);
 	int ret;
 
+	mutex_lock(&dht11->lock);
 	if (dht11->timestamp + DHT11_DATA_VALID_TIME < iio_get_time_ns()) {
 		reinit_completion(&dht11->completion);
 
@@ -157,8 +185,17 @@ static int dht11_read_raw(struct iio_dev *iio_dev,
 		if (ret)
 			goto err;
 
+		ret = request_irq(dht11->irq, dht11_handle_irq,
+				  IRQF_TRIGGER_RISING | IRQF_TRIGGER_FALLING,
+				  iio_dev->name, iio_dev);
+		if (ret)
+			goto err;
+
 		ret = wait_for_completion_killable_timeout(&dht11->completion,
 								 HZ);
+
+		free_irq(dht11->irq, iio_dev);
+
 		if (ret == 0 && dht11->num_edges < DHT11_EDGES_PER_READ - 1) {
 			dev_err(&iio_dev->dev,
 					"Only %d signal edges detected\n",
@@ -185,6 +222,7 @@ static int dht11_read_raw(struct iio_dev *iio_dev,
 		ret = -EINVAL;
 err:
 	dht11->num_edges = -1;
+	mutex_unlock(&dht11->lock);
 	return ret;
 }
 
@@ -193,27 +231,6 @@ static const struct iio_info dht11_iio_info = {
 	.read_raw		= dht11_read_raw,
 };
 
-/*
- * IRQ handler called on GPIO edges
-*/
-static irqreturn_t dht11_handle_irq(int irq, void *data)
-{
-	struct iio_dev *iio = data;
-	struct dht11 *dht11 = iio_priv(iio);
-
-	/* TODO: Consider making the handler safe for IRQ sharing */
-	if (dht11->num_edges < DHT11_EDGES_PER_READ && dht11->num_edges >= 0) {
-		dht11->edges[dht11->num_edges].ts = iio_get_time_ns();
-		dht11->edges[dht11->num_edges++].value =
-						gpio_get_value(dht11->gpio);
-
-		if (dht11->num_edges >= DHT11_EDGES_PER_READ)
-			complete(&dht11->completion);
-	}
-
-	return IRQ_HANDLED;
-}
-
 static const struct iio_chan_spec dht11_chan_spec[] = {
 	{ .type = IIO_TEMP,
 		.info_mask_separate = BIT(IIO_CHAN_INFO_PROCESSED), },
@@ -256,11 +273,6 @@ static int dht11_probe(struct platform_device *pdev)
 		dev_err(dev, "GPIO %d has no interrupt\n", dht11->gpio);
 		return -EINVAL;
 	}
-	ret = devm_request_irq(dev, dht11->irq, dht11_handle_irq,
-				IRQF_TRIGGER_RISING | IRQF_TRIGGER_FALLING,
-				pdev->name, iio);
-	if (ret)
-		return ret;
 
 	dht11->timestamp = iio_get_time_ns() - DHT11_DATA_VALID_TIME - 1;
 	dht11->num_edges = -1;
@@ -268,6 +280,7 @@ static int dht11_probe(struct platform_device *pdev)
 	platform_set_drvdata(pdev, iio);
 
 	init_completion(&dht11->completion);
+	mutex_init(&dht11->lock);
 	iio->name = pdev->name;
 	iio->dev.parent = &pdev->dev;
 	iio->info = &dht11_iio_info;
diff --git a/drivers/iio/humidity/si7020.c b/drivers/iio/humidity/si7020.c
index b54164677b89..fa3b809aff5e 100644
--- a/drivers/iio/humidity/si7020.c
+++ b/drivers/iio/humidity/si7020.c
@@ -45,12 +45,12 @@ static int si7020_read_raw(struct iio_dev *indio_dev,
 			   struct iio_chan_spec const *chan, int *val,
 			   int *val2, long mask)
 {
-	struct i2c_client *client = iio_priv(indio_dev);
+	struct i2c_client **client = iio_priv(indio_dev);
 	int ret;
 
 	switch (mask) {
 	case IIO_CHAN_INFO_RAW:
-		ret = i2c_smbus_read_word_data(client,
+		ret = i2c_smbus_read_word_data(*client,
 					       chan->type == IIO_TEMP ?
 					       SI7020CMD_TEMP_HOLD :
 					       SI7020CMD_RH_HOLD);
@@ -126,7 +126,7 @@ static int si7020_probe(struct i2c_client *client,
 	/* Wait the maximum power-up time after software reset. */
 	msleep(15);
 
-	indio_dev = devm_iio_device_alloc(&client->dev, sizeof(*client));
+	indio_dev = devm_iio_device_alloc(&client->dev, sizeof(*data));
 	if (!indio_dev)
 		return -ENOMEM;
 
diff --git a/drivers/iio/imu/adis16400_core.c b/drivers/iio/imu/adis16400_core.c
index b70873de04ea..fa795dcd5f75 100644
--- a/drivers/iio/imu/adis16400_core.c
+++ b/drivers/iio/imu/adis16400_core.c
@@ -26,6 +26,7 @@
 #include <linux/list.h>
 #include <linux/module.h>
 #include <linux/debugfs.h>
+#include <linux/bitops.h>
 
 #include <linux/iio/iio.h>
 #include <linux/iio/sysfs.h>
@@ -414,7 +415,7 @@ static int adis16400_read_raw(struct iio_dev *indio_dev,
 		mutex_unlock(&indio_dev->mlock);
 		if (ret)
 			return ret;
-		val16 = ((val16 & 0xFFF) << 4) >> 4;
+		val16 = sign_extend32(val16, 11);
 		*val = val16;
 		return IIO_VAL_INT;
 	case IIO_CHAN_INFO_OFFSET:
diff --git a/drivers/iio/imu/adis_trigger.c b/drivers/iio/imu/adis_trigger.c
index e0017c22bb9c..f53e9a803a0e 100644
--- a/drivers/iio/imu/adis_trigger.c
+++ b/drivers/iio/imu/adis_trigger.c
@@ -60,7 +60,7 @@ int adis_probe_trigger(struct adis *adis, struct iio_dev *indio_dev)
 	iio_trigger_set_drvdata(adis->trig, adis);
 	ret = iio_trigger_register(adis->trig);
 
-	indio_dev->trig = adis->trig;
+	indio_dev->trig = iio_trigger_get(adis->trig);
 	if (ret)
 		goto error_free_irq;
 
diff --git a/drivers/iio/imu/inv_mpu6050/Makefile b/drivers/iio/imu/inv_mpu6050/Makefile
index 3a677c778afb..f566f6a7b3a9 100644
--- a/drivers/iio/imu/inv_mpu6050/Makefile
+++ b/drivers/iio/imu/inv_mpu6050/Makefile
@@ -3,4 +3,4 @@
 #
 
 obj-$(CONFIG_INV_MPU6050_IIO) += inv-mpu6050.o
-inv-mpu6050-objs := inv_mpu_core.o inv_mpu_ring.o inv_mpu_trigger.o
+inv-mpu6050-objs := inv_mpu_core.o inv_mpu_ring.o inv_mpu_trigger.o inv_mpu_acpi.o
diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_acpi.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_acpi.c
new file mode 100644
index 000000000000..1c982a56acd5
--- /dev/null
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_acpi.c
@@ -0,0 +1,211 @@
+/*
+ * inv_mpu_acpi: ACPI processing for creating client devices
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ */
+
+#ifdef CONFIG_ACPI
+
+#include <linux/kernel.h>
+#include <linux/i2c.h>
+#include <linux/dmi.h>
+#include <linux/acpi.h>
+#include "inv_mpu_iio.h"
+
+enum inv_mpu_product_name {
+	INV_MPU_NOT_MATCHED,
+	INV_MPU_ASUS_T100TA,
+};
+
+static enum inv_mpu_product_name matched_product_name;
+
+static int __init asus_t100_matched(const struct dmi_system_id *d)
+{
+	matched_product_name = INV_MPU_ASUS_T100TA;
+
+	return 0;
+}
+
+static const struct dmi_system_id inv_mpu_dev_list[] = {
+	{
+	.callback = asus_t100_matched,
+	.ident = "Asus Transformer Book T100",
+		.matches = {
+			DMI_MATCH(DMI_SYS_VENDOR, "ASUSTeK COMPUTER INC"),
+			DMI_MATCH(DMI_PRODUCT_NAME, "T100TA"),
+			DMI_MATCH(DMI_PRODUCT_VERSION, "1.0"),
+		},
+	},
+	/* Add more matching tables here..*/
+	{}
+};
+
+static int asus_acpi_get_sensor_info(struct acpi_device *adev,
+				     struct i2c_client *client,
+				     struct i2c_board_info *info)
+{
+	struct acpi_buffer buffer = {ACPI_ALLOCATE_BUFFER, NULL};
+	int i;
+	acpi_status status;
+	union acpi_object *cpm;
+
+	status = acpi_evaluate_object(adev->handle, "CNF0", NULL, &buffer);
+	if (ACPI_FAILURE(status))
+		return -ENODEV;
+
+	cpm = buffer.pointer;
+	for (i = 0; i < cpm->package.count; ++i) {
+		union acpi_object *elem;
+		int j;
+
+		elem = &(cpm->package.elements[i]);
+		for (j = 0; j < elem->package.count; ++j) {
+			union acpi_object *sub_elem;
+
+			sub_elem = &(elem->package.elements[j]);
+			if (sub_elem->type == ACPI_TYPE_STRING)
+				strlcpy(info->type, sub_elem->string.pointer,
+					sizeof(info->type));
+			else if (sub_elem->type == ACPI_TYPE_INTEGER) {
+				if (sub_elem->integer.value != client->addr) {
+					info->addr = sub_elem->integer.value;
+					break; /* Not a MPU6500 primary */
+				}
+			}
+		}
+	}
+
+	kfree(buffer.pointer);
+
+	return cpm->package.count;
+}
+
+static int acpi_i2c_check_resource(struct acpi_resource *ares, void *data)
+{
+	u32 *addr = data;
+
+	if (ares->type == ACPI_RESOURCE_TYPE_SERIAL_BUS) {
+		struct acpi_resource_i2c_serialbus *sb;
+
+		sb = &ares->data.i2c_serial_bus;
+		if (sb->type == ACPI_RESOURCE_SERIAL_TYPE_I2C) {
+			if (*addr)
+				*addr |= (sb->slave_address << 16);
+			else
+				*addr = sb->slave_address;
+		}
+	}
+
+	/* Tell the ACPI core that we already copied this address */
+	return 1;
+}
+
+static int inv_mpu_process_acpi_config(struct i2c_client *client,
+				       unsigned short *primary_addr,
+				       unsigned short *secondary_addr)
+{
+	const struct acpi_device_id *id;
+	struct acpi_device *adev;
+	u32 i2c_addr = 0;
+	LIST_HEAD(resources);
+	int ret;
+
+	id = acpi_match_device(client->dev.driver->acpi_match_table,
+			       &client->dev);
+	if (!id)
+		return -ENODEV;
+
+	adev = ACPI_COMPANION(&client->dev);
+	if (!adev)
+		return -ENODEV;
+
+	ret = acpi_dev_get_resources(adev, &resources,
+				     acpi_i2c_check_resource, &i2c_addr);
+	if (ret < 0)
+		return ret;
+
+	acpi_dev_free_resource_list(&resources);
+	*primary_addr = i2c_addr & 0x0000ffff;
+	*secondary_addr = (i2c_addr & 0xffff0000) >> 16;
+
+	return 0;
+}
+
+int inv_mpu_acpi_create_mux_client(struct inv_mpu6050_state *st)
+{
+
+	st->mux_client = NULL;
+	if (ACPI_HANDLE(&st->client->dev)) {
+		struct i2c_board_info info;
+		struct acpi_device *adev;
+		int ret = -1;
+
+		adev = ACPI_COMPANION(&st->client->dev);
+		memset(&info, 0, sizeof(info));
+
+		dmi_check_system(inv_mpu_dev_list);
+		switch (matched_product_name) {
+		case INV_MPU_ASUS_T100TA:
+			ret = asus_acpi_get_sensor_info(adev, st->client,
+							&info);
+			break;
+		/* Add more matched product processing here */
+		default:
+			break;
+		}
+
+		if (ret < 0) {
+			/* No matching DMI, so create device on INV6XX type */
+			unsigned short primary, secondary;
+
+			ret = inv_mpu_process_acpi_config(st->client, &primary,
+							  &secondary);
+			if (!ret && secondary) {
+				char *name;
+
+				info.addr = secondary;
+				strlcpy(info.type, dev_name(&adev->dev),
+					sizeof(info.type));
+				name = strchr(info.type, ':');
+				if (name)
+					*name = '\0';
+				strlcat(info.type, "-client",
+					sizeof(info.type));
+			} else
+				return 0; /* no secondary addr, which is OK */
+		}
+		st->mux_client = i2c_new_device(st->mux_adapter, &info);
+		if (!st->mux_client)
+			return -ENODEV;
+
+	}
+
+	return 0;
+}
+
+void inv_mpu_acpi_delete_mux_client(struct inv_mpu6050_state *st)
+{
+	if (st->mux_client)
+		i2c_unregister_device(st->mux_client);
+}
+#else
+
+#include "inv_mpu_iio.h"
+
+int inv_mpu_acpi_create_mux_client(struct inv_mpu6050_state *st)
+{
+	return 0;
+}
+
+void inv_mpu_acpi_delete_mux_client(struct inv_mpu6050_state *st)
+{
+}
+#endif
diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
index f73e60b7a796..17d4bb15be4d 100644
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
@@ -410,42 +410,46 @@ error_read_raw:
 	}
 }
 
-static int inv_mpu6050_write_fsr(struct inv_mpu6050_state *st, int fsr)
+static int inv_mpu6050_write_gyro_scale(struct inv_mpu6050_state *st, int val)
 {
-	int result;
+	int result, i;
 	u8 d;
 
-	if (fsr < 0 || fsr > INV_MPU6050_MAX_GYRO_FS_PARAM)
-		return -EINVAL;
-	if (fsr == st->chip_config.fsr)
-		return 0;
+	for (i = 0; i < ARRAY_SIZE(gyro_scale_6050); ++i) {
+		if (gyro_scale_6050[i] == val) {
+			d = (i << INV_MPU6050_GYRO_CONFIG_FSR_SHIFT);
+			result = inv_mpu6050_write_reg(st,
+					st->reg->gyro_config, d);
+			if (result)
+				return result;
 
-	d = (fsr << INV_MPU6050_GYRO_CONFIG_FSR_SHIFT);
-	result = inv_mpu6050_write_reg(st, st->reg->gyro_config, d);
-	if (result)
-		return result;
-	st->chip_config.fsr = fsr;
+			st->chip_config.fsr = i;
+			return 0;
+		}
+	}
 
-	return 0;
+	return -EINVAL;
 }
 
-static int inv_mpu6050_write_accel_fs(struct inv_mpu6050_state *st, int fs)
+static int inv_mpu6050_write_accel_scale(struct inv_mpu6050_state *st, int val)
 {
-	int result;
+	int result, i;
 	u8 d;
 
-	if (fs < 0 || fs > INV_MPU6050_MAX_ACCL_FS_PARAM)
-		return -EINVAL;
-	if (fs == st->chip_config.accl_fs)
-		return 0;
+	for (i = 0; i < ARRAY_SIZE(accel_scale); ++i) {
+		if (accel_scale[i] == val) {
+			d = (i << INV_MPU6050_ACCL_CONFIG_FSR_SHIFT);
+			result = inv_mpu6050_write_reg(st,
+					st->reg->accl_config, d);
+			if (result)
+				return result;
 
-	d = (fs << INV_MPU6050_ACCL_CONFIG_FSR_SHIFT);
-	result = inv_mpu6050_write_reg(st, st->reg->accl_config, d);
-	if (result)
-		return result;
-	st->chip_config.accl_fs = fs;
+			st->chip_config.accl_fs = i;
+			return 0;
+		}
+	}
 
-	return 0;
+	return -EINVAL;
 }
 
 static int inv_mpu6050_write_raw(struct iio_dev *indio_dev,
@@ -471,10 +475,10 @@ static int inv_mpu6050_write_raw(struct iio_dev *indio_dev,
 	case IIO_CHAN_INFO_SCALE:
 		switch (chan->type) {
 		case IIO_ANGL_VEL:
-			result = inv_mpu6050_write_fsr(st, val);
+			result = inv_mpu6050_write_gyro_scale(st, val2);
 			break;
 		case IIO_ACCEL:
-			result = inv_mpu6050_write_accel_fs(st, val);
+			result = inv_mpu6050_write_accel_scale(st, val2);
 			break;
 		default:
 			result = -EINVAL;
@@ -780,7 +784,11 @@ static int inv_mpu_probe(struct i2c_client *client,
 
 	i2c_set_clientdata(client, indio_dev);
 	indio_dev->dev.parent = &client->dev;
-	indio_dev->name = id->name;
+	/* id will be NULL when enumerated via ACPI */
+	if (id)
+		indio_dev->name = (char *)id->name;
+	else
+		indio_dev->name = (char *)dev_name(&client->dev);
 	indio_dev->channels = inv_mpu_channels;
 	indio_dev->num_channels = ARRAY_SIZE(inv_mpu_channels);
 
@@ -821,8 +829,14 @@ static int inv_mpu_probe(struct i2c_client *client,
 		goto out_unreg_device;
 	}
 
+	result = inv_mpu_acpi_create_mux_client(st);
+	if (result)
+		goto out_del_mux;
+
 	return 0;
 
+out_del_mux:
+	i2c_del_mux_adapter(st->mux_adapter);
 out_unreg_device:
 	iio_device_unregister(indio_dev);
 out_remove_trigger:
@@ -837,6 +851,7 @@ static int inv_mpu_remove(struct i2c_client *client)
 	struct iio_dev *indio_dev = i2c_get_clientdata(client);
 	struct inv_mpu6050_state *st = iio_priv(indio_dev);
 
+	inv_mpu_acpi_delete_mux_client(st);
 	i2c_del_mux_adapter(st->mux_adapter);
 	iio_device_unregister(indio_dev);
 	inv_mpu6050_remove_trigger(st);
diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h b/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
index aa837de57079..db0a4a2758ab 100644
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
@@ -121,6 +121,7 @@ struct inv_mpu6050_state {
 	spinlock_t time_stamp_lock;
 	struct i2c_client *client;
 	struct i2c_adapter *mux_adapter;
+	struct i2c_client *mux_client;
 	unsigned int powerup_count;
 	struct inv_mpu6050_platform_data plat_data;
 	DECLARE_KFIFO(timestamps, long long, TIMESTAMP_FIFO_SIZE);
@@ -251,3 +252,5 @@ int inv_reset_fifo(struct iio_dev *indio_dev);
 int inv_mpu6050_switch_engine(struct inv_mpu6050_state *st, bool en, u32 mask);
 int inv_mpu6050_write_reg(struct inv_mpu6050_state *st, int reg, u8 val);
 int inv_mpu6050_set_power_itg(struct inv_mpu6050_state *st, bool power_on);
+int inv_mpu_acpi_create_mux_client(struct inv_mpu6050_state *st);
+void inv_mpu_acpi_delete_mux_client(struct inv_mpu6050_state *st);
diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
index 0cd306a72a6e..ba27e277511f 100644
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
@@ -24,6 +24,16 @@
 #include <linux/poll.h>
 #include "inv_mpu_iio.h"
 
+static void inv_clear_kfifo(struct inv_mpu6050_state *st)
+{
+	unsigned long flags;
+
+	/* take the spin lock sem to avoid interrupt kick in */
+	spin_lock_irqsave(&st->time_stamp_lock, flags);
+	kfifo_reset(&st->timestamps);
+	spin_unlock_irqrestore(&st->time_stamp_lock, flags);
+}
+
 int inv_reset_fifo(struct iio_dev *indio_dev)
 {
 	int result;
@@ -50,6 +60,10 @@ int inv_reset_fifo(struct iio_dev *indio_dev)
 					INV_MPU6050_BIT_FIFO_RST);
 	if (result)
 		goto reset_fifo_fail;
+
+	/* clear timestamps fifo */
+	inv_clear_kfifo(st);
+
 	/* enable interrupt */
 	if (st->chip_config.accl_fifo_enable ||
 	    st->chip_config.gyro_fifo_enable) {
@@ -83,16 +97,6 @@ reset_fifo_fail:
 	return result;
 }
 
-static void inv_clear_kfifo(struct inv_mpu6050_state *st)
-{
-	unsigned long flags;
-
-	/* take the spin lock sem to avoid interrupt kick in */
-	spin_lock_irqsave(&st->time_stamp_lock, flags);
-	kfifo_reset(&st->timestamps);
-	spin_unlock_irqrestore(&st->time_stamp_lock, flags);
-}
-
 /**
  * inv_mpu6050_irq_handler() - Cache a timestamp at each data ready interrupt.
  */
@@ -184,7 +188,6 @@ end_session:
 flush_fifo:
 	/* Flush HW and SW FIFOs. */
 	inv_reset_fifo(indio_dev);
-	inv_clear_kfifo(st);
 	mutex_unlock(&indio_dev->mlock);
 	iio_trigger_notify_done(indio_dev->trig);
 
diff --git a/drivers/iio/imu/kmx61.c b/drivers/iio/imu/kmx61.c
index 5cc3692acf37..462a010628cd 100644
--- a/drivers/iio/imu/kmx61.c
+++ b/drivers/iio/imu/kmx61.c
@@ -169,19 +169,18 @@ static const u16 kmx61_uscale_table[] = {9582, 19163, 38326};
 static const struct {
 	int val;
 	int val2;
-	u8 odr_bits;
-} kmx61_samp_freq_table[] = { {12, 500000, 0x00},
-			{25, 0, 0x01},
-			{50, 0, 0x02},
-			{100, 0, 0x03},
-			{200, 0, 0x04},
-			{400, 0, 0x05},
-			{800, 0, 0x06},
-			{1600, 0, 0x07},
-			{0, 781000, 0x08},
-			{1, 563000, 0x09},
-			{3, 125000, 0x0A},
-			{6, 250000, 0x0B} };
+} kmx61_samp_freq_table[] = { {12, 500000},
+			{25, 0},
+			{50, 0},
+			{100, 0},
+			{200, 0},
+			{400, 0},
+			{800, 0},
+			{1600, 0},
+			{0, 781000},
+			{1, 563000},
+			{3, 125000},
+			{6, 250000} };
 
 static const struct {
 	int val;
@@ -302,24 +301,10 @@ static int kmx61_convert_freq_to_bit(int val, int val2)
 	for (i = 0; i < ARRAY_SIZE(kmx61_samp_freq_table); i++)
 		if (val == kmx61_samp_freq_table[i].val &&
 		    val2 == kmx61_samp_freq_table[i].val2)
-			return kmx61_samp_freq_table[i].odr_bits;
+			return i;
 	return -EINVAL;
 }
 
-static int kmx61_convert_bit_to_freq(u8 odr_bits, int *val, int *val2)
-{
-	int i;
-
-	for (i = 0; i < ARRAY_SIZE(kmx61_samp_freq_table); i++)
-		if (odr_bits == kmx61_samp_freq_table[i].odr_bits) {
-			*val = kmx61_samp_freq_table[i].val;
-			*val2 = kmx61_samp_freq_table[i].val2;
-			return 0;
-		}
-	return -EINVAL;
-}
-
-
 static int kmx61_convert_wake_up_odr_to_bit(int val, int val2)
 {
 	int i;
@@ -478,7 +463,7 @@ static int kmx61_set_odr(struct kmx61_data *data, int val, int val2, u8 device)
 
 static int kmx61_get_odr(struct kmx61_data *data, int *val, int *val2,
 			 u8 device)
-{	int i;
+{
 	u8 lodr_bits;
 
 	if (device & KMX61_ACC)
@@ -490,13 +475,13 @@ static int kmx61_get_odr(struct kmx61_data *data, int *val, int *val2,
 	else
 		return -EINVAL;
 
-	for (i = 0; i < ARRAY_SIZE(kmx61_samp_freq_table); i++)
-		if (lodr_bits == kmx61_samp_freq_table[i].odr_bits) {
-			*val = kmx61_samp_freq_table[i].val;
-			*val2 = kmx61_samp_freq_table[i].val2;
-			return 0;
-		}
-	return -EINVAL;
+	if (lodr_bits >= ARRAY_SIZE(kmx61_samp_freq_table))
+		return -EINVAL;
+
+	*val = kmx61_samp_freq_table[lodr_bits].val;
+	*val2 = kmx61_samp_freq_table[lodr_bits].val2;
+
+	return 0;
 }
 
 static int kmx61_set_range(struct kmx61_data *data, u8 range)
@@ -580,8 +565,11 @@ static int kmx61_chip_init(struct kmx61_data *data)
 	}
 	data->odr_bits = ret;
 
-	/* set output data rate for wake up (motion detection) function */
-	ret = kmx61_convert_bit_to_freq(data->odr_bits, &val, &val2);
+	/*
+	 * set output data rate for wake up (motion detection) function
+	 * to match data rate for accelerometer sampling
+	 */
+	ret = kmx61_get_odr(data, &val, &val2, KMX61_ACC);
 	if (ret < 0)
 		return ret;
 
@@ -1227,7 +1215,7 @@ static irqreturn_t kmx61_trigger_handler(int irq, void *p)
 		base = KMX61_MAG_XOUT_L;
 
 	mutex_lock(&data->lock);
-	for_each_set_bit(bit, indio_dev->buffer->scan_mask,
+	for_each_set_bit(bit, indio_dev->active_scan_mask,
 			 indio_dev->masklength) {
 		ret = kmx61_read_measurement(data, base, bit);
 		if (ret < 0) {
@@ -1267,16 +1255,12 @@ static int kmx61_gpio_probe(struct i2c_client *client, struct kmx61_data *data)
 	dev = &client->dev;
 
 	/* data ready gpio interrupt pin */
-	gpio = devm_gpiod_get_index(dev, KMX61_GPIO_NAME, 0);
+	gpio = devm_gpiod_get_index(dev, KMX61_GPIO_NAME, 0, GPIOD_IN);
 	if (IS_ERR(gpio)) {
 		dev_err(dev, "acpi gpio get index failed\n");
 		return PTR_ERR(gpio);
 	}
 
-	ret = gpiod_direction_input(gpio);
-	if (ret)
-		return ret;
-
 	ret = gpiod_to_irq(gpio);
 
 	dev_dbg(dev, "GPIO resource, no:%d irq:%d\n", desc_to_gpio(gpio), ret);
diff --git a/drivers/iio/industrialio-buffer.c b/drivers/iio/industrialio-buffer.c
index 71333140d42c..df919f44d513 100644
--- a/drivers/iio/industrialio-buffer.c
+++ b/drivers/iio/industrialio-buffer.c
@@ -37,11 +37,57 @@ static bool iio_buffer_is_active(struct iio_buffer *buf)
 	return !list_empty(&buf->buffer_list);
 }
 
-static bool iio_buffer_data_available(struct iio_buffer *buf)
+static size_t iio_buffer_data_available(struct iio_buffer *buf)
 {
 	return buf->access->data_available(buf);
 }
 
+static int iio_buffer_flush_hwfifo(struct iio_dev *indio_dev,
+				   struct iio_buffer *buf, size_t required)
+{
+	if (!indio_dev->info->hwfifo_flush_to_buffer)
+		return -ENODEV;
+
+	return indio_dev->info->hwfifo_flush_to_buffer(indio_dev, required);
+}
+
+static bool iio_buffer_ready(struct iio_dev *indio_dev, struct iio_buffer *buf,
+			     size_t to_wait, int to_flush)
+{
+	size_t avail;
+	int flushed = 0;
+
+	/* wakeup if the device was unregistered */
+	if (!indio_dev->info)
+		return true;
+
+	/* drain the buffer if it was disabled */
+	if (!iio_buffer_is_active(buf)) {
+		to_wait = min_t(size_t, to_wait, 1);
+		to_flush = 0;
+	}
+
+	avail = iio_buffer_data_available(buf);
+
+	if (avail >= to_wait) {
+		/* force a flush for non-blocking reads */
+		if (!to_wait && !avail && to_flush)
+			iio_buffer_flush_hwfifo(indio_dev, buf, to_flush);
+		return true;
+	}
+
+	if (to_flush)
+		flushed = iio_buffer_flush_hwfifo(indio_dev, buf,
+						  to_wait - avail);
+	if (flushed <= 0)
+		return false;
+
+	if (avail + flushed >= to_wait)
+		return true;
+
+	return false;
+}
+
 /**
  * iio_buffer_read_first_n_outer() - chrdev read for buffer access
  *
@@ -53,6 +99,9 @@ ssize_t iio_buffer_read_first_n_outer(struct file *filp, char __user *buf,
 {
 	struct iio_dev *indio_dev = filp->private_data;
 	struct iio_buffer *rb = indio_dev->buffer;
+	size_t datum_size;
+	size_t to_wait = 0;
+	size_t to_read;
 	int ret;
 
 	if (!indio_dev->info)
@@ -61,19 +110,28 @@ ssize_t iio_buffer_read_first_n_outer(struct file *filp, char __user *buf,
 	if (!rb || !rb->access->read_first_n)
 		return -EINVAL;
 
+	datum_size = rb->bytes_per_datum;
+
+	/*
+	 * If datum_size is 0 there will never be anything to read from the
+	 * buffer, so signal end of file now.
+	 */
+	if (!datum_size)
+		return 0;
+
+	to_read = min_t(size_t, n / datum_size, rb->watermark);
+
+	if (!(filp->f_flags & O_NONBLOCK))
+		to_wait = to_read;
+
 	do {
-		if (!iio_buffer_data_available(rb)) {
-			if (filp->f_flags & O_NONBLOCK)
-				return -EAGAIN;
+		ret = wait_event_interruptible(rb->pollq,
+			iio_buffer_ready(indio_dev, rb, to_wait, to_read));
+		if (ret)
+			return ret;
 
-			ret = wait_event_interruptible(rb->pollq,
-					iio_buffer_data_available(rb) ||
-					indio_dev->info == NULL);
-			if (ret)
-				return ret;
-			if (indio_dev->info == NULL)
-				return -ENODEV;
-		}
+		if (!indio_dev->info)
+			return -ENODEV;
 
 		ret = rb->access->read_first_n(rb, n, buf);
 		if (ret == 0 && (filp->f_flags & O_NONBLOCK))
@@ -96,9 +154,8 @@ unsigned int iio_buffer_poll(struct file *filp,
 		return -ENODEV;
 
 	poll_wait(filp, &rb->pollq, wait);
-	if (iio_buffer_data_available(rb))
+	if (iio_buffer_ready(indio_dev, rb, rb->watermark, 0))
 		return POLLIN | POLLRDNORM;
-	/* need a way of knowing if there may be enough data... */
 	return 0;
 }
 
@@ -123,6 +180,7 @@ void iio_buffer_init(struct iio_buffer *buffer)
 	INIT_LIST_HEAD(&buffer->buffer_list);
 	init_waitqueue_head(&buffer->pollq);
 	kref_init(&buffer->ref);
+	buffer->watermark = 1;
 }
 EXPORT_SYMBOL(iio_buffer_init);
 
@@ -416,6 +474,11 @@ static ssize_t iio_buffer_write_length(struct device *dev,
 		buffer->access->set_length(buffer, val);
 		ret = 0;
 	}
+	if (ret)
+		goto out;
+	if (buffer->length && buffer->length < buffer->watermark)
+		buffer->watermark = buffer->length;
+out:
 	mutex_unlock(&indio_dev->mlock);
 
 	return ret ? ret : len;
@@ -472,6 +535,7 @@ static void iio_buffer_activate(struct iio_dev *indio_dev,
 static void iio_buffer_deactivate(struct iio_buffer *buffer)
 {
 	list_del_init(&buffer->buffer_list);
+	wake_up_interruptible(&buffer->pollq);
 	iio_buffer_put(buffer);
 }
 
@@ -629,19 +693,16 @@ static int __iio_update_buffers(struct iio_dev *indio_dev,
 		}
 	}
 	/* Definitely possible for devices to support both of these. */
-	if (indio_dev->modes & INDIO_BUFFER_TRIGGERED) {
-		if (!indio_dev->trig) {
-			printk(KERN_INFO "Buffer not started: no trigger\n");
-			ret = -EINVAL;
-			/* Can only occur on first buffer */
-			goto error_run_postdisable;
-		}
+	if ((indio_dev->modes & INDIO_BUFFER_TRIGGERED) && indio_dev->trig) {
 		indio_dev->currentmode = INDIO_BUFFER_TRIGGERED;
 	} else if (indio_dev->modes & INDIO_BUFFER_HARDWARE) {
 		indio_dev->currentmode = INDIO_BUFFER_HARDWARE;
 	} else if (indio_dev->modes & INDIO_BUFFER_SOFTWARE) {
 		indio_dev->currentmode = INDIO_BUFFER_SOFTWARE;
 	} else { /* Should never be reached */
+		/* Can only occur on first buffer */
+		if (indio_dev->modes & INDIO_BUFFER_TRIGGERED)
+			pr_info("Buffer not started: no trigger\n");
 		ret = -EINVAL;
 		goto error_run_postdisable;
 	}
@@ -754,12 +815,68 @@ done:
 
 static const char * const iio_scan_elements_group_name = "scan_elements";
 
+static ssize_t iio_buffer_show_watermark(struct device *dev,
+					 struct device_attribute *attr,
+					 char *buf)
+{
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
+	struct iio_buffer *buffer = indio_dev->buffer;
+
+	return sprintf(buf, "%u\n", buffer->watermark);
+}
+
+static ssize_t iio_buffer_store_watermark(struct device *dev,
+					  struct device_attribute *attr,
+					  const char *buf,
+					  size_t len)
+{
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
+	struct iio_buffer *buffer = indio_dev->buffer;
+	unsigned int val;
+	int ret;
+
+	ret = kstrtouint(buf, 10, &val);
+	if (ret)
+		return ret;
+	if (!val)
+		return -EINVAL;
+
+	mutex_lock(&indio_dev->mlock);
+
+	if (val > buffer->length) {
+		ret = -EINVAL;
+		goto out;
+	}
+
+	if (iio_buffer_is_active(indio_dev->buffer)) {
+		ret = -EBUSY;
+		goto out;
+	}
+
+	buffer->watermark = val;
+
+	if (indio_dev->info->hwfifo_set_watermark)
+		indio_dev->info->hwfifo_set_watermark(indio_dev, val);
+out:
+	mutex_unlock(&indio_dev->mlock);
+
+	return ret ? ret : len;
+}
+
 static DEVICE_ATTR(length, S_IRUGO | S_IWUSR, iio_buffer_read_length,
 		   iio_buffer_write_length);
 static struct device_attribute dev_attr_length_ro = __ATTR(length,
 	S_IRUGO, iio_buffer_read_length, NULL);
 static DEVICE_ATTR(enable, S_IRUGO | S_IWUSR,
 		   iio_buffer_show_enable, iio_buffer_store_enable);
+static DEVICE_ATTR(watermark, S_IRUGO | S_IWUSR,
+		   iio_buffer_show_watermark, iio_buffer_store_watermark);
+
+static struct attribute *iio_buffer_attrs[] = {
+	&dev_attr_length.attr,
+	&dev_attr_enable.attr,
+	&dev_attr_watermark.attr,
+};
 
 int iio_buffer_alloc_sysfs_and_mask(struct iio_dev *indio_dev)
 {
@@ -778,21 +895,23 @@ int iio_buffer_alloc_sysfs_and_mask(struct iio_dev *indio_dev)
 			attrcount++;
 	}
 
-	buffer->buffer_group.name = "buffer";
-	buffer->buffer_group.attrs = kcalloc(attrcount + 3,
-			sizeof(*buffer->buffer_group.attrs), GFP_KERNEL);
-	if (!buffer->buffer_group.attrs)
+	attr = kcalloc(attrcount + ARRAY_SIZE(iio_buffer_attrs) + 1,
+		       sizeof(struct attribute *), GFP_KERNEL);
+	if (!attr)
 		return -ENOMEM;
 
-	if (buffer->access->set_length)
-		buffer->buffer_group.attrs[0] = &dev_attr_length.attr;
-	else
-		buffer->buffer_group.attrs[0] = &dev_attr_length_ro.attr;
-	buffer->buffer_group.attrs[1] = &dev_attr_enable.attr;
+	memcpy(attr, iio_buffer_attrs, sizeof(iio_buffer_attrs));
+	if (!buffer->access->set_length)
+		attr[0] = &dev_attr_length_ro.attr;
+
 	if (buffer->attrs)
-		memcpy(&buffer->buffer_group.attrs[2], buffer->attrs,
-			sizeof(*&buffer->buffer_group.attrs) * attrcount);
-	buffer->buffer_group.attrs[attrcount+2] = NULL;
+		memcpy(&attr[ARRAY_SIZE(iio_buffer_attrs)], buffer->attrs,
+		       sizeof(struct attribute *) * attrcount);
+
+	attr[attrcount + ARRAY_SIZE(iio_buffer_attrs)] = NULL;
+
+	buffer->buffer_group.name = "buffer";
+	buffer->buffer_group.attrs = attr;
 
 	indio_dev->groups[indio_dev->groupcounter++] = &buffer->buffer_group;
 
@@ -937,8 +1056,18 @@ static const void *iio_demux(struct iio_buffer *buffer,
 static int iio_push_to_buffer(struct iio_buffer *buffer, const void *data)
 {
 	const void *dataout = iio_demux(buffer, data);
+	int ret;
 
-	return buffer->access->store_to(buffer, dataout);
+	ret = buffer->access->store_to(buffer, dataout);
+	if (ret)
+		return ret;
+
+	/*
+	 * We can't just test for watermark to decide if we wake the poll queue
+	 * because read may request less samples than the watermark.
+	 */
+	wake_up_interruptible_poll(&buffer->pollq, POLLIN | POLLRDNORM);
+	return 0;
 }
 
 static void iio_buffer_demux_free(struct iio_buffer *buffer)
diff --git a/drivers/iio/industrialio-core.c b/drivers/iio/industrialio-core.c
index aaba9d3d980e..4df97f650e44 100644
--- a/drivers/iio/industrialio-core.c
+++ b/drivers/iio/industrialio-core.c
@@ -847,8 +847,7 @@ static int iio_device_add_channel_sysfs(struct iio_dev *indio_dev,
  * @attr_list: List of IIO device attributes
  *
  * This function frees the memory allocated for each of the IIO device
- * attributes in the list. Note: if you want to reuse the list after calling
- * this function you have to reinitialize it using INIT_LIST_HEAD().
+ * attributes in the list.
  */
 void iio_free_chan_devattr_list(struct list_head *attr_list)
 {
@@ -856,6 +855,7 @@ void iio_free_chan_devattr_list(struct list_head *attr_list)
 
 	list_for_each_entry_safe(p, n, attr_list, l) {
 		kfree(p->dev_attr.attr.name);
+		list_del(&p->l);
 		kfree(p);
 	}
 }
@@ -936,6 +936,7 @@ static void iio_device_unregister_sysfs(struct iio_dev *indio_dev)
 
 	iio_free_chan_devattr_list(&indio_dev->channel_attr_list);
 	kfree(indio_dev->chan_attr_group.attrs);
+	indio_dev->chan_attr_group.attrs = NULL;
 }
 
 static void iio_dev_release(struct device *device)
diff --git a/drivers/iio/industrialio-event.c b/drivers/iio/industrialio-event.c
index a4b397048f71..a99692ba91bc 100644
--- a/drivers/iio/industrialio-event.c
+++ b/drivers/iio/industrialio-event.c
@@ -500,6 +500,7 @@ int iio_device_register_eventset(struct iio_dev *indio_dev)
 error_free_setup_event_lines:
 	iio_free_chan_devattr_list(&indio_dev->event_interface->dev_attr_list);
 	kfree(indio_dev->event_interface);
+	indio_dev->event_interface = NULL;
 	return ret;
 }
 
diff --git a/drivers/iio/kfifo_buf.c b/drivers/iio/kfifo_buf.c
index b2beea01c49b..847ca561afe0 100644
--- a/drivers/iio/kfifo_buf.c
+++ b/drivers/iio/kfifo_buf.c
@@ -83,9 +83,6 @@ static int iio_store_to_kfifo(struct iio_buffer *r,
 	ret = kfifo_in(&kf->kf, data, 1);
 	if (ret != 1)
 		return -EBUSY;
-
-	wake_up_interruptible_poll(&r->pollq, POLLIN | POLLRDNORM);
-
 	return 0;
 }
 
@@ -109,16 +106,16 @@ static int iio_read_first_n_kfifo(struct iio_buffer *r,
 	return copied;
 }
 
-static bool iio_kfifo_buf_data_available(struct iio_buffer *r)
+static size_t iio_kfifo_buf_data_available(struct iio_buffer *r)
 {
 	struct iio_kfifo *kf = iio_to_kfifo(r);
-	bool empty;
+	size_t samples;
 
 	mutex_lock(&kf->user_lock);
-	empty = kfifo_is_empty(&kf->kf);
+	samples = kfifo_len(&kf->kf);
 	mutex_unlock(&kf->user_lock);
 
-	return !empty;
+	return samples;
 }
 
 static void iio_kfifo_buffer_release(struct iio_buffer *buffer)
diff --git a/drivers/iio/light/Kconfig b/drivers/iio/light/Kconfig
index ae68c64bdad3..01a1a16ab7be 100644
--- a/drivers/iio/light/Kconfig
+++ b/drivers/iio/light/Kconfig
@@ -59,6 +59,16 @@ config CM3232
 	 To compile this driver as a module, choose M here:
 	 the module will be called cm3232.
 
+config CM3323
+	depends on I2C
+	tristate "Capella CM3323 color light sensor"
+	help
+	 Say Y here if you want to build a driver for Capela CM3323
+	 color sensor.
+
+	 To compile this driver as a module, choose M here: the module will
+	 be called cm3323.
+
 config CM36651
 	depends on I2C
 	tristate "CM36651 driver"
@@ -73,6 +83,7 @@ config CM36651
 config GP2AP020A00F
 	tristate "Sharp GP2AP020A00F Proximity/ALS sensor"
 	depends on I2C
+	select REGMAP_I2C
 	select IIO_BUFFER
 	select IIO_TRIGGERED_BUFFER
 	select IRQ_WORK
@@ -126,6 +137,7 @@ config HID_SENSOR_PROX
 config JSA1212
 	tristate "JSA1212 ALS and proximity sensor driver"
 	depends on I2C
+	select REGMAP_I2C
 	help
 	 Say Y here if you want to build a IIO driver for JSA1212
 	 proximity & ALS sensor device.
diff --git a/drivers/iio/light/Makefile b/drivers/iio/light/Makefile
index b12a5160d9e0..ad7c30fe443b 100644
--- a/drivers/iio/light/Makefile
+++ b/drivers/iio/light/Makefile
@@ -8,6 +8,7 @@ obj-$(CONFIG_AL3320A)		+= al3320a.o
 obj-$(CONFIG_APDS9300)		+= apds9300.o
 obj-$(CONFIG_CM32181)		+= cm32181.o
 obj-$(CONFIG_CM3232)		+= cm3232.o
+obj-$(CONFIG_CM3323)		+= cm3323.o
 obj-$(CONFIG_CM36651)		+= cm36651.o
 obj-$(CONFIG_GP2AP020A00F)	+= gp2ap020a00f.o
 obj-$(CONFIG_HID_SENSOR_ALS)	+= hid-sensor-als.o
diff --git a/drivers/iio/light/cm3232.c b/drivers/iio/light/cm3232.c
index 90e3519a91de..39c8d99cc48e 100644
--- a/drivers/iio/light/cm3232.c
+++ b/drivers/iio/light/cm3232.c
@@ -378,6 +378,39 @@ static const struct i2c_device_id cm3232_id[] = {
 	{}
 };
 
+#ifdef CONFIG_PM_SLEEP
+static int cm3232_suspend(struct device *dev)
+{
+	struct iio_dev *indio_dev = i2c_get_clientdata(to_i2c_client(dev));
+	struct cm3232_chip *chip = iio_priv(indio_dev);
+	struct i2c_client *client = chip->client;
+	int ret;
+
+	chip->regs_cmd |= CM3232_CMD_ALS_DISABLE;
+	ret = i2c_smbus_write_byte_data(client, CM3232_REG_ADDR_CMD,
+					chip->regs_cmd);
+
+	return ret;
+}
+
+static int cm3232_resume(struct device *dev)
+{
+	struct iio_dev *indio_dev = i2c_get_clientdata(to_i2c_client(dev));
+	struct cm3232_chip *chip = iio_priv(indio_dev);
+	struct i2c_client *client = chip->client;
+	int ret;
+
+	chip->regs_cmd &= ~CM3232_CMD_ALS_DISABLE;
+	ret = i2c_smbus_write_byte_data(client, CM3232_REG_ADDR_CMD,
+					chip->regs_cmd | CM3232_CMD_ALS_RESET);
+
+	return ret;
+}
+
+static const struct dev_pm_ops cm3232_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(cm3232_suspend, cm3232_resume)};
+#endif
+
 MODULE_DEVICE_TABLE(i2c, cm3232_id);
 
 static const struct of_device_id cm3232_of_match[] = {
@@ -390,6 +423,9 @@ static struct i2c_driver cm3232_driver = {
 		.name	= "cm3232",
 		.owner	= THIS_MODULE,
 		.of_match_table = of_match_ptr(cm3232_of_match),
+#ifdef CONFIG_PM_SLEEP
+		.pm	= &cm3232_pm_ops,
+#endif
 	},
 	.id_table	= cm3232_id,
 	.probe		= cm3232_probe,
diff --git a/drivers/iio/light/cm3323.c b/drivers/iio/light/cm3323.c
new file mode 100644
index 000000000000..869033e48a1f
--- /dev/null
+++ b/drivers/iio/light/cm3323.c
@@ -0,0 +1,286 @@
+/*
+ * CM3323 - Capella Color Light Sensor
+ *
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This file is subject to the terms and conditions of version 2 of
+ * the GNU General Public License.  See the file COPYING in the main
+ * directory of this archive for more details.
+ *
+ * IIO driver for CM3323 (7-bit I2C slave address 0x10)
+ *
+ * TODO: calibscale to correct the lens factor
+ */
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/i2c.h>
+#include <linux/mutex.h>
+
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
+
+#define CM3323_DRV_NAME "cm3323"
+
+#define CM3323_CMD_CONF		0x00
+#define CM3323_CMD_RED_DATA	0x08
+#define CM3323_CMD_GREEN_DATA	0x09
+#define CM3323_CMD_BLUE_DATA	0x0A
+#define CM3323_CMD_CLEAR_DATA	0x0B
+
+#define CM3323_CONF_SD_BIT	BIT(0) /* sensor disable */
+#define CM3323_CONF_AF_BIT	BIT(1) /* auto/manual force mode */
+#define CM3323_CONF_IT_MASK	(BIT(4) | BIT(5) | BIT(6))
+#define CM3323_CONF_IT_SHIFT	4
+
+#define CM3323_INT_TIME_AVAILABLE "0.04 0.08 0.16 0.32 0.64 1.28"
+
+static const struct {
+	int val;
+	int val2;
+} cm3323_int_time[] = {
+	{0, 40000},  /* 40 ms */
+	{0, 80000},  /* 80 ms */
+	{0, 160000}, /* 160 ms */
+	{0, 320000}, /* 320 ms */
+	{0, 640000}, /* 640 ms */
+	{1, 280000}, /* 1280 ms */
+};
+
+struct cm3323_data {
+	struct i2c_client *client;
+	u16 reg_conf;
+	struct mutex mutex;
+};
+
+#define CM3323_COLOR_CHANNEL(_color, _addr) { \
+	.type = IIO_INTENSITY, \
+	.modified = 1, \
+	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW), \
+	.info_mask_shared_by_all = BIT(IIO_CHAN_INFO_INT_TIME), \
+	.channel2 = IIO_MOD_LIGHT_##_color, \
+	.address = _addr, \
+}
+
+static const struct iio_chan_spec cm3323_channels[] = {
+	CM3323_COLOR_CHANNEL(RED, CM3323_CMD_RED_DATA),
+	CM3323_COLOR_CHANNEL(GREEN, CM3323_CMD_GREEN_DATA),
+	CM3323_COLOR_CHANNEL(BLUE, CM3323_CMD_BLUE_DATA),
+	CM3323_COLOR_CHANNEL(CLEAR, CM3323_CMD_CLEAR_DATA),
+};
+
+static IIO_CONST_ATTR_INT_TIME_AVAIL(CM3323_INT_TIME_AVAILABLE);
+
+static struct attribute *cm3323_attributes[] = {
+	&iio_const_attr_integration_time_available.dev_attr.attr,
+	NULL
+};
+
+static const struct attribute_group cm3323_attribute_group = {
+	.attrs = cm3323_attributes,
+};
+
+static int cm3323_init(struct iio_dev *indio_dev)
+{
+	int ret;
+	struct cm3323_data *data = iio_priv(indio_dev);
+
+	ret = i2c_smbus_read_word_data(data->client, CM3323_CMD_CONF);
+	if (ret < 0) {
+		dev_err(&data->client->dev, "Error reading reg_conf\n");
+		return ret;
+	}
+
+	/* enable sensor and set auto force mode */
+	ret &= ~(CM3323_CONF_SD_BIT | CM3323_CONF_AF_BIT);
+
+	ret = i2c_smbus_write_word_data(data->client, CM3323_CMD_CONF, ret);
+	if (ret < 0) {
+		dev_err(&data->client->dev, "Error writing reg_conf\n");
+		return ret;
+	}
+
+	data->reg_conf = ret;
+
+	return 0;
+}
+
+static void cm3323_disable(struct iio_dev *indio_dev)
+{
+	int ret;
+	struct cm3323_data *data = iio_priv(indio_dev);
+
+	ret = i2c_smbus_write_word_data(data->client, CM3323_CMD_CONF,
+					CM3323_CONF_SD_BIT);
+	if (ret < 0)
+		dev_err(&data->client->dev, "Error writing reg_conf\n");
+}
+
+static int cm3323_set_it_bits(struct cm3323_data *data, int val, int val2)
+{
+	int i, ret;
+	u16 reg_conf;
+
+	for (i = 0; i < ARRAY_SIZE(cm3323_int_time); i++) {
+		if (val == cm3323_int_time[i].val &&
+		    val2 == cm3323_int_time[i].val2) {
+			reg_conf = data->reg_conf;
+			reg_conf |= i << CM3323_CONF_IT_SHIFT;
+
+			ret = i2c_smbus_write_word_data(data->client,
+							CM3323_CMD_CONF,
+							reg_conf);
+			if (ret < 0)
+				return ret;
+
+			data->reg_conf = reg_conf;
+			return 0;
+		}
+	}
+	return -EINVAL;
+}
+
+static int cm3323_get_it_bits(struct cm3323_data *data)
+{
+	int bits;
+
+	bits = (data->reg_conf & CM3323_CONF_IT_MASK) >>
+		CM3323_CONF_IT_SHIFT;
+
+	if (bits >= ARRAY_SIZE(cm3323_int_time))
+		return -EINVAL;
+	return bits;
+}
+
+static int cm3323_read_raw(struct iio_dev *indio_dev,
+			   struct iio_chan_spec const *chan, int *val,
+			   int *val2, long mask)
+{
+	int i, ret;
+	struct cm3323_data *data = iio_priv(indio_dev);
+
+	switch (mask) {
+	case IIO_CHAN_INFO_RAW:
+		mutex_lock(&data->mutex);
+		ret = i2c_smbus_read_word_data(data->client, chan->address);
+		if (ret < 0) {
+			mutex_unlock(&data->mutex);
+			return ret;
+		}
+		*val = ret;
+		mutex_unlock(&data->mutex);
+
+		return IIO_VAL_INT;
+	case IIO_CHAN_INFO_INT_TIME:
+		mutex_lock(&data->mutex);
+		i = cm3323_get_it_bits(data);
+		if (i < 0) {
+			mutex_unlock(&data->mutex);
+			return -EINVAL;
+		}
+
+		*val = cm3323_int_time[i].val;
+		*val2 = cm3323_int_time[i].val2;
+		mutex_unlock(&data->mutex);
+
+		return IIO_VAL_INT_PLUS_MICRO;
+	default:
+		return -EINVAL;
+	}
+}
+
+static int cm3323_write_raw(struct iio_dev *indio_dev,
+			    struct iio_chan_spec const *chan, int val,
+			    int val2, long mask)
+{
+	struct cm3323_data *data = iio_priv(indio_dev);
+	int ret;
+
+	switch (mask) {
+	case IIO_CHAN_INFO_INT_TIME:
+		mutex_lock(&data->mutex);
+		ret = cm3323_set_it_bits(data, val, val2);
+		mutex_unlock(&data->mutex);
+
+		return ret;
+	default:
+		return -EINVAL;
+	}
+}
+
+static const struct iio_info cm3323_info = {
+	.driver_module	= THIS_MODULE,
+	.read_raw	= cm3323_read_raw,
+	.write_raw	= cm3323_write_raw,
+	.attrs		= &cm3323_attribute_group,
+};
+
+static int cm3323_probe(struct i2c_client *client,
+			const struct i2c_device_id *id)
+{
+	struct cm3323_data *data;
+	struct iio_dev *indio_dev;
+	int ret;
+
+	indio_dev = devm_iio_device_alloc(&client->dev, sizeof(*data));
+	if (!indio_dev)
+		return -ENOMEM;
+
+	data = iio_priv(indio_dev);
+	i2c_set_clientdata(client, indio_dev);
+	data->client = client;
+
+	mutex_init(&data->mutex);
+
+	indio_dev->dev.parent = &client->dev;
+	indio_dev->info = &cm3323_info;
+	indio_dev->name = CM3323_DRV_NAME;
+	indio_dev->channels = cm3323_channels;
+	indio_dev->num_channels = ARRAY_SIZE(cm3323_channels);
+	indio_dev->modes = INDIO_DIRECT_MODE;
+
+	ret = cm3323_init(indio_dev);
+	if (ret < 0) {
+		dev_err(&client->dev, "cm3323 chip init failed\n");
+		return ret;
+	}
+	ret = iio_device_register(indio_dev);
+	if (ret < 0) {
+		dev_err(&client->dev, "failed to register iio dev\n");
+		goto err_init;
+	}
+	return 0;
+err_init:
+	cm3323_disable(indio_dev);
+	return ret;
+}
+
+static int cm3323_remove(struct i2c_client *client)
+{
+	struct iio_dev *indio_dev = i2c_get_clientdata(client);
+
+	iio_device_unregister(indio_dev);
+	cm3323_disable(indio_dev);
+
+	return 0;
+}
+
+static const struct i2c_device_id cm3323_id[] = {
+	{"cm3323", 0},
+	{}
+};
+MODULE_DEVICE_TABLE(i2c, cm3323_id);
+
+static struct i2c_driver cm3323_driver = {
+	.driver = {
+		.name = CM3323_DRV_NAME,
+	},
+	.probe		= cm3323_probe,
+	.remove		= cm3323_remove,
+	.id_table	= cm3323_id,
+};
+
+module_i2c_driver(cm3323_driver);
+
+MODULE_AUTHOR("Daniel Baluta <daniel.baluta@intel.com>");
+MODULE_DESCRIPTION("Capella CM3323 Color Light Sensor driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/iio/light/gp2ap020a00f.c b/drivers/iio/light/gp2ap020a00f.c
index 221ed16de1f7..32b6449833fa 100644
--- a/drivers/iio/light/gp2ap020a00f.c
+++ b/drivers/iio/light/gp2ap020a00f.c
@@ -46,6 +46,7 @@
 #include <linux/regmap.h>
 #include <linux/regulator/consumer.h>
 #include <linux/slab.h>
+#include <asm/unaligned.h>
 #include <linux/iio/buffer.h>
 #include <linux/iio/events.h>
 #include <linux/iio/iio.h>
@@ -966,7 +967,6 @@ static irqreturn_t gp2ap020a00f_trigger_handler(int irq, void *data)
 	struct iio_dev *indio_dev = pf->indio_dev;
 	struct gp2ap020a00f_data *priv = iio_priv(indio_dev);
 	size_t d_size = 0;
-	__le32 light_lux;
 	int i, out_val, ret;
 
 	for_each_set_bit(i, indio_dev->active_scan_mask,
@@ -981,8 +981,8 @@ static irqreturn_t gp2ap020a00f_trigger_handler(int irq, void *data)
 		    i == GP2AP020A00F_SCAN_MODE_LIGHT_IR) {
 			out_val = le16_to_cpup((__le16 *)&priv->buffer[d_size]);
 			gp2ap020a00f_output_to_lux(priv, &out_val);
-			light_lux = cpu_to_le32(out_val);
-			memcpy(&priv->buffer[d_size], (u8 *)&light_lux, 4);
+
+			put_unaligned_le32(out_val, &priv->buffer[d_size]);
 			d_size += 4;
 		} else {
 			d_size += 2;
diff --git a/drivers/iio/light/jsa1212.c b/drivers/iio/light/jsa1212.c
index 29de7e7d9562..3a3af89beaf9 100644
--- a/drivers/iio/light/jsa1212.c
+++ b/drivers/iio/light/jsa1212.c
@@ -308,7 +308,7 @@ static bool jsa1212_is_volatile_reg(struct device *dev, unsigned int reg)
 	}
 }
 
-static struct regmap_config jsa1212_regmap_config = {
+static const struct regmap_config jsa1212_regmap_config = {
 	.name =  JSA1212_REGMAP_NAME,
 	.reg_bits = 8,
 	.val_bits = 8,
diff --git a/drivers/iio/light/ltr501.c b/drivers/iio/light/ltr501.c
index 62b7072af4de..78b87839c4b9 100644
--- a/drivers/iio/light/ltr501.c
+++ b/drivers/iio/light/ltr501.c
@@ -333,6 +333,13 @@ static int ltr501_init(struct ltr501_data *data)
 		data->ps_contr);
 }
 
+static int ltr501_powerdown(struct ltr501_data *data)
+{
+	return ltr501_write_contr(data->client,
+				  data->als_contr & ~LTR501_CONTR_ACTIVE,
+				  data->ps_contr & ~LTR501_CONTR_ACTIVE);
+}
+
 static int ltr501_probe(struct i2c_client *client,
 			  const struct i2c_device_id *id)
 {
@@ -370,7 +377,7 @@ static int ltr501_probe(struct i2c_client *client,
 	ret = iio_triggered_buffer_setup(indio_dev, NULL,
 		ltr501_trigger_handler, NULL);
 	if (ret)
-		return ret;
+		goto powerdown_on_error;
 
 	ret = iio_device_register(indio_dev);
 	if (ret)
@@ -380,16 +387,11 @@ static int ltr501_probe(struct i2c_client *client,
 
 error_unreg_buffer:
 	iio_triggered_buffer_cleanup(indio_dev);
+powerdown_on_error:
+	ltr501_powerdown(data);
 	return ret;
 }
 
-static int ltr501_powerdown(struct ltr501_data *data)
-{
-	return ltr501_write_contr(data->client,
-		data->als_contr & ~LTR501_CONTR_ACTIVE,
-		data->ps_contr & ~LTR501_CONTR_ACTIVE);
-}
-
 static int ltr501_remove(struct i2c_client *client)
 {
 	struct iio_dev *indio_dev = i2c_get_clientdata(client);
diff --git a/drivers/iio/magnetometer/Kconfig b/drivers/iio/magnetometer/Kconfig
index 4c7a4c52dd06..a5d6de72c523 100644
--- a/drivers/iio/magnetometer/Kconfig
+++ b/drivers/iio/magnetometer/Kconfig
@@ -18,6 +18,8 @@ config AK8975
 
 config AK09911
 	tristate "Asahi Kasei AK09911 3-axis Compass"
+	depends on I2C
+	depends on GPIOLIB
 	select AK8975
 	help
 	  Deprecated: AK09911 is now supported by AK8975 driver.
diff --git a/drivers/iio/magnetometer/mag3110.c b/drivers/iio/magnetometer/mag3110.c
index e3106b43ef48..261d517428e4 100644
--- a/drivers/iio/magnetometer/mag3110.c
+++ b/drivers/iio/magnetometer/mag3110.c
@@ -321,6 +321,12 @@ static const struct iio_info mag3110_info = {
 
 static const unsigned long mag3110_scan_masks[] = {0x7, 0xf, 0};
 
+static int mag3110_standby(struct mag3110_data *data)
+{
+	return i2c_smbus_write_byte_data(data->client, MAG3110_CTRL_REG1,
+		data->ctrl_reg1 & ~MAG3110_CTRL_AC);
+}
+
 static int mag3110_probe(struct i2c_client *client,
 			 const struct i2c_device_id *id)
 {
@@ -360,12 +366,12 @@ static int mag3110_probe(struct i2c_client *client,
 	ret = i2c_smbus_write_byte_data(client, MAG3110_CTRL_REG2,
 		MAG3110_CTRL_AUTO_MRST_EN);
 	if (ret < 0)
-		return ret;
+		goto standby_on_error;
 
 	ret = iio_triggered_buffer_setup(indio_dev, NULL,
 		mag3110_trigger_handler, NULL);
 	if (ret < 0)
-		return ret;
+		goto standby_on_error;
 
 	ret = iio_device_register(indio_dev);
 	if (ret < 0)
@@ -374,15 +380,11 @@ static int mag3110_probe(struct i2c_client *client,
 
 buffer_cleanup:
 	iio_triggered_buffer_cleanup(indio_dev);
+standby_on_error:
+	mag3110_standby(iio_priv(indio_dev));
 	return ret;
 }
 
-static int mag3110_standby(struct mag3110_data *data)
-{
-	return i2c_smbus_write_byte_data(data->client, MAG3110_CTRL_REG1,
-		data->ctrl_reg1 & ~MAG3110_CTRL_AC);
-}
-
 static int mag3110_remove(struct i2c_client *client)
 {
 	struct iio_dev *indio_dev = i2c_get_clientdata(client);
diff --git a/drivers/iio/pressure/Kconfig b/drivers/iio/pressure/Kconfig
index a3be53792072..fa6295041947 100644
--- a/drivers/iio/pressure/Kconfig
+++ b/drivers/iio/pressure/Kconfig
@@ -52,6 +52,33 @@ config MPL3115
           To compile this driver as a module, choose M here: the module
           will be called mpl3115.
 
+config MS5611
+	tristate "Measurement Specialities MS5611 pressure sensor driver"
+	help
+	  Say Y here to build support for the Measurement Specialities
+	  MS5611 pressure and temperature sensor.
+
+	  To compile this driver as a module, choose M here: the module will
+	  be called ms5611_core.
+
+config MS5611_I2C
+	tristate "support I2C bus connection"
+	depends on I2C && MS5611
+	help
+	  Say Y here to build I2C bus support for MS5611.
+
+	  To compile this driver as a module, choose M here: the module will
+	  be called ms5611_i2c.
+
+config MS5611_SPI
+	tristate "support SPI bus connection"
+	depends on SPI_MASTER && MS5611
+	help
+	  Say Y here to build SPI bus support for MS5611.
+
+	  To compile this driver as a module, choose M here: the module will
+	  be called ms5611_spi.
+
 config IIO_ST_PRESS
 	tristate "STMicroelectronics pressure sensor Driver"
 	depends on (I2C || SPI_MASTER) && SYSFS
diff --git a/drivers/iio/pressure/Makefile b/drivers/iio/pressure/Makefile
index 88011f2ae00e..a4f98f8d90ed 100644
--- a/drivers/iio/pressure/Makefile
+++ b/drivers/iio/pressure/Makefile
@@ -7,6 +7,9 @@ obj-$(CONFIG_BMP280) += bmp280.o
 obj-$(CONFIG_HID_SENSOR_PRESS)   += hid-sensor-press.o
 obj-$(CONFIG_MPL115) += mpl115.o
 obj-$(CONFIG_MPL3115) += mpl3115.o
+obj-$(CONFIG_MS5611) += ms5611_core.o
+obj-$(CONFIG_MS5611_I2C) += ms5611_i2c.o
+obj-$(CONFIG_MS5611_SPI) += ms5611_spi.o
 obj-$(CONFIG_IIO_ST_PRESS) += st_pressure.o
 st_pressure-y := st_pressure_core.o
 st_pressure-$(CONFIG_IIO_BUFFER) += st_pressure_buffer.o
diff --git a/drivers/iio/pressure/ms5611.h b/drivers/iio/pressure/ms5611.h
new file mode 100644
index 000000000000..099c6cdea43f
--- /dev/null
+++ b/drivers/iio/pressure/ms5611.h
@@ -0,0 +1,44 @@
+/*
+ * MS5611 pressure and temperature sensor driver
+ *
+ * Copyright (c) Tomasz Duszynski <tduszyns@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ */
+
+#ifndef _MS5611_H
+#define _MS5611_H
+
+#include <linux/device.h>
+#include <linux/iio/iio.h>
+#include <linux/mutex.h>
+
+#define MS5611_RESET			0x1e
+#define MS5611_READ_ADC			0x00
+#define MS5611_READ_PROM_WORD		0xA0
+#define MS5611_START_TEMP_CONV		0x58
+#define MS5611_START_PRESSURE_CONV	0x48
+
+#define MS5611_CONV_TIME_MIN		9040
+#define MS5611_CONV_TIME_MAX		10000
+
+#define MS5611_PROM_WORDS_NB		8
+
+struct ms5611_state {
+	void *client;
+	struct mutex lock;
+
+	int (*reset)(struct device *dev);
+	int (*read_prom_word)(struct device *dev, int index, u16 *word);
+	int (*read_adc_temp_and_pressure)(struct device *dev,
+					  s32 *temp, s32 *pressure);
+
+	u16 prom[MS5611_PROM_WORDS_NB];
+};
+
+int ms5611_probe(struct iio_dev *indio_dev, struct device *dev);
+
+#endif /* _MS5611_H */
diff --git a/drivers/iio/pressure/ms5611_core.c b/drivers/iio/pressure/ms5611_core.c
new file mode 100644
index 000000000000..e42c8531d9b3
--- /dev/null
+++ b/drivers/iio/pressure/ms5611_core.c
@@ -0,0 +1,215 @@
+/*
+ * MS5611 pressure and temperature sensor driver
+ *
+ * Copyright (c) Tomasz Duszynski <tduszyns@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * Data sheet:
+ *  http://www.meas-spec.com/downloads/MS5611-01BA03.pdf
+ *
+ */
+
+#include <linux/module.h>
+#include <linux/iio/iio.h>
+#include <linux/delay.h>
+
+#include "ms5611.h"
+
+static bool ms5611_prom_is_valid(u16 *prom, size_t len)
+{
+	int i, j;
+	uint16_t crc = 0, crc_orig = prom[7] & 0x000F;
+
+	prom[7] &= 0xFF00;
+
+	for (i = 0; i < len * 2; i++) {
+		if (i % 2 == 1)
+			crc ^= prom[i >> 1] & 0x00FF;
+		else
+			crc ^= prom[i >> 1] >> 8;
+
+		for (j = 0; j < 8; j++) {
+			if (crc & 0x8000)
+				crc = (crc << 1) ^ 0x3000;
+			else
+				crc <<= 1;
+		}
+	}
+
+	crc = (crc >> 12) & 0x000F;
+
+	return crc_orig != 0x0000 && crc == crc_orig;
+}
+
+static int ms5611_read_prom(struct iio_dev *indio_dev)
+{
+	int ret, i;
+	struct ms5611_state *st = iio_priv(indio_dev);
+
+	for (i = 0; i < MS5611_PROM_WORDS_NB; i++) {
+		ret = st->read_prom_word(&indio_dev->dev, i, &st->prom[i]);
+		if (ret < 0) {
+			dev_err(&indio_dev->dev,
+				"failed to read prom at %d\n", i);
+			return ret;
+		}
+	}
+
+	if (!ms5611_prom_is_valid(st->prom, MS5611_PROM_WORDS_NB)) {
+		dev_err(&indio_dev->dev, "PROM integrity check failed\n");
+		return -ENODEV;
+	}
+
+	return 0;
+}
+
+static int ms5611_read_temp_and_pressure(struct iio_dev *indio_dev,
+					 s32 *temp, s32 *pressure)
+{
+	int ret;
+	s32 t, p;
+	s64 off, sens, dt;
+	struct ms5611_state *st = iio_priv(indio_dev);
+
+	ret = st->read_adc_temp_and_pressure(&indio_dev->dev, &t, &p);
+	if (ret < 0) {
+		dev_err(&indio_dev->dev,
+			"failed to read temperature and pressure\n");
+		return ret;
+	}
+
+	dt = t - (st->prom[5] << 8);
+	off = ((s64)st->prom[2] << 16) + ((st->prom[4] * dt) >> 7);
+	sens = ((s64)st->prom[1] << 15) + ((st->prom[3] * dt) >> 8);
+
+	t = 2000 + ((st->prom[6] * dt) >> 23);
+	if (t < 2000) {
+		s64 off2, sens2, t2;
+
+		t2 = (dt * dt) >> 31;
+		off2 = (5 * (t - 2000) * (t - 2000)) >> 1;
+		sens2 = off2 >> 1;
+
+		if (t < -1500) {
+			s64 tmp = (t + 1500) * (t + 1500);
+
+			off2 += 7 * tmp;
+			sens2 += (11 * tmp) >> 1;
+		}
+
+		t -= t2;
+		off -= off2;
+		sens -= sens2;
+	}
+
+	*temp = t;
+	*pressure = (((p * sens) >> 21) - off) >> 15;
+
+	return 0;
+}
+
+static int ms5611_reset(struct iio_dev *indio_dev)
+{
+	int ret;
+	struct ms5611_state *st = iio_priv(indio_dev);
+
+	ret = st->reset(&indio_dev->dev);
+	if (ret < 0) {
+		dev_err(&indio_dev->dev, "failed to reset device\n");
+		return ret;
+	}
+
+	usleep_range(3000, 4000);
+
+	return 0;
+}
+
+static int ms5611_read_raw(struct iio_dev *indio_dev,
+			   struct iio_chan_spec const *chan,
+			   int *val, int *val2, long mask)
+{
+	int ret;
+	s32 temp, pressure;
+	struct ms5611_state *st = iio_priv(indio_dev);
+
+	switch (mask) {
+	case IIO_CHAN_INFO_PROCESSED:
+		mutex_lock(&st->lock);
+		ret = ms5611_read_temp_and_pressure(indio_dev,
+						    &temp, &pressure);
+		mutex_unlock(&st->lock);
+		if (ret < 0)
+			return ret;
+
+		switch (chan->type) {
+		case IIO_TEMP:
+			*val = temp * 10;
+			return IIO_VAL_INT;
+		case IIO_PRESSURE:
+			*val = pressure / 1000;
+			*val2 = (pressure % 1000) * 1000;
+			return IIO_VAL_INT_PLUS_MICRO;
+		default:
+			return -EINVAL;
+		}
+	}
+
+	return -EINVAL;
+}
+
+static const struct iio_chan_spec ms5611_channels[] = {
+	{
+		.type = IIO_PRESSURE,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_PROCESSED) |
+			BIT(IIO_CHAN_INFO_SCALE)
+	},
+	{
+		.type = IIO_TEMP,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_PROCESSED) |
+			BIT(IIO_CHAN_INFO_SCALE)
+	}
+};
+
+static const struct iio_info ms5611_info = {
+	.read_raw = &ms5611_read_raw,
+	.driver_module = THIS_MODULE,
+};
+
+static int ms5611_init(struct iio_dev *indio_dev)
+{
+	int ret;
+
+	ret = ms5611_reset(indio_dev);
+	if (ret < 0)
+		return ret;
+
+	return ms5611_read_prom(indio_dev);
+}
+
+int ms5611_probe(struct iio_dev *indio_dev, struct device *dev)
+{
+	int ret;
+	struct ms5611_state *st = iio_priv(indio_dev);
+
+	mutex_init(&st->lock);
+	indio_dev->dev.parent = dev;
+	indio_dev->name = dev->driver->name;
+	indio_dev->info = &ms5611_info;
+	indio_dev->channels = ms5611_channels;
+	indio_dev->num_channels = ARRAY_SIZE(ms5611_channels);
+	indio_dev->modes = INDIO_DIRECT_MODE;
+
+	ret = ms5611_init(indio_dev);
+	if (ret < 0)
+		return ret;
+
+	return devm_iio_device_register(dev, indio_dev);
+}
+EXPORT_SYMBOL(ms5611_probe);
+
+MODULE_AUTHOR("Tomasz Duszynski <tduszyns@gmail.com>");
+MODULE_DESCRIPTION("MS5611 core driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/iio/pressure/ms5611_i2c.c b/drivers/iio/pressure/ms5611_i2c.c
new file mode 100644
index 000000000000..748fd9acaad8
--- /dev/null
+++ b/drivers/iio/pressure/ms5611_i2c.c
@@ -0,0 +1,128 @@
+/*
+ * MS5611 pressure and temperature sensor driver (I2C bus)
+ *
+ * Copyright (c) Tomasz Duszynski <tduszyns@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * 7-bit I2C slave addresses:
+ *
+ * 0x77 (CSB pin low)
+ * 0x76 (CSB pin high)
+ *
+ */
+
+#include <linux/delay.h>
+#include <linux/i2c.h>
+#include <linux/module.h>
+
+#include "ms5611.h"
+
+static int ms5611_i2c_reset(struct device *dev)
+{
+	struct ms5611_state *st = iio_priv(dev_to_iio_dev(dev));
+
+	return i2c_smbus_write_byte(st->client, MS5611_RESET);
+}
+
+static int ms5611_i2c_read_prom_word(struct device *dev, int index, u16 *word)
+{
+	int ret;
+	struct ms5611_state *st = iio_priv(dev_to_iio_dev(dev));
+
+	ret = i2c_smbus_read_word_swapped(st->client,
+			MS5611_READ_PROM_WORD + (index << 1));
+	if (ret < 0)
+		return ret;
+
+	*word = ret;
+
+	return 0;
+}
+
+static int ms5611_i2c_read_adc(struct ms5611_state *st, s32 *val)
+{
+	int ret;
+	u8 buf[3];
+
+	ret = i2c_smbus_read_i2c_block_data(st->client, MS5611_READ_ADC,
+					    3, buf);
+	if (ret < 0)
+		return ret;
+
+	*val = (buf[0] << 16) | (buf[1] << 8) | buf[2];
+
+	return 0;
+}
+
+static int ms5611_i2c_read_adc_temp_and_pressure(struct device *dev,
+						 s32 *temp, s32 *pressure)
+{
+	int ret;
+	struct ms5611_state *st = iio_priv(dev_to_iio_dev(dev));
+
+	ret = i2c_smbus_write_byte(st->client, MS5611_START_TEMP_CONV);
+	if (ret < 0)
+		return ret;
+
+	usleep_range(MS5611_CONV_TIME_MIN, MS5611_CONV_TIME_MAX);
+
+	ret = ms5611_i2c_read_adc(st, temp);
+	if (ret < 0)
+		return ret;
+
+	ret = i2c_smbus_write_byte(st->client, MS5611_START_PRESSURE_CONV);
+	if (ret < 0)
+		return ret;
+
+	usleep_range(MS5611_CONV_TIME_MIN, MS5611_CONV_TIME_MAX);
+
+	return ms5611_i2c_read_adc(st, pressure);
+}
+
+static int ms5611_i2c_probe(struct i2c_client *client,
+			    const struct i2c_device_id *id)
+{
+	struct ms5611_state *st;
+	struct iio_dev *indio_dev;
+
+	if (!i2c_check_functionality(client->adapter,
+				     I2C_FUNC_SMBUS_WRITE_BYTE |
+				     I2C_FUNC_SMBUS_READ_WORD_DATA |
+				     I2C_FUNC_SMBUS_READ_I2C_BLOCK))
+		return -ENODEV;
+
+	indio_dev = devm_iio_device_alloc(&client->dev, sizeof(*st));
+	if (!indio_dev)
+		return -ENOMEM;
+
+	st = iio_priv(indio_dev);
+	st->reset = ms5611_i2c_reset;
+	st->read_prom_word = ms5611_i2c_read_prom_word;
+	st->read_adc_temp_and_pressure = ms5611_i2c_read_adc_temp_and_pressure;
+	st->client = client;
+
+	return ms5611_probe(indio_dev, &client->dev);
+}
+
+static const struct i2c_device_id ms5611_id[] = {
+	{ "ms5611", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, ms5611_id);
+
+static struct i2c_driver ms5611_driver = {
+	.driver = {
+		.name = "ms5611",
+		.owner = THIS_MODULE,
+	},
+	.id_table = ms5611_id,
+	.probe = ms5611_i2c_probe,
+};
+module_i2c_driver(ms5611_driver);
+
+MODULE_AUTHOR("Tomasz Duszynski <tduszyns@gmail.com>");
+MODULE_DESCRIPTION("MS5611 i2c driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/iio/pressure/ms5611_spi.c b/drivers/iio/pressure/ms5611_spi.c
new file mode 100644
index 000000000000..976726fd4e6c
--- /dev/null
+++ b/drivers/iio/pressure/ms5611_spi.c
@@ -0,0 +1,127 @@
+/*
+ * MS5611 pressure and temperature sensor driver (SPI bus)
+ *
+ * Copyright (c) Tomasz Duszynski <tduszyns@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ */
+
+#include <linux/delay.h>
+#include <linux/module.h>
+#include <linux/spi/spi.h>
+
+#include "ms5611.h"
+
+static int ms5611_spi_reset(struct device *dev)
+{
+	u8 cmd = MS5611_RESET;
+	struct ms5611_state *st = iio_priv(dev_to_iio_dev(dev));
+
+	return spi_write_then_read(st->client, &cmd, 1, NULL, 0);
+}
+
+static int ms5611_spi_read_prom_word(struct device *dev, int index, u16 *word)
+{
+	int ret;
+	struct ms5611_state *st = iio_priv(dev_to_iio_dev(dev));
+
+	ret = spi_w8r16be(st->client, MS5611_READ_PROM_WORD + (index << 1));
+	if (ret < 0)
+		return ret;
+
+	*word = ret;
+
+	return 0;
+}
+
+static int ms5611_spi_read_adc(struct device *dev, s32 *val)
+{
+	int ret;
+	u8 buf[3] = { MS5611_READ_ADC };
+	struct ms5611_state *st = iio_priv(dev_to_iio_dev(dev));
+
+	ret = spi_write_then_read(st->client, buf, 1, buf, 3);
+	if (ret < 0)
+		return ret;
+
+	*val = (buf[0] << 16) | (buf[1] << 8) | buf[2];
+
+	return 0;
+}
+
+static int ms5611_spi_read_adc_temp_and_pressure(struct device *dev,
+						 s32 *temp, s32 *pressure)
+{
+	u8 cmd;
+	int ret;
+	struct ms5611_state *st = iio_priv(dev_to_iio_dev(dev));
+
+	cmd = MS5611_START_TEMP_CONV;
+	ret = spi_write_then_read(st->client, &cmd, 1, NULL, 0);
+	if (ret < 0)
+		return ret;
+
+	usleep_range(MS5611_CONV_TIME_MIN, MS5611_CONV_TIME_MAX);
+
+	ret = ms5611_spi_read_adc(dev, temp);
+	if (ret < 0)
+		return ret;
+
+	cmd = MS5611_START_PRESSURE_CONV;
+	ret = spi_write_then_read(st->client, &cmd, 1, NULL, 0);
+	if (ret < 0)
+		return ret;
+
+	usleep_range(MS5611_CONV_TIME_MIN, MS5611_CONV_TIME_MAX);
+
+	return ms5611_spi_read_adc(dev, pressure);
+}
+
+static int ms5611_spi_probe(struct spi_device *spi)
+{
+	int ret;
+	struct ms5611_state *st;
+	struct iio_dev *indio_dev;
+
+	indio_dev = devm_iio_device_alloc(&spi->dev, sizeof(*st));
+	if (!indio_dev)
+		return -ENOMEM;
+
+	spi->mode = SPI_MODE_0;
+	spi->max_speed_hz = 20000000;
+	spi->bits_per_word = 8;
+	ret = spi_setup(spi);
+	if (ret < 0)
+		return ret;
+
+	st = iio_priv(indio_dev);
+	st->reset = ms5611_spi_reset;
+	st->read_prom_word = ms5611_spi_read_prom_word;
+	st->read_adc_temp_and_pressure = ms5611_spi_read_adc_temp_and_pressure;
+	st->client = spi;
+
+	return ms5611_probe(indio_dev, &spi->dev);
+}
+
+static const struct spi_device_id ms5611_id[] = {
+	{ "ms5611", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(spi, ms5611_id);
+
+static struct spi_driver ms5611_driver = {
+	.driver = {
+		.name = "ms5611",
+		.owner = THIS_MODULE,
+	},
+	.id_table = ms5611_id,
+	.probe = ms5611_spi_probe,
+};
+module_spi_driver(ms5611_driver);
+
+MODULE_AUTHOR("Tomasz Duszynski <tduszyns@gmail.com>");
+MODULE_DESCRIPTION("MS5611 spi driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/iio/proximity/sx9500.c b/drivers/iio/proximity/sx9500.c
index 74dff4e4a11a..fa40f6d0ca39 100644
--- a/drivers/iio/proximity/sx9500.c
+++ b/drivers/iio/proximity/sx9500.c
@@ -494,7 +494,7 @@ static irqreturn_t sx9500_trigger_handler(int irq, void *private)
 
 	mutex_lock(&data->mutex);
 
-	for_each_set_bit(bit, indio_dev->buffer->scan_mask,
+	for_each_set_bit(bit, indio_dev->active_scan_mask,
 			 indio_dev->masklength) {
 		ret = sx9500_read_proximity(data, &indio_dev->channels[bit],
 					    &val);
@@ -618,16 +618,12 @@ static int sx9500_gpio_probe(struct i2c_client *client,
 	dev = &client->dev;
 
 	/* data ready gpio interrupt pin */
-	gpio = devm_gpiod_get_index(dev, SX9500_GPIO_NAME, 0);
+	gpio = devm_gpiod_get_index(dev, SX9500_GPIO_NAME, 0, GPIOD_IN);
 	if (IS_ERR(gpio)) {
 		dev_err(dev, "acpi gpio get index failed\n");
 		return PTR_ERR(gpio);
 	}
 
-	ret = gpiod_direction_input(gpio);
-	if (ret)
-		return ret;
-
 	ret = gpiod_to_irq(gpio);
 
 	dev_dbg(dev, "GPIO resource, no:%d irq:%d\n", desc_to_gpio(gpio), ret);
diff --git a/drivers/iio/temperature/mlx90614.c b/drivers/iio/temperature/mlx90614.c
index c8b6ac8b2d69..a112fc9abf43 100644
--- a/drivers/iio/temperature/mlx90614.c
+++ b/drivers/iio/temperature/mlx90614.c
@@ -2,6 +2,7 @@
  * mlx90614.c - Support for Melexis MLX90614 contactless IR temperature sensor
  *
  * Copyright (c) 2014 Peter Meerwald <pmeerw@pmeerw.net>
+ * Copyright (c) 2015 Essensium NV
  *
  * This file is subject to the terms and conditions of version 2 of
  * the GNU General Public License.  See the file COPYING in the main
@@ -20,11 +21,35 @@
 
 #include <linux/iio/iio.h>
 
-#define MLX90614_OP_RAM 0x00
+#define MLX90614_OP_RAM		0x00
+#define MLX90614_OP_EEPROM	0x20
+#define MLX90614_OP_SLEEP	0xff
 
 /* RAM offsets with 16-bit data, MSB first */
-#define MLX90614_TA 0x06 /* ambient temperature */
-#define MLX90614_TOBJ1 0x07 /* object temperature */
+#define MLX90614_RAW1	(MLX90614_OP_RAM | 0x04) /* raw data IR channel 1 */
+#define MLX90614_RAW2	(MLX90614_OP_RAM | 0x05) /* raw data IR channel 2 */
+#define MLX90614_TA	(MLX90614_OP_RAM | 0x06) /* ambient temperature */
+#define MLX90614_TOBJ1	(MLX90614_OP_RAM | 0x07) /* object 1 temperature */
+#define MLX90614_TOBJ2	(MLX90614_OP_RAM | 0x08) /* object 2 temperature */
+
+/* EEPROM offsets with 16-bit data, MSB first */
+#define MLX90614_EMISSIVITY	(MLX90614_OP_EEPROM | 0x04) /* emissivity correction coefficient */
+#define MLX90614_CONFIG		(MLX90614_OP_EEPROM | 0x05) /* configuration register */
+
+/* Control bits in configuration register */
+#define MLX90614_CONFIG_IIR_SHIFT 0 /* IIR coefficient */
+#define MLX90614_CONFIG_IIR_MASK (0x7 << MLX90614_CONFIG_IIR_SHIFT)
+#define MLX90614_CONFIG_DUAL_SHIFT 6 /* single (0) or dual (1) IR sensor */
+#define MLX90614_CONFIG_DUAL_MASK (1 << MLX90614_CONFIG_DUAL_SHIFT)
+#define MLX90614_CONFIG_FIR_SHIFT 8 /* FIR coefficient */
+#define MLX90614_CONFIG_FIR_MASK (0x7 << MLX90614_CONFIG_FIR_SHIFT)
+#define MLX90614_CONFIG_GAIN_SHIFT 11 /* gain */
+#define MLX90614_CONFIG_GAIN_MASK (0x7 << MLX90614_CONFIG_GAIN_SHIFT)
+
+/* Timings (in ms) */
+#define MLX90614_TIMING_EEPROM 20 /* time for EEPROM write/erase to complete */
+#define MLX90614_TIMING_WAKEUP 34 /* time to hold SDA low for wake-up */
+#define MLX90614_TIMING_STARTUP 250 /* time before first data after wake-up */
 
 struct mlx90614_data {
 	struct i2c_client *client;
@@ -35,26 +60,34 @@ static int mlx90614_read_raw(struct iio_dev *indio_dev,
 			    int *val2, long mask)
 {
 	struct mlx90614_data *data = iio_priv(indio_dev);
+	u8 cmd;
 	s32 ret;
 
 	switch (mask) {
 	case IIO_CHAN_INFO_RAW: /* 0.02K / LSB */
 		switch (channel->channel2) {
 		case IIO_MOD_TEMP_AMBIENT:
-			ret = i2c_smbus_read_word_data(data->client,
-			    MLX90614_OP_RAM | MLX90614_TA);
-			if (ret < 0)
-				return ret;
+			cmd = MLX90614_TA;
 			break;
 		case IIO_MOD_TEMP_OBJECT:
-			ret = i2c_smbus_read_word_data(data->client,
-			    MLX90614_OP_RAM | MLX90614_TOBJ1);
-			if (ret < 0)
-				return ret;
+			switch (channel->channel) {
+			case 0:
+				cmd = MLX90614_TOBJ1;
+				break;
+			case 1:
+				cmd = MLX90614_TOBJ2;
+				break;
+			default:
+				return -EINVAL;
+			}
 			break;
 		default:
 			return -EINVAL;
 		}
+
+		ret = i2c_smbus_read_word_data(data->client, cmd);
+		if (ret < 0)
+			return ret;
 		*val = ret;
 		return IIO_VAL_INT;
 	case IIO_CHAN_INFO_OFFSET:
@@ -86,6 +119,16 @@ static const struct iio_chan_spec mlx90614_channels[] = {
 		.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_OFFSET) |
 		    BIT(IIO_CHAN_INFO_SCALE),
 	},
+	{
+		.type = IIO_TEMP,
+		.indexed = 1,
+		.modified = 1,
+		.channel = 1,
+		.channel2 = IIO_MOD_TEMP_OBJECT,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),
+		.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_OFFSET) |
+		    BIT(IIO_CHAN_INFO_SCALE),
+	},
 };
 
 static const struct iio_info mlx90614_info = {
@@ -93,11 +136,25 @@ static const struct iio_info mlx90614_info = {
 	.driver_module = THIS_MODULE,
 };
 
+/* Return 0 for single sensor, 1 for dual sensor, <0 on error. */
+static int mlx90614_probe_num_ir_sensors(struct i2c_client *client)
+{
+	s32 ret;
+
+	ret = i2c_smbus_read_word_data(client, MLX90614_CONFIG);
+
+	if (ret < 0)
+		return ret;
+
+	return (ret & MLX90614_CONFIG_DUAL_MASK) ? 1 : 0;
+}
+
 static int mlx90614_probe(struct i2c_client *client,
 			 const struct i2c_device_id *id)
 {
 	struct iio_dev *indio_dev;
 	struct mlx90614_data *data;
+	int ret;
 
 	if (!i2c_check_functionality(client->adapter, I2C_FUNC_SMBUS_WORD_DATA))
 		return -ENODEV;
@@ -115,8 +172,21 @@ static int mlx90614_probe(struct i2c_client *client,
 	indio_dev->modes = INDIO_DIRECT_MODE;
 	indio_dev->info = &mlx90614_info;
 
-	indio_dev->channels = mlx90614_channels;
-	indio_dev->num_channels = ARRAY_SIZE(mlx90614_channels);
+	ret = mlx90614_probe_num_ir_sensors(client);
+	switch (ret) {
+	case 0:
+		dev_dbg(&client->dev, "Found single sensor");
+		indio_dev->channels = mlx90614_channels;
+		indio_dev->num_channels = 2;
+		break;
+	case 1:
+		dev_dbg(&client->dev, "Found dual sensor");
+		indio_dev->channels = mlx90614_channels;
+		indio_dev->num_channels = 3;
+		break;
+	default:
+		return ret;
+	}
 
 	return iio_device_register(indio_dev);
 }
@@ -146,5 +216,6 @@ static struct i2c_driver mlx90614_driver = {
 module_i2c_driver(mlx90614_driver);
 
 MODULE_AUTHOR("Peter Meerwald <pmeerw@pmeerw.net>");
+MODULE_AUTHOR("Vianney le Clément de Saint-Marcq <vianney.leclement@essensium.com>");
 MODULE_DESCRIPTION("Melexis MLX90614 contactless IR temperature sensor driver");
 MODULE_LICENSE("GPL");
diff --git a/drivers/infiniband/core/umem.c b/drivers/infiniband/core/umem.c
index aec7a6aa2951..8c014b5dab4c 100644
--- a/drivers/infiniband/core/umem.c
+++ b/drivers/infiniband/core/umem.c
@@ -99,6 +99,14 @@ struct ib_umem *ib_umem_get(struct ib_ucontext *context, unsigned long addr,
 	if (dmasync)
 		dma_set_attr(DMA_ATTR_WRITE_BARRIER, &attrs);
 
+	/*
+	 * If the combination of the addr and size requested for this memory
+	 * region causes an integer overflow, return error.
+	 */
+	if ((PAGE_ALIGN(addr + size) <= size) ||
+	    (PAGE_ALIGN(addr + size) <= addr))
+		return ERR_PTR(-EINVAL);
+
 	if (!can_do_mlock())
 		return ERR_PTR(-EPERM);
 
diff --git a/drivers/infiniband/hw/mlx4/mad.c b/drivers/infiniband/hw/mlx4/mad.c
index c7619716c31d..59040265e361 100644
--- a/drivers/infiniband/hw/mlx4/mad.c
+++ b/drivers/infiniband/hw/mlx4/mad.c
@@ -64,6 +64,14 @@ enum {
 #define GUID_TBL_BLK_NUM_ENTRIES 8
 #define GUID_TBL_BLK_SIZE (GUID_TBL_ENTRY_SIZE * GUID_TBL_BLK_NUM_ENTRIES)
 
+/* Counters should be saturate once they reach their maximum value */
+#define ASSIGN_32BIT_COUNTER(counter, value) do {\
+	if ((value) > U32_MAX)			 \
+		counter = cpu_to_be32(U32_MAX); \
+	else					 \
+		counter = cpu_to_be32(value);	 \
+} while (0)
+
 struct mlx4_mad_rcv_buf {
 	struct ib_grh grh;
 	u8 payload[256];
@@ -806,10 +814,14 @@ static int ib_process_mad(struct ib_device *ibdev, int mad_flags, u8 port_num,
 static void edit_counter(struct mlx4_counter *cnt,
 					struct ib_pma_portcounters *pma_cnt)
 {
-	pma_cnt->port_xmit_data = cpu_to_be32((be64_to_cpu(cnt->tx_bytes)>>2));
-	pma_cnt->port_rcv_data  = cpu_to_be32((be64_to_cpu(cnt->rx_bytes)>>2));
-	pma_cnt->port_xmit_packets = cpu_to_be32(be64_to_cpu(cnt->tx_frames));
-	pma_cnt->port_rcv_packets  = cpu_to_be32(be64_to_cpu(cnt->rx_frames));
+	ASSIGN_32BIT_COUNTER(pma_cnt->port_xmit_data,
+			     (be64_to_cpu(cnt->tx_bytes) >> 2));
+	ASSIGN_32BIT_COUNTER(pma_cnt->port_rcv_data,
+			     (be64_to_cpu(cnt->rx_bytes) >> 2));
+	ASSIGN_32BIT_COUNTER(pma_cnt->port_xmit_packets,
+			     be64_to_cpu(cnt->tx_frames));
+	ASSIGN_32BIT_COUNTER(pma_cnt->port_rcv_packets,
+			     be64_to_cpu(cnt->rx_frames));
 }
 
 static int iboe_process_mad(struct ib_device *ibdev, int mad_flags, u8 port_num,
diff --git a/drivers/infiniband/hw/mlx4/main.c b/drivers/infiniband/hw/mlx4/main.c
index ac6e2b710ea6..b972c0b41799 100644
--- a/drivers/infiniband/hw/mlx4/main.c
+++ b/drivers/infiniband/hw/mlx4/main.c
@@ -2697,8 +2697,12 @@ static void handle_bonded_port_state_event(struct work_struct *work)
 	spin_lock_bh(&ibdev->iboe.lock);
 	for (i = 0; i < MLX4_MAX_PORTS; ++i) {
 		struct net_device *curr_netdev = ibdev->iboe.netdevs[i];
+		enum ib_port_state curr_port_state;
 
-		enum ib_port_state curr_port_state =
+		if (!curr_netdev)
+			continue;
+
+		curr_port_state =
 			(netif_running(curr_netdev) &&
 			 netif_carrier_ok(curr_netdev)) ?
 			IB_PORT_ACTIVE : IB_PORT_DOWN;
diff --git a/drivers/input/keyboard/tc3589x-keypad.c b/drivers/input/keyboard/tc3589x-keypad.c
index 8ff612d160b0..563932500ff1 100644
--- a/drivers/input/keyboard/tc3589x-keypad.c
+++ b/drivers/input/keyboard/tc3589x-keypad.c
@@ -411,9 +411,9 @@ static int tc3589x_keypad_probe(struct platform_device *pdev)
 
 	input_set_drvdata(input, keypad);
 
-	error = request_threaded_irq(irq, NULL,
-			tc3589x_keypad_irq, plat->irqtype,
-			"tc3589x-keypad", keypad);
+	error = request_threaded_irq(irq, NULL, tc3589x_keypad_irq,
+				     plat->irqtype | IRQF_ONESHOT,
+				     "tc3589x-keypad", keypad);
 	if (error < 0) {
 		dev_err(&pdev->dev,
 				"Could not allocate irq %d,error %d\n",
diff --git a/drivers/input/misc/mma8450.c b/drivers/input/misc/mma8450.c
index 59d4dcddf6de..98228773a111 100644
--- a/drivers/input/misc/mma8450.c
+++ b/drivers/input/misc/mma8450.c
@@ -187,6 +187,7 @@ static int mma8450_probe(struct i2c_client *c,
 	idev->private		= m;
 	idev->input->name	= MMA8450_DRV_NAME;
 	idev->input->id.bustype	= BUS_I2C;
+	idev->input->dev.parent = &c->dev;
 	idev->poll		= mma8450_poll;
 	idev->poll_interval	= POLL_INTERVAL;
 	idev->poll_interval_max	= POLL_INTERVAL_MAX;
diff --git a/drivers/input/mouse/alps.c b/drivers/input/mouse/alps.c
index d28726a0ef85..27bcdbc950c9 100644
--- a/drivers/input/mouse/alps.c
+++ b/drivers/input/mouse/alps.c
@@ -1154,10 +1154,28 @@ out:
 	mutex_unlock(&alps_mutex);
 }
 
-static void alps_report_bare_ps2_packet(struct input_dev *dev,
+static void alps_report_bare_ps2_packet(struct psmouse *psmouse,
 					unsigned char packet[],
 					bool report_buttons)
 {
+	struct alps_data *priv = psmouse->private;
+	struct input_dev *dev;
+
+	/* Figure out which device to use to report the bare packet */
+	if (priv->proto_version == ALPS_PROTO_V2 &&
+	    (priv->flags & ALPS_DUALPOINT)) {
+		/* On V2 devices the DualPoint Stick reports bare packets */
+		dev = priv->dev2;
+	} else if (unlikely(IS_ERR_OR_NULL(priv->dev3))) {
+		/* Register dev3 mouse if we received PS/2 packet first time */
+		if (!IS_ERR(priv->dev3))
+			psmouse_queue_work(psmouse, &priv->dev3_register_work,
+					   0);
+		return;
+	} else {
+		dev = priv->dev3;
+	}
+
 	if (report_buttons)
 		alps_report_buttons(dev, NULL,
 				packet[0] & 1, packet[0] & 2, packet[0] & 4);
@@ -1232,8 +1250,8 @@ static psmouse_ret_t alps_handle_interleaved_ps2(struct psmouse *psmouse)
 		 * de-synchronization.
 		 */
 
-		alps_report_bare_ps2_packet(priv->dev2,
-					    &psmouse->packet[3], false);
+		alps_report_bare_ps2_packet(psmouse, &psmouse->packet[3],
+					    false);
 
 		/*
 		 * Continue with the standard ALPS protocol handling,
@@ -1289,18 +1307,9 @@ static psmouse_ret_t alps_process_byte(struct psmouse *psmouse)
 	 * properly we only do this if the device is fully synchronized.
 	 */
 	if (!psmouse->out_of_sync_cnt && (psmouse->packet[0] & 0xc8) == 0x08) {
-
-		/* Register dev3 mouse if we received PS/2 packet first time */
-		if (unlikely(!priv->dev3))
-			psmouse_queue_work(psmouse,
-					   &priv->dev3_register_work, 0);
-
 		if (psmouse->pktcnt == 3) {
-			/* Once dev3 mouse device is registered report data */
-			if (likely(!IS_ERR_OR_NULL(priv->dev3)))
-				alps_report_bare_ps2_packet(priv->dev3,
-							    psmouse->packet,
-							    true);
+			alps_report_bare_ps2_packet(psmouse, psmouse->packet,
+						    true);
 			return PSMOUSE_FULL_PACKET;
 		}
 		return PSMOUSE_GOOD_DATA;
@@ -2281,10 +2290,12 @@ static int alps_set_protocol(struct psmouse *psmouse,
 		priv->set_abs_params = alps_set_abs_params_mt;
 		priv->nibble_commands = alps_v3_nibble_commands;
 		priv->addr_command = PSMOUSE_CMD_RESET_WRAP;
-		priv->x_max = 1360;
-		priv->y_max = 660;
 		priv->x_bits = 23;
 		priv->y_bits = 12;
+
+		if (alps_dolphin_get_device_area(psmouse, priv))
+			return -EIO;
+
 		break;
 
 	case ALPS_PROTO_V6:
@@ -2303,9 +2314,8 @@ static int alps_set_protocol(struct psmouse *psmouse,
 		priv->set_abs_params = alps_set_abs_params_mt;
 		priv->nibble_commands = alps_v3_nibble_commands;
 		priv->addr_command = PSMOUSE_CMD_RESET_WRAP;
-
-		if (alps_dolphin_get_device_area(psmouse, priv))
-			return -EIO;
+		priv->x_max = 0xfff;
+		priv->y_max = 0x7ff;
 
 		if (priv->fw_ver[1] != 0xba)
 			priv->flags |= ALPS_BUTTONPAD;
@@ -2605,8 +2615,10 @@ int alps_detect(struct psmouse *psmouse, bool set_properties)
 		return -ENOMEM;
 
 	error = alps_identify(psmouse, priv);
-	if (error)
+	if (error) {
+		kfree(priv);
 		return error;
+	}
 
 	if (set_properties) {
 		psmouse->vendor = "ALPS";
diff --git a/drivers/input/mouse/cyapa_gen3.c b/drivers/input/mouse/cyapa_gen3.c
index 77e9d70a986b..1e2291c378fe 100644
--- a/drivers/input/mouse/cyapa_gen3.c
+++ b/drivers/input/mouse/cyapa_gen3.c
@@ -20,7 +20,7 @@
 #include <linux/input/mt.h>
 #include <linux/module.h>
 #include <linux/slab.h>
-#include <linux/unaligned/access_ok.h>
+#include <asm/unaligned.h>
 #include "cyapa.h"
 
 
diff --git a/drivers/input/mouse/cyapa_gen5.c b/drivers/input/mouse/cyapa_gen5.c
index ddf5393a1180..5b611dd71e79 100644
--- a/drivers/input/mouse/cyapa_gen5.c
+++ b/drivers/input/mouse/cyapa_gen5.c
@@ -17,7 +17,7 @@
 #include <linux/mutex.h>
 #include <linux/completion.h>
 #include <linux/slab.h>
-#include <linux/unaligned/access_ok.h>
+#include <asm/unaligned.h>
 #include <linux/crc-itu-t.h>
 #include "cyapa.h"
 
@@ -1926,7 +1926,7 @@ static int cyapa_gen5_read_idac_data(struct cyapa *cyapa,
 				electrodes_tx = cyapa->electrodes_x;
 			max_element_cnt = ((cyapa->aligned_electrodes_rx + 7) &
 						~7u) * electrodes_tx;
-		} else if (idac_data_type == GEN5_RETRIEVE_SELF_CAP_PWC_DATA) {
+		} else {
 			offset = 2;
 			max_element_cnt = cyapa->electrodes_x +
 						cyapa->electrodes_y;
diff --git a/drivers/input/mouse/focaltech.c b/drivers/input/mouse/focaltech.c
index 757f78a94aec..23d259416f2f 100644
--- a/drivers/input/mouse/focaltech.c
+++ b/drivers/input/mouse/focaltech.c
@@ -67,9 +67,6 @@ static void focaltech_reset(struct psmouse *psmouse)
 
 #define FOC_MAX_FINGERS 5
 
-#define FOC_MAX_X 2431
-#define FOC_MAX_Y 1663
-
 /*
  * Current state of a single finger on the touchpad.
  */
@@ -129,9 +126,17 @@ static void focaltech_report_state(struct psmouse *psmouse)
 		input_mt_slot(dev, i);
 		input_mt_report_slot_state(dev, MT_TOOL_FINGER, active);
 		if (active) {
-			input_report_abs(dev, ABS_MT_POSITION_X, finger->x);
+			unsigned int clamped_x, clamped_y;
+			/*
+			 * The touchpad might report invalid data, so we clamp
+			 * the resulting values so that we do not confuse
+			 * userspace.
+			 */
+			clamped_x = clamp(finger->x, 0U, priv->x_max);
+			clamped_y = clamp(finger->y, 0U, priv->y_max);
+			input_report_abs(dev, ABS_MT_POSITION_X, clamped_x);
 			input_report_abs(dev, ABS_MT_POSITION_Y,
-					 FOC_MAX_Y - finger->y);
+					 priv->y_max - clamped_y);
 		}
 	}
 	input_mt_report_pointer_emulation(dev, true);
@@ -180,16 +185,6 @@ static void focaltech_process_abs_packet(struct psmouse *psmouse,
 
 	state->pressed = (packet[0] >> 4) & 1;
 
-	/*
-	 * packet[5] contains some kind of tool size in the most
-	 * significant nibble. 0xff is a special value (latching) that
-	 * signals a large contact area.
-	 */
-	if (packet[5] == 0xff) {
-		state->fingers[finger].valid = false;
-		return;
-	}
-
 	state->fingers[finger].x = ((packet[1] & 0xf) << 8) | packet[2];
 	state->fingers[finger].y = (packet[3] << 8) | packet[4];
 	state->fingers[finger].valid = true;
@@ -381,6 +376,23 @@ static int focaltech_read_size(struct psmouse *psmouse)
 
 	return 0;
 }
+
+void focaltech_set_resolution(struct psmouse *psmouse, unsigned int resolution)
+{
+	/* not supported yet */
+}
+
+static void focaltech_set_rate(struct psmouse *psmouse, unsigned int rate)
+{
+	/* not supported yet */
+}
+
+static void focaltech_set_scale(struct psmouse *psmouse,
+				enum psmouse_scale scale)
+{
+	/* not supported yet */
+}
+
 int focaltech_init(struct psmouse *psmouse)
 {
 	struct focaltech_data *priv;
@@ -415,6 +427,14 @@ int focaltech_init(struct psmouse *psmouse)
 	psmouse->cleanup = focaltech_reset;
 	/* resync is not supported yet */
 	psmouse->resync_time = 0;
+	/*
+	 * rate/resolution/scale changes are not supported yet, and
+	 * the generic implementations of these functions seem to
+	 * confuse some touchpads
+	 */
+	psmouse->set_resolution = focaltech_set_resolution;
+	psmouse->set_rate = focaltech_set_rate;
+	psmouse->set_scale = focaltech_set_scale;
 
 	return 0;
 
diff --git a/drivers/input/mouse/psmouse-base.c b/drivers/input/mouse/psmouse-base.c
index 4ccd01d7a48d..8bc61237bc1b 100644
--- a/drivers/input/mouse/psmouse-base.c
+++ b/drivers/input/mouse/psmouse-base.c
@@ -454,6 +454,17 @@ static void psmouse_set_rate(struct psmouse *psmouse, unsigned int rate)
 }
 
 /*
+ * Here we set the mouse scaling.
+ */
+
+static void psmouse_set_scale(struct psmouse *psmouse, enum psmouse_scale scale)
+{
+	ps2_command(&psmouse->ps2dev, NULL,
+		    scale == PSMOUSE_SCALE21 ? PSMOUSE_CMD_SETSCALE21 :
+					       PSMOUSE_CMD_SETSCALE11);
+}
+
+/*
  * psmouse_poll() - default poll handler. Everyone except for ALPS uses it.
  */
 
@@ -689,6 +700,7 @@ static void psmouse_apply_defaults(struct psmouse *psmouse)
 
 	psmouse->set_rate = psmouse_set_rate;
 	psmouse->set_resolution = psmouse_set_resolution;
+	psmouse->set_scale = psmouse_set_scale;
 	psmouse->poll = psmouse_poll;
 	psmouse->protocol_handler = psmouse_process_byte;
 	psmouse->pktsize = 3;
@@ -1160,7 +1172,7 @@ static void psmouse_initialize(struct psmouse *psmouse)
 	if (psmouse_max_proto != PSMOUSE_PS2) {
 		psmouse->set_rate(psmouse, psmouse->rate);
 		psmouse->set_resolution(psmouse, psmouse->resolution);
-		ps2_command(&psmouse->ps2dev, NULL, PSMOUSE_CMD_SETSCALE11);
+		psmouse->set_scale(psmouse, PSMOUSE_SCALE11);
 	}
 }
 
diff --git a/drivers/input/mouse/psmouse.h b/drivers/input/mouse/psmouse.h
index c2ff137ecbdb..d02e1bdc9ae4 100644
--- a/drivers/input/mouse/psmouse.h
+++ b/drivers/input/mouse/psmouse.h
@@ -36,6 +36,11 @@ typedef enum {
 	PSMOUSE_FULL_PACKET
 } psmouse_ret_t;
 
+enum psmouse_scale {
+	PSMOUSE_SCALE11,
+	PSMOUSE_SCALE21
+};
+
 struct psmouse {
 	void *private;
 	struct input_dev *dev;
@@ -67,6 +72,7 @@ struct psmouse {
 	psmouse_ret_t (*protocol_handler)(struct psmouse *psmouse);
 	void (*set_rate)(struct psmouse *psmouse, unsigned int rate);
 	void (*set_resolution)(struct psmouse *psmouse, unsigned int resolution);
+	void (*set_scale)(struct psmouse *psmouse, enum psmouse_scale scale);
 
 	int (*reconnect)(struct psmouse *psmouse);
 	void (*disconnect)(struct psmouse *psmouse);
diff --git a/drivers/input/mouse/synaptics.c b/drivers/input/mouse/synaptics.c
index f2cceb6493a0..3b06c8a360b6 100644
--- a/drivers/input/mouse/synaptics.c
+++ b/drivers/input/mouse/synaptics.c
@@ -67,9 +67,6 @@
 #define X_MAX_POSITIVE 8176
 #define Y_MAX_POSITIVE 8176
 
-/* maximum ABS_MT_POSITION displacement (in mm) */
-#define DMAX 10
-
 /*****************************************************************************
  *	Stuff we need even when we do not want native Synaptics support
  ****************************************************************************/
@@ -123,32 +120,46 @@ void synaptics_reset(struct psmouse *psmouse)
 
 static bool cr48_profile_sensor;
 
+#define ANY_BOARD_ID 0
 struct min_max_quirk {
 	const char * const *pnp_ids;
+	struct {
+		unsigned long int min, max;
+	} board_id;
 	int x_min, x_max, y_min, y_max;
 };
 
 static const struct min_max_quirk min_max_pnpid_table[] = {
 	{
 		(const char * const []){"LEN0033", NULL},
+		{ANY_BOARD_ID, ANY_BOARD_ID},
 		1024, 5052, 2258, 4832
 	},
 	{
-		(const char * const []){"LEN0035", "LEN0042", NULL},
+		(const char * const []){"LEN0042", NULL},
+		{ANY_BOARD_ID, ANY_BOARD_ID},
 		1232, 5710, 1156, 4696
 	},
 	{
 		(const char * const []){"LEN0034", "LEN0036", "LEN0037",
 					"LEN0039", "LEN2002", "LEN2004",
 					NULL},
+		{ANY_BOARD_ID, 2961},
 		1024, 5112, 2024, 4832
 	},
 	{
 		(const char * const []){"LEN2001", NULL},
+		{ANY_BOARD_ID, ANY_BOARD_ID},
 		1024, 5022, 2508, 4832
 	},
 	{
 		(const char * const []){"LEN2006", NULL},
+		{2691, 2691},
+		1024, 5045, 2457, 4832
+	},
+	{
+		(const char * const []){"LEN2006", NULL},
+		{ANY_BOARD_ID, ANY_BOARD_ID},
 		1264, 5675, 1171, 4688
 	},
 	{ }
@@ -175,9 +186,7 @@ static const char * const topbuttonpad_pnp_ids[] = {
 	"LEN0041",
 	"LEN0042", /* Yoga */
 	"LEN0045",
-	"LEN0046",
 	"LEN0047",
-	"LEN0048",
 	"LEN0049",
 	"LEN2000",
 	"LEN2001", /* Edge E431 */
@@ -185,7 +194,7 @@ static const char * const topbuttonpad_pnp_ids[] = {
 	"LEN2003",
 	"LEN2004", /* L440 */
 	"LEN2005",
-	"LEN2006",
+	"LEN2006", /* Edge E440/E540 */
 	"LEN2007",
 	"LEN2008",
 	"LEN2009",
@@ -235,18 +244,39 @@ static int synaptics_model_id(struct psmouse *psmouse)
 	return 0;
 }
 
+static int synaptics_more_extended_queries(struct psmouse *psmouse)
+{
+	struct synaptics_data *priv = psmouse->private;
+	unsigned char buf[3];
+
+	if (synaptics_send_cmd(psmouse, SYN_QUE_MEXT_CAPAB_10, buf))
+		return -1;
+
+	priv->ext_cap_10 = (buf[0]<<16) | (buf[1]<<8) | buf[2];
+
+	return 0;
+}
+
 /*
- * Read the board id from the touchpad
+ * Read the board id and the "More Extended Queries" from the touchpad
  * The board id is encoded in the "QUERY MODES" response
  */
-static int synaptics_board_id(struct psmouse *psmouse)
+static int synaptics_query_modes(struct psmouse *psmouse)
 {
 	struct synaptics_data *priv = psmouse->private;
 	unsigned char bid[3];
 
+	/* firmwares prior 7.5 have no board_id encoded */
+	if (SYN_ID_FULL(priv->identity) < 0x705)
+		return 0;
+
 	if (synaptics_send_cmd(psmouse, SYN_QUE_MODES, bid))
 		return -1;
 	priv->board_id = ((bid[0] & 0xfc) << 6) | bid[1];
+
+	if (SYN_MEXT_CAP_BIT(bid[0]))
+		return synaptics_more_extended_queries(psmouse);
+
 	return 0;
 }
 
@@ -346,7 +376,6 @@ static int synaptics_resolution(struct psmouse *psmouse)
 {
 	struct synaptics_data *priv = psmouse->private;
 	unsigned char resp[3];
-	int i;
 
 	if (SYN_ID_MAJOR(priv->identity) < 4)
 		return 0;
@@ -358,17 +387,6 @@ static int synaptics_resolution(struct psmouse *psmouse)
 		}
 	}
 
-	for (i = 0; min_max_pnpid_table[i].pnp_ids; i++) {
-		if (psmouse_matches_pnp_id(psmouse,
-					   min_max_pnpid_table[i].pnp_ids)) {
-			priv->x_min = min_max_pnpid_table[i].x_min;
-			priv->x_max = min_max_pnpid_table[i].x_max;
-			priv->y_min = min_max_pnpid_table[i].y_min;
-			priv->y_max = min_max_pnpid_table[i].y_max;
-			return 0;
-		}
-	}
-
 	if (SYN_EXT_CAP_REQUESTS(priv->capabilities) >= 5 &&
 	    SYN_CAP_MAX_DIMENSIONS(priv->ext_cap_0c)) {
 		if (synaptics_send_cmd(psmouse, SYN_QUE_EXT_MAX_COORDS, resp)) {
@@ -377,23 +395,69 @@ static int synaptics_resolution(struct psmouse *psmouse)
 		} else {
 			priv->x_max = (resp[0] << 5) | ((resp[1] & 0x0f) << 1);
 			priv->y_max = (resp[2] << 5) | ((resp[1] & 0xf0) >> 3);
+			psmouse_info(psmouse,
+				     "queried max coordinates: x [..%d], y [..%d]\n",
+				     priv->x_max, priv->y_max);
 		}
 	}
 
-	if (SYN_EXT_CAP_REQUESTS(priv->capabilities) >= 7 &&
-	    SYN_CAP_MIN_DIMENSIONS(priv->ext_cap_0c)) {
+	if (SYN_CAP_MIN_DIMENSIONS(priv->ext_cap_0c) &&
+	    (SYN_EXT_CAP_REQUESTS(priv->capabilities) >= 7 ||
+	     /*
+	      * Firmware v8.1 does not report proper number of extended
+	      * capabilities, but has been proven to report correct min
+	      * coordinates.
+	      */
+	     SYN_ID_FULL(priv->identity) == 0x801)) {
 		if (synaptics_send_cmd(psmouse, SYN_QUE_EXT_MIN_COORDS, resp)) {
 			psmouse_warn(psmouse,
 				     "device claims to have min coordinates query, but I'm not able to read it.\n");
 		} else {
 			priv->x_min = (resp[0] << 5) | ((resp[1] & 0x0f) << 1);
 			priv->y_min = (resp[2] << 5) | ((resp[1] & 0xf0) >> 3);
+			psmouse_info(psmouse,
+				     "queried min coordinates: x [%d..], y [%d..]\n",
+				     priv->x_min, priv->y_min);
 		}
 	}
 
 	return 0;
 }
 
+/*
+ * Apply quirk(s) if the hardware matches
+ */
+
+static void synaptics_apply_quirks(struct psmouse *psmouse)
+{
+	struct synaptics_data *priv = psmouse->private;
+	int i;
+
+	for (i = 0; min_max_pnpid_table[i].pnp_ids; i++) {
+		if (!psmouse_matches_pnp_id(psmouse,
+					    min_max_pnpid_table[i].pnp_ids))
+			continue;
+
+		if (min_max_pnpid_table[i].board_id.min != ANY_BOARD_ID &&
+		    priv->board_id < min_max_pnpid_table[i].board_id.min)
+			continue;
+
+		if (min_max_pnpid_table[i].board_id.max != ANY_BOARD_ID &&
+		    priv->board_id > min_max_pnpid_table[i].board_id.max)
+			continue;
+
+		priv->x_min = min_max_pnpid_table[i].x_min;
+		priv->x_max = min_max_pnpid_table[i].x_max;
+		priv->y_min = min_max_pnpid_table[i].y_min;
+		priv->y_max = min_max_pnpid_table[i].y_max;
+		psmouse_info(psmouse,
+			     "quirked min/max coordinates: x [%d..%d], y [%d..%d]\n",
+			     priv->x_min, priv->x_max,
+			     priv->y_min, priv->y_max);
+		break;
+	}
+}
+
 static int synaptics_query_hardware(struct psmouse *psmouse)
 {
 	if (synaptics_identify(psmouse))
@@ -402,13 +466,15 @@ static int synaptics_query_hardware(struct psmouse *psmouse)
 		return -1;
 	if (synaptics_firmware_id(psmouse))
 		return -1;
-	if (synaptics_board_id(psmouse))
+	if (synaptics_query_modes(psmouse))
 		return -1;
 	if (synaptics_capability(psmouse))
 		return -1;
 	if (synaptics_resolution(psmouse))
 		return -1;
 
+	synaptics_apply_quirks(psmouse);
+
 	return 0;
 }
 
@@ -516,18 +582,22 @@ static int synaptics_is_pt_packet(unsigned char *buf)
 	return (buf[0] & 0xFC) == 0x84 && (buf[3] & 0xCC) == 0xC4;
 }
 
-static void synaptics_pass_pt_packet(struct serio *ptport, unsigned char *packet)
+static void synaptics_pass_pt_packet(struct psmouse *psmouse,
+				     struct serio *ptport,
+				     unsigned char *packet)
 {
+	struct synaptics_data *priv = psmouse->private;
 	struct psmouse *child = serio_get_drvdata(ptport);
 
 	if (child && child->state == PSMOUSE_ACTIVATED) {
-		serio_interrupt(ptport, packet[1], 0);
+		serio_interrupt(ptport, packet[1] | priv->pt_buttons, 0);
 		serio_interrupt(ptport, packet[4], 0);
 		serio_interrupt(ptport, packet[5], 0);
 		if (child->pktsize == 4)
 			serio_interrupt(ptport, packet[2], 0);
-	} else
+	} else {
 		serio_interrupt(ptport, packet[1], 0);
+	}
 }
 
 static void synaptics_pt_activate(struct psmouse *psmouse)
@@ -605,6 +675,18 @@ static void synaptics_parse_agm(const unsigned char buf[],
 	}
 }
 
+static void synaptics_parse_ext_buttons(const unsigned char buf[],
+					struct synaptics_data *priv,
+					struct synaptics_hw_state *hw)
+{
+	unsigned int ext_bits =
+		(SYN_CAP_MULTI_BUTTON_NO(priv->ext_cap) + 1) >> 1;
+	unsigned int ext_mask = GENMASK(ext_bits - 1, 0);
+
+	hw->ext_buttons = buf[4] & ext_mask;
+	hw->ext_buttons |= (buf[5] & ext_mask) << ext_bits;
+}
+
 static bool is_forcepad;
 
 static int synaptics_parse_hw_state(const unsigned char buf[],
@@ -691,28 +773,9 @@ static int synaptics_parse_hw_state(const unsigned char buf[],
 			hw->down = ((buf[0] ^ buf[3]) & 0x02) ? 1 : 0;
 		}
 
-		if (SYN_CAP_MULTI_BUTTON_NO(priv->ext_cap) &&
+		if (SYN_CAP_MULTI_BUTTON_NO(priv->ext_cap) > 0 &&
 		    ((buf[0] ^ buf[3]) & 0x02)) {
-			switch (SYN_CAP_MULTI_BUTTON_NO(priv->ext_cap) & ~0x01) {
-			default:
-				/*
-				 * if nExtBtn is greater than 8 it should be
-				 * considered invalid and treated as 0
-				 */
-				break;
-			case 8:
-				hw->ext_buttons |= ((buf[5] & 0x08)) ? 0x80 : 0;
-				hw->ext_buttons |= ((buf[4] & 0x08)) ? 0x40 : 0;
-			case 6:
-				hw->ext_buttons |= ((buf[5] & 0x04)) ? 0x20 : 0;
-				hw->ext_buttons |= ((buf[4] & 0x04)) ? 0x10 : 0;
-			case 4:
-				hw->ext_buttons |= ((buf[5] & 0x02)) ? 0x08 : 0;
-				hw->ext_buttons |= ((buf[4] & 0x02)) ? 0x04 : 0;
-			case 2:
-				hw->ext_buttons |= ((buf[5] & 0x01)) ? 0x02 : 0;
-				hw->ext_buttons |= ((buf[4] & 0x01)) ? 0x01 : 0;
-			}
+			synaptics_parse_ext_buttons(buf, priv, hw);
 		}
 	} else {
 		hw->x = (((buf[1] & 0x1f) << 8) | buf[2]);
@@ -774,12 +837,54 @@ static void synaptics_report_semi_mt_data(struct input_dev *dev,
 	}
 }
 
+static void synaptics_report_ext_buttons(struct psmouse *psmouse,
+					 const struct synaptics_hw_state *hw)
+{
+	struct input_dev *dev = psmouse->dev;
+	struct synaptics_data *priv = psmouse->private;
+	int ext_bits = (SYN_CAP_MULTI_BUTTON_NO(priv->ext_cap) + 1) >> 1;
+	char buf[6] = { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
+	int i;
+
+	if (!SYN_CAP_MULTI_BUTTON_NO(priv->ext_cap))
+		return;
+
+	/* Bug in FW 8.1, buttons are reported only when ExtBit is 1 */
+	if (SYN_ID_FULL(priv->identity) == 0x801 &&
+	    !((psmouse->packet[0] ^ psmouse->packet[3]) & 0x02))
+		return;
+
+	if (!SYN_CAP_EXT_BUTTONS_STICK(priv->ext_cap_10)) {
+		for (i = 0; i < ext_bits; i++) {
+			input_report_key(dev, BTN_0 + 2 * i,
+				hw->ext_buttons & (1 << i));
+			input_report_key(dev, BTN_1 + 2 * i,
+				hw->ext_buttons & (1 << (i + ext_bits)));
+		}
+		return;
+	}
+
+	/*
+	 * This generation of touchpads has the trackstick buttons
+	 * physically wired to the touchpad. Re-route them through
+	 * the pass-through interface.
+	 */
+	if (!priv->pt_port)
+		return;
+
+	/* The trackstick expects at most 3 buttons */
+	priv->pt_buttons = SYN_CAP_EXT_BUTTON_STICK_L(hw->ext_buttons)      |
+			   SYN_CAP_EXT_BUTTON_STICK_R(hw->ext_buttons) << 1 |
+			   SYN_CAP_EXT_BUTTON_STICK_M(hw->ext_buttons) << 2;
+
+	synaptics_pass_pt_packet(psmouse, priv->pt_port, buf);
+}
+
 static void synaptics_report_buttons(struct psmouse *psmouse,
 				     const struct synaptics_hw_state *hw)
 {
 	struct input_dev *dev = psmouse->dev;
 	struct synaptics_data *priv = psmouse->private;
-	int i;
 
 	input_report_key(dev, BTN_LEFT, hw->left);
 	input_report_key(dev, BTN_RIGHT, hw->right);
@@ -792,8 +897,7 @@ static void synaptics_report_buttons(struct psmouse *psmouse,
 		input_report_key(dev, BTN_BACK, hw->down);
 	}
 
-	for (i = 0; i < SYN_CAP_MULTI_BUTTON_NO(priv->ext_cap); i++)
-		input_report_key(dev, BTN_0 + i, hw->ext_buttons & (1 << i));
+	synaptics_report_ext_buttons(psmouse, hw);
 }
 
 static void synaptics_report_mt_data(struct psmouse *psmouse,
@@ -813,7 +917,7 @@ static void synaptics_report_mt_data(struct psmouse *psmouse,
 		pos[i].y = synaptics_invert_y(hw[i]->y);
 	}
 
-	input_mt_assign_slots(dev, slot, pos, nsemi, DMAX * priv->x_res);
+	input_mt_assign_slots(dev, slot, pos, nsemi, 0);
 
 	for (i = 0; i < nsemi; i++) {
 		input_mt_slot(dev, slot[i]);
@@ -1014,7 +1118,8 @@ static psmouse_ret_t synaptics_process_byte(struct psmouse *psmouse)
 		if (SYN_CAP_PASS_THROUGH(priv->capabilities) &&
 		    synaptics_is_pt_packet(psmouse->packet)) {
 			if (priv->pt_port)
-				synaptics_pass_pt_packet(priv->pt_port, psmouse->packet);
+				synaptics_pass_pt_packet(psmouse, priv->pt_port,
+							 psmouse->packet);
 		} else
 			synaptics_process_packet(psmouse);
 
@@ -1116,8 +1221,9 @@ static void set_input_params(struct psmouse *psmouse,
 		__set_bit(BTN_BACK, dev->keybit);
 	}
 
-	for (i = 0; i < SYN_CAP_MULTI_BUTTON_NO(priv->ext_cap); i++)
-		__set_bit(BTN_0 + i, dev->keybit);
+	if (!SYN_CAP_EXT_BUTTONS_STICK(priv->ext_cap_10))
+		for (i = 0; i < SYN_CAP_MULTI_BUTTON_NO(priv->ext_cap); i++)
+			__set_bit(BTN_0 + i, dev->keybit);
 
 	__clear_bit(EV_REL, dev->evbit);
 	__clear_bit(REL_X, dev->relbit);
@@ -1125,7 +1231,8 @@ static void set_input_params(struct psmouse *psmouse,
 
 	if (SYN_CAP_CLICKPAD(priv->ext_cap_0c)) {
 		__set_bit(INPUT_PROP_BUTTONPAD, dev->propbit);
-		if (psmouse_matches_pnp_id(psmouse, topbuttonpad_pnp_ids))
+		if (psmouse_matches_pnp_id(psmouse, topbuttonpad_pnp_ids) &&
+		    !SYN_CAP_EXT_BUTTONS_STICK(priv->ext_cap_10))
 			__set_bit(INPUT_PROP_TOPBUTTONPAD, dev->propbit);
 		/* Clickpads report only left button */
 		__clear_bit(BTN_RIGHT, dev->keybit);
diff --git a/drivers/input/mouse/synaptics.h b/drivers/input/mouse/synaptics.h
index aedc3299b14e..ee4bd0d12b26 100644
--- a/drivers/input/mouse/synaptics.h
+++ b/drivers/input/mouse/synaptics.h
@@ -22,6 +22,7 @@
 #define SYN_QUE_EXT_CAPAB_0C		0x0c
 #define SYN_QUE_EXT_MAX_COORDS		0x0d
 #define SYN_QUE_EXT_MIN_COORDS		0x0f
+#define SYN_QUE_MEXT_CAPAB_10		0x10
 
 /* synatics modes */
 #define SYN_BIT_ABSOLUTE_MODE		(1 << 7)
@@ -53,6 +54,7 @@
 #define SYN_EXT_CAP_REQUESTS(c)		(((c) & 0x700000) >> 20)
 #define SYN_CAP_MULTI_BUTTON_NO(ec)	(((ec) & 0x00f000) >> 12)
 #define SYN_CAP_PRODUCT_ID(ec)		(((ec) & 0xff0000) >> 16)
+#define SYN_MEXT_CAP_BIT(m)		((m) & (1 << 1))
 
 /*
  * The following describes response for the 0x0c query.
@@ -89,6 +91,30 @@
 #define SYN_CAP_REDUCED_FILTERING(ex0c)	((ex0c) & 0x000400)
 #define SYN_CAP_IMAGE_SENSOR(ex0c)	((ex0c) & 0x000800)
 
+/*
+ * The following descibes response for the 0x10 query.
+ *
+ * byte	mask	name			meaning
+ * ----	----	-------			------------
+ * 1	0x01	ext buttons are stick	buttons exported in the extended
+ *					capability are actually meant to be used
+ *					by the tracktick (pass-through).
+ * 1	0x02	SecurePad		the touchpad is a SecurePad, so it
+ *					contains a built-in fingerprint reader.
+ * 1	0xe0	more ext count		how many more extented queries are
+ *					available after this one.
+ * 2	0xff	SecurePad width		the width of the SecurePad fingerprint
+ *					reader.
+ * 3	0xff	SecurePad height	the height of the SecurePad fingerprint
+ *					reader.
+ */
+#define SYN_CAP_EXT_BUTTONS_STICK(ex10)	((ex10) & 0x010000)
+#define SYN_CAP_SECUREPAD(ex10)		((ex10) & 0x020000)
+
+#define SYN_CAP_EXT_BUTTON_STICK_L(eb)	(!!((eb) & 0x01))
+#define SYN_CAP_EXT_BUTTON_STICK_M(eb)	(!!((eb) & 0x02))
+#define SYN_CAP_EXT_BUTTON_STICK_R(eb)	(!!((eb) & 0x04))
+
 /* synaptics modes query bits */
 #define SYN_MODE_ABSOLUTE(m)		((m) & (1 << 7))
 #define SYN_MODE_RATE(m)		((m) & (1 << 6))
@@ -143,6 +169,7 @@ struct synaptics_data {
 	unsigned long int capabilities;		/* Capabilities */
 	unsigned long int ext_cap;		/* Extended Capabilities */
 	unsigned long int ext_cap_0c;		/* Ext Caps from 0x0c query */
+	unsigned long int ext_cap_10;		/* Ext Caps from 0x10 query */
 	unsigned long int identity;		/* Identification */
 	unsigned int x_res, y_res;		/* X/Y resolution in units/mm */
 	unsigned int x_max, y_max;		/* Max coordinates (from FW) */
@@ -156,6 +183,7 @@ struct synaptics_data {
 	bool disable_gesture;			/* disable gestures */
 
 	struct serio *pt_port;			/* Pass-through serio port */
+	unsigned char pt_buttons;		/* Pass-through buttons */
 
 	/*
 	 * Last received Advanced Gesture Mode (AGM) packet. An AGM packet
diff --git a/drivers/input/touchscreen/Kconfig b/drivers/input/touchscreen/Kconfig
index 58917525126e..6261fd6d7c3c 100644
--- a/drivers/input/touchscreen/Kconfig
+++ b/drivers/input/touchscreen/Kconfig
@@ -943,6 +943,7 @@ config TOUCHSCREEN_SUN4I
 	tristate "Allwinner sun4i resistive touchscreen controller support"
 	depends on ARCH_SUNXI || COMPILE_TEST
 	depends on HWMON
+	depends on THERMAL || !THERMAL_OF
 	help
 	  This selects support for the resistive touchscreen controller
 	  found on Allwinner sunxi SoCs.
diff --git a/drivers/iommu/Kconfig b/drivers/iommu/Kconfig
index baa0d9786f50..1ae4e547b419 100644
--- a/drivers/iommu/Kconfig
+++ b/drivers/iommu/Kconfig
@@ -23,6 +23,7 @@ config IOMMU_IO_PGTABLE
 config IOMMU_IO_PGTABLE_LPAE
 	bool "ARMv7/v8 Long Descriptor Format"
 	select IOMMU_IO_PGTABLE
+	depends on ARM || ARM64 || COMPILE_TEST
 	help
 	  Enable support for the ARM long descriptor pagetable format.
 	  This allocator supports 4K/2M/1G, 16K/32M and 64K/512M page
@@ -63,6 +64,7 @@ config MSM_IOMMU
 	bool "MSM IOMMU Support"
 	depends on ARM
 	depends on ARCH_MSM8X60 || ARCH_MSM8960 || COMPILE_TEST
+	depends on BROKEN
 	select IOMMU_API
 	help
 	  Support for the IOMMUs found on certain Qualcomm SOCs.
diff --git a/drivers/iommu/arm-smmu.c b/drivers/iommu/arm-smmu.c
index fc13dd56953e..a3adde6519f0 100644
--- a/drivers/iommu/arm-smmu.c
+++ b/drivers/iommu/arm-smmu.c
@@ -1288,10 +1288,13 @@ static phys_addr_t arm_smmu_iova_to_phys(struct iommu_domain *domain,
 		return 0;
 
 	spin_lock_irqsave(&smmu_domain->pgtbl_lock, flags);
-	if (smmu_domain->smmu->features & ARM_SMMU_FEAT_TRANS_OPS)
+	if (smmu_domain->smmu->features & ARM_SMMU_FEAT_TRANS_OPS &&
+			smmu_domain->stage == ARM_SMMU_DOMAIN_S1) {
 		ret = arm_smmu_iova_to_phys_hard(domain, iova);
-	else
+	} else {
 		ret = ops->iova_to_phys(ops, iova);
+	}
+
 	spin_unlock_irqrestore(&smmu_domain->pgtbl_lock, flags);
 
 	return ret;
@@ -1556,7 +1559,7 @@ static int arm_smmu_device_cfg_probe(struct arm_smmu_device *smmu)
 		return -ENODEV;
 	}
 
-	if (smmu->version == 1 || (!(id & ID0_ATOSNS) && (id & ID0_S1TS))) {
+	if ((id & ID0_S1TS) && ((smmu->version == 1) || (id & ID0_ATOSNS))) {
 		smmu->features |= ARM_SMMU_FEAT_TRANS_OPS;
 		dev_notice(smmu->dev, "\taddress translation ops\n");
 	}
diff --git a/drivers/iommu/exynos-iommu.c b/drivers/iommu/exynos-iommu.c
index 7ce52737c7a1..dc14fec4ede1 100644
--- a/drivers/iommu/exynos-iommu.c
+++ b/drivers/iommu/exynos-iommu.c
@@ -1186,8 +1186,15 @@ static const struct iommu_ops exynos_iommu_ops = {
 
 static int __init exynos_iommu_init(void)
 {
+	struct device_node *np;
 	int ret;
 
+	np = of_find_matching_node(NULL, sysmmu_of_match);
+	if (!np)
+		return 0;
+
+	of_node_put(np);
+
 	lv2table_kmem_cache = kmem_cache_create("exynos-iommu-lv2table",
 				LV2TABLE_SIZE, LV2TABLE_SIZE, 0, NULL);
 	if (!lv2table_kmem_cache) {
diff --git a/drivers/iommu/intel-iommu.c b/drivers/iommu/intel-iommu.c
index ae4c1a854e57..2d1e05bdbb53 100644
--- a/drivers/iommu/intel-iommu.c
+++ b/drivers/iommu/intel-iommu.c
@@ -1742,9 +1742,8 @@ static int domain_init(struct dmar_domain *domain, int guest_width)
 
 static void domain_exit(struct dmar_domain *domain)
 {
-	struct dmar_drhd_unit *drhd;
-	struct intel_iommu *iommu;
 	struct page *freelist = NULL;
+	int i;
 
 	/* Domain 0 is reserved, so dont process it */
 	if (!domain)
@@ -1764,8 +1763,8 @@ static void domain_exit(struct dmar_domain *domain)
 
 	/* clear attached or cached domains */
 	rcu_read_lock();
-	for_each_active_iommu(iommu, drhd)
-		iommu_detach_domain(domain, iommu);
+	for_each_set_bit(i, domain->iommu_bmp, g_num_of_iommus)
+		iommu_detach_domain(domain, g_iommus[i]);
 	rcu_read_unlock();
 
 	dma_free_pagelist(freelist);
diff --git a/drivers/iommu/io-pgtable-arm.c b/drivers/iommu/io-pgtable-arm.c
index 5a500edf00cc..b610a8dee238 100644
--- a/drivers/iommu/io-pgtable-arm.c
+++ b/drivers/iommu/io-pgtable-arm.c
@@ -56,7 +56,8 @@
 	((((d)->levels - ((l) - ARM_LPAE_START_LVL(d) + 1))		\
 	  * (d)->bits_per_level) + (d)->pg_shift)
 
-#define ARM_LPAE_PAGES_PER_PGD(d)	((d)->pgd_size >> (d)->pg_shift)
+#define ARM_LPAE_PAGES_PER_PGD(d)					\
+	DIV_ROUND_UP((d)->pgd_size, 1UL << (d)->pg_shift)
 
 /*
  * Calculate the index at level l used to map virtual address a using the
@@ -66,7 +67,7 @@
 	((l) == ARM_LPAE_START_LVL(d) ? ilog2(ARM_LPAE_PAGES_PER_PGD(d)) : 0)
 
 #define ARM_LPAE_LVL_IDX(a,l,d)						\
-	(((a) >> ARM_LPAE_LVL_SHIFT(l,d)) &				\
+	(((u64)(a) >> ARM_LPAE_LVL_SHIFT(l,d)) &			\
 	 ((1 << ((d)->bits_per_level + ARM_LPAE_PGD_IDX(l,d))) - 1))
 
 /* Calculate the block/page mapping size at level l for pagetable in d. */
diff --git a/drivers/iommu/ipmmu-vmsa.c b/drivers/iommu/ipmmu-vmsa.c
index 10186cac7716..bc39bdf7b99b 100644
--- a/drivers/iommu/ipmmu-vmsa.c
+++ b/drivers/iommu/ipmmu-vmsa.c
@@ -851,6 +851,7 @@ static int ipmmu_remove(struct platform_device *pdev)
 
 static const struct of_device_id ipmmu_of_ids[] = {
 	{ .compatible = "renesas,ipmmu-vmsa", },
+	{ }
 };
 
 static struct platform_driver ipmmu_driver = {
diff --git a/drivers/iommu/of_iommu.c b/drivers/iommu/of_iommu.c
index af1dc6a1c0a1..43429ab62228 100644
--- a/drivers/iommu/of_iommu.c
+++ b/drivers/iommu/of_iommu.c
@@ -133,19 +133,25 @@ struct iommu_ops *of_iommu_get_ops(struct device_node *np)
 	return ops;
 }
 
-struct iommu_ops *of_iommu_configure(struct device *dev)
+struct iommu_ops *of_iommu_configure(struct device *dev,
+				     struct device_node *master_np)
 {
 	struct of_phandle_args iommu_spec;
 	struct device_node *np;
 	struct iommu_ops *ops = NULL;
 	int idx = 0;
 
+	if (dev_is_pci(dev)) {
+		dev_err(dev, "IOMMU is currently not supported for PCI\n");
+		return NULL;
+	}
+
 	/*
 	 * We don't currently walk up the tree looking for a parent IOMMU.
 	 * See the `Notes:' section of
 	 * Documentation/devicetree/bindings/iommu/iommu.txt
 	 */
-	while (!of_parse_phandle_with_args(dev->of_node, "iommus",
+	while (!of_parse_phandle_with_args(master_np, "iommus",
 					   "#iommu-cells", idx,
 					   &iommu_spec)) {
 		np = iommu_spec.np;
diff --git a/drivers/iommu/omap-iommu.c b/drivers/iommu/omap-iommu.c
index f59f857b702e..a4ba851825c2 100644
--- a/drivers/iommu/omap-iommu.c
+++ b/drivers/iommu/omap-iommu.c
@@ -1376,6 +1376,13 @@ static int __init omap_iommu_init(void)
 	struct kmem_cache *p;
 	const unsigned long flags = SLAB_HWCACHE_ALIGN;
 	size_t align = 1 << 10; /* L2 pagetable alignement */
+	struct device_node *np;
+
+	np = of_find_matching_node(NULL, omap_iommu_of_match);
+	if (!np)
+		return 0;
+
+	of_node_put(np);
 
 	p = kmem_cache_create("iopte_cache", IOPTE_TABLE_SIZE, align, flags,
 			      iopte_cachep_ctor);
diff --git a/drivers/iommu/rockchip-iommu.c b/drivers/iommu/rockchip-iommu.c
index 6a8b1ec4a48a..9f74fddcd304 100644
--- a/drivers/iommu/rockchip-iommu.c
+++ b/drivers/iommu/rockchip-iommu.c
@@ -1015,8 +1015,15 @@ static struct platform_driver rk_iommu_driver = {
 
 static int __init rk_iommu_init(void)
 {
+	struct device_node *np;
 	int ret;
 
+	np = of_find_matching_node(NULL, rk_iommu_dt_ids);
+	if (!np)
+		return 0;
+
+	of_node_put(np);
+
 	ret = bus_set_iommu(&platform_bus_type, &rk_iommu_ops);
 	if (ret)
 		return ret;
diff --git a/drivers/irqchip/Kconfig b/drivers/irqchip/Kconfig
index cc79d2a5a8c2..c8d260e33a90 100644
--- a/drivers/irqchip/Kconfig
+++ b/drivers/irqchip/Kconfig
@@ -110,6 +110,13 @@ config RENESAS_IRQC
 	bool
 	select IRQ_DOMAIN
 
+config ST_IRQCHIP
+	bool
+	select REGMAP
+	select MFD_SYSCON
+	help
+	  Enables SysCfg Controlled IRQs on STi based platforms.
+
 config TB10X_IRQC
 	bool
 	select IRQ_DOMAIN
diff --git a/drivers/irqchip/Makefile b/drivers/irqchip/Makefile
index 42965d2476bb..552a74027601 100644
--- a/drivers/irqchip/Makefile
+++ b/drivers/irqchip/Makefile
@@ -6,6 +6,7 @@ obj-$(CONFIG_ARCH_HIP04)		+= irq-hip04.o
 obj-$(CONFIG_ARCH_MMP)			+= irq-mmp.o
 obj-$(CONFIG_ARCH_MVEBU)		+= irq-armada-370-xp.o
 obj-$(CONFIG_ARCH_MXS)			+= irq-mxs.o
+obj-$(CONFIG_ARCH_TEGRA)		+= irq-tegra.o
 obj-$(CONFIG_ARCH_S3C24XX)		+= irq-s3c24xx.o
 obj-$(CONFIG_DW_APB_ICTL)		+= irq-dw-apb-ictl.o
 obj-$(CONFIG_METAG)			+= irq-metag-ext.o
@@ -33,10 +34,12 @@ 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_ST_IRQCHIP)		+= irq-st.o
 obj-$(CONFIG_TB10X_IRQC)		+= irq-tb10x.o
 obj-$(CONFIG_XTENSA)			+= irq-xtensa-pic.o
 obj-$(CONFIG_XTENSA_MX)			+= irq-xtensa-mx.o
 obj-$(CONFIG_IRQ_CROSSBAR)		+= irq-crossbar.o
+obj-$(CONFIG_SOC_VF610)			+= irq-vf610-mscm-ir.o
 obj-$(CONFIG_BCM7120_L2_IRQ)		+= irq-bcm7120-l2.o
 obj-$(CONFIG_BRCMSTB_L2_IRQ)		+= irq-brcmstb-l2.o
 obj-$(CONFIG_KEYSTONE_IRQ)		+= irq-keystone.o
diff --git a/drivers/irqchip/irq-armada-370-xp.c b/drivers/irqchip/irq-armada-370-xp.c
index 463c235acbdc..daccc8bdbb42 100644
--- a/drivers/irqchip/irq-armada-370-xp.c
+++ b/drivers/irqchip/irq-armada-370-xp.c
@@ -38,6 +38,8 @@
 /* Interrupt Controller Registers Map */
 #define ARMADA_370_XP_INT_SET_MASK_OFFS		(0x48)
 #define ARMADA_370_XP_INT_CLEAR_MASK_OFFS	(0x4C)
+#define ARMADA_370_XP_INT_FABRIC_MASK_OFFS	(0x54)
+#define ARMADA_370_XP_INT_CAUSE_PERF(cpu)	(1 << cpu)
 
 #define ARMADA_370_XP_INT_CONTROL		(0x00)
 #define ARMADA_370_XP_INT_SET_ENABLE_OFFS	(0x30)
@@ -56,6 +58,7 @@
 #define ARMADA_370_XP_MAX_PER_CPU_IRQS		(28)
 
 #define ARMADA_370_XP_TIMER0_PER_CPU_IRQ	(5)
+#define ARMADA_370_XP_FABRIC_IRQ		(3)
 
 #define IPI_DOORBELL_START                      (0)
 #define IPI_DOORBELL_END                        (8)
@@ -69,6 +72,7 @@ static void __iomem *per_cpu_int_base;
 static void __iomem *main_int_base;
 static struct irq_domain *armada_370_xp_mpic_domain;
 static u32 doorbell_mask_reg;
+static int parent_irq;
 #ifdef CONFIG_PCI_MSI
 static struct irq_domain *armada_370_xp_msi_domain;
 static DECLARE_BITMAP(msi_used, PCI_MSI_DOORBELL_NR);
@@ -76,6 +80,17 @@ static DEFINE_MUTEX(msi_used_lock);
 static phys_addr_t msi_doorbell_addr;
 #endif
 
+static inline bool is_percpu_irq(irq_hw_number_t irq)
+{
+	switch (irq) {
+	case ARMADA_370_XP_TIMER0_PER_CPU_IRQ:
+	case ARMADA_370_XP_FABRIC_IRQ:
+		return true;
+	default:
+		return false;
+	}
+}
+
 /*
  * In SMP mode:
  * For shared global interrupts, mask/unmask global enable bit
@@ -85,7 +100,7 @@ static void armada_370_xp_irq_mask(struct irq_data *d)
 {
 	irq_hw_number_t hwirq = irqd_to_hwirq(d);
 
-	if (hwirq != ARMADA_370_XP_TIMER0_PER_CPU_IRQ)
+	if (!is_percpu_irq(hwirq))
 		writel(hwirq, main_int_base +
 				ARMADA_370_XP_INT_CLEAR_ENABLE_OFFS);
 	else
@@ -97,7 +112,7 @@ static void armada_370_xp_irq_unmask(struct irq_data *d)
 {
 	irq_hw_number_t hwirq = irqd_to_hwirq(d);
 
-	if (hwirq != ARMADA_370_XP_TIMER0_PER_CPU_IRQ)
+	if (!is_percpu_irq(hwirq))
 		writel(hwirq, main_int_base +
 				ARMADA_370_XP_INT_SET_ENABLE_OFFS);
 	else
@@ -280,20 +295,21 @@ static struct irq_chip armada_370_xp_irq_chip = {
 #ifdef CONFIG_SMP
 	.irq_set_affinity = armada_xp_set_affinity,
 #endif
+	.flags		= IRQCHIP_SKIP_SET_WAKE | IRQCHIP_MASK_ON_SUSPEND,
 };
 
 static int armada_370_xp_mpic_irq_map(struct irq_domain *h,
 				      unsigned int virq, irq_hw_number_t hw)
 {
 	armada_370_xp_irq_mask(irq_get_irq_data(virq));
-	if (hw != ARMADA_370_XP_TIMER0_PER_CPU_IRQ)
+	if (!is_percpu_irq(hw))
 		writel(hw, per_cpu_int_base +
 			ARMADA_370_XP_INT_CLEAR_MASK_OFFS);
 	else
 		writel(hw, main_int_base + ARMADA_370_XP_INT_SET_ENABLE_OFFS);
 	irq_set_status_flags(virq, IRQ_LEVEL);
 
-	if (hw == ARMADA_370_XP_TIMER0_PER_CPU_IRQ) {
+	if (is_percpu_irq(hw)) {
 		irq_set_percpu_devid(virq);
 		irq_set_chip_and_handler(virq, &armada_370_xp_irq_chip,
 					handle_percpu_devid_irq);
@@ -307,28 +323,6 @@ static int armada_370_xp_mpic_irq_map(struct irq_domain *h,
 	return 0;
 }
 
-#ifdef CONFIG_SMP
-static void armada_mpic_send_doorbell(const struct cpumask *mask,
-				      unsigned int irq)
-{
-	int cpu;
-	unsigned long map = 0;
-
-	/* Convert our logical CPU mask into a physical one. */
-	for_each_cpu(cpu, mask)
-		map |= 1 << cpu_logical_map(cpu);
-
-	/*
-	 * Ensure that stores to Normal memory are visible to the
-	 * other CPUs before issuing the IPI.
-	 */
-	dsb();
-
-	/* submit softirq */
-	writel((map << 8) | irq, main_int_base +
-		ARMADA_370_XP_SW_TRIG_INT_OFFS);
-}
-
 static void armada_xp_mpic_smp_cpu_init(void)
 {
 	u32 control;
@@ -351,11 +345,45 @@ static void armada_xp_mpic_smp_cpu_init(void)
 	writel(0, per_cpu_int_base + ARMADA_370_XP_INT_CLEAR_MASK_OFFS);
 }
 
+static void armada_xp_mpic_perf_init(void)
+{
+	unsigned long cpuid = cpu_logical_map(smp_processor_id());
+
+	/* Enable Performance Counter Overflow interrupts */
+	writel(ARMADA_370_XP_INT_CAUSE_PERF(cpuid),
+	       per_cpu_int_base + ARMADA_370_XP_INT_FABRIC_MASK_OFFS);
+}
+
+#ifdef CONFIG_SMP
+static void armada_mpic_send_doorbell(const struct cpumask *mask,
+				      unsigned int irq)
+{
+	int cpu;
+	unsigned long map = 0;
+
+	/* Convert our logical CPU mask into a physical one. */
+	for_each_cpu(cpu, mask)
+		map |= 1 << cpu_logical_map(cpu);
+
+	/*
+	 * Ensure that stores to Normal memory are visible to the
+	 * other CPUs before issuing the IPI.
+	 */
+	dsb();
+
+	/* submit softirq */
+	writel((map << 8) | irq, main_int_base +
+		ARMADA_370_XP_SW_TRIG_INT_OFFS);
+}
+
 static int armada_xp_mpic_secondary_init(struct notifier_block *nfb,
 					 unsigned long action, void *hcpu)
 {
-	if (action == CPU_STARTING || action == CPU_STARTING_FROZEN)
+	if (action == CPU_STARTING || action == CPU_STARTING_FROZEN) {
+		armada_xp_mpic_perf_init();
 		armada_xp_mpic_smp_cpu_init();
+	}
+
 	return NOTIFY_OK;
 }
 
@@ -364,6 +392,21 @@ static struct notifier_block armada_370_xp_mpic_cpu_notifier = {
 	.priority = 100,
 };
 
+static int mpic_cascaded_secondary_init(struct notifier_block *nfb,
+					unsigned long action, void *hcpu)
+{
+	if (action == CPU_STARTING || action == CPU_STARTING_FROZEN) {
+		armada_xp_mpic_perf_init();
+		enable_percpu_irq(parent_irq, IRQ_TYPE_NONE);
+	}
+
+	return NOTIFY_OK;
+}
+
+static struct notifier_block mpic_cascaded_cpu_notifier = {
+	.notifier_call = mpic_cascaded_secondary_init,
+	.priority = 100,
+};
 #endif /* CONFIG_SMP */
 
 static struct irq_domain_ops armada_370_xp_mpic_irq_ops = {
@@ -539,7 +582,7 @@ static int __init armada_370_xp_mpic_of_init(struct device_node *node,
 					     struct device_node *parent)
 {
 	struct resource main_int_res, per_cpu_int_res;
-	int parent_irq, nr_irqs, i;
+	int nr_irqs, i;
 	u32 control;
 
 	BUG_ON(of_address_to_resource(node, 0, &main_int_res));
@@ -572,9 +615,9 @@ static int __init armada_370_xp_mpic_of_init(struct device_node *node,
 
 	BUG_ON(!armada_370_xp_mpic_domain);
 
-#ifdef CONFIG_SMP
+	/* Setup for the boot CPU */
+	armada_xp_mpic_perf_init();
 	armada_xp_mpic_smp_cpu_init();
-#endif
 
 	armada_370_xp_msi_init(node, main_int_res.start);
 
@@ -587,6 +630,9 @@ static int __init armada_370_xp_mpic_of_init(struct device_node *node,
 		register_cpu_notifier(&armada_370_xp_mpic_cpu_notifier);
 #endif
 	} else {
+#ifdef CONFIG_SMP
+		register_cpu_notifier(&mpic_cascaded_cpu_notifier);
+#endif
 		irq_set_chained_handler(parent_irq,
 					armada_370_xp_mpic_handle_cascade_irq);
 	}
diff --git a/drivers/irqchip/irq-crossbar.c b/drivers/irqchip/irq-crossbar.c
index bbbaf5de65d2..692fe2bc8197 100644
--- a/drivers/irqchip/irq-crossbar.c
+++ b/drivers/irqchip/irq-crossbar.c
@@ -11,11 +11,12 @@
  */
 #include <linux/err.h>
 #include <linux/io.h>
+#include <linux/irqdomain.h>
 #include <linux/of_address.h>
 #include <linux/of_irq.h>
 #include <linux/slab.h>
-#include <linux/irqchip/arm-gic.h>
-#include <linux/irqchip/irq-crossbar.h>
+
+#include "irqchip.h"
 
 #define IRQ_FREE	-1
 #define IRQ_RESERVED	-2
@@ -24,6 +25,7 @@
 
 /**
  * struct crossbar_device - crossbar device description
+ * @lock: spinlock serializing access to @irq_map
  * @int_max: maximum number of supported interrupts
  * @safe_map: safe default value to initialize the crossbar
  * @max_crossbar_sources: Maximum number of crossbar sources
@@ -33,6 +35,7 @@
  * @write: register write function pointer
  */
 struct crossbar_device {
+	raw_spinlock_t lock;
 	uint int_max;
 	uint safe_map;
 	uint max_crossbar_sources;
@@ -44,72 +47,101 @@ struct crossbar_device {
 
 static struct crossbar_device *cb;
 
-static inline void crossbar_writel(int irq_no, int cb_no)
+static void crossbar_writel(int irq_no, int cb_no)
 {
 	writel(cb_no, cb->crossbar_base + cb->register_offsets[irq_no]);
 }
 
-static inline void crossbar_writew(int irq_no, int cb_no)
+static void crossbar_writew(int irq_no, int cb_no)
 {
 	writew(cb_no, cb->crossbar_base + cb->register_offsets[irq_no]);
 }
 
-static inline void crossbar_writeb(int irq_no, int cb_no)
+static void crossbar_writeb(int irq_no, int cb_no)
 {
 	writeb(cb_no, cb->crossbar_base + cb->register_offsets[irq_no]);
 }
 
-static inline int get_prev_map_irq(int cb_no)
-{
-	int i;
-
-	for (i = cb->int_max - 1; i >= 0; i--)
-		if (cb->irq_map[i] == cb_no)
-			return i;
-
-	return -ENODEV;
-}
+static struct irq_chip crossbar_chip = {
+	.name			= "CBAR",
+	.irq_eoi		= irq_chip_eoi_parent,
+	.irq_mask		= irq_chip_mask_parent,
+	.irq_unmask		= irq_chip_unmask_parent,
+	.irq_retrigger		= irq_chip_retrigger_hierarchy,
+	.irq_set_wake		= irq_chip_set_wake_parent,
+#ifdef CONFIG_SMP
+	.irq_set_affinity	= irq_chip_set_affinity_parent,
+#endif
+};
 
-static inline int allocate_free_irq(int cb_no)
+static int allocate_gic_irq(struct irq_domain *domain, unsigned virq,
+			    irq_hw_number_t hwirq)
 {
+	struct of_phandle_args args;
 	int i;
+	int err;
 
+	raw_spin_lock(&cb->lock);
 	for (i = cb->int_max - 1; i >= 0; i--) {
 		if (cb->irq_map[i] == IRQ_FREE) {
-			cb->irq_map[i] = cb_no;
-			return i;
+			cb->irq_map[i] = hwirq;
+			break;
 		}
 	}
+	raw_spin_unlock(&cb->lock);
 
-	return -ENODEV;
-}
+	if (i < 0)
+		return -ENODEV;
 
-static inline bool needs_crossbar_write(irq_hw_number_t hw)
-{
-	int cb_no;
+	args.np = domain->parent->of_node;
+	args.args_count = 3;
+	args.args[0] = 0;	/* SPI */
+	args.args[1] = i;
+	args.args[2] = IRQ_TYPE_LEVEL_HIGH;
 
-	if (hw > GIC_IRQ_START) {
-		cb_no = cb->irq_map[hw - GIC_IRQ_START];
-		if (cb_no != IRQ_RESERVED && cb_no != IRQ_SKIP)
-			return true;
-	}
+	err = irq_domain_alloc_irqs_parent(domain, virq, 1, &args);
+	if (err)
+		cb->irq_map[i] = IRQ_FREE;
+	else
+		cb->write(i, hwirq);
 
-	return false;
+	return err;
 }
 
-static int crossbar_domain_map(struct irq_domain *d, unsigned int irq,
-			       irq_hw_number_t hw)
+static int crossbar_domain_alloc(struct irq_domain *d, unsigned int virq,
+				 unsigned int nr_irqs, void *data)
 {
-	if (needs_crossbar_write(hw))
-		cb->write(hw - GIC_IRQ_START, cb->irq_map[hw - GIC_IRQ_START]);
+	struct of_phandle_args *args = data;
+	irq_hw_number_t hwirq;
+	int i;
+
+	if (args->args_count != 3)
+		return -EINVAL;	/* Not GIC compliant */
+	if (args->args[0] != 0)
+		return -EINVAL;	/* No PPI should point to this domain */
+
+	hwirq = args->args[1];
+	if ((hwirq + nr_irqs) > cb->max_crossbar_sources)
+		return -EINVAL;	/* Can't deal with this */
+
+	for (i = 0; i < nr_irqs; i++) {
+		int err = allocate_gic_irq(d, virq + i, hwirq + i);
+
+		if (err)
+			return err;
+
+		irq_domain_set_hwirq_and_chip(d, virq + i, hwirq + i,
+					      &crossbar_chip, NULL);
+	}
 
 	return 0;
 }
 
 /**
- * crossbar_domain_unmap - unmap a crossbar<->irq connection
- * @d: domain of irq to unmap
- * @irq: virq number
+ * crossbar_domain_free - unmap/free a crossbar<->irq connection
+ * @domain: domain of irq to unmap
+ * @virq: virq number
+ * @nr_irqs: number of irqs to free
  *
  * We do not maintain a use count of total number of map/unmap
  * calls for a particular irq to find out if a irq can be really
@@ -117,14 +149,20 @@ static int crossbar_domain_map(struct irq_domain *d, unsigned int irq,
  * after which irq is anyways unusable. So an explicit map has to be called
  * after that.
  */
-static void crossbar_domain_unmap(struct irq_domain *d, unsigned int irq)
+static void crossbar_domain_free(struct irq_domain *domain, unsigned int virq,
+				 unsigned int nr_irqs)
 {
-	irq_hw_number_t hw = irq_get_irq_data(irq)->hwirq;
+	int i;
 
-	if (needs_crossbar_write(hw)) {
-		cb->irq_map[hw - GIC_IRQ_START] = IRQ_FREE;
-		cb->write(hw - GIC_IRQ_START, cb->safe_map);
+	raw_spin_lock(&cb->lock);
+	for (i = 0; i < nr_irqs; i++) {
+		struct irq_data *d = irq_domain_get_irq_data(domain, virq + i);
+
+		irq_domain_reset_irq_data(d);
+		cb->irq_map[d->hwirq] = IRQ_FREE;
+		cb->write(d->hwirq, cb->safe_map);
 	}
+	raw_spin_unlock(&cb->lock);
 }
 
 static int crossbar_domain_xlate(struct irq_domain *d,
@@ -133,44 +171,22 @@ static int crossbar_domain_xlate(struct irq_domain *d,
 				 unsigned long *out_hwirq,
 				 unsigned int *out_type)
 {
-	int ret;
-	int req_num = intspec[1];
-	int direct_map_num;
-
-	if (req_num >= cb->max_crossbar_sources) {
-		direct_map_num = req_num - cb->max_crossbar_sources;
-		if (direct_map_num < cb->int_max) {
-			ret = cb->irq_map[direct_map_num];
-			if (ret == IRQ_RESERVED || ret == IRQ_SKIP) {
-				/* We use the interrupt num as h/w irq num */
-				ret = direct_map_num;
-				goto found;
-			}
-		}
-
-		pr_err("%s: requested crossbar number %d > max %d\n",
-		       __func__, req_num, cb->max_crossbar_sources);
-		return -EINVAL;
-	}
-
-	ret = get_prev_map_irq(req_num);
-	if (ret >= 0)
-		goto found;
-
-	ret = allocate_free_irq(req_num);
-
-	if (ret < 0)
-		return ret;
-
-found:
-	*out_hwirq = ret + GIC_IRQ_START;
+	if (d->of_node != controller)
+		return -EINVAL;	/* Shouldn't happen, really... */
+	if (intsize != 3)
+		return -EINVAL;	/* Not GIC compliant */
+	if (intspec[0] != 0)
+		return -EINVAL;	/* No PPI should point to this domain */
+
+	*out_hwirq = intspec[1];
+	*out_type = intspec[2];
 	return 0;
 }
 
-static const struct irq_domain_ops routable_irq_domain_ops = {
-	.map = crossbar_domain_map,
-	.unmap = crossbar_domain_unmap,
-	.xlate = crossbar_domain_xlate
+static const struct irq_domain_ops crossbar_domain_ops = {
+	.alloc	= crossbar_domain_alloc,
+	.free	= crossbar_domain_free,
+	.xlate	= crossbar_domain_xlate,
 };
 
 static int __init crossbar_of_init(struct device_node *node)
@@ -293,7 +309,8 @@ static int __init crossbar_of_init(struct device_node *node)
 		cb->write(i, cb->safe_map);
 	}
 
-	register_routable_domain_ops(&routable_irq_domain_ops);
+	raw_spin_lock_init(&cb->lock);
+
 	return 0;
 
 err_reg_offset:
@@ -309,18 +326,37 @@ err_cb:
 	return ret;
 }
 
-static const struct of_device_id crossbar_match[] __initconst = {
-	{ .compatible = "ti,irq-crossbar" },
-	{}
-};
-
-int __init irqcrossbar_init(void)
+static int __init irqcrossbar_init(struct device_node *node,
+				   struct device_node *parent)
 {
-	struct device_node *np;
-	np = of_find_matching_node(NULL, crossbar_match);
-	if (!np)
+	struct irq_domain *parent_domain, *domain;
+	int err;
+
+	if (!parent) {
+		pr_err("%s: no parent, giving up\n", node->full_name);
 		return -ENODEV;
+	}
+
+	parent_domain = irq_find_host(parent);
+	if (!parent_domain) {
+		pr_err("%s: unable to obtain parent domain\n", node->full_name);
+		return -ENXIO;
+	}
+
+	err = crossbar_of_init(node);
+	if (err)
+		return err;
+
+	domain = irq_domain_add_hierarchy(parent_domain, 0,
+					  cb->max_crossbar_sources,
+					  node, &crossbar_domain_ops,
+					  NULL);
+	if (!domain) {
+		pr_err("%s: failed to allocated domain\n", node->full_name);
+		return -ENOMEM;
+	}
 
-	crossbar_of_init(np);
 	return 0;
 }
+
+IRQCHIP_DECLARE(ti_irqcrossbar, "ti,irq-crossbar", irqcrossbar_init);
diff --git a/drivers/irqchip/irq-digicolor.c b/drivers/irqchip/irq-digicolor.c
index 930a2a2fac7f..3cbc658afe27 100644
--- a/drivers/irqchip/irq-digicolor.c
+++ b/drivers/irqchip/irq-digicolor.c
@@ -55,8 +55,8 @@ static void __exception_irq_entry digicolor_handle_irq(struct pt_regs *regs)
 	} while (1);
 }
 
-static void digicolor_set_gc(void __iomem *reg_base, unsigned irq_base,
-			     unsigned en_reg, unsigned ack_reg)
+static void __init digicolor_set_gc(void __iomem *reg_base, unsigned irq_base,
+				    unsigned en_reg, unsigned ack_reg)
 {
 	struct irq_chip_generic *gc;
 
diff --git a/drivers/irqchip/irq-gic-v3-its.c b/drivers/irqchip/irq-gic-v3-its.c
index d8996bdf0f61..9687f8afebff 100644
--- a/drivers/irqchip/irq-gic-v3-its.c
+++ b/drivers/irqchip/irq-gic-v3-its.c
@@ -169,7 +169,7 @@ static void its_encode_cmd(struct its_cmd_block *cmd, u8 cmd_nr)
 
 static void its_encode_devid(struct its_cmd_block *cmd, u32 devid)
 {
-	cmd->raw_cmd[0] &= ~(0xffffUL << 32);
+	cmd->raw_cmd[0] &= BIT_ULL(32) - 1;
 	cmd->raw_cmd[0] |= ((u64)devid) << 32;
 }
 
@@ -416,13 +416,14 @@ static void its_send_single_command(struct its_node *its,
 {
 	struct its_cmd_block *cmd, *sync_cmd, *next_cmd;
 	struct its_collection *sync_col;
+	unsigned long flags;
 
-	raw_spin_lock(&its->lock);
+	raw_spin_lock_irqsave(&its->lock, flags);
 
 	cmd = its_allocate_entry(its);
 	if (!cmd) {		/* We're soooooo screewed... */
 		pr_err_ratelimited("ITS can't allocate, dropping command\n");
-		raw_spin_unlock(&its->lock);
+		raw_spin_unlock_irqrestore(&its->lock, flags);
 		return;
 	}
 	sync_col = builder(cmd, desc);
@@ -442,7 +443,7 @@ static void its_send_single_command(struct its_node *its,
 
 post:
 	next_cmd = its_post_commands(its);
-	raw_spin_unlock(&its->lock);
+	raw_spin_unlock_irqrestore(&its->lock, flags);
 
 	its_wait_for_range_completion(its, cmd, next_cmd);
 }
@@ -799,21 +800,44 @@ static int its_alloc_tables(struct its_node *its)
 {
 	int err;
 	int i;
-	int psz = PAGE_SIZE;
+	int psz = SZ_64K;
 	u64 shr = GITS_BASER_InnerShareable;
+	u64 cache = GITS_BASER_WaWb;
 
 	for (i = 0; i < GITS_BASER_NR_REGS; i++) {
 		u64 val = readq_relaxed(its->base + GITS_BASER + i * 8);
 		u64 type = GITS_BASER_TYPE(val);
 		u64 entry_size = GITS_BASER_ENTRY_SIZE(val);
+		int order = get_order(psz);
+		int alloc_size;
 		u64 tmp;
 		void *base;
 
 		if (type == GITS_BASER_TYPE_NONE)
 			continue;
 
-		/* We're lazy and only allocate a single page for now */
-		base = (void *)get_zeroed_page(GFP_KERNEL);
+		/*
+		 * Allocate as many entries as required to fit the
+		 * range of device IDs that the ITS can grok... The ID
+		 * space being incredibly sparse, this results in a
+		 * massive waste of memory.
+		 *
+		 * For other tables, only allocate a single page.
+		 */
+		if (type == GITS_BASER_TYPE_DEVICE) {
+			u64 typer = readq_relaxed(its->base + GITS_TYPER);
+			u32 ids = GITS_TYPER_DEVBITS(typer);
+
+			order = get_order((1UL << ids) * entry_size);
+			if (order >= MAX_ORDER) {
+				order = MAX_ORDER - 1;
+				pr_warn("%s: Device Table too large, reduce its page order to %u\n",
+					its->msi_chip.of_node->full_name, order);
+			}
+		}
+
+		alloc_size = (1 << order) * PAGE_SIZE;
+		base = (void *)__get_free_pages(GFP_KERNEL | __GFP_ZERO, order);
 		if (!base) {
 			err = -ENOMEM;
 			goto out_free;
@@ -825,7 +849,7 @@ retry_baser:
 		val = (virt_to_phys(base) 				 |
 		       (type << GITS_BASER_TYPE_SHIFT)			 |
 		       ((entry_size - 1) << GITS_BASER_ENTRY_SIZE_SHIFT) |
-		       GITS_BASER_WaWb					 |
+		       cache						 |
 		       shr						 |
 		       GITS_BASER_VALID);
 
@@ -841,7 +865,7 @@ retry_baser:
 			break;
 		}
 
-		val |= (PAGE_SIZE / psz) - 1;
+		val |= (alloc_size / psz) - 1;
 
 		writeq_relaxed(val, its->base + GITS_BASER + i * 8);
 		tmp = readq_relaxed(its->base + GITS_BASER + i * 8);
@@ -851,9 +875,12 @@ retry_baser:
 			 * Shareability didn't stick. Just use
 			 * whatever the read reported, which is likely
 			 * to be the only thing this redistributor
-			 * supports.
+			 * supports. If that's zero, make it
+			 * non-cacheable as well.
 			 */
 			shr = tmp & GITS_BASER_SHAREABILITY_MASK;
+			if (!shr)
+				cache = GITS_BASER_nC;
 			goto retry_baser;
 		}
 
@@ -882,7 +909,7 @@ retry_baser:
 		}
 
 		pr_info("ITS: allocated %d %s @%lx (psz %dK, shr %d)\n",
-			(int)(PAGE_SIZE / entry_size),
+			(int)(alloc_size / entry_size),
 			its_base_type_string[type],
 			(unsigned long)virt_to_phys(base),
 			psz / SZ_1K, (int)shr >> GITS_BASER_SHAREABILITY_SHIFT);
@@ -957,16 +984,39 @@ static void its_cpu_init_lpis(void)
 	tmp = readq_relaxed(rbase + GICR_PROPBASER);
 
 	if ((tmp ^ val) & GICR_PROPBASER_SHAREABILITY_MASK) {
+		if (!(tmp & GICR_PROPBASER_SHAREABILITY_MASK)) {
+			/*
+			 * The HW reports non-shareable, we must
+			 * remove the cacheability attributes as
+			 * well.
+			 */
+			val &= ~(GICR_PROPBASER_SHAREABILITY_MASK |
+				 GICR_PROPBASER_CACHEABILITY_MASK);
+			val |= GICR_PROPBASER_nC;
+			writeq_relaxed(val, rbase + GICR_PROPBASER);
+		}
 		pr_info_once("GIC: using cache flushing for LPI property table\n");
 		gic_rdists->flags |= RDIST_FLAGS_PROPBASE_NEEDS_FLUSHING;
 	}
 
 	/* set PENDBASE */
 	val = (page_to_phys(pend_page) |
-	       GICR_PROPBASER_InnerShareable |
-	       GICR_PROPBASER_WaWb);
+	       GICR_PENDBASER_InnerShareable |
+	       GICR_PENDBASER_WaWb);
 
 	writeq_relaxed(val, rbase + GICR_PENDBASER);
+	tmp = readq_relaxed(rbase + GICR_PENDBASER);
+
+	if (!(tmp & GICR_PENDBASER_SHAREABILITY_MASK)) {
+		/*
+		 * The HW reports non-shareable, we must remove the
+		 * cacheability attributes as well.
+		 */
+		val &= ~(GICR_PENDBASER_SHAREABILITY_MASK |
+			 GICR_PENDBASER_CACHEABILITY_MASK);
+		val |= GICR_PENDBASER_nC;
+		writeq_relaxed(val, rbase + GICR_PENDBASER);
+	}
 
 	/* Enable LPIs */
 	val = readl_relaxed(rbase + GICR_CTLR);
@@ -1003,7 +1053,7 @@ static void its_cpu_init_collection(void)
 			 * This ITS wants a linear CPU number.
 			 */
 			target = readq_relaxed(gic_data_rdist_rd_base() + GICR_TYPER);
-			target = GICR_TYPER_CPU_NUMBER(target);
+			target = GICR_TYPER_CPU_NUMBER(target) << 16;
 		}
 
 		/* Perform collection mapping */
@@ -1020,8 +1070,9 @@ static void its_cpu_init_collection(void)
 static struct its_device *its_find_device(struct its_node *its, u32 dev_id)
 {
 	struct its_device *its_dev = NULL, *tmp;
+	unsigned long flags;
 
-	raw_spin_lock(&its->lock);
+	raw_spin_lock_irqsave(&its->lock, flags);
 
 	list_for_each_entry(tmp, &its->its_device_list, entry) {
 		if (tmp->device_id == dev_id) {
@@ -1030,7 +1081,7 @@ static struct its_device *its_find_device(struct its_node *its, u32 dev_id)
 		}
 	}
 
-	raw_spin_unlock(&its->lock);
+	raw_spin_unlock_irqrestore(&its->lock, flags);
 
 	return its_dev;
 }
@@ -1040,6 +1091,7 @@ static struct its_device *its_create_device(struct its_node *its, u32 dev_id,
 {
 	struct its_device *dev;
 	unsigned long *lpi_map;
+	unsigned long flags;
 	void *itt;
 	int lpi_base;
 	int nr_lpis;
@@ -1056,7 +1108,7 @@ static struct its_device *its_create_device(struct its_node *its, u32 dev_id,
 	nr_ites = max(2UL, roundup_pow_of_two(nvecs));
 	sz = nr_ites * its->ite_size;
 	sz = max(sz, ITS_ITT_ALIGN) + ITS_ITT_ALIGN - 1;
-	itt = kmalloc(sz, GFP_KERNEL);
+	itt = kzalloc(sz, GFP_KERNEL);
 	lpi_map = its_lpi_alloc_chunks(nvecs, &lpi_base, &nr_lpis);
 
 	if (!dev || !itt || !lpi_map) {
@@ -1075,9 +1127,9 @@ static struct its_device *its_create_device(struct its_node *its, u32 dev_id,
 	dev->device_id = dev_id;
 	INIT_LIST_HEAD(&dev->entry);
 
-	raw_spin_lock(&its->lock);
+	raw_spin_lock_irqsave(&its->lock, flags);
 	list_add(&dev->entry, &its->its_device_list);
-	raw_spin_unlock(&its->lock);
+	raw_spin_unlock_irqrestore(&its->lock, flags);
 
 	/* Bind the device to the first possible CPU */
 	cpu = cpumask_first(cpu_online_mask);
@@ -1091,9 +1143,11 @@ static struct its_device *its_create_device(struct its_node *its, u32 dev_id,
 
 static void its_free_device(struct its_device *its_dev)
 {
-	raw_spin_lock(&its_dev->its->lock);
+	unsigned long flags;
+
+	raw_spin_lock_irqsave(&its_dev->its->lock, flags);
 	list_del(&its_dev->entry);
-	raw_spin_unlock(&its_dev->its->lock);
+	raw_spin_unlock_irqrestore(&its_dev->its->lock, flags);
 	kfree(its_dev->itt);
 	kfree(its_dev);
 }
@@ -1112,31 +1166,69 @@ static int its_alloc_device_irq(struct its_device *dev, irq_hw_number_t *hwirq)
 	return 0;
 }
 
+struct its_pci_alias {
+	struct pci_dev	*pdev;
+	u32		dev_id;
+	u32		count;
+};
+
+static int its_pci_msi_vec_count(struct pci_dev *pdev)
+{
+	int msi, msix;
+
+	msi = max(pci_msi_vec_count(pdev), 0);
+	msix = max(pci_msix_vec_count(pdev), 0);
+
+	return max(msi, msix);
+}
+
+static int its_get_pci_alias(struct pci_dev *pdev, u16 alias, void *data)
+{
+	struct its_pci_alias *dev_alias = data;
+
+	dev_alias->dev_id = alias;
+	if (pdev != dev_alias->pdev)
+		dev_alias->count += its_pci_msi_vec_count(dev_alias->pdev);
+
+	return 0;
+}
+
 static int its_msi_prepare(struct irq_domain *domain, struct device *dev,
 			   int nvec, msi_alloc_info_t *info)
 {
 	struct pci_dev *pdev;
 	struct its_node *its;
-	u32 dev_id;
 	struct its_device *its_dev;
+	struct its_pci_alias dev_alias;
 
 	if (!dev_is_pci(dev))
 		return -EINVAL;
 
 	pdev = to_pci_dev(dev);
-	dev_id = PCI_DEVID(pdev->bus->number, pdev->devfn);
+	dev_alias.pdev = pdev;
+	dev_alias.count = nvec;
+
+	pci_for_each_dma_alias(pdev, its_get_pci_alias, &dev_alias);
 	its = domain->parent->host_data;
 
-	its_dev = its_find_device(its, dev_id);
-	if (WARN_ON(its_dev))
-		return -EINVAL;
+	its_dev = its_find_device(its, dev_alias.dev_id);
+	if (its_dev) {
+		/*
+		 * We already have seen this ID, probably through
+		 * another alias (PCI bridge of some sort). No need to
+		 * create the device.
+		 */
+		dev_dbg(dev, "Reusing ITT for devID %x\n", dev_alias.dev_id);
+		goto out;
+	}
 
-	its_dev = its_create_device(its, dev_id, nvec);
+	its_dev = its_create_device(its, dev_alias.dev_id, dev_alias.count);
 	if (!its_dev)
 		return -ENOMEM;
 
-	dev_dbg(&pdev->dev, "ITT %d entries, %d bits\n", nvec, ilog2(nvec));
-
+	dev_dbg(&pdev->dev, "ITT %d entries, %d bits\n",
+		dev_alias.count, ilog2(dev_alias.count));
+out:
 	info->scratchpad[0].ptr = its_dev;
 	info->scratchpad[1].ptr = dev;
 	return 0;
@@ -1255,6 +1347,34 @@ static const struct irq_domain_ops its_domain_ops = {
 	.deactivate		= its_irq_domain_deactivate,
 };
 
+static int its_force_quiescent(void __iomem *base)
+{
+	u32 count = 1000000;	/* 1s */
+	u32 val;
+
+	val = readl_relaxed(base + GITS_CTLR);
+	if (val & GITS_CTLR_QUIESCENT)
+		return 0;
+
+	/* Disable the generation of all interrupts to this ITS */
+	val &= ~GITS_CTLR_ENABLE;
+	writel_relaxed(val, base + GITS_CTLR);
+
+	/* Poll GITS_CTLR and wait until ITS becomes quiescent */
+	while (1) {
+		val = readl_relaxed(base + GITS_CTLR);
+		if (val & GITS_CTLR_QUIESCENT)
+			return 0;
+
+		count--;
+		if (!count)
+			return -EBUSY;
+
+		cpu_relax();
+		udelay(1);
+	}
+}
+
 static int its_probe(struct device_node *node, struct irq_domain *parent)
 {
 	struct resource res;
@@ -1283,6 +1403,13 @@ static int its_probe(struct device_node *node, struct irq_domain *parent)
 		goto out_unmap;
 	}
 
+	err = its_force_quiescent(its_base);
+	if (err) {
+		pr_warn("%s: failed to quiesce, giving up\n",
+			node->full_name);
+		goto out_unmap;
+	}
+
 	pr_info("ITS: %s\n", node->full_name);
 
 	its = kzalloc(sizeof(*its), GFP_KERNEL);
@@ -1322,14 +1449,26 @@ static int its_probe(struct device_node *node, struct irq_domain *parent)
 
 	writeq_relaxed(baser, its->base + GITS_CBASER);
 	tmp = readq_relaxed(its->base + GITS_CBASER);
-	writeq_relaxed(0, its->base + GITS_CWRITER);
-	writel_relaxed(1, its->base + GITS_CTLR);
 
-	if ((tmp ^ baser) & GITS_BASER_SHAREABILITY_MASK) {
+	if ((tmp ^ baser) & GITS_CBASER_SHAREABILITY_MASK) {
+		if (!(tmp & GITS_CBASER_SHAREABILITY_MASK)) {
+			/*
+			 * The HW reports non-shareable, we must
+			 * remove the cacheability attributes as
+			 * well.
+			 */
+			baser &= ~(GITS_CBASER_SHAREABILITY_MASK |
+				   GITS_CBASER_CACHEABILITY_MASK);
+			baser |= GITS_CBASER_nC;
+			writeq_relaxed(baser, its->base + GITS_CBASER);
+		}
 		pr_info("ITS: using cache flushing for cmd queue\n");
 		its->flags |= ITS_FLAGS_CMDQ_NEEDS_FLUSHING;
 	}
 
+	writeq_relaxed(0, its->base + GITS_CWRITER);
+	writel_relaxed(GITS_CTLR_ENABLE, its->base + GITS_CTLR);
+
 	if (of_property_read_bool(its->msi_chip.of_node, "msi-controller")) {
 		its->domain = irq_domain_add_tree(NULL, &its_domain_ops, its);
 		if (!its->domain) {
@@ -1382,12 +1521,11 @@ static bool gic_rdists_supports_plpis(void)
 
 int its_cpu_init(void)
 {
-	if (!gic_rdists_supports_plpis()) {
-		pr_info("CPU%d: LPIs not supported\n", smp_processor_id());
-		return -ENXIO;
-	}
-
 	if (!list_empty(&its_nodes)) {
+		if (!gic_rdists_supports_plpis()) {
+			pr_info("CPU%d: LPIs not supported\n", smp_processor_id());
+			return -ENXIO;
+		}
 		its_cpu_init_lpis();
 		its_cpu_init_collection();
 	}
diff --git a/drivers/irqchip/irq-gic-v3.c b/drivers/irqchip/irq-gic-v3.c
index 1c6dea2fbc34..4f2fb62e6f37 100644
--- a/drivers/irqchip/irq-gic-v3.c
+++ b/drivers/irqchip/irq-gic-v3.c
@@ -195,6 +195,19 @@ static void gic_enable_redist(bool enable)
 /*
  * Routines to disable, enable, EOI and route interrupts
  */
+static int gic_peek_irq(struct irq_data *d, u32 offset)
+{
+	u32 mask = 1 << (gic_irq(d) % 32);
+	void __iomem *base;
+
+	if (gic_irq_in_rdist(d))
+		base = gic_data_rdist_sgi_base();
+	else
+		base = gic_data.dist_base;
+
+	return !!(readl_relaxed(base + offset + (gic_irq(d) / 32) * 4) & mask);
+}
+
 static void gic_poke_irq(struct irq_data *d, u32 offset)
 {
 	u32 mask = 1 << (gic_irq(d) % 32);
@@ -223,6 +236,61 @@ static void gic_unmask_irq(struct irq_data *d)
 	gic_poke_irq(d, GICD_ISENABLER);
 }
 
+static int gic_irq_set_irqchip_state(struct irq_data *d,
+				     enum irqchip_irq_state which, bool val)
+{
+	u32 reg;
+
+	if (d->hwirq >= gic_data.irq_nr) /* PPI/SPI only */
+		return -EINVAL;
+
+	switch (which) {
+	case IRQCHIP_STATE_PENDING:
+		reg = val ? GICD_ISPENDR : GICD_ICPENDR;
+		break;
+
+	case IRQCHIP_STATE_ACTIVE:
+		reg = val ? GICD_ISACTIVER : GICD_ICACTIVER;
+		break;
+
+	case IRQCHIP_STATE_MASKED:
+		reg = val ? GICD_ICENABLER : GICD_ISENABLER;
+		break;
+
+	default:
+		return -EINVAL;
+	}
+
+	gic_poke_irq(d, reg);
+	return 0;
+}
+
+static int gic_irq_get_irqchip_state(struct irq_data *d,
+				     enum irqchip_irq_state which, bool *val)
+{
+	if (d->hwirq >= gic_data.irq_nr) /* PPI/SPI only */
+		return -EINVAL;
+
+	switch (which) {
+	case IRQCHIP_STATE_PENDING:
+		*val = gic_peek_irq(d, GICD_ISPENDR);
+		break;
+
+	case IRQCHIP_STATE_ACTIVE:
+		*val = gic_peek_irq(d, GICD_ISACTIVER);
+		break;
+
+	case IRQCHIP_STATE_MASKED:
+		*val = !gic_peek_irq(d, GICD_ISENABLER);
+		break;
+
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
 static void gic_eoi_irq(struct irq_data *d)
 {
 	gic_write_eoir(gic_irq(d));
@@ -418,19 +486,6 @@ static void gic_cpu_init(void)
 }
 
 #ifdef CONFIG_SMP
-static int gic_peek_irq(struct irq_data *d, u32 offset)
-{
-	u32 mask = 1 << (gic_irq(d) % 32);
-	void __iomem *base;
-
-	if (gic_irq_in_rdist(d))
-		base = gic_data_rdist_sgi_base();
-	else
-		base = gic_data.dist_base;
-
-	return !!(readl_relaxed(base + offset + (gic_irq(d) / 32) * 4) & mask);
-}
-
 static int gic_secondary_init(struct notifier_block *nfb,
 			      unsigned long action, void *hcpu)
 {
@@ -466,7 +521,7 @@ static u16 gic_compute_target_list(int *base_cpu, const struct cpumask *mask,
 		tlist |= 1 << (mpidr & 0xf);
 
 		cpu = cpumask_next(cpu, mask);
-		if (cpu == nr_cpu_ids)
+		if (cpu >= nr_cpu_ids)
 			goto out;
 
 		mpidr = cpu_logical_map(cpu);
@@ -601,6 +656,8 @@ static struct irq_chip gic_chip = {
 	.irq_eoi		= gic_eoi_irq,
 	.irq_set_type		= gic_set_type,
 	.irq_set_affinity	= gic_set_affinity,
+	.irq_get_irqchip_state	= gic_irq_get_irqchip_state,
+	.irq_set_irqchip_state	= gic_irq_set_irqchip_state,
 };
 
 #define GIC_ID_NR		(1U << gic_data.rdists.id_bits)
diff --git a/drivers/irqchip/irq-gic.c b/drivers/irqchip/irq-gic.c
index 4634cf7d0ec3..a6ce3476834e 100644
--- a/drivers/irqchip/irq-gic.c
+++ b/drivers/irqchip/irq-gic.c
@@ -151,26 +151,38 @@ static inline unsigned int gic_irq(struct irq_data *d)
 /*
  * Routines to acknowledge, disable and enable interrupts
  */
-static void gic_mask_irq(struct irq_data *d)
+static void gic_poke_irq(struct irq_data *d, u32 offset)
 {
 	u32 mask = 1 << (gic_irq(d) % 32);
+	writel_relaxed(mask, gic_dist_base(d) + offset + (gic_irq(d) / 32) * 4);
+}
 
-	raw_spin_lock(&irq_controller_lock);
-	writel_relaxed(mask, gic_dist_base(d) + GIC_DIST_ENABLE_CLEAR + (gic_irq(d) / 32) * 4);
+static int gic_peek_irq(struct irq_data *d, u32 offset)
+{
+	u32 mask = 1 << (gic_irq(d) % 32);
+	return !!(readl_relaxed(gic_dist_base(d) + offset + (gic_irq(d) / 32) * 4) & mask);
+}
+
+static void gic_mask_irq(struct irq_data *d)
+{
+	unsigned long flags;
+
+	raw_spin_lock_irqsave(&irq_controller_lock, flags);
+	gic_poke_irq(d, GIC_DIST_ENABLE_CLEAR);
 	if (gic_arch_extn.irq_mask)
 		gic_arch_extn.irq_mask(d);
-	raw_spin_unlock(&irq_controller_lock);
+	raw_spin_unlock_irqrestore(&irq_controller_lock, flags);
 }
 
 static void gic_unmask_irq(struct irq_data *d)
 {
-	u32 mask = 1 << (gic_irq(d) % 32);
+	unsigned long flags;
 
-	raw_spin_lock(&irq_controller_lock);
+	raw_spin_lock_irqsave(&irq_controller_lock, flags);
 	if (gic_arch_extn.irq_unmask)
 		gic_arch_extn.irq_unmask(d);
-	writel_relaxed(mask, gic_dist_base(d) + GIC_DIST_ENABLE_SET + (gic_irq(d) / 32) * 4);
-	raw_spin_unlock(&irq_controller_lock);
+	gic_poke_irq(d, GIC_DIST_ENABLE_SET);
+	raw_spin_unlock_irqrestore(&irq_controller_lock, flags);
 }
 
 static void gic_eoi_irq(struct irq_data *d)
@@ -184,10 +196,60 @@ static void gic_eoi_irq(struct irq_data *d)
 	writel_relaxed(gic_irq(d), gic_cpu_base(d) + GIC_CPU_EOI);
 }
 
+static int gic_irq_set_irqchip_state(struct irq_data *d,
+				     enum irqchip_irq_state which, bool val)
+{
+	u32 reg;
+
+	switch (which) {
+	case IRQCHIP_STATE_PENDING:
+		reg = val ? GIC_DIST_PENDING_SET : GIC_DIST_PENDING_CLEAR;
+		break;
+
+	case IRQCHIP_STATE_ACTIVE:
+		reg = val ? GIC_DIST_ACTIVE_SET : GIC_DIST_ACTIVE_CLEAR;
+		break;
+
+	case IRQCHIP_STATE_MASKED:
+		reg = val ? GIC_DIST_ENABLE_CLEAR : GIC_DIST_ENABLE_SET;
+		break;
+
+	default:
+		return -EINVAL;
+	}
+
+	gic_poke_irq(d, reg);
+	return 0;
+}
+
+static int gic_irq_get_irqchip_state(struct irq_data *d,
+				      enum irqchip_irq_state which, bool *val)
+{
+	switch (which) {
+	case IRQCHIP_STATE_PENDING:
+		*val = gic_peek_irq(d, GIC_DIST_PENDING_SET);
+		break;
+
+	case IRQCHIP_STATE_ACTIVE:
+		*val = gic_peek_irq(d, GIC_DIST_ACTIVE_SET);
+		break;
+
+	case IRQCHIP_STATE_MASKED:
+		*val = !gic_peek_irq(d, GIC_DIST_ENABLE_SET);
+		break;
+
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
 static int gic_set_type(struct irq_data *d, unsigned int type)
 {
 	void __iomem *base = gic_dist_base(d);
 	unsigned int gicirq = gic_irq(d);
+	unsigned long flags;
 	int ret;
 
 	/* Interrupt configuration for SGIs can't be changed */
@@ -199,14 +261,14 @@ static int gic_set_type(struct irq_data *d, unsigned int type)
 			    type != IRQ_TYPE_EDGE_RISING)
 		return -EINVAL;
 
-	raw_spin_lock(&irq_controller_lock);
+	raw_spin_lock_irqsave(&irq_controller_lock, flags);
 
 	if (gic_arch_extn.irq_set_type)
 		gic_arch_extn.irq_set_type(d, type);
 
 	ret = gic_configure_irq(gicirq, type, base, NULL);
 
-	raw_spin_unlock(&irq_controller_lock);
+	raw_spin_unlock_irqrestore(&irq_controller_lock, flags);
 
 	return ret;
 }
@@ -227,6 +289,7 @@ static int gic_set_affinity(struct irq_data *d, const struct cpumask *mask_val,
 	void __iomem *reg = gic_dist_base(d) + GIC_DIST_TARGET + (gic_irq(d) & ~3);
 	unsigned int cpu, shift = (gic_irq(d) % 4) * 8;
 	u32 val, mask, bit;
+	unsigned long flags;
 
 	if (!force)
 		cpu = cpumask_any_and(mask_val, cpu_online_mask);
@@ -236,12 +299,12 @@ static int gic_set_affinity(struct irq_data *d, const struct cpumask *mask_val,
 	if (cpu >= NR_GIC_CPU_IF || cpu >= nr_cpu_ids)
 		return -EINVAL;
 
-	raw_spin_lock(&irq_controller_lock);
+	raw_spin_lock_irqsave(&irq_controller_lock, flags);
 	mask = 0xff << shift;
 	bit = gic_cpu_map[cpu] << shift;
 	val = readl_relaxed(reg) & ~mask;
 	writel_relaxed(val | bit, reg);
-	raw_spin_unlock(&irq_controller_lock);
+	raw_spin_unlock_irqrestore(&irq_controller_lock, flags);
 
 	return IRQ_SET_MASK_OK;
 }
@@ -325,6 +388,8 @@ static struct irq_chip gic_chip = {
 	.irq_set_affinity	= gic_set_affinity,
 #endif
 	.irq_set_wake		= gic_set_wake,
+	.irq_get_irqchip_state	= gic_irq_get_irqchip_state,
+	.irq_set_irqchip_state	= gic_irq_set_irqchip_state,
 };
 
 void __init gic_cascade_irq(unsigned int gic_nr, unsigned int irq)
@@ -349,7 +414,7 @@ static u8 gic_get_cpumask(struct gic_chip_data *gic)
 			break;
 	}
 
-	if (!mask)
+	if (!mask && num_possible_cpus() > 1)
 		pr_crit("GIC CPU mask not found - kernel will fail to boot.\n");
 
 	return mask;
@@ -798,15 +863,12 @@ static int gic_irq_domain_map(struct irq_domain *d, unsigned int irq,
 		irq_domain_set_info(d, irq, hw, &gic_chip, d->host_data,
 				    handle_fasteoi_irq, NULL, NULL);
 		set_irq_flags(irq, IRQF_VALID | IRQF_PROBE);
-
-		gic_routable_irq_domain_ops->map(d, irq, hw);
 	}
 	return 0;
 }
 
 static void gic_irq_domain_unmap(struct irq_domain *d, unsigned int irq)
 {
-	gic_routable_irq_domain_ops->unmap(d, irq);
 }
 
 static int gic_irq_domain_xlate(struct irq_domain *d,
@@ -825,16 +887,8 @@ static int gic_irq_domain_xlate(struct irq_domain *d,
 	*out_hwirq = intspec[1] + 16;
 
 	/* For SPIs, we need to add 16 more to get the GIC irq ID number */
-	if (!intspec[0]) {
-		ret = gic_routable_irq_domain_ops->xlate(d, controller,
-							 intspec,
-							 intsize,
-							 out_hwirq,
-							 out_type);
-
-		if (IS_ERR_VALUE(ret))
-			return ret;
-	}
+	if (!intspec[0])
+		*out_hwirq += 16;
 
 	*out_type = intspec[2] & IRQ_TYPE_SENSE_MASK;
 
@@ -891,37 +945,11 @@ static const struct irq_domain_ops gic_irq_domain_ops = {
 	.xlate = gic_irq_domain_xlate,
 };
 
-/* Default functions for routable irq domain */
-static int gic_routable_irq_domain_map(struct irq_domain *d, unsigned int irq,
-			      irq_hw_number_t hw)
-{
-	return 0;
-}
-
-static void gic_routable_irq_domain_unmap(struct irq_domain *d,
-					  unsigned int irq)
+void gic_set_irqchip_flags(unsigned long flags)
 {
+	gic_chip.flags |= flags;
 }
 
-static int gic_routable_irq_domain_xlate(struct irq_domain *d,
-				struct device_node *controller,
-				const u32 *intspec, unsigned int intsize,
-				unsigned long *out_hwirq,
-				unsigned int *out_type)
-{
-	*out_hwirq += 16;
-	return 0;
-}
-
-static const struct irq_domain_ops gic_default_routable_irq_domain_ops = {
-	.map = gic_routable_irq_domain_map,
-	.unmap = gic_routable_irq_domain_unmap,
-	.xlate = gic_routable_irq_domain_xlate,
-};
-
-const struct irq_domain_ops *gic_routable_irq_domain_ops =
-					&gic_default_routable_irq_domain_ops;
-
 void __init gic_init_bases(unsigned int gic_nr, int irq_start,
 			   void __iomem *dist_base, void __iomem *cpu_base,
 			   u32 percpu_offset, struct device_node *node)
@@ -929,7 +957,6 @@ void __init gic_init_bases(unsigned int gic_nr, int irq_start,
 	irq_hw_number_t hwirq_base;
 	struct gic_chip_data *gic;
 	int gic_irqs, irq_base, i;
-	int nr_routable_irqs;
 
 	BUG_ON(gic_nr >= MAX_GIC_NR);
 
@@ -985,15 +1012,9 @@ void __init gic_init_bases(unsigned int gic_nr, int irq_start,
 	gic->gic_irqs = gic_irqs;
 
 	if (node) {		/* DT case */
-		const struct irq_domain_ops *ops = &gic_irq_domain_hierarchy_ops;
-
-		if (!of_property_read_u32(node, "arm,routable-irqs",
-					  &nr_routable_irqs)) {
-			ops = &gic_irq_domain_ops;
-			gic_irqs = nr_routable_irqs;
-		}
-
-		gic->domain = irq_domain_add_linear(node, gic_irqs, ops, gic);
+		gic->domain = irq_domain_add_linear(node, gic_irqs,
+						    &gic_irq_domain_hierarchy_ops,
+						    gic);
 	} else {		/* Non-DT case */
 		/*
 		 * For primary GICs, skip over SGIs.
diff --git a/drivers/irqchip/irq-mips-gic.c b/drivers/irqchip/irq-mips-gic.c
index 9acdc080e7ec..f2d269bca789 100644
--- a/drivers/irqchip/irq-mips-gic.c
+++ b/drivers/irqchip/irq-mips-gic.c
@@ -166,6 +166,27 @@ cycle_t gic_read_compare(void)
 
 	return (((cycle_t) hi) << 32) + lo;
 }
+
+void gic_start_count(void)
+{
+	u32 gicconfig;
+
+	/* Start the counter */
+	gicconfig = gic_read(GIC_REG(SHARED, GIC_SH_CONFIG));
+	gicconfig &= ~(1 << GIC_SH_CONFIG_COUNTSTOP_SHF);
+	gic_write(GIC_REG(SHARED, GIC_SH_CONFIG), gicconfig);
+}
+
+void gic_stop_count(void)
+{
+	u32 gicconfig;
+
+	/* Stop the counter */
+	gicconfig = gic_read(GIC_REG(SHARED, GIC_SH_CONFIG));
+	gicconfig |= 1 << GIC_SH_CONFIG_COUNTSTOP_SHF;
+	gic_write(GIC_REG(SHARED, GIC_SH_CONFIG), gicconfig);
+}
+
 #endif
 
 static bool gic_local_irq_is_routable(int intr)
diff --git a/drivers/irqchip/irq-renesas-irqc.c b/drivers/irqchip/irq-renesas-irqc.c
index 384e6ed61d7c..cdf80b7794cd 100644
--- a/drivers/irqchip/irq-renesas-irqc.c
+++ b/drivers/irqchip/irq-renesas-irqc.c
@@ -17,6 +17,7 @@
  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
  */
 
+#include <linux/clk.h>
 #include <linux/init.h>
 #include <linux/platform_device.h>
 #include <linux/spinlock.h>
@@ -29,15 +30,26 @@
 #include <linux/slab.h>
 #include <linux/module.h>
 #include <linux/platform_data/irq-renesas-irqc.h>
+#include <linux/pm_runtime.h>
 
-#define IRQC_IRQ_MAX 32 /* maximum 32 interrupts per driver instance */
+#define IRQC_IRQ_MAX	32	/* maximum 32 interrupts per driver instance */
 
-#define IRQC_REQ_STS 0x00
-#define IRQC_EN_STS 0x04
-#define IRQC_EN_SET 0x08
+#define IRQC_REQ_STS	0x00	/* Interrupt Request Status Register */
+#define IRQC_EN_STS	0x04	/* Interrupt Enable Status Register */
+#define IRQC_EN_SET	0x08	/* Interrupt Enable Set Register */
 #define IRQC_INT_CPU_BASE(n) (0x000 + ((n) * 0x10))
-#define DETECT_STATUS 0x100
+				/* SYS-CPU vs. RT-CPU */
+#define DETECT_STATUS	0x100	/* IRQn Detect Status Register */
+#define MONITOR		0x104	/* IRQn Signal Level Monitor Register */
+#define HLVL_STS	0x108	/* IRQn High Level Detect Status Register */
+#define LLVL_STS	0x10c	/* IRQn Low Level Detect Status Register */
+#define S_R_EDGE_STS	0x110	/* IRQn Sync Rising Edge Detect Status Reg. */
+#define S_F_EDGE_STS	0x114	/* IRQn Sync Falling Edge Detect Status Reg. */
+#define A_R_EDGE_STS	0x118	/* IRQn Async Rising Edge Detect Status Reg. */
+#define A_F_EDGE_STS	0x11c	/* IRQn Async Falling Edge Detect Status Reg. */
+#define CHTEN_STS	0x120	/* Chattering Reduction Status Register */
 #define IRQC_CONFIG(n) (0x180 + ((n) * 0x04))
+				/* IRQn Configuration Register */
 
 struct irqc_irq {
 	int hw_irq;
@@ -55,6 +67,7 @@ struct irqc_priv {
 	struct platform_device *pdev;
 	struct irq_chip irq_chip;
 	struct irq_domain *irq_domain;
+	struct clk *clk;
 };
 
 static void irqc_dbg(struct irqc_irq *i, char *str)
@@ -94,7 +107,7 @@ static int irqc_irq_set_type(struct irq_data *d, unsigned int type)
 	struct irqc_priv *p = irq_data_get_irq_chip_data(d);
 	int hw_irq = irqd_to_hwirq(d);
 	unsigned char value = irqc_sense[type & IRQ_TYPE_SENSE_MASK];
-	unsigned long tmp;
+	u32 tmp;
 
 	irqc_dbg(&p->irq[hw_irq], "sense");
 
@@ -108,11 +121,26 @@ static int irqc_irq_set_type(struct irq_data *d, unsigned int type)
 	return 0;
 }
 
+static int irqc_irq_set_wake(struct irq_data *d, unsigned int on)
+{
+	struct irqc_priv *p = irq_data_get_irq_chip_data(d);
+
+	if (!p->clk)
+		return 0;
+
+	if (on)
+		clk_enable(p->clk);
+	else
+		clk_disable(p->clk);
+
+	return 0;
+}
+
 static irqreturn_t irqc_irq_handler(int irq, void *dev_id)
 {
 	struct irqc_irq *i = dev_id;
 	struct irqc_priv *p = i->p;
-	unsigned long bit = BIT(i->hw_irq);
+	u32 bit = BIT(i->hw_irq);
 
 	irqc_dbg(i, "demux1");
 
@@ -170,6 +198,15 @@ static int irqc_probe(struct platform_device *pdev)
 	p->pdev = pdev;
 	platform_set_drvdata(pdev, p);
 
+	p->clk = devm_clk_get(&pdev->dev, NULL);
+	if (IS_ERR(p->clk)) {
+		dev_warn(&pdev->dev, "unable to get clock\n");
+		p->clk = NULL;
+	}
+
+	pm_runtime_enable(&pdev->dev);
+	pm_runtime_get_sync(&pdev->dev);
+
 	/* get hold of manadatory IOMEM */
 	io = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	if (!io) {
@@ -210,7 +247,8 @@ static int irqc_probe(struct platform_device *pdev)
 	irq_chip->irq_mask = irqc_irq_disable;
 	irq_chip->irq_unmask = irqc_irq_enable;
 	irq_chip->irq_set_type = irqc_irq_set_type;
-	irq_chip->flags	= IRQCHIP_SKIP_SET_WAKE | IRQCHIP_MASK_ON_SUSPEND;
+	irq_chip->irq_set_wake = irqc_irq_set_wake;
+	irq_chip->flags	= IRQCHIP_MASK_ON_SUSPEND;
 
 	p->irq_domain = irq_domain_add_simple(pdev->dev.of_node,
 					      p->number_of_irqs,
@@ -250,6 +288,8 @@ err3:
 err2:
 	iounmap(p->iomem);
 err1:
+	pm_runtime_put(&pdev->dev);
+	pm_runtime_disable(&pdev->dev);
 	kfree(p);
 err0:
 	return ret;
@@ -265,6 +305,8 @@ static int irqc_remove(struct platform_device *pdev)
 
 	irq_domain_remove(p->irq_domain);
 	iounmap(p->iomem);
+	pm_runtime_put(&pdev->dev);
+	pm_runtime_disable(&pdev->dev);
 	kfree(p);
 	return 0;
 }
diff --git a/drivers/irqchip/irq-st.c b/drivers/irqchip/irq-st.c
new file mode 100644
index 000000000000..9af48a85c16f
--- /dev/null
+++ b/drivers/irqchip/irq-st.c
@@ -0,0 +1,206 @@
+/*
+ *  Copyright (C) 2014 STMicroelectronics – All Rights Reserved
+ *
+ *  Author: Lee Jones <lee.jones@linaro.org>
+ *
+ *  This is a re-write of Christophe Kerello's PMU driver.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <dt-bindings/interrupt-controller/irq-st.h>
+#include <linux/err.h>
+#include <linux/mfd/syscon.h>
+#include <linux/of_device.h>
+#include <linux/platform_device.h>
+#include <linux/regmap.h>
+#include <linux/slab.h>
+
+#define STIH415_SYSCFG_642		0x0a8
+#define STIH416_SYSCFG_7543		0x87c
+#define STIH407_SYSCFG_5102		0x198
+#define STID127_SYSCFG_734		0x088
+
+#define ST_A9_IRQ_MASK			0x001FFFFF
+#define ST_A9_IRQ_MAX_CHANS		2
+
+#define ST_A9_IRQ_EN_CTI_0		BIT(0)
+#define ST_A9_IRQ_EN_CTI_1		BIT(1)
+#define ST_A9_IRQ_EN_PMU_0		BIT(2)
+#define ST_A9_IRQ_EN_PMU_1		BIT(3)
+#define ST_A9_IRQ_EN_PL310_L2		BIT(4)
+#define ST_A9_IRQ_EN_EXT_0		BIT(5)
+#define ST_A9_IRQ_EN_EXT_1		BIT(6)
+#define ST_A9_IRQ_EN_EXT_2		BIT(7)
+
+#define ST_A9_FIQ_N_SEL(dev, chan)	(dev << (8  + (chan * 3)))
+#define ST_A9_IRQ_N_SEL(dev, chan)	(dev << (14 + (chan * 3)))
+#define ST_A9_EXTIRQ_INV_SEL(dev)	(dev << 20)
+
+struct st_irq_syscfg {
+	struct regmap *regmap;
+	unsigned int syscfg;
+	unsigned int config;
+	bool ext_inverted;
+};
+
+static const struct of_device_id st_irq_syscfg_match[] = {
+	{
+		.compatible = "st,stih415-irq-syscfg",
+		.data = (void *)STIH415_SYSCFG_642,
+	},
+	{
+		.compatible = "st,stih416-irq-syscfg",
+		.data = (void *)STIH416_SYSCFG_7543,
+	},
+	{
+		.compatible = "st,stih407-irq-syscfg",
+		.data = (void *)STIH407_SYSCFG_5102,
+	},
+	{
+		.compatible = "st,stid127-irq-syscfg",
+		.data = (void *)STID127_SYSCFG_734,
+	},
+	{}
+};
+
+static int st_irq_xlate(struct platform_device *pdev,
+			int device, int channel, bool irq)
+{
+	struct st_irq_syscfg *ddata = dev_get_drvdata(&pdev->dev);
+
+	/* Set the device enable bit. */
+	switch (device) {
+	case ST_IRQ_SYSCFG_EXT_0:
+		ddata->config |= ST_A9_IRQ_EN_EXT_0;
+		break;
+	case ST_IRQ_SYSCFG_EXT_1:
+		ddata->config |= ST_A9_IRQ_EN_EXT_1;
+		break;
+	case ST_IRQ_SYSCFG_EXT_2:
+		ddata->config |= ST_A9_IRQ_EN_EXT_2;
+		break;
+	case ST_IRQ_SYSCFG_CTI_0:
+		ddata->config |= ST_A9_IRQ_EN_CTI_0;
+		break;
+	case ST_IRQ_SYSCFG_CTI_1:
+		ddata->config |= ST_A9_IRQ_EN_CTI_1;
+		break;
+	case ST_IRQ_SYSCFG_PMU_0:
+		ddata->config |= ST_A9_IRQ_EN_PMU_0;
+		break;
+	case ST_IRQ_SYSCFG_PMU_1:
+		ddata->config |= ST_A9_IRQ_EN_PMU_1;
+		break;
+	case ST_IRQ_SYSCFG_pl310_L2:
+		ddata->config |= ST_A9_IRQ_EN_PL310_L2;
+		break;
+	case ST_IRQ_SYSCFG_DISABLED:
+		return 0;
+	default:
+		dev_err(&pdev->dev, "Unrecognised device %d\n", device);
+		return -EINVAL;
+	}
+
+	/* Select IRQ/FIQ channel for device. */
+	ddata->config |= irq ?
+		ST_A9_IRQ_N_SEL(device, channel) :
+		ST_A9_FIQ_N_SEL(device, channel);
+
+	return 0;
+}
+
+static int st_irq_syscfg_enable(struct platform_device *pdev)
+{
+	struct device_node *np = pdev->dev.of_node;
+	struct st_irq_syscfg *ddata = dev_get_drvdata(&pdev->dev);
+	int channels, ret, i;
+	u32 device, invert;
+
+	channels = of_property_count_u32_elems(np, "st,irq-device");
+	if (channels != ST_A9_IRQ_MAX_CHANS) {
+		dev_err(&pdev->dev, "st,enable-irq-device must have 2 elems\n");
+		return -EINVAL;
+	}
+
+	channels = of_property_count_u32_elems(np, "st,fiq-device");
+	if (channels != ST_A9_IRQ_MAX_CHANS) {
+		dev_err(&pdev->dev, "st,enable-fiq-device must have 2 elems\n");
+		return -EINVAL;
+	}
+
+	for (i = 0; i < ST_A9_IRQ_MAX_CHANS; i++) {
+		of_property_read_u32_index(np, "st,irq-device", i, &device);
+
+		ret = st_irq_xlate(pdev, device, i, true);
+		if (ret)
+			return ret;
+
+		of_property_read_u32_index(np, "st,fiq-device", i, &device);
+
+		ret = st_irq_xlate(pdev, device, i, false);
+		if (ret)
+			return ret;
+	}
+
+	/* External IRQs may be inverted. */
+	of_property_read_u32(np, "st,invert-ext", &invert);
+	ddata->config |= ST_A9_EXTIRQ_INV_SEL(invert);
+
+	return regmap_update_bits(ddata->regmap, ddata->syscfg,
+				  ST_A9_IRQ_MASK, ddata->config);
+}
+
+static int st_irq_syscfg_probe(struct platform_device *pdev)
+{
+	struct device_node *np = pdev->dev.of_node;
+	const struct of_device_id *match;
+	struct st_irq_syscfg *ddata;
+
+	ddata = devm_kzalloc(&pdev->dev, sizeof(*ddata), GFP_KERNEL);
+	if (!ddata)
+		return -ENOMEM;
+
+	match = of_match_device(st_irq_syscfg_match, &pdev->dev);
+	if (!match)
+		return -ENODEV;
+
+	ddata->syscfg = (unsigned int)match->data;
+
+	ddata->regmap = syscon_regmap_lookup_by_phandle(np, "st,syscfg");
+	if (IS_ERR(ddata->regmap)) {
+		dev_err(&pdev->dev, "syscfg phandle missing\n");
+		return PTR_ERR(ddata->regmap);
+	}
+
+	dev_set_drvdata(&pdev->dev, ddata);
+
+	return st_irq_syscfg_enable(pdev);
+}
+
+static int st_irq_syscfg_resume(struct device *dev)
+{
+	struct st_irq_syscfg *ddata = dev_get_drvdata(dev);
+
+	return regmap_update_bits(ddata->regmap, ddata->syscfg,
+				  ST_A9_IRQ_MASK, ddata->config);
+}
+
+static SIMPLE_DEV_PM_OPS(st_irq_syscfg_pm_ops, NULL, st_irq_syscfg_resume);
+
+static struct platform_driver st_irq_syscfg_driver = {
+	.driver = {
+		.name = "st_irq_syscfg",
+		.pm = &st_irq_syscfg_pm_ops,
+		.of_match_table = st_irq_syscfg_match,
+	},
+	.probe = st_irq_syscfg_probe,
+};
+
+static int __init st_irq_syscfg_init(void)
+{
+	return platform_driver_register(&st_irq_syscfg_driver);
+}
+core_initcall(st_irq_syscfg_init);
diff --git a/drivers/irqchip/irq-tegra.c b/drivers/irqchip/irq-tegra.c
new file mode 100644
index 000000000000..51c485d9a877
--- /dev/null
+++ b/drivers/irqchip/irq-tegra.c
@@ -0,0 +1,377 @@
+/*
+ * Driver code for Tegra's Legacy Interrupt Controller
+ *
+ * Author: Marc Zyngier <marc.zyngier@arm.com>
+ *
+ * Heavily based on the original arch/arm/mach-tegra/irq.c code:
+ * Copyright (C) 2011 Google, Inc.
+ *
+ * Author:
+ *	Colin Cross <ccross@android.com>
+ *
+ * Copyright (C) 2010,2013, NVIDIA Corporation
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#include <linux/io.h>
+#include <linux/irq.h>
+#include <linux/irqdomain.h>
+#include <linux/of_address.h>
+#include <linux/slab.h>
+#include <linux/syscore_ops.h>
+
+#include <dt-bindings/interrupt-controller/arm-gic.h>
+
+#include "irqchip.h"
+
+#define ICTLR_CPU_IEP_VFIQ	0x08
+#define ICTLR_CPU_IEP_FIR	0x14
+#define ICTLR_CPU_IEP_FIR_SET	0x18
+#define ICTLR_CPU_IEP_FIR_CLR	0x1c
+
+#define ICTLR_CPU_IER		0x20
+#define ICTLR_CPU_IER_SET	0x24
+#define ICTLR_CPU_IER_CLR	0x28
+#define ICTLR_CPU_IEP_CLASS	0x2C
+
+#define ICTLR_COP_IER		0x30
+#define ICTLR_COP_IER_SET	0x34
+#define ICTLR_COP_IER_CLR	0x38
+#define ICTLR_COP_IEP_CLASS	0x3c
+
+#define TEGRA_MAX_NUM_ICTLRS	6
+
+static unsigned int num_ictlrs;
+
+struct tegra_ictlr_soc {
+	unsigned int num_ictlrs;
+};
+
+static const struct tegra_ictlr_soc tegra20_ictlr_soc = {
+	.num_ictlrs = 4,
+};
+
+static const struct tegra_ictlr_soc tegra30_ictlr_soc = {
+	.num_ictlrs = 5,
+};
+
+static const struct tegra_ictlr_soc tegra210_ictlr_soc = {
+	.num_ictlrs = 6,
+};
+
+static const struct of_device_id ictlr_matches[] = {
+	{ .compatible = "nvidia,tegra210-ictlr", .data = &tegra210_ictlr_soc },
+	{ .compatible = "nvidia,tegra30-ictlr", .data = &tegra30_ictlr_soc },
+	{ .compatible = "nvidia,tegra20-ictlr", .data = &tegra20_ictlr_soc },
+	{ }
+};
+
+struct tegra_ictlr_info {
+	void __iomem *base[TEGRA_MAX_NUM_ICTLRS];
+#ifdef CONFIG_PM_SLEEP
+	u32 cop_ier[TEGRA_MAX_NUM_ICTLRS];
+	u32 cop_iep[TEGRA_MAX_NUM_ICTLRS];
+	u32 cpu_ier[TEGRA_MAX_NUM_ICTLRS];
+	u32 cpu_iep[TEGRA_MAX_NUM_ICTLRS];
+
+	u32 ictlr_wake_mask[TEGRA_MAX_NUM_ICTLRS];
+#endif
+};
+
+static struct tegra_ictlr_info *lic;
+
+static inline void tegra_ictlr_write_mask(struct irq_data *d, unsigned long reg)
+{
+	void __iomem *base = d->chip_data;
+	u32 mask;
+
+	mask = BIT(d->hwirq % 32);
+	writel_relaxed(mask, base + reg);
+}
+
+static void tegra_mask(struct irq_data *d)
+{
+	tegra_ictlr_write_mask(d, ICTLR_CPU_IER_CLR);
+	irq_chip_mask_parent(d);
+}
+
+static void tegra_unmask(struct irq_data *d)
+{
+	tegra_ictlr_write_mask(d, ICTLR_CPU_IER_SET);
+	irq_chip_unmask_parent(d);
+}
+
+static void tegra_eoi(struct irq_data *d)
+{
+	tegra_ictlr_write_mask(d, ICTLR_CPU_IEP_FIR_CLR);
+	irq_chip_eoi_parent(d);
+}
+
+static int tegra_retrigger(struct irq_data *d)
+{
+	tegra_ictlr_write_mask(d, ICTLR_CPU_IEP_FIR_SET);
+	return irq_chip_retrigger_hierarchy(d);
+}
+
+#ifdef CONFIG_PM_SLEEP
+static int tegra_set_wake(struct irq_data *d, unsigned int enable)
+{
+	u32 irq = d->hwirq;
+	u32 index, mask;
+
+	index = (irq / 32);
+	mask = BIT(irq % 32);
+	if (enable)
+		lic->ictlr_wake_mask[index] |= mask;
+	else
+		lic->ictlr_wake_mask[index] &= ~mask;
+
+	/*
+	 * Do *not* call into the parent, as the GIC doesn't have any
+	 * wake-up facility...
+	 */
+	return 0;
+}
+
+static int tegra_ictlr_suspend(void)
+{
+	unsigned long flags;
+	unsigned int i;
+
+	local_irq_save(flags);
+	for (i = 0; i < num_ictlrs; i++) {
+		void __iomem *ictlr = lic->base[i];
+
+		/* Save interrupt state */
+		lic->cpu_ier[i] = readl_relaxed(ictlr + ICTLR_CPU_IER);
+		lic->cpu_iep[i] = readl_relaxed(ictlr + ICTLR_CPU_IEP_CLASS);
+		lic->cop_ier[i] = readl_relaxed(ictlr + ICTLR_COP_IER);
+		lic->cop_iep[i] = readl_relaxed(ictlr + ICTLR_COP_IEP_CLASS);
+
+		/* Disable COP interrupts */
+		writel_relaxed(~0ul, ictlr + ICTLR_COP_IER_CLR);
+
+		/* Disable CPU interrupts */
+		writel_relaxed(~0ul, ictlr + ICTLR_CPU_IER_CLR);
+
+		/* Enable the wakeup sources of ictlr */
+		writel_relaxed(lic->ictlr_wake_mask[i], ictlr + ICTLR_CPU_IER_SET);
+	}
+	local_irq_restore(flags);
+
+	return 0;
+}
+
+static void tegra_ictlr_resume(void)
+{
+	unsigned long flags;
+	unsigned int i;
+
+	local_irq_save(flags);
+	for (i = 0; i < num_ictlrs; i++) {
+		void __iomem *ictlr = lic->base[i];
+
+		writel_relaxed(lic->cpu_iep[i],
+			       ictlr + ICTLR_CPU_IEP_CLASS);
+		writel_relaxed(~0ul, ictlr + ICTLR_CPU_IER_CLR);
+		writel_relaxed(lic->cpu_ier[i],
+			       ictlr + ICTLR_CPU_IER_SET);
+		writel_relaxed(lic->cop_iep[i],
+			       ictlr + ICTLR_COP_IEP_CLASS);
+		writel_relaxed(~0ul, ictlr + ICTLR_COP_IER_CLR);
+		writel_relaxed(lic->cop_ier[i],
+			       ictlr + ICTLR_COP_IER_SET);
+	}
+	local_irq_restore(flags);
+}
+
+static struct syscore_ops tegra_ictlr_syscore_ops = {
+	.suspend	= tegra_ictlr_suspend,
+	.resume		= tegra_ictlr_resume,
+};
+
+static void tegra_ictlr_syscore_init(void)
+{
+	register_syscore_ops(&tegra_ictlr_syscore_ops);
+}
+#else
+#define tegra_set_wake	NULL
+static inline void tegra_ictlr_syscore_init(void) {}
+#endif
+
+static struct irq_chip tegra_ictlr_chip = {
+	.name			= "LIC",
+	.irq_eoi		= tegra_eoi,
+	.irq_mask		= tegra_mask,
+	.irq_unmask		= tegra_unmask,
+	.irq_retrigger		= tegra_retrigger,
+	.irq_set_wake		= tegra_set_wake,
+	.flags			= IRQCHIP_MASK_ON_SUSPEND,
+#ifdef CONFIG_SMP
+	.irq_set_affinity	= irq_chip_set_affinity_parent,
+#endif
+};
+
+static int tegra_ictlr_domain_xlate(struct irq_domain *domain,
+				    struct device_node *controller,
+				    const u32 *intspec,
+				    unsigned int intsize,
+				    unsigned long *out_hwirq,
+				    unsigned int *out_type)
+{
+	if (domain->of_node != controller)
+		return -EINVAL;	/* Shouldn't happen, really... */
+	if (intsize != 3)
+		return -EINVAL;	/* Not GIC compliant */
+	if (intspec[0] != GIC_SPI)
+		return -EINVAL;	/* No PPI should point to this domain */
+
+	*out_hwirq = intspec[1];
+	*out_type = intspec[2];
+	return 0;
+}
+
+static int tegra_ictlr_domain_alloc(struct irq_domain *domain,
+				    unsigned int virq,
+				    unsigned int nr_irqs, void *data)
+{
+	struct of_phandle_args *args = data;
+	struct of_phandle_args parent_args;
+	struct tegra_ictlr_info *info = domain->host_data;
+	irq_hw_number_t hwirq;
+	unsigned int i;
+
+	if (args->args_count != 3)
+		return -EINVAL;	/* Not GIC compliant */
+	if (args->args[0] != GIC_SPI)
+		return -EINVAL;	/* No PPI should point to this domain */
+
+	hwirq = args->args[1];
+	if (hwirq >= (num_ictlrs * 32))
+		return -EINVAL;
+
+	for (i = 0; i < nr_irqs; i++) {
+		int ictlr = (hwirq + i) / 32;
+
+		irq_domain_set_hwirq_and_chip(domain, virq + i, hwirq + i,
+					      &tegra_ictlr_chip,
+					      &info->base[ictlr]);
+	}
+
+	parent_args = *args;
+	parent_args.np = domain->parent->of_node;
+	return irq_domain_alloc_irqs_parent(domain, virq, nr_irqs, &parent_args);
+}
+
+static void tegra_ictlr_domain_free(struct irq_domain *domain,
+				    unsigned int virq,
+				    unsigned int nr_irqs)
+{
+	unsigned int i;
+
+	for (i = 0; i < nr_irqs; i++) {
+		struct irq_data *d = irq_domain_get_irq_data(domain, virq + i);
+		irq_domain_reset_irq_data(d);
+	}
+}
+
+static const struct irq_domain_ops tegra_ictlr_domain_ops = {
+	.xlate	= tegra_ictlr_domain_xlate,
+	.alloc	= tegra_ictlr_domain_alloc,
+	.free	= tegra_ictlr_domain_free,
+};
+
+static int __init tegra_ictlr_init(struct device_node *node,
+				   struct device_node *parent)
+{
+	struct irq_domain *parent_domain, *domain;
+	const struct of_device_id *match;
+	const struct tegra_ictlr_soc *soc;
+	unsigned int i;
+	int err;
+
+	if (!parent) {
+		pr_err("%s: no parent, giving up\n", node->full_name);
+		return -ENODEV;
+	}
+
+	parent_domain = irq_find_host(parent);
+	if (!parent_domain) {
+		pr_err("%s: unable to obtain parent domain\n", node->full_name);
+		return -ENXIO;
+	}
+
+	match = of_match_node(ictlr_matches, node);
+	if (!match)		/* Should never happen... */
+		return -ENODEV;
+
+	soc = match->data;
+
+	lic = kzalloc(sizeof(*lic), GFP_KERNEL);
+	if (!lic)
+		return -ENOMEM;
+
+	for (i = 0; i < TEGRA_MAX_NUM_ICTLRS; i++) {
+		void __iomem *base;
+
+		base = of_iomap(node, i);
+		if (!base)
+			break;
+
+		lic->base[i] = base;
+
+		/* Disable all interrupts */
+		writel_relaxed(~0UL, base + ICTLR_CPU_IER_CLR);
+		/* All interrupts target IRQ */
+		writel_relaxed(0, base + ICTLR_CPU_IEP_CLASS);
+
+		num_ictlrs++;
+	}
+
+	if (!num_ictlrs) {
+		pr_err("%s: no valid regions, giving up\n", node->full_name);
+		err = -ENOMEM;
+		goto out_free;
+	}
+
+	WARN(num_ictlrs != soc->num_ictlrs,
+	     "%s: Found %u interrupt controllers in DT; expected %u.\n",
+	     node->full_name, num_ictlrs, soc->num_ictlrs);
+
+
+	domain = irq_domain_add_hierarchy(parent_domain, 0, num_ictlrs * 32,
+					  node, &tegra_ictlr_domain_ops,
+					  lic);
+	if (!domain) {
+		pr_err("%s: failed to allocated domain\n", node->full_name);
+		err = -ENOMEM;
+		goto out_unmap;
+	}
+
+	tegra_ictlr_syscore_init();
+
+	pr_info("%s: %d interrupts forwarded to %s\n",
+		node->full_name, num_ictlrs * 32, parent->full_name);
+
+	return 0;
+
+out_unmap:
+	for (i = 0; i < num_ictlrs; i++)
+		iounmap(lic->base[i]);
+out_free:
+	kfree(lic);
+	return err;
+}
+
+IRQCHIP_DECLARE(tegra20_ictlr, "nvidia,tegra20-ictlr", tegra_ictlr_init);
+IRQCHIP_DECLARE(tegra30_ictlr, "nvidia,tegra30-ictlr", tegra_ictlr_init);
+IRQCHIP_DECLARE(tegra210_ictlr, "nvidia,tegra210-ictlr", tegra_ictlr_init);
diff --git a/drivers/irqchip/irq-vf610-mscm-ir.c b/drivers/irqchip/irq-vf610-mscm-ir.c
new file mode 100644
index 000000000000..9521057d4744
--- /dev/null
+++ b/drivers/irqchip/irq-vf610-mscm-ir.c
@@ -0,0 +1,212 @@
+/*
+ * Copyright (C) 2014-2015 Toradex AG
+ * Author: Stefan Agner <stefan@agner.ch>
+ *
+ * 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.
+ *
+ *
+ * IRQ chip driver for MSCM interrupt router available on Vybrid SoC's.
+ * The interrupt router is between the CPU's interrupt controller and the
+ * peripheral. The router allows to route the peripheral interrupts to
+ * one of the two available CPU's on Vybrid VF6xx SoC's (Cortex-A5 or
+ * Cortex-M4). The router will be configured transparently on a IRQ
+ * request.
+ *
+ * o All peripheral interrupts of the Vybrid SoC can be routed to
+ *   CPU 0, CPU 1 or both. The routing is useful for dual-core
+ *   variants of Vybrid SoC such as VF6xx. This driver routes the
+ *   requested interrupt to the CPU currently running on.
+ *
+ * o It is required to setup the interrupt router even on single-core
+ *   variants of Vybrid.
+ */
+
+#include <linux/cpu_pm.h>
+#include <linux/io.h>
+#include <linux/irq.h>
+#include <linux/irqdomain.h>
+#include <linux/mfd/syscon.h>
+#include <dt-bindings/interrupt-controller/arm-gic.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/slab.h>
+#include <linux/regmap.h>
+
+#include "irqchip.h"
+
+#define MSCM_CPxNUM		0x4
+
+#define MSCM_IRSPRC(n)		(0x80 + 2 * (n))
+#define MSCM_IRSPRC_CPEN_MASK	0x3
+
+#define MSCM_IRSPRC_NUM		112
+
+struct vf610_mscm_ir_chip_data {
+	void __iomem *mscm_ir_base;
+	u16 cpu_mask;
+	u16 saved_irsprc[MSCM_IRSPRC_NUM];
+};
+
+static struct vf610_mscm_ir_chip_data *mscm_ir_data;
+
+static inline void vf610_mscm_ir_save(struct vf610_mscm_ir_chip_data *data)
+{
+	int i;
+
+	for (i = 0; i < MSCM_IRSPRC_NUM; i++)
+		data->saved_irsprc[i] = readw_relaxed(data->mscm_ir_base + MSCM_IRSPRC(i));
+}
+
+static inline void vf610_mscm_ir_restore(struct vf610_mscm_ir_chip_data *data)
+{
+	int i;
+
+	for (i = 0; i < MSCM_IRSPRC_NUM; i++)
+		writew_relaxed(data->saved_irsprc[i], data->mscm_ir_base + MSCM_IRSPRC(i));
+}
+
+static int vf610_mscm_ir_notifier(struct notifier_block *self,
+				  unsigned long cmd, void *v)
+{
+	switch (cmd) {
+	case CPU_CLUSTER_PM_ENTER:
+		vf610_mscm_ir_save(mscm_ir_data);
+		break;
+	case CPU_CLUSTER_PM_ENTER_FAILED:
+	case CPU_CLUSTER_PM_EXIT:
+		vf610_mscm_ir_restore(mscm_ir_data);
+		break;
+	}
+
+	return NOTIFY_OK;
+}
+
+static struct notifier_block mscm_ir_notifier_block = {
+	.notifier_call = vf610_mscm_ir_notifier,
+};
+
+static void vf610_mscm_ir_enable(struct irq_data *data)
+{
+	irq_hw_number_t hwirq = data->hwirq;
+	struct vf610_mscm_ir_chip_data *chip_data = data->chip_data;
+	u16 irsprc;
+
+	irsprc = readw_relaxed(chip_data->mscm_ir_base + MSCM_IRSPRC(hwirq));
+	irsprc &= MSCM_IRSPRC_CPEN_MASK;
+
+	WARN_ON(irsprc & ~chip_data->cpu_mask);
+
+	writew_relaxed(chip_data->cpu_mask,
+		       chip_data->mscm_ir_base + MSCM_IRSPRC(hwirq));
+
+	irq_chip_unmask_parent(data);
+}
+
+static void vf610_mscm_ir_disable(struct irq_data *data)
+{
+	irq_hw_number_t hwirq = data->hwirq;
+	struct vf610_mscm_ir_chip_data *chip_data = data->chip_data;
+
+	writew_relaxed(0x0, chip_data->mscm_ir_base + MSCM_IRSPRC(hwirq));
+
+	irq_chip_mask_parent(data);
+}
+
+static struct irq_chip vf610_mscm_ir_irq_chip = {
+	.name			= "mscm-ir",
+	.irq_mask		= irq_chip_mask_parent,
+	.irq_unmask		= irq_chip_unmask_parent,
+	.irq_eoi		= irq_chip_eoi_parent,
+	.irq_enable		= vf610_mscm_ir_enable,
+	.irq_disable		= vf610_mscm_ir_disable,
+	.irq_retrigger		= irq_chip_retrigger_hierarchy,
+	.irq_set_affinity	= irq_chip_set_affinity_parent,
+};
+
+static int vf610_mscm_ir_domain_alloc(struct irq_domain *domain, unsigned int virq,
+				      unsigned int nr_irqs, void *arg)
+{
+	int i;
+	irq_hw_number_t hwirq;
+	struct of_phandle_args *irq_data = arg;
+	struct of_phandle_args gic_data;
+
+	if (irq_data->args_count != 2)
+		return -EINVAL;
+
+	hwirq = irq_data->args[0];
+	for (i = 0; i < nr_irqs; i++)
+		irq_domain_set_hwirq_and_chip(domain, virq + i, hwirq + i,
+					      &vf610_mscm_ir_irq_chip,
+					      domain->host_data);
+
+	gic_data.np = domain->parent->of_node;
+	gic_data.args_count = 3;
+	gic_data.args[0] = GIC_SPI;
+	gic_data.args[1] = irq_data->args[0];
+	gic_data.args[2] = irq_data->args[1];
+	return irq_domain_alloc_irqs_parent(domain, virq, nr_irqs, &gic_data);
+}
+
+static const struct irq_domain_ops mscm_irq_domain_ops = {
+	.xlate = irq_domain_xlate_twocell,
+	.alloc = vf610_mscm_ir_domain_alloc,
+	.free = irq_domain_free_irqs_common,
+};
+
+static int __init vf610_mscm_ir_of_init(struct device_node *node,
+			       struct device_node *parent)
+{
+	struct irq_domain *domain, *domain_parent;
+	struct regmap *mscm_cp_regmap;
+	int ret, cpuid;
+
+	domain_parent = irq_find_host(parent);
+	if (!domain_parent) {
+		pr_err("vf610_mscm_ir: interrupt-parent not found\n");
+		return -EINVAL;
+	}
+
+	mscm_ir_data = kzalloc(sizeof(*mscm_ir_data), GFP_KERNEL);
+	if (!mscm_ir_data)
+		return -ENOMEM;
+
+	mscm_ir_data->mscm_ir_base = of_io_request_and_map(node, 0, "mscm-ir");
+
+	if (!mscm_ir_data->mscm_ir_base) {
+		pr_err("vf610_mscm_ir: unable to map mscm register\n");
+		ret = -ENOMEM;
+		goto out_free;
+	}
+
+	mscm_cp_regmap = syscon_regmap_lookup_by_phandle(node, "fsl,cpucfg");
+	if (IS_ERR(mscm_cp_regmap)) {
+		ret = PTR_ERR(mscm_cp_regmap);
+		pr_err("vf610_mscm_ir: regmap lookup for cpucfg failed\n");
+		goto out_unmap;
+	}
+
+	regmap_read(mscm_cp_regmap, MSCM_CPxNUM, &cpuid);
+	mscm_ir_data->cpu_mask = 0x1 << cpuid;
+
+	domain = irq_domain_add_hierarchy(domain_parent, 0,
+					  MSCM_IRSPRC_NUM, node,
+					  &mscm_irq_domain_ops, mscm_ir_data);
+	if (!domain) {
+		ret = -ENOMEM;
+		goto out_unmap;
+	}
+
+	cpu_pm_register_notifier(&mscm_ir_notifier_block);
+
+	return 0;
+
+out_unmap:
+	iounmap(mscm_ir_data->mscm_ir_base);
+out_free:
+	kfree(mscm_ir_data);
+	return ret;
+}
+IRQCHIP_DECLARE(vf610_mscm_ir, "fsl,vf610-mscm-ir", vf610_mscm_ir_of_init);
diff --git a/drivers/isdn/hardware/mISDN/hfcpci.c b/drivers/isdn/hardware/mISDN/hfcpci.c
index 3c92780bda09..ff48da61c94c 100644
--- a/drivers/isdn/hardware/mISDN/hfcpci.c
+++ b/drivers/isdn/hardware/mISDN/hfcpci.c
@@ -1755,7 +1755,7 @@ init_card(struct hfc_pci *hc)
 		enable_hwirq(hc);
 		spin_unlock_irqrestore(&hc->lock, flags);
 		/* Timeout 80ms */
-		current->state = TASK_UNINTERRUPTIBLE;
+		set_current_state(TASK_UNINTERRUPTIBLE);
 		schedule_timeout((80 * HZ) / 1000);
 		printk(KERN_INFO "HFC PCI: IRQ %d count %d\n",
 		       hc->irq, hc->irqcnt);
diff --git a/drivers/isdn/icn/icn.c b/drivers/isdn/icn/icn.c
index 6a7447c304ac..358a574d9e8b 100644
--- a/drivers/isdn/icn/icn.c
+++ b/drivers/isdn/icn/icn.c
@@ -1609,7 +1609,7 @@ icn_setup(char *line)
 	if (ints[0] > 1)
 		membase = (unsigned long)ints[2];
 	if (str && *str) {
-		strcpy(sid, str);
+		strlcpy(sid, str, sizeof(sid));
 		icn_id = sid;
 		if ((p = strchr(sid, ','))) {
 			*p++ = 0;
diff --git a/drivers/lguest/Kconfig b/drivers/lguest/Kconfig
index ee035ec4526b..169172d2ba05 100644
--- a/drivers/lguest/Kconfig
+++ b/drivers/lguest/Kconfig
@@ -1,6 +1,6 @@
 config LGUEST
 	tristate "Linux hypervisor example code"
-	depends on X86_32 && EVENTFD && TTY
+	depends on X86_32 && EVENTFD && TTY && PCI_DIRECT
 	select HVC_DRIVER
 	---help---
 	  This is a very simple module which allows you to run
diff --git a/drivers/md/dm-io.c b/drivers/md/dm-io.c
index 37de0173b6d2..74adcd2c967e 100644
--- a/drivers/md/dm-io.c
+++ b/drivers/md/dm-io.c
@@ -289,9 +289,16 @@ static void do_region(int rw, unsigned region, struct dm_io_region *where,
 	struct request_queue *q = bdev_get_queue(where->bdev);
 	unsigned short logical_block_size = queue_logical_block_size(q);
 	sector_t num_sectors;
+	unsigned int uninitialized_var(special_cmd_max_sectors);
 
-	/* Reject unsupported discard requests */
-	if ((rw & REQ_DISCARD) && !blk_queue_discard(q)) {
+	/*
+	 * Reject unsupported discard and write same requests.
+	 */
+	if (rw & REQ_DISCARD)
+		special_cmd_max_sectors = q->limits.max_discard_sectors;
+	else if (rw & REQ_WRITE_SAME)
+		special_cmd_max_sectors = q->limits.max_write_same_sectors;
+	if ((rw & (REQ_DISCARD | REQ_WRITE_SAME)) && special_cmd_max_sectors == 0) {
 		dec_count(io, region, -EOPNOTSUPP);
 		return;
 	}
@@ -317,7 +324,7 @@ static void do_region(int rw, unsigned region, struct dm_io_region *where,
 		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);
+			num_sectors = min_t(sector_t, special_cmd_max_sectors, remaining);
 			bio->bi_iter.bi_size = num_sectors << SECTOR_SHIFT;
 			remaining -= num_sectors;
 		} else if (rw & REQ_WRITE_SAME) {
@@ -326,7 +333,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);
+			num_sectors = min_t(sector_t, special_cmd_max_sectors, remaining);
 			bio->bi_iter.bi_size = num_sectors << SECTOR_SHIFT;
 
 			offset = 0;
diff --git a/drivers/md/dm-snap.c b/drivers/md/dm-snap.c
index 8b204ae216ab..f83a0f3fc365 100644
--- a/drivers/md/dm-snap.c
+++ b/drivers/md/dm-snap.c
@@ -20,6 +20,8 @@
 #include <linux/log2.h>
 #include <linux/dm-kcopyd.h>
 
+#include "dm.h"
+
 #include "dm-exception-store.h"
 
 #define DM_MSG_PREFIX "snapshots"
@@ -291,12 +293,23 @@ struct origin {
 };
 
 /*
+ * This structure is allocated for each origin target
+ */
+struct dm_origin {
+	struct dm_dev *dev;
+	struct dm_target *ti;
+	unsigned split_boundary;
+	struct list_head hash_list;
+};
+
+/*
  * Size of the hash table for origin volumes. If we make this
  * the size of the minors list then it should be nearly perfect
  */
 #define ORIGIN_HASH_SIZE 256
 #define ORIGIN_MASK      0xFF
 static struct list_head *_origins;
+static struct list_head *_dm_origins;
 static struct rw_semaphore _origins_lock;
 
 static DECLARE_WAIT_QUEUE_HEAD(_pending_exceptions_done);
@@ -310,12 +323,22 @@ static int init_origin_hash(void)
 	_origins = kmalloc(ORIGIN_HASH_SIZE * sizeof(struct list_head),
 			   GFP_KERNEL);
 	if (!_origins) {
-		DMERR("unable to allocate memory");
+		DMERR("unable to allocate memory for _origins");
 		return -ENOMEM;
 	}
-
 	for (i = 0; i < ORIGIN_HASH_SIZE; i++)
 		INIT_LIST_HEAD(_origins + i);
+
+	_dm_origins = kmalloc(ORIGIN_HASH_SIZE * sizeof(struct list_head),
+			      GFP_KERNEL);
+	if (!_dm_origins) {
+		DMERR("unable to allocate memory for _dm_origins");
+		kfree(_origins);
+		return -ENOMEM;
+	}
+	for (i = 0; i < ORIGIN_HASH_SIZE; i++)
+		INIT_LIST_HEAD(_dm_origins + i);
+
 	init_rwsem(&_origins_lock);
 
 	return 0;
@@ -324,6 +347,7 @@ static int init_origin_hash(void)
 static void exit_origin_hash(void)
 {
 	kfree(_origins);
+	kfree(_dm_origins);
 }
 
 static unsigned origin_hash(struct block_device *bdev)
@@ -350,6 +374,30 @@ static void __insert_origin(struct origin *o)
 	list_add_tail(&o->hash_list, sl);
 }
 
+static struct dm_origin *__lookup_dm_origin(struct block_device *origin)
+{
+	struct list_head *ol;
+	struct dm_origin *o;
+
+	ol = &_dm_origins[origin_hash(origin)];
+	list_for_each_entry (o, ol, hash_list)
+		if (bdev_equal(o->dev->bdev, origin))
+			return o;
+
+	return NULL;
+}
+
+static void __insert_dm_origin(struct dm_origin *o)
+{
+	struct list_head *sl = &_dm_origins[origin_hash(o->dev->bdev)];
+	list_add_tail(&o->hash_list, sl);
+}
+
+static void __remove_dm_origin(struct dm_origin *o)
+{
+	list_del(&o->hash_list);
+}
+
 /*
  * _origins_lock must be held when calling this function.
  * Returns number of snapshots registered using the supplied cow device, plus:
@@ -1840,9 +1888,40 @@ static int snapshot_preresume(struct dm_target *ti)
 static void snapshot_resume(struct dm_target *ti)
 {
 	struct dm_snapshot *s = ti->private;
-	struct dm_snapshot *snap_src = NULL, *snap_dest = NULL;
+	struct dm_snapshot *snap_src = NULL, *snap_dest = NULL, *snap_merging = NULL;
+	struct dm_origin *o;
+	struct mapped_device *origin_md = NULL;
+	bool must_restart_merging = false;
 
 	down_read(&_origins_lock);
+
+	o = __lookup_dm_origin(s->origin->bdev);
+	if (o)
+		origin_md = dm_table_get_md(o->ti->table);
+	if (!origin_md) {
+		(void) __find_snapshots_sharing_cow(s, NULL, NULL, &snap_merging);
+		if (snap_merging)
+			origin_md = dm_table_get_md(snap_merging->ti->table);
+	}
+	if (origin_md == dm_table_get_md(ti->table))
+		origin_md = NULL;
+	if (origin_md) {
+		if (dm_hold(origin_md))
+			origin_md = NULL;
+	}
+
+	up_read(&_origins_lock);
+
+	if (origin_md) {
+		dm_internal_suspend_fast(origin_md);
+		if (snap_merging && test_bit(RUNNING_MERGE, &snap_merging->state_bits)) {
+			must_restart_merging = true;
+			stop_merge(snap_merging);
+		}
+	}
+
+	down_read(&_origins_lock);
+
 	(void) __find_snapshots_sharing_cow(s, &snap_src, &snap_dest, NULL);
 	if (snap_src && snap_dest) {
 		down_write(&snap_src->lock);
@@ -1851,8 +1930,16 @@ static void snapshot_resume(struct dm_target *ti)
 		up_write(&snap_dest->lock);
 		up_write(&snap_src->lock);
 	}
+
 	up_read(&_origins_lock);
 
+	if (origin_md) {
+		if (must_restart_merging)
+			start_merge(snap_merging);
+		dm_internal_resume_fast(origin_md);
+		dm_put(origin_md);
+	}
+
 	/* Now we have correct chunk size, reregister */
 	reregister_snapshot(s);
 
@@ -2133,11 +2220,6 @@ static int origin_write_extent(struct dm_snapshot *merging_snap,
  * Origin: maps a linear range of a device, with hooks for snapshotting.
  */
 
-struct dm_origin {
-	struct dm_dev *dev;
-	unsigned split_boundary;
-};
-
 /*
  * Construct an origin mapping: <dev_path>
  * The context for an origin is merely a 'struct dm_dev *'
@@ -2166,6 +2248,7 @@ static int origin_ctr(struct dm_target *ti, unsigned int argc, char **argv)
 		goto bad_open;
 	}
 
+	o->ti = ti;
 	ti->private = o;
 	ti->num_flush_bios = 1;
 
@@ -2180,6 +2263,7 @@ bad_alloc:
 static void origin_dtr(struct dm_target *ti)
 {
 	struct dm_origin *o = ti->private;
+
 	dm_put_device(ti, o->dev);
 	kfree(o);
 }
@@ -2216,6 +2300,19 @@ static void origin_resume(struct dm_target *ti)
 	struct dm_origin *o = ti->private;
 
 	o->split_boundary = get_origin_minimum_chunksize(o->dev->bdev);
+
+	down_write(&_origins_lock);
+	__insert_dm_origin(o);
+	up_write(&_origins_lock);
+}
+
+static void origin_postsuspend(struct dm_target *ti)
+{
+	struct dm_origin *o = ti->private;
+
+	down_write(&_origins_lock);
+	__remove_dm_origin(o);
+	up_write(&_origins_lock);
 }
 
 static void origin_status(struct dm_target *ti, status_type_t type,
@@ -2258,12 +2355,13 @@ static int origin_iterate_devices(struct dm_target *ti,
 
 static struct target_type origin_target = {
 	.name    = "snapshot-origin",
-	.version = {1, 8, 1},
+	.version = {1, 9, 0},
 	.module  = THIS_MODULE,
 	.ctr     = origin_ctr,
 	.dtr     = origin_dtr,
 	.map     = origin_map,
 	.resume  = origin_resume,
+	.postsuspend = origin_postsuspend,
 	.status  = origin_status,
 	.merge	 = origin_merge,
 	.iterate_devices = origin_iterate_devices,
@@ -2271,7 +2369,7 @@ static struct target_type origin_target = {
 
 static struct target_type snapshot_target = {
 	.name    = "snapshot",
-	.version = {1, 12, 0},
+	.version = {1, 13, 0},
 	.module  = THIS_MODULE,
 	.ctr     = snapshot_ctr,
 	.dtr     = snapshot_dtr,
@@ -2285,7 +2383,7 @@ static struct target_type snapshot_target = {
 
 static struct target_type merge_target = {
 	.name    = dm_snapshot_merge_target_name,
-	.version = {1, 2, 0},
+	.version = {1, 3, 0},
 	.module  = THIS_MODULE,
 	.ctr     = snapshot_ctr,
 	.dtr     = snapshot_dtr,
diff --git a/drivers/md/dm-thin.c b/drivers/md/dm-thin.c
index 654773cb1eee..921aafd12aee 100644
--- a/drivers/md/dm-thin.c
+++ b/drivers/md/dm-thin.c
@@ -2358,17 +2358,6 @@ static int thin_bio_map(struct dm_target *ti, struct bio *bio)
 		return DM_MAPIO_REMAPPED;
 
 	case -ENODATA:
-		if (get_pool_mode(tc->pool) == PM_READ_ONLY) {
-			/*
-			 * This block isn't provisioned, and we have no way
-			 * of doing so.
-			 */
-			handle_unserviceable_bio(tc->pool, bio);
-			cell_defer_no_holder(tc, virt_cell);
-			return DM_MAPIO_SUBMITTED;
-		}
-		/* fall through */
-
 	case -EWOULDBLOCK:
 		thin_defer_cell(tc, virt_cell);
 		return DM_MAPIO_SUBMITTED;
diff --git a/drivers/md/dm.c b/drivers/md/dm.c
index 73f28802dc7a..8001fe9e3434 100644
--- a/drivers/md/dm.c
+++ b/drivers/md/dm.c
@@ -433,7 +433,6 @@ static int dm_blk_open(struct block_device *bdev, fmode_t mode)
 
 	dm_get(md);
 	atomic_inc(&md->open_count);
-
 out:
 	spin_unlock(&_minor_lock);
 
@@ -442,16 +441,20 @@ out:
 
 static void dm_blk_close(struct gendisk *disk, fmode_t mode)
 {
-	struct mapped_device *md = disk->private_data;
+	struct mapped_device *md;
 
 	spin_lock(&_minor_lock);
 
+	md = disk->private_data;
+	if (WARN_ON(!md))
+		goto out;
+
 	if (atomic_dec_and_test(&md->open_count) &&
 	    (test_bit(DMF_DEFERRED_REMOVE, &md->flags)))
 		queue_work(deferred_remove_workqueue, &deferred_remove_work);
 
 	dm_put(md);
-
+out:
 	spin_unlock(&_minor_lock);
 }
 
@@ -2241,7 +2244,6 @@ static void free_dev(struct mapped_device *md)
 	int minor = MINOR(disk_devt(md->disk));
 
 	unlock_fs(md);
-	bdput(md->bdev);
 	destroy_workqueue(md->wq);
 
 	if (md->kworker_task)
@@ -2252,19 +2254,22 @@ static void free_dev(struct mapped_device *md)
 		mempool_destroy(md->rq_pool);
 	if (md->bs)
 		bioset_free(md->bs);
-	blk_integrity_unregister(md->disk);
-	del_gendisk(md->disk);
+
 	cleanup_srcu_struct(&md->io_barrier);
 	free_table_devices(&md->table_devices);
-	free_minor(minor);
+	dm_stats_cleanup(&md->stats);
 
 	spin_lock(&_minor_lock);
 	md->disk->private_data = NULL;
 	spin_unlock(&_minor_lock);
-
+	if (blk_get_integrity(md->disk))
+		blk_integrity_unregister(md->disk);
+	del_gendisk(md->disk);
 	put_disk(md->disk);
 	blk_cleanup_queue(md->queue);
-	dm_stats_cleanup(&md->stats);
+	bdput(md->bdev);
+	free_minor(minor);
+
 	module_put(THIS_MODULE);
 	kfree(md);
 }
@@ -2616,6 +2621,19 @@ void dm_get(struct mapped_device *md)
 	BUG_ON(test_bit(DMF_FREEING, &md->flags));
 }
 
+int dm_hold(struct mapped_device *md)
+{
+	spin_lock(&_minor_lock);
+	if (test_bit(DMF_FREEING, &md->flags)) {
+		spin_unlock(&_minor_lock);
+		return -EBUSY;
+	}
+	dm_get(md);
+	spin_unlock(&_minor_lock);
+	return 0;
+}
+EXPORT_SYMBOL_GPL(dm_hold);
+
 const char *dm_device_name(struct mapped_device *md)
 {
 	return md->name;
@@ -2629,8 +2647,9 @@ static void __dm_destroy(struct mapped_device *md, bool wait)
 
 	might_sleep();
 
-	spin_lock(&_minor_lock);
 	map = dm_get_live_table(md, &srcu_idx);
+
+	spin_lock(&_minor_lock);
 	idr_replace(&_minor_idr, MINOR_ALLOCED, MINOR(disk_devt(dm_disk(md))));
 	set_bit(DMF_FREEING, &md->flags);
 	spin_unlock(&_minor_lock);
@@ -2638,10 +2657,16 @@ static void __dm_destroy(struct mapped_device *md, bool wait)
 	if (dm_request_based(md))
 		flush_kthread_worker(&md->kworker);
 
+	/*
+	 * Take suspend_lock so that presuspend and postsuspend methods
+	 * do not race with internal suspend.
+	 */
+	mutex_lock(&md->suspend_lock);
 	if (!dm_suspended_md(md)) {
 		dm_table_presuspend_targets(map);
 		dm_table_postsuspend_targets(map);
 	}
+	mutex_unlock(&md->suspend_lock);
 
 	/* dm_put_live_table must be before msleep, otherwise deadlock is possible */
 	dm_put_live_table(md, srcu_idx);
@@ -3115,6 +3140,7 @@ void dm_internal_suspend_fast(struct mapped_device *md)
 	flush_workqueue(md->wq);
 	dm_wait_for_completion(md, TASK_UNINTERRUPTIBLE);
 }
+EXPORT_SYMBOL_GPL(dm_internal_suspend_fast);
 
 void dm_internal_resume_fast(struct mapped_device *md)
 {
@@ -3126,6 +3152,7 @@ void dm_internal_resume_fast(struct mapped_device *md)
 done:
 	mutex_unlock(&md->suspend_lock);
 }
+EXPORT_SYMBOL_GPL(dm_internal_resume_fast);
 
 /*-----------------------------------------------------------------
  * Event notification.
diff --git a/drivers/md/md.c b/drivers/md/md.c
index c8d2bac4e28b..e6178787ce3d 100644
--- a/drivers/md/md.c
+++ b/drivers/md/md.c
@@ -249,6 +249,7 @@ static void md_make_request(struct request_queue *q, struct bio *bio)
 	const int rw = bio_data_dir(bio);
 	struct mddev *mddev = q->queuedata;
 	unsigned int sectors;
+	int cpu;
 
 	if (mddev == NULL || mddev->pers == NULL
 	    || !mddev->ready) {
@@ -284,7 +285,10 @@ static void md_make_request(struct request_queue *q, struct bio *bio)
 	sectors = bio_sectors(bio);
 	mddev->pers->make_request(mddev, bio);
 
-	generic_start_io_acct(rw, sectors, &mddev->gendisk->part0);
+	cpu = part_stat_lock();
+	part_stat_inc(cpu, &mddev->gendisk->part0, ios[rw]);
+	part_stat_add(cpu, &mddev->gendisk->part0, sectors[rw], sectors);
+	part_stat_unlock();
 
 	if (atomic_dec_and_test(&mddev->active_io) && mddev->suspended)
 		wake_up(&mddev->sb_wait);
@@ -2555,7 +2559,7 @@ state_store(struct md_rdev *rdev, const char *buf, size_t len)
 	return err ? err : len;
 }
 static struct rdev_sysfs_entry rdev_state =
-__ATTR(state, S_IRUGO|S_IWUSR, state_show, state_store);
+__ATTR_PREALLOC(state, S_IRUGO|S_IWUSR, state_show, state_store);
 
 static ssize_t
 errors_show(struct md_rdev *rdev, char *page)
@@ -3638,7 +3642,8 @@ resync_start_store(struct mddev *mddev, const char *buf, size_t len)
 	return err ?: len;
 }
 static struct md_sysfs_entry md_resync_start =
-__ATTR(resync_start, S_IRUGO|S_IWUSR, resync_start_show, resync_start_store);
+__ATTR_PREALLOC(resync_start, S_IRUGO|S_IWUSR,
+		resync_start_show, resync_start_store);
 
 /*
  * The array state can be:
@@ -3851,7 +3856,7 @@ array_state_store(struct mddev *mddev, const char *buf, size_t len)
 	return err ?: len;
 }
 static struct md_sysfs_entry md_array_state =
-__ATTR(array_state, S_IRUGO|S_IWUSR, array_state_show, array_state_store);
+__ATTR_PREALLOC(array_state, S_IRUGO|S_IWUSR, array_state_show, array_state_store);
 
 static ssize_t
 max_corrected_read_errors_show(struct mddev *mddev, char *page) {
@@ -4101,7 +4106,7 @@ out_unlock:
 }
 
 static struct md_sysfs_entry md_metadata =
-__ATTR(metadata_version, S_IRUGO|S_IWUSR, metadata_show, metadata_store);
+__ATTR_PREALLOC(metadata_version, S_IRUGO|S_IWUSR, metadata_show, metadata_store);
 
 static ssize_t
 action_show(struct mddev *mddev, char *page)
@@ -4189,7 +4194,7 @@ action_store(struct mddev *mddev, const char *page, size_t len)
 }
 
 static struct md_sysfs_entry md_scan_mode =
-__ATTR(sync_action, S_IRUGO|S_IWUSR, action_show, action_store);
+__ATTR_PREALLOC(sync_action, S_IRUGO|S_IWUSR, action_show, action_store);
 
 static ssize_t
 last_sync_action_show(struct mddev *mddev, char *page)
@@ -4335,7 +4340,8 @@ sync_completed_show(struct mddev *mddev, char *page)
 	return sprintf(page, "%llu / %llu\n", resync, max_sectors);
 }
 
-static struct md_sysfs_entry md_sync_completed = __ATTR_RO(sync_completed);
+static struct md_sysfs_entry md_sync_completed =
+	__ATTR_PREALLOC(sync_completed, S_IRUGO, sync_completed_show, NULL);
 
 static ssize_t
 min_sync_show(struct mddev *mddev, char *page)
@@ -5078,7 +5084,8 @@ int md_run(struct mddev *mddev)
 	}
 	if (err) {
 		mddev_detach(mddev);
-		pers->free(mddev, mddev->private);
+		if (mddev->private)
+			pers->free(mddev, mddev->private);
 		module_put(pers->owner);
 		bitmap_destroy(mddev);
 		return err;
diff --git a/drivers/md/raid0.c b/drivers/md/raid0.c
index a13f738a7b39..3b5d7f704aa3 100644
--- a/drivers/md/raid0.c
+++ b/drivers/md/raid0.c
@@ -313,7 +313,7 @@ static struct strip_zone *find_zone(struct r0conf *conf,
 
 /*
  * remaps the bio to the target device. we separate two flows.
- * power 2 flow and a general flow for the sake of perfromance
+ * power 2 flow and a general flow for the sake of performance
 */
 static struct md_rdev *map_sector(struct mddev *mddev, struct strip_zone *zone,
 				sector_t sector, sector_t *sector_offset)
@@ -467,8 +467,6 @@ static int raid0_run(struct mddev *mddev)
 	dump_zones(mddev);
 
 	ret = md_integrity_register(mddev);
-	if (ret)
-		raid0_free(mddev, conf);
 
 	return ret;
 }
@@ -526,6 +524,7 @@ static void raid0_make_request(struct mddev *mddev, struct bio *bio)
 			split = bio;
 		}
 
+		sector = bio->bi_iter.bi_sector;
 		zone = find_zone(mddev->private, &sector);
 		tmp_dev = map_sector(mddev, zone, sector, &sector);
 		split->bi_bdev = tmp_dev->bdev;
diff --git a/drivers/md/raid1.c b/drivers/md/raid1.c
index 4153da5d4011..d34e238afa54 100644
--- a/drivers/md/raid1.c
+++ b/drivers/md/raid1.c
@@ -560,7 +560,7 @@ static int read_balance(struct r1conf *conf, struct r1bio *r1_bio, int *max_sect
 		if (test_bit(WriteMostly, &rdev->flags)) {
 			/* Don't balance among write-mostly, just
 			 * use the first as a last resort */
-			if (best_disk < 0) {
+			if (best_dist_disk < 0) {
 				if (is_badblock(rdev, this_sector, sectors,
 						&first_bad, &bad_sectors)) {
 					if (first_bad < this_sector)
@@ -569,7 +569,8 @@ static int read_balance(struct r1conf *conf, struct r1bio *r1_bio, int *max_sect
 					best_good_sectors = first_bad - this_sector;
 				} else
 					best_good_sectors = sectors;
-				best_disk = disk;
+				best_dist_disk = disk;
+				best_pending_disk = disk;
 			}
 			continue;
 		}
diff --git a/drivers/md/raid5.c b/drivers/md/raid5.c
index e75d48c0421a..cd2f96b2c572 100644
--- a/drivers/md/raid5.c
+++ b/drivers/md/raid5.c
@@ -5121,12 +5121,17 @@ static inline sector_t sync_request(struct mddev *mddev, sector_t sector_nr, int
 		schedule_timeout_uninterruptible(1);
 	}
 	/* Need to check if array will still be degraded after recovery/resync
-	 * We don't need to check the 'failed' flag as when that gets set,
-	 * recovery aborts.
+	 * Note in case of > 1 drive failures it's possible we're rebuilding
+	 * one drive while leaving another faulty drive in array.
 	 */
-	for (i = 0; i < conf->raid_disks; i++)
-		if (conf->disks[i].rdev == NULL)
+	rcu_read_lock();
+	for (i = 0; i < conf->raid_disks; i++) {
+		struct md_rdev *rdev = ACCESS_ONCE(conf->disks[i].rdev);
+
+		if (rdev == NULL || test_bit(Faulty, &rdev->flags))
 			still_degraded = 1;
+	}
+	rcu_read_unlock();
 
 	bitmap_start_sync(mddev->bitmap, sector_nr, &sync_blocks, still_degraded);
 
diff --git a/drivers/media/dvb-frontends/rtl2832.c b/drivers/media/dvb-frontends/rtl2832.c
index 5d2d8f45b4b6..67faa8d6950e 100644
--- a/drivers/media/dvb-frontends/rtl2832.c
+++ b/drivers/media/dvb-frontends/rtl2832.c
@@ -1240,7 +1240,7 @@ static int rtl2832_probe(struct i2c_client *client,
 	dev->regmap_config.max_register = 5 * 0x100,
 	dev->regmap_config.ranges = regmap_range_cfg,
 	dev->regmap_config.num_ranges = ARRAY_SIZE(regmap_range_cfg),
-	dev->regmap_config.cache_type = REGCACHE_RBTREE,
+	dev->regmap_config.cache_type = REGCACHE_NONE,
 	dev->regmap = regmap_init(&client->dev, &regmap_bus, client,
 				  &dev->regmap_config);
 	if (IS_ERR(dev->regmap)) {
diff --git a/drivers/media/pci/cx23885/cx23885-417.c b/drivers/media/pci/cx23885/cx23885-417.c
index e4901a503c73..63c0ee5d0bf5 100644
--- a/drivers/media/pci/cx23885/cx23885-417.c
+++ b/drivers/media/pci/cx23885/cx23885-417.c
@@ -1339,14 +1339,13 @@ static int vidioc_querycap(struct file *file, void  *priv,
 	strlcpy(cap->driver, dev->name, sizeof(cap->driver));
 	strlcpy(cap->card, cx23885_boards[tsport->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     |
-		0;
+	sprintf(cap->bus_info, "PCIe:%s", pci_name(dev->pci));
+	cap->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_READWRITE |
+			   V4L2_CAP_STREAMING;
 	if (dev->tuner_type != TUNER_ABSENT)
-		cap->capabilities |= V4L2_CAP_TUNER;
+		cap->device_caps |= V4L2_CAP_TUNER;
+	cap->capabilities = cap->device_caps | V4L2_CAP_VBI_CAPTURE |
+		V4L2_CAP_AUDIO | V4L2_CAP_DEVICE_CAPS;
 
 	return 0;
 }
diff --git a/drivers/media/platform/s5p-jpeg/jpeg-core.c b/drivers/media/platform/s5p-jpeg/jpeg-core.c
index 12f7452edce3..a92ff4249d10 100644
--- a/drivers/media/platform/s5p-jpeg/jpeg-core.c
+++ b/drivers/media/platform/s5p-jpeg/jpeg-core.c
@@ -1845,6 +1845,9 @@ static void exynos4_jpeg_set_img_addr(struct s5p_jpeg_ctx *ctx)
 	struct s5p_jpeg_addr jpeg_addr;
 	u32 pix_size, padding_bytes = 0;
 
+	jpeg_addr.cb = 0;
+	jpeg_addr.cr = 0;
+
 	pix_size = ctx->cap_q.w * ctx->cap_q.h;
 
 	if (ctx->mode == S5P_JPEG_ENCODE) {
diff --git a/drivers/media/platform/s5p-jpeg/jpeg-hw-exynos3250.c b/drivers/media/platform/s5p-jpeg/jpeg-hw-exynos3250.c
index e8c2cad93962..0974b9a7a584 100644
--- a/drivers/media/platform/s5p-jpeg/jpeg-hw-exynos3250.c
+++ b/drivers/media/platform/s5p-jpeg/jpeg-hw-exynos3250.c
@@ -20,7 +20,7 @@
 
 void exynos3250_jpeg_reset(void __iomem *regs)
 {
-	u32 reg = 0;
+	u32 reg = 1;
 	int count = 1000;
 
 	writel(1, regs + EXYNOS3250_SW_RESET);
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc.c b/drivers/media/platform/s5p-mfc/s5p_mfc.c
index 8e44a59d8ec2..98374e8bad3e 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc.c
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc.c
@@ -833,6 +833,7 @@ 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;
+	q->lock = &dev->mfc_mutex;
 	if (vdev == dev->vfd_dec) {
 		q->io_modes = VB2_MMAP;
 		q->ops = get_dec_queue_ops();
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_common.h b/drivers/media/platform/s5p-mfc/s5p_mfc_common.h
index 15f7663dd9f5..24262bbb1a35 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc_common.h
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_common.h
@@ -29,7 +29,7 @@
 
 /* Offset base used to differentiate between CAPTURE and OUTPUT
 *  while mmaping */
-#define DST_QUEUE_OFF_BASE      (TASK_SIZE / 2)
+#define DST_QUEUE_OFF_BASE	(1 << 30)
 
 #define MFC_BANK1_ALLOC_CTX	0
 #define MFC_BANK2_ALLOC_CTX	1
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_opr.h b/drivers/media/platform/s5p-mfc/s5p_mfc_opr.h
index de2b8c69daa5..22dfb3effda8 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc_opr.h
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_opr.h
@@ -302,7 +302,7 @@ struct s5p_mfc_hw_ops {
 	void (*write_info)(struct s5p_mfc_ctx *ctx, unsigned int data,
 			unsigned int ofs);
 	unsigned int (*read_info)(struct s5p_mfc_ctx *ctx,
-			unsigned int ofs);
+			unsigned long ofs);
 	int (*get_dspl_y_adr)(struct s5p_mfc_dev *dev);
 	int (*get_dec_y_adr)(struct s5p_mfc_dev *dev);
 	int (*get_dspl_status)(struct s5p_mfc_dev *dev);
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v5.c b/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v5.c
index 0c4fcf2dfd09..b09bcd140491 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v5.c
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v5.c
@@ -263,15 +263,15 @@ static void s5p_mfc_release_dev_context_buffer_v5(struct s5p_mfc_dev *dev)
 static void s5p_mfc_write_info_v5(struct s5p_mfc_ctx *ctx, unsigned int data,
 			unsigned int ofs)
 {
-	writel(data, (volatile void __iomem *)(ctx->shm.virt + ofs));
+	writel(data, (void *)(ctx->shm.virt + ofs));
 	wmb();
 }
 
 static unsigned int s5p_mfc_read_info_v5(struct s5p_mfc_ctx *ctx,
-				unsigned int ofs)
+				unsigned long ofs)
 {
 	rmb();
-	return readl((volatile void __iomem *)(ctx->shm.virt + ofs));
+	return readl((void *)(ctx->shm.virt + ofs));
 }
 
 static void s5p_mfc_dec_calc_dpb_size_v5(struct s5p_mfc_ctx *ctx)
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 d826c58b5d53..cefad184fe96 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v6.c
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v6.c
@@ -1852,17 +1852,17 @@ static void s5p_mfc_write_info_v6(struct s5p_mfc_ctx *ctx, unsigned int data,
 		unsigned int ofs)
 {
 	s5p_mfc_clock_on();
-	writel(data, (volatile void __iomem *)((unsigned long)ofs));
+	writel(data, (void *)((unsigned long)ofs));
 	s5p_mfc_clock_off();
 }
 
 static unsigned int
-s5p_mfc_read_info_v6(struct s5p_mfc_ctx *ctx, unsigned int ofs)
+s5p_mfc_read_info_v6(struct s5p_mfc_ctx *ctx, unsigned long ofs)
 {
 	int ret;
 
 	s5p_mfc_clock_on();
-	ret = readl((volatile void __iomem *)((unsigned long)ofs));
+	ret = readl((void *)ofs);
 	s5p_mfc_clock_off();
 
 	return ret;
diff --git a/drivers/media/platform/s5p-tv/Kconfig b/drivers/media/platform/s5p-tv/Kconfig
index 5a1835dd65e8..697aaed42486 100644
--- a/drivers/media/platform/s5p-tv/Kconfig
+++ b/drivers/media/platform/s5p-tv/Kconfig
@@ -20,6 +20,7 @@ if VIDEO_SAMSUNG_S5P_TV
 config VIDEO_SAMSUNG_S5P_HDMI
 	tristate "Samsung HDMI Driver"
 	depends on VIDEO_V4L2
+	depends on I2C
 	depends on VIDEO_SAMSUNG_S5P_TV
 	select VIDEO_SAMSUNG_S5P_HDMIPHY
 	help
diff --git a/drivers/media/platform/sh_veu.c b/drivers/media/platform/sh_veu.c
index a901b6248557..2554f3719b9e 100644
--- a/drivers/media/platform/sh_veu.c
+++ b/drivers/media/platform/sh_veu.c
@@ -1158,6 +1158,7 @@ static int sh_veu_probe(struct platform_device *pdev)
 	}
 
 	*vdev = sh_veu_videodev;
+	vdev->v4l2_dev = &veu->v4l2_dev;
 	spin_lock_init(&veu->lock);
 	mutex_init(&veu->fop_lock);
 	vdev->lock = &veu->fop_lock;
diff --git a/drivers/media/platform/soc_camera/atmel-isi.c b/drivers/media/platform/soc_camera/atmel-isi.c
index 8526bf5c8429..c835beb2a1a8 100644
--- a/drivers/media/platform/soc_camera/atmel-isi.c
+++ b/drivers/media/platform/soc_camera/atmel-isi.c
@@ -843,6 +843,8 @@ static int isi_camera_set_bus_param(struct soc_camera_device *icd)
 	if (isi->pdata.full_mode)
 		cfg1 |= ISI_CFG1_FULL_MODE;
 
+	cfg1 |= ISI_CFG1_THMASK_BEATS_16;
+
 	isi_writel(isi, ISI_CTRL, ISI_CTRL_DIS);
 	isi_writel(isi, ISI_CFG1, cfg1);
 
diff --git a/drivers/media/platform/soc_camera/soc_camera.c b/drivers/media/platform/soc_camera/soc_camera.c
index cee7b56f8404..66634b469c98 100644
--- a/drivers/media/platform/soc_camera/soc_camera.c
+++ b/drivers/media/platform/soc_camera/soc_camera.c
@@ -1665,7 +1665,7 @@ eclkreg:
 eaddpdev:
 	platform_device_put(sasc->pdev);
 eallocpdev:
-	devm_kfree(ici->v4l2_dev.dev, sasc);
+	devm_kfree(ici->v4l2_dev.dev, info);
 	dev_err(ici->v4l2_dev.dev, "group probe failed: %d\n", ret);
 
 	return ret;
diff --git a/drivers/media/usb/dvb-usb-v2/rtl28xxu.c b/drivers/media/usb/dvb-usb-v2/rtl28xxu.c
index 77dcfdf547ac..87fc0fe29ebd 100644
--- a/drivers/media/usb/dvb-usb-v2/rtl28xxu.c
+++ b/drivers/media/usb/dvb-usb-v2/rtl28xxu.c
@@ -780,8 +780,6 @@ static int rtl2832u_frontend_callback(void *adapter_priv, int component,
 		case TUNER_RTL2832_TUA9001:
 			return rtl2832u_tua9001_tuner_callback(d, cmd, arg);
 		}
-	default:
-		return -EINVAL;
 	}
 
 	return 0;
diff --git a/drivers/media/usb/gspca/Kconfig b/drivers/media/usb/gspca/Kconfig
index 60af3b167f3b..3fd94fe7e1eb 100644
--- a/drivers/media/usb/gspca/Kconfig
+++ b/drivers/media/usb/gspca/Kconfig
@@ -1,6 +1,7 @@
 menuconfig USB_GSPCA
 	tristate "GSPCA based webcams"
 	depends on VIDEO_V4L2
+	depends on INPUT || INPUT=n
 	default m
 	---help---
 	  Say Y here if you want to enable selecting webcams based
diff --git a/drivers/media/v4l2-core/videobuf2-core.c b/drivers/media/v4l2-core/videobuf2-core.c
index bc08a829bc13..cc16e76a2493 100644
--- a/drivers/media/v4l2-core/videobuf2-core.c
+++ b/drivers/media/v4l2-core/videobuf2-core.c
@@ -3230,18 +3230,13 @@ int vb2_thread_stop(struct vb2_queue *q)
 
 	if (threadio == NULL)
 		return 0;
-	call_void_qop(q, wait_finish, q);
 	threadio->stop = true;
-	vb2_internal_streamoff(q, q->type);
-	call_void_qop(q, wait_prepare, q);
+	/* Wake up all pending sleeps in the thread */
+	vb2_queue_error(q);
 	err = kthread_stop(threadio->thread);
-	q->fileio = NULL;
-	fileio->req.count = 0;
-	vb2_reqbufs(q, &fileio->req);
-	kfree(fileio);
+	__vb2_cleanup_fileio(q);
 	threadio->thread = NULL;
 	kfree(threadio);
-	q->fileio = NULL;
 	q->threadio = NULL;
 	return err;
 }
diff --git a/drivers/media/v4l2-core/videobuf2-dma-contig.c b/drivers/media/v4l2-core/videobuf2-dma-contig.c
index b481d20c8372..69e0483adfee 100644
--- a/drivers/media/v4l2-core/videobuf2-dma-contig.c
+++ b/drivers/media/v4l2-core/videobuf2-dma-contig.c
@@ -632,8 +632,7 @@ static void *vb2_dc_get_userptr(void *alloc_ctx, unsigned long vaddr,
 	}
 
 	/* extract page list from userspace mapping */
-	ret = vb2_dc_get_user_pages(start, pages, n_pages, vma,
-				    dma_dir == DMA_FROM_DEVICE);
+	ret = vb2_dc_get_user_pages(start, pages, n_pages, vma, dma_dir);
 	if (ret) {
 		unsigned long pfn;
 		if (vb2_dc_get_user_pfn(start, n_pages, vma, &pfn) == 0) {
diff --git a/drivers/mfd/ab8500-sysctrl.c b/drivers/mfd/ab8500-sysctrl.c
index cfff0b643f1b..0d1825696153 100644
--- a/drivers/mfd/ab8500-sysctrl.c
+++ b/drivers/mfd/ab8500-sysctrl.c
@@ -49,7 +49,9 @@ static void ab8500_power_off(void)
 		if (!psy)
 			continue;
 
-		ret = psy->get_property(psy, POWER_SUPPLY_PROP_ONLINE, &val);
+		ret = power_supply_get_property(psy, POWER_SUPPLY_PROP_ONLINE,
+				&val);
+		power_supply_put(psy);
 
 		if (!ret && val.intval) {
 			charger_present = true;
@@ -63,8 +65,8 @@ static void ab8500_power_off(void)
 	/* Check if battery is known */
 	psy = power_supply_get_by_name("ab8500_btemp");
 	if (psy) {
-		ret = psy->get_property(psy, POWER_SUPPLY_PROP_TECHNOLOGY,
-					&val);
+		ret = power_supply_get_property(psy,
+				POWER_SUPPLY_PROP_TECHNOLOGY, &val);
 		if (!ret && val.intval != POWER_SUPPLY_TECHNOLOGY_UNKNOWN) {
 			printk(KERN_INFO
 			       "Charger \"%s\" is connected with known battery."
@@ -72,6 +74,7 @@ static void ab8500_power_off(void)
 			       pss[i]);
 			machine_restart("charging");
 		}
+		power_supply_put(psy);
 	}
 
 shutdown:
diff --git a/drivers/mfd/axp20x.c b/drivers/mfd/axp20x.c
index b1b580a88654..0acbe52b2411 100644
--- a/drivers/mfd/axp20x.c
+++ b/drivers/mfd/axp20x.c
@@ -87,7 +87,7 @@ static struct resource axp20x_pek_resources[] = {
 	},
 };
 
-static struct resource axp288_battery_resources[] = {
+static struct resource axp288_fuel_gauge_resources[] = {
 	{
 		.start = AXP288_IRQ_QWBTU,
 		.end   = AXP288_IRQ_QWBTU,
@@ -350,9 +350,9 @@ static struct mfd_cell axp288_cells[] = {
 		.resources = axp288_charger_resources,
 	},
 	{
-		.name = "axp288_battery",
-		.num_resources = ARRAY_SIZE(axp288_battery_resources),
-		.resources = axp288_battery_resources,
+		.name = "axp288_fuel_gauge",
+		.num_resources = ARRAY_SIZE(axp288_fuel_gauge_resources),
+		.resources = axp288_fuel_gauge_resources,
 	},
 	{
 		.name = "axp288_pmic_acpi",
diff --git a/drivers/mfd/kempld-core.c b/drivers/mfd/kempld-core.c
index f38ec424872e..5615522f8d62 100644
--- a/drivers/mfd/kempld-core.c
+++ b/drivers/mfd/kempld-core.c
@@ -739,7 +739,7 @@ static int __init kempld_init(void)
 		for (id = kempld_dmi_table;
 		     id->matches[0].slot != DMI_NONE; id++)
 			if (strstr(id->ident, force_device_id))
-				if (id->callback && id->callback(id))
+				if (id->callback && !id->callback(id))
 					break;
 		if (id->matches[0].slot == DMI_NONE)
 			return -ENODEV;
diff --git a/drivers/mfd/rtsx_usb.c b/drivers/mfd/rtsx_usb.c
index ede50244f265..dbd907d7170e 100644
--- a/drivers/mfd/rtsx_usb.c
+++ b/drivers/mfd/rtsx_usb.c
@@ -196,18 +196,27 @@ EXPORT_SYMBOL_GPL(rtsx_usb_ep0_write_register);
 int rtsx_usb_ep0_read_register(struct rtsx_ucr *ucr, u16 addr, u8 *data)
 {
 	u16 value;
+	u8 *buf;
+	int ret;
 
 	if (!data)
 		return -EINVAL;
-	*data = 0;
+
+	buf = kzalloc(sizeof(u8), GFP_KERNEL);
+	if (!buf)
+		return -ENOMEM;
 
 	addr |= EP0_READ_REG_CMD << EP0_OP_SHIFT;
 	value = swab16(addr);
 
-	return usb_control_msg(ucr->pusb_dev,
+	ret = usb_control_msg(ucr->pusb_dev,
 			usb_rcvctrlpipe(ucr->pusb_dev, 0), RTSX_USB_REQ_REG_OP,
 			USB_DIR_IN | USB_TYPE_VENDOR | USB_RECIP_DEVICE,
-			value, 0, data, 1, 100);
+			value, 0, buf, 1, 100);
+	*data = *buf;
+
+	kfree(buf);
+	return ret;
 }
 EXPORT_SYMBOL_GPL(rtsx_usb_ep0_read_register);
 
@@ -288,18 +297,27 @@ static int rtsx_usb_get_status_with_bulk(struct rtsx_ucr *ucr, u16 *status)
 int rtsx_usb_get_card_status(struct rtsx_ucr *ucr, u16 *status)
 {
 	int ret;
+	u16 *buf;
 
 	if (!status)
 		return -EINVAL;
 
-	if (polling_pipe == 0)
+	if (polling_pipe == 0) {
+		buf = kzalloc(sizeof(u16), GFP_KERNEL);
+		if (!buf)
+			return -ENOMEM;
+
 		ret = usb_control_msg(ucr->pusb_dev,
 				usb_rcvctrlpipe(ucr->pusb_dev, 0),
 				RTSX_USB_REQ_POLL,
 				USB_DIR_IN | USB_TYPE_VENDOR | USB_RECIP_DEVICE,
-				0, 0, status, 2, 100);
-	else
+				0, 0, buf, 2, 100);
+		*status = *buf;
+
+		kfree(buf);
+	} else {
 		ret = rtsx_usb_get_status_with_bulk(ucr, status);
+	}
 
 	/* usb_control_msg may return positive when success */
 	if (ret < 0)
diff --git a/drivers/misc/enclosure.c b/drivers/misc/enclosure.c
index 38552a31304a..65fed7146e9b 100644
--- a/drivers/misc/enclosure.c
+++ b/drivers/misc/enclosure.c
@@ -202,16 +202,17 @@ static void enclosure_remove_links(struct enclosure_component *cdev)
 {
 	char name[ENCLOSURE_NAME_SIZE];
 
+	enclosure_link_name(cdev, name);
+
 	/*
 	 * In odd circumstances, like multipath devices, something else may
 	 * already have removed the links, so check for this condition first.
 	 */
-	if (!cdev->dev->kobj.sd)
-		return;
+	if (cdev->dev->kobj.sd)
+		sysfs_remove_link(&cdev->dev->kobj, name);
 
-	enclosure_link_name(cdev, name);
-	sysfs_remove_link(&cdev->dev->kobj, name);
-	sysfs_remove_link(&cdev->cdev.kobj, "device");
+	if (cdev->cdev.kobj.sd)
+		sysfs_remove_link(&cdev->cdev.kobj, "device");
 }
 
 static int enclosure_add_links(struct enclosure_component *cdev)
diff --git a/drivers/misc/mei/init.c b/drivers/misc/mei/init.c
index 9306219d5675..6ad049a08e4d 100644
--- a/drivers/misc/mei/init.c
+++ b/drivers/misc/mei/init.c
@@ -341,6 +341,8 @@ void mei_stop(struct mei_device *dev)
 
 	dev->dev_state = MEI_DEV_POWER_DOWN;
 	mei_reset(dev);
+	/* move device to disabled state unconditionally */
+	dev->dev_state = MEI_DEV_DISABLED;
 
 	mutex_unlock(&dev->device_lock);
 
diff --git a/drivers/misc/sgi-xp/xpc_main.c b/drivers/misc/sgi-xp/xpc_main.c
index 82dc5748f873..7f327121e6d7 100644
--- a/drivers/misc/sgi-xp/xpc_main.c
+++ b/drivers/misc/sgi-xp/xpc_main.c
@@ -1210,7 +1210,7 @@ xpc_system_die(struct notifier_block *nb, unsigned long event, void *_die_args)
 
 		if (((die_args->trapnr == X86_TRAP_MF) ||
 		     (die_args->trapnr == X86_TRAP_XF)) &&
-		    !user_mode_vm(die_args->regs))
+		    !user_mode(die_args->regs))
 			xpc_die_deactivate();
 
 		break;
diff --git a/drivers/mmc/core/core.c b/drivers/mmc/core/core.c
index 23f10f72e5f3..c296bc098fe2 100644
--- a/drivers/mmc/core/core.c
+++ b/drivers/mmc/core/core.c
@@ -897,6 +897,7 @@ int __mmc_claim_host(struct mmc_host *host, atomic_t *abort)
 	DECLARE_WAITQUEUE(wait, current);
 	unsigned long flags;
 	int stop;
+	bool pm = false;
 
 	might_sleep();
 
@@ -916,15 +917,18 @@ int __mmc_claim_host(struct mmc_host *host, atomic_t *abort)
 		host->claimed = 1;
 		host->claimer = current;
 		host->claim_cnt += 1;
+		if (host->claim_cnt == 1)
+			pm = true;
 	} else
 		wake_up(&host->wq);
 	spin_unlock_irqrestore(&host->lock, flags);
 	remove_wait_queue(&host->wq, &wait);
-	if (host->ops->enable && !stop && host->claim_cnt == 1)
-		host->ops->enable(host);
+
+	if (pm)
+		pm_runtime_get_sync(mmc_dev(host));
+
 	return stop;
 }
-
 EXPORT_SYMBOL(__mmc_claim_host);
 
 /**
@@ -940,9 +944,6 @@ void mmc_release_host(struct mmc_host *host)
 
 	WARN_ON(!host->claimed);
 
-	if (host->ops->disable && host->claim_cnt == 1)
-		host->ops->disable(host);
-
 	spin_lock_irqsave(&host->lock, flags);
 	if (--host->claim_cnt) {
 		/* Release for nested claim */
@@ -952,6 +953,8 @@ void mmc_release_host(struct mmc_host *host)
 		host->claimer = NULL;
 		spin_unlock_irqrestore(&host->lock, flags);
 		wake_up(&host->wq);
+		pm_runtime_mark_last_busy(mmc_dev(host));
+		pm_runtime_put_autosuspend(mmc_dev(host));
 	}
 }
 EXPORT_SYMBOL(mmc_release_host);
diff --git a/drivers/mmc/core/mmc.c b/drivers/mmc/core/mmc.c
index 1d41e8541f38..c84131e28625 100644
--- a/drivers/mmc/core/mmc.c
+++ b/drivers/mmc/core/mmc.c
@@ -11,6 +11,7 @@
  */
 
 #include <linux/err.h>
+#include <linux/of.h>
 #include <linux/slab.h>
 #include <linux/stat.h>
 #include <linux/pm_runtime.h>
@@ -336,6 +337,8 @@ static int mmc_decode_ext_csd(struct mmc_card *card, u8 *ext_csd)
 {
 	int err = 0, idx;
 	unsigned int part_size;
+	struct device_node *np;
+	bool broken_hpi = false;
 
 	/* Version is coded in the CSD_STRUCTURE byte in the EXT_CSD register */
 	card->ext_csd.raw_ext_csd_structure = ext_csd[EXT_CSD_STRUCTURE];
@@ -349,6 +352,11 @@ static int mmc_decode_ext_csd(struct mmc_card *card, u8 *ext_csd)
 		}
 	}
 
+	np = mmc_of_find_child_device(card->host, 0);
+	if (np && of_device_is_compatible(np, "mmc-card"))
+		broken_hpi = of_property_read_bool(np, "broken-hpi");
+	of_node_put(np);
+
 	/*
 	 * The EXT_CSD format is meant to be forward compatible. As long
 	 * as CSD_STRUCTURE does not change, all values for EXT_CSD_REV
@@ -494,7 +502,7 @@ static int mmc_decode_ext_csd(struct mmc_card *card, u8 *ext_csd)
 		}
 
 		/* check whether the eMMC card supports HPI */
-		if (ext_csd[EXT_CSD_HPI_FEATURES] & 0x1) {
+		if (!broken_hpi && (ext_csd[EXT_CSD_HPI_FEATURES] & 0x1)) {
 			card->ext_csd.hpi = 1;
 			if (ext_csd[EXT_CSD_HPI_FEATURES] & 0x2)
 				card->ext_csd.hpi_cmd =	MMC_STOP_TRANSMISSION;
diff --git a/drivers/mmc/core/pwrseq.c b/drivers/mmc/core/pwrseq.c
index 862356123d78..ab2129781161 100644
--- a/drivers/mmc/core/pwrseq.c
+++ b/drivers/mmc/core/pwrseq.c
@@ -19,7 +19,7 @@
 
 struct mmc_pwrseq_match {
 	const char *compatible;
-	int (*alloc)(struct mmc_host *host, struct device *dev);
+	struct mmc_pwrseq *(*alloc)(struct mmc_host *host, struct device *dev);
 };
 
 static struct mmc_pwrseq_match pwrseq_match[] = {
@@ -52,6 +52,7 @@ int mmc_pwrseq_alloc(struct mmc_host *host)
 	struct platform_device *pdev;
 	struct device_node *np;
 	struct mmc_pwrseq_match *match;
+	struct mmc_pwrseq *pwrseq;
 	int ret = 0;
 
 	np = of_parse_phandle(host->parent->of_node, "mmc-pwrseq", 0);
@@ -70,9 +71,14 @@ int mmc_pwrseq_alloc(struct mmc_host *host)
 		goto err;
 	}
 
-	ret = match->alloc(host, &pdev->dev);
-	if (!ret)
-		dev_info(host->parent, "allocated mmc-pwrseq\n");
+	pwrseq = match->alloc(host, &pdev->dev);
+	if (IS_ERR(pwrseq)) {
+		ret = PTR_ERR(host->pwrseq);
+		goto err;
+	}
+
+	host->pwrseq = pwrseq;
+	dev_info(host->parent, "allocated mmc-pwrseq\n");
 
 err:
 	of_node_put(np);
@@ -109,4 +115,6 @@ void mmc_pwrseq_free(struct mmc_host *host)
 
 	if (pwrseq && pwrseq->ops && pwrseq->ops->free)
 		pwrseq->ops->free(host);
+
+	host->pwrseq = NULL;
 }
diff --git a/drivers/mmc/core/pwrseq.h b/drivers/mmc/core/pwrseq.h
index aba3409e8d6e..096da48c6a7e 100644
--- a/drivers/mmc/core/pwrseq.h
+++ b/drivers/mmc/core/pwrseq.h
@@ -27,8 +27,10 @@ void mmc_pwrseq_post_power_on(struct mmc_host *host);
 void mmc_pwrseq_power_off(struct mmc_host *host);
 void mmc_pwrseq_free(struct mmc_host *host);
 
-int mmc_pwrseq_simple_alloc(struct mmc_host *host, struct device *dev);
-int mmc_pwrseq_emmc_alloc(struct mmc_host *host, struct device *dev);
+struct mmc_pwrseq *mmc_pwrseq_simple_alloc(struct mmc_host *host,
+					   struct device *dev);
+struct mmc_pwrseq *mmc_pwrseq_emmc_alloc(struct mmc_host *host,
+					 struct device *dev);
 
 #else
 
diff --git a/drivers/mmc/core/pwrseq_emmc.c b/drivers/mmc/core/pwrseq_emmc.c
index a2d545904fbf..9d6d2fb21796 100644
--- a/drivers/mmc/core/pwrseq_emmc.c
+++ b/drivers/mmc/core/pwrseq_emmc.c
@@ -49,7 +49,6 @@ static void mmc_pwrseq_emmc_free(struct mmc_host *host)
 	unregister_restart_handler(&pwrseq->reset_nb);
 	gpiod_put(pwrseq->reset_gpio);
 	kfree(pwrseq);
-	host->pwrseq = NULL;
 }
 
 static struct mmc_pwrseq_ops mmc_pwrseq_emmc_ops = {
@@ -67,14 +66,15 @@ static int mmc_pwrseq_emmc_reset_nb(struct notifier_block *this,
 	return NOTIFY_DONE;
 }
 
-int mmc_pwrseq_emmc_alloc(struct mmc_host *host, struct device *dev)
+struct mmc_pwrseq *mmc_pwrseq_emmc_alloc(struct mmc_host *host,
+					 struct device *dev)
 {
 	struct mmc_pwrseq_emmc *pwrseq;
 	int ret = 0;
 
 	pwrseq = kzalloc(sizeof(struct mmc_pwrseq_emmc), GFP_KERNEL);
 	if (!pwrseq)
-		return -ENOMEM;
+		return ERR_PTR(-ENOMEM);
 
 	pwrseq->reset_gpio = gpiod_get_index(dev, "reset", 0, GPIOD_OUT_LOW);
 	if (IS_ERR(pwrseq->reset_gpio)) {
@@ -92,10 +92,9 @@ int mmc_pwrseq_emmc_alloc(struct mmc_host *host, struct device *dev)
 	register_restart_handler(&pwrseq->reset_nb);
 
 	pwrseq->pwrseq.ops = &mmc_pwrseq_emmc_ops;
-	host->pwrseq = &pwrseq->pwrseq;
 
-	return 0;
+	return &pwrseq->pwrseq;
 free:
 	kfree(pwrseq);
-	return ret;
+	return ERR_PTR(ret);
 }
diff --git a/drivers/mmc/core/pwrseq_simple.c b/drivers/mmc/core/pwrseq_simple.c
index e9f1d8d84613..0b14b83a53d6 100644
--- a/drivers/mmc/core/pwrseq_simple.c
+++ b/drivers/mmc/core/pwrseq_simple.c
@@ -85,7 +85,6 @@ static void mmc_pwrseq_simple_free(struct mmc_host *host)
 		clk_put(pwrseq->ext_clk);
 
 	kfree(pwrseq);
-	host->pwrseq = NULL;
 }
 
 static struct mmc_pwrseq_ops mmc_pwrseq_simple_ops = {
@@ -95,7 +94,8 @@ static struct mmc_pwrseq_ops mmc_pwrseq_simple_ops = {
 	.free = mmc_pwrseq_simple_free,
 };
 
-int mmc_pwrseq_simple_alloc(struct mmc_host *host, struct device *dev)
+struct mmc_pwrseq *mmc_pwrseq_simple_alloc(struct mmc_host *host,
+					   struct device *dev)
 {
 	struct mmc_pwrseq_simple *pwrseq;
 	int i, nr_gpios, ret = 0;
@@ -107,7 +107,7 @@ int mmc_pwrseq_simple_alloc(struct mmc_host *host, struct device *dev)
 	pwrseq = kzalloc(sizeof(struct mmc_pwrseq_simple) + nr_gpios *
 			 sizeof(struct gpio_desc *), GFP_KERNEL);
 	if (!pwrseq)
-		return -ENOMEM;
+		return ERR_PTR(-ENOMEM);
 
 	pwrseq->ext_clk = clk_get(dev, "ext_clock");
 	if (IS_ERR(pwrseq->ext_clk) &&
@@ -124,7 +124,7 @@ int mmc_pwrseq_simple_alloc(struct mmc_host *host, struct device *dev)
 		    PTR_ERR(pwrseq->reset_gpios[i]) != -ENOSYS) {
 			ret = PTR_ERR(pwrseq->reset_gpios[i]);
 
-			while (--i)
+			while (i--)
 				gpiod_put(pwrseq->reset_gpios[i]);
 
 			goto clk_put;
@@ -133,13 +133,12 @@ int mmc_pwrseq_simple_alloc(struct mmc_host *host, struct device *dev)
 
 	pwrseq->nr_gpios = nr_gpios;
 	pwrseq->pwrseq.ops = &mmc_pwrseq_simple_ops;
-	host->pwrseq = &pwrseq->pwrseq;
 
-	return 0;
+	return &pwrseq->pwrseq;
 clk_put:
 	if (!IS_ERR(pwrseq->ext_clk))
 		clk_put(pwrseq->ext_clk);
 free:
 	kfree(pwrseq);
-	return ret;
+	return ERR_PTR(ret);
 }
diff --git a/drivers/mmc/core/sdio.c b/drivers/mmc/core/sdio.c
index ce6cc47206b0..5bc6c7dbbd60 100644
--- a/drivers/mmc/core/sdio.c
+++ b/drivers/mmc/core/sdio.c
@@ -293,19 +293,22 @@ static int sdio_enable_4bit_bus(struct mmc_card *card)
 	int err;
 
 	if (card->type == MMC_TYPE_SDIO)
-		return sdio_enable_wide(card);
-
-	if ((card->host->caps & MMC_CAP_4_BIT_DATA) &&
-		(card->scr.bus_widths & SD_SCR_BUS_WIDTH_4)) {
+		err = sdio_enable_wide(card);
+	else if ((card->host->caps & MMC_CAP_4_BIT_DATA) &&
+		 (card->scr.bus_widths & SD_SCR_BUS_WIDTH_4)) {
 		err = mmc_app_set_bus_width(card, MMC_BUS_WIDTH_4);
 		if (err)
 			return err;
+		err = sdio_enable_wide(card);
+		if (err <= 0)
+			mmc_app_set_bus_width(card, MMC_BUS_WIDTH_1);
 	} else
 		return 0;
 
-	err = sdio_enable_wide(card);
-	if (err <= 0)
-		mmc_app_set_bus_width(card, MMC_BUS_WIDTH_1);
+	if (err > 0) {
+		mmc_set_bus_width(card->host, MMC_BUS_WIDTH_4);
+		err = 0;
+	}
 
 	return err;
 }
@@ -547,13 +550,8 @@ static int mmc_sdio_init_uhs_card(struct mmc_card *card)
 	/*
 	 * Switch to wider bus (if supported).
 	 */
-	if (card->host->caps & MMC_CAP_4_BIT_DATA) {
+	if (card->host->caps & MMC_CAP_4_BIT_DATA)
 		err = sdio_enable_4bit_bus(card);
-		if (err > 0) {
-			mmc_set_bus_width(card->host, MMC_BUS_WIDTH_4);
-			err = 0;
-		}
-	}
 
 	/* Set the driver strength for the card */
 	sdio_select_driver_type(card);
@@ -803,9 +801,7 @@ try_again:
 		 * Switch to wider bus (if supported).
 		 */
 		err = sdio_enable_4bit_bus(card);
-		if (err > 0)
-			mmc_set_bus_width(card->host, MMC_BUS_WIDTH_4);
-		else if (err)
+		if (err)
 			goto remove;
 	}
 finish:
@@ -983,10 +979,6 @@ static int mmc_sdio_resume(struct mmc_host *host)
 	} else if (mmc_card_keep_power(host) && mmc_card_wake_sdio_irq(host)) {
 		/* We may have switched to 1-bit mode during suspend */
 		err = sdio_enable_4bit_bus(host->card);
-		if (err > 0) {
-			mmc_set_bus_width(host, MMC_BUS_WIDTH_4);
-			err = 0;
-		}
 	}
 
 	if (!err && host->sdio_irqs) {
diff --git a/drivers/mmc/host/Kconfig b/drivers/mmc/host/Kconfig
index 61ac63a3776a..7f4db908f89b 100644
--- a/drivers/mmc/host/Kconfig
+++ b/drivers/mmc/host/Kconfig
@@ -132,7 +132,7 @@ config MMC_SDHCI_OF_ARASAN
 config MMC_SDHCI_OF_ESDHC
 	tristate "SDHCI OF support for the Freescale eSDHC controller"
 	depends on MMC_SDHCI_PLTFM
-	depends on PPC_OF
+	depends on PPC
 	select MMC_SDHCI_BIG_ENDIAN_32BIT_BYTE_SWAPPER
 	help
 	  This selects the Freescale eSDHC controller support.
@@ -144,7 +144,7 @@ config MMC_SDHCI_OF_ESDHC
 config MMC_SDHCI_OF_HLWD
 	tristate "SDHCI OF support for the Nintendo Wii SDHCI controllers"
 	depends on MMC_SDHCI_PLTFM
-	depends on PPC_OF
+	depends on PPC
 	select MMC_SDHCI_BIG_ENDIAN_32BIT_BYTE_SWAPPER
 	help
 	  This selects the Secure Digital Host Controller Interface (SDHCI)
@@ -230,7 +230,7 @@ config MMC_SDHCI_PXAV3
 	tristate "Marvell MMP2 SD Host Controller support (PXAV3)"
 	depends on CLKDEV_LOOKUP
 	depends on MMC_SDHCI_PLTFM
-	depends on ARCH_MMP || COMPILE_TEST
+	depends on ARCH_BERLIN || ARCH_MMP || ARCH_MVEBU || COMPILE_TEST
 	default CPU_MMP2
 	help
 	  This selects the Marvell(R) PXAV3 SD Host Controller.
@@ -255,6 +255,7 @@ config MMC_SDHCI_PXAV2
 config MMC_SDHCI_SPEAR
 	tristate "SDHCI support on ST SPEAr platform"
 	depends on MMC_SDHCI && PLAT_SPEAR
+	depends on OF
 	help
 	  This selects the Secure Digital Host Controller Interface (SDHCI)
 	  often referrered to as the HSMMC block in some of the ST SPEAR range
@@ -307,6 +308,20 @@ config MMC_SDHCI_F_SDH30
 
 	  If unsure, say N.
 
+config MMC_SDHCI_IPROC
+	tristate "SDHCI platform support for the iProc SD/MMC Controller"
+	depends on ARCH_BCM_IPROC || COMPILE_TEST
+	depends on MMC_SDHCI_PLTFM
+	default ARCH_BCM_IPROC
+	select MMC_SDHCI_IO_ACCESSORS
+	help
+	  This selects the iProc SD/MMC controller.
+
+	  If you have an IPROC platform with SD or MMC devices,
+	  say Y or M here.
+
+	  If unsure, say N.
+
 config MMC_MOXART
 	tristate "MOXART SD/MMC Host Controller support"
 	depends on ARCH_MOXART && MMC
diff --git a/drivers/mmc/host/Makefile b/drivers/mmc/host/Makefile
index 6a7cfe0de332..711e913450f5 100644
--- a/drivers/mmc/host/Makefile
+++ b/drivers/mmc/host/Makefile
@@ -71,6 +71,7 @@ obj-$(CONFIG_MMC_SDHCI_OF_ESDHC)	+= sdhci-of-esdhc.o
 obj-$(CONFIG_MMC_SDHCI_OF_HLWD)		+= sdhci-of-hlwd.o
 obj-$(CONFIG_MMC_SDHCI_BCM_KONA)	+= sdhci-bcm-kona.o
 obj-$(CONFIG_MMC_SDHCI_BCM2835)		+= sdhci-bcm2835.o
+obj-$(CONFIG_MMC_SDHCI_IPROC)		+= sdhci-iproc.o
 obj-$(CONFIG_MMC_SDHCI_MSM)		+= sdhci-msm.o
 obj-$(CONFIG_MMC_SDHCI_ST)		+= sdhci-st.o
 
diff --git a/drivers/mmc/host/atmel-mci-regs.h b/drivers/mmc/host/atmel-mci-regs.h
index c97001e15227..0aa44e679df4 100644
--- a/drivers/mmc/host/atmel-mci-regs.h
+++ b/drivers/mmc/host/atmel-mci-regs.h
@@ -135,10 +135,17 @@
 #define ATMCI_REGS_SIZE		0x100
 
 /* Register access macros */
-#define atmci_readl(port,reg)				\
+#ifdef CONFIG_AVR32
+#define atmci_readl(port, reg)			\
 	__raw_readl((port)->regs + reg)
-#define atmci_writel(port,reg,value)			\
+#define atmci_writel(port, reg, value)			\
 	__raw_writel((value), (port)->regs + reg)
+#else
+#define atmci_readl(port, reg)			\
+	readl_relaxed((port)->regs + reg)
+#define atmci_writel(port, reg, value)			\
+	writel_relaxed((value), (port)->regs + reg)
+#endif
 
 /* On AVR chips the Peripheral DMA Controller is not connected to MCI. */
 #ifdef CONFIG_AVR32
diff --git a/drivers/mmc/host/dw_mmc-exynos.c b/drivers/mmc/host/dw_mmc-exynos.c
index fe32948c6114..e761eb1b1441 100644
--- a/drivers/mmc/host/dw_mmc-exynos.c
+++ b/drivers/mmc/host/dw_mmc-exynos.c
@@ -40,7 +40,12 @@ struct dw_mci_exynos_priv_data {
 	u8				ciu_div;
 	u32				sdr_timing;
 	u32				ddr_timing;
+	u32				hs400_timing;
+	u32				tuned_sample;
 	u32				cur_speed;
+	u32				dqs_delay;
+	u32				saved_dqs_en;
+	u32				saved_strobe_ctrl;
 };
 
 static struct dw_mci_exynos_compatible {
@@ -71,6 +76,21 @@ static struct dw_mci_exynos_compatible {
 	},
 };
 
+static inline u8 dw_mci_exynos_get_ciu_div(struct dw_mci *host)
+{
+	struct dw_mci_exynos_priv_data *priv = host->priv;
+
+	if (priv->ctrl_type == DW_MCI_TYPE_EXYNOS4412)
+		return EXYNOS4412_FIXED_CIU_CLK_DIV;
+	else if (priv->ctrl_type == DW_MCI_TYPE_EXYNOS4210)
+		return EXYNOS4210_FIXED_CIU_CLK_DIV;
+	else if (priv->ctrl_type == DW_MCI_TYPE_EXYNOS7 ||
+			priv->ctrl_type == DW_MCI_TYPE_EXYNOS7_SMU)
+		return SDMMC_CLKSEL_GET_DIV(mci_readl(host, CLKSEL64)) + 1;
+	else
+		return SDMMC_CLKSEL_GET_DIV(mci_readl(host, CLKSEL)) + 1;
+}
+
 static int dw_mci_exynos_priv_init(struct dw_mci *host)
 {
 	struct dw_mci_exynos_priv_data *priv = host->priv;
@@ -85,6 +105,16 @@ static int dw_mci_exynos_priv_init(struct dw_mci *host)
 			   SDMMC_MPSCTRL_NON_SECURE_WRITE_BIT);
 	}
 
+	if (priv->ctrl_type >= DW_MCI_TYPE_EXYNOS5420) {
+		priv->saved_strobe_ctrl = mci_readl(host, HS400_DLINE_CTRL);
+		priv->saved_dqs_en = mci_readl(host, HS400_DQS_EN);
+		priv->saved_dqs_en |= AXI_NON_BLOCKING_WR;
+		mci_writel(host, HS400_DQS_EN, priv->saved_dqs_en);
+		if (!priv->dqs_delay)
+			priv->dqs_delay =
+				DQS_CTRL_GET_RD_DELAY(priv->saved_strobe_ctrl);
+	}
+
 	return 0;
 }
 
@@ -97,6 +127,26 @@ static int dw_mci_exynos_setup_clock(struct dw_mci *host)
 	return 0;
 }
 
+static void dw_mci_exynos_set_clksel_timing(struct dw_mci *host, u32 timing)
+{
+	struct dw_mci_exynos_priv_data *priv = host->priv;
+	u32 clksel;
+
+	if (priv->ctrl_type == DW_MCI_TYPE_EXYNOS7 ||
+		priv->ctrl_type == DW_MCI_TYPE_EXYNOS7_SMU)
+		clksel = mci_readl(host, CLKSEL64);
+	else
+		clksel = mci_readl(host, CLKSEL);
+
+	clksel = (clksel & ~SDMMC_CLKSEL_TIMING_MASK) | timing;
+
+	if (priv->ctrl_type == DW_MCI_TYPE_EXYNOS7 ||
+		priv->ctrl_type == DW_MCI_TYPE_EXYNOS7_SMU)
+		mci_writel(host, CLKSEL64, clksel);
+	else
+		mci_writel(host, CLKSEL, clksel);
+}
+
 #ifdef CONFIG_PM_SLEEP
 static int dw_mci_exynos_suspend(struct device *dev)
 {
@@ -172,30 +222,38 @@ static void dw_mci_exynos_prepare_command(struct dw_mci *host, u32 *cmdr)
 	}
 }
 
-static void dw_mci_exynos_set_ios(struct dw_mci *host, struct mmc_ios *ios)
+static void dw_mci_exynos_config_hs400(struct dw_mci *host, u32 timing)
 {
 	struct dw_mci_exynos_priv_data *priv = host->priv;
-	unsigned int wanted = ios->clock;
-	unsigned long actual;
-	u8 div = priv->ciu_div + 1;
+	u32 dqs, strobe;
 
-	if (ios->timing == MMC_TIMING_MMC_DDR52) {
-		if (priv->ctrl_type == DW_MCI_TYPE_EXYNOS7 ||
-			priv->ctrl_type == DW_MCI_TYPE_EXYNOS7_SMU)
-			mci_writel(host, CLKSEL64, priv->ddr_timing);
-		else
-			mci_writel(host, CLKSEL, priv->ddr_timing);
-		/* Should be double rate for DDR mode */
-		if (ios->bus_width == MMC_BUS_WIDTH_8)
-			wanted <<= 1;
+	/*
+	 * Not supported to configure register
+	 * related to HS400
+	 */
+	if (priv->ctrl_type < DW_MCI_TYPE_EXYNOS5420)
+		return;
+
+	dqs = priv->saved_dqs_en;
+	strobe = priv->saved_strobe_ctrl;
+
+	if (timing == MMC_TIMING_MMC_HS400) {
+		dqs |= DATA_STROBE_EN;
+		strobe = DQS_CTRL_RD_DELAY(strobe, priv->dqs_delay);
 	} else {
-		if (priv->ctrl_type == DW_MCI_TYPE_EXYNOS7 ||
-			priv->ctrl_type == DW_MCI_TYPE_EXYNOS7_SMU)
-			mci_writel(host, CLKSEL64, priv->sdr_timing);
-		else
-			mci_writel(host, CLKSEL, priv->sdr_timing);
+		dqs &= ~DATA_STROBE_EN;
 	}
 
+	mci_writel(host, HS400_DQS_EN, dqs);
+	mci_writel(host, HS400_DLINE_CTRL, strobe);
+}
+
+static void dw_mci_exynos_adjust_clock(struct dw_mci *host, unsigned int wanted)
+{
+	struct dw_mci_exynos_priv_data *priv = host->priv;
+	unsigned long actual;
+	u8 div;
+	int ret;
 	/*
 	 * Don't care if wanted clock is zero or
 	 * ciu clock is unavailable
@@ -207,17 +265,52 @@ static void dw_mci_exynos_set_ios(struct dw_mci *host, struct mmc_ios *ios)
 	if (wanted < EXYNOS_CCLKIN_MIN)
 		wanted = EXYNOS_CCLKIN_MIN;
 
-	if (wanted != priv->cur_speed) {
-		int ret = clk_set_rate(host->ciu_clk, wanted * div);
-		if (ret)
-			dev_warn(host->dev,
-				"failed to set clk-rate %u error: %d\n",
-				 wanted * div, ret);
-		actual = clk_get_rate(host->ciu_clk);
-		host->bus_hz = actual / div;
-		priv->cur_speed = wanted;
-		host->current_speed = 0;
+	if (wanted == priv->cur_speed)
+		return;
+
+	div = dw_mci_exynos_get_ciu_div(host);
+	ret = clk_set_rate(host->ciu_clk, wanted * div);
+	if (ret)
+		dev_warn(host->dev,
+			"failed to set clk-rate %u error: %d\n",
+			wanted * div, ret);
+	actual = clk_get_rate(host->ciu_clk);
+	host->bus_hz = actual / div;
+	priv->cur_speed = wanted;
+	host->current_speed = 0;
+}
+
+static void dw_mci_exynos_set_ios(struct dw_mci *host, struct mmc_ios *ios)
+{
+	struct dw_mci_exynos_priv_data *priv = host->priv;
+	unsigned int wanted = ios->clock;
+	u32 timing = ios->timing, clksel;
+
+	switch (timing) {
+	case MMC_TIMING_MMC_HS400:
+		/* Update tuned sample timing */
+		clksel = SDMMC_CLKSEL_UP_SAMPLE(
+				priv->hs400_timing, priv->tuned_sample);
+		wanted <<= 1;
+		break;
+	case MMC_TIMING_MMC_DDR52:
+		clksel = priv->ddr_timing;
+		/* Should be double rate for DDR mode */
+		if (ios->bus_width == MMC_BUS_WIDTH_8)
+			wanted <<= 1;
+		break;
+	default:
+		clksel = priv->sdr_timing;
 	}
+
+	/* Set clock timing for the requested speed mode*/
+	dw_mci_exynos_set_clksel_timing(host, clksel);
+
+	/* Configure setting for HS400 */
+	dw_mci_exynos_config_hs400(host, timing);
+
+	/* Configure clock rate */
+	dw_mci_exynos_adjust_clock(host, wanted);
 }
 
 static int dw_mci_exynos_parse_dt(struct dw_mci *host)
@@ -260,6 +353,16 @@ static int dw_mci_exynos_parse_dt(struct dw_mci *host)
 		return ret;
 
 	priv->ddr_timing = SDMMC_CLKSEL_TIMING(timing[0], timing[1], div);
+
+	ret = of_property_read_u32_array(np,
+			"samsung,dw-mshc-hs400-timing", timing, 2);
+	if (!ret && of_property_read_u32(np,
+				"samsung,read-strobe-delay", &priv->dqs_delay))
+		dev_dbg(host->dev,
+			"read-strobe-delay is not found, assuming usage of default value\n");
+
+	priv->hs400_timing = SDMMC_CLKSEL_TIMING(timing[0], timing[1],
+						HS400_FIXED_CIU_CLK_DIV);
 	host->priv = priv;
 	return 0;
 }
@@ -285,7 +388,7 @@ static inline void dw_mci_exynos_set_clksmpl(struct dw_mci *host, u8 sample)
 		clksel = mci_readl(host, CLKSEL64);
 	else
 		clksel = mci_readl(host, CLKSEL);
-	clksel = (clksel & ~0x7) | SDMMC_CLKSEL_CCLK_SAMPLE(sample);
+	clksel = SDMMC_CLKSEL_UP_SAMPLE(clksel, sample);
 	if (priv->ctrl_type == DW_MCI_TYPE_EXYNOS7 ||
 		priv->ctrl_type == DW_MCI_TYPE_EXYNOS7_SMU)
 		mci_writel(host, CLKSEL64, clksel);
@@ -304,13 +407,16 @@ static inline u8 dw_mci_exynos_move_next_clksmpl(struct dw_mci *host)
 		clksel = mci_readl(host, CLKSEL64);
 	else
 		clksel = mci_readl(host, CLKSEL);
+
 	sample = (clksel + 1) & 0x7;
-	clksel = (clksel & ~0x7) | sample;
+	clksel = SDMMC_CLKSEL_UP_SAMPLE(clksel, sample);
+
 	if (priv->ctrl_type == DW_MCI_TYPE_EXYNOS7 ||
 		priv->ctrl_type == DW_MCI_TYPE_EXYNOS7_SMU)
 		mci_writel(host, CLKSEL64, clksel);
 	else
 		mci_writel(host, CLKSEL, clksel);
+
 	return sample;
 }
 
@@ -343,6 +449,7 @@ out:
 static int dw_mci_exynos_execute_tuning(struct dw_mci_slot *slot)
 {
 	struct dw_mci *host = slot->host;
+	struct dw_mci_exynos_priv_data *priv = host->priv;
 	struct mmc_host *mmc = slot->mmc;
 	u8 start_smpl, smpl, candiates = 0;
 	s8 found = -1;
@@ -360,14 +467,27 @@ static int dw_mci_exynos_execute_tuning(struct dw_mci_slot *slot)
 	} while (start_smpl != smpl);
 
 	found = dw_mci_exynos_get_best_clksmpl(candiates);
-	if (found >= 0)
+	if (found >= 0) {
 		dw_mci_exynos_set_clksmpl(host, found);
-	else
+		priv->tuned_sample = found;
+	} else {
 		ret = -EIO;
+	}
 
 	return ret;
 }
 
+static int dw_mci_exynos_prepare_hs400_tuning(struct dw_mci *host,
+					struct mmc_ios *ios)
+{
+	struct dw_mci_exynos_priv_data *priv = host->priv;
+
+	dw_mci_exynos_set_clksel_timing(host, priv->hs400_timing);
+	dw_mci_exynos_adjust_clock(host, (ios->clock) << 1);
+
+	return 0;
+}
+
 /* Common capabilities of Exynos4/Exynos5 SoC */
 static unsigned long exynos_dwmmc_caps[4] = {
 	MMC_CAP_1_8V_DDR | MMC_CAP_8_BIT_DATA | MMC_CAP_CMD23,
@@ -384,6 +504,7 @@ static const struct dw_mci_drv_data exynos_drv_data = {
 	.set_ios		= dw_mci_exynos_set_ios,
 	.parse_dt		= dw_mci_exynos_parse_dt,
 	.execute_tuning		= dw_mci_exynos_execute_tuning,
+	.prepare_hs400_tuning	= dw_mci_exynos_prepare_hs400_tuning,
 };
 
 static const struct of_device_id dw_mci_exynos_match[] = {
diff --git a/drivers/mmc/host/dw_mmc-exynos.h b/drivers/mmc/host/dw_mmc-exynos.h
index 7872ce586b55..595c934e6166 100644
--- a/drivers/mmc/host/dw_mmc-exynos.h
+++ b/drivers/mmc/host/dw_mmc-exynos.h
@@ -12,20 +12,36 @@
 #ifndef _DW_MMC_EXYNOS_H_
 #define _DW_MMC_EXYNOS_H_
 
-/* Extended Register's Offset */
 #define SDMMC_CLKSEL			0x09C
 #define SDMMC_CLKSEL64			0x0A8
 
+/* Extended Register's Offset */
+#define SDMMC_HS400_DQS_EN		0x180
+#define SDMMC_HS400_ASYNC_FIFO_CTRL	0x184
+#define SDMMC_HS400_DLINE_CTRL		0x188
+
 /* CLKSEL register defines */
 #define SDMMC_CLKSEL_CCLK_SAMPLE(x)	(((x) & 7) << 0)
 #define SDMMC_CLKSEL_CCLK_DRIVE(x)	(((x) & 7) << 16)
 #define SDMMC_CLKSEL_CCLK_DIVIDER(x)	(((x) & 7) << 24)
 #define SDMMC_CLKSEL_GET_DRV_WD3(x)	(((x) >> 16) & 0x7)
+#define SDMMC_CLKSEL_GET_DIV(x)		(((x) >> 24) & 0x7)
+#define SDMMC_CLKSEL_UP_SAMPLE(x, y)	(((x) & ~SDMMC_CLKSEL_CCLK_SAMPLE(7)) |\
+					 SDMMC_CLKSEL_CCLK_SAMPLE(y))
 #define SDMMC_CLKSEL_TIMING(x, y, z)	(SDMMC_CLKSEL_CCLK_SAMPLE(x) |	\
 					 SDMMC_CLKSEL_CCLK_DRIVE(y) |	\
 					 SDMMC_CLKSEL_CCLK_DIVIDER(z))
+#define SDMMC_CLKSEL_TIMING_MASK	SDMMC_CLKSEL_TIMING(0x7, 0x7, 0x7)
 #define SDMMC_CLKSEL_WAKEUP_INT		BIT(11)
 
+/* RCLK_EN register defines */
+#define DATA_STROBE_EN			BIT(0)
+#define AXI_NON_BLOCKING_WR	BIT(7)
+
+/* DLINE_CTRL register defines */
+#define DQS_CTRL_RD_DELAY(x, y)		(((x) & ~0x3FF) | ((y) & 0x3FF))
+#define DQS_CTRL_GET_RD_DELAY(x)	((x) & 0x3FF)
+
 /* Protector Register */
 #define SDMMC_EMMCP_BASE	0x1000
 #define SDMMC_MPSECURITY	(SDMMC_EMMCP_BASE + 0x0010)
@@ -49,6 +65,7 @@
 /* Fixed clock divider */
 #define EXYNOS4210_FIXED_CIU_CLK_DIV	2
 #define EXYNOS4412_FIXED_CIU_CLK_DIV	4
+#define HS400_FIXED_CIU_CLK_DIV		1
 
 /* Minimal required clock frequency for cclkin, unit: HZ */
 #define EXYNOS_CCLKIN_MIN	50000000
diff --git a/drivers/mmc/host/dw_mmc-rockchip.c b/drivers/mmc/host/dw_mmc-rockchip.c
index e2a726a503ee..dbf166f94f1b 100644
--- a/drivers/mmc/host/dw_mmc-rockchip.c
+++ b/drivers/mmc/host/dw_mmc-rockchip.c
@@ -76,12 +76,20 @@ static int dw_mci_rockchip_init(struct dw_mci *host)
 	return 0;
 }
 
+/* Common capabilities of RK3288 SoC */
+static unsigned long dw_mci_rk3288_dwmmc_caps[4] = {
+	MMC_CAP_RUNTIME_RESUME, /* emmc */
+	MMC_CAP_RUNTIME_RESUME, /* sdmmc */
+	MMC_CAP_RUNTIME_RESUME, /* sdio0 */
+	MMC_CAP_RUNTIME_RESUME, /* sdio1 */
+};
 static const struct dw_mci_drv_data rk2928_drv_data = {
 	.prepare_command        = dw_mci_rockchip_prepare_command,
 	.init			= dw_mci_rockchip_init,
 };
 
 static const struct dw_mci_drv_data rk3288_drv_data = {
+	.caps			= dw_mci_rk3288_dwmmc_caps,
 	.prepare_command        = dw_mci_rockchip_prepare_command,
 	.set_ios		= dw_mci_rk3288_set_ios,
 	.setup_clock    = dw_mci_rk3288_setup_clock,
diff --git a/drivers/mmc/host/dw_mmc.c b/drivers/mmc/host/dw_mmc.c
index 4d2e3c2e1830..38b29265cc7c 100644
--- a/drivers/mmc/host/dw_mmc.c
+++ b/drivers/mmc/host/dw_mmc.c
@@ -69,7 +69,8 @@ struct idmac_desc_64addr {
 
 	u32		des2;	/*Buffer sizes */
 #define IDMAC_64ADDR_SET_BUFFER1_SIZE(d, s) \
-	((d)->des2 = ((d)->des2 & 0x03ffe000) | ((s) & 0x1fff))
+	((d)->des2 = ((d)->des2 & cpu_to_le32(0x03ffe000)) | \
+	 ((cpu_to_le32(s)) & cpu_to_le32(0x1fff)))
 
 	u32		des3;	/* Reserved */
 
@@ -81,7 +82,7 @@ struct idmac_desc_64addr {
 };
 
 struct idmac_desc {
-	u32		des0;	/* Control Descriptor */
+	__le32		des0;	/* Control Descriptor */
 #define IDMAC_DES0_DIC	BIT(1)
 #define IDMAC_DES0_LD	BIT(2)
 #define IDMAC_DES0_FD	BIT(3)
@@ -90,18 +91,19 @@ struct idmac_desc {
 #define IDMAC_DES0_CES	BIT(30)
 #define IDMAC_DES0_OWN	BIT(31)
 
-	u32		des1;	/* Buffer sizes */
+	__le32		des1;	/* Buffer sizes */
 #define IDMAC_SET_BUFFER1_SIZE(d, s) \
 	((d)->des1 = ((d)->des1 & 0x03ffe000) | ((s) & 0x1fff))
 
-	u32		des2;	/* buffer 1 physical address */
+	__le32		des2;	/* buffer 1 physical address */
 
-	u32		des3;	/* buffer 2 physical address */
+	__le32		des3;	/* buffer 2 physical address */
 };
 #endif /* CONFIG_MMC_DW_IDMAC */
 
 static bool dw_mci_reset(struct dw_mci *host);
 static bool dw_mci_ctrl_reset(struct dw_mci *host, u32 reset);
+static int dw_mci_card_busy(struct mmc_host *mmc);
 
 #if defined(CONFIG_DEBUG_FS)
 static int dw_mci_req_show(struct seq_file *s, void *v)
@@ -335,6 +337,31 @@ static u32 dw_mci_prep_stop_abort(struct dw_mci *host, struct mmc_command *cmd)
 	return cmdr;
 }
 
+static void dw_mci_wait_while_busy(struct dw_mci *host, u32 cmd_flags)
+{
+	unsigned long timeout = jiffies + msecs_to_jiffies(500);
+
+	/*
+	 * Databook says that before issuing a new data transfer command
+	 * we need to check to see if the card is busy.  Data transfer commands
+	 * all have SDMMC_CMD_PRV_DAT_WAIT set, so we'll key off that.
+	 *
+	 * ...also allow sending for SDMMC_CMD_VOLT_SWITCH where busy is
+	 * expected.
+	 */
+	if ((cmd_flags & SDMMC_CMD_PRV_DAT_WAIT) &&
+	    !(cmd_flags & SDMMC_CMD_VOLT_SWITCH)) {
+		while (mci_readl(host, STATUS) & SDMMC_STATUS_BUSY) {
+			if (time_after(jiffies, timeout)) {
+				/* Command will fail; we'll pass error then */
+				dev_err(host->dev, "Busy; trying anyway\n");
+				break;
+			}
+			udelay(10);
+		}
+	}
+}
+
 static void dw_mci_start_command(struct dw_mci *host,
 				 struct mmc_command *cmd, u32 cmd_flags)
 {
@@ -345,6 +372,7 @@ static void dw_mci_start_command(struct dw_mci *host,
 
 	mci_writel(host, CMDARG, cmd->arg);
 	wmb();
+	dw_mci_wait_while_busy(host, cmd_flags);
 
 	mci_writel(host, CMD, cmd_flags | SDMMC_CMD_START);
 }
@@ -477,23 +505,23 @@ static void dw_mci_translate_sglist(struct dw_mci *host, struct mmc_data *data,
 			 * Set the OWN bit and disable interrupts for this
 			 * descriptor
 			 */
-			desc->des0 = IDMAC_DES0_OWN | IDMAC_DES0_DIC |
-						IDMAC_DES0_CH;
+			desc->des0 = cpu_to_le32(IDMAC_DES0_OWN |
+					IDMAC_DES0_DIC | IDMAC_DES0_CH);
 			/* Buffer length */
 			IDMAC_SET_BUFFER1_SIZE(desc, length);
 
 			/* Physical address to DMA to/from */
-			desc->des2 = mem_addr;
+			desc->des2 = cpu_to_le32(mem_addr);
 		}
 
 		/* Set first descriptor */
 		desc = host->sg_cpu;
-		desc->des0 |= IDMAC_DES0_FD;
+		desc->des0 |= cpu_to_le32(IDMAC_DES0_FD);
 
 		/* Set last descriptor */
 		desc = host->sg_cpu + (i - 1) * sizeof(struct idmac_desc);
-		desc->des0 &= ~(IDMAC_DES0_CH | IDMAC_DES0_DIC);
-		desc->des0 |= IDMAC_DES0_LD;
+		desc->des0 &= cpu_to_le32(~(IDMAC_DES0_CH | IDMAC_DES0_DIC));
+		desc->des0 |= cpu_to_le32(IDMAC_DES0_LD);
 	}
 
 	wmb();
@@ -562,12 +590,12 @@ static int dw_mci_idmac_init(struct dw_mci *host)
 
 		/* Forward link the descriptor list */
 		for (i = 0, p = host->sg_cpu; i < host->ring_size - 1; i++, p++)
-			p->des3 = host->sg_dma + (sizeof(struct idmac_desc) *
-								(i + 1));
+			p->des3 = cpu_to_le32(host->sg_dma +
+					(sizeof(struct idmac_desc) * (i + 1)));
 
 		/* Set the last descriptor as the end-of-ring descriptor */
-		p->des3 = host->sg_dma;
-		p->des0 = IDMAC_DES0_ER;
+		p->des3 = cpu_to_le32(host->sg_dma);
+		p->des0 = cpu_to_le32(IDMAC_DES0_ER);
 	}
 
 	dw_mci_idmac_reset(host);
@@ -737,6 +765,7 @@ static void dw_mci_ctrl_rd_thld(struct dw_mci *host, struct mmc_data *data)
 		return;
 
 	if (host->timing != MMC_TIMING_MMC_HS200 &&
+	    host->timing != MMC_TIMING_MMC_HS400 &&
 	    host->timing != MMC_TIMING_UHS_SDR104)
 		goto disable;
 
@@ -876,6 +905,7 @@ static void mci_send_cmd(struct dw_mci_slot *slot, u32 cmd, u32 arg)
 
 	mci_writel(host, CMDARG, arg);
 	wmb();
+	dw_mci_wait_while_busy(host, cmd);
 	mci_writel(host, CMD, SDMMC_CMD_START | cmd);
 
 	while (time_before(jiffies, timeout)) {
@@ -992,6 +1022,26 @@ static void __dw_mci_start_request(struct dw_mci *host,
 
 	dw_mci_start_command(host, cmd, cmdflags);
 
+	if (cmd->opcode == SD_SWITCH_VOLTAGE) {
+		unsigned long irqflags;
+
+		/*
+		 * Databook says to fail after 2ms w/ no response, but evidence
+		 * shows that sometimes the cmd11 interrupt takes over 130ms.
+		 * We'll set to 500ms, plus an extra jiffy just in case jiffies
+		 * is just about to roll over.
+		 *
+		 * We do this whole thing under spinlock and only if the
+		 * command hasn't already completed (indicating the the irq
+		 * already ran so we don't want the timeout).
+		 */
+		spin_lock_irqsave(&host->irq_lock, irqflags);
+		if (!test_bit(EVENT_CMD_COMPLETE, &host->pending_events))
+			mod_timer(&host->cmd11_timer,
+				jiffies + msecs_to_jiffies(500) + 1);
+		spin_unlock_irqrestore(&host->irq_lock, irqflags);
+	}
+
 	if (mrq->stop)
 		host->stop_cmdr = dw_mci_prepare_command(slot->mmc, mrq->stop);
 	else
@@ -1084,7 +1134,8 @@ static void dw_mci_set_ios(struct mmc_host *mmc, struct mmc_ios *ios)
 	regs = mci_readl(slot->host, UHS_REG);
 
 	/* DDR mode set */
-	if (ios->timing == MMC_TIMING_MMC_DDR52)
+	if (ios->timing == MMC_TIMING_MMC_DDR52 ||
+	    ios->timing == MMC_TIMING_MMC_HS400)
 		regs |= ((0x1 << slot->id) << 16);
 	else
 		regs &= ~((0x1 << slot->id) << 16);
@@ -1101,12 +1152,6 @@ static void dw_mci_set_ios(struct mmc_host *mmc, struct mmc_ios *ios)
 	if (drv_data && drv_data->set_ios)
 		drv_data->set_ios(slot->host, ios);
 
-	/* Slot specific timing and width adjustment */
-	dw_mci_setup_bus(slot, false);
-
-	if (slot->host->state == STATE_WAITING_CMD11_DONE && ios->clock != 0)
-		slot->host->state = STATE_IDLE;
-
 	switch (ios->power_mode) {
 	case MMC_POWER_UP:
 		if (!IS_ERR(mmc->supply.vmmc)) {
@@ -1125,23 +1170,39 @@ static void dw_mci_set_ios(struct mmc_host *mmc, struct mmc_ios *ios)
 		mci_writel(slot->host, PWREN, regs);
 		break;
 	case MMC_POWER_ON:
-		if (!IS_ERR(mmc->supply.vqmmc) && !slot->host->vqmmc_enabled) {
-			ret = regulator_enable(mmc->supply.vqmmc);
-			if (ret < 0)
-				dev_err(slot->host->dev,
-					"failed to enable vqmmc regulator\n");
-			else
+		if (!slot->host->vqmmc_enabled) {
+			if (!IS_ERR(mmc->supply.vqmmc)) {
+				ret = regulator_enable(mmc->supply.vqmmc);
+				if (ret < 0)
+					dev_err(slot->host->dev,
+						"failed to enable vqmmc\n");
+				else
+					slot->host->vqmmc_enabled = true;
+
+			} else {
+				/* Keep track so we don't reset again */
 				slot->host->vqmmc_enabled = true;
+			}
+
+			/* Reset our state machine after powering on */
+			dw_mci_ctrl_reset(slot->host,
+					  SDMMC_CTRL_ALL_RESET_FLAGS);
 		}
+
+		/* Adjust clock / bus width after power is up */
+		dw_mci_setup_bus(slot, false);
+
 		break;
 	case MMC_POWER_OFF:
+		/* Turn clock off before power goes down */
+		dw_mci_setup_bus(slot, false);
+
 		if (!IS_ERR(mmc->supply.vmmc))
 			mmc_regulator_set_ocr(mmc, mmc->supply.vmmc, 0);
 
-		if (!IS_ERR(mmc->supply.vqmmc) && slot->host->vqmmc_enabled) {
+		if (!IS_ERR(mmc->supply.vqmmc) && slot->host->vqmmc_enabled)
 			regulator_disable(mmc->supply.vqmmc);
-			slot->host->vqmmc_enabled = false;
-		}
+		slot->host->vqmmc_enabled = false;
 
 		regs = mci_readl(slot->host, PWREN);
 		regs &= ~(1 << slot->id);
@@ -1150,6 +1211,9 @@ static void dw_mci_set_ios(struct mmc_host *mmc, struct mmc_ios *ios)
 	default:
 		break;
 	}
+
+	if (slot->host->state == STATE_WAITING_CMD11_DONE && ios->clock != 0)
+		slot->host->state = STATE_IDLE;
 }
 
 static int dw_mci_card_busy(struct mmc_host *mmc)
@@ -1323,6 +1387,18 @@ static int dw_mci_execute_tuning(struct mmc_host *mmc, u32 opcode)
 	return err;
 }
 
+static int dw_mci_prepare_hs400_tuning(struct mmc_host *mmc, struct mmc_ios *ios)
+{
+	struct dw_mci_slot *slot = mmc_priv(mmc);
+	struct dw_mci *host = slot->host;
+	const struct dw_mci_drv_data *drv_data = host->drv_data;
+
+	if (drv_data && drv_data->prepare_hs400_tuning)
+		return drv_data->prepare_hs400_tuning(host, ios);
+
+	return 0;
+}
+
 static const struct mmc_host_ops dw_mci_ops = {
 	.request		= dw_mci_request,
 	.pre_req		= dw_mci_pre_req,
@@ -1335,6 +1411,7 @@ static const struct mmc_host_ops dw_mci_ops = {
 	.card_busy		= dw_mci_card_busy,
 	.start_signal_voltage_switch = dw_mci_switch_voltage,
 	.init_card		= dw_mci_init_card,
+	.prepare_hs400_tuning	= dw_mci_prepare_hs400_tuning,
 };
 
 static void dw_mci_request_end(struct dw_mci *host, struct mmc_request *mrq)
@@ -1520,7 +1597,10 @@ static void dw_mci_tasklet_func(unsigned long priv)
 			if (test_and_clear_bit(EVENT_DATA_ERROR,
 					       &host->pending_events)) {
 				dw_mci_stop_dma(host);
-				send_stop_abort(host, data);
+				if (data->stop ||
+				    !(host->data_status & (SDMMC_INT_DRTO |
+							   SDMMC_INT_EBE)))
+					send_stop_abort(host, data);
 				state = STATE_DATA_ERROR;
 				break;
 			}
@@ -1547,7 +1627,10 @@ static void dw_mci_tasklet_func(unsigned long priv)
 			if (test_and_clear_bit(EVENT_DATA_ERROR,
 					       &host->pending_events)) {
 				dw_mci_stop_dma(host);
-				send_stop_abort(host, data);
+				if (data->stop ||
+				    !(host->data_status & (SDMMC_INT_DRTO |
+							   SDMMC_INT_EBE)))
+					send_stop_abort(host, data);
 				state = STATE_DATA_ERROR;
 				break;
 			}
@@ -1685,8 +1768,7 @@ static void dw_mci_push_data16(struct dw_mci *host, void *buf, int cnt)
 		buf += len;
 		cnt -= len;
 		if (host->part_buf_count == 2) {
-			mci_writew(host, DATA(host->data_offset),
-					host->part_buf16);
+			mci_fifo_writew(host->fifo_reg, host->part_buf16);
 			host->part_buf_count = 0;
 		}
 	}
@@ -1703,15 +1785,14 @@ static void dw_mci_push_data16(struct dw_mci *host, void *buf, int cnt)
 			cnt -= len;
 			/* push data from aligned buffer into fifo */
 			for (i = 0; i < items; ++i)
-				mci_writew(host, DATA(host->data_offset),
-						aligned_buf[i]);
+				mci_fifo_writew(host->fifo_reg, aligned_buf[i]);
 		}
 	} else
 #endif
 	{
 		u16 *pdata = buf;
 		for (; cnt >= 2; cnt -= 2)
-			mci_writew(host, DATA(host->data_offset), *pdata++);
+			mci_fifo_writew(host->fifo_reg, *pdata++);
 		buf = pdata;
 	}
 	/* put anything remaining in the part_buf */
@@ -1720,8 +1801,7 @@ static void dw_mci_push_data16(struct dw_mci *host, void *buf, int cnt)
 		 /* Push data if we have reached the expected data length */
 		if ((data->bytes_xfered + init_cnt) ==
 		    (data->blksz * data->blocks))
-			mci_writew(host, DATA(host->data_offset),
-				   host->part_buf16);
+			mci_fifo_writew(host->fifo_reg, host->part_buf16);
 	}
 }
 
@@ -1736,8 +1816,7 @@ static void dw_mci_pull_data16(struct dw_mci *host, void *buf, int cnt)
 			int items = len >> 1;
 			int i;
 			for (i = 0; i < items; ++i)
-				aligned_buf[i] = mci_readw(host,
-						DATA(host->data_offset));
+				aligned_buf[i] = mci_fifo_readw(host->fifo_reg);
 			/* memcpy from aligned buffer into output buffer */
 			memcpy(buf, aligned_buf, len);
 			buf += len;
@@ -1748,11 +1827,11 @@ static void dw_mci_pull_data16(struct dw_mci *host, void *buf, int cnt)
 	{
 		u16 *pdata = buf;
 		for (; cnt >= 2; cnt -= 2)
-			*pdata++ = mci_readw(host, DATA(host->data_offset));
+			*pdata++ = mci_fifo_readw(host->fifo_reg);
 		buf = pdata;
 	}
 	if (cnt) {
-		host->part_buf16 = mci_readw(host, DATA(host->data_offset));
+		host->part_buf16 = mci_fifo_readw(host->fifo_reg);
 		dw_mci_pull_final_bytes(host, buf, cnt);
 	}
 }
@@ -1768,8 +1847,7 @@ static void dw_mci_push_data32(struct dw_mci *host, void *buf, int cnt)
 		buf += len;
 		cnt -= len;
 		if (host->part_buf_count == 4) {
-			mci_writel(host, DATA(host->data_offset),
-					host->part_buf32);
+			mci_fifo_writel(host->fifo_reg,	host->part_buf32);
 			host->part_buf_count = 0;
 		}
 	}
@@ -1786,15 +1864,14 @@ static void dw_mci_push_data32(struct dw_mci *host, void *buf, int cnt)
 			cnt -= len;
 			/* push data from aligned buffer into fifo */
 			for (i = 0; i < items; ++i)
-				mci_writel(host, DATA(host->data_offset),
-						aligned_buf[i]);
+				mci_fifo_writel(host->fifo_reg,	aligned_buf[i]);
 		}
 	} else
 #endif
 	{
 		u32 *pdata = buf;
 		for (; cnt >= 4; cnt -= 4)
-			mci_writel(host, DATA(host->data_offset), *pdata++);
+			mci_fifo_writel(host->fifo_reg, *pdata++);
 		buf = pdata;
 	}
 	/* put anything remaining in the part_buf */
@@ -1803,8 +1880,7 @@ static void dw_mci_push_data32(struct dw_mci *host, void *buf, int cnt)
 		 /* Push data if we have reached the expected data length */
 		if ((data->bytes_xfered + init_cnt) ==
 		    (data->blksz * data->blocks))
-			mci_writel(host, DATA(host->data_offset),
-				   host->part_buf32);
+			mci_fifo_writel(host->fifo_reg, host->part_buf32);
 	}
 }
 
@@ -1819,8 +1895,7 @@ static void dw_mci_pull_data32(struct dw_mci *host, void *buf, int cnt)
 			int items = len >> 2;
 			int i;
 			for (i = 0; i < items; ++i)
-				aligned_buf[i] = mci_readl(host,
-						DATA(host->data_offset));
+				aligned_buf[i] = mci_fifo_readl(host->fifo_reg);
 			/* memcpy from aligned buffer into output buffer */
 			memcpy(buf, aligned_buf, len);
 			buf += len;
@@ -1831,11 +1906,11 @@ static void dw_mci_pull_data32(struct dw_mci *host, void *buf, int cnt)
 	{
 		u32 *pdata = buf;
 		for (; cnt >= 4; cnt -= 4)
-			*pdata++ = mci_readl(host, DATA(host->data_offset));
+			*pdata++ = mci_fifo_readl(host->fifo_reg);
 		buf = pdata;
 	}
 	if (cnt) {
-		host->part_buf32 = mci_readl(host, DATA(host->data_offset));
+		host->part_buf32 = mci_fifo_readl(host->fifo_reg);
 		dw_mci_pull_final_bytes(host, buf, cnt);
 	}
 }
@@ -1852,8 +1927,7 @@ static void dw_mci_push_data64(struct dw_mci *host, void *buf, int cnt)
 		cnt -= len;
 
 		if (host->part_buf_count == 8) {
-			mci_writeq(host, DATA(host->data_offset),
-					host->part_buf);
+			mci_fifo_writeq(host->fifo_reg,	host->part_buf);
 			host->part_buf_count = 0;
 		}
 	}
@@ -1870,15 +1944,14 @@ static void dw_mci_push_data64(struct dw_mci *host, void *buf, int cnt)
 			cnt -= len;
 			/* push data from aligned buffer into fifo */
 			for (i = 0; i < items; ++i)
-				mci_writeq(host, DATA(host->data_offset),
-						aligned_buf[i]);
+				mci_fifo_writeq(host->fifo_reg,	aligned_buf[i]);
 		}
 	} else
 #endif
 	{
 		u64 *pdata = buf;
 		for (; cnt >= 8; cnt -= 8)
-			mci_writeq(host, DATA(host->data_offset), *pdata++);
+			mci_fifo_writeq(host->fifo_reg, *pdata++);
 		buf = pdata;
 	}
 	/* put anything remaining in the part_buf */
@@ -1887,8 +1960,7 @@ static void dw_mci_push_data64(struct dw_mci *host, void *buf, int cnt)
 		/* Push data if we have reached the expected data length */
 		if ((data->bytes_xfered + init_cnt) ==
 		    (data->blksz * data->blocks))
-			mci_writeq(host, DATA(host->data_offset),
-				   host->part_buf);
+			mci_fifo_writeq(host->fifo_reg, host->part_buf);
 	}
 }
 
@@ -1903,8 +1975,8 @@ static void dw_mci_pull_data64(struct dw_mci *host, void *buf, int cnt)
 			int items = len >> 3;
 			int i;
 			for (i = 0; i < items; ++i)
-				aligned_buf[i] = mci_readq(host,
-						DATA(host->data_offset));
+				aligned_buf[i] = mci_fifo_readq(host->fifo_reg);
+
 			/* memcpy from aligned buffer into output buffer */
 			memcpy(buf, aligned_buf, len);
 			buf += len;
@@ -1915,11 +1987,11 @@ static void dw_mci_pull_data64(struct dw_mci *host, void *buf, int cnt)
 	{
 		u64 *pdata = buf;
 		for (; cnt >= 8; cnt -= 8)
-			*pdata++ = mci_readq(host, DATA(host->data_offset));
+			*pdata++ = mci_fifo_readq(host->fifo_reg);
 		buf = pdata;
 	}
 	if (cnt) {
-		host->part_buf = mci_readq(host, DATA(host->data_offset));
+		host->part_buf = mci_fifo_readq(host->fifo_reg);
 		dw_mci_pull_final_bytes(host, buf, cnt);
 	}
 }
@@ -2097,9 +2169,20 @@ static irqreturn_t dw_mci_interrupt(int irq, void *dev_id)
 		/* Check volt switch first, since it can look like an error */
 		if ((host->state == STATE_SENDING_CMD11) &&
 		    (pending & SDMMC_INT_VOLT_SWITCH)) {
+			unsigned long irqflags;
+
 			mci_writel(host, RINTSTS, SDMMC_INT_VOLT_SWITCH);
 			pending &= ~SDMMC_INT_VOLT_SWITCH;
+
+			/*
+			 * Hold the lock; we know cmd11_timer can't be kicked
+			 * off after the lock is released, so safe to delete.
+			 */
+			spin_lock_irqsave(&host->irq_lock, irqflags);
 			dw_mci_cmd_interrupt(host, pending);
+			spin_unlock_irqrestore(&host->irq_lock, irqflags);
+
+			del_timer(&host->cmd11_timer);
 		}
 
 		if (pending & DW_MCI_CMD_ERROR_FLAGS) {
@@ -2156,6 +2239,10 @@ static irqreturn_t dw_mci_interrupt(int irq, void *dev_id)
 		/* Handle SDIO Interrupts */
 		for (i = 0; i < host->num_slots; i++) {
 			struct dw_mci_slot *slot = host->slot[i];
+
+			if (!slot)
+				continue;
+
 			if (pending & SDMMC_INT_SDIO(slot->sdio_id)) {
 				mci_writel(host, RINTSTS,
 					   SDMMC_INT_SDIO(slot->sdio_id));
@@ -2506,6 +2593,20 @@ ciu_out:
 	return ret;
 }
 
+static void dw_mci_cmd11_timer(unsigned long arg)
+{
+	struct dw_mci *host = (struct dw_mci *)arg;
+
+	if (host->state != STATE_SENDING_CMD11) {
+		dev_warn(host->dev, "Unexpected CMD11 timeout\n");
+		return;
+	}
+
+	host->cmd_status = SDMMC_INT_RTO;
+	set_bit(EVENT_CMD_COMPLETE, &host->pending_events);
+	tasklet_schedule(&host->tasklet);
+}
+
 #ifdef CONFIG_OF
 static struct dw_mci_of_quirks {
 	char *quirk;
@@ -2574,6 +2675,34 @@ static struct dw_mci_board *dw_mci_parse_dt(struct dw_mci *host)
 }
 #endif /* CONFIG_OF */
 
+static void dw_mci_enable_cd(struct dw_mci *host)
+{
+	struct dw_mci_board *brd = host->pdata;
+	unsigned long irqflags;
+	u32 temp;
+	int i;
+
+	/* No need for CD if broken card detection */
+	if (brd->quirks & DW_MCI_QUIRK_BROKEN_CARD_DETECTION)
+		return;
+
+	/* No need for CD if all slots have a non-error GPIO */
+	for (i = 0; i < host->num_slots; i++) {
+		struct dw_mci_slot *slot = host->slot[i];
+
+		if (IS_ERR_VALUE(mmc_gpio_get_cd(slot->mmc)))
+			break;
+	}
+	if (i == host->num_slots)
+		return;
+
+	spin_lock_irqsave(&host->irq_lock, irqflags);
+	temp = mci_readl(host, INTMASK);
+	temp  |= SDMMC_INT_CD;
+	mci_writel(host, INTMASK, temp);
+	spin_unlock_irqrestore(&host->irq_lock, irqflags);
+}
+
 int dw_mci_probe(struct dw_mci *host)
 {
 	const struct dw_mci_drv_data *drv_data = host->drv_data;
@@ -2652,6 +2781,9 @@ int dw_mci_probe(struct dw_mci *host)
 		}
 	}
 
+	setup_timer(&host->cmd11_timer,
+		    dw_mci_cmd11_timer, (unsigned long)host);
+
 	host->quirks = host->pdata->quirks;
 
 	spin_lock_init(&host->lock);
@@ -2731,9 +2863,9 @@ int dw_mci_probe(struct dw_mci *host)
 	dev_info(host->dev, "Version ID is %04x\n", host->verid);
 
 	if (host->verid < DW_MMC_240A)
-		host->data_offset = DATA_OFFSET;
+		host->fifo_reg = host->regs + DATA_OFFSET;
 	else
-		host->data_offset = DATA_240A_OFFSET;
+		host->fifo_reg = host->regs + DATA_240A_OFFSET;
 
 	tasklet_init(&host->tasklet, dw_mci_tasklet_func, (unsigned long)host);
 	ret = devm_request_irq(host->dev, host->irq, dw_mci_interrupt,
@@ -2747,13 +2879,13 @@ int dw_mci_probe(struct dw_mci *host)
 		host->num_slots = ((mci_readl(host, HCON) >> 1) & 0x1F) + 1;
 
 	/*
-	 * Enable interrupts for command done, data over, data empty, card det,
+	 * Enable interrupts for command done, data over, data empty,
 	 * receive ready and error such as transmit, receive timeout, crc error
 	 */
 	mci_writel(host, RINTSTS, 0xFFFFFFFF);
 	mci_writel(host, INTMASK, SDMMC_INT_CMD_DONE | SDMMC_INT_DATA_OVER |
 		   SDMMC_INT_TXDR | SDMMC_INT_RXDR |
-		   DW_MCI_ERROR_FLAGS | SDMMC_INT_CD);
+		   DW_MCI_ERROR_FLAGS);
 	mci_writel(host, CTRL, SDMMC_CTRL_INT_ENABLE); /* Enable mci interrupt */
 
 	dev_info(host->dev, "DW MMC controller at irq %d, "
@@ -2778,6 +2910,9 @@ int dw_mci_probe(struct dw_mci *host)
 		goto err_dmaunmap;
 	}
 
+	/* Now that slots are all setup, we can enable card detect */
+	dw_mci_enable_cd(host);
+
 	if (host->quirks & DW_MCI_QUIRK_IDMAC_DTO)
 		dev_info(host->dev, "Internal DMAC interrupt fix enabled.\n");
 
@@ -2864,7 +2999,7 @@ int dw_mci_resume(struct dw_mci *host)
 	mci_writel(host, RINTSTS, 0xFFFFFFFF);
 	mci_writel(host, INTMASK, SDMMC_INT_CMD_DONE | SDMMC_INT_DATA_OVER |
 		   SDMMC_INT_TXDR | SDMMC_INT_RXDR |
-		   DW_MCI_ERROR_FLAGS | SDMMC_INT_CD);
+		   DW_MCI_ERROR_FLAGS);
 	mci_writel(host, CTRL, SDMMC_CTRL_INT_ENABLE);
 
 	for (i = 0; i < host->num_slots; i++) {
@@ -2876,6 +3011,10 @@ int dw_mci_resume(struct dw_mci *host)
 			dw_mci_setup_bus(slot, true);
 		}
 	}
+
+	/* Now that slots are all setup, we can enable card detect */
+	dw_mci_enable_cd(host);
+
 	return 0;
 }
 EXPORT_SYMBOL(dw_mci_resume);
diff --git a/drivers/mmc/host/dw_mmc.h b/drivers/mmc/host/dw_mmc.h
index 18c4afe683b8..f45ab91de339 100644
--- a/drivers/mmc/host/dw_mmc.h
+++ b/drivers/mmc/host/dw_mmc.h
@@ -169,24 +169,34 @@
 #define SDMMC_CTRL_ALL_RESET_FLAGS \
 	(SDMMC_CTRL_RESET | SDMMC_CTRL_FIFO_RESET | SDMMC_CTRL_DMA_RESET)
 
+/* FIFO register access macros. These should not change the data endian-ness
+ * as they are written to memory to be dealt with by the upper layers */
+#define mci_fifo_readw(__reg)	__raw_readw(__reg)
+#define mci_fifo_readl(__reg)	__raw_readl(__reg)
+#define mci_fifo_readq(__reg)	__raw_readq(__reg)
+
+#define mci_fifo_writew(__value, __reg)	__raw_writew(__reg, __value)
+#define mci_fifo_writel(__value, __reg)	__raw_writel(__reg, __value)
+#define mci_fifo_writeq(__value, __reg)	__raw_writeq(__reg, __value)
+
 /* Register access macros */
 #define mci_readl(dev, reg)			\
-	__raw_readl((dev)->regs + SDMMC_##reg)
+	readl_relaxed((dev)->regs + SDMMC_##reg)
 #define mci_writel(dev, reg, value)			\
-	__raw_writel((value), (dev)->regs + SDMMC_##reg)
+	writel_relaxed((value), (dev)->regs + SDMMC_##reg)
 
 /* 16-bit FIFO access macros */
 #define mci_readw(dev, reg)			\
-	__raw_readw((dev)->regs + SDMMC_##reg)
+	readw_relaxed((dev)->regs + SDMMC_##reg)
 #define mci_writew(dev, reg, value)			\
-	__raw_writew((value), (dev)->regs + SDMMC_##reg)
+	writew_relaxed((value), (dev)->regs + SDMMC_##reg)
 
 /* 64-bit FIFO access macros */
 #ifdef readq
 #define mci_readq(dev, reg)			\
-	__raw_readq((dev)->regs + SDMMC_##reg)
+	readq_relaxed((dev)->regs + SDMMC_##reg)
 #define mci_writeq(dev, reg, value)			\
-	__raw_writeq((value), (dev)->regs + SDMMC_##reg)
+	writeq_relaxed((value), (dev)->regs + SDMMC_##reg)
 #else
 /*
  * Dummy readq implementation for architectures that don't define it.
@@ -200,6 +210,10 @@
 	(*(volatile u64 __force *)((dev)->regs + SDMMC_##reg))
 #define mci_writeq(dev, reg, value)			\
 	(*(volatile u64 __force *)((dev)->regs + SDMMC_##reg) = (value))
+
+#define __raw_writeq(__value, __reg) \
+	(*(volatile u64 __force *)(__reg) = (__value))
+#define __raw_readq(__reg) (*(volatile u64 __force *)(__reg))
 #endif
 
 extern int dw_mci_probe(struct dw_mci *host);
@@ -271,5 +285,7 @@ struct dw_mci_drv_data {
 	void		(*set_ios)(struct dw_mci *host, struct mmc_ios *ios);
 	int		(*parse_dt)(struct dw_mci *host);
 	int		(*execute_tuning)(struct dw_mci_slot *slot);
+	int		(*prepare_hs400_tuning)(struct dw_mci *host,
+						struct mmc_ios *ios);
 };
 #endif /* _DW_MMC_H_ */
diff --git a/drivers/mmc/host/mmc_spi.c b/drivers/mmc/host/mmc_spi.c
index e4a07546f8b6..ae19d83bb9de 100644
--- a/drivers/mmc/host/mmc_spi.c
+++ b/drivers/mmc/host/mmc_spi.c
@@ -1507,7 +1507,7 @@ static int mmc_spi_remove(struct spi_device *spi)
 	return 0;
 }
 
-static struct of_device_id mmc_spi_of_match_table[] = {
+static const struct of_device_id mmc_spi_of_match_table[] = {
 	{ .compatible = "mmc-spi-slot", },
 	{},
 };
diff --git a/drivers/mmc/host/mmci.c b/drivers/mmc/host/mmci.c
index 7fe16194ebc8..fb266745f824 100644
--- a/drivers/mmc/host/mmci.c
+++ b/drivers/mmc/host/mmci.c
@@ -1613,7 +1613,10 @@ static int mmci_probe(struct amba_device *dev,
 	dev_dbg(mmc_dev(mmc), "clocking block at %u Hz\n", mmc->f_max);
 
 	/* Get regulators and the supported OCR mask */
-	mmc_regulator_get_supply(mmc);
+	ret = mmc_regulator_get_supply(mmc);
+	if (ret == -EPROBE_DEFER)
+		goto clk_disable;
+
 	if (!mmc->ocr_avail)
 		mmc->ocr_avail = plat->ocr_mask;
 	else if (plat->ocr_mask)
diff --git a/drivers/mmc/host/omap_hsmmc.c b/drivers/mmc/host/omap_hsmmc.c
index f84cfb01716d..9df2b6801f76 100644
--- a/drivers/mmc/host/omap_hsmmc.c
+++ b/drivers/mmc/host/omap_hsmmc.c
@@ -222,10 +222,6 @@ struct omap_hsmmc_host {
 	struct omap_hsmmc_next	next_data;
 	struct	omap_hsmmc_platform_data	*pdata;
 
-	/* To handle board related suspend/resume functionality for MMC */
-	int (*suspend)(struct device *dev);
-	int (*resume)(struct device *dev);
-
 	/* return MMC cover switch state, can be NULL if not supported.
 	 *
 	 * possible return values:
@@ -234,12 +230,7 @@ struct omap_hsmmc_host {
 	 */
 	int (*get_cover_state)(struct device *dev);
 
-	/* Card detection IRQs */
-	int card_detect_irq;
-
 	int (*card_detect)(struct device *dev);
-	int (*get_ro)(struct device *dev);
-
 };
 
 struct omap_mmc_of_data {
@@ -256,13 +247,6 @@ static int omap_hsmmc_card_detect(struct device *dev)
 	return mmc_gpio_get_cd(host->mmc);
 }
 
-static int omap_hsmmc_get_wp(struct device *dev)
-{
-	struct omap_hsmmc_host *host = dev_get_drvdata(dev);
-
-	return mmc_gpio_get_ro(host->mmc);
-}
-
 static int omap_hsmmc_get_cover_state(struct device *dev)
 {
 	struct omap_hsmmc_host *host = dev_get_drvdata(dev);
@@ -434,7 +418,7 @@ static inline int omap_hsmmc_have_reg(void)
 
 #endif
 
-static irqreturn_t omap_hsmmc_detect(int irq, void *dev_id);
+static irqreturn_t omap_hsmmc_cover_irq(int irq, void *dev_id);
 
 static int omap_hsmmc_gpio_init(struct mmc_host *mmc,
 				struct omap_hsmmc_host *host,
@@ -442,29 +426,25 @@ static int omap_hsmmc_gpio_init(struct mmc_host *mmc,
 {
 	int ret;
 
-	if (gpio_is_valid(pdata->switch_pin)) {
-		if (pdata->cover)
-			host->get_cover_state =
-				omap_hsmmc_get_cover_state;
-		else
-			host->card_detect = omap_hsmmc_card_detect;
-		host->card_detect_irq =
-				gpio_to_irq(pdata->switch_pin);
-		mmc_gpio_set_cd_isr(mmc, omap_hsmmc_detect);
-		ret = mmc_gpio_request_cd(mmc, pdata->switch_pin, 0);
+	if (gpio_is_valid(pdata->gpio_cod)) {
+		ret = mmc_gpio_request_cd(mmc, pdata->gpio_cod, 0);
 		if (ret)
 			return ret;
-	} else {
-		pdata->switch_pin = -EINVAL;
+
+		host->get_cover_state = omap_hsmmc_get_cover_state;
+		mmc_gpio_set_cd_isr(mmc, omap_hsmmc_cover_irq);
+	} else if (gpio_is_valid(pdata->gpio_cd)) {
+		ret = mmc_gpio_request_cd(mmc, pdata->gpio_cd, 0);
+		if (ret)
+			return ret;
+
+		host->card_detect = omap_hsmmc_card_detect;
 	}
 
 	if (gpio_is_valid(pdata->gpio_wp)) {
-		host->get_ro = omap_hsmmc_get_wp;
 		ret = mmc_gpio_request_ro(mmc, pdata->gpio_wp);
 		if (ret)
 			return ret;
-	} else {
-		pdata->gpio_wp = -EINVAL;
 	}
 
 	return 0;
@@ -882,6 +862,8 @@ static void omap_hsmmc_request_done(struct omap_hsmmc_host *host, struct mmc_req
 		return;
 	host->mrq = NULL;
 	mmc_request_done(host->mmc, mrq);
+	pm_runtime_mark_last_busy(host->dev);
+	pm_runtime_put_autosuspend(host->dev);
 }
 
 /*
@@ -1252,26 +1234,16 @@ static void omap_hsmmc_protect_card(struct omap_hsmmc_host *host)
 }
 
 /*
- * irq handler to notify the core about card insertion/removal
+ * irq handler when (cell-phone) cover is mounted/removed
  */
-static irqreturn_t omap_hsmmc_detect(int irq, void *dev_id)
+static irqreturn_t omap_hsmmc_cover_irq(int irq, void *dev_id)
 {
 	struct omap_hsmmc_host *host = dev_id;
-	int carddetect;
 
 	sysfs_notify(&host->mmc->class_dev.kobj, NULL, "cover_switch");
 
-	if (host->card_detect)
-		carddetect = host->card_detect(host->dev);
-	else {
-		omap_hsmmc_protect_card(host);
-		carddetect = -ENOSYS;
-	}
-
-	if (carddetect)
-		mmc_detect_change(host->mmc, (HZ * 200) / 1000);
-	else
-		mmc_detect_change(host->mmc, (HZ * 50) / 1000);
+	omap_hsmmc_protect_card(host);
+	mmc_detect_change(host->mmc, (HZ * 200) / 1000);
 	return IRQ_HANDLED;
 }
 
@@ -1305,6 +1277,8 @@ static void omap_hsmmc_dma_callback(void *param)
 
 		host->mrq = NULL;
 		mmc_request_done(host->mmc, mrq);
+		pm_runtime_mark_last_busy(host->dev);
+		pm_runtime_put_autosuspend(host->dev);
 	}
 }
 
@@ -1537,6 +1511,7 @@ static void omap_hsmmc_request(struct mmc_host *mmc, struct mmc_request *req)
 
 	BUG_ON(host->req_in_progress);
 	BUG_ON(host->dma_ch != -1);
+	pm_runtime_get_sync(host->dev);
 	if (host->protect_card) {
 		if (host->reqs_blocked < 3) {
 			/*
@@ -1553,6 +1528,8 @@ static void omap_hsmmc_request(struct mmc_host *mmc, struct mmc_request *req)
 			req->data->error = -EBADF;
 		req->cmd->retries = 0;
 		mmc_request_done(mmc, req);
+		pm_runtime_mark_last_busy(host->dev);
+		pm_runtime_put_autosuspend(host->dev);
 		return;
 	} else if (host->reqs_blocked)
 		host->reqs_blocked = 0;
@@ -1566,6 +1543,8 @@ static void omap_hsmmc_request(struct mmc_host *mmc, struct mmc_request *req)
 			req->data->error = err;
 		host->mrq = NULL;
 		mmc_request_done(mmc, req);
+		pm_runtime_mark_last_busy(host->dev);
+		pm_runtime_put_autosuspend(host->dev);
 		return;
 	}
 	if (req->sbc && !(host->flags & AUTO_CMD23)) {
@@ -1641,15 +1620,6 @@ static int omap_hsmmc_get_cd(struct mmc_host *mmc)
 	return host->card_detect(host->dev);
 }
 
-static int omap_hsmmc_get_ro(struct mmc_host *mmc)
-{
-	struct omap_hsmmc_host *host = mmc_priv(mmc);
-
-	if (!host->get_ro)
-		return -ENOSYS;
-	return host->get_ro(host->dev);
-}
-
 static void omap_hsmmc_init_card(struct mmc_host *mmc, struct mmc_card *card)
 {
 	struct omap_hsmmc_host *host = mmc_priv(mmc);
@@ -1778,25 +1748,6 @@ static void omap_hsmmc_conf_bus_power(struct omap_hsmmc_host *host)
 	set_sd_bus_power(host);
 }
 
-static int omap_hsmmc_enable_fclk(struct mmc_host *mmc)
-{
-	struct omap_hsmmc_host *host = mmc_priv(mmc);
-
-	pm_runtime_get_sync(host->dev);
-
-	return 0;
-}
-
-static int omap_hsmmc_disable_fclk(struct mmc_host *mmc)
-{
-	struct omap_hsmmc_host *host = mmc_priv(mmc);
-
-	pm_runtime_mark_last_busy(host->dev);
-	pm_runtime_put_autosuspend(host->dev);
-
-	return 0;
-}
-
 static int omap_hsmmc_multi_io_quirk(struct mmc_card *card,
 				     unsigned int direction, int blk_size)
 {
@@ -1808,14 +1759,12 @@ static int omap_hsmmc_multi_io_quirk(struct mmc_card *card,
 }
 
 static struct mmc_host_ops omap_hsmmc_ops = {
-	.enable = omap_hsmmc_enable_fclk,
-	.disable = omap_hsmmc_disable_fclk,
 	.post_req = omap_hsmmc_post_req,
 	.pre_req = omap_hsmmc_pre_req,
 	.request = omap_hsmmc_request,
 	.set_ios = omap_hsmmc_set_ios,
 	.get_cd = omap_hsmmc_get_cd,
-	.get_ro = omap_hsmmc_get_ro,
+	.get_ro = mmc_gpio_get_ro,
 	.init_card = omap_hsmmc_init_card,
 	.enable_sdio_irq = omap_hsmmc_enable_sdio_irq,
 };
@@ -1937,7 +1886,8 @@ static struct omap_hsmmc_platform_data *of_get_hsmmc_pdata(struct device *dev)
 	if (of_find_property(np, "ti,dual-volt", NULL))
 		pdata->controller_flags |= OMAP_HSMMC_SUPPORTS_DUAL_VOLT;
 
-	pdata->switch_pin = -EINVAL;
+	pdata->gpio_cd = -EINVAL;
+	pdata->gpio_cod = -EINVAL;
 	pdata->gpio_wp = -EINVAL;
 
 	if (of_find_property(np, "ti,non-removable", NULL)) {
@@ -2179,9 +2129,9 @@ static int omap_hsmmc_probe(struct platform_device *pdev)
 		if (ret < 0)
 			goto err_slot_name;
 	}
-	if (host->card_detect_irq && host->get_cover_state) {
+	if (host->get_cover_state) {
 		ret = device_create_file(&mmc->class_dev,
-					&dev_attr_cover_switch);
+					 &dev_attr_cover_switch);
 		if (ret < 0)
 			goto err_slot_name;
 	}
@@ -2236,7 +2186,7 @@ static int omap_hsmmc_remove(struct platform_device *pdev)
 	return 0;
 }
 
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 static int omap_hsmmc_suspend(struct device *dev)
 {
 	struct omap_hsmmc_host *host = dev_get_drvdata(dev);
@@ -2292,10 +2242,6 @@ static int omap_hsmmc_resume(struct device *dev)
 	pm_runtime_put_autosuspend(host->dev);
 	return 0;
 }
-
-#else
-#define omap_hsmmc_suspend	NULL
-#define omap_hsmmc_resume	NULL
 #endif
 
 static int omap_hsmmc_runtime_suspend(struct device *dev)
@@ -2376,8 +2322,7 @@ static int omap_hsmmc_runtime_resume(struct device *dev)
 }
 
 static struct dev_pm_ops omap_hsmmc_dev_pm_ops = {
-	.suspend	= omap_hsmmc_suspend,
-	.resume		= omap_hsmmc_resume,
+	SET_SYSTEM_SLEEP_PM_OPS(omap_hsmmc_suspend, omap_hsmmc_resume)
 	.runtime_suspend = omap_hsmmc_runtime_suspend,
 	.runtime_resume = omap_hsmmc_runtime_resume,
 };
diff --git a/drivers/mmc/host/sdhci-acpi.c b/drivers/mmc/host/sdhci-acpi.c
index a45ed39d062c..22d929fa3371 100644
--- a/drivers/mmc/host/sdhci-acpi.c
+++ b/drivers/mmc/host/sdhci-acpi.c
@@ -40,7 +40,6 @@
 #include <linux/mmc/host.h>
 #include <linux/mmc/pm.h>
 #include <linux/mmc/slot-gpio.h>
-#include <linux/mmc/sdhci.h>
 
 #include "sdhci.h"
 
diff --git a/drivers/mmc/host/sdhci-bcm-kona.c b/drivers/mmc/host/sdhci-bcm-kona.c
index 34bb8f92586e..2bd90fb35c75 100644
--- a/drivers/mmc/host/sdhci-bcm-kona.c
+++ b/drivers/mmc/host/sdhci-bcm-kona.c
@@ -54,7 +54,6 @@
 
 struct sdhci_bcm_kona_dev {
 	struct mutex	write_lock; /* protect back to back writes */
-	struct clk	*external_clk;
 };
 
 
@@ -175,24 +174,6 @@ static void sdhci_bcm_kona_card_event(struct sdhci_host *host)
 	}
 }
 
-/*
- * Get the base clock. Use central clock source for now. Not sure if different
- * clock speed to each dev is allowed
- */
-static unsigned int sdhci_bcm_kona_get_max_clk(struct sdhci_host *host)
-{
-	struct sdhci_bcm_kona_dev *kona_dev;
-	struct sdhci_pltfm_host *pltfm_priv = sdhci_priv(host);
-	kona_dev = sdhci_pltfm_priv(pltfm_priv);
-
-	return host->mmc->f_max;
-}
-
-static unsigned int sdhci_bcm_kona_get_timeout_clock(struct sdhci_host *host)
-{
-	return sdhci_bcm_kona_get_max_clk(host);
-}
-
 static void sdhci_bcm_kona_init_74_clocks(struct sdhci_host *host,
 				u8 power_mode)
 {
@@ -207,8 +188,8 @@ static void sdhci_bcm_kona_init_74_clocks(struct sdhci_host *host,
 
 static struct sdhci_ops sdhci_bcm_kona_ops = {
 	.set_clock = sdhci_set_clock,
-	.get_max_clock = sdhci_bcm_kona_get_max_clk,
-	.get_timeout_clock = sdhci_bcm_kona_get_timeout_clock,
+	.get_max_clock = sdhci_pltfm_clk_get_max_clock,
+	.get_timeout_clock = sdhci_pltfm_clk_get_max_clock,
 	.platform_send_init_74_clocks = sdhci_bcm_kona_init_74_clocks,
 	.set_bus_width = sdhci_set_bus_width,
 	.reset = sdhci_reset,
@@ -264,21 +245,21 @@ static int sdhci_bcm_kona_probe(struct platform_device *pdev)
 		goto err_pltfm_free;
 	}
 
-	/* Get and enable the external clock */
-	kona_dev->external_clk = devm_clk_get(dev, NULL);
-	if (IS_ERR(kona_dev->external_clk)) {
-		dev_err(dev, "Failed to get external clock\n");
-		ret = PTR_ERR(kona_dev->external_clk);
+	/* Get and enable the core clock */
+	pltfm_priv->clk = devm_clk_get(dev, NULL);
+	if (IS_ERR(pltfm_priv->clk)) {
+		dev_err(dev, "Failed to get core clock\n");
+		ret = PTR_ERR(pltfm_priv->clk);
 		goto err_pltfm_free;
 	}
 
-	if (clk_set_rate(kona_dev->external_clk, host->mmc->f_max) != 0) {
-		dev_err(dev, "Failed to set rate external clock\n");
+	if (clk_set_rate(pltfm_priv->clk, host->mmc->f_max) != 0) {
+		dev_err(dev, "Failed to set rate core clock\n");
 		goto err_pltfm_free;
 	}
 
-	if (clk_prepare_enable(kona_dev->external_clk) != 0) {
-		dev_err(dev, "Failed to enable external clock\n");
+	if (clk_prepare_enable(pltfm_priv->clk) != 0) {
+		dev_err(dev, "Failed to enable core clock\n");
 		goto err_pltfm_free;
 	}
 
@@ -333,7 +314,7 @@ err_reset:
 	sdhci_bcm_kona_sd_reset(host);
 
 err_clk_disable:
-	clk_disable_unprepare(kona_dev->external_clk);
+	clk_disable_unprepare(pltfm_priv->clk);
 
 err_pltfm_free:
 	sdhci_pltfm_free(pdev);
@@ -342,22 +323,6 @@ err_pltfm_free:
 	return ret;
 }
 
-static int sdhci_bcm_kona_remove(struct platform_device *pdev)
-{
-	struct sdhci_host *host = platform_get_drvdata(pdev);
-	struct sdhci_pltfm_host *pltfm_priv = sdhci_priv(host);
-	struct sdhci_bcm_kona_dev *kona_dev = sdhci_pltfm_priv(pltfm_priv);
-	int dead = (readl(host->ioaddr + SDHCI_INT_STATUS) == 0xffffffff);
-
-	sdhci_remove_host(host, dead);
-
-	clk_disable_unprepare(kona_dev->external_clk);
-
-	sdhci_pltfm_free(pdev);
-
-	return 0;
-}
-
 static struct platform_driver sdhci_bcm_kona_driver = {
 	.driver		= {
 		.name	= "sdhci-kona",
@@ -365,7 +330,7 @@ static struct platform_driver sdhci_bcm_kona_driver = {
 		.of_match_table = sdhci_bcm_kona_of_match,
 	},
 	.probe		= sdhci_bcm_kona_probe,
-	.remove		= sdhci_bcm_kona_remove,
+	.remove		= sdhci_pltfm_unregister,
 };
 module_platform_driver(sdhci_bcm_kona_driver);
 
diff --git a/drivers/mmc/host/sdhci-bcm2835.c b/drivers/mmc/host/sdhci-bcm2835.c
index 439d259fdf1d..0ef0343c603a 100644
--- a/drivers/mmc/host/sdhci-bcm2835.c
+++ b/drivers/mmc/host/sdhci-bcm2835.c
@@ -180,11 +180,6 @@ err:
 	return ret;
 }
 
-static int bcm2835_sdhci_remove(struct platform_device *pdev)
-{
-	return sdhci_pltfm_unregister(pdev);
-}
-
 static const struct of_device_id bcm2835_sdhci_of_match[] = {
 	{ .compatible = "brcm,bcm2835-sdhci" },
 	{ }
@@ -198,7 +193,7 @@ static struct platform_driver bcm2835_sdhci_driver = {
 		.pm = SDHCI_PLTFM_PMOPS,
 	},
 	.probe = bcm2835_sdhci_probe,
-	.remove = bcm2835_sdhci_remove,
+	.remove = sdhci_pltfm_unregister,
 };
 module_platform_driver(bcm2835_sdhci_driver);
 
diff --git a/drivers/mmc/host/sdhci-cns3xxx.c b/drivers/mmc/host/sdhci-cns3xxx.c
index a7935a8d0922..59f2923f8054 100644
--- a/drivers/mmc/host/sdhci-cns3xxx.c
+++ b/drivers/mmc/host/sdhci-cns3xxx.c
@@ -98,18 +98,13 @@ static int sdhci_cns3xxx_probe(struct platform_device *pdev)
 	return sdhci_pltfm_register(pdev, &sdhci_cns3xxx_pdata, 0);
 }
 
-static int sdhci_cns3xxx_remove(struct platform_device *pdev)
-{
-	return sdhci_pltfm_unregister(pdev);
-}
-
 static struct platform_driver sdhci_cns3xxx_driver = {
 	.driver		= {
 		.name	= "sdhci-cns3xxx",
 		.pm	= SDHCI_PLTFM_PMOPS,
 	},
 	.probe		= sdhci_cns3xxx_probe,
-	.remove		= sdhci_cns3xxx_remove,
+	.remove		= sdhci_pltfm_unregister,
 };
 
 module_platform_driver(sdhci_cns3xxx_driver);
diff --git a/drivers/mmc/host/sdhci-dove.c b/drivers/mmc/host/sdhci-dove.c
index ca969d271a27..407c21f152b2 100644
--- a/drivers/mmc/host/sdhci-dove.c
+++ b/drivers/mmc/host/sdhci-dove.c
@@ -28,10 +28,6 @@
 
 #include "sdhci-pltfm.h"
 
-struct sdhci_dove_priv {
-	struct clk *clk;
-};
-
 static u16 sdhci_dove_readw(struct sdhci_host *host, int reg)
 {
 	u16 ret;
@@ -84,27 +80,17 @@ static int sdhci_dove_probe(struct platform_device *pdev)
 {
 	struct sdhci_host *host;
 	struct sdhci_pltfm_host *pltfm_host;
-	struct sdhci_dove_priv *priv;
 	int ret;
 
-	priv = devm_kzalloc(&pdev->dev, sizeof(struct sdhci_dove_priv),
-			    GFP_KERNEL);
-	if (!priv) {
-		dev_err(&pdev->dev, "unable to allocate private data");
-		return -ENOMEM;
-	}
-
-	priv->clk = devm_clk_get(&pdev->dev, NULL);
-
 	host = sdhci_pltfm_init(pdev, &sdhci_dove_pdata, 0);
 	if (IS_ERR(host))
 		return PTR_ERR(host);
 
 	pltfm_host = sdhci_priv(host);
-	pltfm_host->priv = priv;
+	pltfm_host->clk = devm_clk_get(&pdev->dev, NULL);
 
-	if (!IS_ERR(priv->clk))
-		clk_prepare_enable(priv->clk);
+	if (!IS_ERR(pltfm_host->clk))
+		clk_prepare_enable(pltfm_host->clk);
 
 	ret = mmc_of_parse(host->mmc);
 	if (ret)
@@ -117,26 +103,11 @@ static int sdhci_dove_probe(struct platform_device *pdev)
 	return 0;
 
 err_sdhci_add:
-	if (!IS_ERR(priv->clk))
-		clk_disable_unprepare(priv->clk);
+	clk_disable_unprepare(pltfm_host->clk);
 	sdhci_pltfm_free(pdev);
 	return ret;
 }
 
-static int sdhci_dove_remove(struct platform_device *pdev)
-{
-	struct sdhci_host *host = platform_get_drvdata(pdev);
-	struct sdhci_pltfm_host *pltfm_host = sdhci_priv(host);
-	struct sdhci_dove_priv *priv = pltfm_host->priv;
-
-	sdhci_pltfm_unregister(pdev);
-
-	if (!IS_ERR(priv->clk))
-		clk_disable_unprepare(priv->clk);
-
-	return 0;
-}
-
 static const struct of_device_id sdhci_dove_of_match_table[] = {
 	{ .compatible = "marvell,dove-sdhci", },
 	{}
@@ -150,7 +121,7 @@ static struct platform_driver sdhci_dove_driver = {
 		.of_match_table = sdhci_dove_of_match_table,
 	},
 	.probe		= sdhci_dove_probe,
-	.remove		= sdhci_dove_remove,
+	.remove		= sdhci_pltfm_unregister,
 };
 
 module_platform_driver(sdhci_dove_driver);
diff --git a/drivers/mmc/host/sdhci-esdhc-imx.c b/drivers/mmc/host/sdhci-esdhc-imx.c
index 10ef8244a239..82f512d87cb8 100644
--- a/drivers/mmc/host/sdhci-esdhc-imx.c
+++ b/drivers/mmc/host/sdhci-esdhc-imx.c
@@ -416,7 +416,7 @@ static void esdhc_writew_le(struct sdhci_host *host, u16 val, int reg)
 			new_val |= ESDHC_VENDOR_SPEC_FRC_SDCLK_ON;
 		else
 			new_val &= ~ESDHC_VENDOR_SPEC_FRC_SDCLK_ON;
-			writel(new_val, host->ioaddr + ESDHC_VENDOR_SPEC);
+		writel(new_val, host->ioaddr + ESDHC_VENDOR_SPEC);
 		return;
 	case SDHCI_HOST_CONTROL2:
 		new_val = readl(host->ioaddr + ESDHC_VENDOR_SPEC);
@@ -864,6 +864,7 @@ static const struct sdhci_pltfm_data sdhci_esdhc_imx_pdata = {
 #ifdef CONFIG_OF
 static int
 sdhci_esdhc_imx_probe_dt(struct platform_device *pdev,
+			 struct sdhci_host *host,
 			 struct esdhc_platform_data *boarddata)
 {
 	struct device_node *np = pdev->dev.of_node;
@@ -900,11 +901,14 @@ sdhci_esdhc_imx_probe_dt(struct platform_device *pdev,
 	if (of_property_read_u32(np, "fsl,delay-line", &boarddata->delay_line))
 		boarddata->delay_line = 0;
 
+	mmc_of_parse_voltage(np, &host->ocr_mask);
+
 	return 0;
 }
 #else
 static inline int
 sdhci_esdhc_imx_probe_dt(struct platform_device *pdev,
+			 struct sdhci_host *host,
 			 struct esdhc_platform_data *boarddata)
 {
 	return -ENODEV;
@@ -999,7 +1003,7 @@ static int sdhci_esdhc_imx_probe(struct platform_device *pdev)
 			host->ioaddr + ESDHC_TUNING_CTRL);
 
 	boarddata = &imx_data->boarddata;
-	if (sdhci_esdhc_imx_probe_dt(pdev, boarddata) < 0) {
+	if (sdhci_esdhc_imx_probe_dt(pdev, host, boarddata) < 0) {
 		if (!host->mmc->parent->platform_data) {
 			dev_err(mmc_dev(host->mmc), "no board data!\n");
 			err = -EINVAL;
@@ -1009,40 +1013,9 @@ static int sdhci_esdhc_imx_probe(struct platform_device *pdev)
 					host->mmc->parent->platform_data);
 	}
 
-	/* write_protect */
-	if (boarddata->wp_type == ESDHC_WP_GPIO) {
-		err = mmc_gpio_request_ro(host->mmc, boarddata->wp_gpio);
-		if (err) {
-			dev_err(mmc_dev(host->mmc),
-				"failed to request write-protect gpio!\n");
-			goto disable_clk;
-		}
-		host->mmc->caps2 |= MMC_CAP2_RO_ACTIVE_HIGH;
-	}
-
 	/* card_detect */
-	switch (boarddata->cd_type) {
-	case ESDHC_CD_GPIO:
-		err = mmc_gpio_request_cd(host->mmc, boarddata->cd_gpio, 0);
-		if (err) {
-			dev_err(mmc_dev(host->mmc),
-				"failed to request card-detect gpio!\n");
-			goto disable_clk;
-		}
-		/* fall through */
-
-	case ESDHC_CD_CONTROLLER:
-		/* we have a working card_detect back */
+	if (boarddata->cd_type == ESDHC_CD_CONTROLLER)
 		host->quirks &= ~SDHCI_QUIRK_BROKEN_CARD_DETECTION;
-		break;
-
-	case ESDHC_CD_PERMANENT:
-		host->mmc->caps |= MMC_CAP_NONREMOVABLE;
-		break;
-
-	case ESDHC_CD_NONE:
-		break;
-	}
 
 	switch (boarddata->max_bus_width) {
 	case 8:
@@ -1075,6 +1048,11 @@ static int sdhci_esdhc_imx_probe(struct platform_device *pdev)
 		host->quirks2 |= SDHCI_QUIRK2_NO_1_8_V;
 	}
 
+	/* call to generic mmc_of_parse to support additional capabilities */
+	err = mmc_of_parse(host->mmc);
+	if (err)
+		goto disable_clk;
+
 	err = sdhci_add_host(host);
 	if (err)
 		goto disable_clk;
diff --git a/drivers/mmc/host/sdhci-iproc.c b/drivers/mmc/host/sdhci-iproc.c
new file mode 100644
index 000000000000..3b423b0ad8e7
--- /dev/null
+++ b/drivers/mmc/host/sdhci-iproc.c
@@ -0,0 +1,241 @@
+/*
+ * Copyright (C) 2014 Broadcom Corporation
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation version 2.
+ *
+ * This program is distributed "as is" WITHOUT ANY WARRANTY of any
+ * kind, whether express or implied; without even the implied warranty
+ * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+/*
+ * iProc SDHCI platform driver
+ */
+
+#include <linux/delay.h>
+#include <linux/module.h>
+#include <linux/mmc/host.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include "sdhci-pltfm.h"
+
+struct sdhci_iproc_data {
+	const struct sdhci_pltfm_data *pdata;
+	u32 caps;
+	u32 caps1;
+};
+
+struct sdhci_iproc_host {
+	const struct sdhci_iproc_data *data;
+	u32 shadow_cmd;
+	u32 shadow_blk;
+};
+
+#define REG_OFFSET_IN_BITS(reg) ((reg) << 3 & 0x18)
+
+static inline u32 sdhci_iproc_readl(struct sdhci_host *host, int reg)
+{
+	u32 val = readl(host->ioaddr + reg);
+
+	pr_debug("%s: readl [0x%02x] 0x%08x\n",
+		 mmc_hostname(host->mmc), reg, val);
+	return val;
+}
+
+static u16 sdhci_iproc_readw(struct sdhci_host *host, int reg)
+{
+	u32 val = sdhci_iproc_readl(host, (reg & ~3));
+	u16 word = val >> REG_OFFSET_IN_BITS(reg) & 0xffff;
+	return word;
+}
+
+static u8 sdhci_iproc_readb(struct sdhci_host *host, int reg)
+{
+	u32 val = sdhci_iproc_readl(host, (reg & ~3));
+	u8 byte = val >> REG_OFFSET_IN_BITS(reg) & 0xff;
+	return byte;
+}
+
+static inline void sdhci_iproc_writel(struct sdhci_host *host, u32 val, int reg)
+{
+	pr_debug("%s: writel [0x%02x] 0x%08x\n",
+		 mmc_hostname(host->mmc), reg, val);
+
+	writel(val, host->ioaddr + reg);
+
+	if (host->clock <= 400000) {
+		/* Round up to micro-second four SD clock delay */
+		if (host->clock)
+			udelay((4 * 1000000 + host->clock - 1) / host->clock);
+		else
+			udelay(10);
+	}
+}
+
+/*
+ * The Arasan has a bugette whereby it may lose the content of successive
+ * writes to the same register that are within two SD-card clock cycles of
+ * each other (a clock domain crossing problem). The data
+ * register does not have this problem, which is just as well - otherwise we'd
+ * have to nobble the DMA engine too.
+ *
+ * This wouldn't be a problem with the code except that we can only write the
+ * controller with 32-bit writes.  So two different 16-bit registers are
+ * written back to back creates the problem.
+ *
+ * In reality, this only happens when SDHCI_BLOCK_SIZE and SDHCI_BLOCK_COUNT
+ * are written followed by SDHCI_TRANSFER_MODE and SDHCI_COMMAND.
+ * The BLOCK_SIZE and BLOCK_COUNT are meaningless until a command issued so
+ * the work around can be further optimized. We can keep shadow values of
+ * BLOCK_SIZE, BLOCK_COUNT, and TRANSFER_MODE until a COMMAND is issued.
+ * Then, write the BLOCK_SIZE+BLOCK_COUNT in a single 32-bit write followed
+ * by the TRANSFER+COMMAND in another 32-bit write.
+ */
+static void sdhci_iproc_writew(struct sdhci_host *host, u16 val, int reg)
+{
+	struct sdhci_pltfm_host *pltfm_host = sdhci_priv(host);
+	struct sdhci_iproc_host *iproc_host = sdhci_pltfm_priv(pltfm_host);
+	u32 word_shift = REG_OFFSET_IN_BITS(reg);
+	u32 mask = 0xffff << word_shift;
+	u32 oldval, newval;
+
+	if (reg == SDHCI_COMMAND) {
+		/* Write the block now as we are issuing a command */
+		if (iproc_host->shadow_blk != 0) {
+			sdhci_iproc_writel(host, iproc_host->shadow_blk,
+				SDHCI_BLOCK_SIZE);
+			iproc_host->shadow_blk = 0;
+		}
+		oldval = iproc_host->shadow_cmd;
+	} else if (reg == SDHCI_BLOCK_SIZE || reg == SDHCI_BLOCK_COUNT) {
+		/* Block size and count are stored in shadow reg */
+		oldval = iproc_host->shadow_blk;
+	} else {
+		/* Read reg, all other registers are not shadowed */
+		oldval = sdhci_iproc_readl(host, (reg & ~3));
+	}
+	newval = (oldval & ~mask) | (val << word_shift);
+
+	if (reg == SDHCI_TRANSFER_MODE) {
+		/* Save the transfer mode until the command is issued */
+		iproc_host->shadow_cmd = newval;
+	} else if (reg == SDHCI_BLOCK_SIZE || reg == SDHCI_BLOCK_COUNT) {
+		/* Save the block info until the command is issued */
+		iproc_host->shadow_blk = newval;
+	} else {
+		/* Command or other regular 32-bit write */
+		sdhci_iproc_writel(host, newval, reg & ~3);
+	}
+}
+
+static void sdhci_iproc_writeb(struct sdhci_host *host, u8 val, int reg)
+{
+	u32 oldval = sdhci_iproc_readl(host, (reg & ~3));
+	u32 byte_shift = REG_OFFSET_IN_BITS(reg);
+	u32 mask = 0xff << byte_shift;
+	u32 newval = (oldval & ~mask) | (val << byte_shift);
+
+	sdhci_iproc_writel(host, newval, reg & ~3);
+}
+
+static const struct sdhci_ops sdhci_iproc_ops = {
+	.read_l = sdhci_iproc_readl,
+	.read_w = sdhci_iproc_readw,
+	.read_b = sdhci_iproc_readb,
+	.write_l = sdhci_iproc_writel,
+	.write_w = sdhci_iproc_writew,
+	.write_b = sdhci_iproc_writeb,
+	.set_clock = sdhci_set_clock,
+	.get_max_clock = sdhci_pltfm_clk_get_max_clock,
+	.set_bus_width = sdhci_set_bus_width,
+	.reset = sdhci_reset,
+	.set_uhs_signaling = sdhci_set_uhs_signaling,
+};
+
+static const struct sdhci_pltfm_data sdhci_iproc_pltfm_data = {
+	.quirks = SDHCI_QUIRK_DATA_TIMEOUT_USES_SDCLK,
+	.quirks2 = SDHCI_QUIRK2_ACMD23_BROKEN,
+	.ops = &sdhci_iproc_ops,
+};
+
+static const struct sdhci_iproc_data iproc_data = {
+	.pdata = &sdhci_iproc_pltfm_data,
+	.caps = 0x05E90000,
+	.caps1 = 0x00000064,
+};
+
+static const struct of_device_id sdhci_iproc_of_match[] = {
+	{ .compatible = "brcm,sdhci-iproc-cygnus", .data = &iproc_data },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, sdhci_iproc_of_match);
+
+static int sdhci_iproc_probe(struct platform_device *pdev)
+{
+	const struct of_device_id *match;
+	const struct sdhci_iproc_data *iproc_data;
+	struct sdhci_host *host;
+	struct sdhci_iproc_host *iproc_host;
+	struct sdhci_pltfm_host *pltfm_host;
+	int ret;
+
+	match = of_match_device(sdhci_iproc_of_match, &pdev->dev);
+	if (!match)
+		return -EINVAL;
+	iproc_data = match->data;
+
+	host = sdhci_pltfm_init(pdev, iproc_data->pdata, sizeof(*iproc_host));
+	if (IS_ERR(host))
+		return PTR_ERR(host);
+
+	pltfm_host = sdhci_priv(host);
+	iproc_host = sdhci_pltfm_priv(pltfm_host);
+
+	iproc_host->data = iproc_data;
+
+	mmc_of_parse(host->mmc);
+	sdhci_get_of_property(pdev);
+
+	/* Enable EMMC 1/8V DDR capable */
+	host->mmc->caps |= MMC_CAP_1_8V_DDR;
+
+	pltfm_host->clk = devm_clk_get(&pdev->dev, NULL);
+	if (IS_ERR(pltfm_host->clk)) {
+		ret = PTR_ERR(pltfm_host->clk);
+		goto err;
+	}
+
+	if (iproc_host->data->pdata->quirks & SDHCI_QUIRK_MISSING_CAPS) {
+		host->caps = iproc_host->data->caps;
+		host->caps1 = iproc_host->data->caps1;
+	}
+
+	return sdhci_add_host(host);
+
+err:
+	sdhci_pltfm_free(pdev);
+	return ret;
+}
+
+static int sdhci_iproc_remove(struct platform_device *pdev)
+{
+	return sdhci_pltfm_unregister(pdev);
+}
+
+static struct platform_driver sdhci_iproc_driver = {
+	.driver = {
+		.name = "sdhci-iproc",
+		.of_match_table = sdhci_iproc_of_match,
+		.pm = SDHCI_PLTFM_PMOPS,
+	},
+	.probe = sdhci_iproc_probe,
+	.remove = sdhci_iproc_remove,
+};
+module_platform_driver(sdhci_iproc_driver);
+
+MODULE_AUTHOR("Broadcom");
+MODULE_DESCRIPTION("IPROC SDHCI driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/mmc/host/sdhci-msm.c b/drivers/mmc/host/sdhci-msm.c
index 3d32ce896b09..4a09f7608c66 100644
--- a/drivers/mmc/host/sdhci-msm.c
+++ b/drivers/mmc/host/sdhci-msm.c
@@ -22,6 +22,11 @@
 
 #include "sdhci-pltfm.h"
 
+#define CORE_MCI_VERSION		0x50
+#define CORE_VERSION_MAJOR_SHIFT	28
+#define CORE_VERSION_MAJOR_MASK		(0xf << CORE_VERSION_MAJOR_SHIFT)
+#define CORE_VERSION_MINOR_MASK		0xff
+
 #define CORE_HC_MODE		0x78
 #define HC_MODE_EN		0x1
 #define CORE_POWER		0x0
@@ -41,6 +46,8 @@
 #define CORE_VENDOR_SPEC	0x10c
 #define CORE_CLK_PWRSAVE	BIT(1)
 
+#define CORE_VENDOR_SPEC_CAPABILITIES0	0x11c
+
 #define CDR_SELEXT_SHIFT	20
 #define CDR_SELEXT_MASK		(0xf << CDR_SELEXT_SHIFT)
 #define CMUX_SHIFT_PHASE_SHIFT	24
@@ -426,7 +433,9 @@ static int sdhci_msm_probe(struct platform_device *pdev)
 	struct sdhci_msm_host *msm_host;
 	struct resource *core_memres;
 	int ret;
-	u16 host_version;
+	u16 host_version, core_minor;
+	u32 core_version, caps;
+	u8 core_major;
 
 	msm_host = devm_kzalloc(&pdev->dev, sizeof(*msm_host), GFP_KERNEL);
 	if (!msm_host)
@@ -516,6 +525,24 @@ static int sdhci_msm_probe(struct platform_device *pdev)
 		host_version, ((host_version & SDHCI_VENDOR_VER_MASK) >>
 			       SDHCI_VENDOR_VER_SHIFT));
 
+	core_version = readl_relaxed(msm_host->core_mem + CORE_MCI_VERSION);
+	core_major = (core_version & CORE_VERSION_MAJOR_MASK) >>
+		      CORE_VERSION_MAJOR_SHIFT;
+	core_minor = core_version & CORE_VERSION_MINOR_MASK;
+	dev_dbg(&pdev->dev, "MCI Version: 0x%08x, major: 0x%04x, minor: 0x%02x\n",
+		core_version, core_major, core_minor);
+
+	/*
+	 * Support for some capabilities is not advertised by newer
+	 * controller versions and must be explicitly enabled.
+	 */
+	if (core_major >= 1 && core_minor != 0x11 && core_minor != 0x12) {
+		caps = readl_relaxed(host->ioaddr + SDHCI_CAPABILITIES);
+		caps |= SDHCI_CAN_VDD_300 | SDHCI_CAN_DO_8BIT;
+		writel_relaxed(caps, host->ioaddr +
+			       CORE_VENDOR_SPEC_CAPABILITIES0);
+	}
+
 	ret = sdhci_add_host(host);
 	if (ret)
 		goto clk_disable;
diff --git a/drivers/mmc/host/sdhci-of-arasan.c b/drivers/mmc/host/sdhci-of-arasan.c
index bcb51e9dfdcd..6287d426c96b 100644
--- a/drivers/mmc/host/sdhci-of-arasan.c
+++ b/drivers/mmc/host/sdhci-of-arasan.c
@@ -173,6 +173,12 @@ static int sdhci_arasan_probe(struct platform_device *pdev)
 	pltfm_host->priv = sdhci_arasan;
 	pltfm_host->clk = clk_xin;
 
+	ret = mmc_of_parse(host->mmc);
+	if (ret) {
+		dev_err(&pdev->dev, "parsing dt failed (%u)\n", ret);
+		goto clk_disable_all;
+	}
+
 	ret = sdhci_add_host(host);
 	if (ret)
 		goto err_pltfm_free;
@@ -195,7 +201,6 @@ static int sdhci_arasan_remove(struct platform_device *pdev)
 	struct sdhci_pltfm_host *pltfm_host = sdhci_priv(host);
 	struct sdhci_arasan_data *sdhci_arasan = pltfm_host->priv;
 
-	clk_disable_unprepare(pltfm_host->clk);
 	clk_disable_unprepare(sdhci_arasan->clk_ahb);
 
 	return sdhci_pltfm_unregister(pdev);
diff --git a/drivers/mmc/host/sdhci-of-esdhc.c b/drivers/mmc/host/sdhci-of-esdhc.c
index 17fe02ed6672..22e9111b11ff 100644
--- a/drivers/mmc/host/sdhci-of-esdhc.c
+++ b/drivers/mmc/host/sdhci-of-esdhc.c
@@ -386,11 +386,6 @@ static int sdhci_esdhc_probe(struct platform_device *pdev)
 	return ret;
 }
 
-static int sdhci_esdhc_remove(struct platform_device *pdev)
-{
-	return sdhci_pltfm_unregister(pdev);
-}
-
 static const struct of_device_id sdhci_esdhc_of_match[] = {
 	{ .compatible = "fsl,mpc8379-esdhc" },
 	{ .compatible = "fsl,mpc8536-esdhc" },
@@ -406,7 +401,7 @@ static struct platform_driver sdhci_esdhc_driver = {
 		.pm = ESDHC_PMOPS,
 	},
 	.probe = sdhci_esdhc_probe,
-	.remove = sdhci_esdhc_remove,
+	.remove = sdhci_pltfm_unregister,
 };
 
 module_platform_driver(sdhci_esdhc_driver);
diff --git a/drivers/mmc/host/sdhci-of-hlwd.c b/drivers/mmc/host/sdhci-of-hlwd.c
index be479279a1d5..4079a96ad37e 100644
--- a/drivers/mmc/host/sdhci-of-hlwd.c
+++ b/drivers/mmc/host/sdhci-of-hlwd.c
@@ -75,11 +75,6 @@ static int sdhci_hlwd_probe(struct platform_device *pdev)
 	return sdhci_pltfm_register(pdev, &sdhci_hlwd_pdata, 0);
 }
 
-static int sdhci_hlwd_remove(struct platform_device *pdev)
-{
-	return sdhci_pltfm_unregister(pdev);
-}
-
 static const struct of_device_id sdhci_hlwd_of_match[] = {
 	{ .compatible = "nintendo,hollywood-sdhci" },
 	{ }
@@ -93,7 +88,7 @@ static struct platform_driver sdhci_hlwd_driver = {
 		.pm = SDHCI_PLTFM_PMOPS,
 	},
 	.probe = sdhci_hlwd_probe,
-	.remove = sdhci_hlwd_remove,
+	.remove = sdhci_pltfm_unregister,
 };
 
 module_platform_driver(sdhci_hlwd_driver);
diff --git a/drivers/mmc/host/sdhci-pci.c b/drivers/mmc/host/sdhci-pci.c
index 29eaff78238e..7a3fc16d0a6c 100644
--- a/drivers/mmc/host/sdhci-pci.c
+++ b/drivers/mmc/host/sdhci-pci.c
@@ -650,6 +650,7 @@ static int rtsx_probe_slot(struct sdhci_pci_slot *slot)
 
 static const struct sdhci_pci_fixes sdhci_rtsx = {
 	.quirks2	= SDHCI_QUIRK2_PRESET_VALUE_BROKEN |
+			SDHCI_QUIRK2_BROKEN_64_BIT_DMA |
 			SDHCI_QUIRK2_BROKEN_DDR50,
 	.probe_slot	= rtsx_probe_slot,
 };
diff --git a/drivers/mmc/host/sdhci-pltfm.c b/drivers/mmc/host/sdhci-pltfm.c
index c5b01d6bb85d..a207f5aaf62f 100644
--- a/drivers/mmc/host/sdhci-pltfm.c
+++ b/drivers/mmc/host/sdhci-pltfm.c
@@ -75,43 +75,41 @@ void sdhci_get_of_property(struct platform_device *pdev)
 	u32 bus_width;
 	int size;
 
-	if (of_device_is_available(np)) {
-		if (of_get_property(np, "sdhci,auto-cmd12", NULL))
-			host->quirks |= SDHCI_QUIRK_MULTIBLOCK_READ_ACMD12;
+	if (of_get_property(np, "sdhci,auto-cmd12", NULL))
+		host->quirks |= SDHCI_QUIRK_MULTIBLOCK_READ_ACMD12;
 
-		if (of_get_property(np, "sdhci,1-bit-only", NULL) ||
-		    (of_property_read_u32(np, "bus-width", &bus_width) == 0 &&
-		    bus_width == 1))
-			host->quirks |= SDHCI_QUIRK_FORCE_1_BIT_DATA;
+	if (of_get_property(np, "sdhci,1-bit-only", NULL) ||
+	    (of_property_read_u32(np, "bus-width", &bus_width) == 0 &&
+	    bus_width == 1))
+		host->quirks |= SDHCI_QUIRK_FORCE_1_BIT_DATA;
 
-		if (sdhci_of_wp_inverted(np))
-			host->quirks |= SDHCI_QUIRK_INVERTED_WRITE_PROTECT;
+	if (sdhci_of_wp_inverted(np))
+		host->quirks |= SDHCI_QUIRK_INVERTED_WRITE_PROTECT;
 
-		if (of_get_property(np, "broken-cd", NULL))
-			host->quirks |= SDHCI_QUIRK_BROKEN_CARD_DETECTION;
+	if (of_get_property(np, "broken-cd", NULL))
+		host->quirks |= SDHCI_QUIRK_BROKEN_CARD_DETECTION;
 
-		if (of_get_property(np, "no-1-8-v", NULL))
-			host->quirks2 |= SDHCI_QUIRK2_NO_1_8_V;
+	if (of_get_property(np, "no-1-8-v", NULL))
+		host->quirks2 |= SDHCI_QUIRK2_NO_1_8_V;
 
-		if (of_device_is_compatible(np, "fsl,p2020-rev1-esdhc"))
-			host->quirks |= SDHCI_QUIRK_BROKEN_DMA;
+	if (of_device_is_compatible(np, "fsl,p2020-rev1-esdhc"))
+		host->quirks |= SDHCI_QUIRK_BROKEN_DMA;
 
-		if (of_device_is_compatible(np, "fsl,p2020-esdhc") ||
-		    of_device_is_compatible(np, "fsl,p1010-esdhc") ||
-		    of_device_is_compatible(np, "fsl,t4240-esdhc") ||
-		    of_device_is_compatible(np, "fsl,mpc8536-esdhc"))
-			host->quirks |= SDHCI_QUIRK_BROKEN_TIMEOUT_VAL;
+	if (of_device_is_compatible(np, "fsl,p2020-esdhc") ||
+	    of_device_is_compatible(np, "fsl,p1010-esdhc") ||
+	    of_device_is_compatible(np, "fsl,t4240-esdhc") ||
+	    of_device_is_compatible(np, "fsl,mpc8536-esdhc"))
+		host->quirks |= SDHCI_QUIRK_BROKEN_TIMEOUT_VAL;
 
-		clk = of_get_property(np, "clock-frequency", &size);
-		if (clk && size == sizeof(*clk) && *clk)
-			pltfm_host->clock = be32_to_cpup(clk);
+	clk = of_get_property(np, "clock-frequency", &size);
+	if (clk && size == sizeof(*clk) && *clk)
+		pltfm_host->clock = be32_to_cpup(clk);
 
-		if (of_find_property(np, "keep-power-in-suspend", NULL))
-			host->mmc->pm_caps |= MMC_PM_KEEP_POWER;
+	if (of_find_property(np, "keep-power-in-suspend", NULL))
+		host->mmc->pm_caps |= MMC_PM_KEEP_POWER;
 
-		if (of_find_property(np, "enable-sdio-wakeup", NULL))
-			host->mmc->pm_caps |= MMC_PM_WAKE_SDIO_IRQ;
-	}
+	if (of_find_property(np, "enable-sdio-wakeup", NULL))
+		host->mmc->pm_caps |= MMC_PM_WAKE_SDIO_IRQ;
 }
 #else
 void sdhci_get_of_property(struct platform_device *pdev) {}
@@ -225,9 +223,11 @@ EXPORT_SYMBOL_GPL(sdhci_pltfm_register);
 int sdhci_pltfm_unregister(struct platform_device *pdev)
 {
 	struct sdhci_host *host = platform_get_drvdata(pdev);
+	struct sdhci_pltfm_host *pltfm_host = sdhci_priv(host);
 	int dead = (readl(host->ioaddr + SDHCI_INT_STATUS) == 0xffffffff);
 
 	sdhci_remove_host(host, dead);
+	clk_disable_unprepare(pltfm_host->clk);
 	sdhci_pltfm_free(pdev);
 
 	return 0;
diff --git a/drivers/mmc/host/sdhci-sirf.c b/drivers/mmc/host/sdhci-sirf.c
index f6f82ec3618d..32848eb7ad80 100644
--- a/drivers/mmc/host/sdhci-sirf.c
+++ b/drivers/mmc/host/sdhci-sirf.c
@@ -20,17 +20,9 @@
 #define SIRF_TUNING_COUNT 128
 
 struct sdhci_sirf_priv {
-	struct clk *clk;
 	int gpio_cd;
 };
 
-static unsigned int sdhci_sirf_get_max_clk(struct sdhci_host *host)
-{
-	struct sdhci_pltfm_host *pltfm_host = sdhci_priv(host);
-	struct sdhci_sirf_priv *priv = sdhci_pltfm_priv(pltfm_host);
-	return clk_get_rate(priv->clk);
-}
-
 static void sdhci_sirf_set_bus_width(struct sdhci_host *host, int width)
 {
 	u8 ctrl;
@@ -56,7 +48,7 @@ static int sdhci_sirf_execute_tuning(struct sdhci_host *host, u32 opcode)
 	int tuning_seq_cnt = 3;
 	u8 phase, tuned_phases[SIRF_TUNING_COUNT];
 	u8 tuned_phase_cnt = 0;
-	int rc, longest_range = 0;
+	int rc = 0, longest_range = 0;
 	int start = -1, end = 0, tuning_value = -1, range = 0;
 	u16 clock_setting;
 	struct mmc_host *mmc = host->mmc;
@@ -68,7 +60,7 @@ retry:
 	phase = 0;
 	do {
 		sdhci_writel(host,
-			clock_setting | phase | (phase << 7) | (phase << 16),
+			clock_setting | phase,
 			SDHCI_CLK_DELAY_SETTING);
 
 		if (!mmc_send_tuning(mmc)) {
@@ -102,7 +94,7 @@ retry:
 		 */
 		phase = tuning_value;
 		sdhci_writel(host,
-			clock_setting | phase | (phase << 7) | (phase << 16),
+			clock_setting | phase,
 			SDHCI_CLK_DELAY_SETTING);
 
 		dev_dbg(mmc_dev(mmc), "%s: Setting the tuning phase to %d\n",
@@ -122,7 +114,7 @@ retry:
 static struct sdhci_ops sdhci_sirf_ops = {
 	.platform_execute_tuning = sdhci_sirf_execute_tuning,
 	.set_clock = sdhci_set_clock,
-	.get_max_clock	= sdhci_sirf_get_max_clk,
+	.get_max_clock	= sdhci_pltfm_clk_get_max_clock,
 	.set_bus_width = sdhci_sirf_set_bus_width,
 	.reset = sdhci_reset,
 	.set_uhs_signaling = sdhci_set_uhs_signaling,
@@ -162,13 +154,13 @@ static int sdhci_sirf_probe(struct platform_device *pdev)
 		return PTR_ERR(host);
 
 	pltfm_host = sdhci_priv(host);
+	pltfm_host->clk = clk;
 	priv = sdhci_pltfm_priv(pltfm_host);
-	priv->clk = clk;
 	priv->gpio_cd = gpio_cd;
 
 	sdhci_get_of_property(pdev);
 
-	ret = clk_prepare_enable(priv->clk);
+	ret = clk_prepare_enable(pltfm_host->clk);
 	if (ret)
 		goto err_clk_prepare;
 
@@ -195,37 +187,24 @@ static int sdhci_sirf_probe(struct platform_device *pdev)
 err_request_cd:
 	sdhci_remove_host(host, 0);
 err_sdhci_add:
-	clk_disable_unprepare(priv->clk);
+	clk_disable_unprepare(pltfm_host->clk);
 err_clk_prepare:
 	sdhci_pltfm_free(pdev);
 	return ret;
 }
 
-static int sdhci_sirf_remove(struct platform_device *pdev)
-{
-	struct sdhci_host *host = platform_get_drvdata(pdev);
-	struct sdhci_pltfm_host *pltfm_host = sdhci_priv(host);
-	struct sdhci_sirf_priv *priv = sdhci_pltfm_priv(pltfm_host);
-
-	sdhci_pltfm_unregister(pdev);
-
-	clk_disable_unprepare(priv->clk);
-	return 0;
-}
-
 #ifdef CONFIG_PM_SLEEP
 static int sdhci_sirf_suspend(struct device *dev)
 {
 	struct sdhci_host *host = dev_get_drvdata(dev);
 	struct sdhci_pltfm_host *pltfm_host = sdhci_priv(host);
-	struct sdhci_sirf_priv *priv = sdhci_pltfm_priv(pltfm_host);
 	int ret;
 
 	ret = sdhci_suspend_host(host);
 	if (ret)
 		return ret;
 
-	clk_disable(priv->clk);
+	clk_disable(pltfm_host->clk);
 
 	return 0;
 }
@@ -234,10 +213,9 @@ static int sdhci_sirf_resume(struct device *dev)
 {
 	struct sdhci_host *host = dev_get_drvdata(dev);
 	struct sdhci_pltfm_host *pltfm_host = sdhci_priv(host);
-	struct sdhci_sirf_priv *priv = sdhci_pltfm_priv(pltfm_host);
 	int ret;
 
-	ret = clk_enable(priv->clk);
+	ret = clk_enable(pltfm_host->clk);
 	if (ret) {
 		dev_dbg(dev, "Resume: Error enabling clock\n");
 		return ret;
@@ -264,7 +242,7 @@ static struct platform_driver sdhci_sirf_driver = {
 #endif
 	},
 	.probe		= sdhci_sirf_probe,
-	.remove		= sdhci_sirf_remove,
+	.remove		= sdhci_pltfm_unregister,
 };
 
 module_platform_driver(sdhci_sirf_driver);
diff --git a/drivers/mmc/host/sdhci-spear.c b/drivers/mmc/host/sdhci-spear.c
index 22e58268545f..df088343d60f 100644
--- a/drivers/mmc/host/sdhci-spear.c
+++ b/drivers/mmc/host/sdhci-spear.c
@@ -26,14 +26,13 @@
 #include <linux/pm.h>
 #include <linux/slab.h>
 #include <linux/mmc/host.h>
-#include <linux/mmc/sdhci-spear.h>
 #include <linux/mmc/slot-gpio.h>
 #include <linux/io.h>
 #include "sdhci.h"
 
 struct spear_sdhci {
 	struct clk *clk;
-	struct sdhci_plat_data *data;
+	int card_int_gpio;
 };
 
 /* sdhci ops */
@@ -44,38 +43,20 @@ static const struct sdhci_ops sdhci_pltfm_ops = {
 	.set_uhs_signaling = sdhci_set_uhs_signaling,
 };
 
-#ifdef CONFIG_OF
-static struct sdhci_plat_data *sdhci_probe_config_dt(struct platform_device *pdev)
+static void sdhci_probe_config_dt(struct device_node *np,
+				struct spear_sdhci *host)
 {
-	struct device_node *np = pdev->dev.of_node;
-	struct sdhci_plat_data *pdata = NULL;
 	int cd_gpio;
 
 	cd_gpio = of_get_named_gpio(np, "cd-gpios", 0);
 	if (!gpio_is_valid(cd_gpio))
 		cd_gpio = -1;
 
-	/* If pdata is required */
-	if (cd_gpio != -1) {
-		pdata = devm_kzalloc(&pdev->dev, sizeof(*pdata), GFP_KERNEL);
-		if (!pdata)
-			dev_err(&pdev->dev, "DT: kzalloc failed\n");
-		else
-			pdata->card_int_gpio = cd_gpio;
-	}
-
-	return pdata;
-}
-#else
-static struct sdhci_plat_data *sdhci_probe_config_dt(struct platform_device *pdev)
-{
-	return ERR_PTR(-ENOSYS);
+	host->card_int_gpio = cd_gpio;
 }
-#endif
 
 static int sdhci_probe(struct platform_device *pdev)
 {
-	struct device_node *np = pdev->dev.of_node;
 	struct sdhci_host *host;
 	struct resource *iomem;
 	struct spear_sdhci *sdhci;
@@ -124,28 +105,18 @@ static int sdhci_probe(struct platform_device *pdev)
 		dev_dbg(&pdev->dev, "Error setting desired clk, clk=%lu\n",
 				clk_get_rate(sdhci->clk));
 
-	if (np) {
-		sdhci->data = sdhci_probe_config_dt(pdev);
-		if (IS_ERR(sdhci->data)) {
-			dev_err(&pdev->dev, "DT: Failed to get pdata\n");
-			goto disable_clk;
-		}
-	} else {
-		sdhci->data = dev_get_platdata(&pdev->dev);
-	}
-
+	sdhci_probe_config_dt(pdev->dev.of_node, sdhci);
 	/*
 	 * It is optional to use GPIOs for sdhci card detection. If
-	 * sdhci->data is NULL, then use original sdhci lines otherwise
+	 * sdhci->card_int_gpio < 0, then use original sdhci lines otherwise
 	 * GPIO lines. We use the built-in GPIO support for this.
 	 */
-	if (sdhci->data && sdhci->data->card_int_gpio >= 0) {
-		ret = mmc_gpio_request_cd(host->mmc,
-					  sdhci->data->card_int_gpio, 0);
+	if (sdhci->card_int_gpio >= 0) {
+		ret = mmc_gpio_request_cd(host->mmc, sdhci->card_int_gpio, 0);
 		if (ret < 0) {
 			dev_dbg(&pdev->dev,
 				"failed to request card-detect gpio%d\n",
-				sdhci->data->card_int_gpio);
+				sdhci->card_int_gpio);
 			goto disable_clk;
 		}
 	}
diff --git a/drivers/mmc/host/sdhci-st.c b/drivers/mmc/host/sdhci-st.c
index 882b07e9667e..682f2bb0f4bf 100644
--- a/drivers/mmc/host/sdhci-st.c
+++ b/drivers/mmc/host/sdhci-st.c
@@ -23,9 +23,295 @@
 #include <linux/module.h>
 #include <linux/err.h>
 #include <linux/mmc/host.h>
-
+#include <linux/reset.h>
 #include "sdhci-pltfm.h"
 
+struct st_mmc_platform_data {
+	struct  reset_control *rstc;
+	void __iomem *top_ioaddr;
+};
+
+/* MMCSS glue logic to setup the HC on some ST SoCs (e.g. STiH407 family) */
+
+#define ST_MMC_CCONFIG_REG_1		0x400
+#define ST_MMC_CCONFIG_TIMEOUT_CLK_UNIT	BIT(24)
+#define ST_MMC_CCONFIG_TIMEOUT_CLK_FREQ	BIT(12)
+#define ST_MMC_CCONFIG_TUNING_COUNT_DEFAULT	BIT(8)
+#define ST_MMC_CCONFIG_ASYNC_WAKEUP	BIT(0)
+#define ST_MMC_CCONFIG_1_DEFAULT	\
+				((ST_MMC_CCONFIG_TIMEOUT_CLK_UNIT) | \
+				 (ST_MMC_CCONFIG_TIMEOUT_CLK_FREQ) | \
+				 (ST_MMC_CCONFIG_TUNING_COUNT_DEFAULT))
+
+#define ST_MMC_CCONFIG_REG_2		0x404
+#define ST_MMC_CCONFIG_HIGH_SPEED	BIT(28)
+#define ST_MMC_CCONFIG_ADMA2		BIT(24)
+#define ST_MMC_CCONFIG_8BIT		BIT(20)
+#define ST_MMC_CCONFIG_MAX_BLK_LEN	16
+#define  MAX_BLK_LEN_1024		1
+#define  MAX_BLK_LEN_2048		2
+#define BASE_CLK_FREQ_200		0xc8
+#define BASE_CLK_FREQ_100		0x64
+#define BASE_CLK_FREQ_50		0x32
+#define ST_MMC_CCONFIG_2_DEFAULT \
+	(ST_MMC_CCONFIG_HIGH_SPEED | ST_MMC_CCONFIG_ADMA2 | \
+	 ST_MMC_CCONFIG_8BIT | \
+	 (MAX_BLK_LEN_1024 << ST_MMC_CCONFIG_MAX_BLK_LEN))
+
+#define ST_MMC_CCONFIG_REG_3			0x408
+#define ST_MMC_CCONFIG_EMMC_SLOT_TYPE		BIT(28)
+#define ST_MMC_CCONFIG_64BIT			BIT(24)
+#define ST_MMC_CCONFIG_ASYNCH_INTR_SUPPORT	BIT(20)
+#define ST_MMC_CCONFIG_1P8_VOLT			BIT(16)
+#define ST_MMC_CCONFIG_3P0_VOLT			BIT(12)
+#define ST_MMC_CCONFIG_3P3_VOLT			BIT(8)
+#define ST_MMC_CCONFIG_SUSP_RES_SUPPORT		BIT(4)
+#define ST_MMC_CCONFIG_SDMA			BIT(0)
+#define ST_MMC_CCONFIG_3_DEFAULT	\
+			 (ST_MMC_CCONFIG_ASYNCH_INTR_SUPPORT	| \
+			  ST_MMC_CCONFIG_3P3_VOLT		| \
+			  ST_MMC_CCONFIG_SUSP_RES_SUPPORT	| \
+			  ST_MMC_CCONFIG_SDMA)
+
+#define ST_MMC_CCONFIG_REG_4	0x40c
+#define ST_MMC_CCONFIG_D_DRIVER	BIT(20)
+#define ST_MMC_CCONFIG_C_DRIVER	BIT(16)
+#define ST_MMC_CCONFIG_A_DRIVER	BIT(12)
+#define ST_MMC_CCONFIG_DDR50	BIT(8)
+#define ST_MMC_CCONFIG_SDR104	BIT(4)
+#define ST_MMC_CCONFIG_SDR50	BIT(0)
+#define ST_MMC_CCONFIG_4_DEFAULT	0
+
+#define ST_MMC_CCONFIG_REG_5		0x410
+#define ST_MMC_CCONFIG_TUNING_FOR_SDR50	BIT(8)
+#define RETUNING_TIMER_CNT_MAX		0xf
+#define ST_MMC_CCONFIG_5_DEFAULT	0
+
+/* I/O configuration for Arasan IP */
+#define ST_MMC_GP_OUTPUT	0x450
+#define ST_MMC_GP_OUTPUT_CD	BIT(12)
+
+#define ST_MMC_STATUS_R		0x460
+
+#define ST_TOP_MMC_DLY_FIX_OFF(x)	(x - 0x8)
+
+/* TOP config registers to manage static and dynamic delay */
+#define ST_TOP_MMC_TX_CLK_DLY			ST_TOP_MMC_DLY_FIX_OFF(0x8)
+#define ST_TOP_MMC_RX_CLK_DLY			ST_TOP_MMC_DLY_FIX_OFF(0xc)
+/* MMC delay control register */
+#define ST_TOP_MMC_DLY_CTRL			ST_TOP_MMC_DLY_FIX_OFF(0x18)
+#define ST_TOP_MMC_DLY_CTRL_DLL_BYPASS_CMD	BIT(0)
+#define ST_TOP_MMC_DLY_CTRL_DLL_BYPASS_PH_SEL	BIT(1)
+#define ST_TOP_MMC_DLY_CTRL_TX_DLL_ENABLE	BIT(8)
+#define ST_TOP_MMC_DLY_CTRL_RX_DLL_ENABLE	BIT(9)
+#define ST_TOP_MMC_DLY_CTRL_ATUNE_NOT_CFG_DLY	BIT(10)
+#define ST_TOP_MMC_START_DLL_LOCK		BIT(11)
+
+/* register to provide the phase-shift value for DLL */
+#define ST_TOP_MMC_TX_DLL_STEP_DLY		ST_TOP_MMC_DLY_FIX_OFF(0x1c)
+#define ST_TOP_MMC_RX_DLL_STEP_DLY		ST_TOP_MMC_DLY_FIX_OFF(0x20)
+#define ST_TOP_MMC_RX_CMD_STEP_DLY		ST_TOP_MMC_DLY_FIX_OFF(0x24)
+
+/* phase shift delay on the tx clk 2.188ns */
+#define ST_TOP_MMC_TX_DLL_STEP_DLY_VALID	0x6
+
+#define ST_TOP_MMC_DLY_MAX			0xf
+
+#define ST_TOP_MMC_DYN_DLY_CONF	\
+		(ST_TOP_MMC_DLY_CTRL_TX_DLL_ENABLE | \
+		 ST_TOP_MMC_DLY_CTRL_ATUNE_NOT_CFG_DLY | \
+		 ST_TOP_MMC_START_DLL_LOCK)
+
+/*
+ * For clock speeds greater than 90MHz, we need to check that the
+ * DLL procedure has finished before switching to ultra-speed modes.
+ */
+#define	CLK_TO_CHECK_DLL_LOCK	90000000
+
+static inline void st_mmcss_set_static_delay(void __iomem *ioaddr)
+{
+	if (!ioaddr)
+		return;
+
+	writel_relaxed(0x0, ioaddr + ST_TOP_MMC_DLY_CTRL);
+	writel_relaxed(ST_TOP_MMC_DLY_MAX,
+			ioaddr + ST_TOP_MMC_TX_CLK_DLY);
+}
+
+/**
+ * st_mmcss_cconfig: configure the Arasan HC inside the flashSS.
+ * @np: dt device node.
+ * @host: sdhci host
+ * Description: this function is to configure the Arasan host controller.
+ * On some ST SoCs, i.e. STiH407 family, the MMC devices inside a dedicated
+ * flashSS sub-system which needs to be configured to be compliant to eMMC 4.5
+ * or eMMC4.3.  This has to be done before registering the sdhci host.
+ */
+static void st_mmcss_cconfig(struct device_node *np, struct sdhci_host *host)
+{
+	struct sdhci_pltfm_host *pltfm_host = sdhci_priv(host);
+	struct mmc_host *mhost = host->mmc;
+	u32 cconf2, cconf3, cconf4, cconf5;
+
+	if (!of_device_is_compatible(np, "st,sdhci-stih407"))
+		return;
+
+	cconf2 = ST_MMC_CCONFIG_2_DEFAULT;
+	cconf3 = ST_MMC_CCONFIG_3_DEFAULT;
+	cconf4 = ST_MMC_CCONFIG_4_DEFAULT;
+	cconf5 = ST_MMC_CCONFIG_5_DEFAULT;
+
+	writel_relaxed(ST_MMC_CCONFIG_1_DEFAULT,
+			host->ioaddr + ST_MMC_CCONFIG_REG_1);
+
+	/* Set clock frequency, default to 50MHz if max-frequency is not
+	 * provided */
+
+	switch (mhost->f_max) {
+	case 200000000:
+		clk_set_rate(pltfm_host->clk, mhost->f_max);
+		cconf2 |= BASE_CLK_FREQ_200;
+		break;
+	case 100000000:
+		clk_set_rate(pltfm_host->clk, mhost->f_max);
+		cconf2 |= BASE_CLK_FREQ_100;
+		break;
+	default:
+		clk_set_rate(pltfm_host->clk, 50000000);
+		cconf2 |= BASE_CLK_FREQ_50;
+	}
+
+	writel_relaxed(cconf2, host->ioaddr + ST_MMC_CCONFIG_REG_2);
+
+	if (mhost->caps & MMC_CAP_NONREMOVABLE)
+		cconf3 |= ST_MMC_CCONFIG_EMMC_SLOT_TYPE;
+	else
+		/* CARD _D ET_CTRL */
+		writel_relaxed(ST_MMC_GP_OUTPUT_CD,
+				host->ioaddr + ST_MMC_GP_OUTPUT);
+
+	if (mhost->caps & MMC_CAP_UHS_SDR50) {
+		/* use 1.8V */
+		cconf3 |= ST_MMC_CCONFIG_1P8_VOLT;
+		cconf4 |= ST_MMC_CCONFIG_SDR50;
+		/* Use tuning */
+		cconf5 |= ST_MMC_CCONFIG_TUNING_FOR_SDR50;
+		/* Max timeout for retuning */
+		cconf5 |= RETUNING_TIMER_CNT_MAX;
+	}
+
+	if (mhost->caps & MMC_CAP_UHS_SDR104) {
+		/*
+		 * SDR104 implies the HC can support HS200 mode, so
+		 * it's mandatory to use 1.8V
+		 */
+		cconf3 |= ST_MMC_CCONFIG_1P8_VOLT;
+		cconf4 |= ST_MMC_CCONFIG_SDR104;
+		/* Max timeout for retuning */
+		cconf5 |= RETUNING_TIMER_CNT_MAX;
+	}
+
+	if (mhost->caps & MMC_CAP_UHS_DDR50)
+		cconf4 |= ST_MMC_CCONFIG_DDR50;
+
+	writel_relaxed(cconf3, host->ioaddr + ST_MMC_CCONFIG_REG_3);
+	writel_relaxed(cconf4, host->ioaddr + ST_MMC_CCONFIG_REG_4);
+	writel_relaxed(cconf5, host->ioaddr + ST_MMC_CCONFIG_REG_5);
+}
+
+static inline void st_mmcss_set_dll(void __iomem *ioaddr)
+{
+	if (!ioaddr)
+		return;
+
+	writel_relaxed(ST_TOP_MMC_DYN_DLY_CONF,	ioaddr + ST_TOP_MMC_DLY_CTRL);
+	writel_relaxed(ST_TOP_MMC_TX_DLL_STEP_DLY_VALID,
+			ioaddr + ST_TOP_MMC_TX_DLL_STEP_DLY);
+}
+
+static int st_mmcss_lock_dll(void __iomem *ioaddr)
+{
+	unsigned long curr, value;
+	unsigned long finish = jiffies + HZ;
+
+	/* Checks if the DLL procedure is finished */
+	do {
+		curr = jiffies;
+		value = readl(ioaddr + ST_MMC_STATUS_R);
+		if (value & 0x1)
+			return 0;
+
+		cpu_relax();
+	} while (!time_after_eq(curr, finish));
+
+	return -EBUSY;
+}
+
+static int sdhci_st_set_dll_for_clock(struct sdhci_host *host)
+{
+	int ret = 0;
+	struct sdhci_pltfm_host *pltfm_host = sdhci_priv(host);
+	struct st_mmc_platform_data *pdata = pltfm_host->priv;
+
+	if (host->clock > CLK_TO_CHECK_DLL_LOCK) {
+		st_mmcss_set_dll(pdata->top_ioaddr);
+		ret = st_mmcss_lock_dll(host->ioaddr);
+	}
+
+	return ret;
+}
+
+static void sdhci_st_set_uhs_signaling(struct sdhci_host *host,
+					unsigned int uhs)
+{
+	struct sdhci_pltfm_host *pltfm_host = sdhci_priv(host);
+	struct st_mmc_platform_data *pdata = pltfm_host->priv;
+	u16 ctrl_2 = sdhci_readw(host, SDHCI_HOST_CONTROL2);
+	int ret = 0;
+
+	/* Select Bus Speed Mode for host */
+	ctrl_2 &= ~SDHCI_CTRL_UHS_MASK;
+	switch (uhs) {
+	/*
+	 * Set V18_EN -- UHS modes do not work without this.
+	 * does not change signaling voltage
+	 */
+
+	case MMC_TIMING_UHS_SDR12:
+		st_mmcss_set_static_delay(pdata->top_ioaddr);
+		ctrl_2 |= SDHCI_CTRL_UHS_SDR12 | SDHCI_CTRL_VDD_180;
+		break;
+	case MMC_TIMING_UHS_SDR25:
+		st_mmcss_set_static_delay(pdata->top_ioaddr);
+		ctrl_2 |= SDHCI_CTRL_UHS_SDR25 | SDHCI_CTRL_VDD_180;
+		break;
+	case MMC_TIMING_UHS_SDR50:
+		st_mmcss_set_static_delay(pdata->top_ioaddr);
+		ctrl_2 |= SDHCI_CTRL_UHS_SDR50 | SDHCI_CTRL_VDD_180;
+		ret = sdhci_st_set_dll_for_clock(host);
+		break;
+	case MMC_TIMING_UHS_SDR104:
+	case MMC_TIMING_MMC_HS200:
+		st_mmcss_set_static_delay(pdata->top_ioaddr);
+		ctrl_2 |= SDHCI_CTRL_UHS_SDR104 | SDHCI_CTRL_VDD_180;
+		ret =  sdhci_st_set_dll_for_clock(host);
+		break;
+	case MMC_TIMING_UHS_DDR50:
+	case MMC_TIMING_MMC_DDR52:
+		st_mmcss_set_static_delay(pdata->top_ioaddr);
+		ctrl_2 |= SDHCI_CTRL_UHS_DDR50 | SDHCI_CTRL_VDD_180;
+		break;
+	}
+
+	if (ret)
+		dev_warn(mmc_dev(host->mmc), "Error setting dll for clock "
+						"(uhs %d)\n", uhs);
+
+	dev_dbg(mmc_dev(host->mmc), "uhs %d, ctrl_2 %04X\n", uhs, ctrl_2);
+
+	sdhci_writew(host, ctrl_2, SDHCI_HOST_CONTROL2);
+}
+
 static u32 sdhci_st_readl(struct sdhci_host *host, int reg)
 {
 	u32 ret;
@@ -48,22 +334,33 @@ static const struct sdhci_ops sdhci_st_ops = {
 	.set_bus_width = sdhci_set_bus_width,
 	.read_l = sdhci_st_readl,
 	.reset = sdhci_reset,
+	.set_uhs_signaling = sdhci_st_set_uhs_signaling,
 };
 
 static const struct sdhci_pltfm_data sdhci_st_pdata = {
 	.ops = &sdhci_st_ops,
 	.quirks = SDHCI_QUIRK_NO_ENDATTR_IN_NOPDESC |
-	    SDHCI_QUIRK_CAP_CLOCK_BASE_BROKEN,
+		SDHCI_QUIRK_CAP_CLOCK_BASE_BROKEN |
+		SDHCI_QUIRK_NO_HISPD_BIT,
+	.quirks2 = SDHCI_QUIRK2_PRESET_VALUE_BROKEN |
+		SDHCI_QUIRK2_STOP_WITH_TC,
 };
 
 
 static int sdhci_st_probe(struct platform_device *pdev)
 {
+	struct device_node *np = pdev->dev.of_node;
 	struct sdhci_host *host;
+	struct st_mmc_platform_data *pdata;
 	struct sdhci_pltfm_host *pltfm_host;
 	struct clk *clk;
 	int ret = 0;
 	u16 host_version;
+	struct resource *res;
+
+	pdata = devm_kzalloc(&pdev->dev, sizeof(*pdata), GFP_KERNEL);
+	if (!pdata)
+		return -ENOMEM;
 
 	clk =  devm_clk_get(&pdev->dev, "mmc");
 	if (IS_ERR(clk)) {
@@ -71,10 +368,17 @@ static int sdhci_st_probe(struct platform_device *pdev)
 		return PTR_ERR(clk);
 	}
 
+	pdata->rstc = devm_reset_control_get(&pdev->dev, NULL);
+	if (IS_ERR(pdata->rstc))
+		pdata->rstc = NULL;
+	else
+		reset_control_deassert(pdata->rstc);
+
 	host = sdhci_pltfm_init(pdev, &sdhci_st_pdata, 0);
 	if (IS_ERR(host)) {
 		dev_err(&pdev->dev, "Failed sdhci_pltfm_init\n");
-		return PTR_ERR(host);
+		ret = PTR_ERR(host);
+		goto err_pltfm_init;
 	}
 
 	ret = mmc_of_parse(host->mmc);
@@ -85,9 +389,22 @@ static int sdhci_st_probe(struct platform_device *pdev)
 
 	clk_prepare_enable(clk);
 
+	/* Configure the FlashSS Top registers for setting eMMC TX/RX delay */
+	res = platform_get_resource_byname(pdev, IORESOURCE_MEM,
+					   "top-mmc-delay");
+	pdata->top_ioaddr = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(pdata->top_ioaddr)) {
+		dev_warn(&pdev->dev, "FlashSS Top Dly registers not available");
+		pdata->top_ioaddr = NULL;
+	}
+
 	pltfm_host = sdhci_priv(host);
+	pltfm_host->priv = pdata;
 	pltfm_host->clk = clk;
 
+	/* Configure the Arasan HC inside the flashSS */
+	st_mmcss_cconfig(np, host);
+
 	ret = sdhci_add_host(host);
 	if (ret) {
 		dev_err(&pdev->dev, "Failed sdhci_add_host\n");
@@ -109,6 +426,9 @@ err_out:
 	clk_disable_unprepare(clk);
 err_of:
 	sdhci_pltfm_free(pdev);
+err_pltfm_init:
+	if (pdata->rstc)
+		reset_control_assert(pdata->rstc);
 
 	return ret;
 }
@@ -117,10 +437,15 @@ static int sdhci_st_remove(struct platform_device *pdev)
 {
 	struct sdhci_host *host = platform_get_drvdata(pdev);
 	struct sdhci_pltfm_host *pltfm_host = sdhci_priv(host);
+	struct st_mmc_platform_data *pdata = pltfm_host->priv;
+	int ret;
 
-	clk_disable_unprepare(pltfm_host->clk);
+	ret = sdhci_pltfm_unregister(pdev);
+
+	if (pdata->rstc)
+		reset_control_assert(pdata->rstc);
 
-	return sdhci_pltfm_unregister(pdev);
+	return ret;
 }
 
 #ifdef CONFIG_PM_SLEEP
@@ -128,11 +453,15 @@ static int sdhci_st_suspend(struct device *dev)
 {
 	struct sdhci_host *host = dev_get_drvdata(dev);
 	struct sdhci_pltfm_host *pltfm_host = sdhci_priv(host);
+	struct st_mmc_platform_data *pdata = pltfm_host->priv;
 	int ret = sdhci_suspend_host(host);
 
 	if (ret)
 		goto out;
 
+	if (pdata->rstc)
+		reset_control_assert(pdata->rstc);
+
 	clk_disable_unprepare(pltfm_host->clk);
 out:
 	return ret;
@@ -142,9 +471,16 @@ static int sdhci_st_resume(struct device *dev)
 {
 	struct sdhci_host *host = dev_get_drvdata(dev);
 	struct sdhci_pltfm_host *pltfm_host = sdhci_priv(host);
+	struct st_mmc_platform_data *pdata = pltfm_host->priv;
+	struct device_node *np = dev->of_node;
 
 	clk_prepare_enable(pltfm_host->clk);
 
+	if (pdata->rstc)
+		reset_control_deassert(pdata->rstc);
+
+	st_mmcss_cconfig(np, host);
+
 	return sdhci_resume_host(host);
 }
 #endif
diff --git a/drivers/mmc/host/sdhci-tegra.c b/drivers/mmc/host/sdhci-tegra.c
index f3778d58d1cd..ad28b49f0203 100644
--- a/drivers/mmc/host/sdhci-tegra.c
+++ b/drivers/mmc/host/sdhci-tegra.c
@@ -20,11 +20,10 @@
 #include <linux/io.h>
 #include <linux/of.h>
 #include <linux/of_device.h>
-#include <linux/of_gpio.h>
-#include <linux/gpio.h>
 #include <linux/mmc/card.h>
 #include <linux/mmc/host.h>
 #include <linux/mmc/slot-gpio.h>
+#include <linux/gpio/consumer.h>
 
 #include "sdhci-pltfm.h"
 
@@ -41,7 +40,6 @@
 #define NVQUIRK_DISABLE_SDR50		BIT(3)
 #define NVQUIRK_DISABLE_SDR104		BIT(4)
 #define NVQUIRK_DISABLE_DDR50		BIT(5)
-#define NVQUIRK_SHADOW_XFER_MODE_REG	BIT(6)
 
 struct sdhci_tegra_soc_data {
 	const struct sdhci_pltfm_data *pdata;
@@ -50,7 +48,7 @@ struct sdhci_tegra_soc_data {
 
 struct sdhci_tegra {
 	const struct sdhci_tegra_soc_data *soc_data;
-	int power_gpio;
+	struct gpio_desc *power_gpio;
 };
 
 static u16 tegra_sdhci_readw(struct sdhci_host *host, int reg)
@@ -71,23 +69,19 @@ static u16 tegra_sdhci_readw(struct sdhci_host *host, int reg)
 static void tegra_sdhci_writew(struct sdhci_host *host, u16 val, int reg)
 {
 	struct sdhci_pltfm_host *pltfm_host = sdhci_priv(host);
-	struct sdhci_tegra *tegra_host = pltfm_host->priv;
-	const struct sdhci_tegra_soc_data *soc_data = tegra_host->soc_data;
 
-	if (soc_data->nvquirks & NVQUIRK_SHADOW_XFER_MODE_REG) {
-		switch (reg) {
-		case SDHCI_TRANSFER_MODE:
-			/*
-			 * Postpone this write, we must do it together with a
-			 * command write that is down below.
-			 */
-			pltfm_host->xfer_mode_shadow = val;
-			return;
-		case SDHCI_COMMAND:
-			writel((val << 16) | pltfm_host->xfer_mode_shadow,
-				host->ioaddr + SDHCI_TRANSFER_MODE);
-			return;
-		}
+	switch (reg) {
+	case SDHCI_TRANSFER_MODE:
+		/*
+		 * Postpone this write, we must do it together with a
+		 * command write that is down below.
+		 */
+		pltfm_host->xfer_mode_shadow = val;
+		return;
+	case SDHCI_COMMAND:
+		writel((val << 16) | pltfm_host->xfer_mode_shadow,
+			host->ioaddr + SDHCI_TRANSFER_MODE);
+		return;
 	}
 
 	writew(val, host->ioaddr + reg);
@@ -173,7 +167,6 @@ static void tegra_sdhci_set_bus_width(struct sdhci_host *host, int bus_width)
 static const struct sdhci_ops tegra_sdhci_ops = {
 	.get_ro     = tegra_sdhci_get_ro,
 	.read_w     = tegra_sdhci_readw,
-	.write_w    = tegra_sdhci_writew,
 	.write_l    = tegra_sdhci_writel,
 	.set_clock  = sdhci_set_clock,
 	.set_bus_width = tegra_sdhci_set_bus_width,
@@ -214,6 +207,18 @@ static struct sdhci_tegra_soc_data soc_data_tegra30 = {
 		    NVQUIRK_DISABLE_SDR104,
 };
 
+static const struct sdhci_ops tegra114_sdhci_ops = {
+	.get_ro     = tegra_sdhci_get_ro,
+	.read_w     = tegra_sdhci_readw,
+	.write_w    = tegra_sdhci_writew,
+	.write_l    = tegra_sdhci_writel,
+	.set_clock  = sdhci_set_clock,
+	.set_bus_width = tegra_sdhci_set_bus_width,
+	.reset      = tegra_sdhci_reset,
+	.set_uhs_signaling = sdhci_set_uhs_signaling,
+	.get_max_clock = sdhci_pltfm_clk_get_max_clock,
+};
+
 static const struct sdhci_pltfm_data sdhci_tegra114_pdata = {
 	.quirks = SDHCI_QUIRK_BROKEN_TIMEOUT_VAL |
 		  SDHCI_QUIRK_DATA_TIMEOUT_USES_SDCLK |
@@ -221,15 +226,14 @@ static const struct sdhci_pltfm_data sdhci_tegra114_pdata = {
 		  SDHCI_QUIRK_NO_HISPD_BIT |
 		  SDHCI_QUIRK_BROKEN_ADMA_ZEROLEN_DESC |
 		  SDHCI_QUIRK_CAP_CLOCK_BASE_BROKEN,
-	.ops  = &tegra_sdhci_ops,
+	.ops  = &tegra114_sdhci_ops,
 };
 
 static struct sdhci_tegra_soc_data soc_data_tegra114 = {
 	.pdata = &sdhci_tegra114_pdata,
 	.nvquirks = NVQUIRK_DISABLE_SDR50 |
 		    NVQUIRK_DISABLE_DDR50 |
-		    NVQUIRK_DISABLE_SDR104 |
-		    NVQUIRK_SHADOW_XFER_MODE_REG,
+		    NVQUIRK_DISABLE_SDR104,
 };
 
 static const struct of_device_id sdhci_tegra_dt_match[] = {
@@ -241,17 +245,6 @@ static const struct of_device_id sdhci_tegra_dt_match[] = {
 };
 MODULE_DEVICE_TABLE(of, sdhci_tegra_dt_match);
 
-static int sdhci_tegra_parse_dt(struct device *dev)
-{
-	struct device_node *np = dev->of_node;
-	struct sdhci_host *host = dev_get_drvdata(dev);
-	struct sdhci_pltfm_host *pltfm_host = sdhci_priv(host);
-	struct sdhci_tegra *tegra_host = pltfm_host->priv;
-
-	tegra_host->power_gpio = of_get_named_gpio(np, "power-gpios", 0);
-	return mmc_of_parse(host->mmc);
-}
-
 static int sdhci_tegra_probe(struct platform_device *pdev)
 {
 	const struct of_device_id *match;
@@ -281,21 +274,18 @@ static int sdhci_tegra_probe(struct platform_device *pdev)
 	tegra_host->soc_data = soc_data;
 	pltfm_host->priv = tegra_host;
 
-	rc = sdhci_tegra_parse_dt(&pdev->dev);
+	rc = mmc_of_parse(host->mmc);
 	if (rc)
 		goto err_parse_dt;
 
-	if (gpio_is_valid(tegra_host->power_gpio)) {
-		rc = gpio_request(tegra_host->power_gpio, "sdhci_power");
-		if (rc) {
-			dev_err(mmc_dev(host->mmc),
-				"failed to allocate power gpio\n");
-			goto err_power_req;
-		}
-		gpio_direction_output(tegra_host->power_gpio, 1);
+	tegra_host->power_gpio = devm_gpiod_get_optional(&pdev->dev, "power",
+							 GPIOD_OUT_HIGH);
+	if (IS_ERR(tegra_host->power_gpio)) {
+		rc = PTR_ERR(tegra_host->power_gpio);
+		goto err_power_req;
 	}
 
-	clk = clk_get(mmc_dev(host->mmc), NULL);
+	clk = devm_clk_get(mmc_dev(host->mmc), NULL);
 	if (IS_ERR(clk)) {
 		dev_err(mmc_dev(host->mmc), "clk err\n");
 		rc = PTR_ERR(clk);
@@ -312,10 +302,7 @@ static int sdhci_tegra_probe(struct platform_device *pdev)
 
 err_add_host:
 	clk_disable_unprepare(pltfm_host->clk);
-	clk_put(pltfm_host->clk);
 err_clk_get:
-	if (gpio_is_valid(tegra_host->power_gpio))
-		gpio_free(tegra_host->power_gpio);
 err_power_req:
 err_parse_dt:
 err_alloc_tegra_host:
@@ -323,26 +310,6 @@ err_alloc_tegra_host:
 	return rc;
 }
 
-static int sdhci_tegra_remove(struct platform_device *pdev)
-{
-	struct sdhci_host *host = platform_get_drvdata(pdev);
-	struct sdhci_pltfm_host *pltfm_host = sdhci_priv(host);
-	struct sdhci_tegra *tegra_host = pltfm_host->priv;
-	int dead = (readl(host->ioaddr + SDHCI_INT_STATUS) == 0xffffffff);
-
-	sdhci_remove_host(host, dead);
-
-	if (gpio_is_valid(tegra_host->power_gpio))
-		gpio_free(tegra_host->power_gpio);
-
-	clk_disable_unprepare(pltfm_host->clk);
-	clk_put(pltfm_host->clk);
-
-	sdhci_pltfm_free(pdev);
-
-	return 0;
-}
-
 static struct platform_driver sdhci_tegra_driver = {
 	.driver		= {
 		.name	= "sdhci-tegra",
@@ -350,7 +317,7 @@ static struct platform_driver sdhci_tegra_driver = {
 		.pm	= SDHCI_PLTFM_PMOPS,
 	},
 	.probe		= sdhci_tegra_probe,
-	.remove		= sdhci_tegra_remove,
+	.remove		= sdhci_pltfm_unregister,
 };
 
 module_platform_driver(sdhci_tegra_driver);
diff --git a/drivers/mmc/host/sdhci.c b/drivers/mmc/host/sdhci.c
index 0ad412a4876f..c80287a02735 100644
--- a/drivers/mmc/host/sdhci.c
+++ b/drivers/mmc/host/sdhci.c
@@ -28,6 +28,7 @@
 #include <linux/mmc/mmc.h>
 #include <linux/mmc/host.h>
 #include <linux/mmc/card.h>
+#include <linux/mmc/sdio.h>
 #include <linux/mmc/slot-gpio.h>
 
 #include "sdhci.h"
@@ -56,6 +57,7 @@ static void sdhci_enable_preset_value(struct sdhci_host *host, bool enable);
 static int sdhci_pre_dma_transfer(struct sdhci_host *host,
 					struct mmc_data *data,
 					struct sdhci_host_next *next);
+static int sdhci_do_get_cd(struct sdhci_host *host);
 
 #ifdef CONFIG_PM
 static int sdhci_runtime_pm_get(struct sdhci_host *host);
@@ -931,7 +933,8 @@ static void sdhci_set_transfer_mode(struct sdhci_host *host,
 		 * If we are sending CMD23, CMD12 never gets sent
 		 * on successful completion (so no Auto-CMD12).
 		 */
-		if (!host->mrq->sbc && (host->flags & SDHCI_AUTO_CMD12))
+		if (!host->mrq->sbc && (host->flags & SDHCI_AUTO_CMD12) &&
+		    (cmd->opcode != SD_IO_RW_EXTENDED))
 			mode |= SDHCI_TRNS_AUTO_CMD12;
 		else if (host->mrq->sbc && (host->flags & SDHCI_AUTO_CMD23)) {
 			mode |= SDHCI_TRNS_AUTO_CMD23;
@@ -1356,7 +1359,8 @@ static void sdhci_request(struct mmc_host *mmc, struct mmc_request *mrq)
 
 	sdhci_runtime_pm_get(host);
 
-	present = mmc_gpio_get_cd(host->mmc);
+	/* Firstly check card presence */
+	present = sdhci_do_get_cd(host);
 
 	spin_lock_irqsave(&host->lock, flags);
 
@@ -1379,22 +1383,6 @@ static void sdhci_request(struct mmc_host *mmc, struct mmc_request *mrq)
 
 	host->mrq = mrq;
 
-	/*
-	 * Firstly check card presence from cd-gpio.  The return could
-	 * be one of the following possibilities:
-	 *     negative: cd-gpio is not available
-	 *     zero: cd-gpio is used, and card is removed
-	 *     one: cd-gpio is used, and card is present
-	 */
-	if (present < 0) {
-		/* If polling, assume that the card is always present. */
-		if (host->quirks & SDHCI_QUIRK_BROKEN_CARD_DETECTION)
-			present = 1;
-		else
-			present = sdhci_readl(host, SDHCI_PRESENT_STATE) &
-					SDHCI_CARD_PRESENT;
-	}
-
 	if (!present || host->flags & SDHCI_DEVICE_DEAD) {
 		host->mrq->cmd->error = -ENOMEDIUM;
 		tasklet_schedule(&host->finish_tasklet);
@@ -3164,7 +3152,8 @@ int sdhci_add_host(struct sdhci_host *host)
 	/* Auto-CMD23 stuff only works in ADMA or PIO. */
 	if ((host->version >= SDHCI_SPEC_300) &&
 	    ((host->flags & SDHCI_USE_ADMA) ||
-	     !(host->flags & SDHCI_USE_SDMA))) {
+	     !(host->flags & SDHCI_USE_SDMA)) &&
+	     !(host->quirks2 & SDHCI_QUIRK2_ACMD23_BROKEN)) {
 		host->flags |= SDHCI_AUTO_CMD23;
 		DBG("%s: Auto-CMD23 available\n", mmc_hostname(mmc));
 	} else {
diff --git a/drivers/mmc/host/sdhci.h b/drivers/mmc/host/sdhci.h
index 0315e1844330..e639b7f435e5 100644
--- a/drivers/mmc/host/sdhci.h
+++ b/drivers/mmc/host/sdhci.h
@@ -18,7 +18,7 @@
 #include <linux/types.h>
 #include <linux/io.h>
 
-#include <linux/mmc/sdhci.h>
+#include <linux/mmc/host.h>
 
 /*
  * Controller registers
@@ -309,6 +309,207 @@ struct sdhci_adma2_64_desc {
  */
 #define SDHCI_MAX_SEGS		128
 
+struct sdhci_host_next {
+	unsigned int	sg_count;
+	s32		cookie;
+};
+
+struct sdhci_host {
+	/* Data set by hardware interface driver */
+	const char *hw_name;	/* Hardware bus name */
+
+	unsigned int quirks;	/* Deviations from spec. */
+
+/* Controller doesn't honor resets unless we touch the clock register */
+#define SDHCI_QUIRK_CLOCK_BEFORE_RESET			(1<<0)
+/* Controller has bad caps bits, but really supports DMA */
+#define SDHCI_QUIRK_FORCE_DMA				(1<<1)
+/* Controller doesn't like to be reset when there is no card inserted. */
+#define SDHCI_QUIRK_NO_CARD_NO_RESET			(1<<2)
+/* Controller doesn't like clearing the power reg before a change */
+#define SDHCI_QUIRK_SINGLE_POWER_WRITE			(1<<3)
+/* Controller has flaky internal state so reset it on each ios change */
+#define SDHCI_QUIRK_RESET_CMD_DATA_ON_IOS		(1<<4)
+/* Controller has an unusable DMA engine */
+#define SDHCI_QUIRK_BROKEN_DMA				(1<<5)
+/* Controller has an unusable ADMA engine */
+#define SDHCI_QUIRK_BROKEN_ADMA				(1<<6)
+/* Controller can only DMA from 32-bit aligned addresses */
+#define SDHCI_QUIRK_32BIT_DMA_ADDR			(1<<7)
+/* Controller can only DMA chunk sizes that are a multiple of 32 bits */
+#define SDHCI_QUIRK_32BIT_DMA_SIZE			(1<<8)
+/* Controller can only ADMA chunks that are a multiple of 32 bits */
+#define SDHCI_QUIRK_32BIT_ADMA_SIZE			(1<<9)
+/* Controller needs to be reset after each request to stay stable */
+#define SDHCI_QUIRK_RESET_AFTER_REQUEST			(1<<10)
+/* Controller needs voltage and power writes to happen separately */
+#define SDHCI_QUIRK_NO_SIMULT_VDD_AND_POWER		(1<<11)
+/* Controller provides an incorrect timeout value for transfers */
+#define SDHCI_QUIRK_BROKEN_TIMEOUT_VAL			(1<<12)
+/* Controller has an issue with buffer bits for small transfers */
+#define SDHCI_QUIRK_BROKEN_SMALL_PIO			(1<<13)
+/* Controller does not provide transfer-complete interrupt when not busy */
+#define SDHCI_QUIRK_NO_BUSY_IRQ				(1<<14)
+/* Controller has unreliable card detection */
+#define SDHCI_QUIRK_BROKEN_CARD_DETECTION		(1<<15)
+/* Controller reports inverted write-protect state */
+#define SDHCI_QUIRK_INVERTED_WRITE_PROTECT		(1<<16)
+/* Controller does not like fast PIO transfers */
+#define SDHCI_QUIRK_PIO_NEEDS_DELAY			(1<<18)
+/* Controller has to be forced to use block size of 2048 bytes */
+#define SDHCI_QUIRK_FORCE_BLK_SZ_2048			(1<<20)
+/* Controller cannot do multi-block transfers */
+#define SDHCI_QUIRK_NO_MULTIBLOCK			(1<<21)
+/* Controller can only handle 1-bit data transfers */
+#define SDHCI_QUIRK_FORCE_1_BIT_DATA			(1<<22)
+/* Controller needs 10ms delay between applying power and clock */
+#define SDHCI_QUIRK_DELAY_AFTER_POWER			(1<<23)
+/* Controller uses SDCLK instead of TMCLK for data timeouts */
+#define SDHCI_QUIRK_DATA_TIMEOUT_USES_SDCLK		(1<<24)
+/* Controller reports wrong base clock capability */
+#define SDHCI_QUIRK_CAP_CLOCK_BASE_BROKEN		(1<<25)
+/* Controller cannot support End Attribute in NOP ADMA descriptor */
+#define SDHCI_QUIRK_NO_ENDATTR_IN_NOPDESC		(1<<26)
+/* Controller is missing device caps. Use caps provided by host */
+#define SDHCI_QUIRK_MISSING_CAPS			(1<<27)
+/* Controller uses Auto CMD12 command to stop the transfer */
+#define SDHCI_QUIRK_MULTIBLOCK_READ_ACMD12		(1<<28)
+/* Controller doesn't have HISPD bit field in HI-SPEED SD card */
+#define SDHCI_QUIRK_NO_HISPD_BIT			(1<<29)
+/* Controller treats ADMA descriptors with length 0000h incorrectly */
+#define SDHCI_QUIRK_BROKEN_ADMA_ZEROLEN_DESC		(1<<30)
+/* The read-only detection via SDHCI_PRESENT_STATE register is unstable */
+#define SDHCI_QUIRK_UNSTABLE_RO_DETECT			(1<<31)
+
+	unsigned int quirks2;	/* More deviations from spec. */
+
+#define SDHCI_QUIRK2_HOST_OFF_CARD_ON			(1<<0)
+#define SDHCI_QUIRK2_HOST_NO_CMD23			(1<<1)
+/* The system physically doesn't support 1.8v, even if the host does */
+#define SDHCI_QUIRK2_NO_1_8_V				(1<<2)
+#define SDHCI_QUIRK2_PRESET_VALUE_BROKEN		(1<<3)
+#define SDHCI_QUIRK2_CARD_ON_NEEDS_BUS_ON		(1<<4)
+/* Controller has a non-standard host control register */
+#define SDHCI_QUIRK2_BROKEN_HOST_CONTROL		(1<<5)
+/* Controller does not support HS200 */
+#define SDHCI_QUIRK2_BROKEN_HS200			(1<<6)
+/* Controller does not support DDR50 */
+#define SDHCI_QUIRK2_BROKEN_DDR50			(1<<7)
+/* Stop command (CMD12) can set Transfer Complete when not using MMC_RSP_BUSY */
+#define SDHCI_QUIRK2_STOP_WITH_TC			(1<<8)
+/* Controller does not support 64-bit DMA */
+#define SDHCI_QUIRK2_BROKEN_64_BIT_DMA			(1<<9)
+/* need clear transfer mode register before send cmd */
+#define SDHCI_QUIRK2_CLEAR_TRANSFERMODE_REG_BEFORE_CMD	(1<<10)
+/* Capability register bit-63 indicates HS400 support */
+#define SDHCI_QUIRK2_CAPS_BIT63_FOR_HS400		(1<<11)
+/* forced tuned clock */
+#define SDHCI_QUIRK2_TUNING_WORK_AROUND			(1<<12)
+/* disable the block count for single block transactions */
+#define SDHCI_QUIRK2_SUPPORT_SINGLE			(1<<13)
+/* Controller broken with using ACMD23 */
+#define SDHCI_QUIRK2_ACMD23_BROKEN			(1<<14)
+
+	int irq;		/* Device IRQ */
+	void __iomem *ioaddr;	/* Mapped address */
+
+	const struct sdhci_ops *ops;	/* Low level hw interface */
+
+	/* Internal data */
+	struct mmc_host *mmc;	/* MMC structure */
+	u64 dma_mask;		/* custom DMA mask */
+
+#if defined(CONFIG_LEDS_CLASS) || defined(CONFIG_LEDS_CLASS_MODULE)
+	struct led_classdev led;	/* LED control */
+	char led_name[32];
+#endif
+
+	spinlock_t lock;	/* Mutex */
+
+	int flags;		/* Host attributes */
+#define SDHCI_USE_SDMA		(1<<0)	/* Host is SDMA capable */
+#define SDHCI_USE_ADMA		(1<<1)	/* Host is ADMA capable */
+#define SDHCI_REQ_USE_DMA	(1<<2)	/* Use DMA for this req. */
+#define SDHCI_DEVICE_DEAD	(1<<3)	/* Device unresponsive */
+#define SDHCI_SDR50_NEEDS_TUNING (1<<4)	/* SDR50 needs tuning */
+#define SDHCI_NEEDS_RETUNING	(1<<5)	/* Host needs retuning */
+#define SDHCI_AUTO_CMD12	(1<<6)	/* Auto CMD12 support */
+#define SDHCI_AUTO_CMD23	(1<<7)	/* Auto CMD23 support */
+#define SDHCI_PV_ENABLED	(1<<8)	/* Preset value enabled */
+#define SDHCI_SDIO_IRQ_ENABLED	(1<<9)	/* SDIO irq enabled */
+#define SDHCI_SDR104_NEEDS_TUNING (1<<10)	/* SDR104/HS200 needs tuning */
+#define SDHCI_USING_RETUNING_TIMER (1<<11)	/* Host is using a retuning timer for the card */
+#define SDHCI_USE_64_BIT_DMA	(1<<12)	/* Use 64-bit DMA */
+#define SDHCI_HS400_TUNING	(1<<13)	/* Tuning for HS400 */
+
+	unsigned int version;	/* SDHCI spec. version */
+
+	unsigned int max_clk;	/* Max possible freq (MHz) */
+	unsigned int timeout_clk;	/* Timeout freq (KHz) */
+	unsigned int clk_mul;	/* Clock Muliplier value */
+
+	unsigned int clock;	/* Current clock (MHz) */
+	u8 pwr;			/* Current voltage */
+
+	bool runtime_suspended;	/* Host is runtime suspended */
+	bool bus_on;		/* Bus power prevents runtime suspend */
+	bool preset_enabled;	/* Preset is enabled */
+
+	struct mmc_request *mrq;	/* Current request */
+	struct mmc_command *cmd;	/* Current command */
+	struct mmc_data *data;	/* Current data request */
+	unsigned int data_early:1;	/* Data finished before cmd */
+	unsigned int busy_handle:1;	/* Handling the order of Busy-end */
+
+	struct sg_mapping_iter sg_miter;	/* SG state for PIO */
+	unsigned int blocks;	/* remaining PIO blocks */
+
+	int sg_count;		/* Mapped sg entries */
+
+	void *adma_table;	/* ADMA descriptor table */
+	void *align_buffer;	/* Bounce buffer */
+
+	size_t adma_table_sz;	/* ADMA descriptor table size */
+	size_t align_buffer_sz;	/* Bounce buffer size */
+
+	dma_addr_t adma_addr;	/* Mapped ADMA descr. table */
+	dma_addr_t align_addr;	/* Mapped bounce buffer */
+
+	unsigned int desc_sz;	/* ADMA descriptor size */
+	unsigned int align_sz;	/* ADMA alignment */
+	unsigned int align_mask;	/* ADMA alignment mask */
+
+	struct tasklet_struct finish_tasklet;	/* Tasklet structures */
+
+	struct timer_list timer;	/* Timer for timeouts */
+
+	u32 caps;		/* Alternative CAPABILITY_0 */
+	u32 caps1;		/* Alternative CAPABILITY_1 */
+
+	unsigned int            ocr_avail_sdio;	/* OCR bit masks */
+	unsigned int            ocr_avail_sd;
+	unsigned int            ocr_avail_mmc;
+	u32 ocr_mask;		/* available voltages */
+
+	unsigned		timing;		/* Current timing */
+
+	u32			thread_isr;
+
+	/* cached registers */
+	u32			ier;
+
+	wait_queue_head_t	buf_ready_int;	/* Waitqueue for Buffer Read Ready interrupt */
+	unsigned int		tuning_done;	/* Condition flag set when CMD19 succeeds */
+
+	unsigned int		tuning_count;	/* Timer count for re-tuning */
+	unsigned int		tuning_mode;	/* Re-tuning mode supported by host */
+#define SDHCI_TUNING_MODE_1	0
+	struct timer_list	tuning_timer;	/* Timer for tuning */
+
+	struct sdhci_host_next	next_data;
+	unsigned long private[0] ____cacheline_aligned;
+};
+
 struct sdhci_ops {
 #ifdef CONFIG_MMC_SDHCI_IO_ACCESSORS
 	u32		(*read_l)(struct sdhci_host *host, int reg);
diff --git a/drivers/mmc/host/sh_mmcif.c b/drivers/mmc/host/sh_mmcif.c
index 7d9d6a321521..072f67066df3 100644
--- a/drivers/mmc/host/sh_mmcif.c
+++ b/drivers/mmc/host/sh_mmcif.c
@@ -875,6 +875,7 @@ static void sh_mmcif_start_cmd(struct sh_mmcif_host *host,
 	struct mmc_command *cmd = mrq->cmd;
 	u32 opc = cmd->opcode;
 	u32 mask;
+	unsigned long flags;
 
 	switch (opc) {
 	/* response busy check */
@@ -909,10 +910,12 @@ static void sh_mmcif_start_cmd(struct sh_mmcif_host *host,
 	/* set arg */
 	sh_mmcif_writel(host->addr, MMCIF_CE_ARG, cmd->arg);
 	/* set cmd */
+	spin_lock_irqsave(&host->lock, flags);
 	sh_mmcif_writel(host->addr, MMCIF_CE_CMD_SET, opc);
 
 	host->wait_for = MMCIF_WAIT_FOR_CMD;
 	schedule_delayed_work(&host->timeout_work, host->timeout);
+	spin_unlock_irqrestore(&host->lock, flags);
 }
 
 static void sh_mmcif_stop_cmd(struct sh_mmcif_host *host,
@@ -1171,6 +1174,12 @@ static irqreturn_t sh_mmcif_irqt(int irq, void *dev_id)
 	struct sh_mmcif_host *host = dev_id;
 	struct mmc_request *mrq;
 	bool wait = false;
+	unsigned long flags;
+	int wait_work;
+
+	spin_lock_irqsave(&host->lock, flags);
+	wait_work = host->wait_for;
+	spin_unlock_irqrestore(&host->lock, flags);
 
 	cancel_delayed_work_sync(&host->timeout_work);
 
@@ -1188,7 +1197,7 @@ static irqreturn_t sh_mmcif_irqt(int irq, void *dev_id)
 	 * All handlers return true, if processing continues, and false, if the
 	 * request has to be completed - successfully or not
 	 */
-	switch (host->wait_for) {
+	switch (wait_work) {
 	case MMCIF_WAIT_FOR_REQUEST:
 		/* We're too late, the timeout has already kicked in */
 		mutex_unlock(&host->thread_lock);
@@ -1312,15 +1321,15 @@ static void mmcif_timeout_work(struct work_struct *work)
 		/* Don't run after mmc_remove_host() */
 		return;
 
-	dev_err(&host->pd->dev, "Timeout waiting for %u on CMD%u\n",
-		host->wait_for, mrq->cmd->opcode);
-
 	spin_lock_irqsave(&host->lock, flags);
 	if (host->state == STATE_IDLE) {
 		spin_unlock_irqrestore(&host->lock, flags);
 		return;
 	}
 
+	dev_err(&host->pd->dev, "Timeout waiting for %u on CMD%u\n",
+		host->wait_for, mrq->cmd->opcode);
+
 	host->state = STATE_TIMEOUT;
 	spin_unlock_irqrestore(&host->lock, flags);
 
diff --git a/drivers/mmc/host/sunxi-mmc.c b/drivers/mmc/host/sunxi-mmc.c
index e8a4218b5726..4d3e1ffe5508 100644
--- a/drivers/mmc/host/sunxi-mmc.c
+++ b/drivers/mmc/host/sunxi-mmc.c
@@ -293,7 +293,7 @@ static void sunxi_mmc_init_idma_des(struct sunxi_mmc_host *host,
 				    struct mmc_data *data)
 {
 	struct sunxi_idma_des *pdes = (struct sunxi_idma_des *)host->sg_cpu;
-	struct sunxi_idma_des *pdes_pa = (struct sunxi_idma_des *)host->sg_dma;
+	dma_addr_t next_desc = host->sg_dma;
 	int i, max_len = (1 << host->idma_des_size_bits);
 
 	for (i = 0; i < data->sg_len; i++) {
@@ -305,8 +305,9 @@ static void sunxi_mmc_init_idma_des(struct sunxi_mmc_host *host,
 		else
 			pdes[i].buf_size = data->sg[i].length;
 
+		next_desc += sizeof(struct sunxi_idma_des);
 		pdes[i].buf_addr_ptr1 = sg_dma_address(&data->sg[i]);
-		pdes[i].buf_addr_ptr2 = (u32)&pdes_pa[i + 1];
+		pdes[i].buf_addr_ptr2 = (u32)next_desc;
 	}
 
 	pdes[0].config |= SDXC_IDMAC_DES0_FD;
@@ -930,7 +931,9 @@ static int sunxi_mmc_resource_request(struct sunxi_mmc_host *host,
 		return PTR_ERR(host->clk_sample);
 	}
 
-	host->reset = devm_reset_control_get(&pdev->dev, "ahb");
+	host->reset = devm_reset_control_get_optional(&pdev->dev, "ahb");
+	if (PTR_ERR(host->reset) == -EPROBE_DEFER)
+		return PTR_ERR(host->reset);
 
 	ret = clk_prepare_enable(host->clk_ahb);
 	if (ret) {
@@ -1028,7 +1031,7 @@ static int sunxi_mmc_probe(struct platform_device *pdev)
 	mmc->f_min		=   400000;
 	mmc->f_max		= 50000000;
 	mmc->caps	       |= MMC_CAP_MMC_HIGHSPEED | MMC_CAP_SD_HIGHSPEED |
-				  MMC_CAP_ERASE;
+				  MMC_CAP_ERASE | MMC_CAP_SDIO_IRQ;
 
 	ret = mmc_of_parse(mmc);
 	if (ret)
diff --git a/drivers/mmc/host/tmio_mmc_pio.c b/drivers/mmc/host/tmio_mmc_pio.c
index a31c3573d386..dba7e1c19dd7 100644
--- a/drivers/mmc/host/tmio_mmc_pio.c
+++ b/drivers/mmc/host/tmio_mmc_pio.c
@@ -1073,8 +1073,6 @@ EXPORT_SYMBOL(tmio_mmc_host_alloc);
 void tmio_mmc_host_free(struct tmio_mmc_host *host)
 {
 	mmc_free_host(host->mmc);
-
-	host->mmc = NULL;
 }
 EXPORT_SYMBOL(tmio_mmc_host_free);
 
diff --git a/drivers/mmc/host/wmt-sdmmc.c b/drivers/mmc/host/wmt-sdmmc.c
index dd2e1aa95ba3..5af00559e9d6 100644
--- a/drivers/mmc/host/wmt-sdmmc.c
+++ b/drivers/mmc/host/wmt-sdmmc.c
@@ -744,7 +744,7 @@ static struct wmt_mci_caps wm8505_caps = {
 	.max_blk_size = 2048,
 };
 
-static struct of_device_id wmt_mci_dt_ids[] = {
+static const struct of_device_id wmt_mci_dt_ids[] = {
 	{ .compatible = "wm,wm8505-sdhc", .data = &wm8505_caps },
 	{ /* Sentinel */ },
 };
diff --git a/drivers/mtd/nand/Kconfig b/drivers/mtd/nand/Kconfig
index 5b76a173cd95..5897d8d8fa5a 100644
--- a/drivers/mtd/nand/Kconfig
+++ b/drivers/mtd/nand/Kconfig
@@ -526,6 +526,7 @@ config MTD_NAND_SUNXI
 
 config MTD_NAND_HISI504
 	tristate "Support for NAND controller on Hisilicon SoC Hip04"
+	depends on HAS_DMA
 	help
 	  Enables support for NAND controller on Hisilicon SoC Hip04.
 
diff --git a/drivers/mtd/nand/hisi504_nand.c b/drivers/mtd/nand/hisi504_nand.c
index 289ad3ac3e80..8dcc7b8fee40 100644
--- a/drivers/mtd/nand/hisi504_nand.c
+++ b/drivers/mtd/nand/hisi504_nand.c
@@ -758,8 +758,7 @@ static int hisi_nfc_probe(struct platform_device *pdev)
 
 	hisi_nfc_host_init(host);
 
-	ret = devm_request_irq(dev, irq, hinfc_irq_handle, IRQF_DISABLED,
-				"nandc", host);
+	ret = devm_request_irq(dev, irq, hinfc_irq_handle, 0x0, "nandc", host);
 	if (ret) {
 		dev_err(dev, "failed to request IRQ\n");
 		goto err_res;
diff --git a/drivers/mtd/nand/pxa3xx_nand.c b/drivers/mtd/nand/pxa3xx_nand.c
index 96b0b1d27df1..10b1f7a4fe50 100644
--- a/drivers/mtd/nand/pxa3xx_nand.c
+++ b/drivers/mtd/nand/pxa3xx_nand.c
@@ -480,6 +480,42 @@ static void disable_int(struct pxa3xx_nand_info *info, uint32_t int_mask)
 	nand_writel(info, NDCR, ndcr | int_mask);
 }
 
+static void drain_fifo(struct pxa3xx_nand_info *info, void *data, int len)
+{
+	if (info->ecc_bch) {
+		int timeout;
+
+		/*
+		 * According to the datasheet, when reading from NDDB
+		 * with BCH enabled, after each 32 bytes reads, we
+		 * have to make sure that the NDSR.RDDREQ bit is set.
+		 *
+		 * Drain the FIFO 8 32 bits reads at a time, and skip
+		 * the polling on the last read.
+		 */
+		while (len > 8) {
+			__raw_readsl(info->mmio_base + NDDB, data, 8);
+
+			for (timeout = 0;
+			     !(nand_readl(info, NDSR) & NDSR_RDDREQ);
+			     timeout++) {
+				if (timeout >= 5) {
+					dev_err(&info->pdev->dev,
+						"Timeout on RDDREQ while draining the FIFO\n");
+					return;
+				}
+
+				mdelay(1);
+			}
+
+			data += 32;
+			len -= 8;
+		}
+	}
+
+	__raw_readsl(info->mmio_base + NDDB, data, len);
+}
+
 static void handle_data_pio(struct pxa3xx_nand_info *info)
 {
 	unsigned int do_bytes = min(info->data_size, info->chunk_size);
@@ -496,14 +532,14 @@ static void handle_data_pio(struct pxa3xx_nand_info *info)
 				      DIV_ROUND_UP(info->oob_size, 4));
 		break;
 	case STATE_PIO_READING:
-		__raw_readsl(info->mmio_base + NDDB,
-			     info->data_buff + info->data_buff_pos,
-			     DIV_ROUND_UP(do_bytes, 4));
+		drain_fifo(info,
+			   info->data_buff + info->data_buff_pos,
+			   DIV_ROUND_UP(do_bytes, 4));
 
 		if (info->oob_size > 0)
-			__raw_readsl(info->mmio_base + NDDB,
-				     info->oob_buff + info->oob_buff_pos,
-				     DIV_ROUND_UP(info->oob_size, 4));
+			drain_fifo(info,
+				   info->oob_buff + info->oob_buff_pos,
+				   DIV_ROUND_UP(info->oob_size, 4));
 		break;
 	default:
 		dev_err(&info->pdev->dev, "%s: invalid state %d\n", __func__,
@@ -1572,6 +1608,8 @@ static int alloc_nand_resource(struct platform_device *pdev)
 	int ret, irq, cs;
 
 	pdata = dev_get_platdata(&pdev->dev);
+	if (pdata->num_cs <= 0)
+		return -ENODEV;
 	info = devm_kzalloc(&pdev->dev, sizeof(*info) + (sizeof(*mtd) +
 			    sizeof(*host)) * pdata->num_cs, GFP_KERNEL);
 	if (!info)
diff --git a/drivers/mtd/ubi/eba.c b/drivers/mtd/ubi/eba.c
index da4c79259f67..16e34b37d134 100644
--- a/drivers/mtd/ubi/eba.c
+++ b/drivers/mtd/ubi/eba.c
@@ -425,9 +425,10 @@ retry:
 					ubi_warn(ubi, "corrupted VID header at PEB %d, LEB %d:%d",
 						 pnum, vol_id, lnum);
 					err = -EBADMSG;
-				} else
+				} else {
 					err = -EINVAL;
 					ubi_ro_mode(ubi);
+				}
 			}
 			goto out_free;
 		} else if (err == UBI_IO_BITFLIPS)
diff --git a/drivers/net/Kconfig b/drivers/net/Kconfig
index 84673ebcf428..df51d6025a90 100644
--- a/drivers/net/Kconfig
+++ b/drivers/net/Kconfig
@@ -157,7 +157,7 @@ config IPVLAN
       making it transparent to the connected L2 switch.
 
       Ipvlan devices can be added using the "ip" command from the
-      iproute2 package starting with the iproute2-X.Y.ZZ release:
+      iproute2 package starting with the iproute2-3.19 release:
 
       "ip link add link <main-dev> [ NAME ] type ipvlan"
 
diff --git a/drivers/net/appletalk/Kconfig b/drivers/net/appletalk/Kconfig
index 4ce6ca5f3d36..dc6b78e5342f 100644
--- a/drivers/net/appletalk/Kconfig
+++ b/drivers/net/appletalk/Kconfig
@@ -40,7 +40,7 @@ config DEV_APPLETALK
 
 config LTPC
 	tristate "Apple/Farallon LocalTalk PC support"
-	depends on DEV_APPLETALK && (ISA || EISA) && ISA_DMA_API
+	depends on DEV_APPLETALK && (ISA || EISA) && ISA_DMA_API && VIRT_TO_BUS
 	help
 	  This allows you to use the AppleTalk PC card to connect to LocalTalk
 	  networks. The card is also known as the Farallon PhoneNet PC card.
diff --git a/drivers/net/bonding/bond_main.c b/drivers/net/bonding/bond_main.c
index b979c265fc51..089a4028859d 100644
--- a/drivers/net/bonding/bond_main.c
+++ b/drivers/net/bonding/bond_main.c
@@ -3850,7 +3850,8 @@ static inline int bond_slave_override(struct bonding *bond,
 	/* Find out if any slaves have the same mapping as this skb. */
 	bond_for_each_slave_rcu(bond, slave, iter) {
 		if (slave->queue_id == skb->queue_mapping) {
-			if (bond_slave_can_tx(slave)) {
+			if (bond_slave_is_up(slave) &&
+			    slave->link == BOND_LINK_UP) {
 				bond_dev_queue_xmit(bond, skb, slave->dev);
 				return 0;
 			}
diff --git a/drivers/net/can/Kconfig b/drivers/net/can/Kconfig
index 98d73aab52fe..58808f651452 100644
--- a/drivers/net/can/Kconfig
+++ b/drivers/net/can/Kconfig
@@ -131,7 +131,7 @@ config CAN_RCAR
 
 config CAN_XILINXCAN
 	tristate "Xilinx CAN"
-	depends on ARCH_ZYNQ || MICROBLAZE || COMPILE_TEST
+	depends on ARCH_ZYNQ || ARM64 || MICROBLAZE || COMPILE_TEST
 	depends on COMMON_CLK && HAS_IOMEM
 	---help---
 	  Xilinx CAN driver. This driver supports both soft AXI CAN IP and
diff --git a/drivers/net/can/dev.c b/drivers/net/can/dev.c
index 3c82e02e3dae..b0f69248cb71 100644
--- a/drivers/net/can/dev.c
+++ b/drivers/net/can/dev.c
@@ -579,6 +579,10 @@ struct sk_buff *alloc_can_skb(struct net_device *dev, struct can_frame **cf)
 	skb->pkt_type = PACKET_BROADCAST;
 	skb->ip_summed = CHECKSUM_UNNECESSARY;
 
+	skb_reset_mac_header(skb);
+	skb_reset_network_header(skb);
+	skb_reset_transport_header(skb);
+
 	can_skb_reserve(skb);
 	can_skb_prv(skb)->ifindex = dev->ifindex;
 
@@ -603,6 +607,10 @@ struct sk_buff *alloc_canfd_skb(struct net_device *dev,
 	skb->pkt_type = PACKET_BROADCAST;
 	skb->ip_summed = CHECKSUM_UNNECESSARY;
 
+	skb_reset_mac_header(skb);
+	skb_reset_network_header(skb);
+	skb_reset_transport_header(skb);
+
 	can_skb_reserve(skb);
 	can_skb_prv(skb)->ifindex = dev->ifindex;
 
diff --git a/drivers/net/can/flexcan.c b/drivers/net/can/flexcan.c
index 80c46ad4cee4..ad0a7e8c2c2b 100644
--- a/drivers/net/can/flexcan.c
+++ b/drivers/net/can/flexcan.c
@@ -592,13 +592,12 @@ static int flexcan_poll_state(struct net_device *dev, u32 reg_esr)
 		rx_state = unlikely(reg_esr & FLEXCAN_ESR_RX_WRN) ?
 			   CAN_STATE_ERROR_WARNING : CAN_STATE_ERROR_ACTIVE;
 		new_state = max(tx_state, rx_state);
-	} else if (unlikely(flt == FLEXCAN_ESR_FLT_CONF_PASSIVE)) {
+	} else {
 		__flexcan_get_berr_counter(dev, &bec);
-		new_state = CAN_STATE_ERROR_PASSIVE;
+		new_state = flt == FLEXCAN_ESR_FLT_CONF_PASSIVE ?
+			    CAN_STATE_ERROR_PASSIVE : CAN_STATE_BUS_OFF;
 		rx_state = bec.rxerr >= bec.txerr ? new_state : 0;
 		tx_state = bec.rxerr <= bec.txerr ? new_state : 0;
-	} else {
-		new_state = CAN_STATE_BUS_OFF;
 	}
 
 	/* state hasn't changed */
@@ -1158,12 +1157,19 @@ static int flexcan_probe(struct platform_device *pdev)
 	const struct flexcan_devtype_data *devtype_data;
 	struct net_device *dev;
 	struct flexcan_priv *priv;
+	struct regulator *reg_xceiver;
 	struct resource *mem;
 	struct clk *clk_ipg = NULL, *clk_per = NULL;
 	void __iomem *base;
 	int err, irq;
 	u32 clock_freq = 0;
 
+	reg_xceiver = devm_regulator_get(&pdev->dev, "xceiver");
+	if (PTR_ERR(reg_xceiver) == -EPROBE_DEFER)
+		return -EPROBE_DEFER;
+	else if (IS_ERR(reg_xceiver))
+		reg_xceiver = NULL;
+
 	if (pdev->dev.of_node)
 		of_property_read_u32(pdev->dev.of_node,
 						"clock-frequency", &clock_freq);
@@ -1224,9 +1230,7 @@ static int flexcan_probe(struct platform_device *pdev)
 	priv->pdata = dev_get_platdata(&pdev->dev);
 	priv->devtype_data = devtype_data;
 
-	priv->reg_xceiver = devm_regulator_get(&pdev->dev, "xceiver");
-	if (IS_ERR(priv->reg_xceiver))
-		priv->reg_xceiver = NULL;
+	priv->reg_xceiver = reg_xceiver;
 
 	netif_napi_add(dev, &priv->napi, flexcan_poll, FLEXCAN_NAPI_WEIGHT);
 
diff --git a/drivers/net/can/usb/gs_usb.c b/drivers/net/can/usb/gs_usb.c
index 009acc8641fc..8b4d3e6875eb 100644
--- a/drivers/net/can/usb/gs_usb.c
+++ b/drivers/net/can/usb/gs_usb.c
@@ -901,6 +901,8 @@ static int gs_usb_probe(struct usb_interface *intf, const struct usb_device_id *
 	}
 
 	dev = kzalloc(sizeof(*dev), GFP_KERNEL);
+	if (!dev)
+		return -ENOMEM;
 	init_usb_anchor(&dev->rx_submitted);
 
 	atomic_set(&dev->active_channels, 0);
diff --git a/drivers/net/can/usb/kvaser_usb.c b/drivers/net/can/usb/kvaser_usb.c
index 2928f7003041..57611fd91229 100644
--- a/drivers/net/can/usb/kvaser_usb.c
+++ b/drivers/net/can/usb/kvaser_usb.c
@@ -14,6 +14,8 @@
  * Copyright (C) 2015 Valeo S.A.
  */
 
+#include <linux/spinlock.h>
+#include <linux/kernel.h>
 #include <linux/completion.h>
 #include <linux/module.h>
 #include <linux/netdevice.h>
@@ -23,7 +25,6 @@
 #include <linux/can/dev.h>
 #include <linux/can/error.h>
 
-#define MAX_TX_URBS			16
 #define MAX_RX_URBS			4
 #define START_TIMEOUT			1000 /* msecs */
 #define STOP_TIMEOUT			1000 /* msecs */
@@ -441,6 +442,7 @@ struct kvaser_usb_error_summary {
 	};
 };
 
+/* Context for an outstanding, not yet ACKed, transmission */
 struct kvaser_usb_tx_urb_context {
 	struct kvaser_usb_net_priv *priv;
 	u32 echo_index;
@@ -454,8 +456,13 @@ struct kvaser_usb {
 	struct usb_endpoint_descriptor *bulk_in, *bulk_out;
 	struct usb_anchor rx_submitted;
 
+	/* @max_tx_urbs: Firmware-reported maximum number of oustanding,
+	 * not yet ACKed, transmissions on this device. This value is
+	 * also used as a sentinel for marking free tx contexts.
+	 */
 	u32 fw_version;
 	unsigned int nchannels;
+	unsigned int max_tx_urbs;
 	enum kvaser_usb_family family;
 
 	bool rxinitdone;
@@ -465,18 +472,18 @@ struct kvaser_usb {
 
 struct kvaser_usb_net_priv {
 	struct can_priv can;
-
-	atomic_t active_tx_urbs;
-	struct usb_anchor tx_submitted;
-	struct kvaser_usb_tx_urb_context tx_contexts[MAX_TX_URBS];
-
-	struct completion start_comp, stop_comp;
+	struct can_berr_counter bec;
 
 	struct kvaser_usb *dev;
 	struct net_device *netdev;
 	int channel;
 
-	struct can_berr_counter bec;
+	struct completion start_comp, stop_comp;
+	struct usb_anchor tx_submitted;
+
+	spinlock_t tx_contexts_lock;
+	int active_tx_contexts;
+	struct kvaser_usb_tx_urb_context tx_contexts[];
 };
 
 static const struct usb_device_id kvaser_usb_table[] = {
@@ -584,8 +591,15 @@ static int kvaser_usb_wait_msg(const struct kvaser_usb *dev, u8 id,
 		while (pos <= actual_len - MSG_HEADER_LEN) {
 			tmp = buf + pos;
 
-			if (!tmp->len)
-				break;
+			/* Handle messages crossing the USB endpoint max packet
+			 * size boundary. Check kvaser_usb_read_bulk_callback()
+			 * for further details.
+			 */
+			if (tmp->len == 0) {
+				pos = round_up(pos, le16_to_cpu(dev->bulk_in->
+								wMaxPacketSize));
+				continue;
+			}
 
 			if (pos + tmp->len > actual_len) {
 				dev_err(dev->udev->dev.parent,
@@ -647,9 +661,13 @@ static int kvaser_usb_get_software_info(struct kvaser_usb *dev)
 	switch (dev->family) {
 	case KVASER_LEAF:
 		dev->fw_version = le32_to_cpu(msg.u.leaf.softinfo.fw_version);
+		dev->max_tx_urbs =
+			le16_to_cpu(msg.u.leaf.softinfo.max_outstanding_tx);
 		break;
 	case KVASER_USBCAN:
 		dev->fw_version = le32_to_cpu(msg.u.usbcan.softinfo.fw_version);
+		dev->max_tx_urbs =
+			le16_to_cpu(msg.u.usbcan.softinfo.max_outstanding_tx);
 		break;
 	}
 
@@ -686,6 +704,7 @@ static void kvaser_usb_tx_acknowledge(const struct kvaser_usb *dev,
 	struct kvaser_usb_net_priv *priv;
 	struct sk_buff *skb;
 	struct can_frame *cf;
+	unsigned long flags;
 	u8 channel, tid;
 
 	channel = msg->u.tx_acknowledge_header.channel;
@@ -704,7 +723,7 @@ static void kvaser_usb_tx_acknowledge(const struct kvaser_usb *dev,
 
 	stats = &priv->netdev->stats;
 
-	context = &priv->tx_contexts[tid % MAX_TX_URBS];
+	context = &priv->tx_contexts[tid % dev->max_tx_urbs];
 
 	/* Sometimes the state change doesn't come after a bus-off event */
 	if (priv->can.restart_ms &&
@@ -729,12 +748,15 @@ static void kvaser_usb_tx_acknowledge(const struct kvaser_usb *dev,
 
 	stats->tx_packets++;
 	stats->tx_bytes += context->dlc;
-	can_get_echo_skb(priv->netdev, context->echo_index);
 
-	context->echo_index = MAX_TX_URBS;
-	atomic_dec(&priv->active_tx_urbs);
+	spin_lock_irqsave(&priv->tx_contexts_lock, flags);
 
+	can_get_echo_skb(priv->netdev, context->echo_index);
+	context->echo_index = dev->max_tx_urbs;
+	--priv->active_tx_contexts;
 	netif_wake_queue(priv->netdev);
+
+	spin_unlock_irqrestore(&priv->tx_contexts_lock, flags);
 }
 
 static void kvaser_usb_simple_msg_callback(struct urb *urb)
@@ -787,7 +809,6 @@ static int kvaser_usb_simple_msg_async(struct kvaser_usb_net_priv *priv,
 		netdev_err(netdev, "Error transmitting URB\n");
 		usb_unanchor_urb(urb);
 		usb_free_urb(urb);
-		kfree(buf);
 		return err;
 	}
 
@@ -796,17 +817,6 @@ static int kvaser_usb_simple_msg_async(struct kvaser_usb_net_priv *priv,
 	return 0;
 }
 
-static void kvaser_usb_unlink_tx_urbs(struct kvaser_usb_net_priv *priv)
-{
-	int i;
-
-	usb_kill_anchored_urbs(&priv->tx_submitted);
-	atomic_set(&priv->active_tx_urbs, 0);
-
-	for (i = 0; i < MAX_TX_URBS; i++)
-		priv->tx_contexts[i].echo_index = MAX_TX_URBS;
-}
-
 static void kvaser_usb_rx_error_update_can_state(struct kvaser_usb_net_priv *priv,
 						 const struct kvaser_usb_error_summary *es,
 						 struct can_frame *cf)
@@ -1317,8 +1327,20 @@ static void kvaser_usb_read_bulk_callback(struct urb *urb)
 	while (pos <= urb->actual_length - MSG_HEADER_LEN) {
 		msg = urb->transfer_buffer + pos;
 
-		if (!msg->len)
-			break;
+		/* The Kvaser firmware can only read and write messages that
+		 * does not cross the USB's endpoint wMaxPacketSize boundary.
+		 * If a follow-up command crosses such boundary, firmware puts
+		 * a placeholder zero-length command in its place then aligns
+		 * the real command to the next max packet size.
+		 *
+		 * Handle such cases or we're going to miss a significant
+		 * number of events in case of a heavy rx load on the bus.
+		 */
+		if (msg->len == 0) {
+			pos = round_up(pos, le16_to_cpu(dev->bulk_in->
+							wMaxPacketSize));
+			continue;
+		}
 
 		if (pos + msg->len > urb->actual_length) {
 			dev_err(dev->udev->dev.parent, "Format error\n");
@@ -1326,7 +1348,6 @@ static void kvaser_usb_read_bulk_callback(struct urb *urb)
 		}
 
 		kvaser_usb_handle_message(dev, msg);
-
 		pos += msg->len;
 	}
 
@@ -1498,6 +1519,26 @@ error:
 	return err;
 }
 
+static void kvaser_usb_reset_tx_urb_contexts(struct kvaser_usb_net_priv *priv)
+{
+	int i, max_tx_urbs;
+
+	max_tx_urbs = priv->dev->max_tx_urbs;
+
+	priv->active_tx_contexts = 0;
+	for (i = 0; i < max_tx_urbs; i++)
+		priv->tx_contexts[i].echo_index = max_tx_urbs;
+}
+
+/* This method might sleep. Do not call it in the atomic context
+ * of URB completions.
+ */
+static void kvaser_usb_unlink_tx_urbs(struct kvaser_usb_net_priv *priv)
+{
+	usb_kill_anchored_urbs(&priv->tx_submitted);
+	kvaser_usb_reset_tx_urb_contexts(priv);
+}
+
 static void kvaser_usb_unlink_all_urbs(struct kvaser_usb *dev)
 {
 	int i;
@@ -1615,9 +1656,9 @@ static netdev_tx_t kvaser_usb_start_xmit(struct sk_buff *skb,
 	struct urb *urb;
 	void *buf;
 	struct kvaser_msg *msg;
-	int i, err;
-	int ret = NETDEV_TX_OK;
+	int i, err, ret = NETDEV_TX_OK;
 	u8 *msg_tx_can_flags = NULL;		/* GCC */
+	unsigned long flags;
 
 	if (can_dropped_invalid_skb(netdev, skb))
 		return NETDEV_TX_OK;
@@ -1634,7 +1675,7 @@ static netdev_tx_t kvaser_usb_start_xmit(struct sk_buff *skb,
 	if (!buf) {
 		stats->tx_dropped++;
 		dev_kfree_skb(skb);
-		goto nobufmem;
+		goto freeurb;
 	}
 
 	msg = buf;
@@ -1671,22 +1712,32 @@ static netdev_tx_t kvaser_usb_start_xmit(struct sk_buff *skb,
 	if (cf->can_id & CAN_RTR_FLAG)
 		*msg_tx_can_flags |= MSG_FLAG_REMOTE_FRAME;
 
-	for (i = 0; i < ARRAY_SIZE(priv->tx_contexts); i++) {
-		if (priv->tx_contexts[i].echo_index == MAX_TX_URBS) {
+	spin_lock_irqsave(&priv->tx_contexts_lock, flags);
+	for (i = 0; i < dev->max_tx_urbs; i++) {
+		if (priv->tx_contexts[i].echo_index == dev->max_tx_urbs) {
 			context = &priv->tx_contexts[i];
+
+			context->echo_index = i;
+			can_put_echo_skb(skb, netdev, context->echo_index);
+			++priv->active_tx_contexts;
+			if (priv->active_tx_contexts >= dev->max_tx_urbs)
+				netif_stop_queue(netdev);
+
 			break;
 		}
 	}
+	spin_unlock_irqrestore(&priv->tx_contexts_lock, flags);
 
 	/* This should never happen; it implies a flow control bug */
 	if (!context) {
 		netdev_warn(netdev, "cannot find free context\n");
+
+		kfree(buf);
 		ret =  NETDEV_TX_BUSY;
-		goto releasebuf;
+		goto freeurb;
 	}
 
 	context->priv = priv;
-	context->echo_index = i;
 	context->dlc = cf->can_dlc;
 
 	msg->u.tx_can.tid = context->echo_index;
@@ -1698,18 +1749,17 @@ static netdev_tx_t kvaser_usb_start_xmit(struct sk_buff *skb,
 			  kvaser_usb_write_bulk_callback, context);
 	usb_anchor_urb(urb, &priv->tx_submitted);
 
-	can_put_echo_skb(skb, netdev, context->echo_index);
-
-	atomic_inc(&priv->active_tx_urbs);
-
-	if (atomic_read(&priv->active_tx_urbs) >= MAX_TX_URBS)
-		netif_stop_queue(netdev);
-
 	err = usb_submit_urb(urb, GFP_ATOMIC);
 	if (unlikely(err)) {
+		spin_lock_irqsave(&priv->tx_contexts_lock, flags);
+
 		can_free_echo_skb(netdev, context->echo_index);
+		context->echo_index = dev->max_tx_urbs;
+		--priv->active_tx_contexts;
+		netif_wake_queue(netdev);
+
+		spin_unlock_irqrestore(&priv->tx_contexts_lock, flags);
 
-		atomic_dec(&priv->active_tx_urbs);
 		usb_unanchor_urb(urb);
 
 		stats->tx_dropped++;
@@ -1719,16 +1769,12 @@ static netdev_tx_t kvaser_usb_start_xmit(struct sk_buff *skb,
 		else
 			netdev_warn(netdev, "Failed tx_urb %d\n", err);
 
-		goto releasebuf;
+		goto freeurb;
 	}
 
-	usb_free_urb(urb);
-
-	return NETDEV_TX_OK;
+	ret = NETDEV_TX_OK;
 
-releasebuf:
-	kfree(buf);
-nobufmem:
+freeurb:
 	usb_free_urb(urb);
 	return ret;
 }
@@ -1840,13 +1886,15 @@ static int kvaser_usb_init_one(struct usb_interface *intf,
 	struct kvaser_usb *dev = usb_get_intfdata(intf);
 	struct net_device *netdev;
 	struct kvaser_usb_net_priv *priv;
-	int i, err;
+	int err;
 
 	err = kvaser_usb_send_simple_msg(dev, CMD_RESET_CHIP, channel);
 	if (err)
 		return err;
 
-	netdev = alloc_candev(sizeof(*priv), MAX_TX_URBS);
+	netdev = alloc_candev(sizeof(*priv) +
+			      dev->max_tx_urbs * sizeof(*priv->tx_contexts),
+			      dev->max_tx_urbs);
 	if (!netdev) {
 		dev_err(&intf->dev, "Cannot alloc candev\n");
 		return -ENOMEM;
@@ -1854,19 +1902,17 @@ static int kvaser_usb_init_one(struct usb_interface *intf,
 
 	priv = netdev_priv(netdev);
 
+	init_usb_anchor(&priv->tx_submitted);
 	init_completion(&priv->start_comp);
 	init_completion(&priv->stop_comp);
 
-	init_usb_anchor(&priv->tx_submitted);
-	atomic_set(&priv->active_tx_urbs, 0);
-
-	for (i = 0; i < ARRAY_SIZE(priv->tx_contexts); i++)
-		priv->tx_contexts[i].echo_index = MAX_TX_URBS;
-
 	priv->dev = dev;
 	priv->netdev = netdev;
 	priv->channel = channel;
 
+	spin_lock_init(&priv->tx_contexts_lock);
+	kvaser_usb_reset_tx_urb_contexts(priv);
+
 	priv->can.state = CAN_STATE_STOPPED;
 	priv->can.clock.freq = CAN_USB_CLOCK;
 	priv->can.bittiming_const = &kvaser_usb_bittiming_const;
@@ -1976,6 +2022,13 @@ static int kvaser_usb_probe(struct usb_interface *intf,
 		return err;
 	}
 
+	dev_dbg(&intf->dev, "Firmware version: %d.%d.%d\n",
+		((dev->fw_version >> 24) & 0xff),
+		((dev->fw_version >> 16) & 0xff),
+		(dev->fw_version & 0xffff));
+
+	dev_dbg(&intf->dev, "Max oustanding tx = %d URBs\n", dev->max_tx_urbs);
+
 	err = kvaser_usb_get_card_info(dev);
 	if (err) {
 		dev_err(&intf->dev,
@@ -1983,11 +2036,6 @@ static int kvaser_usb_probe(struct usb_interface *intf,
 		return err;
 	}
 
-	dev_dbg(&intf->dev, "Firmware version: %d.%d.%d\n",
-		((dev->fw_version >> 24) & 0xff),
-		((dev->fw_version >> 16) & 0xff),
-		(dev->fw_version & 0xffff));
-
 	for (i = 0; i < dev->nchannels; i++) {
 		err = kvaser_usb_init_one(intf, id, i);
 		if (err) {
diff --git a/drivers/net/can/usb/peak_usb/pcan_ucan.h b/drivers/net/can/usb/peak_usb/pcan_ucan.h
index 1ba7c25002e1..e8fc4952c6b0 100644
--- a/drivers/net/can/usb/peak_usb/pcan_ucan.h
+++ b/drivers/net/can/usb/peak_usb/pcan_ucan.h
@@ -26,8 +26,8 @@
 #define PUCAN_CMD_FILTER_STD		0x008
 #define PUCAN_CMD_TX_ABORT		0x009
 #define PUCAN_CMD_WR_ERR_CNT		0x00a
-#define PUCAN_CMD_RX_FRAME_ENABLE	0x00b
-#define PUCAN_CMD_RX_FRAME_DISABLE	0x00c
+#define PUCAN_CMD_SET_EN_OPTION		0x00b
+#define PUCAN_CMD_CLR_DIS_OPTION	0x00c
 #define PUCAN_CMD_END_OF_COLLECTION	0x3ff
 
 /* uCAN received messages list */
@@ -101,14 +101,15 @@ struct __packed pucan_wr_err_cnt {
 	u16	unused;
 };
 
-/* uCAN RX_FRAME_ENABLE command fields */
-#define PUCAN_FLTEXT_ERROR		0x0001
-#define PUCAN_FLTEXT_BUSLOAD		0x0002
+/* uCAN SET_EN/CLR_DIS _OPTION command fields */
+#define PUCAN_OPTION_ERROR		0x0001
+#define PUCAN_OPTION_BUSLOAD		0x0002
+#define PUCAN_OPTION_CANDFDISO		0x0004
 
-struct __packed pucan_filter_ext {
+struct __packed pucan_options {
 	__le16	opcode_channel;
 
-	__le16	ext_mask;
+	__le16	options;
 	u32	unused;
 };
 
diff --git a/drivers/net/can/usb/peak_usb/pcan_usb_fd.c b/drivers/net/can/usb/peak_usb/pcan_usb_fd.c
index 962c3f027383..a9221ad9f1a0 100644
--- a/drivers/net/can/usb/peak_usb/pcan_usb_fd.c
+++ b/drivers/net/can/usb/peak_usb/pcan_usb_fd.c
@@ -110,13 +110,13 @@ struct __packed pcan_ufd_led {
 	u8	unused[5];
 };
 
-/* Extended usage of uCAN commands CMD_RX_FRAME_xxxABLE for PCAN-USB Pro FD */
+/* Extended usage of uCAN commands CMD_xxx_xx_OPTION for PCAN-USB Pro FD */
 #define PCAN_UFD_FLTEXT_CALIBRATION	0x8000
 
-struct __packed pcan_ufd_filter_ext {
+struct __packed pcan_ufd_options {
 	__le16	opcode_channel;
 
-	__le16	ext_mask;
+	__le16	ucan_mask;
 	u16	unused;
 	__le16	usb_mask;
 };
@@ -251,6 +251,27 @@ static int pcan_usb_fd_build_restart_cmd(struct peak_usb_device *dev, u8 *buf)
 	/* moves the pointer forward */
 	pc += sizeof(struct pucan_wr_err_cnt);
 
+	/* add command to switch from ISO to non-ISO mode, if fw allows it */
+	if (dev->can.ctrlmode_supported & CAN_CTRLMODE_FD_NON_ISO) {
+		struct pucan_options *puo = (struct pucan_options *)pc;
+
+		puo->opcode_channel =
+			(dev->can.ctrlmode & CAN_CTRLMODE_FD_NON_ISO) ?
+			pucan_cmd_opcode_channel(dev,
+						 PUCAN_CMD_CLR_DIS_OPTION) :
+			pucan_cmd_opcode_channel(dev, PUCAN_CMD_SET_EN_OPTION);
+
+		puo->options = cpu_to_le16(PUCAN_OPTION_CANDFDISO);
+
+		/* to be sure that no other extended bits will be taken into
+		 * account
+		 */
+		puo->unused = 0;
+
+		/* moves the pointer forward */
+		pc += sizeof(struct pucan_options);
+	}
+
 	/* next, go back to operational mode */
 	cmd = (struct pucan_command *)pc;
 	cmd->opcode_channel = pucan_cmd_opcode_channel(dev,
@@ -321,21 +342,21 @@ static int pcan_usb_fd_set_filter_std(struct peak_usb_device *dev, int idx,
 	return pcan_usb_fd_send_cmd(dev, cmd);
 }
 
-/* set/unset notifications filter:
+/* set/unset options
  *
- *	onoff	sets(1)/unset(0) notifications
- *	mask	each bit defines a kind of notification to set/unset
+ *	onoff	set(1)/unset(0) options
+ *	mask	each bit defines a kind of options to set/unset
  */
-static int pcan_usb_fd_set_filter_ext(struct peak_usb_device *dev,
-				      bool onoff, u16 ext_mask, u16 usb_mask)
+static int pcan_usb_fd_set_options(struct peak_usb_device *dev,
+				   bool onoff, u16 ucan_mask, u16 usb_mask)
 {
-	struct pcan_ufd_filter_ext *cmd = pcan_usb_fd_cmd_buffer(dev);
+	struct pcan_ufd_options *cmd = pcan_usb_fd_cmd_buffer(dev);
 
 	cmd->opcode_channel = pucan_cmd_opcode_channel(dev,
-					(onoff) ? PUCAN_CMD_RX_FRAME_ENABLE :
-						  PUCAN_CMD_RX_FRAME_DISABLE);
+					(onoff) ? PUCAN_CMD_SET_EN_OPTION :
+						  PUCAN_CMD_CLR_DIS_OPTION);
 
-	cmd->ext_mask = cpu_to_le16(ext_mask);
+	cmd->ucan_mask = cpu_to_le16(ucan_mask);
 	cmd->usb_mask = cpu_to_le16(usb_mask);
 
 	/* send the command */
@@ -770,9 +791,9 @@ static int pcan_usb_fd_start(struct peak_usb_device *dev)
 				       &pcan_usb_pro_fd);
 
 		/* enable USB calibration messages */
-		err = pcan_usb_fd_set_filter_ext(dev, 1,
-						 PUCAN_FLTEXT_ERROR,
-						 PCAN_UFD_FLTEXT_CALIBRATION);
+		err = pcan_usb_fd_set_options(dev, 1,
+					      PUCAN_OPTION_ERROR,
+					      PCAN_UFD_FLTEXT_CALIBRATION);
 	}
 
 	pdev->usb_if->dev_opened_count++;
@@ -806,9 +827,9 @@ static int pcan_usb_fd_stop(struct peak_usb_device *dev)
 
 	/* turn off special msgs for that interface if no other dev opened */
 	if (pdev->usb_if->dev_opened_count == 1)
-		pcan_usb_fd_set_filter_ext(dev, 0,
-					   PUCAN_FLTEXT_ERROR,
-					   PCAN_UFD_FLTEXT_CALIBRATION);
+		pcan_usb_fd_set_options(dev, 0,
+					PUCAN_OPTION_ERROR,
+					PCAN_UFD_FLTEXT_CALIBRATION);
 	pdev->usb_if->dev_opened_count--;
 
 	return 0;
@@ -860,8 +881,14 @@ static int pcan_usb_fd_init(struct peak_usb_device *dev)
 			 pdev->usb_if->fw_info.fw_version[2],
 			 dev->adapter->ctrl_count);
 
-		/* the currently supported hw is non-ISO */
-		dev->can.ctrlmode = CAN_CTRLMODE_FD_NON_ISO;
+		/* check for ability to switch between ISO/non-ISO modes */
+		if (pdev->usb_if->fw_info.fw_version[0] >= 2) {
+			/* firmware >= 2.x supports ISO/non-ISO switching */
+			dev->can.ctrlmode_supported |= CAN_CTRLMODE_FD_NON_ISO;
+		} else {
+			/* firmware < 2.x only supports fixed(!) non-ISO */
+			dev->can.ctrlmode |= CAN_CTRLMODE_FD_NON_ISO;
+		}
 
 		/* tell the hardware the can driver is running */
 		err = pcan_usb_fd_drv_loaded(dev, 1);
@@ -879,6 +906,10 @@ static int pcan_usb_fd_init(struct peak_usb_device *dev)
 
 		pdev->usb_if = ppdev->usb_if;
 		pdev->cmd_buffer_addr = ppdev->cmd_buffer_addr;
+
+		/* do a copy of the ctrlmode[_supported] too */
+		dev->can.ctrlmode = ppdev->dev.can.ctrlmode;
+		dev->can.ctrlmode_supported = ppdev->dev.can.ctrlmode_supported;
 	}
 
 	pdev->usb_if->dev[dev->ctrl_idx] = dev;
@@ -933,9 +964,9 @@ static void pcan_usb_fd_exit(struct peak_usb_device *dev)
 	if (dev->ctrl_idx == 0) {
 		/* turn off calibration message if any device were opened */
 		if (pdev->usb_if->dev_opened_count > 0)
-			pcan_usb_fd_set_filter_ext(dev, 0,
-						   PUCAN_FLTEXT_ERROR,
-						   PCAN_UFD_FLTEXT_CALIBRATION);
+			pcan_usb_fd_set_options(dev, 0,
+						PUCAN_OPTION_ERROR,
+						PCAN_UFD_FLTEXT_CALIBRATION);
 
 		/* tell USB adapter that the driver is being unloaded */
 		pcan_usb_fd_drv_loaded(dev, 0);
diff --git a/drivers/net/dsa/bcm_sf2.h b/drivers/net/dsa/bcm_sf2.h
index ee9f650d5026..7b7053d3c5fa 100644
--- a/drivers/net/dsa/bcm_sf2.h
+++ b/drivers/net/dsa/bcm_sf2.h
@@ -105,8 +105,8 @@ static inline u64 name##_readq(struct bcm_sf2_priv *priv, u32 off)	\
 {									\
 	u32 indir, dir;							\
 	spin_lock(&priv->indir_lock);					\
-	indir = reg_readl(priv, REG_DIR_DATA_READ);			\
 	dir = __raw_readl(priv->name + off);				\
+	indir = reg_readl(priv, REG_DIR_DATA_READ);			\
 	spin_unlock(&priv->indir_lock);					\
 	return (u64)indir << 32 | dir;					\
 }									\
diff --git a/drivers/net/ethernet/8390/axnet_cs.c b/drivers/net/ethernet/8390/axnet_cs.c
index 7769c05543f1..ec6eac1f8c95 100644
--- a/drivers/net/ethernet/8390/axnet_cs.c
+++ b/drivers/net/ethernet/8390/axnet_cs.c
@@ -484,11 +484,8 @@ static int axnet_open(struct net_device *dev)
     link->open++;
 
     info->link_status = 0x00;
-    init_timer(&info->watchdog);
-    info->watchdog.function = ei_watchdog;
-    info->watchdog.data = (u_long)dev;
-    info->watchdog.expires = jiffies + HZ;
-    add_timer(&info->watchdog);
+    setup_timer(&info->watchdog, ei_watchdog, (u_long)dev);
+    mod_timer(&info->watchdog, jiffies + HZ);
 
     return ax_open(dev);
 } /* axnet_open */
diff --git a/drivers/net/ethernet/8390/pcnet_cs.c b/drivers/net/ethernet/8390/pcnet_cs.c
index 9fb7b9d4fd6c..2777289a26c0 100644
--- a/drivers/net/ethernet/8390/pcnet_cs.c
+++ b/drivers/net/ethernet/8390/pcnet_cs.c
@@ -918,11 +918,8 @@ static int pcnet_open(struct net_device *dev)
 
     info->phy_id = info->eth_phy;
     info->link_status = 0x00;
-    init_timer(&info->watchdog);
-    info->watchdog.function = ei_watchdog;
-    info->watchdog.data = (u_long)dev;
-    info->watchdog.expires = jiffies + HZ;
-    add_timer(&info->watchdog);
+    setup_timer(&info->watchdog, ei_watchdog, (u_long)dev);
+    mod_timer(&info->watchdog, jiffies + HZ);
 
     return ei_open(dev);
 } /* pcnet_open */
diff --git a/drivers/net/ethernet/altera/altera_tse_main.c b/drivers/net/ethernet/altera/altera_tse_main.c
index 760c72c6e2ac..6725dc00750b 100644
--- a/drivers/net/ethernet/altera/altera_tse_main.c
+++ b/drivers/net/ethernet/altera/altera_tse_main.c
@@ -376,7 +376,8 @@ static int tse_rx(struct altera_tse_private *priv, int limit)
 	u16 pktlength;
 	u16 pktstatus;
 
-	while ((rxstatus = priv->dmaops->get_rx_status(priv)) != 0) {
+	while (((rxstatus = priv->dmaops->get_rx_status(priv)) != 0) &&
+	       (count < limit))  {
 		pktstatus = rxstatus >> 16;
 		pktlength = rxstatus & 0xffff;
 
@@ -491,28 +492,27 @@ static int tse_poll(struct napi_struct *napi, int budget)
 	struct altera_tse_private *priv =
 			container_of(napi, struct altera_tse_private, napi);
 	int rxcomplete = 0;
-	int txcomplete = 0;
 	unsigned long int flags;
 
-	txcomplete = tse_tx_complete(priv);
+	tse_tx_complete(priv);
 
 	rxcomplete = tse_rx(priv, budget);
 
-	if (rxcomplete >= budget || txcomplete > 0)
-		return rxcomplete;
+	if (rxcomplete < budget) {
 
-	napi_gro_flush(napi, false);
-	__napi_complete(napi);
+		napi_gro_flush(napi, false);
+		__napi_complete(napi);
 
-	netdev_dbg(priv->dev,
-		   "NAPI Complete, did %d packets with budget %d\n",
-		   txcomplete+rxcomplete, budget);
+		netdev_dbg(priv->dev,
+			   "NAPI Complete, did %d packets with budget %d\n",
+			   rxcomplete, budget);
 
-	spin_lock_irqsave(&priv->rxdma_irq_lock, flags);
-	priv->dmaops->enable_rxirq(priv);
-	priv->dmaops->enable_txirq(priv);
-	spin_unlock_irqrestore(&priv->rxdma_irq_lock, flags);
-	return rxcomplete + txcomplete;
+		spin_lock_irqsave(&priv->rxdma_irq_lock, flags);
+		priv->dmaops->enable_rxirq(priv);
+		priv->dmaops->enable_txirq(priv);
+		spin_unlock_irqrestore(&priv->rxdma_irq_lock, flags);
+	}
+	return rxcomplete;
 }
 
 /* DMA TX & RX FIFO interrupt routing
@@ -521,7 +521,6 @@ static irqreturn_t altera_isr(int irq, void *dev_id)
 {
 	struct net_device *dev = dev_id;
 	struct altera_tse_private *priv;
-	unsigned long int flags;
 
 	if (unlikely(!dev)) {
 		pr_err("%s: invalid dev pointer\n", __func__);
@@ -529,20 +528,20 @@ static irqreturn_t altera_isr(int irq, void *dev_id)
 	}
 	priv = netdev_priv(dev);
 
-	/* turn off desc irqs and enable napi rx */
-	spin_lock_irqsave(&priv->rxdma_irq_lock, flags);
+	spin_lock(&priv->rxdma_irq_lock);
+	/* reset IRQs */
+	priv->dmaops->clear_rxirq(priv);
+	priv->dmaops->clear_txirq(priv);
+	spin_unlock(&priv->rxdma_irq_lock);
 
 	if (likely(napi_schedule_prep(&priv->napi))) {
+		spin_lock(&priv->rxdma_irq_lock);
 		priv->dmaops->disable_rxirq(priv);
 		priv->dmaops->disable_txirq(priv);
+		spin_unlock(&priv->rxdma_irq_lock);
 		__napi_schedule(&priv->napi);
 	}
 
-	/* reset IRQs */
-	priv->dmaops->clear_rxirq(priv);
-	priv->dmaops->clear_txirq(priv);
-
-	spin_unlock_irqrestore(&priv->rxdma_irq_lock, flags);
 
 	return IRQ_HANDLED;
 }
@@ -1399,7 +1398,7 @@ static int altera_tse_probe(struct platform_device *pdev)
 	}
 
 	if (of_property_read_u32(pdev->dev.of_node, "tx-fifo-depth",
-				 &priv->rx_fifo_depth)) {
+				 &priv->tx_fifo_depth)) {
 		dev_err(&pdev->dev, "cannot obtain tx-fifo-depth\n");
 		ret = -ENXIO;
 		goto err_free_netdev;
diff --git a/drivers/net/ethernet/amd/pcnet32.c b/drivers/net/ethernet/amd/pcnet32.c
index 11d6e6561df1..15a8190a6f75 100644
--- a/drivers/net/ethernet/amd/pcnet32.c
+++ b/drivers/net/ethernet/amd/pcnet32.c
@@ -1543,7 +1543,7 @@ pcnet32_probe1(unsigned long ioaddr, int shared, struct pci_dev *pdev)
 {
 	struct pcnet32_private *lp;
 	int i, media;
-	int fdx, mii, fset, dxsuflo;
+	int fdx, mii, fset, dxsuflo, sram;
 	int chip_version;
 	char *chipname;
 	struct net_device *dev;
@@ -1580,7 +1580,7 @@ pcnet32_probe1(unsigned long ioaddr, int shared, struct pci_dev *pdev)
 	}
 
 	/* initialize variables */
-	fdx = mii = fset = dxsuflo = 0;
+	fdx = mii = fset = dxsuflo = sram = 0;
 	chip_version = (chip_version >> 12) & 0xffff;
 
 	switch (chip_version) {
@@ -1613,6 +1613,7 @@ pcnet32_probe1(unsigned long ioaddr, int shared, struct pci_dev *pdev)
 		chipname = "PCnet/FAST III 79C973";	/* PCI */
 		fdx = 1;
 		mii = 1;
+		sram = 1;
 		break;
 	case 0x2626:
 		chipname = "PCnet/Home 79C978";	/* PCI */
@@ -1636,6 +1637,7 @@ pcnet32_probe1(unsigned long ioaddr, int shared, struct pci_dev *pdev)
 		chipname = "PCnet/FAST III 79C975";	/* PCI */
 		fdx = 1;
 		mii = 1;
+		sram = 1;
 		break;
 	case 0x2628:
 		chipname = "PCnet/PRO 79C976";
@@ -1664,6 +1666,31 @@ pcnet32_probe1(unsigned long ioaddr, int shared, struct pci_dev *pdev)
 		dxsuflo = 1;
 	}
 
+	/*
+	 * The Am79C973/Am79C975 controllers come with 12K of SRAM
+	 * which we can use for the Tx/Rx buffers but most importantly,
+	 * the use of SRAM allow us to use the BCR18:NOUFLO bit to avoid
+	 * Tx fifo underflows.
+	 */
+	if (sram) {
+		/*
+		 * The SRAM is being configured in two steps. First we
+		 * set the SRAM size in the BCR25:SRAM_SIZE bits. According
+		 * to the datasheet, each bit corresponds to a 512-byte
+		 * page so we can have at most 24 pages. The SRAM_SIZE
+		 * holds the value of the upper 8 bits of the 16-bit SRAM size.
+		 * The low 8-bits start at 0x00 and end at 0xff. So the
+		 * address range is from 0x0000 up to 0x17ff. Therefore,
+		 * the SRAM_SIZE is set to 0x17. The next step is to set
+		 * the BCR26:SRAM_BND midway through so the Tx and Rx
+		 * buffers can share the SRAM equally.
+		 */
+		a->write_bcr(ioaddr, 25, 0x17);
+		a->write_bcr(ioaddr, 26, 0xc);
+		/* And finally enable the NOUFLO bit */
+		a->write_bcr(ioaddr, 18, a->read_bcr(ioaddr, 18) | (1 << 11));
+	}
+
 	dev = alloc_etherdev(sizeof(*lp));
 	if (!dev) {
 		ret = -ENOMEM;
diff --git a/drivers/net/ethernet/amd/xgbe/xgbe-drv.c b/drivers/net/ethernet/amd/xgbe/xgbe-drv.c
index b93d4404d975..885b02b5be07 100644
--- a/drivers/net/ethernet/amd/xgbe/xgbe-drv.c
+++ b/drivers/net/ethernet/amd/xgbe/xgbe-drv.c
@@ -609,6 +609,68 @@ static void xgbe_napi_disable(struct xgbe_prv_data *pdata, unsigned int del)
 	}
 }
 
+static int xgbe_request_irqs(struct xgbe_prv_data *pdata)
+{
+	struct xgbe_channel *channel;
+	struct net_device *netdev = pdata->netdev;
+	unsigned int i;
+	int ret;
+
+	ret = devm_request_irq(pdata->dev, pdata->dev_irq, xgbe_isr, 0,
+			       netdev->name, pdata);
+	if (ret) {
+		netdev_alert(netdev, "error requesting irq %d\n",
+			     pdata->dev_irq);
+		return ret;
+	}
+
+	if (!pdata->per_channel_irq)
+		return 0;
+
+	channel = pdata->channel;
+	for (i = 0; i < pdata->channel_count; i++, channel++) {
+		snprintf(channel->dma_irq_name,
+			 sizeof(channel->dma_irq_name) - 1,
+			 "%s-TxRx-%u", netdev_name(netdev),
+			 channel->queue_index);
+
+		ret = devm_request_irq(pdata->dev, channel->dma_irq,
+				       xgbe_dma_isr, 0,
+				       channel->dma_irq_name, channel);
+		if (ret) {
+			netdev_alert(netdev, "error requesting irq %d\n",
+				     channel->dma_irq);
+			goto err_irq;
+		}
+	}
+
+	return 0;
+
+err_irq:
+	/* Using an unsigned int, 'i' will go to UINT_MAX and exit */
+	for (i--, channel--; i < pdata->channel_count; i--, channel--)
+		devm_free_irq(pdata->dev, channel->dma_irq, channel);
+
+	devm_free_irq(pdata->dev, pdata->dev_irq, pdata);
+
+	return ret;
+}
+
+static void xgbe_free_irqs(struct xgbe_prv_data *pdata)
+{
+	struct xgbe_channel *channel;
+	unsigned int i;
+
+	devm_free_irq(pdata->dev, pdata->dev_irq, pdata);
+
+	if (!pdata->per_channel_irq)
+		return;
+
+	channel = pdata->channel;
+	for (i = 0; i < pdata->channel_count; i++, channel++)
+		devm_free_irq(pdata->dev, channel->dma_irq, channel);
+}
+
 void xgbe_init_tx_coalesce(struct xgbe_prv_data *pdata)
 {
 	struct xgbe_hw_if *hw_if = &pdata->hw_if;
@@ -810,20 +872,20 @@ int xgbe_powerdown(struct net_device *netdev, unsigned int caller)
 		return -EINVAL;
 	}
 
-	phy_stop(pdata->phydev);
-
 	spin_lock_irqsave(&pdata->lock, flags);
 
 	if (caller == XGMAC_DRIVER_CONTEXT)
 		netif_device_detach(netdev);
 
 	netif_tx_stop_all_queues(netdev);
-	xgbe_napi_disable(pdata, 0);
 
-	/* Powerdown Tx/Rx */
 	hw_if->powerdown_tx(pdata);
 	hw_if->powerdown_rx(pdata);
 
+	xgbe_napi_disable(pdata, 0);
+
+	phy_stop(pdata->phydev);
+
 	pdata->power_down = 1;
 
 	spin_unlock_irqrestore(&pdata->lock, flags);
@@ -854,14 +916,14 @@ int xgbe_powerup(struct net_device *netdev, unsigned int caller)
 
 	phy_start(pdata->phydev);
 
-	/* Enable Tx/Rx */
+	xgbe_napi_enable(pdata, 0);
+
 	hw_if->powerup_tx(pdata);
 	hw_if->powerup_rx(pdata);
 
 	if (caller == XGMAC_DRIVER_CONTEXT)
 		netif_device_attach(netdev);
 
-	xgbe_napi_enable(pdata, 0);
 	netif_tx_start_all_queues(netdev);
 
 	spin_unlock_irqrestore(&pdata->lock, flags);
@@ -875,6 +937,7 @@ static int xgbe_start(struct xgbe_prv_data *pdata)
 {
 	struct xgbe_hw_if *hw_if = &pdata->hw_if;
 	struct net_device *netdev = pdata->netdev;
+	int ret;
 
 	DBGPR("-->xgbe_start\n");
 
@@ -884,17 +947,31 @@ static int xgbe_start(struct xgbe_prv_data *pdata)
 
 	phy_start(pdata->phydev);
 
+	xgbe_napi_enable(pdata, 1);
+
+	ret = xgbe_request_irqs(pdata);
+	if (ret)
+		goto err_napi;
+
 	hw_if->enable_tx(pdata);
 	hw_if->enable_rx(pdata);
 
 	xgbe_init_tx_timers(pdata);
 
-	xgbe_napi_enable(pdata, 1);
 	netif_tx_start_all_queues(netdev);
 
 	DBGPR("<--xgbe_start\n");
 
 	return 0;
+
+err_napi:
+	xgbe_napi_disable(pdata, 1);
+
+	phy_stop(pdata->phydev);
+
+	hw_if->exit(pdata);
+
+	return ret;
 }
 
 static void xgbe_stop(struct xgbe_prv_data *pdata)
@@ -907,16 +984,21 @@ static void xgbe_stop(struct xgbe_prv_data *pdata)
 
 	DBGPR("-->xgbe_stop\n");
 
-	phy_stop(pdata->phydev);
-
 	netif_tx_stop_all_queues(netdev);
-	xgbe_napi_disable(pdata, 1);
 
 	xgbe_stop_tx_timers(pdata);
 
 	hw_if->disable_tx(pdata);
 	hw_if->disable_rx(pdata);
 
+	xgbe_free_irqs(pdata);
+
+	xgbe_napi_disable(pdata, 1);
+
+	phy_stop(pdata->phydev);
+
+	hw_if->exit(pdata);
+
 	channel = pdata->channel;
 	for (i = 0; i < pdata->channel_count; i++, channel++) {
 		if (!channel->tx_ring)
@@ -931,10 +1013,6 @@ static void xgbe_stop(struct xgbe_prv_data *pdata)
 
 static void xgbe_restart_dev(struct xgbe_prv_data *pdata)
 {
-	struct xgbe_channel *channel;
-	struct xgbe_hw_if *hw_if = &pdata->hw_if;
-	unsigned int i;
-
 	DBGPR("-->xgbe_restart_dev\n");
 
 	/* If not running, "restart" will happen on open */
@@ -942,19 +1020,10 @@ static void xgbe_restart_dev(struct xgbe_prv_data *pdata)
 		return;
 
 	xgbe_stop(pdata);
-	synchronize_irq(pdata->dev_irq);
-	if (pdata->per_channel_irq) {
-		channel = pdata->channel;
-		for (i = 0; i < pdata->channel_count; i++, channel++)
-			synchronize_irq(channel->dma_irq);
-	}
 
 	xgbe_free_tx_data(pdata);
 	xgbe_free_rx_data(pdata);
 
-	/* Issue software reset to device */
-	hw_if->exit(pdata);
-
 	xgbe_start(pdata);
 
 	DBGPR("<--xgbe_restart_dev\n");
@@ -1283,10 +1352,7 @@ static void xgbe_packet_info(struct xgbe_prv_data *pdata,
 static int xgbe_open(struct net_device *netdev)
 {
 	struct xgbe_prv_data *pdata = netdev_priv(netdev);
-	struct xgbe_hw_if *hw_if = &pdata->hw_if;
 	struct xgbe_desc_if *desc_if = &pdata->desc_if;
-	struct xgbe_channel *channel = NULL;
-	unsigned int i = 0;
 	int ret;
 
 	DBGPR("-->xgbe_open\n");
@@ -1329,55 +1395,14 @@ static int xgbe_open(struct net_device *netdev)
 	INIT_WORK(&pdata->restart_work, xgbe_restart);
 	INIT_WORK(&pdata->tx_tstamp_work, xgbe_tx_tstamp);
 
-	/* Request interrupts */
-	ret = devm_request_irq(pdata->dev, pdata->dev_irq, xgbe_isr, 0,
-			       netdev->name, pdata);
-	if (ret) {
-		netdev_alert(netdev, "error requesting irq %d\n",
-			     pdata->dev_irq);
-		goto err_rings;
-	}
-
-	if (pdata->per_channel_irq) {
-		channel = pdata->channel;
-		for (i = 0; i < pdata->channel_count; i++, channel++) {
-			snprintf(channel->dma_irq_name,
-				 sizeof(channel->dma_irq_name) - 1,
-				 "%s-TxRx-%u", netdev_name(netdev),
-				 channel->queue_index);
-
-			ret = devm_request_irq(pdata->dev, channel->dma_irq,
-					       xgbe_dma_isr, 0,
-					       channel->dma_irq_name, channel);
-			if (ret) {
-				netdev_alert(netdev,
-					     "error requesting irq %d\n",
-					     channel->dma_irq);
-				goto err_irq;
-			}
-		}
-	}
-
 	ret = xgbe_start(pdata);
 	if (ret)
-		goto err_start;
+		goto err_rings;
 
 	DBGPR("<--xgbe_open\n");
 
 	return 0;
 
-err_start:
-	hw_if->exit(pdata);
-
-err_irq:
-	if (pdata->per_channel_irq) {
-		/* Using an unsigned int, 'i' will go to UINT_MAX and exit */
-		for (i--, channel--; i < pdata->channel_count; i--, channel--)
-			devm_free_irq(pdata->dev, channel->dma_irq, channel);
-	}
-
-	devm_free_irq(pdata->dev, pdata->dev_irq, pdata);
-
 err_rings:
 	desc_if->free_ring_resources(pdata);
 
@@ -1399,30 +1424,16 @@ err_phy_init:
 static int xgbe_close(struct net_device *netdev)
 {
 	struct xgbe_prv_data *pdata = netdev_priv(netdev);
-	struct xgbe_hw_if *hw_if = &pdata->hw_if;
 	struct xgbe_desc_if *desc_if = &pdata->desc_if;
-	struct xgbe_channel *channel;
-	unsigned int i;
 
 	DBGPR("-->xgbe_close\n");
 
 	/* Stop the device */
 	xgbe_stop(pdata);
 
-	/* Issue software reset to device */
-	hw_if->exit(pdata);
-
 	/* Free the ring descriptors and buffers */
 	desc_if->free_ring_resources(pdata);
 
-	/* Release the interrupts */
-	devm_free_irq(pdata->dev, pdata->dev_irq, pdata);
-	if (pdata->per_channel_irq) {
-		channel = pdata->channel;
-		for (i = 0; i < pdata->channel_count; i++, channel++)
-			devm_free_irq(pdata->dev, channel->dma_irq, channel);
-	}
-
 	/* Free the channel and ring structures */
 	xgbe_free_channels(pdata);
 
diff --git a/drivers/net/ethernet/apm/xgene/xgene_enet_hw.c b/drivers/net/ethernet/apm/xgene/xgene_enet_hw.c
index 869d97fcf781..b927021c6c40 100644
--- a/drivers/net/ethernet/apm/xgene/xgene_enet_hw.c
+++ b/drivers/net/ethernet/apm/xgene/xgene_enet_hw.c
@@ -593,7 +593,7 @@ static int xgene_enet_reset(struct xgene_enet_pdata *pdata)
 	if (!xgene_ring_mgr_init(pdata))
 		return -ENODEV;
 
-	if (!efi_enabled(EFI_BOOT)) {
+	if (pdata->clk) {
 		clk_prepare_enable(pdata->clk);
 		clk_disable_unprepare(pdata->clk);
 		clk_prepare_enable(pdata->clk);
diff --git a/drivers/net/ethernet/apm/xgene/xgene_enet_main.c b/drivers/net/ethernet/apm/xgene/xgene_enet_main.c
index 4de62b210c85..635a83be7e5e 100644
--- a/drivers/net/ethernet/apm/xgene/xgene_enet_main.c
+++ b/drivers/net/ethernet/apm/xgene/xgene_enet_main.c
@@ -1025,6 +1025,8 @@ static int xgene_enet_remove(struct platform_device *pdev)
 #ifdef CONFIG_ACPI
 static const struct acpi_device_id xgene_enet_acpi_match[] = {
 	{ "APMC0D05", },
+	{ "APMC0D30", },
+	{ "APMC0D31", },
 	{ }
 };
 MODULE_DEVICE_TABLE(acpi, xgene_enet_acpi_match);
@@ -1033,6 +1035,8 @@ MODULE_DEVICE_TABLE(acpi, xgene_enet_acpi_match);
 #ifdef CONFIG_OF
 static struct of_device_id xgene_enet_of_match[] = {
 	{.compatible = "apm,xgene-enet",},
+	{.compatible = "apm,xgene1-sgenet",},
+	{.compatible = "apm,xgene1-xgenet",},
 	{},
 };
 
diff --git a/drivers/net/ethernet/broadcom/bcm63xx_enet.c b/drivers/net/ethernet/broadcom/bcm63xx_enet.c
index 21206d33b638..a7f2cc3e485e 100644
--- a/drivers/net/ethernet/broadcom/bcm63xx_enet.c
+++ b/drivers/net/ethernet/broadcom/bcm63xx_enet.c
@@ -486,7 +486,7 @@ static int bcm_enet_poll(struct napi_struct *napi, int budget)
 {
 	struct bcm_enet_priv *priv;
 	struct net_device *dev;
-	int tx_work_done, rx_work_done;
+	int rx_work_done;
 
 	priv = container_of(napi, struct bcm_enet_priv, napi);
 	dev = priv->net_dev;
@@ -498,14 +498,14 @@ static int bcm_enet_poll(struct napi_struct *napi, int budget)
 			 ENETDMAC_IR, priv->tx_chan);
 
 	/* reclaim sent skb */
-	tx_work_done = bcm_enet_tx_reclaim(dev, 0);
+	bcm_enet_tx_reclaim(dev, 0);
 
 	spin_lock(&priv->rx_lock);
 	rx_work_done = bcm_enet_receive_queue(dev, budget);
 	spin_unlock(&priv->rx_lock);
 
-	if (rx_work_done >= budget || tx_work_done > 0) {
-		/* rx/tx queue is not yet empty/clean */
+	if (rx_work_done >= budget) {
+		/* rx queue is not yet empty/clean */
 		return rx_work_done;
 	}
 
diff --git a/drivers/net/ethernet/broadcom/bcmsysport.c b/drivers/net/ethernet/broadcom/bcmsysport.c
index 5b308a4a4d0e..783543ad1fcf 100644
--- a/drivers/net/ethernet/broadcom/bcmsysport.c
+++ b/drivers/net/ethernet/broadcom/bcmsysport.c
@@ -274,9 +274,9 @@ static const struct bcm_sysport_stats bcm_sysport_gstrings_stats[] = {
 	/* RBUF misc statistics */
 	STAT_RBUF("rbuf_ovflow_cnt", mib.rbuf_ovflow_cnt, RBUF_OVFL_DISC_CNTR),
 	STAT_RBUF("rbuf_err_cnt", mib.rbuf_err_cnt, RBUF_ERR_PKT_CNTR),
-	STAT_MIB_RX("alloc_rx_buff_failed", mib.alloc_rx_buff_failed),
-	STAT_MIB_RX("rx_dma_failed", mib.rx_dma_failed),
-	STAT_MIB_TX("tx_dma_failed", mib.tx_dma_failed),
+	STAT_MIB_SOFT("alloc_rx_buff_failed", mib.alloc_rx_buff_failed),
+	STAT_MIB_SOFT("rx_dma_failed", mib.rx_dma_failed),
+	STAT_MIB_SOFT("tx_dma_failed", mib.tx_dma_failed),
 };
 
 #define BCM_SYSPORT_STATS_LEN	ARRAY_SIZE(bcm_sysport_gstrings_stats)
@@ -345,6 +345,7 @@ static void bcm_sysport_update_mib_counters(struct bcm_sysport_priv *priv)
 		s = &bcm_sysport_gstrings_stats[i];
 		switch (s->type) {
 		case BCM_SYSPORT_STAT_NETDEV:
+		case BCM_SYSPORT_STAT_SOFT:
 			continue;
 		case BCM_SYSPORT_STAT_MIB_RX:
 		case BCM_SYSPORT_STAT_MIB_TX:
diff --git a/drivers/net/ethernet/broadcom/bcmsysport.h b/drivers/net/ethernet/broadcom/bcmsysport.h
index fc19417d82a5..7e3d87a88c76 100644
--- a/drivers/net/ethernet/broadcom/bcmsysport.h
+++ b/drivers/net/ethernet/broadcom/bcmsysport.h
@@ -570,6 +570,7 @@ enum bcm_sysport_stat_type {
 	BCM_SYSPORT_STAT_RUNT,
 	BCM_SYSPORT_STAT_RXCHK,
 	BCM_SYSPORT_STAT_RBUF,
+	BCM_SYSPORT_STAT_SOFT,
 };
 
 /* Macros to help define ethtool statistics */
@@ -590,6 +591,7 @@ enum bcm_sysport_stat_type {
 #define STAT_MIB_RX(str, m) STAT_MIB(str, m, BCM_SYSPORT_STAT_MIB_RX)
 #define STAT_MIB_TX(str, m) STAT_MIB(str, m, BCM_SYSPORT_STAT_MIB_TX)
 #define STAT_RUNT(str, m) STAT_MIB(str, m, BCM_SYSPORT_STAT_RUNT)
+#define STAT_MIB_SOFT(str, m) STAT_MIB(str, m, BCM_SYSPORT_STAT_SOFT)
 
 #define STAT_RXCHK(str, m, ofs) { \
 	.stat_string = str, \
diff --git a/drivers/net/ethernet/broadcom/bgmac.c b/drivers/net/ethernet/broadcom/bgmac.c
index 676ffe093180..0469f72c6e7e 100644
--- a/drivers/net/ethernet/broadcom/bgmac.c
+++ b/drivers/net/ethernet/broadcom/bgmac.c
@@ -302,9 +302,6 @@ static int bgmac_dma_rx_skb_for_slot(struct bgmac *bgmac,
 	slot->skb = skb;
 	slot->dma_addr = dma_addr;
 
-	if (slot->dma_addr & 0xC0000000)
-		bgmac_warn(bgmac, "DMA address using 0xC0000000 bit(s), it may need translation trick\n");
-
 	return 0;
 }
 
@@ -505,8 +502,6 @@ static int bgmac_dma_alloc(struct bgmac *bgmac)
 				  ring->mmio_base);
 			goto err_dma_free;
 		}
-		if (ring->dma_base & 0xC0000000)
-			bgmac_warn(bgmac, "DMA address using 0xC0000000 bit(s), it may need translation trick\n");
 
 		ring->unaligned = bgmac_dma_unaligned(bgmac, ring,
 						      BGMAC_DMA_RING_TX);
@@ -536,8 +531,6 @@ static int bgmac_dma_alloc(struct bgmac *bgmac)
 			err = -ENOMEM;
 			goto err_dma_free;
 		}
-		if (ring->dma_base & 0xC0000000)
-			bgmac_warn(bgmac, "DMA address using 0xC0000000 bit(s), it may need translation trick\n");
 
 		ring->unaligned = bgmac_dma_unaligned(bgmac, ring,
 						      BGMAC_DMA_RING_RX);
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x.h b/drivers/net/ethernet/broadcom/bnx2x/bnx2x.h
index 756053c028be..4085c4b31047 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x.h
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x.h
@@ -1811,7 +1811,7 @@ struct bnx2x {
 	int			stats_state;
 
 	/* used for synchronization of concurrent threads statistics handling */
-	spinlock_t		stats_lock;
+	struct mutex		stats_lock;
 
 	/* used by dmae command loader */
 	struct dmae_command	stats_dmae;
@@ -1935,8 +1935,6 @@ struct bnx2x {
 
 	int fp_array_size;
 	u32 dump_preset_idx;
-	bool					stats_started;
-	struct semaphore			stats_sema;
 
 	u8					phys_port_id[ETH_ALEN];
 
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_main.c b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_main.c
index 7155e1d2c208..1ec635f54994 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_main.c
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_main.c
@@ -129,8 +129,8 @@ struct bnx2x_mac_vals {
 	u32 xmac_val;
 	u32 emac_addr;
 	u32 emac_val;
-	u32 umac_addr;
-	u32 umac_val;
+	u32 umac_addr[2];
+	u32 umac_val[2];
 	u32 bmac_addr;
 	u32 bmac_val[2];
 };
@@ -7866,6 +7866,20 @@ int bnx2x_init_hw_func_cnic(struct bnx2x *bp)
 	return 0;
 }
 
+/* previous driver DMAE transaction may have occurred when pre-boot stage ended
+ * and boot began, or when kdump kernel was loaded. Either case would invalidate
+ * the addresses of the transaction, resulting in was-error bit set in the pci
+ * causing all hw-to-host pcie transactions to timeout. If this happened we want
+ * to clear the interrupt which detected this from the pglueb and the was done
+ * bit
+ */
+static void bnx2x_clean_pglue_errors(struct bnx2x *bp)
+{
+	if (!CHIP_IS_E1x(bp))
+		REG_WR(bp, PGLUE_B_REG_WAS_ERROR_PF_7_0_CLR,
+		       1 << BP_ABS_FUNC(bp));
+}
+
 static int bnx2x_init_hw_func(struct bnx2x *bp)
 {
 	int port = BP_PORT(bp);
@@ -7958,8 +7972,7 @@ static int bnx2x_init_hw_func(struct bnx2x *bp)
 
 	bnx2x_init_block(bp, BLOCK_PGLUE_B, init_phase);
 
-	if (!CHIP_IS_E1x(bp))
-		REG_WR(bp, PGLUE_B_REG_WAS_ERROR_PF_7_0_CLR, func);
+	bnx2x_clean_pglue_errors(bp);
 
 	bnx2x_init_block(bp, BLOCK_ATC, init_phase);
 	bnx2x_init_block(bp, BLOCK_DMAE, init_phase);
@@ -10141,6 +10154,25 @@ static u32 bnx2x_get_pretend_reg(struct bnx2x *bp)
 	return base + (BP_ABS_FUNC(bp)) * stride;
 }
 
+static bool bnx2x_prev_unload_close_umac(struct bnx2x *bp,
+					 u8 port, u32 reset_reg,
+					 struct bnx2x_mac_vals *vals)
+{
+	u32 mask = MISC_REGISTERS_RESET_REG_2_UMAC0 << port;
+	u32 base_addr;
+
+	if (!(mask & reset_reg))
+		return false;
+
+	BNX2X_DEV_INFO("Disable umac Rx %02x\n", port);
+	base_addr = port ? GRCBASE_UMAC1 : GRCBASE_UMAC0;
+	vals->umac_addr[port] = base_addr + UMAC_REG_COMMAND_CONFIG;
+	vals->umac_val[port] = REG_RD(bp, vals->umac_addr[port]);
+	REG_WR(bp, vals->umac_addr[port], 0);
+
+	return true;
+}
+
 static void bnx2x_prev_unload_close_mac(struct bnx2x *bp,
 					struct bnx2x_mac_vals *vals)
 {
@@ -10149,10 +10181,7 @@ static void bnx2x_prev_unload_close_mac(struct bnx2x *bp,
 	u8 port = BP_PORT(bp);
 
 	/* reset addresses as they also mark which values were changed */
-	vals->bmac_addr = 0;
-	vals->umac_addr = 0;
-	vals->xmac_addr = 0;
-	vals->emac_addr = 0;
+	memset(vals, 0, sizeof(*vals));
 
 	reset_reg = REG_RD(bp, MISC_REG_RESET_REG_2);
 
@@ -10201,15 +10230,11 @@ static void bnx2x_prev_unload_close_mac(struct bnx2x *bp,
 			REG_WR(bp, vals->xmac_addr, 0);
 			mac_stopped = true;
 		}
-		mask = MISC_REGISTERS_RESET_REG_2_UMAC0 << port;
-		if (mask & reset_reg) {
-			BNX2X_DEV_INFO("Disable umac Rx\n");
-			base_addr = BP_PORT(bp) ? GRCBASE_UMAC1 : GRCBASE_UMAC0;
-			vals->umac_addr = base_addr + UMAC_REG_COMMAND_CONFIG;
-			vals->umac_val = REG_RD(bp, vals->umac_addr);
-			REG_WR(bp, vals->umac_addr, 0);
-			mac_stopped = true;
-		}
+
+		mac_stopped |= bnx2x_prev_unload_close_umac(bp, 0,
+							    reset_reg, vals);
+		mac_stopped |= bnx2x_prev_unload_close_umac(bp, 1,
+							    reset_reg, vals);
 	}
 
 	if (mac_stopped)
@@ -10505,8 +10530,11 @@ static int bnx2x_prev_unload_common(struct bnx2x *bp)
 		/* Close the MAC Rx to prevent BRB from filling up */
 		bnx2x_prev_unload_close_mac(bp, &mac_vals);
 
-		/* close LLH filters towards the BRB */
+		/* close LLH filters for both ports towards the BRB */
+		bnx2x_set_rx_filter(&bp->link_params, 0);
+		bp->link_params.port ^= 1;
 		bnx2x_set_rx_filter(&bp->link_params, 0);
+		bp->link_params.port ^= 1;
 
 		/* Check if the UNDI driver was previously loaded */
 		if (bnx2x_prev_is_after_undi(bp)) {
@@ -10553,8 +10581,10 @@ static int bnx2x_prev_unload_common(struct bnx2x *bp)
 
 	if (mac_vals.xmac_addr)
 		REG_WR(bp, mac_vals.xmac_addr, mac_vals.xmac_val);
-	if (mac_vals.umac_addr)
-		REG_WR(bp, mac_vals.umac_addr, mac_vals.umac_val);
+	if (mac_vals.umac_addr[0])
+		REG_WR(bp, mac_vals.umac_addr[0], mac_vals.umac_val[0]);
+	if (mac_vals.umac_addr[1])
+		REG_WR(bp, mac_vals.umac_addr[1], mac_vals.umac_val[1]);
 	if (mac_vals.emac_addr)
 		REG_WR(bp, mac_vals.emac_addr, mac_vals.emac_val);
 	if (mac_vals.bmac_addr) {
@@ -10571,26 +10601,6 @@ static int bnx2x_prev_unload_common(struct bnx2x *bp)
 	return bnx2x_prev_mcp_done(bp);
 }
 
-/* previous driver DMAE transaction may have occurred when pre-boot stage ended
- * and boot began, or when kdump kernel was loaded. Either case would invalidate
- * the addresses of the transaction, resulting in was-error bit set in the pci
- * causing all hw-to-host pcie transactions to timeout. If this happened we want
- * to clear the interrupt which detected this from the pglueb and the was done
- * bit
- */
-static void bnx2x_prev_interrupted_dmae(struct bnx2x *bp)
-{
-	if (!CHIP_IS_E1x(bp)) {
-		u32 val = REG_RD(bp, PGLUE_B_REG_PGLUE_B_INT_STS);
-		if (val & PGLUE_B_PGLUE_B_INT_STS_REG_WAS_ERROR_ATTN) {
-			DP(BNX2X_MSG_SP,
-			   "'was error' bit was found to be set in pglueb upon startup. Clearing\n");
-			REG_WR(bp, PGLUE_B_REG_WAS_ERROR_PF_7_0_CLR,
-			       1 << BP_FUNC(bp));
-		}
-	}
-}
-
 static int bnx2x_prev_unload(struct bnx2x *bp)
 {
 	int time_counter = 10;
@@ -10600,7 +10610,7 @@ static int bnx2x_prev_unload(struct bnx2x *bp)
 	/* clear hw from errors which may have resulted from an interrupted
 	 * dmae transaction.
 	 */
-	bnx2x_prev_interrupted_dmae(bp);
+	bnx2x_clean_pglue_errors(bp);
 
 	/* Release previously held locks */
 	hw_lock_reg = (BP_FUNC(bp) <= 5) ?
@@ -12037,9 +12047,8 @@ static int bnx2x_init_bp(struct bnx2x *bp)
 	mutex_init(&bp->port.phy_mutex);
 	mutex_init(&bp->fw_mb_mutex);
 	mutex_init(&bp->drv_info_mutex);
+	mutex_init(&bp->stats_lock);
 	bp->drv_info_mng_owner = false;
-	spin_lock_init(&bp->stats_lock);
-	sema_init(&bp->stats_sema, 1);
 
 	INIT_DELAYED_WORK(&bp->sp_task, bnx2x_sp_task);
 	INIT_DELAYED_WORK(&bp->sp_rtnl_task, bnx2x_sp_rtnl_task);
@@ -12722,6 +12731,9 @@ static int bnx2x_init_dev(struct bnx2x *bp, struct pci_dev *pdev,
 	pci_write_config_dword(bp->pdev, PCICFG_GRC_ADDRESS,
 			       PCICFG_VENDOR_ID_OFFSET);
 
+	/* Set PCIe reset type to fundamental for EEH recovery */
+	pdev->needs_freset = 1;
+
 	/* AER (Advanced Error reporting) configuration */
 	rc = pci_enable_pcie_error_reporting(pdev);
 	if (!rc)
@@ -12766,7 +12778,7 @@ static int bnx2x_init_dev(struct bnx2x *bp, struct pci_dev *pdev,
 		NETIF_F_TSO | NETIF_F_TSO_ECN | NETIF_F_TSO6 |
 		NETIF_F_RXCSUM | NETIF_F_LRO | NETIF_F_GRO |
 		NETIF_F_RXHASH | NETIF_F_HW_VLAN_CTAG_TX;
-	if (!CHIP_IS_E1x(bp)) {
+	if (!chip_is_e1x) {
 		dev->hw_features |= NETIF_F_GSO_GRE | NETIF_F_GSO_UDP_TUNNEL |
 				    NETIF_F_GSO_IPIP | NETIF_F_GSO_SIT;
 		dev->hw_enc_features =
@@ -13665,9 +13677,9 @@ static int bnx2x_eeh_nic_unload(struct bnx2x *bp)
 	cancel_delayed_work_sync(&bp->sp_task);
 	cancel_delayed_work_sync(&bp->period_task);
 
-	spin_lock_bh(&bp->stats_lock);
+	mutex_lock(&bp->stats_lock);
 	bp->stats_state = STATS_STATE_DISABLED;
-	spin_unlock_bh(&bp->stats_lock);
+	mutex_unlock(&bp->stats_lock);
 
 	bnx2x_save_statistics(bp);
 
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sriov.c b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sriov.c
index e5aca2de1871..cfe3c7695455 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sriov.c
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sriov.c
@@ -2238,7 +2238,9 @@ int bnx2x_vf_close(struct bnx2x *bp, struct bnx2x_virtf *vf)
 
 		cookie.vf = vf;
 		cookie.state = VF_ACQUIRED;
-		bnx2x_stats_safe_exec(bp, bnx2x_set_vf_state, &cookie);
+		rc = bnx2x_stats_safe_exec(bp, bnx2x_set_vf_state, &cookie);
+		if (rc)
+			goto op_err;
 	}
 
 	DP(BNX2X_MSG_IOV, "set state to acquired\n");
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_stats.c b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_stats.c
index d1608297c773..800ab44a07ce 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_stats.c
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_stats.c
@@ -123,36 +123,28 @@ static void bnx2x_dp_stats(struct bnx2x *bp)
  */
 static void bnx2x_storm_stats_post(struct bnx2x *bp)
 {
-	if (!bp->stats_pending) {
-		int rc;
+	int rc;
 
-		spin_lock_bh(&bp->stats_lock);
-
-		if (bp->stats_pending) {
-			spin_unlock_bh(&bp->stats_lock);
-			return;
-		}
-
-		bp->fw_stats_req->hdr.drv_stats_counter =
-			cpu_to_le16(bp->stats_counter++);
+	if (bp->stats_pending)
+		return;
 
-		DP(BNX2X_MSG_STATS, "Sending statistics ramrod %d\n",
-		   le16_to_cpu(bp->fw_stats_req->hdr.drv_stats_counter));
+	bp->fw_stats_req->hdr.drv_stats_counter =
+		cpu_to_le16(bp->stats_counter++);
 
-		/* adjust the ramrod to include VF queues statistics */
-		bnx2x_iov_adjust_stats_req(bp);
-		bnx2x_dp_stats(bp);
+	DP(BNX2X_MSG_STATS, "Sending statistics ramrod %d\n",
+	   le16_to_cpu(bp->fw_stats_req->hdr.drv_stats_counter));
 
-		/* send FW stats ramrod */
-		rc = bnx2x_sp_post(bp, RAMROD_CMD_ID_COMMON_STAT_QUERY, 0,
-				   U64_HI(bp->fw_stats_req_mapping),
-				   U64_LO(bp->fw_stats_req_mapping),
-				   NONE_CONNECTION_TYPE);
-		if (rc == 0)
-			bp->stats_pending = 1;
+	/* adjust the ramrod to include VF queues statistics */
+	bnx2x_iov_adjust_stats_req(bp);
+	bnx2x_dp_stats(bp);
 
-		spin_unlock_bh(&bp->stats_lock);
-	}
+	/* send FW stats ramrod */
+	rc = bnx2x_sp_post(bp, RAMROD_CMD_ID_COMMON_STAT_QUERY, 0,
+			   U64_HI(bp->fw_stats_req_mapping),
+			   U64_LO(bp->fw_stats_req_mapping),
+			   NONE_CONNECTION_TYPE);
+	if (rc == 0)
+		bp->stats_pending = 1;
 }
 
 static void bnx2x_hw_stats_post(struct bnx2x *bp)
@@ -221,7 +213,7 @@ static void bnx2x_stats_comp(struct bnx2x *bp)
  */
 
 /* should be called under stats_sema */
-static void __bnx2x_stats_pmf_update(struct bnx2x *bp)
+static void bnx2x_stats_pmf_update(struct bnx2x *bp)
 {
 	struct dmae_command *dmae;
 	u32 opcode;
@@ -519,7 +511,7 @@ static void bnx2x_func_stats_init(struct bnx2x *bp)
 }
 
 /* should be called under stats_sema */
-static void __bnx2x_stats_start(struct bnx2x *bp)
+static void bnx2x_stats_start(struct bnx2x *bp)
 {
 	if (IS_PF(bp)) {
 		if (bp->port.pmf)
@@ -531,34 +523,13 @@ static void __bnx2x_stats_start(struct bnx2x *bp)
 		bnx2x_hw_stats_post(bp);
 		bnx2x_storm_stats_post(bp);
 	}
-
-	bp->stats_started = true;
-}
-
-static void bnx2x_stats_start(struct bnx2x *bp)
-{
-	if (down_timeout(&bp->stats_sema, HZ/10))
-		BNX2X_ERR("Unable to acquire stats lock\n");
-	__bnx2x_stats_start(bp);
-	up(&bp->stats_sema);
 }
 
 static void bnx2x_stats_pmf_start(struct bnx2x *bp)
 {
-	if (down_timeout(&bp->stats_sema, HZ/10))
-		BNX2X_ERR("Unable to acquire stats lock\n");
 	bnx2x_stats_comp(bp);
-	__bnx2x_stats_pmf_update(bp);
-	__bnx2x_stats_start(bp);
-	up(&bp->stats_sema);
-}
-
-static void bnx2x_stats_pmf_update(struct bnx2x *bp)
-{
-	if (down_timeout(&bp->stats_sema, HZ/10))
-		BNX2X_ERR("Unable to acquire stats lock\n");
-	__bnx2x_stats_pmf_update(bp);
-	up(&bp->stats_sema);
+	bnx2x_stats_pmf_update(bp);
+	bnx2x_stats_start(bp);
 }
 
 static void bnx2x_stats_restart(struct bnx2x *bp)
@@ -568,11 +539,9 @@ static void bnx2x_stats_restart(struct bnx2x *bp)
 	 */
 	if (IS_VF(bp))
 		return;
-	if (down_timeout(&bp->stats_sema, HZ/10))
-		BNX2X_ERR("Unable to acquire stats lock\n");
+
 	bnx2x_stats_comp(bp);
-	__bnx2x_stats_start(bp);
-	up(&bp->stats_sema);
+	bnx2x_stats_start(bp);
 }
 
 static void bnx2x_bmac_stats_update(struct bnx2x *bp)
@@ -1246,18 +1215,12 @@ static void bnx2x_stats_update(struct bnx2x *bp)
 {
 	u32 *stats_comp = bnx2x_sp(bp, stats_comp);
 
-	/* we run update from timer context, so give up
-	 * if somebody is in the middle of transition
-	 */
-	if (down_trylock(&bp->stats_sema))
+	if (bnx2x_edebug_stats_stopped(bp))
 		return;
 
-	if (bnx2x_edebug_stats_stopped(bp) || !bp->stats_started)
-		goto out;
-
 	if (IS_PF(bp)) {
 		if (*stats_comp != DMAE_COMP_VAL)
-			goto out;
+			return;
 
 		if (bp->port.pmf)
 			bnx2x_hw_stats_update(bp);
@@ -1267,7 +1230,7 @@ static void bnx2x_stats_update(struct bnx2x *bp)
 				BNX2X_ERR("storm stats were not updated for 3 times\n");
 				bnx2x_panic();
 			}
-			goto out;
+			return;
 		}
 	} else {
 		/* vf doesn't collect HW statistics, and doesn't get completions
@@ -1281,7 +1244,7 @@ static void bnx2x_stats_update(struct bnx2x *bp)
 
 	/* vf is done */
 	if (IS_VF(bp))
-		goto out;
+		return;
 
 	if (netif_msg_timer(bp)) {
 		struct bnx2x_eth_stats *estats = &bp->eth_stats;
@@ -1292,9 +1255,6 @@ static void bnx2x_stats_update(struct bnx2x *bp)
 
 	bnx2x_hw_stats_post(bp);
 	bnx2x_storm_stats_post(bp);
-
-out:
-	up(&bp->stats_sema);
 }
 
 static void bnx2x_port_stats_stop(struct bnx2x *bp)
@@ -1358,12 +1318,7 @@ static void bnx2x_port_stats_stop(struct bnx2x *bp)
 
 static void bnx2x_stats_stop(struct bnx2x *bp)
 {
-	int update = 0;
-
-	if (down_timeout(&bp->stats_sema, HZ/10))
-		BNX2X_ERR("Unable to acquire stats lock\n");
-
-	bp->stats_started = false;
+	bool update = false;
 
 	bnx2x_stats_comp(bp);
 
@@ -1381,8 +1336,6 @@ static void bnx2x_stats_stop(struct bnx2x *bp)
 		bnx2x_hw_stats_post(bp);
 		bnx2x_stats_comp(bp);
 	}
-
-	up(&bp->stats_sema);
 }
 
 static void bnx2x_stats_do_nothing(struct bnx2x *bp)
@@ -1410,18 +1363,28 @@ static const struct {
 
 void bnx2x_stats_handle(struct bnx2x *bp, enum bnx2x_stats_event event)
 {
-	enum bnx2x_stats_state state;
-	void (*action)(struct bnx2x *bp);
+	enum bnx2x_stats_state state = bp->stats_state;
+
 	if (unlikely(bp->panic))
 		return;
 
-	spin_lock_bh(&bp->stats_lock);
-	state = bp->stats_state;
+	/* Statistics update run from timer context, and we don't want to stop
+	 * that context in case someone is in the middle of a transition.
+	 * For other events, wait a bit until lock is taken.
+	 */
+	if (!mutex_trylock(&bp->stats_lock)) {
+		if (event == STATS_EVENT_UPDATE)
+			return;
+
+		DP(BNX2X_MSG_STATS,
+		   "Unlikely stats' lock contention [event %d]\n", event);
+		mutex_lock(&bp->stats_lock);
+	}
+
+	bnx2x_stats_stm[state][event].action(bp);
 	bp->stats_state = bnx2x_stats_stm[state][event].next_state;
-	action = bnx2x_stats_stm[state][event].action;
-	spin_unlock_bh(&bp->stats_lock);
 
-	action(bp);
+	mutex_unlock(&bp->stats_lock);
 
 	if ((event != STATS_EVENT_UPDATE) || netif_msg_timer(bp))
 		DP(BNX2X_MSG_STATS, "state %d -> event %d -> state %d\n",
@@ -1998,13 +1961,34 @@ void bnx2x_afex_collect_stats(struct bnx2x *bp, void *void_afex_stats,
 	}
 }
 
-void bnx2x_stats_safe_exec(struct bnx2x *bp,
-			   void (func_to_exec)(void *cookie),
-			   void *cookie){
-	if (down_timeout(&bp->stats_sema, HZ/10))
-		BNX2X_ERR("Unable to acquire stats lock\n");
+int bnx2x_stats_safe_exec(struct bnx2x *bp,
+			  void (func_to_exec)(void *cookie),
+			  void *cookie)
+{
+	int cnt = 10, rc = 0;
+
+	/* Wait for statistics to end [while blocking further requests],
+	 * then run supplied function 'safely'.
+	 */
+	mutex_lock(&bp->stats_lock);
+
 	bnx2x_stats_comp(bp);
+	while (bp->stats_pending && cnt--)
+		if (bnx2x_storm_stats_update(bp))
+			usleep_range(1000, 2000);
+	if (bp->stats_pending) {
+		BNX2X_ERR("Failed to wait for stats pending to clear [possibly FW is stuck]\n");
+		rc = -EBUSY;
+		goto out;
+	}
+
 	func_to_exec(cookie);
-	__bnx2x_stats_start(bp);
-	up(&bp->stats_sema);
+
+out:
+	/* No need to restart statistics - if they're enabled, the timer
+	 * will restart the statistics.
+	 */
+	mutex_unlock(&bp->stats_lock);
+
+	return rc;
 }
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_stats.h b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_stats.h
index 2beceaefdeea..965539a9dabe 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_stats.h
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_stats.h
@@ -539,9 +539,9 @@ struct bnx2x;
 void bnx2x_memset_stats(struct bnx2x *bp);
 void bnx2x_stats_init(struct bnx2x *bp);
 void bnx2x_stats_handle(struct bnx2x *bp, enum bnx2x_stats_event event);
-void bnx2x_stats_safe_exec(struct bnx2x *bp,
-			   void (func_to_exec)(void *cookie),
-			   void *cookie);
+int bnx2x_stats_safe_exec(struct bnx2x *bp,
+			  void (func_to_exec)(void *cookie),
+			  void *cookie);
 
 /**
  * bnx2x_save_statistics - save statistics when unloading.
diff --git a/drivers/net/ethernet/broadcom/genet/bcmgenet.c b/drivers/net/ethernet/broadcom/genet/bcmgenet.c
index ff83c46bc389..6befde61c203 100644
--- a/drivers/net/ethernet/broadcom/genet/bcmgenet.c
+++ b/drivers/net/ethernet/broadcom/genet/bcmgenet.c
@@ -487,6 +487,7 @@ enum bcmgenet_stat_type {
 	BCMGENET_STAT_MIB_TX,
 	BCMGENET_STAT_RUNT,
 	BCMGENET_STAT_MISC,
+	BCMGENET_STAT_SOFT,
 };
 
 struct bcmgenet_stats {
@@ -515,6 +516,7 @@ struct bcmgenet_stats {
 #define STAT_GENET_MIB_RX(str, m) STAT_GENET_MIB(str, m, BCMGENET_STAT_MIB_RX)
 #define STAT_GENET_MIB_TX(str, m) STAT_GENET_MIB(str, m, BCMGENET_STAT_MIB_TX)
 #define STAT_GENET_RUNT(str, m) STAT_GENET_MIB(str, m, BCMGENET_STAT_RUNT)
+#define STAT_GENET_SOFT_MIB(str, m) STAT_GENET_MIB(str, m, BCMGENET_STAT_SOFT)
 
 #define STAT_GENET_MISC(str, m, offset) { \
 	.stat_string = str, \
@@ -614,9 +616,9 @@ static const struct bcmgenet_stats bcmgenet_gstrings_stats[] = {
 			UMAC_RBUF_OVFL_CNT),
 	STAT_GENET_MISC("rbuf_err_cnt", mib.rbuf_err_cnt, UMAC_RBUF_ERR_CNT),
 	STAT_GENET_MISC("mdf_err_cnt", mib.mdf_err_cnt, UMAC_MDF_ERR_CNT),
-	STAT_GENET_MIB_RX("alloc_rx_buff_failed", mib.alloc_rx_buff_failed),
-	STAT_GENET_MIB_RX("rx_dma_failed", mib.rx_dma_failed),
-	STAT_GENET_MIB_TX("tx_dma_failed", mib.tx_dma_failed),
+	STAT_GENET_SOFT_MIB("alloc_rx_buff_failed", mib.alloc_rx_buff_failed),
+	STAT_GENET_SOFT_MIB("rx_dma_failed", mib.rx_dma_failed),
+	STAT_GENET_SOFT_MIB("tx_dma_failed", mib.tx_dma_failed),
 };
 
 #define BCMGENET_STATS_LEN	ARRAY_SIZE(bcmgenet_gstrings_stats)
@@ -668,6 +670,7 @@ static void bcmgenet_update_mib_counters(struct bcmgenet_priv *priv)
 		s = &bcmgenet_gstrings_stats[i];
 		switch (s->type) {
 		case BCMGENET_STAT_NETDEV:
+		case BCMGENET_STAT_SOFT:
 			continue;
 		case BCMGENET_STAT_MIB_RX:
 		case BCMGENET_STAT_MIB_TX:
@@ -971,13 +974,14 @@ static inline void bcmgenet_tx_ring_int_disable(struct bcmgenet_priv *priv,
 }
 
 /* Unlocked version of the reclaim routine */
-static void __bcmgenet_tx_reclaim(struct net_device *dev,
-				  struct bcmgenet_tx_ring *ring)
+static unsigned int __bcmgenet_tx_reclaim(struct net_device *dev,
+					  struct bcmgenet_tx_ring *ring)
 {
 	struct bcmgenet_priv *priv = netdev_priv(dev);
 	int last_tx_cn, last_c_index, num_tx_bds;
 	struct enet_cb *tx_cb_ptr;
 	struct netdev_queue *txq;
+	unsigned int pkts_compl = 0;
 	unsigned int bds_compl;
 	unsigned int c_index;
 
@@ -1005,6 +1009,7 @@ static void __bcmgenet_tx_reclaim(struct net_device *dev,
 		tx_cb_ptr = ring->cbs + last_c_index;
 		bds_compl = 0;
 		if (tx_cb_ptr->skb) {
+			pkts_compl++;
 			bds_compl = skb_shinfo(tx_cb_ptr->skb)->nr_frags + 1;
 			dev->stats.tx_bytes += tx_cb_ptr->skb->len;
 			dma_unmap_single(&dev->dev,
@@ -1028,23 +1033,45 @@ static void __bcmgenet_tx_reclaim(struct net_device *dev,
 		last_c_index &= (num_tx_bds - 1);
 	}
 
-	if (ring->free_bds > (MAX_SKB_FRAGS + 1))
-		ring->int_disable(priv, ring);
-
-	if (netif_tx_queue_stopped(txq))
-		netif_tx_wake_queue(txq);
+	if (ring->free_bds > (MAX_SKB_FRAGS + 1)) {
+		if (netif_tx_queue_stopped(txq))
+			netif_tx_wake_queue(txq);
+	}
 
 	ring->c_index = c_index;
+
+	return pkts_compl;
 }
 
-static void bcmgenet_tx_reclaim(struct net_device *dev,
+static unsigned int bcmgenet_tx_reclaim(struct net_device *dev,
 				struct bcmgenet_tx_ring *ring)
 {
+	unsigned int released;
 	unsigned long flags;
 
 	spin_lock_irqsave(&ring->lock, flags);
-	__bcmgenet_tx_reclaim(dev, ring);
+	released = __bcmgenet_tx_reclaim(dev, ring);
 	spin_unlock_irqrestore(&ring->lock, flags);
+
+	return released;
+}
+
+static int bcmgenet_tx_poll(struct napi_struct *napi, int budget)
+{
+	struct bcmgenet_tx_ring *ring =
+		container_of(napi, struct bcmgenet_tx_ring, napi);
+	unsigned int work_done = 0;
+
+	work_done = bcmgenet_tx_reclaim(ring->priv->dev, ring);
+
+	if (work_done == 0) {
+		napi_complete(napi);
+		ring->int_enable(ring->priv, ring);
+
+		return 0;
+	}
+
+	return budget;
 }
 
 static void bcmgenet_tx_reclaim_all(struct net_device *dev)
@@ -1302,10 +1329,8 @@ static netdev_tx_t bcmgenet_xmit(struct sk_buff *skb, struct net_device *dev)
 	bcmgenet_tdma_ring_writel(priv, ring->index,
 				  ring->prod_index, TDMA_PROD_INDEX);
 
-	if (ring->free_bds <= (MAX_SKB_FRAGS + 1)) {
+	if (ring->free_bds <= (MAX_SKB_FRAGS + 1))
 		netif_tx_stop_queue(txq);
-		ring->int_enable(priv, ring);
-	}
 
 out:
 	spin_unlock_irqrestore(&ring->lock, flags);
@@ -1621,6 +1646,7 @@ static int init_umac(struct bcmgenet_priv *priv)
 	struct device *kdev = &priv->pdev->dev;
 	int ret;
 	u32 reg, cpu_mask_clear;
+	int index;
 
 	dev_dbg(&priv->pdev->dev, "bcmgenet: init_umac\n");
 
@@ -1647,7 +1673,7 @@ static int init_umac(struct bcmgenet_priv *priv)
 
 	bcmgenet_intr_disable(priv);
 
-	cpu_mask_clear = UMAC_IRQ_RXDMA_BDONE;
+	cpu_mask_clear = UMAC_IRQ_RXDMA_BDONE | UMAC_IRQ_TXDMA_BDONE;
 
 	dev_dbg(kdev, "%s:Enabling RXDMA_BDONE interrupt\n", __func__);
 
@@ -1674,6 +1700,10 @@ static int init_umac(struct bcmgenet_priv *priv)
 
 	bcmgenet_intrl2_0_writel(priv, cpu_mask_clear, INTRL2_CPU_MASK_CLEAR);
 
+	for (index = 0; index < priv->hw_params->tx_queues; index++)
+		bcmgenet_intrl2_1_writel(priv, (1 << index),
+					 INTRL2_CPU_MASK_CLEAR);
+
 	/* Enable rx/tx engine.*/
 	dev_dbg(kdev, "done init umac\n");
 
@@ -1693,6 +1723,8 @@ static void bcmgenet_init_tx_ring(struct bcmgenet_priv *priv,
 	unsigned int first_bd;
 
 	spin_lock_init(&ring->lock);
+	ring->priv = priv;
+	netif_napi_add(priv->dev, &ring->napi, bcmgenet_tx_poll, 64);
 	ring->index = index;
 	if (index == DESC_INDEX) {
 		ring->queue = 0;
@@ -1738,6 +1770,17 @@ static void bcmgenet_init_tx_ring(struct bcmgenet_priv *priv,
 				  TDMA_WRITE_PTR);
 	bcmgenet_tdma_ring_writel(priv, index, end_ptr * words_per_bd - 1,
 				  DMA_END_ADDR);
+
+	napi_enable(&ring->napi);
+}
+
+static void bcmgenet_fini_tx_ring(struct bcmgenet_priv *priv,
+				  unsigned int index)
+{
+	struct bcmgenet_tx_ring *ring = &priv->tx_rings[index];
+
+	napi_disable(&ring->napi);
+	netif_napi_del(&ring->napi);
 }
 
 /* Initialize a RDMA ring */
@@ -1907,7 +1950,7 @@ static int bcmgenet_dma_teardown(struct bcmgenet_priv *priv)
 	return ret;
 }
 
-static void bcmgenet_fini_dma(struct bcmgenet_priv *priv)
+static void __bcmgenet_fini_dma(struct bcmgenet_priv *priv)
 {
 	int i;
 
@@ -1926,6 +1969,18 @@ static void bcmgenet_fini_dma(struct bcmgenet_priv *priv)
 	kfree(priv->tx_cbs);
 }
 
+static void bcmgenet_fini_dma(struct bcmgenet_priv *priv)
+{
+	int i;
+
+	bcmgenet_fini_tx_ring(priv, DESC_INDEX);
+
+	for (i = 0; i < priv->hw_params->tx_queues; i++)
+		bcmgenet_fini_tx_ring(priv, i);
+
+	__bcmgenet_fini_dma(priv);
+}
+
 /* init_edma: Initialize DMA control register */
 static int bcmgenet_init_dma(struct bcmgenet_priv *priv)
 {
@@ -1952,7 +2007,7 @@ static int bcmgenet_init_dma(struct bcmgenet_priv *priv)
 	priv->tx_cbs = kcalloc(priv->num_tx_bds, sizeof(struct enet_cb),
 			       GFP_KERNEL);
 	if (!priv->tx_cbs) {
-		bcmgenet_fini_dma(priv);
+		__bcmgenet_fini_dma(priv);
 		return -ENOMEM;
 	}
 
@@ -1975,9 +2030,6 @@ static int bcmgenet_poll(struct napi_struct *napi, int budget)
 			struct bcmgenet_priv, napi);
 	unsigned int work_done;
 
-	/* tx reclaim */
-	bcmgenet_tx_reclaim(priv->dev, &priv->tx_rings[DESC_INDEX]);
-
 	work_done = bcmgenet_desc_rx(priv, budget);
 
 	/* Advancing our consumer index*/
@@ -2022,28 +2074,34 @@ static void bcmgenet_irq_task(struct work_struct *work)
 static irqreturn_t bcmgenet_isr1(int irq, void *dev_id)
 {
 	struct bcmgenet_priv *priv = dev_id;
+	struct bcmgenet_tx_ring *ring;
 	unsigned int index;
 
 	/* Save irq status for bottom-half processing. */
 	priv->irq1_stat =
 		bcmgenet_intrl2_1_readl(priv, INTRL2_CPU_STAT) &
-		~priv->int1_mask;
+		~bcmgenet_intrl2_1_readl(priv, INTRL2_CPU_MASK_STATUS);
 	/* clear interrupts */
 	bcmgenet_intrl2_1_writel(priv, priv->irq1_stat, INTRL2_CPU_CLEAR);
 
 	netif_dbg(priv, intr, priv->dev,
 		  "%s: IRQ=0x%x\n", __func__, priv->irq1_stat);
+
 	/* Check the MBDONE interrupts.
 	 * packet is done, reclaim descriptors
 	 */
-	if (priv->irq1_stat & 0x0000ffff) {
-		index = 0;
-		for (index = 0; index < 16; index++) {
-			if (priv->irq1_stat & (1 << index))
-				bcmgenet_tx_reclaim(priv->dev,
-						    &priv->tx_rings[index]);
+	for (index = 0; index < priv->hw_params->tx_queues; index++) {
+		if (!(priv->irq1_stat & BIT(index)))
+			continue;
+
+		ring = &priv->tx_rings[index];
+
+		if (likely(napi_schedule_prep(&ring->napi))) {
+			ring->int_disable(priv, ring);
+			__napi_schedule(&ring->napi);
 		}
 	}
+
 	return IRQ_HANDLED;
 }
 
@@ -2075,8 +2133,12 @@ static irqreturn_t bcmgenet_isr0(int irq, void *dev_id)
 	}
 	if (priv->irq0_stat &
 			(UMAC_IRQ_TXDMA_BDONE | UMAC_IRQ_TXDMA_PDONE)) {
-		/* Tx reclaim */
-		bcmgenet_tx_reclaim(priv->dev, &priv->tx_rings[DESC_INDEX]);
+		struct bcmgenet_tx_ring *ring = &priv->tx_rings[DESC_INDEX];
+
+		if (likely(napi_schedule_prep(&ring->napi))) {
+			ring->int_disable(priv, ring);
+			__napi_schedule(&ring->napi);
+		}
 	}
 	if (priv->irq0_stat & (UMAC_IRQ_PHY_DET_R |
 				UMAC_IRQ_PHY_DET_F |
diff --git a/drivers/net/ethernet/broadcom/genet/bcmgenet.h b/drivers/net/ethernet/broadcom/genet/bcmgenet.h
index b36ddec0cc0a..0d370d168aee 100644
--- a/drivers/net/ethernet/broadcom/genet/bcmgenet.h
+++ b/drivers/net/ethernet/broadcom/genet/bcmgenet.h
@@ -520,6 +520,7 @@ struct bcmgenet_hw_params {
 
 struct bcmgenet_tx_ring {
 	spinlock_t	lock;		/* ring lock */
+	struct napi_struct napi;	/* NAPI per tx queue */
 	unsigned int	index;		/* ring index */
 	unsigned int	queue;		/* queue index */
 	struct enet_cb	*cbs;		/* tx ring buffer control block*/
@@ -534,6 +535,7 @@ struct bcmgenet_tx_ring {
 			   struct bcmgenet_tx_ring *);
 	void (*int_disable)(struct bcmgenet_priv *priv,
 			    struct bcmgenet_tx_ring *);
+	struct bcmgenet_priv *priv;
 };
 
 /* device context */
diff --git a/drivers/net/ethernet/broadcom/genet/bcmgenet_wol.c b/drivers/net/ethernet/broadcom/genet/bcmgenet_wol.c
index 149a0d70c108..b97122926d3a 100644
--- a/drivers/net/ethernet/broadcom/genet/bcmgenet_wol.c
+++ b/drivers/net/ethernet/broadcom/genet/bcmgenet_wol.c
@@ -73,15 +73,17 @@ int bcmgenet_set_wol(struct net_device *dev, struct ethtool_wolinfo *wol)
 	if (wol->wolopts & ~(WAKE_MAGIC | WAKE_MAGICSECURE))
 		return -EINVAL;
 
+	reg = bcmgenet_umac_readl(priv, UMAC_MPD_CTRL);
 	if (wol->wolopts & WAKE_MAGICSECURE) {
 		bcmgenet_umac_writel(priv, get_unaligned_be16(&wol->sopass[0]),
 				     UMAC_MPD_PW_MS);
 		bcmgenet_umac_writel(priv, get_unaligned_be32(&wol->sopass[2]),
 				     UMAC_MPD_PW_LS);
-		reg = bcmgenet_umac_readl(priv, UMAC_MPD_CTRL);
 		reg |= MPD_PW_EN;
-		bcmgenet_umac_writel(priv, reg, UMAC_MPD_CTRL);
+	} else {
+		reg &= ~MPD_PW_EN;
 	}
+	bcmgenet_umac_writel(priv, reg, UMAC_MPD_CTRL);
 
 	/* Flag the device and relevant IRQ as wakeup capable */
 	if (wol->wolopts) {
diff --git a/drivers/net/ethernet/cadence/macb.c b/drivers/net/ethernet/cadence/macb.c
index ad76b8e35a00..81d41539fcba 100644
--- a/drivers/net/ethernet/cadence/macb.c
+++ b/drivers/net/ethernet/cadence/macb.c
@@ -2113,17 +2113,17 @@ static const struct net_device_ops macb_netdev_ops = {
 };
 
 #if defined(CONFIG_OF)
-static struct macb_config pc302gem_config = {
+static const struct macb_config pc302gem_config = {
 	.caps = MACB_CAPS_SG_DISABLED | MACB_CAPS_GIGABIT_MODE_AVAILABLE,
 	.dma_burst_length = 16,
 };
 
-static struct macb_config sama5d3_config = {
+static const struct macb_config sama5d3_config = {
 	.caps = MACB_CAPS_SG_DISABLED | MACB_CAPS_GIGABIT_MODE_AVAILABLE,
 	.dma_burst_length = 16,
 };
 
-static struct macb_config sama5d4_config = {
+static const struct macb_config sama5d4_config = {
 	.caps = 0,
 	.dma_burst_length = 4,
 };
@@ -2154,7 +2154,7 @@ static void macb_configure_caps(struct macb *bp)
 	if (bp->pdev->dev.of_node) {
 		match = of_match_node(macb_dt_ids, bp->pdev->dev.of_node);
 		if (match && match->data) {
-			config = (const struct macb_config *)match->data;
+			config = match->data;
 
 			bp->caps = config->caps;
 			/*
diff --git a/drivers/net/ethernet/cadence/macb.h b/drivers/net/ethernet/cadence/macb.h
index 31dc080f2437..ff85619a9732 100644
--- a/drivers/net/ethernet/cadence/macb.h
+++ b/drivers/net/ethernet/cadence/macb.h
@@ -351,7 +351,7 @@
 
 /* Bitfields in MID */
 #define MACB_IDNUM_OFFSET			16
-#define MACB_IDNUM_SIZE				16
+#define MACB_IDNUM_SIZE				12
 #define MACB_REV_OFFSET				0
 #define MACB_REV_SIZE				16
 
diff --git a/drivers/net/ethernet/chelsio/cxgb4/clip_tbl.c b/drivers/net/ethernet/chelsio/cxgb4/clip_tbl.c
index 9062a8434246..c308429dd9c7 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/clip_tbl.c
+++ b/drivers/net/ethernet/chelsio/cxgb4/clip_tbl.c
@@ -35,10 +35,10 @@ static inline unsigned int ipv6_clip_hash(struct clip_tbl *d, const u32 *key)
 }
 
 static unsigned int clip_addr_hash(struct clip_tbl *ctbl, const u32 *addr,
-				   int addr_len)
+				   u8 v6)
 {
-	return addr_len == 4 ? ipv4_clip_hash(ctbl, addr) :
-				ipv6_clip_hash(ctbl, addr);
+	return v6 ? ipv6_clip_hash(ctbl, addr) :
+			ipv4_clip_hash(ctbl, addr);
 }
 
 static int clip6_get_mbox(const struct net_device *dev,
@@ -78,23 +78,22 @@ int cxgb4_clip_get(const struct net_device *dev, const u32 *lip, u8 v6)
 	struct clip_entry *ce, *cte;
 	u32 *addr = (u32 *)lip;
 	int hash;
-	int addr_len;
-	int ret = 0;
+	int ret = -1;
 
 	if (!ctbl)
 		return 0;
 
-	if (v6)
-		addr_len = 16;
-	else
-		addr_len = 4;
-
-	hash = clip_addr_hash(ctbl, addr, addr_len);
+	hash = clip_addr_hash(ctbl, addr, v6);
 
 	read_lock_bh(&ctbl->lock);
 	list_for_each_entry(cte, &ctbl->hash_list[hash], list) {
-		if (addr_len == cte->addr_len &&
-		    memcmp(lip, cte->addr, cte->addr_len) == 0) {
+		if (cte->addr6.sin6_family == AF_INET6 && v6)
+			ret = memcmp(lip, cte->addr6.sin6_addr.s6_addr,
+				     sizeof(struct in6_addr));
+		else if (cte->addr.sin_family == AF_INET && !v6)
+			ret = memcmp(lip, (char *)(&cte->addr.sin_addr),
+				     sizeof(struct in_addr));
+		if (!ret) {
 			ce = cte;
 			read_unlock_bh(&ctbl->lock);
 			goto found;
@@ -111,15 +110,20 @@ int cxgb4_clip_get(const struct net_device *dev, const u32 *lip, u8 v6)
 		spin_lock_init(&ce->lock);
 		atomic_set(&ce->refcnt, 0);
 		atomic_dec(&ctbl->nfree);
-		ce->addr_len = addr_len;
-		memcpy(ce->addr, lip, addr_len);
 		list_add_tail(&ce->list, &ctbl->hash_list[hash]);
 		if (v6) {
+			ce->addr6.sin6_family = AF_INET6;
+			memcpy(ce->addr6.sin6_addr.s6_addr,
+			       lip, sizeof(struct in6_addr));
 			ret = clip6_get_mbox(dev, (const struct in6_addr *)lip);
 			if (ret) {
 				write_unlock_bh(&ctbl->lock);
 				return ret;
 			}
+		} else {
+			ce->addr.sin_family = AF_INET;
+			memcpy((char *)(&ce->addr.sin_addr), lip,
+			       sizeof(struct in_addr));
 		}
 	} else {
 		write_unlock_bh(&ctbl->lock);
@@ -140,19 +144,19 @@ void cxgb4_clip_release(const struct net_device *dev, const u32 *lip, u8 v6)
 	struct clip_entry *ce, *cte;
 	u32 *addr = (u32 *)lip;
 	int hash;
-	int addr_len;
-
-	if (v6)
-		addr_len = 16;
-	else
-		addr_len = 4;
+	int ret = -1;
 
-	hash = clip_addr_hash(ctbl, addr, addr_len);
+	hash = clip_addr_hash(ctbl, addr, v6);
 
 	read_lock_bh(&ctbl->lock);
 	list_for_each_entry(cte, &ctbl->hash_list[hash], list) {
-		if (addr_len == cte->addr_len &&
-		    memcmp(lip, cte->addr, cte->addr_len) == 0) {
+		if (cte->addr6.sin6_family == AF_INET6 && v6)
+			ret = memcmp(lip, cte->addr6.sin6_addr.s6_addr,
+				     sizeof(struct in6_addr));
+		else if (cte->addr.sin_family == AF_INET && !v6)
+			ret = memcmp(lip, (char *)(&cte->addr.sin_addr),
+				     sizeof(struct in_addr));
+		if (!ret) {
 			ce = cte;
 			read_unlock_bh(&ctbl->lock);
 			goto found;
@@ -249,10 +253,7 @@ int clip_tbl_show(struct seq_file *seq, void *v)
 	for (i = 0 ; i < ctbl->clipt_size;  ++i) {
 		list_for_each_entry(ce, &ctbl->hash_list[i], list) {
 			ip[0] = '\0';
-			if (ce->addr_len == 16)
-				sprintf(ip, "%pI6c", ce->addr);
-			else
-				sprintf(ip, "%pI4c", ce->addr);
+			sprintf(ip, "%pISc", &ce->addr);
 			seq_printf(seq, "%-25s   %u\n", ip,
 				   atomic_read(&ce->refcnt));
 		}
diff --git a/drivers/net/ethernet/chelsio/cxgb4/clip_tbl.h b/drivers/net/ethernet/chelsio/cxgb4/clip_tbl.h
index 2eaba0161cf8..35eb43c6bcbb 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/clip_tbl.h
+++ b/drivers/net/ethernet/chelsio/cxgb4/clip_tbl.h
@@ -14,8 +14,10 @@ struct clip_entry {
 	spinlock_t lock;	/* Hold while modifying clip reference */
 	atomic_t refcnt;
 	struct list_head list;
-	u32 addr[4];
-	int addr_len;
+	union {
+		struct sockaddr_in addr;
+		struct sockaddr_in6 addr6;
+	};
 };
 
 struct clip_tbl {
diff --git a/drivers/net/ethernet/chelsio/cxgb4/cxgb4.h b/drivers/net/ethernet/chelsio/cxgb4/cxgb4.h
index d6cda17efe6e..c6ff4890d171 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/cxgb4.h
+++ b/drivers/net/ethernet/chelsio/cxgb4/cxgb4.h
@@ -376,8 +376,6 @@ enum {
 enum {
 	INGQ_EXTRAS = 2,        /* firmware event queue and */
 				/*   forwarded interrupts */
-	MAX_EGRQ = MAX_ETH_QSETS*2 + MAX_OFLD_QSETS*2
-		   + MAX_CTRL_QUEUES + MAX_RDMA_QUEUES + MAX_ISCSI_QUEUES,
 	MAX_INGQ = MAX_ETH_QSETS + MAX_OFLD_QSETS + MAX_RDMA_QUEUES
 		   + MAX_RDMA_CIQS + MAX_ISCSI_QUEUES + INGQ_EXTRAS,
 };
@@ -616,11 +614,13 @@ struct sge {
 	unsigned int idma_qid[2];   /* SGE IDMA Hung Ingress Queue ID */
 
 	unsigned int egr_start;
+	unsigned int egr_sz;
 	unsigned int ingr_start;
-	void *egr_map[MAX_EGRQ];    /* qid->queue egress queue map */
-	struct sge_rspq *ingr_map[MAX_INGQ]; /* qid->queue ingress queue map */
-	DECLARE_BITMAP(starving_fl, MAX_EGRQ);
-	DECLARE_BITMAP(txq_maperr, MAX_EGRQ);
+	unsigned int ingr_sz;
+	void **egr_map;    /* qid->queue egress queue map */
+	struct sge_rspq **ingr_map; /* qid->queue ingress queue map */
+	unsigned long *starving_fl;
+	unsigned long *txq_maperr;
 	struct timer_list rx_timer; /* refills starving FLs */
 	struct timer_list tx_timer; /* checks Tx queues */
 };
@@ -1103,7 +1103,7 @@ int t4_restart_aneg(struct adapter *adap, unsigned int mbox, unsigned int port);
 #define T4_MEMORY_WRITE	0
 #define T4_MEMORY_READ	1
 int t4_memory_rw(struct adapter *adap, int win, int mtype, u32 addr, u32 len,
-		 __be32 *buf, int dir);
+		 void *buf, int dir);
 static inline int t4_memory_write(struct adapter *adap, int mtype, u32 addr,
 				  u32 len, __be32 *buf)
 {
@@ -1136,6 +1136,8 @@ int cxgb4_t4_bar2_sge_qregs(struct adapter *adapter,
 
 unsigned int qtimer_val(const struct adapter *adap,
 			const struct sge_rspq *q);
+
+int t4_init_devlog_params(struct adapter *adapter);
 int t4_init_sge_params(struct adapter *adapter);
 int t4_init_tp_params(struct adapter *adap);
 int t4_filter_field_shift(const struct adapter *adap, int filter_sel);
diff --git a/drivers/net/ethernet/chelsio/cxgb4/cxgb4_debugfs.c b/drivers/net/ethernet/chelsio/cxgb4/cxgb4_debugfs.c
index 78854ceb0870..dcb047945290 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/cxgb4_debugfs.c
+++ b/drivers/net/ethernet/chelsio/cxgb4/cxgb4_debugfs.c
@@ -670,9 +670,13 @@ static int cctrl_tbl_show(struct seq_file *seq, void *v)
 		"0.9375" };
 
 	int i;
-	u16 incr[NMTUS][NCCTRL_WIN];
+	u16 (*incr)[NCCTRL_WIN];
 	struct adapter *adap = seq->private;
 
+	incr = kmalloc(sizeof(*incr) * NMTUS, GFP_KERNEL);
+	if (!incr)
+		return -ENOMEM;
+
 	t4_read_cong_tbl(adap, incr);
 
 	for (i = 0; i < NCCTRL_WIN; ++i) {
@@ -685,6 +689,8 @@ static int cctrl_tbl_show(struct seq_file *seq, void *v)
 			   adap->params.a_wnd[i],
 			   dec_fac[adap->params.b_wnd[i]]);
 	}
+
+	kfree(incr);
 	return 0;
 }
 
diff --git a/drivers/net/ethernet/chelsio/cxgb4/cxgb4_main.c b/drivers/net/ethernet/chelsio/cxgb4/cxgb4_main.c
index a22cf932ca35..d92995138f7e 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/cxgb4_main.c
+++ b/drivers/net/ethernet/chelsio/cxgb4/cxgb4_main.c
@@ -920,7 +920,7 @@ static void quiesce_rx(struct adapter *adap)
 {
 	int i;
 
-	for (i = 0; i < ARRAY_SIZE(adap->sge.ingr_map); i++) {
+	for (i = 0; i < adap->sge.ingr_sz; i++) {
 		struct sge_rspq *q = adap->sge.ingr_map[i];
 
 		if (q && q->handler) {
@@ -934,6 +934,21 @@ static void quiesce_rx(struct adapter *adap)
 	}
 }
 
+/* Disable interrupt and napi handler */
+static void disable_interrupts(struct adapter *adap)
+{
+	if (adap->flags & FULL_INIT_DONE) {
+		t4_intr_disable(adap);
+		if (adap->flags & USING_MSIX) {
+			free_msix_queue_irqs(adap);
+			free_irq(adap->msix_info[0].vec, adap);
+		} else {
+			free_irq(adap->pdev->irq, adap);
+		}
+		quiesce_rx(adap);
+	}
+}
+
 /*
  * Enable NAPI scheduling and interrupt generation for all Rx queues.
  */
@@ -941,7 +956,7 @@ static void enable_rx(struct adapter *adap)
 {
 	int i;
 
-	for (i = 0; i < ARRAY_SIZE(adap->sge.ingr_map); i++) {
+	for (i = 0; i < adap->sge.ingr_sz; i++) {
 		struct sge_rspq *q = adap->sge.ingr_map[i];
 
 		if (!q)
@@ -970,8 +985,8 @@ static int setup_sge_queues(struct adapter *adap)
 	int err, msi_idx, i, j;
 	struct sge *s = &adap->sge;
 
-	bitmap_zero(s->starving_fl, MAX_EGRQ);
-	bitmap_zero(s->txq_maperr, MAX_EGRQ);
+	bitmap_zero(s->starving_fl, s->egr_sz);
+	bitmap_zero(s->txq_maperr, s->egr_sz);
 
 	if (adap->flags & USING_MSIX)
 		msi_idx = 1;         /* vector 0 is for non-queue interrupts */
@@ -983,6 +998,19 @@ static int setup_sge_queues(struct adapter *adap)
 		msi_idx = -((int)s->intrq.abs_id + 1);
 	}
 
+	/* NOTE: If you add/delete any Ingress/Egress Queue allocations in here,
+	 * don't forget to update the following which need to be
+	 * synchronized to and changes here.
+	 *
+	 * 1. The calculations of MAX_INGQ in cxgb4.h.
+	 *
+	 * 2. Update enable_msix/name_msix_vecs/request_msix_queue_irqs
+	 *    to accommodate any new/deleted Ingress Queues
+	 *    which need MSI-X Vectors.
+	 *
+	 * 3. Update sge_qinfo_show() to include information on the
+	 *    new/deleted queues.
+	 */
 	err = t4_sge_alloc_rxq(adap, &s->fw_evtq, true, adap->port[0],
 			       msi_idx, NULL, fwevtq_handler);
 	if (err) {
@@ -4244,19 +4272,12 @@ static int cxgb_up(struct adapter *adap)
 
 static void cxgb_down(struct adapter *adapter)
 {
-	t4_intr_disable(adapter);
 	cancel_work_sync(&adapter->tid_release_task);
 	cancel_work_sync(&adapter->db_full_task);
 	cancel_work_sync(&adapter->db_drop_task);
 	adapter->tid_release_task_busy = false;
 	adapter->tid_release_head = NULL;
 
-	if (adapter->flags & USING_MSIX) {
-		free_msix_queue_irqs(adapter);
-		free_irq(adapter->msix_info[0].vec, adapter);
-	} else
-		free_irq(adapter->pdev->irq, adapter);
-	quiesce_rx(adapter);
 	t4_sge_stop(adapter);
 	t4_free_sge_resources(adapter);
 	adapter->flags &= ~FULL_INIT_DONE;
@@ -4733,8 +4754,9 @@ static int adap_init1(struct adapter *adap, struct fw_caps_config_cmd *c)
 	if (ret < 0)
 		return ret;
 
-	ret = t4_cfg_pfvf(adap, adap->fn, adap->fn, 0, MAX_EGRQ, 64, MAX_INGQ,
-			  0, 0, 4, 0xf, 0xf, 16, FW_CMD_CAP_PF, FW_CMD_CAP_PF);
+	ret = t4_cfg_pfvf(adap, adap->fn, adap->fn, 0, adap->sge.egr_sz, 64,
+			  MAX_INGQ, 0, 0, 4, 0xf, 0xf, 16, FW_CMD_CAP_PF,
+			  FW_CMD_CAP_PF);
 	if (ret < 0)
 		return ret;
 
@@ -5088,10 +5110,15 @@ static int adap_init0(struct adapter *adap)
 	enum dev_state state;
 	u32 params[7], val[7];
 	struct fw_caps_config_cmd caps_cmd;
-	struct fw_devlog_cmd devlog_cmd;
-	u32 devlog_meminfo;
 	int reset = 1;
 
+	/* Grab Firmware Device Log parameters as early as possible so we have
+	 * access to it for debugging, etc.
+	 */
+	ret = t4_init_devlog_params(adap);
+	if (ret < 0)
+		return ret;
+
 	/* Contact FW, advertising Master capability */
 	ret = t4_fw_hello(adap, adap->mbox, adap->mbox, MASTER_MAY, &state);
 	if (ret < 0) {
@@ -5169,30 +5196,6 @@ static int adap_init0(struct adapter *adap)
 	if (ret < 0)
 		goto bye;
 
-	/* Read firmware device log parameters.  We really need to find a way
-	 * to get these parameters initialized with some default values (which
-	 * are likely to be correct) for the case where we either don't
-	 * attache to the firmware or it's crashed when we probe the adapter.
-	 * That way we'll still be able to perform early firmware startup
-	 * debugging ...  If the request to get the Firmware's Device Log
-	 * parameters fails, we'll live so we don't make that a fatal error.
-	 */
-	memset(&devlog_cmd, 0, sizeof(devlog_cmd));
-	devlog_cmd.op_to_write = htonl(FW_CMD_OP_V(FW_DEVLOG_CMD) |
-				       FW_CMD_REQUEST_F | FW_CMD_READ_F);
-	devlog_cmd.retval_len16 = htonl(FW_LEN16(devlog_cmd));
-	ret = t4_wr_mbox(adap, adap->mbox, &devlog_cmd, sizeof(devlog_cmd),
-			 &devlog_cmd);
-	if (ret == 0) {
-		devlog_meminfo =
-			ntohl(devlog_cmd.memtype_devlog_memaddr16_devlog);
-		adap->params.devlog.memtype =
-			FW_DEVLOG_CMD_MEMTYPE_DEVLOG_G(devlog_meminfo);
-		adap->params.devlog.start =
-			FW_DEVLOG_CMD_MEMADDR16_DEVLOG_G(devlog_meminfo) << 4;
-		adap->params.devlog.size = ntohl(devlog_cmd.memsize_devlog);
-	}
-
 	/*
 	 * Find out what ports are available to us.  Note that we need to do
 	 * this before calling adap_init0_no_config() since it needs nports
@@ -5293,6 +5296,51 @@ static int adap_init0(struct adapter *adap)
 	adap->tids.nftids = val[4] - val[3] + 1;
 	adap->sge.ingr_start = val[5];
 
+	/* qids (ingress/egress) returned from firmware can be anywhere
+	 * in the range from EQ(IQFLINT)_START to EQ(IQFLINT)_END.
+	 * Hence driver needs to allocate memory for this range to
+	 * store the queue info. Get the highest IQFLINT/EQ index returned
+	 * in FW_EQ_*_CMD.alloc command.
+	 */
+	params[0] = FW_PARAM_PFVF(EQ_END);
+	params[1] = FW_PARAM_PFVF(IQFLINT_END);
+	ret = t4_query_params(adap, adap->mbox, adap->fn, 0, 2, params, val);
+	if (ret < 0)
+		goto bye;
+	adap->sge.egr_sz = val[0] - adap->sge.egr_start + 1;
+	adap->sge.ingr_sz = val[1] - adap->sge.ingr_start + 1;
+
+	adap->sge.egr_map = kcalloc(adap->sge.egr_sz,
+				    sizeof(*adap->sge.egr_map), GFP_KERNEL);
+	if (!adap->sge.egr_map) {
+		ret = -ENOMEM;
+		goto bye;
+	}
+
+	adap->sge.ingr_map = kcalloc(adap->sge.ingr_sz,
+				     sizeof(*adap->sge.ingr_map), GFP_KERNEL);
+	if (!adap->sge.ingr_map) {
+		ret = -ENOMEM;
+		goto bye;
+	}
+
+	/* Allocate the memory for the vaious egress queue bitmaps
+	 * ie starving_fl and txq_maperr.
+	 */
+	adap->sge.starving_fl =	kcalloc(BITS_TO_LONGS(adap->sge.egr_sz),
+					sizeof(long), GFP_KERNEL);
+	if (!adap->sge.starving_fl) {
+		ret = -ENOMEM;
+		goto bye;
+	}
+
+	adap->sge.txq_maperr = kcalloc(BITS_TO_LONGS(adap->sge.egr_sz),
+				       sizeof(long), GFP_KERNEL);
+	if (!adap->sge.txq_maperr) {
+		ret = -ENOMEM;
+		goto bye;
+	}
+
 	params[0] = FW_PARAM_PFVF(CLIP_START);
 	params[1] = FW_PARAM_PFVF(CLIP_END);
 	ret = t4_query_params(adap, adap->mbox, adap->fn, 0, 2, params, val);
@@ -5501,6 +5549,10 @@ static int adap_init0(struct adapter *adap)
 	 * happened to HW/FW, stop issuing commands.
 	 */
 bye:
+	kfree(adap->sge.egr_map);
+	kfree(adap->sge.ingr_map);
+	kfree(adap->sge.starving_fl);
+	kfree(adap->sge.txq_maperr);
 	if (ret != -ETIMEDOUT && ret != -EIO)
 		t4_fw_bye(adap, adap->mbox);
 	return ret;
@@ -5528,6 +5580,7 @@ static pci_ers_result_t eeh_err_detected(struct pci_dev *pdev,
 		netif_carrier_off(dev);
 	}
 	spin_unlock(&adap->stats_lock);
+	disable_interrupts(adap);
 	if (adap->flags & FULL_INIT_DONE)
 		cxgb_down(adap);
 	rtnl_unlock();
@@ -5912,6 +5965,10 @@ static void free_some_resources(struct adapter *adapter)
 
 	t4_free_mem(adapter->l2t);
 	t4_free_mem(adapter->tids.tid_tab);
+	kfree(adapter->sge.egr_map);
+	kfree(adapter->sge.ingr_map);
+	kfree(adapter->sge.starving_fl);
+	kfree(adapter->sge.txq_maperr);
 	disable_msi(adapter);
 
 	for_each_port(adapter, i)
@@ -6237,6 +6294,8 @@ static void remove_one(struct pci_dev *pdev)
 		if (is_offload(adapter))
 			detach_ulds(adapter);
 
+		disable_interrupts(adapter);
+
 		for_each_port(adapter, i)
 			if (adapter->port[i]->reg_state == NETREG_REGISTERED)
 				unregister_netdev(adapter->port[i]);
diff --git a/drivers/net/ethernet/chelsio/cxgb4/sge.c b/drivers/net/ethernet/chelsio/cxgb4/sge.c
index b4b9f6048fe7..b688b32c21fe 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/sge.c
+++ b/drivers/net/ethernet/chelsio/cxgb4/sge.c
@@ -2171,7 +2171,7 @@ static void sge_rx_timer_cb(unsigned long data)
 	struct adapter *adap = (struct adapter *)data;
 	struct sge *s = &adap->sge;
 
-	for (i = 0; i < ARRAY_SIZE(s->starving_fl); i++)
+	for (i = 0; i < BITS_TO_LONGS(s->egr_sz); i++)
 		for (m = s->starving_fl[i]; m; m &= m - 1) {
 			struct sge_eth_rxq *rxq;
 			unsigned int id = __ffs(m) + i * BITS_PER_LONG;
@@ -2259,7 +2259,7 @@ static void sge_tx_timer_cb(unsigned long data)
 	struct adapter *adap = (struct adapter *)data;
 	struct sge *s = &adap->sge;
 
-	for (i = 0; i < ARRAY_SIZE(s->txq_maperr); i++)
+	for (i = 0; i < BITS_TO_LONGS(s->egr_sz); i++)
 		for (m = s->txq_maperr[i]; m; m &= m - 1) {
 			unsigned long id = __ffs(m) + i * BITS_PER_LONG;
 			struct sge_ofld_txq *txq = s->egr_map[id];
@@ -2741,7 +2741,8 @@ void t4_free_sge_resources(struct adapter *adap)
 		free_rspq_fl(adap, &adap->sge.intrq, NULL);
 
 	/* clear the reverse egress queue map */
-	memset(adap->sge.egr_map, 0, sizeof(adap->sge.egr_map));
+	memset(adap->sge.egr_map, 0,
+	       adap->sge.egr_sz * sizeof(*adap->sge.egr_map));
 }
 
 void t4_sge_start(struct adapter *adap)
diff --git a/drivers/net/ethernet/chelsio/cxgb4/t4_hw.c b/drivers/net/ethernet/chelsio/cxgb4/t4_hw.c
index 4d643b65265e..ee394dc68303 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/t4_hw.c
+++ b/drivers/net/ethernet/chelsio/cxgb4/t4_hw.c
@@ -449,7 +449,7 @@ int t4_edc_read(struct adapter *adap, int idx, u32 addr, __be32 *data, u64 *ecc)
  *	@mtype: memory type: MEM_EDC0, MEM_EDC1 or MEM_MC
  *	@addr: address within indicated memory type
  *	@len: amount of memory to transfer
- *	@buf: host memory buffer
+ *	@hbuf: host memory buffer
  *	@dir: direction of transfer T4_MEMORY_READ (1) or T4_MEMORY_WRITE (0)
  *
  *	Reads/writes an [almost] arbitrary memory region in the firmware: the
@@ -460,15 +460,17 @@ int t4_edc_read(struct adapter *adap, int idx, u32 addr, __be32 *data, u64 *ecc)
  *	caller's responsibility to perform appropriate byte order conversions.
  */
 int t4_memory_rw(struct adapter *adap, int win, int mtype, u32 addr,
-		 u32 len, __be32 *buf, int dir)
+		 u32 len, void *hbuf, int dir)
 {
 	u32 pos, offset, resid, memoffset;
 	u32 edc_size, mc_size, win_pf, mem_reg, mem_aperture, mem_base;
+	u32 *buf;
 
 	/* Argument sanity checks ...
 	 */
-	if (addr & 0x3)
+	if (addr & 0x3 || (uintptr_t)hbuf & 0x3)
 		return -EINVAL;
+	buf = (u32 *)hbuf;
 
 	/* It's convenient to be able to handle lengths which aren't a
 	 * multiple of 32-bits because we often end up transferring files to
@@ -532,14 +534,45 @@ int t4_memory_rw(struct adapter *adap, int win, int mtype, u32 addr,
 
 	/* Transfer data to/from the adapter as long as there's an integral
 	 * number of 32-bit transfers to complete.
+	 *
+	 * A note on Endianness issues:
+	 *
+	 * The "register" reads and writes below from/to the PCI-E Memory
+	 * Window invoke the standard adapter Big-Endian to PCI-E Link
+	 * Little-Endian "swizzel."  As a result, if we have the following
+	 * data in adapter memory:
+	 *
+	 *     Memory:  ... | b0 | b1 | b2 | b3 | ...
+	 *     Address:      i+0  i+1  i+2  i+3
+	 *
+	 * Then a read of the adapter memory via the PCI-E Memory Window
+	 * will yield:
+	 *
+	 *     x = readl(i)
+	 *         31                  0
+	 *         [ b3 | b2 | b1 | b0 ]
+	 *
+	 * If this value is stored into local memory on a Little-Endian system
+	 * it will show up correctly in local memory as:
+	 *
+	 *     ( ..., b0, b1, b2, b3, ... )
+	 *
+	 * But on a Big-Endian system, the store will show up in memory
+	 * incorrectly swizzled as:
+	 *
+	 *     ( ..., b3, b2, b1, b0, ... )
+	 *
+	 * So we need to account for this in the reads and writes to the
+	 * PCI-E Memory Window below by undoing the register read/write
+	 * swizzels.
 	 */
 	while (len > 0) {
 		if (dir == T4_MEMORY_READ)
-			*buf++ = (__force __be32) t4_read_reg(adap,
-							mem_base + offset);
+			*buf++ = le32_to_cpu((__force __le32)t4_read_reg(adap,
+						mem_base + offset));
 		else
 			t4_write_reg(adap, mem_base + offset,
-				     (__force u32) *buf++);
+				     (__force u32)cpu_to_le32(*buf++));
 		offset += sizeof(__be32);
 		len -= sizeof(__be32);
 
@@ -568,15 +601,16 @@ int t4_memory_rw(struct adapter *adap, int win, int mtype, u32 addr,
 	 */
 	if (resid) {
 		union {
-			__be32 word;
+			u32 word;
 			char byte[4];
 		} last;
 		unsigned char *bp;
 		int i;
 
 		if (dir == T4_MEMORY_READ) {
-			last.word = (__force __be32) t4_read_reg(adap,
-							mem_base + offset);
+			last.word = le32_to_cpu(
+					(__force __le32)t4_read_reg(adap,
+						mem_base + offset));
 			for (bp = (unsigned char *)buf, i = resid; i < 4; i++)
 				bp[i] = last.byte[i];
 		} else {
@@ -584,7 +618,7 @@ int t4_memory_rw(struct adapter *adap, int win, int mtype, u32 addr,
 			for (i = resid; i < 4; i++)
 				last.byte[i] = 0;
 			t4_write_reg(adap, mem_base + offset,
-				     (__force u32) last.word);
+				     (__force u32)cpu_to_le32(last.word));
 		}
 	}
 
@@ -1086,7 +1120,7 @@ int t4_prep_fw(struct adapter *adap, struct fw_info *fw_info,
 		}
 
 		/* Installed successfully, update the cached header too. */
-		memcpy(card_fw, fs_fw, sizeof(*card_fw));
+		*card_fw = *fs_fw;
 		card_fw_usable = 1;
 		*reset = 0;	/* already reset as part of load_fw */
 	}
@@ -4425,6 +4459,59 @@ int cxgb4_t4_bar2_sge_qregs(struct adapter *adapter,
 }
 
 /**
+ *	t4_init_devlog_params - initialize adapter->params.devlog
+ *	@adap: the adapter
+ *
+ *	Initialize various fields of the adapter's Firmware Device Log
+ *	Parameters structure.
+ */
+int t4_init_devlog_params(struct adapter *adap)
+{
+	struct devlog_params *dparams = &adap->params.devlog;
+	u32 pf_dparams;
+	unsigned int devlog_meminfo;
+	struct fw_devlog_cmd devlog_cmd;
+	int ret;
+
+	/* If we're dealing with newer firmware, the Device Log Paramerters
+	 * are stored in a designated register which allows us to access the
+	 * Device Log even if we can't talk to the firmware.
+	 */
+	pf_dparams =
+		t4_read_reg(adap, PCIE_FW_REG(PCIE_FW_PF_A, PCIE_FW_PF_DEVLOG));
+	if (pf_dparams) {
+		unsigned int nentries, nentries128;
+
+		dparams->memtype = PCIE_FW_PF_DEVLOG_MEMTYPE_G(pf_dparams);
+		dparams->start = PCIE_FW_PF_DEVLOG_ADDR16_G(pf_dparams) << 4;
+
+		nentries128 = PCIE_FW_PF_DEVLOG_NENTRIES128_G(pf_dparams);
+		nentries = (nentries128 + 1) * 128;
+		dparams->size = nentries * sizeof(struct fw_devlog_e);
+
+		return 0;
+	}
+
+	/* Otherwise, ask the firmware for it's Device Log Parameters.
+	 */
+	memset(&devlog_cmd, 0, sizeof(devlog_cmd));
+	devlog_cmd.op_to_write = htonl(FW_CMD_OP_V(FW_DEVLOG_CMD) |
+				       FW_CMD_REQUEST_F | FW_CMD_READ_F);
+	devlog_cmd.retval_len16 = htonl(FW_LEN16(devlog_cmd));
+	ret = t4_wr_mbox(adap, adap->mbox, &devlog_cmd, sizeof(devlog_cmd),
+			 &devlog_cmd);
+	if (ret)
+		return ret;
+
+	devlog_meminfo = ntohl(devlog_cmd.memtype_devlog_memaddr16_devlog);
+	dparams->memtype = FW_DEVLOG_CMD_MEMTYPE_DEVLOG_G(devlog_meminfo);
+	dparams->start = FW_DEVLOG_CMD_MEMADDR16_DEVLOG_G(devlog_meminfo) << 4;
+	dparams->size = ntohl(devlog_cmd.memsize_devlog);
+
+	return 0;
+}
+
+/**
  *	t4_init_sge_params - initialize adap->params.sge
  *	@adapter: the adapter
  *
diff --git a/drivers/net/ethernet/chelsio/cxgb4/t4_regs.h b/drivers/net/ethernet/chelsio/cxgb4/t4_regs.h
index 231a725f6d5d..326674b19983 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/t4_regs.h
+++ b/drivers/net/ethernet/chelsio/cxgb4/t4_regs.h
@@ -63,6 +63,8 @@
 #define MC_BIST_STATUS_REG(reg_addr, idx) ((reg_addr) + (idx) * 4)
 #define EDC_BIST_STATUS_REG(reg_addr, idx) ((reg_addr) + (idx) * 4)
 
+#define PCIE_FW_REG(reg_addr, idx) ((reg_addr) + (idx) * 4)
+
 #define SGE_PF_KDOORBELL_A 0x0
 
 #define QID_S    15
@@ -707,6 +709,7 @@
 #define PFNUM_V(x) ((x) << PFNUM_S)
 
 #define PCIE_FW_A 0x30b8
+#define PCIE_FW_PF_A 0x30bc
 
 #define PCIE_CORE_UTL_SYSTEM_BUS_AGENT_STATUS_A 0x5908
 
diff --git a/drivers/net/ethernet/chelsio/cxgb4/t4fw_api.h b/drivers/net/ethernet/chelsio/cxgb4/t4fw_api.h
index 9b353a88cbda..a4a19e0ec7f5 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/t4fw_api.h
+++ b/drivers/net/ethernet/chelsio/cxgb4/t4fw_api.h
@@ -101,7 +101,7 @@ enum fw_wr_opcodes {
 	FW_RI_BIND_MW_WR               = 0x18,
 	FW_RI_FR_NSMR_WR               = 0x19,
 	FW_RI_INV_LSTAG_WR             = 0x1a,
-	FW_LASTC2E_WR                  = 0x40
+	FW_LASTC2E_WR                  = 0x70
 };
 
 struct fw_wr_hdr {
@@ -993,6 +993,7 @@ enum fw_memtype_cf {
 	FW_MEMTYPE_CF_EXTMEM		= 0x2,
 	FW_MEMTYPE_CF_FLASH		= 0x4,
 	FW_MEMTYPE_CF_INTERNAL		= 0x5,
+	FW_MEMTYPE_CF_EXTMEM1           = 0x6,
 };
 
 struct fw_caps_config_cmd {
@@ -1035,6 +1036,7 @@ enum fw_params_mnem {
 	FW_PARAMS_MNEM_PFVF		= 2,	/* function params */
 	FW_PARAMS_MNEM_REG		= 3,	/* limited register access */
 	FW_PARAMS_MNEM_DMAQ		= 4,	/* dma queue params */
+	FW_PARAMS_MNEM_CHNET            = 5,    /* chnet params */
 	FW_PARAMS_MNEM_LAST
 };
 
@@ -3102,7 +3104,8 @@ enum fw_devlog_facility {
 	FW_DEVLOG_FACILITY_FCOE		= 0x2E,
 	FW_DEVLOG_FACILITY_FOISCSI	= 0x30,
 	FW_DEVLOG_FACILITY_FOFCOE	= 0x32,
-	FW_DEVLOG_FACILITY_MAX		= 0x32,
+	FW_DEVLOG_FACILITY_CHNET        = 0x34,
+	FW_DEVLOG_FACILITY_MAX          = 0x34,
 };
 
 /* log message format */
@@ -3139,4 +3142,36 @@ struct fw_devlog_cmd {
 	(((x) >> FW_DEVLOG_CMD_MEMADDR16_DEVLOG_S) & \
 	 FW_DEVLOG_CMD_MEMADDR16_DEVLOG_M)
 
+/* P C I E   F W   P F 7   R E G I S T E R */
+
+/* PF7 stores the Firmware Device Log parameters which allows Host Drivers to
+ * access the "devlog" which needing to contact firmware.  The encoding is
+ * mostly the same as that returned by the DEVLOG command except for the size
+ * which is encoded as the number of entries in multiples-1 of 128 here rather
+ * than the memory size as is done in the DEVLOG command.  Thus, 0 means 128
+ * and 15 means 2048.  This of course in turn constrains the allowed values
+ * for the devlog size ...
+ */
+#define PCIE_FW_PF_DEVLOG		7
+
+#define PCIE_FW_PF_DEVLOG_NENTRIES128_S	28
+#define PCIE_FW_PF_DEVLOG_NENTRIES128_M	0xf
+#define PCIE_FW_PF_DEVLOG_NENTRIES128_V(x) \
+	((x) << PCIE_FW_PF_DEVLOG_NENTRIES128_S)
+#define PCIE_FW_PF_DEVLOG_NENTRIES128_G(x) \
+	(((x) >> PCIE_FW_PF_DEVLOG_NENTRIES128_S) & \
+	 PCIE_FW_PF_DEVLOG_NENTRIES128_M)
+
+#define PCIE_FW_PF_DEVLOG_ADDR16_S	4
+#define PCIE_FW_PF_DEVLOG_ADDR16_M	0xffffff
+#define PCIE_FW_PF_DEVLOG_ADDR16_V(x)	((x) << PCIE_FW_PF_DEVLOG_ADDR16_S)
+#define PCIE_FW_PF_DEVLOG_ADDR16_G(x) \
+	(((x) >> PCIE_FW_PF_DEVLOG_ADDR16_S) & PCIE_FW_PF_DEVLOG_ADDR16_M)
+
+#define PCIE_FW_PF_DEVLOG_MEMTYPE_S	0
+#define PCIE_FW_PF_DEVLOG_MEMTYPE_M	0xf
+#define PCIE_FW_PF_DEVLOG_MEMTYPE_V(x)	((x) << PCIE_FW_PF_DEVLOG_MEMTYPE_S)
+#define PCIE_FW_PF_DEVLOG_MEMTYPE_G(x) \
+	(((x) >> PCIE_FW_PF_DEVLOG_MEMTYPE_S) & PCIE_FW_PF_DEVLOG_MEMTYPE_M)
+
 #endif /* _T4FW_INTERFACE_H_ */
diff --git a/drivers/net/ethernet/chelsio/cxgb4/t4fw_version.h b/drivers/net/ethernet/chelsio/cxgb4/t4fw_version.h
index e2bd3f747858..b9d1cbac0eee 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/t4fw_version.h
+++ b/drivers/net/ethernet/chelsio/cxgb4/t4fw_version.h
@@ -36,13 +36,13 @@
 #define __T4FW_VERSION_H__
 
 #define T4FW_VERSION_MAJOR 0x01
-#define T4FW_VERSION_MINOR 0x0C
-#define T4FW_VERSION_MICRO 0x19
+#define T4FW_VERSION_MINOR 0x0D
+#define T4FW_VERSION_MICRO 0x20
 #define T4FW_VERSION_BUILD 0x00
 
 #define T5FW_VERSION_MAJOR 0x01
-#define T5FW_VERSION_MINOR 0x0C
-#define T5FW_VERSION_MICRO 0x19
+#define T5FW_VERSION_MINOR 0x0D
+#define T5FW_VERSION_MICRO 0x20
 #define T5FW_VERSION_BUILD 0x00
 
 #endif
diff --git a/drivers/net/ethernet/chelsio/cxgb4vf/sge.c b/drivers/net/ethernet/chelsio/cxgb4vf/sge.c
index 0545f0de1c52..e0d711071afb 100644
--- a/drivers/net/ethernet/chelsio/cxgb4vf/sge.c
+++ b/drivers/net/ethernet/chelsio/cxgb4vf/sge.c
@@ -1004,7 +1004,7 @@ static inline void ring_tx_db(struct adapter *adapter, struct sge_txq *tq,
 					      ? (tq->pidx - 1)
 					      : (tq->size - 1));
 			__be64 *src = (__be64 *)&tq->desc[index];
-			__be64 __iomem *dst = (__be64 *)(tq->bar2_addr +
+			__be64 __iomem *dst = (__be64 __iomem *)(tq->bar2_addr +
 							 SGE_UDB_WCDOORBELL);
 			unsigned int count = EQ_UNIT / sizeof(__be64);
 
@@ -1018,7 +1018,11 @@ static inline void ring_tx_db(struct adapter *adapter, struct sge_txq *tq,
 			 * DMA.
 			 */
 			while (count) {
-				writeq(*src, dst);
+				/* the (__force u64) is because the compiler
+				 * doesn't understand the endian swizzling
+				 * going on
+				 */
+				writeq((__force u64)*src, dst);
 				src++;
 				dst++;
 				count--;
@@ -1252,8 +1256,8 @@ int t4vf_eth_xmit(struct sk_buff *skb, struct net_device *dev)
 	BUG_ON(DIV_ROUND_UP(ETHTXQ_MAX_HDR, TXD_PER_EQ_UNIT) > 1);
 	wr = (void *)&txq->q.desc[txq->q.pidx];
 	wr->equiq_to_len16 = cpu_to_be32(wr_mid);
-	wr->r3[0] = cpu_to_be64(0);
-	wr->r3[1] = cpu_to_be64(0);
+	wr->r3[0] = cpu_to_be32(0);
+	wr->r3[1] = cpu_to_be32(0);
 	skb_copy_from_linear_data(skb, (void *)wr->ethmacdst, fw_hdr_copy_len);
 	end = (u64 *)wr + flits;
 
diff --git a/drivers/net/ethernet/chelsio/cxgb4vf/t4vf_hw.c b/drivers/net/ethernet/chelsio/cxgb4vf/t4vf_hw.c
index 1b5506df35b1..280b4a215849 100644
--- a/drivers/net/ethernet/chelsio/cxgb4vf/t4vf_hw.c
+++ b/drivers/net/ethernet/chelsio/cxgb4vf/t4vf_hw.c
@@ -210,10 +210,10 @@ int t4vf_wr_mbox_core(struct adapter *adapter, const void *cmd, int size,
 
 			if (rpl) {
 				/* request bit in high-order BE word */
-				WARN_ON((be32_to_cpu(*(const u32 *)cmd)
+				WARN_ON((be32_to_cpu(*(const __be32 *)cmd)
 					 & FW_CMD_REQUEST_F) == 0);
 				get_mbox_rpl(adapter, rpl, size, mbox_data);
-				WARN_ON((be32_to_cpu(*(u32 *)rpl)
+				WARN_ON((be32_to_cpu(*(__be32 *)rpl)
 					 & FW_CMD_REQUEST_F) != 0);
 			}
 			t4_write_reg(adapter, mbox_ctl,
@@ -484,7 +484,7 @@ int t4_bar2_sge_qregs(struct adapter *adapter,
 	 *  o The BAR2 Queue ID.
 	 *  o The BAR2 Queue ID Offset into the BAR2 page.
 	 */
-	bar2_page_offset = ((qid >> qpp_shift) << page_shift);
+	bar2_page_offset = ((u64)(qid >> qpp_shift) << page_shift);
 	bar2_qid = qid & qpp_mask;
 	bar2_qid_offset = bar2_qid * SGE_UDB_SIZE;
 
diff --git a/drivers/net/ethernet/cisco/enic/enic_main.c b/drivers/net/ethernet/cisco/enic/enic_main.c
index 9cbe038a388e..a5179bfcdc2c 100644
--- a/drivers/net/ethernet/cisco/enic/enic_main.c
+++ b/drivers/net/ethernet/cisco/enic/enic_main.c
@@ -272,8 +272,8 @@ static irqreturn_t enic_isr_legacy(int irq, void *data)
 	}
 
 	if (ENIC_TEST_INTR(pba, notify_intr)) {
-		vnic_intr_return_all_credits(&enic->intr[notify_intr]);
 		enic_notify_check(enic);
+		vnic_intr_return_all_credits(&enic->intr[notify_intr]);
 	}
 
 	if (ENIC_TEST_INTR(pba, err_intr)) {
@@ -346,8 +346,8 @@ static irqreturn_t enic_isr_msix_notify(int irq, void *data)
 	struct enic *enic = data;
 	unsigned int intr = enic_msix_notify_intr(enic);
 
-	vnic_intr_return_all_credits(&enic->intr[intr]);
 	enic_notify_check(enic);
+	vnic_intr_return_all_credits(&enic->intr[intr]);
 
 	return IRQ_HANDLED;
 }
diff --git a/drivers/net/ethernet/dec/tulip/tulip_core.c b/drivers/net/ethernet/dec/tulip/tulip_core.c
index 3b42556f7f8d..ed41559bae77 100644
--- a/drivers/net/ethernet/dec/tulip/tulip_core.c
+++ b/drivers/net/ethernet/dec/tulip/tulip_core.c
@@ -589,7 +589,7 @@ static void tulip_tx_timeout(struct net_device *dev)
 			       (unsigned int)tp->rx_ring[i].buffer1,
 			       (unsigned int)tp->rx_ring[i].buffer2,
 			       buf[0], buf[1], buf[2]);
-			for (j = 0; buf[j] != 0xee && j < 1600; j++)
+			for (j = 0; ((j < 1600) && buf[j] != 0xee); j++)
 				if (j < 100)
 					pr_cont(" %02x", buf[j]);
 			pr_cont(" j=%d\n", j);
diff --git a/drivers/net/ethernet/emulex/benet/be.h b/drivers/net/ethernet/emulex/benet/be.h
index 27de37aa90af..27b9fe99a9bd 100644
--- a/drivers/net/ethernet/emulex/benet/be.h
+++ b/drivers/net/ethernet/emulex/benet/be.h
@@ -354,6 +354,7 @@ struct be_vf_cfg {
 	u16 vlan_tag;
 	u32 tx_rate;
 	u32 plink_tracking;
+	u32 privileges;
 };
 
 enum vf_state {
@@ -423,6 +424,7 @@ struct be_adapter {
 
 	u8 __iomem *csr;	/* CSR BAR used only for BE2/3 */
 	u8 __iomem *db;		/* Door Bell */
+	u8 __iomem *pcicfg;	/* On SH,BEx only. Shadow of PCI config space */
 
 	struct mutex mbox_lock; /* For serializing mbox cmds to BE card */
 	struct be_dma_mem mbox_mem;
diff --git a/drivers/net/ethernet/emulex/benet/be_cmds.c b/drivers/net/ethernet/emulex/benet/be_cmds.c
index 36916cfa70f9..7f05f309e935 100644
--- a/drivers/net/ethernet/emulex/benet/be_cmds.c
+++ b/drivers/net/ethernet/emulex/benet/be_cmds.c
@@ -1902,15 +1902,11 @@ int be_cmd_modify_eqd(struct be_adapter *adapter, struct be_set_eqd *set_eqd,
 {
 	int num_eqs, i = 0;
 
-	if (lancer_chip(adapter) && num > 8) {
-		while (num) {
-			num_eqs = min(num, 8);
-			__be_cmd_modify_eqd(adapter, &set_eqd[i], num_eqs);
-			i += num_eqs;
-			num -= num_eqs;
-		}
-	} else {
-		__be_cmd_modify_eqd(adapter, set_eqd, num);
+	while (num) {
+		num_eqs = min(num, 8);
+		__be_cmd_modify_eqd(adapter, &set_eqd[i], num_eqs);
+		i += num_eqs;
+		num -= num_eqs;
 	}
 
 	return 0;
@@ -1918,7 +1914,7 @@ int be_cmd_modify_eqd(struct be_adapter *adapter, struct be_set_eqd *set_eqd,
 
 /* Uses sycnhronous mcc */
 int be_cmd_vlan_config(struct be_adapter *adapter, u32 if_id, u16 *vtag_array,
-		       u32 num)
+		       u32 num, u32 domain)
 {
 	struct be_mcc_wrb *wrb;
 	struct be_cmd_req_vlan_config *req;
@@ -1936,6 +1932,7 @@ int be_cmd_vlan_config(struct be_adapter *adapter, u32 if_id, u16 *vtag_array,
 	be_wrb_cmd_hdr_prepare(&req->hdr, CMD_SUBSYSTEM_COMMON,
 			       OPCODE_COMMON_NTWK_VLAN_CONFIG, sizeof(*req),
 			       wrb, NULL);
+	req->hdr.domain = domain;
 
 	req->interface_id = if_id;
 	req->untagged = BE_IF_FLAGS_UNTAGGED & be_if_cap_flags(adapter) ? 1 : 0;
diff --git a/drivers/net/ethernet/emulex/benet/be_cmds.h b/drivers/net/ethernet/emulex/benet/be_cmds.h
index db761e8e42a3..a7634a3f052a 100644
--- a/drivers/net/ethernet/emulex/benet/be_cmds.h
+++ b/drivers/net/ethernet/emulex/benet/be_cmds.h
@@ -2256,7 +2256,7 @@ int lancer_cmd_get_pport_stats(struct be_adapter *adapter,
 int be_cmd_get_fw_ver(struct be_adapter *adapter);
 int be_cmd_modify_eqd(struct be_adapter *adapter, struct be_set_eqd *, int num);
 int be_cmd_vlan_config(struct be_adapter *adapter, u32 if_id, u16 *vtag_array,
-		       u32 num);
+		       u32 num, u32 domain);
 int be_cmd_rx_filter(struct be_adapter *adapter, u32 flags, u32 status);
 int be_cmd_set_flow_control(struct be_adapter *adapter, u32 tx_fc, u32 rx_fc);
 int be_cmd_get_flow_control(struct be_adapter *adapter, u32 *tx_fc, u32 *rx_fc);
diff --git a/drivers/net/ethernet/emulex/benet/be_main.c b/drivers/net/ethernet/emulex/benet/be_main.c
index 0a816859aca5..e6b790f0d9dc 100644
--- a/drivers/net/ethernet/emulex/benet/be_main.c
+++ b/drivers/net/ethernet/emulex/benet/be_main.c
@@ -1171,7 +1171,7 @@ static int be_vid_config(struct be_adapter *adapter)
 	for_each_set_bit(i, adapter->vids, VLAN_N_VID)
 		vids[num++] = cpu_to_le16(i);
 
-	status = be_cmd_vlan_config(adapter, adapter->if_handle, vids, num);
+	status = be_cmd_vlan_config(adapter, adapter->if_handle, vids, num, 0);
 	if (status) {
 		dev_err(dev, "Setting HW VLAN filtering failed\n");
 		/* Set to VLAN promisc mode as setting VLAN filter failed */
@@ -1380,11 +1380,67 @@ static int be_get_vf_config(struct net_device *netdev, int vf,
 	return 0;
 }
 
+static int be_set_vf_tvt(struct be_adapter *adapter, int vf, u16 vlan)
+{
+	struct be_vf_cfg *vf_cfg = &adapter->vf_cfg[vf];
+	u16 vids[BE_NUM_VLANS_SUPPORTED];
+	int vf_if_id = vf_cfg->if_handle;
+	int status;
+
+	/* Enable Transparent VLAN Tagging */
+	status = be_cmd_set_hsw_config(adapter, vlan, vf + 1, vf_if_id, 0);
+	if (status)
+		return status;
+
+	/* Clear pre-programmed VLAN filters on VF if any, if TVT is enabled */
+	vids[0] = 0;
+	status = be_cmd_vlan_config(adapter, vf_if_id, vids, 1, vf + 1);
+	if (!status)
+		dev_info(&adapter->pdev->dev,
+			 "Cleared guest VLANs on VF%d", vf);
+
+	/* After TVT is enabled, disallow VFs to program VLAN filters */
+	if (vf_cfg->privileges & BE_PRIV_FILTMGMT) {
+		status = be_cmd_set_fn_privileges(adapter, vf_cfg->privileges &
+						  ~BE_PRIV_FILTMGMT, vf + 1);
+		if (!status)
+			vf_cfg->privileges &= ~BE_PRIV_FILTMGMT;
+	}
+	return 0;
+}
+
+static int be_clear_vf_tvt(struct be_adapter *adapter, int vf)
+{
+	struct be_vf_cfg *vf_cfg = &adapter->vf_cfg[vf];
+	struct device *dev = &adapter->pdev->dev;
+	int status;
+
+	/* Reset Transparent VLAN Tagging. */
+	status = be_cmd_set_hsw_config(adapter, BE_RESET_VLAN_TAG_ID, vf + 1,
+				       vf_cfg->if_handle, 0);
+	if (status)
+		return status;
+
+	/* Allow VFs to program VLAN filtering */
+	if (!(vf_cfg->privileges & BE_PRIV_FILTMGMT)) {
+		status = be_cmd_set_fn_privileges(adapter, vf_cfg->privileges |
+						  BE_PRIV_FILTMGMT, vf + 1);
+		if (!status) {
+			vf_cfg->privileges |= BE_PRIV_FILTMGMT;
+			dev_info(dev, "VF%d: FILTMGMT priv enabled", vf);
+		}
+	}
+
+	dev_info(dev,
+		 "Disable/re-enable i/f in VM to clear Transparent VLAN tag");
+	return 0;
+}
+
 static int be_set_vf_vlan(struct net_device *netdev, int vf, u16 vlan, u8 qos)
 {
 	struct be_adapter *adapter = netdev_priv(netdev);
 	struct be_vf_cfg *vf_cfg = &adapter->vf_cfg[vf];
-	int status = 0;
+	int status;
 
 	if (!sriov_enabled(adapter))
 		return -EPERM;
@@ -1394,24 +1450,19 @@ static int be_set_vf_vlan(struct net_device *netdev, int vf, u16 vlan, u8 qos)
 
 	if (vlan || qos) {
 		vlan |= qos << VLAN_PRIO_SHIFT;
-		if (vf_cfg->vlan_tag != vlan)
-			status = be_cmd_set_hsw_config(adapter, vlan, vf + 1,
-						       vf_cfg->if_handle, 0);
+		status = be_set_vf_tvt(adapter, vf, vlan);
 	} else {
-		/* Reset Transparent Vlan Tagging. */
-		status = be_cmd_set_hsw_config(adapter, BE_RESET_VLAN_TAG_ID,
-					       vf + 1, vf_cfg->if_handle, 0);
+		status = be_clear_vf_tvt(adapter, vf);
 	}
 
 	if (status) {
 		dev_err(&adapter->pdev->dev,
-			"VLAN %d config on VF %d failed : %#x\n", vlan,
-			vf, status);
+			"VLAN %d config on VF %d failed : %#x\n", vlan, vf,
+			status);
 		return be_cmd_status(status);
 	}
 
 	vf_cfg->vlan_tag = vlan;
-
 	return 0;
 }
 
@@ -2772,14 +2823,12 @@ void be_detect_error(struct be_adapter *adapter)
 			}
 		}
 	} else {
-		pci_read_config_dword(adapter->pdev,
-				      PCICFG_UE_STATUS_LOW, &ue_lo);
-		pci_read_config_dword(adapter->pdev,
-				      PCICFG_UE_STATUS_HIGH, &ue_hi);
-		pci_read_config_dword(adapter->pdev,
-				      PCICFG_UE_STATUS_LOW_MASK, &ue_lo_mask);
-		pci_read_config_dword(adapter->pdev,
-				      PCICFG_UE_STATUS_HI_MASK, &ue_hi_mask);
+		ue_lo = ioread32(adapter->pcicfg + PCICFG_UE_STATUS_LOW);
+		ue_hi = ioread32(adapter->pcicfg + PCICFG_UE_STATUS_HIGH);
+		ue_lo_mask = ioread32(adapter->pcicfg +
+				      PCICFG_UE_STATUS_LOW_MASK);
+		ue_hi_mask = ioread32(adapter->pcicfg +
+				      PCICFG_UE_STATUS_HI_MASK);
 
 		ue_lo = (ue_lo & ~ue_lo_mask);
 		ue_hi = (ue_hi & ~ue_hi_mask);
@@ -3339,7 +3388,6 @@ static int be_if_create(struct be_adapter *adapter, u32 *if_handle,
 			u32 cap_flags, u32 vf)
 {
 	u32 en_flags;
-	int status;
 
 	en_flags = BE_IF_FLAGS_UNTAGGED | BE_IF_FLAGS_BROADCAST |
 		   BE_IF_FLAGS_MULTICAST | BE_IF_FLAGS_PASS_L3L4_ERRORS |
@@ -3347,10 +3395,7 @@ static int be_if_create(struct be_adapter *adapter, u32 *if_handle,
 
 	en_flags &= cap_flags;
 
-	status = be_cmd_if_create(adapter, cap_flags, en_flags,
-				  if_handle, vf);
-
-	return status;
+	return be_cmd_if_create(adapter, cap_flags, en_flags, if_handle, vf);
 }
 
 static int be_vfs_if_create(struct be_adapter *adapter)
@@ -3368,8 +3413,13 @@ static int be_vfs_if_create(struct be_adapter *adapter)
 		if (!BE3_chip(adapter)) {
 			status = be_cmd_get_profile_config(adapter, &res,
 							   vf + 1);
-			if (!status)
+			if (!status) {
 				cap_flags = res.if_cap_flags;
+				/* Prevent VFs from enabling VLAN promiscuous
+				 * mode
+				 */
+				cap_flags &= ~BE_IF_FLAGS_VLAN_PROMISCUOUS;
+			}
 		}
 
 		status = be_if_create(adapter, &vf_cfg->if_handle,
@@ -3403,7 +3453,6 @@ static int be_vf_setup(struct be_adapter *adapter)
 	struct device *dev = &adapter->pdev->dev;
 	struct be_vf_cfg *vf_cfg;
 	int status, old_vfs, vf;
-	u32 privileges;
 
 	old_vfs = pci_num_vf(adapter->pdev);
 
@@ -3433,15 +3482,18 @@ static int be_vf_setup(struct be_adapter *adapter)
 
 	for_all_vfs(adapter, vf_cfg, vf) {
 		/* Allow VFs to programs MAC/VLAN filters */
-		status = be_cmd_get_fn_privileges(adapter, &privileges, vf + 1);
-		if (!status && !(privileges & BE_PRIV_FILTMGMT)) {
+		status = be_cmd_get_fn_privileges(adapter, &vf_cfg->privileges,
+						  vf + 1);
+		if (!status && !(vf_cfg->privileges & BE_PRIV_FILTMGMT)) {
 			status = be_cmd_set_fn_privileges(adapter,
-							  privileges |
+							  vf_cfg->privileges |
 							  BE_PRIV_FILTMGMT,
 							  vf + 1);
-			if (!status)
+			if (!status) {
+				vf_cfg->privileges |= BE_PRIV_FILTMGMT;
 				dev_info(dev, "VF%d has FILTMGMT privilege\n",
 					 vf);
+			}
 		}
 
 		/* Allow full available bandwidth */
@@ -4820,24 +4872,37 @@ static int be_roce_map_pci_bars(struct be_adapter *adapter)
 
 static int be_map_pci_bars(struct be_adapter *adapter)
 {
+	struct pci_dev *pdev = adapter->pdev;
 	u8 __iomem *addr;
 
 	if (BEx_chip(adapter) && be_physfn(adapter)) {
-		adapter->csr = pci_iomap(adapter->pdev, 2, 0);
+		adapter->csr = pci_iomap(pdev, 2, 0);
 		if (!adapter->csr)
 			return -ENOMEM;
 	}
 
-	addr = pci_iomap(adapter->pdev, db_bar(adapter), 0);
+	addr = pci_iomap(pdev, db_bar(adapter), 0);
 	if (!addr)
 		goto pci_map_err;
 	adapter->db = addr;
 
+	if (skyhawk_chip(adapter) || BEx_chip(adapter)) {
+		if (be_physfn(adapter)) {
+			/* PCICFG is the 2nd BAR in BE2 */
+			addr = pci_iomap(pdev, BE2_chip(adapter) ? 1 : 0, 0);
+			if (!addr)
+				goto pci_map_err;
+			adapter->pcicfg = addr;
+		} else {
+			adapter->pcicfg = adapter->db + SRIOV_VF_PCICFG_OFFSET;
+		}
+	}
+
 	be_roce_map_pci_bars(adapter);
 	return 0;
 
 pci_map_err:
-	dev_err(&adapter->pdev->dev, "Error in mapping PCI BARs\n");
+	dev_err(&pdev->dev, "Error in mapping PCI BARs\n");
 	be_unmap_pci_bars(adapter);
 	return -ENOMEM;
 }
diff --git a/drivers/net/ethernet/freescale/fec_main.c b/drivers/net/ethernet/freescale/fec_main.c
index 9bb6220663b2..f6a3a7abd468 100644
--- a/drivers/net/ethernet/freescale/fec_main.c
+++ b/drivers/net/ethernet/freescale/fec_main.c
@@ -1189,13 +1189,12 @@ static void
 fec_enet_tx_queue(struct net_device *ndev, u16 queue_id)
 {
 	struct	fec_enet_private *fep;
-	struct bufdesc *bdp, *bdp_t;
+	struct bufdesc *bdp;
 	unsigned short status;
 	struct	sk_buff	*skb;
 	struct fec_enet_priv_tx_q *txq;
 	struct netdev_queue *nq;
 	int	index = 0;
-	int	i, bdnum;
 	int	entries_free;
 
 	fep = netdev_priv(ndev);
@@ -1216,29 +1215,18 @@ fec_enet_tx_queue(struct net_device *ndev, u16 queue_id)
 		if (bdp == txq->cur_tx)
 			break;
 
-		bdp_t = bdp;
-		bdnum = 1;
-		index = fec_enet_get_bd_index(txq->tx_bd_base, bdp_t, fep);
-		skb = txq->tx_skbuff[index];
-		while (!skb) {
-			bdp_t = fec_enet_get_nextdesc(bdp_t, fep, queue_id);
-			index = fec_enet_get_bd_index(txq->tx_bd_base, bdp_t, fep);
-			skb = txq->tx_skbuff[index];
-			bdnum++;
-		}
-		if (skb_shinfo(skb)->nr_frags &&
-		    (status = bdp_t->cbd_sc) & BD_ENET_TX_READY)
-			break;
+		index = fec_enet_get_bd_index(txq->tx_bd_base, bdp, fep);
 
-		for (i = 0; i < bdnum; i++) {
-			if (!IS_TSO_HEADER(txq, bdp->cbd_bufaddr))
-				dma_unmap_single(&fep->pdev->dev, bdp->cbd_bufaddr,
-						 bdp->cbd_datlen, DMA_TO_DEVICE);
-			bdp->cbd_bufaddr = 0;
-			if (i < bdnum - 1)
-				bdp = fec_enet_get_nextdesc(bdp, fep, queue_id);
-		}
+		skb = txq->tx_skbuff[index];
 		txq->tx_skbuff[index] = NULL;
+		if (!IS_TSO_HEADER(txq, bdp->cbd_bufaddr))
+			dma_unmap_single(&fep->pdev->dev, bdp->cbd_bufaddr,
+					bdp->cbd_datlen, DMA_TO_DEVICE);
+		bdp->cbd_bufaddr = 0;
+		if (!skb) {
+			bdp = fec_enet_get_nextdesc(bdp, fep, queue_id);
+			continue;
+		}
 
 		/* Check for errors. */
 		if (status & (BD_ENET_TX_HB | BD_ENET_TX_LC |
@@ -1479,8 +1467,7 @@ fec_enet_rx_queue(struct net_device *ndev, int budget, u16 queue_id)
 
 			vlan_packet_rcvd = true;
 
-			skb_copy_to_linear_data_offset(skb, VLAN_HLEN,
-						       data, (2 * ETH_ALEN));
+			memmove(skb->data + VLAN_HLEN, data, ETH_ALEN * 2);
 			skb_pull(skb, VLAN_HLEN);
 		}
 
@@ -1597,7 +1584,7 @@ fec_enet_interrupt(int irq, void *dev_id)
 	writel(int_events, fep->hwp + FEC_IEVENT);
 	fec_enet_collect_events(fep, int_events);
 
-	if (fep->work_tx || fep->work_rx) {
+	if ((fep->work_tx || fep->work_rx) && fep->link) {
 		ret = IRQ_HANDLED;
 
 		if (napi_schedule_prep(&fep->napi)) {
@@ -1967,6 +1954,7 @@ static int fec_enet_mii_init(struct platform_device *pdev)
 	struct fec_enet_private *fep = netdev_priv(ndev);
 	struct device_node *node;
 	int err = -ENXIO, i;
+	u32 mii_speed, holdtime;
 
 	/*
 	 * The i.MX28 dual fec interfaces are not equal.
@@ -2004,10 +1992,33 @@ static int fec_enet_mii_init(struct platform_device *pdev)
 	 * Reference Manual has an error on this, and gets fixed on i.MX6Q
 	 * document.
 	 */
-	fep->phy_speed = DIV_ROUND_UP(clk_get_rate(fep->clk_ipg), 5000000);
+	mii_speed = DIV_ROUND_UP(clk_get_rate(fep->clk_ipg), 5000000);
 	if (fep->quirks & FEC_QUIRK_ENET_MAC)
-		fep->phy_speed--;
-	fep->phy_speed <<= 1;
+		mii_speed--;
+	if (mii_speed > 63) {
+		dev_err(&pdev->dev,
+			"fec clock (%lu) to fast to get right mii speed\n",
+			clk_get_rate(fep->clk_ipg));
+		err = -EINVAL;
+		goto err_out;
+	}
+
+	/*
+	 * The i.MX28 and i.MX6 types have another filed in the MSCR (aka
+	 * MII_SPEED) register that defines the MDIO output hold time. Earlier
+	 * versions are RAZ there, so just ignore the difference and write the
+	 * register always.
+	 * The minimal hold time according to IEE802.3 (clause 22) is 10 ns.
+	 * HOLDTIME + 1 is the number of clk cycles the fec is holding the
+	 * output.
+	 * The HOLDTIME bitfield takes values between 0 and 7 (inclusive).
+	 * Given that ceil(clkrate / 5000000) <= 64, the calculation for
+	 * holdtime cannot result in a value greater than 3.
+	 */
+	holdtime = DIV_ROUND_UP(clk_get_rate(fep->clk_ipg), 100000000) - 1;
+
+	fep->phy_speed = mii_speed << 1 | holdtime << 8;
+
 	writel(fep->phy_speed, fep->hwp + FEC_MII_SPEED);
 
 	fep->mii_bus = mdiobus_alloc();
@@ -3383,7 +3394,6 @@ fec_drv_remove(struct platform_device *pdev)
 		regulator_disable(fep->reg_phy);
 	if (fep->ptp_clock)
 		ptp_clock_unregister(fep->ptp_clock);
-	fec_enet_clk_enable(ndev, false);
 	of_node_put(fep->phy_node);
 	free_netdev(ndev);
 
diff --git a/drivers/net/ethernet/freescale/gianfar.c b/drivers/net/ethernet/freescale/gianfar.c
index 43df78882e48..7bf3682cdf47 100644
--- a/drivers/net/ethernet/freescale/gianfar.c
+++ b/drivers/net/ethernet/freescale/gianfar.c
@@ -747,6 +747,18 @@ static int gfar_parse_group(struct device_node *np,
 	return 0;
 }
 
+static int gfar_of_group_count(struct device_node *np)
+{
+	struct device_node *child;
+	int num = 0;
+
+	for_each_available_child_of_node(np, child)
+		if (!of_node_cmp(child->name, "queue-group"))
+			num++;
+
+	return num;
+}
+
 static int gfar_of_init(struct platform_device *ofdev, struct net_device **pdev)
 {
 	const char *model;
@@ -784,7 +796,7 @@ static int gfar_of_init(struct platform_device *ofdev, struct net_device **pdev)
 		num_rx_qs = 1;
 	} else { /* MQ_MG_MODE */
 		/* get the actual number of supported groups */
-		unsigned int num_grps = of_get_available_child_count(np);
+		unsigned int num_grps = gfar_of_group_count(np);
 
 		if (num_grps == 0 || num_grps > MAXGROUPS) {
 			dev_err(&ofdev->dev, "Invalid # of int groups(%d)\n",
@@ -851,7 +863,10 @@ static int gfar_of_init(struct platform_device *ofdev, struct net_device **pdev)
 
 	/* Parse and initialize group specific information */
 	if (priv->mode == MQ_MG_MODE) {
-		for_each_child_of_node(np, child) {
+		for_each_available_child_of_node(np, child) {
+			if (of_node_cmp(child->name, "queue-group"))
+				continue;
+
 			err = gfar_parse_group(child, priv, model);
 			if (err)
 				goto err_grp_init;
@@ -3162,8 +3177,8 @@ static void adjust_link(struct net_device *dev)
 	struct phy_device *phydev = priv->phydev;
 
 	if (unlikely(phydev->link != priv->oldlink ||
-		     phydev->duplex != priv->oldduplex ||
-		     phydev->speed != priv->oldspeed))
+		     (phydev->link && (phydev->duplex != priv->oldduplex ||
+				       phydev->speed != priv->oldspeed))))
 		gfar_update_link_state(priv);
 }
 
diff --git a/drivers/net/ethernet/freescale/ucc_geth.c b/drivers/net/ethernet/freescale/ucc_geth.c
index 357e8b576905..56b774d3a13d 100644
--- a/drivers/net/ethernet/freescale/ucc_geth.c
+++ b/drivers/net/ethernet/freescale/ucc_geth.c
@@ -3893,6 +3893,9 @@ static int ucc_geth_probe(struct platform_device* ofdev)
 	ugeth->phy_interface = phy_interface;
 	ugeth->max_speed = max_speed;
 
+	/* Carrier starts down, phylib will bring it up */
+	netif_carrier_off(dev);
+
 	err = register_netdev(dev);
 	if (err) {
 		if (netif_msg_probe(ugeth))
diff --git a/drivers/net/ethernet/ibm/ehea/ehea_main.c b/drivers/net/ethernet/ibm/ehea/ehea_main.c
index e8a1adb7a962..c05e50759621 100644
--- a/drivers/net/ethernet/ibm/ehea/ehea_main.c
+++ b/drivers/net/ethernet/ibm/ehea/ehea_main.c
@@ -3262,6 +3262,139 @@ static void ehea_remove_device_sysfs(struct platform_device *dev)
 	device_remove_file(&dev->dev, &dev_attr_remove_port);
 }
 
+static int ehea_reboot_notifier(struct notifier_block *nb,
+				unsigned long action, void *unused)
+{
+	if (action == SYS_RESTART) {
+		pr_info("Reboot: freeing all eHEA resources\n");
+		ibmebus_unregister_driver(&ehea_driver);
+	}
+	return NOTIFY_DONE;
+}
+
+static struct notifier_block ehea_reboot_nb = {
+	.notifier_call = ehea_reboot_notifier,
+};
+
+static int ehea_mem_notifier(struct notifier_block *nb,
+			     unsigned long action, void *data)
+{
+	int ret = NOTIFY_BAD;
+	struct memory_notify *arg = data;
+
+	mutex_lock(&dlpar_mem_lock);
+
+	switch (action) {
+	case MEM_CANCEL_OFFLINE:
+		pr_info("memory offlining canceled");
+		/* Fall through: re-add canceled memory block */
+
+	case MEM_ONLINE:
+		pr_info("memory is going online");
+		set_bit(__EHEA_STOP_XFER, &ehea_driver_flags);
+		if (ehea_add_sect_bmap(arg->start_pfn, arg->nr_pages))
+			goto out_unlock;
+		ehea_rereg_mrs();
+		break;
+
+	case MEM_GOING_OFFLINE:
+		pr_info("memory is going offline");
+		set_bit(__EHEA_STOP_XFER, &ehea_driver_flags);
+		if (ehea_rem_sect_bmap(arg->start_pfn, arg->nr_pages))
+			goto out_unlock;
+		ehea_rereg_mrs();
+		break;
+
+	default:
+		break;
+	}
+
+	ehea_update_firmware_handles();
+	ret = NOTIFY_OK;
+
+out_unlock:
+	mutex_unlock(&dlpar_mem_lock);
+	return ret;
+}
+
+static struct notifier_block ehea_mem_nb = {
+	.notifier_call = ehea_mem_notifier,
+};
+
+static void ehea_crash_handler(void)
+{
+	int i;
+
+	if (ehea_fw_handles.arr)
+		for (i = 0; i < ehea_fw_handles.num_entries; i++)
+			ehea_h_free_resource(ehea_fw_handles.arr[i].adh,
+					     ehea_fw_handles.arr[i].fwh,
+					     FORCE_FREE);
+
+	if (ehea_bcmc_regs.arr)
+		for (i = 0; i < ehea_bcmc_regs.num_entries; i++)
+			ehea_h_reg_dereg_bcmc(ehea_bcmc_regs.arr[i].adh,
+					      ehea_bcmc_regs.arr[i].port_id,
+					      ehea_bcmc_regs.arr[i].reg_type,
+					      ehea_bcmc_regs.arr[i].macaddr,
+					      0, H_DEREG_BCMC);
+}
+
+static atomic_t ehea_memory_hooks_registered;
+
+/* Register memory hooks on probe of first adapter */
+static int ehea_register_memory_hooks(void)
+{
+	int ret = 0;
+
+	if (atomic_inc_and_test(&ehea_memory_hooks_registered))
+		return 0;
+
+	ret = ehea_create_busmap();
+	if (ret) {
+		pr_info("ehea_create_busmap failed\n");
+		goto out;
+	}
+
+	ret = register_reboot_notifier(&ehea_reboot_nb);
+	if (ret) {
+		pr_info("register_reboot_notifier failed\n");
+		goto out;
+	}
+
+	ret = register_memory_notifier(&ehea_mem_nb);
+	if (ret) {
+		pr_info("register_memory_notifier failed\n");
+		goto out2;
+	}
+
+	ret = crash_shutdown_register(ehea_crash_handler);
+	if (ret) {
+		pr_info("crash_shutdown_register failed\n");
+		goto out3;
+	}
+
+	return 0;
+
+out3:
+	unregister_memory_notifier(&ehea_mem_nb);
+out2:
+	unregister_reboot_notifier(&ehea_reboot_nb);
+out:
+	return ret;
+}
+
+static void ehea_unregister_memory_hooks(void)
+{
+	if (atomic_read(&ehea_memory_hooks_registered))
+		return;
+
+	unregister_reboot_notifier(&ehea_reboot_nb);
+	if (crash_shutdown_unregister(ehea_crash_handler))
+		pr_info("failed unregistering crash handler\n");
+	unregister_memory_notifier(&ehea_mem_nb);
+}
+
 static int ehea_probe_adapter(struct platform_device *dev)
 {
 	struct ehea_adapter *adapter;
@@ -3269,6 +3402,10 @@ static int ehea_probe_adapter(struct platform_device *dev)
 	int ret;
 	int i;
 
+	ret = ehea_register_memory_hooks();
+	if (ret)
+		return ret;
+
 	if (!dev || !dev->dev.of_node) {
 		pr_err("Invalid ibmebus device probed\n");
 		return -EINVAL;
@@ -3392,81 +3529,6 @@ static int ehea_remove(struct platform_device *dev)
 	return 0;
 }
 
-static void ehea_crash_handler(void)
-{
-	int i;
-
-	if (ehea_fw_handles.arr)
-		for (i = 0; i < ehea_fw_handles.num_entries; i++)
-			ehea_h_free_resource(ehea_fw_handles.arr[i].adh,
-					     ehea_fw_handles.arr[i].fwh,
-					     FORCE_FREE);
-
-	if (ehea_bcmc_regs.arr)
-		for (i = 0; i < ehea_bcmc_regs.num_entries; i++)
-			ehea_h_reg_dereg_bcmc(ehea_bcmc_regs.arr[i].adh,
-					      ehea_bcmc_regs.arr[i].port_id,
-					      ehea_bcmc_regs.arr[i].reg_type,
-					      ehea_bcmc_regs.arr[i].macaddr,
-					      0, H_DEREG_BCMC);
-}
-
-static int ehea_mem_notifier(struct notifier_block *nb,
-                             unsigned long action, void *data)
-{
-	int ret = NOTIFY_BAD;
-	struct memory_notify *arg = data;
-
-	mutex_lock(&dlpar_mem_lock);
-
-	switch (action) {
-	case MEM_CANCEL_OFFLINE:
-		pr_info("memory offlining canceled");
-		/* Readd canceled memory block */
-	case MEM_ONLINE:
-		pr_info("memory is going online");
-		set_bit(__EHEA_STOP_XFER, &ehea_driver_flags);
-		if (ehea_add_sect_bmap(arg->start_pfn, arg->nr_pages))
-			goto out_unlock;
-		ehea_rereg_mrs();
-		break;
-	case MEM_GOING_OFFLINE:
-		pr_info("memory is going offline");
-		set_bit(__EHEA_STOP_XFER, &ehea_driver_flags);
-		if (ehea_rem_sect_bmap(arg->start_pfn, arg->nr_pages))
-			goto out_unlock;
-		ehea_rereg_mrs();
-		break;
-	default:
-		break;
-	}
-
-	ehea_update_firmware_handles();
-	ret = NOTIFY_OK;
-
-out_unlock:
-	mutex_unlock(&dlpar_mem_lock);
-	return ret;
-}
-
-static struct notifier_block ehea_mem_nb = {
-	.notifier_call = ehea_mem_notifier,
-};
-
-static int ehea_reboot_notifier(struct notifier_block *nb,
-				unsigned long action, void *unused)
-{
-	if (action == SYS_RESTART) {
-		pr_info("Reboot: freeing all eHEA resources\n");
-		ibmebus_unregister_driver(&ehea_driver);
-	}
-	return NOTIFY_DONE;
-}
-
-static struct notifier_block ehea_reboot_nb = {
-	.notifier_call = ehea_reboot_notifier,
-};
-
 static int check_module_parm(void)
 {
 	int ret = 0;
@@ -3520,26 +3582,10 @@ static int __init ehea_module_init(void)
 	if (ret)
 		goto out;
 
-	ret = ehea_create_busmap();
-	if (ret)
-		goto out;
-
-	ret = register_reboot_notifier(&ehea_reboot_nb);
-	if (ret)
-		pr_info("failed registering reboot notifier\n");
-
-	ret = register_memory_notifier(&ehea_mem_nb);
-	if (ret)
-		pr_info("failed registering memory remove notifier\n");
-
-	ret = crash_shutdown_register(ehea_crash_handler);
-	if (ret)
-		pr_info("failed registering crash handler\n");
-
 	ret = ibmebus_register_driver(&ehea_driver);
 	if (ret) {
 		pr_err("failed registering eHEA device driver on ebus\n");
-		goto out2;
+		goto out;
 	}
 
 	ret = driver_create_file(&ehea_driver.driver,
@@ -3547,32 +3593,22 @@ static int __init ehea_module_init(void)
 	if (ret) {
 		pr_err("failed to register capabilities attribute, ret=%d\n",
 		       ret);
-		goto out3;
+		goto out2;
 	}
 
 	return ret;
 
-out3:
-	ibmebus_unregister_driver(&ehea_driver);
 out2:
-	unregister_memory_notifier(&ehea_mem_nb);
-	unregister_reboot_notifier(&ehea_reboot_nb);
-	crash_shutdown_unregister(ehea_crash_handler);
+	ibmebus_unregister_driver(&ehea_driver);
 out:
 	return ret;
 }
 
 static void __exit ehea_module_exit(void)
 {
-	int ret;
-
 	driver_remove_file(&ehea_driver.driver, &driver_attr_capabilities);
 	ibmebus_unregister_driver(&ehea_driver);
-	unregister_reboot_notifier(&ehea_reboot_nb);
-	ret = crash_shutdown_unregister(ehea_crash_handler);
-	if (ret)
-		pr_info("failed unregistering crash handler\n");
-	unregister_memory_notifier(&ehea_mem_nb);
+	ehea_unregister_memory_hooks();
 	kfree(ehea_fw_handles.arr);
 	kfree(ehea_bcmc_regs.arr);
 	ehea_destroy_busmap();
diff --git a/drivers/net/ethernet/ibm/ibmveth.c b/drivers/net/ethernet/ibm/ibmveth.c
index 21978cc019e7..cd7675ac5bf9 100644
--- a/drivers/net/ethernet/ibm/ibmveth.c
+++ b/drivers/net/ethernet/ibm/ibmveth.c
@@ -1136,6 +1136,8 @@ restart_poll:
 	ibmveth_replenish_task(adapter);
 
 	if (frames_processed < budget) {
+		napi_complete(napi);
+
 		/* We think we are done - reenable interrupts,
 		 * then check once more to make sure we are done.
 		 */
@@ -1144,8 +1146,6 @@ restart_poll:
 
 		BUG_ON(lpar_rc != H_SUCCESS);
 
-		napi_complete(napi);
-
 		if (ibmveth_rxq_pending_buffer(adapter) &&
 		    napi_reschedule(napi)) {
 			lpar_rc = h_vio_signal(adapter->vdev->unit_address,
@@ -1327,6 +1327,28 @@ static unsigned long ibmveth_get_desired_dma(struct vio_dev *vdev)
 	return ret;
 }
 
+static int ibmveth_set_mac_addr(struct net_device *dev, void *p)
+{
+	struct ibmveth_adapter *adapter = netdev_priv(dev);
+	struct sockaddr *addr = p;
+	u64 mac_address;
+	int rc;
+
+	if (!is_valid_ether_addr(addr->sa_data))
+		return -EADDRNOTAVAIL;
+
+	mac_address = ibmveth_encode_mac_addr(addr->sa_data);
+	rc = h_change_logical_lan_mac(adapter->vdev->unit_address, mac_address);
+	if (rc) {
+		netdev_err(adapter->netdev, "h_change_logical_lan_mac failed with rc=%d\n", rc);
+		return rc;
+	}
+
+	ether_addr_copy(dev->dev_addr, addr->sa_data);
+
+	return 0;
+}
+
 static const struct net_device_ops ibmveth_netdev_ops = {
 	.ndo_open		= ibmveth_open,
 	.ndo_stop		= ibmveth_close,
@@ -1337,7 +1359,7 @@ static const struct net_device_ops ibmveth_netdev_ops = {
 	.ndo_fix_features	= ibmveth_fix_features,
 	.ndo_set_features	= ibmveth_set_features,
 	.ndo_validate_addr	= eth_validate_addr,
-	.ndo_set_mac_address	= eth_mac_addr,
+	.ndo_set_mac_address    = ibmveth_set_mac_addr,
 #ifdef CONFIG_NET_POLL_CONTROLLER
 	.ndo_poll_controller	= ibmveth_poll_controller,
 #endif
diff --git a/drivers/net/ethernet/intel/i40e/i40e_common.c b/drivers/net/ethernet/intel/i40e/i40e_common.c
index 11a9ffebf8d8..6aea65dae5ed 100644
--- a/drivers/net/ethernet/intel/i40e/i40e_common.c
+++ b/drivers/net/ethernet/intel/i40e/i40e_common.c
@@ -868,8 +868,9 @@ i40e_status i40e_pf_reset(struct i40e_hw *hw)
 	 * The grst delay value is in 100ms units, and we'll wait a
 	 * couple counts longer to be sure we don't just miss the end.
 	 */
-	grst_del = rd32(hw, I40E_GLGEN_RSTCTL) & I40E_GLGEN_RSTCTL_GRSTDEL_MASK
-			>> I40E_GLGEN_RSTCTL_GRSTDEL_SHIFT;
+	grst_del = (rd32(hw, I40E_GLGEN_RSTCTL) &
+		    I40E_GLGEN_RSTCTL_GRSTDEL_MASK) >>
+		    I40E_GLGEN_RSTCTL_GRSTDEL_SHIFT;
 	for (cnt = 0; cnt < grst_del + 2; cnt++) {
 		reg = rd32(hw, I40E_GLGEN_RSTAT);
 		if (!(reg & I40E_GLGEN_RSTAT_DEVSTATE_MASK))
@@ -2846,7 +2847,7 @@ i40e_status i40e_aq_add_udp_tunnel(struct i40e_hw *hw,
 
 	status = i40e_asq_send_command(hw, &desc, NULL, 0, cmd_details);
 
-	if (!status)
+	if (!status && filter_index)
 		*filter_index = resp->index;
 
 	return status;
diff --git a/drivers/net/ethernet/intel/i40e/i40e_dcb_nl.c b/drivers/net/ethernet/intel/i40e/i40e_dcb_nl.c
index 183dcb63ce98..a11c70ca5a28 100644
--- a/drivers/net/ethernet/intel/i40e/i40e_dcb_nl.c
+++ b/drivers/net/ethernet/intel/i40e/i40e_dcb_nl.c
@@ -40,7 +40,7 @@ static void i40e_get_pfc_delay(struct i40e_hw *hw, u16 *delay)
 	u32 val;
 
 	val = rd32(hw, I40E_PRTDCB_GENC);
-	*delay = (u16)(val & I40E_PRTDCB_GENC_PFCLDA_MASK >>
+	*delay = (u16)((val & I40E_PRTDCB_GENC_PFCLDA_MASK) >>
 		       I40E_PRTDCB_GENC_PFCLDA_SHIFT);
 }
 
diff --git a/drivers/net/ethernet/intel/i40e/i40e_debugfs.c b/drivers/net/ethernet/intel/i40e/i40e_debugfs.c
index 61236f983971..c17ee77100d3 100644
--- a/drivers/net/ethernet/intel/i40e/i40e_debugfs.c
+++ b/drivers/net/ethernet/intel/i40e/i40e_debugfs.c
@@ -989,8 +989,10 @@ static ssize_t i40e_dbg_command_write(struct file *filp,
 	if (!cmd_buf)
 		return count;
 	bytes_not_copied = copy_from_user(cmd_buf, buffer, count);
-	if (bytes_not_copied < 0)
+	if (bytes_not_copied < 0) {
+		kfree(cmd_buf);
 		return bytes_not_copied;
+	}
 	if (bytes_not_copied > 0)
 		count -= bytes_not_copied;
 	cmd_buf[count] = '\0';
diff --git a/drivers/net/ethernet/intel/i40e/i40e_main.c b/drivers/net/ethernet/intel/i40e/i40e_main.c
index cbe281be1c9f..dadda3c5d658 100644
--- a/drivers/net/ethernet/intel/i40e/i40e_main.c
+++ b/drivers/net/ethernet/intel/i40e/i40e_main.c
@@ -1512,7 +1512,12 @@ static void i40e_vsi_setup_queue_map(struct i40e_vsi *vsi,
 	vsi->tc_config.numtc = numtc;
 	vsi->tc_config.enabled_tc = enabled_tc ? enabled_tc : 1;
 	/* Number of queues per enabled TC */
-	num_tc_qps = vsi->alloc_queue_pairs/numtc;
+	/* In MFP case we can have a much lower count of MSIx
+	 * vectors available and so we need to lower the used
+	 * q count.
+	 */
+	qcount = min_t(int, vsi->alloc_queue_pairs, pf->num_lan_msix);
+	num_tc_qps = qcount / numtc;
 	num_tc_qps = min_t(int, num_tc_qps, I40E_MAX_QUEUES_PER_TC);
 
 	/* Setup queue offset/count for all TCs for given VSI */
@@ -2684,8 +2689,15 @@ static void i40e_vsi_config_dcb_rings(struct i40e_vsi *vsi)
 	u16 qoffset, qcount;
 	int i, n;
 
-	if (!(vsi->back->flags & I40E_FLAG_DCB_ENABLED))
-		return;
+	if (!(vsi->back->flags & I40E_FLAG_DCB_ENABLED)) {
+		/* Reset the TC information */
+		for (i = 0; i < vsi->num_queue_pairs; i++) {
+			rx_ring = vsi->rx_rings[i];
+			tx_ring = vsi->tx_rings[i];
+			rx_ring->dcb_tc = 0;
+			tx_ring->dcb_tc = 0;
+		}
+	}
 
 	for (n = 0; n < I40E_MAX_TRAFFIC_CLASS; n++) {
 		if (!(vsi->tc_config.enabled_tc & (1 << n)))
@@ -3830,6 +3842,12 @@ static void i40e_clear_interrupt_scheme(struct i40e_pf *pf)
 {
 	int i;
 
+	i40e_stop_misc_vector(pf);
+	if (pf->flags & I40E_FLAG_MSIX_ENABLED) {
+		synchronize_irq(pf->msix_entries[0].vector);
+		free_irq(pf->msix_entries[0].vector, pf);
+	}
+
 	i40e_put_lump(pf->irq_pile, 0, I40E_PILE_VALID_BIT-1);
 	for (i = 0; i < pf->num_alloc_vsi; i++)
 		if (pf->vsi[i])
@@ -5254,8 +5272,14 @@ static int i40e_handle_lldp_event(struct i40e_pf *pf,
 
 	/* Wait for the PF's Tx queues to be disabled */
 	ret = i40e_pf_wait_txq_disabled(pf);
-	if (!ret)
+	if (ret) {
+		/* Schedule PF reset to recover */
+		set_bit(__I40E_PF_RESET_REQUESTED, &pf->state);
+		i40e_service_event_schedule(pf);
+	} else {
 		i40e_pf_unquiesce_all_vsi(pf);
+	}
+
 exit:
 	return ret;
 }
@@ -5587,7 +5611,8 @@ static void i40e_check_hang_subtask(struct i40e_pf *pf)
 	int i, v;
 
 	/* If we're down or resetting, just bail */
-	if (test_bit(__I40E_CONFIG_BUSY, &pf->state))
+	if (test_bit(__I40E_DOWN, &pf->state) ||
+	    test_bit(__I40E_CONFIG_BUSY, &pf->state))
 		return;
 
 	/* for each VSI/netdev
@@ -9533,6 +9558,7 @@ static void i40e_remove(struct pci_dev *pdev)
 	set_bit(__I40E_DOWN, &pf->state);
 	del_timer_sync(&pf->service_timer);
 	cancel_work_sync(&pf->service_task);
+	i40e_fdir_teardown(pf);
 
 	if (pf->flags & I40E_FLAG_SRIOV_ENABLED) {
 		i40e_free_vfs(pf);
@@ -9559,12 +9585,6 @@ static void i40e_remove(struct pci_dev *pdev)
 	if (pf->vsi[pf->lan_vsi])
 		i40e_vsi_release(pf->vsi[pf->lan_vsi]);
 
-	i40e_stop_misc_vector(pf);
-	if (pf->flags & I40E_FLAG_MSIX_ENABLED) {
-		synchronize_irq(pf->msix_entries[0].vector);
-		free_irq(pf->msix_entries[0].vector, pf);
-	}
-
 	/* shutdown and destroy the HMC */
 	if (pf->hw.hmc.hmc_obj) {
 		ret_code = i40e_shutdown_lan_hmc(&pf->hw);
@@ -9718,6 +9738,8 @@ static void i40e_shutdown(struct pci_dev *pdev)
 	wr32(hw, I40E_PFPM_APM, (pf->wol_en ? I40E_PFPM_APM_APME_MASK : 0));
 	wr32(hw, I40E_PFPM_WUFC, (pf->wol_en ? I40E_PFPM_WUFC_MAG_MASK : 0));
 
+	i40e_clear_interrupt_scheme(pf);
+
 	if (system_state == SYSTEM_POWER_OFF) {
 		pci_wake_from_d3(pdev, pf->wol_en);
 		pci_set_power_state(pdev, PCI_D3hot);
diff --git a/drivers/net/ethernet/intel/i40e/i40e_nvm.c b/drivers/net/ethernet/intel/i40e/i40e_nvm.c
index 3e70f2e45a47..5defe0d63514 100644
--- a/drivers/net/ethernet/intel/i40e/i40e_nvm.c
+++ b/drivers/net/ethernet/intel/i40e/i40e_nvm.c
@@ -679,9 +679,11 @@ static i40e_status i40e_nvmupd_state_writing(struct i40e_hw *hw,
 {
 	i40e_status status;
 	enum i40e_nvmupd_cmd upd_cmd;
+	bool retry_attempt = false;
 
 	upd_cmd = i40e_nvmupd_validate_command(hw, cmd, errno);
 
+retry:
 	switch (upd_cmd) {
 	case I40E_NVMUPD_WRITE_CON:
 		status = i40e_nvmupd_nvm_write(hw, cmd, bytes, errno);
@@ -725,6 +727,39 @@ static i40e_status i40e_nvmupd_state_writing(struct i40e_hw *hw,
 		*errno = -ESRCH;
 		break;
 	}
+
+	/* In some circumstances, a multi-write transaction takes longer
+	 * than the default 3 minute timeout on the write semaphore.  If
+	 * the write failed with an EBUSY status, this is likely the problem,
+	 * so here we try to reacquire the semaphore then retry the write.
+	 * We only do one retry, then give up.
+	 */
+	if (status && (hw->aq.asq_last_status == I40E_AQ_RC_EBUSY) &&
+	    !retry_attempt) {
+		i40e_status old_status = status;
+		u32 old_asq_status = hw->aq.asq_last_status;
+		u32 gtime;
+
+		gtime = rd32(hw, I40E_GLVFGEN_TIMER);
+		if (gtime >= hw->nvm.hw_semaphore_timeout) {
+			i40e_debug(hw, I40E_DEBUG_ALL,
+				   "NVMUPD: write semaphore expired (%d >= %lld), retrying\n",
+				   gtime, hw->nvm.hw_semaphore_timeout);
+			i40e_release_nvm(hw);
+			status = i40e_acquire_nvm(hw, I40E_RESOURCE_WRITE);
+			if (status) {
+				i40e_debug(hw, I40E_DEBUG_ALL,
+					   "NVMUPD: write semaphore reacquire failed aq_err = %d\n",
+					   hw->aq.asq_last_status);
+				status = old_status;
+				hw->aq.asq_last_status = old_asq_status;
+			} else {
+				retry_attempt = true;
+				goto retry;
+			}
+		}
+	}
+
 	return status;
 }
 
diff --git a/drivers/net/ethernet/intel/i40e/i40e_txrx.c b/drivers/net/ethernet/intel/i40e/i40e_txrx.c
index 2206d2d36f0f..bbf1b1247ac4 100644
--- a/drivers/net/ethernet/intel/i40e/i40e_txrx.c
+++ b/drivers/net/ethernet/intel/i40e/i40e_txrx.c
@@ -586,6 +586,20 @@ void i40e_free_tx_resources(struct i40e_ring *tx_ring)
 }
 
 /**
+ * i40e_get_head - Retrieve head from head writeback
+ * @tx_ring:  tx ring to fetch head of
+ *
+ * Returns value of Tx ring head based on value stored
+ * in head write-back location
+ **/
+static inline u32 i40e_get_head(struct i40e_ring *tx_ring)
+{
+	void *head = (struct i40e_tx_desc *)tx_ring->desc + tx_ring->count;
+
+	return le32_to_cpu(*(volatile __le32 *)head);
+}
+
+/**
  * i40e_get_tx_pending - how many tx descriptors not processed
  * @tx_ring: the ring of descriptors
  *
@@ -594,10 +608,16 @@ void i40e_free_tx_resources(struct i40e_ring *tx_ring)
  **/
 static u32 i40e_get_tx_pending(struct i40e_ring *ring)
 {
-	u32 ntu = ((ring->next_to_clean <= ring->next_to_use)
-			? ring->next_to_use
-			: ring->next_to_use + ring->count);
-	return ntu - ring->next_to_clean;
+	u32 head, tail;
+
+	head = i40e_get_head(ring);
+	tail = readl(ring->tail);
+
+	if (head != tail)
+		return (head < tail) ?
+			tail - head : (tail + ring->count - head);
+
+	return 0;
 }
 
 /**
@@ -606,6 +626,8 @@ static u32 i40e_get_tx_pending(struct i40e_ring *ring)
  **/
 static bool i40e_check_tx_hang(struct i40e_ring *tx_ring)
 {
+	u32 tx_done = tx_ring->stats.packets;
+	u32 tx_done_old = tx_ring->tx_stats.tx_done_old;
 	u32 tx_pending = i40e_get_tx_pending(tx_ring);
 	struct i40e_pf *pf = tx_ring->vsi->back;
 	bool ret = false;
@@ -623,41 +645,25 @@ static bool i40e_check_tx_hang(struct i40e_ring *tx_ring)
 	 * run the check_tx_hang logic with a transmit completion
 	 * pending but without time to complete it yet.
 	 */
-	if ((tx_ring->tx_stats.tx_done_old == tx_ring->stats.packets) &&
-	    (tx_pending >= I40E_MIN_DESC_PENDING)) {
+	if ((tx_done_old == tx_done) && tx_pending) {
 		/* make sure it is true for two checks in a row */
 		ret = test_and_set_bit(__I40E_HANG_CHECK_ARMED,
 				       &tx_ring->state);
-	} else if ((tx_ring->tx_stats.tx_done_old == tx_ring->stats.packets) &&
-		   (tx_pending < I40E_MIN_DESC_PENDING) &&
-		   (tx_pending > 0)) {
+	} else if (tx_done_old == tx_done &&
+		   (tx_pending < I40E_MIN_DESC_PENDING) && (tx_pending > 0)) {
 		if (I40E_DEBUG_FLOW & pf->hw.debug_mask)
 			dev_info(tx_ring->dev, "HW needs some more descs to do a cacheline flush. tx_pending %d, queue %d",
 				 tx_pending, tx_ring->queue_index);
 		pf->tx_sluggish_count++;
 	} else {
 		/* update completed stats and disarm the hang check */
-		tx_ring->tx_stats.tx_done_old = tx_ring->stats.packets;
+		tx_ring->tx_stats.tx_done_old = tx_done;
 		clear_bit(__I40E_HANG_CHECK_ARMED, &tx_ring->state);
 	}
 
 	return ret;
 }
 
-/**
- * i40e_get_head - Retrieve head from head writeback
- * @tx_ring:  tx ring to fetch head of
- *
- * Returns value of Tx ring head based on value stored
- * in head write-back location
- **/
-static inline u32 i40e_get_head(struct i40e_ring *tx_ring)
-{
-	void *head = (struct i40e_tx_desc *)tx_ring->desc + tx_ring->count;
-
-	return le32_to_cpu(*(volatile __le32 *)head);
-}
-
 #define WB_STRIDE 0x3
 
 /**
@@ -2140,6 +2146,67 @@ static int i40e_maybe_stop_tx(struct i40e_ring *tx_ring, int size)
 }
 
 /**
+ * i40e_chk_linearize - Check if there are more than 8 fragments per packet
+ * @skb:      send buffer
+ * @tx_flags: collected send information
+ * @hdr_len:  size of the packet header
+ *
+ * Note: Our HW can't scatter-gather more than 8 fragments to build
+ * a packet on the wire and so we need to figure out the cases where we
+ * need to linearize the skb.
+ **/
+static bool i40e_chk_linearize(struct sk_buff *skb, u32 tx_flags,
+			       const u8 hdr_len)
+{
+	struct skb_frag_struct *frag;
+	bool linearize = false;
+	unsigned int size = 0;
+	u16 num_frags;
+	u16 gso_segs;
+
+	num_frags = skb_shinfo(skb)->nr_frags;
+	gso_segs = skb_shinfo(skb)->gso_segs;
+
+	if (tx_flags & (I40E_TX_FLAGS_TSO | I40E_TX_FLAGS_FSO)) {
+		u16 j = 1;
+
+		if (num_frags < (I40E_MAX_BUFFER_TXD))
+			goto linearize_chk_done;
+		/* try the simple math, if we have too many frags per segment */
+		if (DIV_ROUND_UP((num_frags + gso_segs), gso_segs) >
+		    I40E_MAX_BUFFER_TXD) {
+			linearize = true;
+			goto linearize_chk_done;
+		}
+		frag = &skb_shinfo(skb)->frags[0];
+		size = hdr_len;
+		/* we might still have more fragments per segment */
+		do {
+			size += skb_frag_size(frag);
+			frag++; j++;
+			if (j == I40E_MAX_BUFFER_TXD) {
+				if (size < skb_shinfo(skb)->gso_size) {
+					linearize = true;
+					break;
+				}
+				j = 1;
+				size -= skb_shinfo(skb)->gso_size;
+				if (size)
+					j++;
+				size += hdr_len;
+			}
+			num_frags--;
+		} while (num_frags);
+	} else {
+		if (num_frags >= I40E_MAX_BUFFER_TXD)
+			linearize = true;
+	}
+
+linearize_chk_done:
+	return linearize;
+}
+
+/**
  * i40e_tx_map - Build the Tx descriptor
  * @tx_ring:  ring to send buffer on
  * @skb:      send buffer
@@ -2396,6 +2463,10 @@ static netdev_tx_t i40e_xmit_frame_ring(struct sk_buff *skb,
 	if (tsyn)
 		tx_flags |= I40E_TX_FLAGS_TSYN;
 
+	if (i40e_chk_linearize(skb, tx_flags, hdr_len))
+		if (skb_linearize(skb))
+			goto out_drop;
+
 	skb_tx_timestamp(skb);
 
 	/* always enable CRC insertion offload */
diff --git a/drivers/net/ethernet/intel/i40e/i40e_txrx.h b/drivers/net/ethernet/intel/i40e/i40e_txrx.h
index 18b00231d2f1..dff0baeb1ecc 100644
--- a/drivers/net/ethernet/intel/i40e/i40e_txrx.h
+++ b/drivers/net/ethernet/intel/i40e/i40e_txrx.h
@@ -112,6 +112,7 @@ enum i40e_dyn_idx_t {
 
 #define i40e_rx_desc i40e_32byte_rx_desc
 
+#define I40E_MAX_BUFFER_TXD	8
 #define I40E_MIN_TX_LEN		17
 #define I40E_MAX_DATA_PER_TXD	8192
 
diff --git a/drivers/net/ethernet/intel/i40evf/i40e_txrx.c b/drivers/net/ethernet/intel/i40evf/i40e_txrx.c
index 29004382f462..708891571dae 100644
--- a/drivers/net/ethernet/intel/i40evf/i40e_txrx.c
+++ b/drivers/net/ethernet/intel/i40evf/i40e_txrx.c
@@ -126,6 +126,20 @@ void i40evf_free_tx_resources(struct i40e_ring *tx_ring)
 }
 
 /**
+ * i40e_get_head - Retrieve head from head writeback
+ * @tx_ring:  tx ring to fetch head of
+ *
+ * Returns value of Tx ring head based on value stored
+ * in head write-back location
+ **/
+static inline u32 i40e_get_head(struct i40e_ring *tx_ring)
+{
+	void *head = (struct i40e_tx_desc *)tx_ring->desc + tx_ring->count;
+
+	return le32_to_cpu(*(volatile __le32 *)head);
+}
+
+/**
  * i40e_get_tx_pending - how many tx descriptors not processed
  * @tx_ring: the ring of descriptors
  *
@@ -134,10 +148,16 @@ void i40evf_free_tx_resources(struct i40e_ring *tx_ring)
  **/
 static u32 i40e_get_tx_pending(struct i40e_ring *ring)
 {
-	u32 ntu = ((ring->next_to_clean <= ring->next_to_use)
-			? ring->next_to_use
-			: ring->next_to_use + ring->count);
-	return ntu - ring->next_to_clean;
+	u32 head, tail;
+
+	head = i40e_get_head(ring);
+	tail = readl(ring->tail);
+
+	if (head != tail)
+		return (head < tail) ?
+			tail - head : (tail + ring->count - head);
+
+	return 0;
 }
 
 /**
@@ -146,6 +166,8 @@ static u32 i40e_get_tx_pending(struct i40e_ring *ring)
  **/
 static bool i40e_check_tx_hang(struct i40e_ring *tx_ring)
 {
+	u32 tx_done = tx_ring->stats.packets;
+	u32 tx_done_old = tx_ring->tx_stats.tx_done_old;
 	u32 tx_pending = i40e_get_tx_pending(tx_ring);
 	bool ret = false;
 
@@ -162,36 +184,20 @@ static bool i40e_check_tx_hang(struct i40e_ring *tx_ring)
 	 * run the check_tx_hang logic with a transmit completion
 	 * pending but without time to complete it yet.
 	 */
-	if ((tx_ring->tx_stats.tx_done_old == tx_ring->stats.packets) &&
-	    (tx_pending >= I40E_MIN_DESC_PENDING)) {
+	if ((tx_done_old == tx_done) && tx_pending) {
 		/* make sure it is true for two checks in a row */
 		ret = test_and_set_bit(__I40E_HANG_CHECK_ARMED,
 				       &tx_ring->state);
-	} else if (!(tx_ring->tx_stats.tx_done_old == tx_ring->stats.packets) ||
-		   !(tx_pending < I40E_MIN_DESC_PENDING) ||
-		   !(tx_pending > 0)) {
+	} else if (tx_done_old == tx_done &&
+		   (tx_pending < I40E_MIN_DESC_PENDING) && (tx_pending > 0)) {
 		/* update completed stats and disarm the hang check */
-		tx_ring->tx_stats.tx_done_old = tx_ring->stats.packets;
+		tx_ring->tx_stats.tx_done_old = tx_done;
 		clear_bit(__I40E_HANG_CHECK_ARMED, &tx_ring->state);
 	}
 
 	return ret;
 }
 
-/**
- * i40e_get_head - Retrieve head from head writeback
- * @tx_ring:  tx ring to fetch head of
- *
- * Returns value of Tx ring head based on value stored
- * in head write-back location
- **/
-static inline u32 i40e_get_head(struct i40e_ring *tx_ring)
-{
-	void *head = (struct i40e_tx_desc *)tx_ring->desc + tx_ring->count;
-
-	return le32_to_cpu(*(volatile __le32 *)head);
-}
-
 #define WB_STRIDE 0x3
 
 /**
@@ -1206,17 +1212,16 @@ static int i40e_tso(struct i40e_ring *tx_ring, struct sk_buff *skb,
 	if (err < 0)
 		return err;
 
-	if (protocol == htons(ETH_P_IP)) {
-		iph = skb->encapsulation ? inner_ip_hdr(skb) : ip_hdr(skb);
+	iph = skb->encapsulation ? inner_ip_hdr(skb) : ip_hdr(skb);
+	ipv6h = skb->encapsulation ? inner_ipv6_hdr(skb) : ipv6_hdr(skb);
+
+	if (iph->version == 4) {
 		tcph = skb->encapsulation ? inner_tcp_hdr(skb) : tcp_hdr(skb);
 		iph->tot_len = 0;
 		iph->check = 0;
 		tcph->check = ~csum_tcpudp_magic(iph->saddr, iph->daddr,
 						 0, IPPROTO_TCP, 0);
-	} else if (skb_is_gso_v6(skb)) {
-
-		ipv6h = skb->encapsulation ? inner_ipv6_hdr(skb)
-					   : ipv6_hdr(skb);
+	} else if (ipv6h->version == 6) {
 		tcph = skb->encapsulation ? inner_tcp_hdr(skb) : tcp_hdr(skb);
 		ipv6h->payload_len = 0;
 		tcph->check = ~csum_ipv6_magic(&ipv6h->saddr, &ipv6h->daddr,
@@ -1274,13 +1279,9 @@ static void i40e_tx_enable_csum(struct sk_buff *skb, u32 tx_flags,
 					 I40E_TX_CTX_EXT_IP_IPV4_NO_CSUM;
 			}
 		} else if (tx_flags & I40E_TX_FLAGS_IPV6) {
-			if (tx_flags & I40E_TX_FLAGS_TSO) {
-				*cd_tunneling |= I40E_TX_CTX_EXT_IP_IPV6;
+			*cd_tunneling |= I40E_TX_CTX_EXT_IP_IPV6;
+			if (tx_flags & I40E_TX_FLAGS_TSO)
 				ip_hdr(skb)->check = 0;
-			} else {
-				*cd_tunneling |=
-					 I40E_TX_CTX_EXT_IP_IPV4_NO_CSUM;
-			}
 		}
 
 		/* Now set the ctx descriptor fields */
@@ -1290,6 +1291,11 @@ static void i40e_tx_enable_csum(struct sk_buff *skb, u32 tx_flags,
 				   ((skb_inner_network_offset(skb) -
 					skb_transport_offset(skb)) >> 1) <<
 				   I40E_TXD_CTX_QW0_NATLEN_SHIFT;
+		if (this_ip_hdr->version == 6) {
+			tx_flags &= ~I40E_TX_FLAGS_IPV4;
+			tx_flags |= I40E_TX_FLAGS_IPV6;
+		}
+
 
 	} else {
 		network_hdr_len = skb_network_header_len(skb);
@@ -1380,6 +1386,67 @@ static void i40e_create_tx_ctx(struct i40e_ring *tx_ring,
 	context_desc->type_cmd_tso_mss = cpu_to_le64(cd_type_cmd_tso_mss);
 }
 
+ /**
+ * i40e_chk_linearize - Check if there are more than 8 fragments per packet
+ * @skb:      send buffer
+ * @tx_flags: collected send information
+ * @hdr_len:  size of the packet header
+ *
+ * Note: Our HW can't scatter-gather more than 8 fragments to build
+ * a packet on the wire and so we need to figure out the cases where we
+ * need to linearize the skb.
+ **/
+static bool i40e_chk_linearize(struct sk_buff *skb, u32 tx_flags,
+			       const u8 hdr_len)
+{
+	struct skb_frag_struct *frag;
+	bool linearize = false;
+	unsigned int size = 0;
+	u16 num_frags;
+	u16 gso_segs;
+
+	num_frags = skb_shinfo(skb)->nr_frags;
+	gso_segs = skb_shinfo(skb)->gso_segs;
+
+	if (tx_flags & (I40E_TX_FLAGS_TSO | I40E_TX_FLAGS_FSO)) {
+		u16 j = 1;
+
+		if (num_frags < (I40E_MAX_BUFFER_TXD))
+			goto linearize_chk_done;
+		/* try the simple math, if we have too many frags per segment */
+		if (DIV_ROUND_UP((num_frags + gso_segs), gso_segs) >
+		    I40E_MAX_BUFFER_TXD) {
+			linearize = true;
+			goto linearize_chk_done;
+		}
+		frag = &skb_shinfo(skb)->frags[0];
+		size = hdr_len;
+		/* we might still have more fragments per segment */
+		do {
+			size += skb_frag_size(frag);
+			frag++; j++;
+			if (j == I40E_MAX_BUFFER_TXD) {
+				if (size < skb_shinfo(skb)->gso_size) {
+					linearize = true;
+					break;
+				}
+				j = 1;
+				size -= skb_shinfo(skb)->gso_size;
+				if (size)
+					j++;
+				size += hdr_len;
+			}
+			num_frags--;
+		} while (num_frags);
+	} else {
+		if (num_frags >= I40E_MAX_BUFFER_TXD)
+			linearize = true;
+	}
+
+linearize_chk_done:
+	return linearize;
+}
+
 /**
  * i40e_tx_map - Build the Tx descriptor
  * @tx_ring:  ring to send buffer on
@@ -1654,6 +1721,10 @@ static netdev_tx_t i40e_xmit_frame_ring(struct sk_buff *skb,
 	else if (tso)
 		tx_flags |= I40E_TX_FLAGS_TSO;
 
+	if (i40e_chk_linearize(skb, tx_flags, hdr_len))
+		if (skb_linearize(skb))
+			goto out_drop;
+
 	skb_tx_timestamp(skb);
 
 	/* always enable CRC insertion offload */
diff --git a/drivers/net/ethernet/intel/i40evf/i40e_txrx.h b/drivers/net/ethernet/intel/i40evf/i40e_txrx.h
index 4e15903b2b6d..c950a038237c 100644
--- a/drivers/net/ethernet/intel/i40evf/i40e_txrx.h
+++ b/drivers/net/ethernet/intel/i40evf/i40e_txrx.h
@@ -112,6 +112,7 @@ enum i40e_dyn_idx_t {
 
 #define i40e_rx_desc i40e_32byte_rx_desc
 
+#define I40E_MAX_BUFFER_TXD	8
 #define I40E_MIN_TX_LEN		17
 #define I40E_MAX_DATA_PER_TXD	8192
 
diff --git a/drivers/net/ethernet/marvell/mvneta.c b/drivers/net/ethernet/marvell/mvneta.c
index 96208f17bb53..2db653225a0e 100644
--- a/drivers/net/ethernet/marvell/mvneta.c
+++ b/drivers/net/ethernet/marvell/mvneta.c
@@ -2658,16 +2658,11 @@ static int mvneta_stop(struct net_device *dev)
 static int mvneta_ioctl(struct net_device *dev, struct ifreq *ifr, int cmd)
 {
 	struct mvneta_port *pp = netdev_priv(dev);
-	int ret;
 
 	if (!pp->phy_dev)
 		return -ENOTSUPP;
 
-	ret = phy_mii_ioctl(pp->phy_dev, ifr, cmd);
-	if (!ret)
-		mvneta_adjust_link(dev);
-
-	return ret;
+	return phy_mii_ioctl(pp->phy_dev, ifr, cmd);
 }
 
 /* Ethtool methods */
diff --git a/drivers/net/ethernet/mellanox/mlx4/cmd.c b/drivers/net/ethernet/mellanox/mlx4/cmd.c
index a681d7c0bb9f..546ca4226916 100644
--- a/drivers/net/ethernet/mellanox/mlx4/cmd.c
+++ b/drivers/net/ethernet/mellanox/mlx4/cmd.c
@@ -724,7 +724,8 @@ static int mlx4_cmd_wait(struct mlx4_dev *dev, u64 in_param, u64 *out_param,
 		 * on the host, we deprecate the error message for this
 		 * specific command/input_mod/opcode_mod/fw-status to be debug.
 		 */
-		if (op == MLX4_CMD_SET_PORT && in_modifier == 1 &&
+		if (op == MLX4_CMD_SET_PORT &&
+		    (in_modifier == 1 || in_modifier == 2) &&
 		    op_modifier == 0 && context->fw_status == CMD_STAT_BAD_SIZE)
 			mlx4_dbg(dev, "command 0x%x failed: fw status = 0x%x\n",
 				 op, context->fw_status);
@@ -1993,7 +1994,6 @@ static void mlx4_master_do_cmd(struct mlx4_dev *dev, int slave, u8 cmd,
 			goto reset_slave;
 		slave_state[slave].vhcr_dma = ((u64) param) << 48;
 		priv->mfunc.master.slave_state[slave].cookie = 0;
-		mutex_init(&priv->mfunc.master.gen_eqe_mutex[slave]);
 		break;
 	case MLX4_COMM_CMD_VHCR1:
 		if (slave_state[slave].last_cmd != MLX4_COMM_CMD_VHCR0)
@@ -2225,6 +2225,7 @@ int mlx4_multi_func_init(struct mlx4_dev *dev)
 		for (i = 0; i < dev->num_slaves; ++i) {
 			s_state = &priv->mfunc.master.slave_state[i];
 			s_state->last_cmd = MLX4_COMM_CMD_RESET;
+			mutex_init(&priv->mfunc.master.gen_eqe_mutex[i]);
 			for (j = 0; j < MLX4_EVENT_TYPES_NUM; ++j)
 				s_state->event_eq[j].eqn = -1;
 			__raw_writel((__force u32) 0,
diff --git a/drivers/net/ethernet/mellanox/mlx4/en_netdev.c b/drivers/net/ethernet/mellanox/mlx4/en_netdev.c
index 2a210c4efb89..3485acf03014 100644
--- a/drivers/net/ethernet/mellanox/mlx4/en_netdev.c
+++ b/drivers/net/ethernet/mellanox/mlx4/en_netdev.c
@@ -1698,8 +1698,6 @@ int mlx4_en_start_port(struct net_device *dev)
 	/* Schedule multicast task to populate multicast list */
 	queue_work(mdev->workqueue, &priv->rx_mode_task);
 
-	mlx4_set_stats_bitmap(mdev->dev, &priv->stats_bitmap);
-
 #ifdef CONFIG_MLX4_EN_VXLAN
 	if (priv->mdev->dev->caps.tunnel_offload_mode == MLX4_TUNNEL_OFFLOAD_MODE_VXLAN)
 		vxlan_get_rx_port(dev);
@@ -2807,13 +2805,6 @@ int mlx4_en_init_netdev(struct mlx4_en_dev *mdev, int port,
 	netif_carrier_off(dev);
 	mlx4_en_set_default_moderation(priv);
 
-	err = register_netdev(dev);
-	if (err) {
-		en_err(priv, "Netdev registration failed for port %d\n", port);
-		goto out;
-	}
-	priv->registered = 1;
-
 	en_warn(priv, "Using %d TX rings\n", prof->tx_ring_num);
 	en_warn(priv, "Using %d RX rings\n", prof->rx_ring_num);
 
@@ -2853,6 +2844,16 @@ int mlx4_en_init_netdev(struct mlx4_en_dev *mdev, int port,
 		queue_delayed_work(mdev->workqueue, &priv->service_task,
 				   SERVICE_TASK_DELAY);
 
+	mlx4_set_stats_bitmap(mdev->dev, &priv->stats_bitmap);
+
+	err = register_netdev(dev);
+	if (err) {
+		en_err(priv, "Netdev registration failed for port %d\n", port);
+		goto out;
+	}
+
+	priv->registered = 1;
+
 	return 0;
 
 out:
diff --git a/drivers/net/ethernet/mellanox/mlx4/en_selftest.c b/drivers/net/ethernet/mellanox/mlx4/en_selftest.c
index 2d8ee66138e8..a61009f4b2df 100644
--- a/drivers/net/ethernet/mellanox/mlx4/en_selftest.c
+++ b/drivers/net/ethernet/mellanox/mlx4/en_selftest.c
@@ -81,12 +81,14 @@ static int mlx4_en_test_loopback(struct mlx4_en_priv *priv)
 {
 	u32 loopback_ok = 0;
 	int i;
-
+	bool gro_enabled;
 
         priv->loopback_ok = 0;
 	priv->validate_loopback = 1;
+	gro_enabled = priv->dev->features & NETIF_F_GRO;
 
 	mlx4_en_update_loopback_state(priv->dev, priv->dev->features);
+	priv->dev->features &= ~NETIF_F_GRO;
 
 	/* xmit */
 	if (mlx4_en_test_loopback_xmit(priv)) {
@@ -108,6 +110,10 @@ static int mlx4_en_test_loopback(struct mlx4_en_priv *priv)
 mlx4_en_test_loopback_exit:
 
 	priv->validate_loopback = 0;
+
+	if (gro_enabled)
+		priv->dev->features |= NETIF_F_GRO;
+
 	mlx4_en_update_loopback_state(priv->dev, priv->dev->features);
 	return !loopback_ok;
 }
diff --git a/drivers/net/ethernet/mellanox/mlx4/eq.c b/drivers/net/ethernet/mellanox/mlx4/eq.c
index 264bc15c1ff2..6e70ffee8e87 100644
--- a/drivers/net/ethernet/mellanox/mlx4/eq.c
+++ b/drivers/net/ethernet/mellanox/mlx4/eq.c
@@ -153,12 +153,10 @@ void mlx4_gen_slave_eqe(struct work_struct *work)
 
 		/* All active slaves need to receive the event */
 		if (slave == ALL_SLAVES) {
-			for (i = 0; i < dev->num_slaves; i++) {
-				if (i != dev->caps.function &&
-				    master->slave_state[i].active)
-					if (mlx4_GEN_EQE(dev, i, eqe))
-						mlx4_warn(dev, "Failed to generate event for slave %d\n",
-							  i);
+			for (i = 0; i <= dev->persist->num_vfs; i++) {
+				if (mlx4_GEN_EQE(dev, i, eqe))
+					mlx4_warn(dev, "Failed to generate event for slave %d\n",
+						  i);
 			}
 		} else {
 			if (mlx4_GEN_EQE(dev, slave, eqe))
@@ -203,13 +201,11 @@ static void mlx4_slave_event(struct mlx4_dev *dev, int slave,
 			     struct mlx4_eqe *eqe)
 {
 	struct mlx4_priv *priv = mlx4_priv(dev);
-	struct mlx4_slave_state *s_slave =
-		&priv->mfunc.master.slave_state[slave];
 
-	if (!s_slave->active) {
-		/*mlx4_warn(dev, "Trying to pass event to inactive slave\n");*/
+	if (slave < 0 || slave > dev->persist->num_vfs ||
+	    slave == dev->caps.function ||
+	    !priv->mfunc.master.slave_state[slave].active)
 		return;
-	}
 
 	slave_event(dev, slave, eqe);
 }
diff --git a/drivers/net/ethernet/mellanox/mlx4/mlx4_en.h b/drivers/net/ethernet/mellanox/mlx4/mlx4_en.h
index 2a8268e6be15..ebbe244e80dd 100644
--- a/drivers/net/ethernet/mellanox/mlx4/mlx4_en.h
+++ b/drivers/net/ethernet/mellanox/mlx4/mlx4_en.h
@@ -453,7 +453,7 @@ struct mlx4_en_port_stats {
 	unsigned long rx_chksum_none;
 	unsigned long rx_chksum_complete;
 	unsigned long tx_chksum_offload;
-#define NUM_PORT_STATS		9
+#define NUM_PORT_STATS		10
 };
 
 struct mlx4_en_perf_stats {
diff --git a/drivers/net/ethernet/mellanox/mlx4/qp.c b/drivers/net/ethernet/mellanox/mlx4/qp.c
index 2bb8553bd905..eda29dbbfcd2 100644
--- a/drivers/net/ethernet/mellanox/mlx4/qp.c
+++ b/drivers/net/ethernet/mellanox/mlx4/qp.c
@@ -412,7 +412,6 @@ err_icm:
 
 EXPORT_SYMBOL_GPL(mlx4_qp_alloc);
 
-#define MLX4_UPDATE_QP_SUPPORTED_ATTRS MLX4_UPDATE_QP_SMAC
 int mlx4_update_qp(struct mlx4_dev *dev, u32 qpn,
 		   enum mlx4_update_qp_attr attr,
 		   struct mlx4_update_qp_params *params)
diff --git a/drivers/net/ethernet/mellanox/mlx4/resource_tracker.c b/drivers/net/ethernet/mellanox/mlx4/resource_tracker.c
index 486e3d26cd4a..6e413ac4e940 100644
--- a/drivers/net/ethernet/mellanox/mlx4/resource_tracker.c
+++ b/drivers/net/ethernet/mellanox/mlx4/resource_tracker.c
@@ -713,7 +713,7 @@ static int update_vport_qp_param(struct mlx4_dev *dev,
 	struct mlx4_vport_oper_state *vp_oper;
 	struct mlx4_priv *priv;
 	u32 qp_type;
-	int port;
+	int port, err = 0;
 
 	port = (qpc->pri_path.sched_queue & 0x40) ? 2 : 1;
 	priv = mlx4_priv(dev);
@@ -738,7 +738,9 @@ static int update_vport_qp_param(struct mlx4_dev *dev,
 			} else {
 				struct mlx4_update_qp_params params = {.flags = 0};
 
-				mlx4_update_qp(dev, qpn, MLX4_UPDATE_QP_VSD, &params);
+				err = mlx4_update_qp(dev, qpn, MLX4_UPDATE_QP_VSD, &params);
+				if (err)
+					goto out;
 			}
 		}
 
@@ -773,7 +775,8 @@ static int update_vport_qp_param(struct mlx4_dev *dev,
 		qpc->pri_path.feup |= MLX4_FSM_FORCE_ETH_SRC_MAC;
 		qpc->pri_path.grh_mylmc = (0x80 & qpc->pri_path.grh_mylmc) + vp_oper->mac_idx;
 	}
-	return 0;
+out:
+	return err;
 }
 
 static int mpt_mask(struct mlx4_dev *dev)
@@ -3092,6 +3095,12 @@ int mlx4_GEN_EQE(struct mlx4_dev *dev, int slave, struct mlx4_eqe *eqe)
 	if (!priv->mfunc.master.slave_state)
 		return -EINVAL;
 
+	/* check for slave valid, slave not PF, and slave active */
+	if (slave < 0 || slave > dev->persist->num_vfs ||
+	    slave == dev->caps.function ||
+	    !priv->mfunc.master.slave_state[slave].active)
+		return 0;
+
 	event_eq = &priv->mfunc.master.slave_state[slave].event_eq[eqe->type];
 
 	/* Create the event only if the slave is registered */
diff --git a/drivers/net/ethernet/pasemi/pasemi_mac.c b/drivers/net/ethernet/pasemi/pasemi_mac.c
index 44e8d7d25547..57a6e6cd74fc 100644
--- a/drivers/net/ethernet/pasemi/pasemi_mac.c
+++ b/drivers/net/ethernet/pasemi/pasemi_mac.c
@@ -1239,11 +1239,9 @@ static int pasemi_mac_open(struct net_device *dev)
 	if (mac->phydev)
 		phy_start(mac->phydev);
 
-	init_timer(&mac->tx->clean_timer);
-	mac->tx->clean_timer.function = pasemi_mac_tx_timer;
-	mac->tx->clean_timer.data = (unsigned long)mac->tx;
-	mac->tx->clean_timer.expires = jiffies+HZ;
-	add_timer(&mac->tx->clean_timer);
+	setup_timer(&mac->tx->clean_timer, pasemi_mac_tx_timer,
+		    (unsigned long)mac->tx);
+	mod_timer(&mac->tx->clean_timer, jiffies + HZ);
 
 	return 0;
 
diff --git a/drivers/net/ethernet/qlogic/netxen/netxen_nic.h b/drivers/net/ethernet/qlogic/netxen/netxen_nic.h
index 6e426ae94692..0a5e204a0179 100644
--- a/drivers/net/ethernet/qlogic/netxen/netxen_nic.h
+++ b/drivers/net/ethernet/qlogic/netxen/netxen_nic.h
@@ -354,7 +354,7 @@ struct cmd_desc_type0 {
 
 } __attribute__ ((aligned(64)));
 
-/* Note: sizeof(rcv_desc) should always be a mutliple of 2 */
+/* Note: sizeof(rcv_desc) should always be a multiple of 2 */
 struct rcv_desc {
 	__le16 reference_handle;
 	__le16 reserved;
@@ -499,7 +499,7 @@ struct uni_data_desc{
 #define NETXEN_IMAGE_START	0x43000	/* compressed image */
 #define NETXEN_SECONDARY_START	0x200000	/* backup images */
 #define NETXEN_PXE_START	0x3E0000	/* PXE boot rom */
-#define NETXEN_USER_START	0x3E8000	/* Firmare info */
+#define NETXEN_USER_START	0x3E8000	/* Firmware info */
 #define NETXEN_FIXED_START	0x3F0000	/* backup of crbinit */
 #define NETXEN_USER_START_OLD	NETXEN_PXE_START /* very old flash */
 
diff --git a/drivers/net/ethernet/qlogic/qlcnic/qlcnic.h b/drivers/net/ethernet/qlogic/qlcnic/qlcnic.h
index fa4317611fd6..f221126a5c4e 100644
--- a/drivers/net/ethernet/qlogic/qlcnic/qlcnic.h
+++ b/drivers/net/ethernet/qlogic/qlcnic/qlcnic.h
@@ -314,7 +314,7 @@ struct qlcnic_fdt {
 #define QLCNIC_BRDCFG_START	0x4000		/* board config */
 #define QLCNIC_BOOTLD_START	0x10000		/* bootld */
 #define QLCNIC_IMAGE_START	0x43000		/* compressed image */
-#define QLCNIC_USER_START	0x3E8000	/* Firmare info */
+#define QLCNIC_USER_START	0x3E8000	/* Firmware info */
 
 #define QLCNIC_FW_VERSION_OFFSET	(QLCNIC_USER_START+0x408)
 #define QLCNIC_FW_SIZE_OFFSET		(QLCNIC_USER_START+0x40c)
diff --git a/drivers/net/ethernet/realtek/r8169.c b/drivers/net/ethernet/realtek/r8169.c
index ad0020af2193..c70ab40d8698 100644
--- a/drivers/net/ethernet/realtek/r8169.c
+++ b/drivers/net/ethernet/realtek/r8169.c
@@ -2561,7 +2561,7 @@ static int rtl_check_firmware(struct rtl8169_private *tp, struct rtl_fw *rtl_fw)
 	int rc = -EINVAL;
 
 	if (!rtl_fw_format_ok(tp, rtl_fw)) {
-		netif_err(tp, ifup, dev, "invalid firwmare\n");
+		netif_err(tp, ifup, dev, "invalid firmware\n");
 		goto out;
 	}
 
@@ -5067,8 +5067,6 @@ static void rtl_hw_reset(struct rtl8169_private *tp)
 	RTL_W8(ChipCmd, CmdReset);
 
 	rtl_udelay_loop_wait_low(tp, &rtl_chipcmd_cond, 100, 100);
-
-	netdev_reset_queue(tp->dev);
 }
 
 static void rtl_request_uncached_firmware(struct rtl8169_private *tp)
@@ -7049,7 +7047,6 @@ static netdev_tx_t rtl8169_start_xmit(struct sk_buff *skb,
 	u32 status, len;
 	u32 opts[2];
 	int frags;
-	bool stop_queue;
 
 	if (unlikely(!TX_FRAGS_READY_FOR(tp, skb_shinfo(skb)->nr_frags))) {
 		netif_err(tp, drv, dev, "BUG! Tx Ring full when queue awake!\n");
@@ -7090,8 +7087,6 @@ static netdev_tx_t rtl8169_start_xmit(struct sk_buff *skb,
 
 	txd->opts2 = cpu_to_le32(opts[1]);
 
-	netdev_sent_queue(dev, skb->len);
-
 	skb_tx_timestamp(skb);
 
 	/* Force memory writes to complete before releasing descriptor */
@@ -7106,16 +7101,11 @@ static netdev_tx_t rtl8169_start_xmit(struct sk_buff *skb,
 
 	tp->cur_tx += frags + 1;
 
-	stop_queue = !TX_FRAGS_READY_FOR(tp, MAX_SKB_FRAGS);
+	RTL_W8(TxPoll, NPQ);
 
-	if (!skb->xmit_more || stop_queue ||
-	    netif_xmit_stopped(netdev_get_tx_queue(dev, 0))) {
-		RTL_W8(TxPoll, NPQ);
-
-		mmiowb();
-	}
+	mmiowb();
 
-	if (stop_queue) {
+	if (!TX_FRAGS_READY_FOR(tp, MAX_SKB_FRAGS)) {
 		/* Avoid wrongly optimistic queue wake-up: rtl_tx thread must
 		 * not miss a ring update when it notices a stopped queue.
 		 */
@@ -7198,7 +7188,6 @@ static void rtl8169_pcierr_interrupt(struct net_device *dev)
 static void rtl_tx(struct net_device *dev, struct rtl8169_private *tp)
 {
 	unsigned int dirty_tx, tx_left;
-	unsigned int bytes_compl = 0, pkts_compl = 0;
 
 	dirty_tx = tp->dirty_tx;
 	smp_rmb();
@@ -7222,8 +7211,10 @@ static void rtl_tx(struct net_device *dev, struct rtl8169_private *tp)
 		rtl8169_unmap_tx_skb(&tp->pci_dev->dev, tx_skb,
 				     tp->TxDescArray + entry);
 		if (status & LastFrag) {
-			pkts_compl++;
-			bytes_compl += tx_skb->skb->len;
+			u64_stats_update_begin(&tp->tx_stats.syncp);
+			tp->tx_stats.packets++;
+			tp->tx_stats.bytes += tx_skb->skb->len;
+			u64_stats_update_end(&tp->tx_stats.syncp);
 			dev_kfree_skb_any(tx_skb->skb);
 			tx_skb->skb = NULL;
 		}
@@ -7232,13 +7223,6 @@ static void rtl_tx(struct net_device *dev, struct rtl8169_private *tp)
 	}
 
 	if (tp->dirty_tx != dirty_tx) {
-		netdev_completed_queue(tp->dev, pkts_compl, bytes_compl);
-
-		u64_stats_update_begin(&tp->tx_stats.syncp);
-		tp->tx_stats.packets += pkts_compl;
-		tp->tx_stats.bytes += bytes_compl;
-		u64_stats_update_end(&tp->tx_stats.syncp);
-
 		tp->dirty_tx = dirty_tx;
 		/* Sync with rtl8169_start_xmit:
 		 * - publish dirty_tx ring index (write barrier)
diff --git a/drivers/net/ethernet/renesas/sh_eth.c b/drivers/net/ethernet/renesas/sh_eth.c
index 4da8bd263997..736d5d1624a1 100644
--- a/drivers/net/ethernet/renesas/sh_eth.c
+++ b/drivers/net/ethernet/renesas/sh_eth.c
@@ -508,7 +508,6 @@ static struct sh_eth_cpu_data r8a779x_data = {
 	.tpauser	= 1,
 	.hw_swap	= 1,
 	.rmiimode	= 1,
-	.shift_rd0	= 1,
 };
 
 static void sh_eth_set_rate_sh7724(struct net_device *ndev)
@@ -1392,6 +1391,9 @@ static void sh_eth_dev_exit(struct net_device *ndev)
 	msleep(2); /* max frame time at 10 Mbps < 1250 us */
 	sh_eth_get_stats(ndev);
 	sh_eth_reset(ndev);
+
+	/* Set MAC address again */
+	update_mac_address(ndev);
 }
 
 /* free Tx skb function */
@@ -1407,6 +1409,8 @@ static int sh_eth_txfree(struct net_device *ndev)
 		txdesc = &mdp->tx_ring[entry];
 		if (txdesc->status & cpu_to_edmac(mdp, TD_TACT))
 			break;
+		/* TACT bit must be checked before all the following reads */
+		rmb();
 		/* Free the original skb. */
 		if (mdp->tx_skbuff[entry]) {
 			dma_unmap_single(&ndev->dev, txdesc->addr,
@@ -1444,6 +1448,8 @@ static int sh_eth_rx(struct net_device *ndev, u32 intr_status, int *quota)
 	limit = boguscnt;
 	rxdesc = &mdp->rx_ring[entry];
 	while (!(rxdesc->status & cpu_to_edmac(mdp, RD_RACT))) {
+		/* RACT bit must be checked before all the following reads */
+		rmb();
 		desc_status = edmac_to_cpu(mdp, rxdesc->status);
 		pkt_len = rxdesc->frame_length;
 
@@ -1455,8 +1461,8 @@ static int sh_eth_rx(struct net_device *ndev, u32 intr_status, int *quota)
 
 		/* In case of almost all GETHER/ETHERs, the Receive Frame State
 		 * (RFS) bits in the Receive Descriptor 0 are from bit 9 to
-		 * bit 0. However, in case of the R8A7740, R8A779x, and
-		 * R7S72100 the RFS bits are from bit 25 to bit 16. So, the
+		 * bit 0. However, in case of the R8A7740 and R7S72100
+		 * the RFS bits are from bit 25 to bit 16. So, the
 		 * driver needs right shifting by 16.
 		 */
 		if (mdp->cd->shift_rd0)
@@ -1523,6 +1529,7 @@ static int sh_eth_rx(struct net_device *ndev, u32 intr_status, int *quota)
 			skb_checksum_none_assert(skb);
 			rxdesc->addr = dma_addr;
 		}
+		wmb(); /* RACT bit must be set after all the above writes */
 		if (entry >= mdp->num_rx_ring - 1)
 			rxdesc->status |=
 				cpu_to_edmac(mdp, RD_RACT | RD_RFP | RD_RDEL);
@@ -1535,7 +1542,7 @@ static int sh_eth_rx(struct net_device *ndev, u32 intr_status, int *quota)
 	/* If we don't need to check status, don't. -KDU */
 	if (!(sh_eth_read(ndev, EDRRR) & EDRRR_R)) {
 		/* fix the values for the next receiving if RDE is set */
-		if (intr_status & EESR_RDE) {
+		if (intr_status & EESR_RDE && mdp->reg_offset[RDFAR] != 0) {
 			u32 count = (sh_eth_read(ndev, RDFAR) -
 				     sh_eth_read(ndev, RDLAR)) >> 4;
 
@@ -2174,7 +2181,7 @@ static int sh_eth_start_xmit(struct sk_buff *skb, struct net_device *ndev)
 	}
 	spin_unlock_irqrestore(&mdp->lock, flags);
 
-	if (skb_padto(skb, ETH_ZLEN))
+	if (skb_put_padto(skb, ETH_ZLEN))
 		return NETDEV_TX_OK;
 
 	entry = mdp->cur_tx % mdp->num_tx_ring;
@@ -2192,6 +2199,7 @@ static int sh_eth_start_xmit(struct sk_buff *skb, struct net_device *ndev)
 	}
 	txdesc->buffer_length = skb->len;
 
+	wmb(); /* TACT bit must be set after all the above writes */
 	if (entry >= mdp->num_tx_ring - 1)
 		txdesc->status |= cpu_to_edmac(mdp, TD_TACT | TD_TDLE);
 	else
diff --git a/drivers/net/ethernet/rocker/rocker.c b/drivers/net/ethernet/rocker/rocker.c
index 34389b6aa67c..5cecec282aba 100644
--- a/drivers/net/ethernet/rocker/rocker.c
+++ b/drivers/net/ethernet/rocker/rocker.c
@@ -1257,9 +1257,9 @@ static void rocker_port_set_enable(struct rocker_port *rocker_port, bool enable)
 	u64 val = rocker_read64(rocker_port->rocker, PORT_PHYS_ENABLE);
 
 	if (enable)
-		val |= 1 << rocker_port->lport;
+		val |= 1ULL << rocker_port->lport;
 	else
-		val &= ~(1 << rocker_port->lport);
+		val &= ~(1ULL << rocker_port->lport);
 	rocker_write64(rocker_port->rocker, PORT_PHYS_ENABLE, val);
 }
 
@@ -4201,6 +4201,8 @@ static int rocker_probe_ports(struct rocker *rocker)
 
 	alloc_size = sizeof(struct rocker_port *) * rocker->port_count;
 	rocker->ports = kmalloc(alloc_size, GFP_KERNEL);
+	if (!rocker->ports)
+		return -ENOMEM;
 	for (i = 0; i < rocker->port_count; i++) {
 		err = rocker_probe_port(rocker, i);
 		if (err)
@@ -4466,10 +4468,16 @@ static int rocker_port_master_changed(struct net_device *dev)
 	struct net_device *master = netdev_master_upper_dev_get(dev);
 	int err = 0;
 
+	/* There are currently three cases handled here:
+	 * 1. Joining a bridge
+	 * 2. Leaving a previously joined bridge
+	 * 3. Other, e.g. being added to or removed from a bond or openvswitch,
+	 *    in which case nothing is done
+	 */
 	if (master && master->rtnl_link_ops &&
 	    !strcmp(master->rtnl_link_ops->kind, "bridge"))
 		err = rocker_port_bridge_join(rocker_port, master);
-	else
+	else if (rocker_port_is_bridged(rocker_port))
 		err = rocker_port_bridge_leave(rocker_port);
 
 	return err;
diff --git a/drivers/net/ethernet/smsc/smc91c92_cs.c b/drivers/net/ethernet/smsc/smc91c92_cs.c
index 6b33127ab352..3449893aea8d 100644
--- a/drivers/net/ethernet/smsc/smc91c92_cs.c
+++ b/drivers/net/ethernet/smsc/smc91c92_cs.c
@@ -1070,11 +1070,8 @@ static int smc_open(struct net_device *dev)
     smc->packets_waiting = 0;
 
     smc_reset(dev);
-    init_timer(&smc->media);
-    smc->media.function = media_check;
-    smc->media.data = (u_long) dev;
-    smc->media.expires = jiffies + HZ;
-    add_timer(&smc->media);
+    setup_timer(&smc->media, media_check, (u_long)dev);
+    mod_timer(&smc->media, jiffies + HZ);
 
     return 0;
 } /* smc_open */
diff --git a/drivers/net/ethernet/smsc/smc91x.c b/drivers/net/ethernet/smsc/smc91x.c
index 88a55f95fe09..8678e39aba08 100644
--- a/drivers/net/ethernet/smsc/smc91x.c
+++ b/drivers/net/ethernet/smsc/smc91x.c
@@ -91,6 +91,11 @@ static const char version[] =
 
 #include "smc91x.h"
 
+#if defined(CONFIG_ASSABET_NEPONSET)
+#include <mach/assabet.h>
+#include <mach/neponset.h>
+#endif
+
 #ifndef SMC_NOWAIT
 # define SMC_NOWAIT		0
 #endif
@@ -2243,10 +2248,9 @@ static int smc_drv_probe(struct platform_device *pdev)
 	const struct of_device_id *match = NULL;
 	struct smc_local *lp;
 	struct net_device *ndev;
-	struct resource *res;
+	struct resource *res, *ires;
 	unsigned int __iomem *addr;
 	unsigned long irq_flags = SMC_IRQ_FLAGS;
-	unsigned long irq_resflags;
 	int ret;
 
 	ndev = alloc_etherdev(sizeof(struct smc_local));
@@ -2338,25 +2342,23 @@ static int smc_drv_probe(struct platform_device *pdev)
 		goto out_free_netdev;
 	}
 
-	ndev->irq = platform_get_irq(pdev, 0);
-	if (ndev->irq <= 0) {
+	ires = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
+	if (!ires) {
 		ret = -ENODEV;
 		goto out_release_io;
 	}
-	/*
-	 * If this platform does not specify any special irqflags, or if
-	 * the resource supplies a trigger, override the irqflags with
-	 * the trigger flags from the resource.
-	 */
-	irq_resflags = irqd_get_trigger_type(irq_get_irq_data(ndev->irq));
-	if (irq_flags == -1 || irq_resflags & IRQF_TRIGGER_MASK)
-		irq_flags = irq_resflags & IRQF_TRIGGER_MASK;
+
+	ndev->irq = ires->start;
+
+	if (irq_flags == -1 || ires->flags & IRQF_TRIGGER_MASK)
+		irq_flags = ires->flags & IRQF_TRIGGER_MASK;
 
 	ret = smc_request_attrib(pdev, ndev);
 	if (ret)
 		goto out_release_io;
-#if defined(CONFIG_SA1100_ASSABET)
-	neponset_ncr_set(NCR_ENET_OSC_EN);
+#if defined(CONFIG_ASSABET_NEPONSET)
+	if (machine_is_assabet() && machine_has_neponset())
+		neponset_ncr_set(NCR_ENET_OSC_EN);
 #endif
 	platform_set_drvdata(pdev, ndev);
 	ret = smc_enable_device(pdev);
diff --git a/drivers/net/ethernet/smsc/smc91x.h b/drivers/net/ethernet/smsc/smc91x.h
index be67baf5f677..3a18501d1068 100644
--- a/drivers/net/ethernet/smsc/smc91x.h
+++ b/drivers/net/ethernet/smsc/smc91x.h
@@ -39,14 +39,7 @@
  * Define your architecture specific bus configuration parameters here.
  */
 
-#if defined(CONFIG_ARCH_LUBBOCK) ||\
-    defined(CONFIG_MACH_MAINSTONE) ||\
-    defined(CONFIG_MACH_ZYLONITE) ||\
-    defined(CONFIG_MACH_LITTLETON) ||\
-    defined(CONFIG_MACH_ZYLONITE2) ||\
-    defined(CONFIG_ARCH_VIPER) ||\
-    defined(CONFIG_MACH_STARGATE2) ||\
-    defined(CONFIG_ARCH_VERSATILE)
+#if defined(CONFIG_ARM)
 
 #include <asm/mach-types.h>
 
@@ -74,95 +67,8 @@
 /* We actually can't write halfwords properly if not word aligned */
 static inline void SMC_outw(u16 val, void __iomem *ioaddr, int reg)
 {
-	if ((machine_is_mainstone() || machine_is_stargate2()) && reg & 2) {
-		unsigned int v = val << 16;
-		v |= readl(ioaddr + (reg & ~2)) & 0xffff;
-		writel(v, ioaddr + (reg & ~2));
-	} else {
-		writew(val, ioaddr + reg);
-	}
-}
-
-#elif defined(CONFIG_SA1100_PLEB)
-/* We can only do 16-bit reads and writes in the static memory space. */
-#define SMC_CAN_USE_8BIT	1
-#define SMC_CAN_USE_16BIT	1
-#define SMC_CAN_USE_32BIT	0
-#define SMC_IO_SHIFT		0
-#define SMC_NOWAIT		1
-
-#define SMC_inb(a, r)		readb((a) + (r))
-#define SMC_insb(a, r, p, l)	readsb((a) + (r), p, (l))
-#define SMC_inw(a, r)		readw((a) + (r))
-#define SMC_insw(a, r, p, l)	readsw((a) + (r), p, l)
-#define SMC_outb(v, a, r)	writeb(v, (a) + (r))
-#define SMC_outsb(a, r, p, l)	writesb((a) + (r), p, (l))
-#define SMC_outw(v, a, r)	writew(v, (a) + (r))
-#define SMC_outsw(a, r, p, l)	writesw((a) + (r), p, l)
-
-#define SMC_IRQ_FLAGS		(-1)
-
-#elif defined(CONFIG_SA1100_ASSABET)
-
-#include <mach/neponset.h>
-
-/* We can only do 8-bit reads and writes in the static memory space. */
-#define SMC_CAN_USE_8BIT	1
-#define SMC_CAN_USE_16BIT	0
-#define SMC_CAN_USE_32BIT	0
-#define SMC_NOWAIT		1
-
-/* The first two address lines aren't connected... */
-#define SMC_IO_SHIFT		2
-
-#define SMC_inb(a, r)		readb((a) + (r))
-#define SMC_outb(v, a, r)	writeb(v, (a) + (r))
-#define SMC_insb(a, r, p, l)	readsb((a) + (r), p, (l))
-#define SMC_outsb(a, r, p, l)	writesb((a) + (r), p, (l))
-#define SMC_IRQ_FLAGS		(-1)	/* from resource */
-
-#elif	defined(CONFIG_MACH_LOGICPD_PXA270) ||	\
-	defined(CONFIG_MACH_NOMADIK_8815NHK)
-
-#define SMC_CAN_USE_8BIT	0
-#define SMC_CAN_USE_16BIT	1
-#define SMC_CAN_USE_32BIT	0
-#define SMC_IO_SHIFT		0
-#define SMC_NOWAIT		1
-
-#define SMC_inw(a, r)		readw((a) + (r))
-#define SMC_outw(v, a, r)	writew(v, (a) + (r))
-#define SMC_insw(a, r, p, l)	readsw((a) + (r), p, l)
-#define SMC_outsw(a, r, p, l)	writesw((a) + (r), p, l)
-
-#elif	defined(CONFIG_ARCH_INNOKOM) || \
-	defined(CONFIG_ARCH_PXA_IDP) || \
-	defined(CONFIG_ARCH_RAMSES) || \
-	defined(CONFIG_ARCH_PCM027)
-
-#define SMC_CAN_USE_8BIT	1
-#define SMC_CAN_USE_16BIT	1
-#define SMC_CAN_USE_32BIT	1
-#define SMC_IO_SHIFT		0
-#define SMC_NOWAIT		1
-#define SMC_USE_PXA_DMA		1
-
-#define SMC_inb(a, r)		readb((a) + (r))
-#define SMC_inw(a, r)		readw((a) + (r))
-#define SMC_inl(a, r)		readl((a) + (r))
-#define SMC_outb(v, a, r)	writeb(v, (a) + (r))
-#define SMC_outl(v, a, r)	writel(v, (a) + (r))
-#define SMC_insl(a, r, p, l)	readsl((a) + (r), p, l)
-#define SMC_outsl(a, r, p, l)	writesl((a) + (r), p, l)
-#define SMC_insw(a, r, p, l)	readsw((a) + (r), p, l)
-#define SMC_outsw(a, r, p, l)	writesw((a) + (r), p, l)
-#define SMC_IRQ_FLAGS		(-1)	/* from resource */
-
-/* We actually can't write halfwords properly if not word aligned */
-static inline void
-SMC_outw(u16 val, void __iomem *ioaddr, int reg)
-{
-	if (reg & 2) {
+	if ((machine_is_mainstone() || machine_is_stargate2() ||
+	     machine_is_pxa_idp()) && reg & 2) {
 		unsigned int v = val << 16;
 		v |= readl(ioaddr + (reg & ~2)) & 0xffff;
 		writel(v, ioaddr + (reg & ~2));
@@ -237,20 +143,6 @@ SMC_outw(u16 val, void __iomem *ioaddr, int reg)
 #define RPC_LSA_DEFAULT         RPC_LED_100_10
 #define RPC_LSB_DEFAULT         RPC_LED_TX_RX
 
-#elif defined(CONFIG_ARCH_MSM)
-
-#define SMC_CAN_USE_8BIT	0
-#define SMC_CAN_USE_16BIT	1
-#define SMC_CAN_USE_32BIT	0
-#define SMC_NOWAIT		1
-
-#define SMC_inw(a, r)		readw((a) + (r))
-#define SMC_outw(v, a, r)	writew(v, (a) + (r))
-#define SMC_insw(a, r, p, l)	readsw((a) + (r), p, l)
-#define SMC_outsw(a, r, p, l)	writesw((a) + (r), p, l)
-
-#define SMC_IRQ_FLAGS		IRQF_TRIGGER_HIGH
-
 #elif defined(CONFIG_COLDFIRE)
 
 #define SMC_CAN_USE_8BIT	0
diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
index 55e89b3838f1..a0ea84fe6519 100644
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
@@ -310,11 +310,11 @@ bool stmmac_eee_init(struct stmmac_priv *priv)
 		spin_lock_irqsave(&priv->lock, flags);
 		if (!priv->eee_active) {
 			priv->eee_active = 1;
-			init_timer(&priv->eee_ctrl_timer);
-			priv->eee_ctrl_timer.function = stmmac_eee_ctrl_timer;
-			priv->eee_ctrl_timer.data = (unsigned long)priv;
-			priv->eee_ctrl_timer.expires = STMMAC_LPI_T(eee_timer);
-			add_timer(&priv->eee_ctrl_timer);
+			setup_timer(&priv->eee_ctrl_timer,
+				    stmmac_eee_ctrl_timer,
+				    (unsigned long)priv);
+			mod_timer(&priv->eee_ctrl_timer,
+				  STMMAC_LPI_T(eee_timer));
 
 			priv->hw->mac->set_eee_timer(priv->hw,
 						     STMMAC_DEFAULT_LIT_LS,
diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_platform.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_platform.c
index fb846ebba1d9..f9b42f11950f 100644
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_platform.c
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_platform.c
@@ -272,6 +272,37 @@ static int stmmac_pltfr_probe(struct platform_device *pdev)
 	struct stmmac_priv *priv = NULL;
 	struct plat_stmmacenet_data *plat_dat = NULL;
 	const char *mac = NULL;
+	int irq, wol_irq, lpi_irq;
+
+	/* Get IRQ information early to have an ability to ask for deferred
+	 * probe if needed before we went too far with resource allocation.
+	 */
+	irq = platform_get_irq_byname(pdev, "macirq");
+	if (irq < 0) {
+		if (irq != -EPROBE_DEFER) {
+			dev_err(dev,
+				"MAC IRQ configuration information not found\n");
+		}
+		return irq;
+	}
+
+	/* On some platforms e.g. SPEAr the wake up irq differs from the mac irq
+	 * The external wake up irq can be passed through the platform code
+	 * named as "eth_wake_irq"
+	 *
+	 * In case the wake up interrupt is not passed from the platform
+	 * so the driver will continue to use the mac irq (ndev->irq)
+	 */
+	wol_irq = platform_get_irq_byname(pdev, "eth_wake_irq");
+	if (wol_irq < 0) {
+		if (wol_irq == -EPROBE_DEFER)
+			return -EPROBE_DEFER;
+		wol_irq = irq;
+	}
+
+	lpi_irq = platform_get_irq_byname(pdev, "eth_lpi");
+	if (lpi_irq == -EPROBE_DEFER)
+		return -EPROBE_DEFER;
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	addr = devm_ioremap_resource(dev, res);
@@ -323,39 +354,15 @@ static int stmmac_pltfr_probe(struct platform_device *pdev)
 		return PTR_ERR(priv);
 	}
 
+	/* Copy IRQ values to priv structure which is now avaialble */
+	priv->dev->irq = irq;
+	priv->wol_irq = wol_irq;
+	priv->lpi_irq = lpi_irq;
+
 	/* Get MAC address if available (DT) */
 	if (mac)
 		memcpy(priv->dev->dev_addr, mac, ETH_ALEN);
 
-	/* Get the MAC information */
-	priv->dev->irq = platform_get_irq_byname(pdev, "macirq");
-	if (priv->dev->irq < 0) {
-		if (priv->dev->irq != -EPROBE_DEFER) {
-			netdev_err(priv->dev,
-				   "MAC IRQ configuration information not found\n");
-		}
-		return priv->dev->irq;
-	}
-
-	/*
-	 * On some platforms e.g. SPEAr the wake up irq differs from the mac irq
-	 * The external wake up irq can be passed through the platform code
-	 * named as "eth_wake_irq"
-	 *
-	 * In case the wake up interrupt is not passed from the platform
-	 * so the driver will continue to use the mac irq (ndev->irq)
-	 */
-	priv->wol_irq = platform_get_irq_byname(pdev, "eth_wake_irq");
-	if (priv->wol_irq < 0) {
-		if (priv->wol_irq == -EPROBE_DEFER)
-			return -EPROBE_DEFER;
-		priv->wol_irq = priv->dev->irq;
-	}
-
-	priv->lpi_irq = platform_get_irq_byname(pdev, "eth_lpi");
-	if (priv->lpi_irq == -EPROBE_DEFER)
-		return -EPROBE_DEFER;
-
 	platform_set_drvdata(pdev, priv->dev);
 
 	pr_debug("STMMAC platform driver registration completed");
diff --git a/drivers/net/ethernet/sun/niu.c b/drivers/net/ethernet/sun/niu.c
index 4b51f903fb73..0c5842aeb807 100644
--- a/drivers/net/ethernet/sun/niu.c
+++ b/drivers/net/ethernet/sun/niu.c
@@ -6989,10 +6989,10 @@ static int niu_class_to_ethflow(u64 class, int *flow_type)
 		*flow_type = IP_USER_FLOW;
 		break;
 	default:
-		return 0;
+		return -EINVAL;
 	}
 
-	return 1;
+	return 0;
 }
 
 static int niu_ethflow_to_class(int flow_type, u64 *class)
@@ -7198,11 +7198,9 @@ static int niu_get_ethtool_tcam_entry(struct niu *np,
 	class = (tp->key[0] & TCAM_V4KEY0_CLASS_CODE) >>
 		TCAM_V4KEY0_CLASS_CODE_SHIFT;
 	ret = niu_class_to_ethflow(class, &fsp->flow_type);
-
 	if (ret < 0) {
 		netdev_info(np->dev, "niu%d: niu_class_to_ethflow failed\n",
 			    parent->index);
-		ret = -EINVAL;
 		goto out;
 	}
 
diff --git a/drivers/net/ethernet/ti/cpsw.c b/drivers/net/ethernet/ti/cpsw.c
index 7d8dd0d2182e..a1bbaf6352ba 100644
--- a/drivers/net/ethernet/ti/cpsw.c
+++ b/drivers/net/ethernet/ti/cpsw.c
@@ -1103,7 +1103,7 @@ static inline void cpsw_add_dual_emac_def_ale_entries(
 	cpsw_ale_add_mcast(priv->ale, priv->ndev->broadcast,
 			   port_mask, ALE_VLAN, slave->port_vlan, 0);
 	cpsw_ale_add_ucast(priv->ale, priv->mac_addr,
-		priv->host_port, ALE_VLAN, slave->port_vlan);
+		priv->host_port, ALE_VLAN | ALE_SECURE, slave->port_vlan);
 }
 
 static void soft_reset_slave(struct cpsw_slave *slave)
@@ -2466,6 +2466,7 @@ static int cpsw_remove(struct platform_device *pdev)
 	return 0;
 }
 
+#ifdef CONFIG_PM_SLEEP
 static int cpsw_suspend(struct device *dev)
 {
 	struct platform_device	*pdev = to_platform_device(dev);
@@ -2518,11 +2519,9 @@ static int cpsw_resume(struct device *dev)
 	}
 	return 0;
 }
+#endif
 
-static const struct dev_pm_ops cpsw_pm_ops = {
-	.suspend	= cpsw_suspend,
-	.resume		= cpsw_resume,
-};
+static SIMPLE_DEV_PM_OPS(cpsw_pm_ops, cpsw_suspend, cpsw_resume);
 
 static const struct of_device_id cpsw_of_mtable[] = {
 	{ .compatible = "ti,cpsw", },
diff --git a/drivers/net/ethernet/ti/davinci_mdio.c b/drivers/net/ethernet/ti/davinci_mdio.c
index 98655b44b97e..c00084d689f3 100644
--- a/drivers/net/ethernet/ti/davinci_mdio.c
+++ b/drivers/net/ethernet/ti/davinci_mdio.c
@@ -423,6 +423,7 @@ static int davinci_mdio_remove(struct platform_device *pdev)
 	return 0;
 }
 
+#ifdef CONFIG_PM_SLEEP
 static int davinci_mdio_suspend(struct device *dev)
 {
 	struct davinci_mdio_data *data = dev_get_drvdata(dev);
@@ -464,10 +465,10 @@ static int davinci_mdio_resume(struct device *dev)
 
 	return 0;
 }
+#endif
 
 static const struct dev_pm_ops davinci_mdio_pm_ops = {
-	.suspend_late	= davinci_mdio_suspend,
-	.resume_early	= davinci_mdio_resume,
+	SET_LATE_SYSTEM_SLEEP_PM_OPS(davinci_mdio_suspend, davinci_mdio_resume)
 };
 
 #if IS_ENABLED(CONFIG_OF)
diff --git a/drivers/net/ethernet/wiznet/w5100.c b/drivers/net/ethernet/wiznet/w5100.c
index a495931a66a1..0e0fbb5842b3 100644
--- a/drivers/net/ethernet/wiznet/w5100.c
+++ b/drivers/net/ethernet/wiznet/w5100.c
@@ -498,9 +498,9 @@ static int w5100_napi_poll(struct napi_struct *napi, int budget)
 	}
 
 	if (rx_count < budget) {
+		napi_complete(napi);
 		w5100_write(priv, W5100_IMR, IR_S0);
 		mmiowb();
-		napi_complete(napi);
 	}
 
 	return rx_count;
diff --git a/drivers/net/ethernet/wiznet/w5300.c b/drivers/net/ethernet/wiznet/w5300.c
index 09322d9db578..4b310002258d 100644
--- a/drivers/net/ethernet/wiznet/w5300.c
+++ b/drivers/net/ethernet/wiznet/w5300.c
@@ -418,9 +418,9 @@ static int w5300_napi_poll(struct napi_struct *napi, int budget)
 	}
 
 	if (rx_count < budget) {
+		napi_complete(napi);
 		w5300_write(priv, W5300_IMR, IR_S0);
 		mmiowb();
-		napi_complete(napi);
 	}
 
 	return rx_count;
diff --git a/drivers/net/ethernet/xscale/ixp4xx_eth.c b/drivers/net/ethernet/xscale/ixp4xx_eth.c
index f7e0f0f7c2e2..9e16a2819d48 100644
--- a/drivers/net/ethernet/xscale/ixp4xx_eth.c
+++ b/drivers/net/ethernet/xscale/ixp4xx_eth.c
@@ -938,7 +938,7 @@ static void eth_set_mcast_list(struct net_device *dev)
 	int i;
 	static const u8 allmulti[] = { 0x01, 0x00, 0x00, 0x00, 0x00, 0x00 };
 
-	if (dev->flags & IFF_ALLMULTI) {
+	if ((dev->flags & IFF_ALLMULTI) && !(dev->flags & IFF_PROMISC)) {
 		for (i = 0; i < ETH_ALEN; i++) {
 			__raw_writel(allmulti[i], &port->regs->mcast_addr[i]);
 			__raw_writel(allmulti[i], &port->regs->mcast_mask[i]);
diff --git a/drivers/net/ipvlan/ipvlan.h b/drivers/net/ipvlan/ipvlan.h
index 924ea98bd531..54549a6223dd 100644
--- a/drivers/net/ipvlan/ipvlan.h
+++ b/drivers/net/ipvlan/ipvlan.h
@@ -114,7 +114,9 @@ unsigned int ipvlan_mac_hash(const unsigned char *addr);
 rx_handler_result_t ipvlan_handle_frame(struct sk_buff **pskb);
 int ipvlan_queue_xmit(struct sk_buff *skb, struct net_device *dev);
 void ipvlan_ht_addr_add(struct ipvl_dev *ipvlan, struct ipvl_addr *addr);
-bool ipvlan_addr_busy(struct ipvl_dev *ipvlan, void *iaddr, bool is_v6);
+struct ipvl_addr *ipvlan_find_addr(const struct ipvl_dev *ipvlan,
+				   const void *iaddr, bool is_v6);
+bool ipvlan_addr_busy(struct ipvl_port *port, void *iaddr, bool is_v6);
 struct ipvl_addr *ipvlan_ht_addr_lookup(const struct ipvl_port *port,
 					const void *iaddr, bool is_v6);
 void ipvlan_ht_addr_del(struct ipvl_addr *addr, bool sync);
diff --git a/drivers/net/ipvlan/ipvlan_core.c b/drivers/net/ipvlan/ipvlan_core.c
index 2a175006028b..b7877a194cfe 100644
--- a/drivers/net/ipvlan/ipvlan_core.c
+++ b/drivers/net/ipvlan/ipvlan_core.c
@@ -81,19 +81,20 @@ void ipvlan_ht_addr_add(struct ipvl_dev *ipvlan, struct ipvl_addr *addr)
 	hash = (addr->atype == IPVL_IPV6) ?
 	       ipvlan_get_v6_hash(&addr->ip6addr) :
 	       ipvlan_get_v4_hash(&addr->ip4addr);
-	hlist_add_head_rcu(&addr->hlnode, &port->hlhead[hash]);
+	if (hlist_unhashed(&addr->hlnode))
+		hlist_add_head_rcu(&addr->hlnode, &port->hlhead[hash]);
 }
 
 void ipvlan_ht_addr_del(struct ipvl_addr *addr, bool sync)
 {
-	hlist_del_rcu(&addr->hlnode);
+	hlist_del_init_rcu(&addr->hlnode);
 	if (sync)
 		synchronize_rcu();
 }
 
-bool ipvlan_addr_busy(struct ipvl_dev *ipvlan, void *iaddr, bool is_v6)
+struct ipvl_addr *ipvlan_find_addr(const struct ipvl_dev *ipvlan,
+				   const void *iaddr, bool is_v6)
 {
-	struct ipvl_port *port = ipvlan->port;
 	struct ipvl_addr *addr;
 
 	list_for_each_entry(addr, &ipvlan->addrs, anode) {
@@ -101,12 +102,21 @@ bool ipvlan_addr_busy(struct ipvl_dev *ipvlan, void *iaddr, bool is_v6)
 		    ipv6_addr_equal(&addr->ip6addr, iaddr)) ||
 		    (!is_v6 && addr->atype == IPVL_IPV4 &&
 		    addr->ip4addr.s_addr == ((struct in_addr *)iaddr)->s_addr))
-			return true;
+			return addr;
 	}
+	return NULL;
+}
+
+bool ipvlan_addr_busy(struct ipvl_port *port, void *iaddr, bool is_v6)
+{
+	struct ipvl_dev *ipvlan;
 
-	if (ipvlan_ht_addr_lookup(port, iaddr, is_v6))
-		return true;
+	ASSERT_RTNL();
 
+	list_for_each_entry(ipvlan, &port->ipvlans, pnode) {
+		if (ipvlan_find_addr(ipvlan, iaddr, is_v6))
+			return true;
+	}
 	return false;
 }
 
@@ -192,7 +202,8 @@ static void ipvlan_multicast_frame(struct ipvl_port *port, struct sk_buff *skb,
 	if (skb->protocol == htons(ETH_P_PAUSE))
 		return;
 
-	list_for_each_entry(ipvlan, &port->ipvlans, pnode) {
+	rcu_read_lock();
+	list_for_each_entry_rcu(ipvlan, &port->ipvlans, pnode) {
 		if (local && (ipvlan == in_dev))
 			continue;
 
@@ -219,6 +230,7 @@ static void ipvlan_multicast_frame(struct ipvl_port *port, struct sk_buff *skb,
 mcast_acct:
 		ipvlan_count_rx(ipvlan, len, ret == NET_RX_SUCCESS, true);
 	}
+	rcu_read_unlock();
 
 	/* Locally generated? ...Forward a copy to the main-device as
 	 * well. On the RX side we'll ignore it (wont give it to any
diff --git a/drivers/net/ipvlan/ipvlan_main.c b/drivers/net/ipvlan/ipvlan_main.c
index 4f4099d5603d..4fa14208d799 100644
--- a/drivers/net/ipvlan/ipvlan_main.c
+++ b/drivers/net/ipvlan/ipvlan_main.c
@@ -505,7 +505,7 @@ static void ipvlan_link_delete(struct net_device *dev, struct list_head *head)
 	if (ipvlan->ipv6cnt > 0 || ipvlan->ipv4cnt > 0) {
 		list_for_each_entry_safe(addr, next, &ipvlan->addrs, anode) {
 			ipvlan_ht_addr_del(addr, !dev->dismantle);
-			list_del_rcu(&addr->anode);
+			list_del(&addr->anode);
 		}
 	}
 	list_del_rcu(&ipvlan->pnode);
@@ -607,7 +607,7 @@ static int ipvlan_add_addr6(struct ipvl_dev *ipvlan, struct in6_addr *ip6_addr)
 {
 	struct ipvl_addr *addr;
 
-	if (ipvlan_addr_busy(ipvlan, ip6_addr, true)) {
+	if (ipvlan_addr_busy(ipvlan->port, ip6_addr, true)) {
 		netif_err(ipvlan, ifup, ipvlan->dev,
 			  "Failed to add IPv6=%pI6c addr for %s intf\n",
 			  ip6_addr, ipvlan->dev->name);
@@ -620,9 +620,13 @@ static int ipvlan_add_addr6(struct ipvl_dev *ipvlan, struct in6_addr *ip6_addr)
 	addr->master = ipvlan;
 	memcpy(&addr->ip6addr, ip6_addr, sizeof(struct in6_addr));
 	addr->atype = IPVL_IPV6;
-	list_add_tail_rcu(&addr->anode, &ipvlan->addrs);
+	list_add_tail(&addr->anode, &ipvlan->addrs);
 	ipvlan->ipv6cnt++;
-	ipvlan_ht_addr_add(ipvlan, addr);
+	/* If the interface is not up, the address will be added to the hash
+	 * list by ipvlan_open.
+	 */
+	if (netif_running(ipvlan->dev))
+		ipvlan_ht_addr_add(ipvlan, addr);
 
 	return 0;
 }
@@ -631,12 +635,12 @@ static void ipvlan_del_addr6(struct ipvl_dev *ipvlan, struct in6_addr *ip6_addr)
 {
 	struct ipvl_addr *addr;
 
-	addr = ipvlan_ht_addr_lookup(ipvlan->port, ip6_addr, true);
+	addr = ipvlan_find_addr(ipvlan, ip6_addr, true);
 	if (!addr)
 		return;
 
 	ipvlan_ht_addr_del(addr, true);
-	list_del_rcu(&addr->anode);
+	list_del(&addr->anode);
 	ipvlan->ipv6cnt--;
 	WARN_ON(ipvlan->ipv6cnt < 0);
 	kfree_rcu(addr, rcu);
@@ -675,7 +679,7 @@ static int ipvlan_add_addr4(struct ipvl_dev *ipvlan, struct in_addr *ip4_addr)
 {
 	struct ipvl_addr *addr;
 
-	if (ipvlan_addr_busy(ipvlan, ip4_addr, false)) {
+	if (ipvlan_addr_busy(ipvlan->port, ip4_addr, false)) {
 		netif_err(ipvlan, ifup, ipvlan->dev,
 			  "Failed to add IPv4=%pI4 on %s intf.\n",
 			  ip4_addr, ipvlan->dev->name);
@@ -688,9 +692,13 @@ static int ipvlan_add_addr4(struct ipvl_dev *ipvlan, struct in_addr *ip4_addr)
 	addr->master = ipvlan;
 	memcpy(&addr->ip4addr, ip4_addr, sizeof(struct in_addr));
 	addr->atype = IPVL_IPV4;
-	list_add_tail_rcu(&addr->anode, &ipvlan->addrs);
+	list_add_tail(&addr->anode, &ipvlan->addrs);
 	ipvlan->ipv4cnt++;
-	ipvlan_ht_addr_add(ipvlan, addr);
+	/* If the interface is not up, the address will be added to the hash
+	 * list by ipvlan_open.
+	 */
+	if (netif_running(ipvlan->dev))
+		ipvlan_ht_addr_add(ipvlan, addr);
 	ipvlan_set_broadcast_mac_filter(ipvlan, true);
 
 	return 0;
@@ -700,12 +708,12 @@ static void ipvlan_del_addr4(struct ipvl_dev *ipvlan, struct in_addr *ip4_addr)
 {
 	struct ipvl_addr *addr;
 
-	addr = ipvlan_ht_addr_lookup(ipvlan->port, ip4_addr, false);
+	addr = ipvlan_find_addr(ipvlan, ip4_addr, false);
 	if (!addr)
 		return;
 
 	ipvlan_ht_addr_del(addr, true);
-	list_del_rcu(&addr->anode);
+	list_del(&addr->anode);
 	ipvlan->ipv4cnt--;
 	WARN_ON(ipvlan->ipv4cnt < 0);
 	if (!ipvlan->ipv4cnt)
diff --git a/drivers/net/macvtap.c b/drivers/net/macvtap.c
index e40fdfccc9c1..27ecc5c4fa26 100644
--- a/drivers/net/macvtap.c
+++ b/drivers/net/macvtap.c
@@ -654,11 +654,14 @@ static void macvtap_skb_to_vnet_hdr(struct macvtap_queue *q,
 	} /* else everything is zero */
 }
 
+/* Neighbour code has some assumptions on HH_DATA_MOD alignment */
+#define MACVTAP_RESERVE HH_DATA_OFF(ETH_HLEN)
+
 /* Get packet from user space buffer */
 static ssize_t macvtap_get_user(struct macvtap_queue *q, struct msghdr *m,
 				struct iov_iter *from, int noblock)
 {
-	int good_linear = SKB_MAX_HEAD(NET_IP_ALIGN);
+	int good_linear = SKB_MAX_HEAD(MACVTAP_RESERVE);
 	struct sk_buff *skb;
 	struct macvlan_dev *vlan;
 	unsigned long total_len = iov_iter_count(from);
@@ -722,7 +725,7 @@ static ssize_t macvtap_get_user(struct macvtap_queue *q, struct msghdr *m,
 			linear = macvtap16_to_cpu(q, vnet_hdr.hdr_len);
 	}
 
-	skb = macvtap_alloc_skb(&q->sk, NET_IP_ALIGN, copylen,
+	skb = macvtap_alloc_skb(&q->sk, MACVTAP_RESERVE, copylen,
 				linear, noblock, &err);
 	if (!skb)
 		goto err;
diff --git a/drivers/net/phy/amd-xgbe-phy.c b/drivers/net/phy/amd-xgbe-phy.c
index 9e3af54c9010..32efbd48f326 100644
--- a/drivers/net/phy/amd-xgbe-phy.c
+++ b/drivers/net/phy/amd-xgbe-phy.c
@@ -92,6 +92,8 @@ MODULE_DESCRIPTION("AMD 10GbE (amd-xgbe) PHY driver");
 #define XGBE_PHY_CDR_RATE_PROPERTY	"amd,serdes-cdr-rate"
 #define XGBE_PHY_PQ_SKEW_PROPERTY	"amd,serdes-pq-skew"
 #define XGBE_PHY_TX_AMP_PROPERTY	"amd,serdes-tx-amp"
+#define XGBE_PHY_DFE_CFG_PROPERTY	"amd,serdes-dfe-tap-config"
+#define XGBE_PHY_DFE_ENA_PROPERTY	"amd,serdes-dfe-tap-enable"
 
 #define XGBE_PHY_SPEEDS			3
 #define XGBE_PHY_SPEED_1000		0
@@ -177,10 +179,12 @@ MODULE_DESCRIPTION("AMD 10GbE (amd-xgbe) PHY driver");
 #define SPEED_10000_BLWC		0
 #define SPEED_10000_CDR			0x7
 #define SPEED_10000_PLL			0x1
-#define SPEED_10000_PQ			0x1e
+#define SPEED_10000_PQ			0x12
 #define SPEED_10000_RATE		0x0
 #define SPEED_10000_TXAMP		0xa
 #define SPEED_10000_WORD		0x7
+#define SPEED_10000_DFE_TAP_CONFIG	0x1
+#define SPEED_10000_DFE_TAP_ENABLE	0x7f
 
 #define SPEED_2500_BLWC			1
 #define SPEED_2500_CDR			0x2
@@ -189,6 +193,8 @@ MODULE_DESCRIPTION("AMD 10GbE (amd-xgbe) PHY driver");
 #define SPEED_2500_RATE			0x1
 #define SPEED_2500_TXAMP		0xf
 #define SPEED_2500_WORD			0x1
+#define SPEED_2500_DFE_TAP_CONFIG	0x3
+#define SPEED_2500_DFE_TAP_ENABLE	0x0
 
 #define SPEED_1000_BLWC			1
 #define SPEED_1000_CDR			0x2
@@ -197,16 +203,25 @@ MODULE_DESCRIPTION("AMD 10GbE (amd-xgbe) PHY driver");
 #define SPEED_1000_RATE			0x3
 #define SPEED_1000_TXAMP		0xf
 #define SPEED_1000_WORD			0x1
+#define SPEED_1000_DFE_TAP_CONFIG	0x3
+#define SPEED_1000_DFE_TAP_ENABLE	0x0
 
 /* SerDes RxTx register offsets */
+#define RXTX_REG6			0x0018
 #define RXTX_REG20			0x0050
+#define RXTX_REG22			0x0058
 #define RXTX_REG114			0x01c8
+#define RXTX_REG129			0x0204
 
 /* SerDes RxTx register entry bit positions and sizes */
+#define RXTX_REG6_RESETB_RXD_INDEX	8
+#define RXTX_REG6_RESETB_RXD_WIDTH	1
 #define RXTX_REG20_BLWC_ENA_INDEX	2
 #define RXTX_REG20_BLWC_ENA_WIDTH	1
 #define RXTX_REG114_PQ_REG_INDEX	9
 #define RXTX_REG114_PQ_REG_WIDTH	7
+#define RXTX_REG129_RXDFE_CONFIG_INDEX	14
+#define RXTX_REG129_RXDFE_CONFIG_WIDTH	2
 
 /* Bit setting and getting macros
  *  The get macro will extract the current bit field value from within
@@ -333,6 +348,18 @@ static const u32 amd_xgbe_phy_serdes_tx_amp[] = {
 	SPEED_10000_TXAMP,
 };
 
+static const u32 amd_xgbe_phy_serdes_dfe_tap_cfg[] = {
+	SPEED_1000_DFE_TAP_CONFIG,
+	SPEED_2500_DFE_TAP_CONFIG,
+	SPEED_10000_DFE_TAP_CONFIG,
+};
+
+static const u32 amd_xgbe_phy_serdes_dfe_tap_ena[] = {
+	SPEED_1000_DFE_TAP_ENABLE,
+	SPEED_2500_DFE_TAP_ENABLE,
+	SPEED_10000_DFE_TAP_ENABLE,
+};
+
 enum amd_xgbe_phy_an {
 	AMD_XGBE_AN_READY = 0,
 	AMD_XGBE_AN_PAGE_RECEIVED,
@@ -393,6 +420,8 @@ struct amd_xgbe_phy_priv {
 	u32 serdes_cdr_rate[XGBE_PHY_SPEEDS];
 	u32 serdes_pq_skew[XGBE_PHY_SPEEDS];
 	u32 serdes_tx_amp[XGBE_PHY_SPEEDS];
+	u32 serdes_dfe_tap_cfg[XGBE_PHY_SPEEDS];
+	u32 serdes_dfe_tap_ena[XGBE_PHY_SPEEDS];
 
 	/* Auto-negotiation state machine support */
 	struct mutex an_mutex;
@@ -481,11 +510,16 @@ static void amd_xgbe_phy_serdes_complete_ratechange(struct phy_device *phydev)
 		status = XSIR0_IOREAD(priv, SIR0_STATUS);
 		if (XSIR_GET_BITS(status, SIR0_STATUS, RX_READY) &&
 		    XSIR_GET_BITS(status, SIR0_STATUS, TX_READY))
-			return;
+			goto rx_reset;
 	}
 
 	netdev_dbg(phydev->attached_dev, "SerDes rx/tx not ready (%#hx)\n",
 		   status);
+
+rx_reset:
+	/* Perform Rx reset for the DFE changes */
+	XRXTX_IOWRITE_BITS(priv, RXTX_REG6, RESETB_RXD, 0);
+	XRXTX_IOWRITE_BITS(priv, RXTX_REG6, RESETB_RXD, 1);
 }
 
 static int amd_xgbe_phy_xgmii_mode(struct phy_device *phydev)
@@ -534,6 +568,10 @@ static int amd_xgbe_phy_xgmii_mode(struct phy_device *phydev)
 			   priv->serdes_blwc[XGBE_PHY_SPEED_10000]);
 	XRXTX_IOWRITE_BITS(priv, RXTX_REG114, PQ_REG,
 			   priv->serdes_pq_skew[XGBE_PHY_SPEED_10000]);
+	XRXTX_IOWRITE_BITS(priv, RXTX_REG129, RXDFE_CONFIG,
+			   priv->serdes_dfe_tap_cfg[XGBE_PHY_SPEED_10000]);
+	XRXTX_IOWRITE(priv, RXTX_REG22,
+		      priv->serdes_dfe_tap_ena[XGBE_PHY_SPEED_10000]);
 
 	amd_xgbe_phy_serdes_complete_ratechange(phydev);
 
@@ -586,6 +624,10 @@ static int amd_xgbe_phy_gmii_2500_mode(struct phy_device *phydev)
 			   priv->serdes_blwc[XGBE_PHY_SPEED_2500]);
 	XRXTX_IOWRITE_BITS(priv, RXTX_REG114, PQ_REG,
 			   priv->serdes_pq_skew[XGBE_PHY_SPEED_2500]);
+	XRXTX_IOWRITE_BITS(priv, RXTX_REG129, RXDFE_CONFIG,
+			   priv->serdes_dfe_tap_cfg[XGBE_PHY_SPEED_2500]);
+	XRXTX_IOWRITE(priv, RXTX_REG22,
+		      priv->serdes_dfe_tap_ena[XGBE_PHY_SPEED_2500]);
 
 	amd_xgbe_phy_serdes_complete_ratechange(phydev);
 
@@ -638,6 +680,10 @@ static int amd_xgbe_phy_gmii_mode(struct phy_device *phydev)
 			   priv->serdes_blwc[XGBE_PHY_SPEED_1000]);
 	XRXTX_IOWRITE_BITS(priv, RXTX_REG114, PQ_REG,
 			   priv->serdes_pq_skew[XGBE_PHY_SPEED_1000]);
+	XRXTX_IOWRITE_BITS(priv, RXTX_REG129, RXDFE_CONFIG,
+			   priv->serdes_dfe_tap_cfg[XGBE_PHY_SPEED_1000]);
+	XRXTX_IOWRITE(priv, RXTX_REG22,
+		      priv->serdes_dfe_tap_ena[XGBE_PHY_SPEED_1000]);
 
 	amd_xgbe_phy_serdes_complete_ratechange(phydev);
 
@@ -1668,6 +1714,38 @@ static int amd_xgbe_phy_probe(struct phy_device *phydev)
 		       sizeof(priv->serdes_tx_amp));
 	}
 
+	if (device_property_present(phy_dev, XGBE_PHY_DFE_CFG_PROPERTY)) {
+		ret = device_property_read_u32_array(phy_dev,
+						     XGBE_PHY_DFE_CFG_PROPERTY,
+						     priv->serdes_dfe_tap_cfg,
+						     XGBE_PHY_SPEEDS);
+		if (ret) {
+			dev_err(dev, "invalid %s property\n",
+				XGBE_PHY_DFE_CFG_PROPERTY);
+			goto err_sir1;
+		}
+	} else {
+		memcpy(priv->serdes_dfe_tap_cfg,
+		       amd_xgbe_phy_serdes_dfe_tap_cfg,
+		       sizeof(priv->serdes_dfe_tap_cfg));
+	}
+
+	if (device_property_present(phy_dev, XGBE_PHY_DFE_ENA_PROPERTY)) {
+		ret = device_property_read_u32_array(phy_dev,
+						     XGBE_PHY_DFE_ENA_PROPERTY,
+						     priv->serdes_dfe_tap_ena,
+						     XGBE_PHY_SPEEDS);
+		if (ret) {
+			dev_err(dev, "invalid %s property\n",
+				XGBE_PHY_DFE_ENA_PROPERTY);
+			goto err_sir1;
+		}
+	} else {
+		memcpy(priv->serdes_dfe_tap_ena,
+		       amd_xgbe_phy_serdes_dfe_tap_ena,
+		       sizeof(priv->serdes_dfe_tap_ena));
+	}
+
 	phydev->priv = priv;
 
 	if (!priv->adev || acpi_disabled)
diff --git a/drivers/net/phy/phy.c b/drivers/net/phy/phy.c
index cdcac6aa4260..52cd8db2c57d 100644
--- a/drivers/net/phy/phy.c
+++ b/drivers/net/phy/phy.c
@@ -236,6 +236,25 @@ static inline unsigned int phy_find_valid(unsigned int idx, u32 features)
 }
 
 /**
+ * phy_check_valid - check if there is a valid PHY setting which matches
+ *		     speed, duplex, and feature mask
+ * @speed: speed to match
+ * @duplex: duplex to match
+ * @features: A mask of the valid settings
+ *
+ * Description: Returns true if there is a valid setting, false otherwise.
+ */
+static inline bool phy_check_valid(int speed, int duplex, u32 features)
+{
+	unsigned int idx;
+
+	idx = phy_find_valid(phy_find_setting(speed, duplex), features);
+
+	return settings[idx].speed == speed && settings[idx].duplex == duplex &&
+		(settings[idx].setting & features);
+}
+
+/**
  * phy_sanitize_settings - make sure the PHY is set to supported speed and duplex
  * @phydev: the target phy_device struct
  *
@@ -1045,7 +1064,6 @@ int phy_init_eee(struct phy_device *phydev, bool clk_stop_enable)
 		int eee_lp, eee_cap, eee_adv;
 		u32 lp, cap, adv;
 		int status;
-		unsigned int idx;
 
 		/* Read phy status to properly get the right settings */
 		status = phy_read_status(phydev);
@@ -1077,8 +1095,7 @@ int phy_init_eee(struct phy_device *phydev, bool clk_stop_enable)
 
 		adv = mmd_eee_adv_to_ethtool_adv_t(eee_adv);
 		lp = mmd_eee_adv_to_ethtool_adv_t(eee_lp);
-		idx = phy_find_setting(phydev->speed, phydev->duplex);
-		if (!(lp & adv & settings[idx].setting))
+		if (!phy_check_valid(phydev->speed, phydev->duplex, lp & adv))
 			goto eee_exit_err;
 
 		if (clk_stop_enable) {
diff --git a/drivers/net/team/team.c b/drivers/net/team/team.c
index 0e62274e884a..7d394846afc2 100644
--- a/drivers/net/team/team.c
+++ b/drivers/net/team/team.c
@@ -43,9 +43,7 @@
 
 static struct team_port *team_port_get_rcu(const struct net_device *dev)
 {
-	struct team_port *port = rcu_dereference(dev->rx_handler_data);
-
-	return team_port_exists(dev) ? port : NULL;
+	return rcu_dereference(dev->rx_handler_data);
 }
 
 static struct team_port *team_port_get_rtnl(const struct net_device *dev)
@@ -1732,11 +1730,11 @@ static int team_set_mac_address(struct net_device *dev, void *p)
 	if (dev->type == ARPHRD_ETHER && !is_valid_ether_addr(addr->sa_data))
 		return -EADDRNOTAVAIL;
 	memcpy(dev->dev_addr, addr->sa_data, dev->addr_len);
-	rcu_read_lock();
-	list_for_each_entry_rcu(port, &team->port_list, list)
+	mutex_lock(&team->lock);
+	list_for_each_entry(port, &team->port_list, list)
 		if (team->ops.port_change_dev_addr)
 			team->ops.port_change_dev_addr(team, port);
-	rcu_read_unlock();
+	mutex_unlock(&team->lock);
 	return 0;
 }
 
diff --git a/drivers/net/usb/Kconfig b/drivers/net/usb/Kconfig
index 3bd9678315ad..7ba8d0885f12 100644
--- a/drivers/net/usb/Kconfig
+++ b/drivers/net/usb/Kconfig
@@ -161,6 +161,7 @@ config USB_NET_AX8817X
 	    * Linksys USB200M
 	    * Netgear FA120
 	    * Sitecom LN-029
+	    * Sitecom LN-028
 	    * Intellinet USB 2.0 Ethernet
 	    * ST Lab USB 2.0 Ethernet
 	    * TrendNet TU2-ET100
diff --git a/drivers/net/usb/asix_common.c b/drivers/net/usb/asix_common.c
index 5c55f11572ba..75d6f26729a3 100644
--- a/drivers/net/usb/asix_common.c
+++ b/drivers/net/usb/asix_common.c
@@ -188,6 +188,8 @@ struct sk_buff *asix_tx_fixup(struct usbnet *dev, struct sk_buff *skb,
 		memcpy(skb_tail_pointer(skb), &padbytes, sizeof(padbytes));
 		skb_put(skb, sizeof(padbytes));
 	}
+
+	usbnet_set_skb_tx_stats(skb, 1, 0);
 	return skb;
 }
 
diff --git a/drivers/net/usb/asix_devices.c b/drivers/net/usb/asix_devices.c
index bf49792062a2..1173a24feda3 100644
--- a/drivers/net/usb/asix_devices.c
+++ b/drivers/net/usb/asix_devices.c
@@ -979,6 +979,10 @@ static const struct usb_device_id	products [] = {
 	USB_DEVICE (0x0df6, 0x0056),
 	.driver_info =  (unsigned long) &ax88178_info,
 }, {
+	// Sitecom LN-028 "USB 2.0 10/100/1000 Ethernet adapter"
+	USB_DEVICE (0x0df6, 0x061c),
+	.driver_info =  (unsigned long) &ax88178_info,
+}, {
 	// corega FEther USB2-TX
 	USB_DEVICE (0x07aa, 0x0017),
 	.driver_info =  (unsigned long) &ax8817x_info,
diff --git a/drivers/net/usb/cdc_ether.c b/drivers/net/usb/cdc_ether.c
index 9311a08565be..4545e78840b0 100644
--- a/drivers/net/usb/cdc_ether.c
+++ b/drivers/net/usb/cdc_ether.c
@@ -522,6 +522,7 @@ static const struct driver_info wwan_info = {
 #define DELL_VENDOR_ID		0x413C
 #define REALTEK_VENDOR_ID	0x0bda
 #define SAMSUNG_VENDOR_ID	0x04e8
+#define LENOVO_VENDOR_ID	0x17ef
 
 static const struct usb_device_id	products[] = {
 /* BLACKLIST !!
@@ -702,6 +703,13 @@ static const struct usb_device_id	products[] = {
 	.driver_info = 0,
 },
 
+/* Lenovo Thinkpad USB 3.0 Ethernet Adapters (based on Realtek RTL8153) */
+{
+	USB_DEVICE_AND_INTERFACE_INFO(LENOVO_VENDOR_ID, 0x7205, USB_CLASS_COMM,
+			USB_CDC_SUBCLASS_ETHERNET, USB_CDC_PROTO_NONE),
+	.driver_info = 0,
+},
+
 /* WHITELIST!!!
  *
  * CDC Ether uses two interfaces, not necessarily consecutive.
diff --git a/drivers/net/usb/cdc_ncm.c b/drivers/net/usb/cdc_ncm.c
index 80a844e0ae03..c3e4da9e79ca 100644
--- a/drivers/net/usb/cdc_ncm.c
+++ b/drivers/net/usb/cdc_ncm.c
@@ -1172,17 +1172,17 @@ cdc_ncm_fill_tx_frame(struct usbnet *dev, struct sk_buff *skb, __le32 sign)
 
 	/* return skb */
 	ctx->tx_curr_skb = NULL;
-	dev->net->stats.tx_packets += ctx->tx_curr_frame_num;
 
 	/* keep private stats: framing overhead and number of NTBs */
 	ctx->tx_overhead += skb_out->len - ctx->tx_curr_frame_payload;
 	ctx->tx_ntbs++;
 
-	/* usbnet has already counted all the framing overhead.
+	/* usbnet will count all the framing overhead by default.
 	 * Adjust the stats so that the tx_bytes counter show real
 	 * payload data instead.
 	 */
-	dev->net->stats.tx_bytes -= skb_out->len - ctx->tx_curr_frame_payload;
+	usbnet_set_skb_tx_stats(skb_out, n,
+				ctx->tx_curr_frame_payload - skb_out->len);
 
 	return skb_out;
 
diff --git a/drivers/net/usb/cx82310_eth.c b/drivers/net/usb/cx82310_eth.c
index 3eed708a6182..1762ad3910b2 100644
--- a/drivers/net/usb/cx82310_eth.c
+++ b/drivers/net/usb/cx82310_eth.c
@@ -46,8 +46,7 @@ enum cx82310_status {
 };
 
 #define CMD_PACKET_SIZE	64
-/* first command after power on can take around 8 seconds */
-#define CMD_TIMEOUT	15000
+#define CMD_TIMEOUT	100
 #define CMD_REPLY_RETRY 5
 
 #define CX82310_MTU	1514
@@ -78,8 +77,9 @@ static int cx82310_cmd(struct usbnet *dev, enum cx82310_cmd cmd, bool reply,
 	ret = usb_bulk_msg(udev, usb_sndbulkpipe(udev, CMD_EP), buf,
 			   CMD_PACKET_SIZE, &actual_len, CMD_TIMEOUT);
 	if (ret < 0) {
-		dev_err(&dev->udev->dev, "send command %#x: error %d\n",
-			cmd, ret);
+		if (cmd != CMD_GET_LINK_STATUS)
+			dev_err(&dev->udev->dev, "send command %#x: error %d\n",
+				cmd, ret);
 		goto end;
 	}
 
@@ -90,8 +90,10 @@ static int cx82310_cmd(struct usbnet *dev, enum cx82310_cmd cmd, bool reply,
 					   buf, CMD_PACKET_SIZE, &actual_len,
 					   CMD_TIMEOUT);
 			if (ret < 0) {
-				dev_err(&dev->udev->dev,
-					"reply receive error %d\n", ret);
+				if (cmd != CMD_GET_LINK_STATUS)
+					dev_err(&dev->udev->dev,
+						"reply receive error %d\n",
+						ret);
 				goto end;
 			}
 			if (actual_len > 0)
@@ -134,6 +136,8 @@ static int cx82310_bind(struct usbnet *dev, struct usb_interface *intf)
 	int ret;
 	char buf[15];
 	struct usb_device *udev = dev->udev;
+	u8 link[3];
+	int timeout = 50;
 
 	/* avoid ADSL modems - continue only if iProduct is "USB NET CARD" */
 	if (usb_string(udev, udev->descriptor.iProduct, buf, sizeof(buf)) > 0
@@ -160,6 +164,20 @@ static int cx82310_bind(struct usbnet *dev, struct usb_interface *intf)
 	if (!dev->partial_data)
 		return -ENOMEM;
 
+	/* wait for firmware to become ready (indicated by the link being up) */
+	while (--timeout) {
+		ret = cx82310_cmd(dev, CMD_GET_LINK_STATUS, true, NULL, 0,
+				  link, sizeof(link));
+		/* the command can time out during boot - it's not an error */
+		if (!ret && link[0] == 1 && link[2] == 1)
+			break;
+		msleep(500);
+	};
+	if (!timeout) {
+		dev_err(&udev->dev, "firmware not ready in time\n");
+		return -ETIMEDOUT;
+	}
+
 	/* enable ethernet mode (?) */
 	ret = cx82310_cmd(dev, CMD_ETHERNET_MODE, true, "\x01", 1, NULL, 0);
 	if (ret) {
@@ -300,9 +318,18 @@ static const struct driver_info	cx82310_info = {
 	.tx_fixup	= cx82310_tx_fixup,
 };
 
+#define USB_DEVICE_CLASS(vend, prod, cl, sc, pr) \
+	.match_flags = USB_DEVICE_ID_MATCH_DEVICE | \
+		       USB_DEVICE_ID_MATCH_DEV_INFO, \
+	.idVendor = (vend), \
+	.idProduct = (prod), \
+	.bDeviceClass = (cl), \
+	.bDeviceSubClass = (sc), \
+	.bDeviceProtocol = (pr)
+
 static const struct usb_device_id products[] = {
 	{
-		USB_DEVICE_AND_INTERFACE_INFO(0x0572, 0xcb01, 0xff, 0, 0),
+		USB_DEVICE_CLASS(0x0572, 0xcb01, 0xff, 0, 0),
 		.driver_info = (unsigned long) &cx82310_info
 	},
 	{ },
diff --git a/drivers/net/usb/hso.c b/drivers/net/usb/hso.c
index 9cdfb3fe9c15..778e91531fac 100644
--- a/drivers/net/usb/hso.c
+++ b/drivers/net/usb/hso.c
@@ -1594,7 +1594,7 @@ hso_wait_modem_status(struct hso_serial *serial, unsigned long arg)
 		}
 		cprev = cnow;
 	}
-	current->state = TASK_RUNNING;
+	__set_current_state(TASK_RUNNING);
 	remove_wait_queue(&tiocmget->waitq, &wait);
 
 	return ret;
diff --git a/drivers/net/usb/plusb.c b/drivers/net/usb/plusb.c
index 3d18bb0eee85..1bfe0fcaccf5 100644
--- a/drivers/net/usb/plusb.c
+++ b/drivers/net/usb/plusb.c
@@ -134,6 +134,11 @@ static const struct usb_device_id	products [] = {
 }, {
 	USB_DEVICE(0x050d, 0x258a),     /* Belkin F5U258/F5U279 (PL-25A1) */
 	.driver_info =  (unsigned long) &prolific_info,
+}, {
+	USB_DEVICE(0x3923, 0x7825),     /* National Instruments USB
+					 * Host-to-Host Cable
+					 */
+	.driver_info =  (unsigned long) &prolific_info,
 },
 
 	{ },		// END
diff --git a/drivers/net/usb/r8152.c b/drivers/net/usb/r8152.c
index 438fc6bcaef1..9f7c0ab3b349 100644
--- a/drivers/net/usb/r8152.c
+++ b/drivers/net/usb/r8152.c
@@ -492,6 +492,7 @@ enum rtl8152_flags {
 /* Define these values to match your device */
 #define VENDOR_ID_REALTEK		0x0bda
 #define VENDOR_ID_SAMSUNG		0x04e8
+#define VENDOR_ID_LENOVO		0x17ef
 
 #define MCU_TYPE_PLA			0x0100
 #define MCU_TYPE_USB			0x0000
@@ -4037,6 +4038,7 @@ static struct usb_device_id rtl8152_table[] = {
 	{REALTEK_USB_DEVICE(VENDOR_ID_REALTEK, 0x8152)},
 	{REALTEK_USB_DEVICE(VENDOR_ID_REALTEK, 0x8153)},
 	{REALTEK_USB_DEVICE(VENDOR_ID_SAMSUNG, 0xa101)},
+	{REALTEK_USB_DEVICE(VENDOR_ID_LENOVO,  0x7205)},
 	{}
 };
 
diff --git a/drivers/net/usb/sr9800.c b/drivers/net/usb/sr9800.c
index b94a0fbb8b3b..953de13267df 100644
--- a/drivers/net/usb/sr9800.c
+++ b/drivers/net/usb/sr9800.c
@@ -144,6 +144,7 @@ static struct sk_buff *sr_tx_fixup(struct usbnet *dev, struct sk_buff *skb,
 		skb_put(skb, sizeof(padbytes));
 	}
 
+	usbnet_set_skb_tx_stats(skb, 1, 0);
 	return skb;
 }
 
diff --git a/drivers/net/usb/usbnet.c b/drivers/net/usb/usbnet.c
index 449835f4331e..777757ae1973 100644
--- a/drivers/net/usb/usbnet.c
+++ b/drivers/net/usb/usbnet.c
@@ -1188,8 +1188,7 @@ static void tx_complete (struct urb *urb)
 	struct usbnet		*dev = entry->dev;
 
 	if (urb->status == 0) {
-		if (!(dev->driver_info->flags & FLAG_MULTI_PACKET))
-			dev->net->stats.tx_packets++;
+		dev->net->stats.tx_packets += entry->packets;
 		dev->net->stats.tx_bytes += entry->length;
 	} else {
 		dev->net->stats.tx_errors++;
@@ -1347,7 +1346,19 @@ netdev_tx_t usbnet_start_xmit (struct sk_buff *skb,
 		} else
 			urb->transfer_flags |= URB_ZERO_PACKET;
 	}
-	entry->length = urb->transfer_buffer_length = length;
+	urb->transfer_buffer_length = length;
+
+	if (info->flags & FLAG_MULTI_PACKET) {
+		/* Driver has set number of packets and a length delta.
+		 * Calculate the complete length and ensure that it's
+		 * positive.
+		 */
+		entry->length += length;
+		if (WARN_ON_ONCE(entry->length <= 0))
+			entry->length = length;
+	} else {
+		usbnet_set_skb_tx_stats(skb, 1, length);
+	}
 
 	spin_lock_irqsave(&dev->txq.lock, flags);
 	retval = usb_autopm_get_interface_async(dev->intf);
diff --git a/drivers/net/virtio_net.c b/drivers/net/virtio_net.c
index f1ff3666f090..59b0e9754ae3 100644
--- a/drivers/net/virtio_net.c
+++ b/drivers/net/virtio_net.c
@@ -1448,8 +1448,10 @@ static void virtnet_free_queues(struct virtnet_info *vi)
 {
 	int i;
 
-	for (i = 0; i < vi->max_queue_pairs; i++)
+	for (i = 0; i < vi->max_queue_pairs; i++) {
+		napi_hash_del(&vi->rq[i].napi);
 		netif_napi_del(&vi->rq[i].napi);
+	}
 
 	kfree(vi->rq);
 	kfree(vi->sq);
@@ -1948,11 +1950,8 @@ static int virtnet_freeze(struct virtio_device *vdev)
 	cancel_delayed_work_sync(&vi->refill);
 
 	if (netif_running(vi->dev)) {
-		for (i = 0; i < vi->max_queue_pairs; i++) {
+		for (i = 0; i < vi->max_queue_pairs; i++)
 			napi_disable(&vi->rq[i].napi);
-			napi_hash_del(&vi->rq[i].napi);
-			netif_napi_del(&vi->rq[i].napi);
-		}
 	}
 
 	remove_vq_common(vi);
diff --git a/drivers/net/vxlan.c b/drivers/net/vxlan.c
index 1e0a775ea882..f8528a4cf54f 100644
--- a/drivers/net/vxlan.c
+++ b/drivers/net/vxlan.c
@@ -1218,7 +1218,7 @@ static int vxlan_udp_encap_recv(struct sock *sk, struct sk_buff *skb)
 			goto drop;
 
 		flags &= ~VXLAN_HF_RCO;
-		vni &= VXLAN_VID_MASK;
+		vni &= VXLAN_VNI_MASK;
 	}
 
 	/* For backwards compatibility, only allow reserved fields to be
@@ -1239,7 +1239,7 @@ static int vxlan_udp_encap_recv(struct sock *sk, struct sk_buff *skb)
 		flags &= ~VXLAN_GBP_USED_BITS;
 	}
 
-	if (flags || (vni & ~VXLAN_VID_MASK)) {
+	if (flags || vni & ~VXLAN_VNI_MASK) {
 		/* If there are any unprocessed flags remaining treat
 		 * this as a malformed packet. This behavior diverges from
 		 * VXLAN RFC (RFC7348) which stipulates that bits in reserved
diff --git a/drivers/net/wan/cosa.c b/drivers/net/wan/cosa.c
index 83c39e2858bf..88d121d43c08 100644
--- a/drivers/net/wan/cosa.c
+++ b/drivers/net/wan/cosa.c
@@ -806,21 +806,21 @@ static ssize_t cosa_read(struct file *file,
 	spin_lock_irqsave(&cosa->lock, flags);
 	add_wait_queue(&chan->rxwaitq, &wait);
 	while (!chan->rx_status) {
-		current->state = TASK_INTERRUPTIBLE;
+		set_current_state(TASK_INTERRUPTIBLE);
 		spin_unlock_irqrestore(&cosa->lock, flags);
 		schedule();
 		spin_lock_irqsave(&cosa->lock, flags);
 		if (signal_pending(current) && chan->rx_status == 0) {
 			chan->rx_status = 1;
 			remove_wait_queue(&chan->rxwaitq, &wait);
-			current->state = TASK_RUNNING;
+			__set_current_state(TASK_RUNNING);
 			spin_unlock_irqrestore(&cosa->lock, flags);
 			mutex_unlock(&chan->rlock);
 			return -ERESTARTSYS;
 		}
 	}
 	remove_wait_queue(&chan->rxwaitq, &wait);
-	current->state = TASK_RUNNING;
+	__set_current_state(TASK_RUNNING);
 	kbuf = chan->rxdata;
 	count = chan->rxsize;
 	spin_unlock_irqrestore(&cosa->lock, flags);
@@ -890,14 +890,14 @@ static ssize_t cosa_write(struct file *file,
 	spin_lock_irqsave(&cosa->lock, flags);
 	add_wait_queue(&chan->txwaitq, &wait);
 	while (!chan->tx_status) {
-		current->state = TASK_INTERRUPTIBLE;
+		set_current_state(TASK_INTERRUPTIBLE);
 		spin_unlock_irqrestore(&cosa->lock, flags);
 		schedule();
 		spin_lock_irqsave(&cosa->lock, flags);
 		if (signal_pending(current) && chan->tx_status == 0) {
 			chan->tx_status = 1;
 			remove_wait_queue(&chan->txwaitq, &wait);
-			current->state = TASK_RUNNING;
+			__set_current_state(TASK_RUNNING);
 			chan->tx_status = 1;
 			spin_unlock_irqrestore(&cosa->lock, flags);
 			up(&chan->wsem);
@@ -905,7 +905,7 @@ static ssize_t cosa_write(struct file *file,
 		}
 	}
 	remove_wait_queue(&chan->txwaitq, &wait);
-	current->state = TASK_RUNNING;
+	__set_current_state(TASK_RUNNING);
 	up(&chan->wsem);
 	spin_unlock_irqrestore(&cosa->lock, flags);
 	kfree(kbuf);
diff --git a/drivers/net/wireless/ath/ath9k/beacon.c b/drivers/net/wireless/ath/ath9k/beacon.c
index cb366adc820b..f50a6bc5d06e 100644
--- a/drivers/net/wireless/ath/ath9k/beacon.c
+++ b/drivers/net/wireless/ath/ath9k/beacon.c
@@ -219,12 +219,15 @@ void ath9k_beacon_remove_slot(struct ath_softc *sc, struct ieee80211_vif *vif)
 	struct ath_common *common = ath9k_hw_common(sc->sc_ah);
 	struct ath_vif *avp = (void *)vif->drv_priv;
 	struct ath_buf *bf = avp->av_bcbuf;
+	struct ath_beacon_config *cur_conf = &sc->cur_chan->beacon;
 
 	ath_dbg(common, CONFIG, "Removing interface at beacon slot: %d\n",
 		avp->av_bslot);
 
 	tasklet_disable(&sc->bcon_tasklet);
 
+	cur_conf->enable_beacon &= ~BIT(avp->av_bslot);
+
 	if (bf && bf->bf_mpdu) {
 		struct sk_buff *skb = bf->bf_mpdu;
 		dma_unmap_single(sc->dev, bf->bf_buf_addr,
@@ -521,8 +524,7 @@ static bool ath9k_allow_beacon_config(struct ath_softc *sc,
 	}
 
 	if (sc->sc_ah->opmode == NL80211_IFTYPE_AP) {
-		if ((vif->type != NL80211_IFTYPE_AP) ||
-		    (sc->nbcnvifs > 1)) {
+		if (vif->type != NL80211_IFTYPE_AP) {
 			ath_dbg(common, CONFIG,
 				"An AP interface is already present !\n");
 			return false;
@@ -616,12 +618,14 @@ void ath9k_beacon_config(struct ath_softc *sc, struct ieee80211_vif *vif,
 	 * enabling/disabling SWBA.
 	 */
 	if (changed & BSS_CHANGED_BEACON_ENABLED) {
-		if (!bss_conf->enable_beacon &&
-		    (sc->nbcnvifs <= 1)) {
-			cur_conf->enable_beacon = false;
-		} else if (bss_conf->enable_beacon) {
-			cur_conf->enable_beacon = true;
-			ath9k_cache_beacon_config(sc, ctx, bss_conf);
+		bool enabled = cur_conf->enable_beacon;
+
+		if (!bss_conf->enable_beacon) {
+			cur_conf->enable_beacon &= ~BIT(avp->av_bslot);
+		} else {
+			cur_conf->enable_beacon |= BIT(avp->av_bslot);
+			if (!enabled)
+				ath9k_cache_beacon_config(sc, ctx, bss_conf);
 		}
 	}
 
diff --git a/drivers/net/wireless/ath/ath9k/common.h b/drivers/net/wireless/ath/ath9k/common.h
index 2b79a568e803..d23737342f4f 100644
--- a/drivers/net/wireless/ath/ath9k/common.h
+++ b/drivers/net/wireless/ath/ath9k/common.h
@@ -54,7 +54,7 @@ struct ath_beacon_config {
 	u16 dtim_period;
 	u16 bmiss_timeout;
 	u8 dtim_count;
-	bool enable_beacon;
+	u8 enable_beacon;
 	bool ibss_creator;
 	u32 nexttbtt;
 	u32 intval;
diff --git a/drivers/net/wireless/ath/ath9k/hw.c b/drivers/net/wireless/ath/ath9k/hw.c
index 60aa8d71e753..8529014e1a5e 100644
--- a/drivers/net/wireless/ath/ath9k/hw.c
+++ b/drivers/net/wireless/ath/ath9k/hw.c
@@ -424,7 +424,7 @@ static void ath9k_hw_init_defaults(struct ath_hw *ah)
 	ah->power_mode = ATH9K_PM_UNDEFINED;
 	ah->htc_reset_init = true;
 
-	ah->tpc_enabled = true;
+	ah->tpc_enabled = false;
 
 	ah->ani_function = ATH9K_ANI_ALL;
 	if (!AR_SREV_9300_20_OR_LATER(ah))
diff --git a/drivers/net/wireless/b43/main.c b/drivers/net/wireless/b43/main.c
index ccbdb05b28cd..75345c1e8c34 100644
--- a/drivers/net/wireless/b43/main.c
+++ b/drivers/net/wireless/b43/main.c
@@ -5370,6 +5370,7 @@ static void b43_supported_bands(struct b43_wldev *dev, bool *have_2ghz_phy,
 	case 0x432a: /* BCM4321 */
 	case 0x432d: /* BCM4322 */
 	case 0x4352: /* BCM43222 */
+	case 0x435a: /* BCM43228 */
 	case 0x4333: /* BCM4331 */
 	case 0x43a2: /* BCM4360 */
 	case 0x43b3: /* BCM4352 */
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/feature.c b/drivers/net/wireless/brcm80211/brcmfmac/feature.c
index defb7a44e0bc..7748a1ccf14f 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/feature.c
+++ b/drivers/net/wireless/brcm80211/brcmfmac/feature.c
@@ -126,7 +126,8 @@ void brcmf_feat_attach(struct brcmf_pub *drvr)
 	brcmf_feat_iovar_int_get(ifp, BRCMF_FEAT_MCHAN, "mchan");
 	if (drvr->bus_if->wowl_supported)
 		brcmf_feat_iovar_int_get(ifp, BRCMF_FEAT_WOWL, "wowl");
-	brcmf_feat_iovar_int_set(ifp, BRCMF_FEAT_MBSS, "mbss", 0);
+	if (drvr->bus_if->chip != BRCM_CC_43362_CHIP_ID)
+		brcmf_feat_iovar_int_set(ifp, BRCMF_FEAT_MBSS, "mbss", 0);
 
 	/* set chip related quirks */
 	switch (drvr->bus_if->chip) {
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/vendor.c b/drivers/net/wireless/brcm80211/brcmfmac/vendor.c
index 50cdf7090198..8eff2753abad 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/vendor.c
+++ b/drivers/net/wireless/brcm80211/brcmfmac/vendor.c
@@ -39,13 +39,22 @@ static int brcmf_cfg80211_vndr_cmds_dcmd_handler(struct wiphy *wiphy,
 	void *dcmd_buf = NULL, *wr_pointer;
 	u16 msglen, maxmsglen = PAGE_SIZE - 0x100;
 
-	brcmf_dbg(TRACE, "cmd %x set %d len %d\n", cmdhdr->cmd, cmdhdr->set,
-		  cmdhdr->len);
+	if (len < sizeof(*cmdhdr)) {
+		brcmf_err("vendor command too short: %d\n", len);
+		return -EINVAL;
+	}
 
 	vif = container_of(wdev, struct brcmf_cfg80211_vif, wdev);
 	ifp = vif->ifp;
 
-	len -= sizeof(struct brcmf_vndr_dcmd_hdr);
+	brcmf_dbg(TRACE, "ifidx=%d, cmd=%d\n", ifp->ifidx, cmdhdr->cmd);
+
+	if (cmdhdr->offset > len) {
+		brcmf_err("bad buffer offset %d > %d\n", cmdhdr->offset, len);
+		return -EINVAL;
+	}
+
+	len -= cmdhdr->offset;
 	ret_len = cmdhdr->len;
 	if (ret_len > 0 || len > 0) {
 		if (len > BRCMF_DCMD_MAXLEN) {
diff --git a/drivers/net/wireless/iwlwifi/dvm/dev.h b/drivers/net/wireless/iwlwifi/dvm/dev.h
index a6f22c32a279..3811878ab9cd 100644
--- a/drivers/net/wireless/iwlwifi/dvm/dev.h
+++ b/drivers/net/wireless/iwlwifi/dvm/dev.h
@@ -708,7 +708,6 @@ struct iwl_priv {
 	unsigned long reload_jiffies;
 	int reload_count;
 	bool ucode_loaded;
-	bool init_ucode_run;		/* Don't run init uCode again */
 
 	u8 plcp_delta_threshold;
 
diff --git a/drivers/net/wireless/iwlwifi/dvm/mac80211.c b/drivers/net/wireless/iwlwifi/dvm/mac80211.c
index 47e64e8b9517..cceb026e0793 100644
--- a/drivers/net/wireless/iwlwifi/dvm/mac80211.c
+++ b/drivers/net/wireless/iwlwifi/dvm/mac80211.c
@@ -1114,16 +1114,17 @@ static void iwlagn_mac_flush(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
 	scd_queues &= ~(BIT(IWL_IPAN_CMD_QUEUE_NUM) |
 			BIT(IWL_DEFAULT_CMD_QUEUE_NUM));
 
-	if (vif)
-		scd_queues &= ~BIT(vif->hw_queue[IEEE80211_AC_VO]);
-
-	IWL_DEBUG_TX_QUEUES(priv, "Flushing SCD queues: 0x%x\n", scd_queues);
-	if (iwlagn_txfifo_flush(priv, scd_queues)) {
-		IWL_ERR(priv, "flush request fail\n");
-		goto done;
+	if (drop) {
+		IWL_DEBUG_TX_QUEUES(priv, "Flushing SCD queues: 0x%x\n",
+				    scd_queues);
+		if (iwlagn_txfifo_flush(priv, scd_queues)) {
+			IWL_ERR(priv, "flush request fail\n");
+			goto done;
+		}
 	}
+
 	IWL_DEBUG_TX_QUEUES(priv, "wait transmit/flush all frames\n");
-	iwl_trans_wait_tx_queue_empty(priv->trans, 0xffffffff);
+	iwl_trans_wait_tx_queue_empty(priv->trans, scd_queues);
 done:
 	mutex_unlock(&priv->mutex);
 	IWL_DEBUG_MAC80211(priv, "leave\n");
diff --git a/drivers/net/wireless/iwlwifi/dvm/ucode.c b/drivers/net/wireless/iwlwifi/dvm/ucode.c
index 4dbef7e58c2e..5244e43bfafb 100644
--- a/drivers/net/wireless/iwlwifi/dvm/ucode.c
+++ b/drivers/net/wireless/iwlwifi/dvm/ucode.c
@@ -418,9 +418,6 @@ int iwl_run_init_ucode(struct iwl_priv *priv)
 	if (!priv->fw->img[IWL_UCODE_INIT].sec[0].len)
 		return 0;
 
-	if (priv->init_ucode_run)
-		return 0;
-
 	iwl_init_notification_wait(&priv->notif_wait, &calib_wait,
 				   calib_complete, ARRAY_SIZE(calib_complete),
 				   iwlagn_wait_calib, priv);
@@ -440,8 +437,6 @@ int iwl_run_init_ucode(struct iwl_priv *priv)
 	 */
 	ret = iwl_wait_notification(&priv->notif_wait, &calib_wait,
 					UCODE_CALIB_TIMEOUT);
-	if (!ret)
-		priv->init_ucode_run = true;
 
 	goto out;
 
diff --git a/drivers/net/wireless/iwlwifi/iwl-1000.c b/drivers/net/wireless/iwlwifi/iwl-1000.c
index c3817fae16c0..06f6cc08f451 100644
--- a/drivers/net/wireless/iwlwifi/iwl-1000.c
+++ b/drivers/net/wireless/iwlwifi/iwl-1000.c
@@ -95,7 +95,8 @@ static const struct iwl_eeprom_params iwl1000_eeprom_params = {
 	.nvm_calib_ver = EEPROM_1000_TX_POWER_VERSION,	\
 	.base_params = &iwl1000_base_params,			\
 	.eeprom_params = &iwl1000_eeprom_params,		\
-	.led_mode = IWL_LED_BLINK
+	.led_mode = IWL_LED_BLINK,				\
+	.max_ht_ampdu_exponent = IEEE80211_HT_MAX_AMPDU_64K
 
 const struct iwl_cfg iwl1000_bgn_cfg = {
 	.name = "Intel(R) Centrino(R) Wireless-N 1000 BGN",
@@ -121,7 +122,8 @@ const struct iwl_cfg iwl1000_bg_cfg = {
 	.base_params = &iwl1000_base_params,			\
 	.eeprom_params = &iwl1000_eeprom_params,		\
 	.led_mode = IWL_LED_RF_STATE,				\
-	.rx_with_siso_diversity = true
+	.rx_with_siso_diversity = true,				\
+	.max_ht_ampdu_exponent = IEEE80211_HT_MAX_AMPDU_64K
 
 const struct iwl_cfg iwl100_bgn_cfg = {
 	.name = "Intel(R) Centrino(R) Wireless-N 100 BGN",
diff --git a/drivers/net/wireless/iwlwifi/iwl-2000.c b/drivers/net/wireless/iwlwifi/iwl-2000.c
index 21e5d0843a62..890b95f497d6 100644
--- a/drivers/net/wireless/iwlwifi/iwl-2000.c
+++ b/drivers/net/wireless/iwlwifi/iwl-2000.c
@@ -123,7 +123,9 @@ static const struct iwl_eeprom_params iwl20x0_eeprom_params = {
 	.nvm_calib_ver = EEPROM_2000_TX_POWER_VERSION,		\
 	.base_params = &iwl2000_base_params,			\
 	.eeprom_params = &iwl20x0_eeprom_params,		\
-	.led_mode = IWL_LED_RF_STATE
+	.led_mode = IWL_LED_RF_STATE,				\
+	.max_ht_ampdu_exponent = IEEE80211_HT_MAX_AMPDU_64K
+
 
 const struct iwl_cfg iwl2000_2bgn_cfg = {
 	.name = "Intel(R) Centrino(R) Wireless-N 2200 BGN",
@@ -149,7 +151,8 @@ const struct iwl_cfg iwl2000_2bgn_d_cfg = {
 	.nvm_calib_ver = EEPROM_2000_TX_POWER_VERSION,	\
 	.base_params = &iwl2030_base_params,			\
 	.eeprom_params = &iwl20x0_eeprom_params,		\
-	.led_mode = IWL_LED_RF_STATE
+	.led_mode = IWL_LED_RF_STATE,				\
+	.max_ht_ampdu_exponent = IEEE80211_HT_MAX_AMPDU_64K
 
 const struct iwl_cfg iwl2030_2bgn_cfg = {
 	.name = "Intel(R) Centrino(R) Wireless-N 2230 BGN",
@@ -170,7 +173,8 @@ const struct iwl_cfg iwl2030_2bgn_cfg = {
 	.base_params = &iwl2000_base_params,			\
 	.eeprom_params = &iwl20x0_eeprom_params,		\
 	.led_mode = IWL_LED_RF_STATE,				\
-	.rx_with_siso_diversity = true
+	.rx_with_siso_diversity = true,				\
+	.max_ht_ampdu_exponent = IEEE80211_HT_MAX_AMPDU_64K
 
 const struct iwl_cfg iwl105_bgn_cfg = {
 	.name = "Intel(R) Centrino(R) Wireless-N 105 BGN",
@@ -197,7 +201,8 @@ const struct iwl_cfg iwl105_bgn_d_cfg = {
 	.base_params = &iwl2030_base_params,			\
 	.eeprom_params = &iwl20x0_eeprom_params,		\
 	.led_mode = IWL_LED_RF_STATE,				\
-	.rx_with_siso_diversity = true
+	.rx_with_siso_diversity = true,				\
+	.max_ht_ampdu_exponent = IEEE80211_HT_MAX_AMPDU_64K
 
 const struct iwl_cfg iwl135_bgn_cfg = {
 	.name = "Intel(R) Centrino(R) Wireless-N 135 BGN",
diff --git a/drivers/net/wireless/iwlwifi/iwl-5000.c b/drivers/net/wireless/iwlwifi/iwl-5000.c
index 332bbede39e5..724194e23414 100644
--- a/drivers/net/wireless/iwlwifi/iwl-5000.c
+++ b/drivers/net/wireless/iwlwifi/iwl-5000.c
@@ -93,7 +93,8 @@ static const struct iwl_eeprom_params iwl5000_eeprom_params = {
 	.nvm_calib_ver = EEPROM_5000_TX_POWER_VERSION,	\
 	.base_params = &iwl5000_base_params,			\
 	.eeprom_params = &iwl5000_eeprom_params,		\
-	.led_mode = IWL_LED_BLINK
+	.led_mode = IWL_LED_BLINK,				\
+	.max_ht_ampdu_exponent = IEEE80211_HT_MAX_AMPDU_64K
 
 const struct iwl_cfg iwl5300_agn_cfg = {
 	.name = "Intel(R) Ultimate N WiFi Link 5300 AGN",
@@ -158,7 +159,8 @@ const struct iwl_cfg iwl5350_agn_cfg = {
 	.base_params = &iwl5000_base_params,			\
 	.eeprom_params = &iwl5000_eeprom_params,		\
 	.led_mode = IWL_LED_BLINK,				\
-	.internal_wimax_coex = true
+	.internal_wimax_coex = true,				\
+	.max_ht_ampdu_exponent = IEEE80211_HT_MAX_AMPDU_64K
 
 const struct iwl_cfg iwl5150_agn_cfg = {
 	.name = "Intel(R) WiMAX/WiFi Link 5150 AGN",
diff --git a/drivers/net/wireless/iwlwifi/iwl-6000.c b/drivers/net/wireless/iwlwifi/iwl-6000.c
index 8f2c3c8c6b84..21b2630763dc 100644
--- a/drivers/net/wireless/iwlwifi/iwl-6000.c
+++ b/drivers/net/wireless/iwlwifi/iwl-6000.c
@@ -145,7 +145,8 @@ static const struct iwl_eeprom_params iwl6000_eeprom_params = {
 	.nvm_calib_ver = EEPROM_6005_TX_POWER_VERSION,	\
 	.base_params = &iwl6000_g2_base_params,			\
 	.eeprom_params = &iwl6000_eeprom_params,		\
-	.led_mode = IWL_LED_RF_STATE
+	.led_mode = IWL_LED_RF_STATE,				\
+	.max_ht_ampdu_exponent = IEEE80211_HT_MAX_AMPDU_64K
 
 const struct iwl_cfg iwl6005_2agn_cfg = {
 	.name = "Intel(R) Centrino(R) Advanced-N 6205 AGN",
@@ -199,7 +200,8 @@ const struct iwl_cfg iwl6005_2agn_mow2_cfg = {
 	.nvm_calib_ver = EEPROM_6030_TX_POWER_VERSION,	\
 	.base_params = &iwl6000_g2_base_params,			\
 	.eeprom_params = &iwl6000_eeprom_params,		\
-	.led_mode = IWL_LED_RF_STATE
+	.led_mode = IWL_LED_RF_STATE,				\
+	.max_ht_ampdu_exponent = IEEE80211_HT_MAX_AMPDU_64K
 
 const struct iwl_cfg iwl6030_2agn_cfg = {
 	.name = "Intel(R) Centrino(R) Advanced-N 6230 AGN",
@@ -235,7 +237,8 @@ const struct iwl_cfg iwl6030_2bg_cfg = {
 	.nvm_calib_ver = EEPROM_6030_TX_POWER_VERSION,	\
 	.base_params = &iwl6000_g2_base_params,			\
 	.eeprom_params = &iwl6000_eeprom_params,		\
-	.led_mode = IWL_LED_RF_STATE
+	.led_mode = IWL_LED_RF_STATE,				\
+	.max_ht_ampdu_exponent = IEEE80211_HT_MAX_AMPDU_64K
 
 const struct iwl_cfg iwl6035_2agn_cfg = {
 	.name = "Intel(R) Centrino(R) Advanced-N 6235 AGN",
@@ -290,7 +293,8 @@ const struct iwl_cfg iwl130_bg_cfg = {
 	.nvm_calib_ver = EEPROM_6000_TX_POWER_VERSION,	\
 	.base_params = &iwl6000_base_params,			\
 	.eeprom_params = &iwl6000_eeprom_params,		\
-	.led_mode = IWL_LED_BLINK
+	.led_mode = IWL_LED_BLINK,				\
+	.max_ht_ampdu_exponent = IEEE80211_HT_MAX_AMPDU_64K
 
 const struct iwl_cfg iwl6000i_2agn_cfg = {
 	.name = "Intel(R) Centrino(R) Advanced-N 6200 AGN",
@@ -322,7 +326,8 @@ const struct iwl_cfg iwl6000i_2bg_cfg = {
 	.base_params = &iwl6050_base_params,			\
 	.eeprom_params = &iwl6000_eeprom_params,		\
 	.led_mode = IWL_LED_BLINK,				\
-	.internal_wimax_coex = true
+	.internal_wimax_coex = true,				\
+	.max_ht_ampdu_exponent = IEEE80211_HT_MAX_AMPDU_64K
 
 const struct iwl_cfg iwl6050_2agn_cfg = {
 	.name = "Intel(R) Centrino(R) Advanced-N + WiMAX 6250 AGN",
@@ -347,7 +352,8 @@ const struct iwl_cfg iwl6050_2abg_cfg = {
 	.base_params = &iwl6050_base_params,			\
 	.eeprom_params = &iwl6000_eeprom_params,		\
 	.led_mode = IWL_LED_BLINK,				\
-	.internal_wimax_coex = true
+	.internal_wimax_coex = true,				\
+	.max_ht_ampdu_exponent = IEEE80211_HT_MAX_AMPDU_64K
 
 const struct iwl_cfg iwl6150_bgn_cfg = {
 	.name = "Intel(R) Centrino(R) Wireless-N + WiMAX 6150 BGN",
diff --git a/drivers/net/wireless/iwlwifi/iwl-drv.c b/drivers/net/wireless/iwlwifi/iwl-drv.c
index 996e7f16adf9..c7154ac42c8c 100644
--- a/drivers/net/wireless/iwlwifi/iwl-drv.c
+++ b/drivers/net/wireless/iwlwifi/iwl-drv.c
@@ -1257,6 +1257,7 @@ static void iwl_req_fw_callback(const struct firmware *ucode_raw, void *context)
 				op->name, err);
 #endif
 	}
+	kfree(pieces);
 	return;
 
  try_again:
diff --git a/drivers/net/wireless/iwlwifi/mvm/coex.c b/drivers/net/wireless/iwlwifi/mvm/coex.c
index 1ec4d55155f7..7810c41cf9a7 100644
--- a/drivers/net/wireless/iwlwifi/mvm/coex.c
+++ b/drivers/net/wireless/iwlwifi/mvm/coex.c
@@ -793,7 +793,8 @@ static void iwl_mvm_bt_notif_iterator(void *_data, u8 *mac,
 	if (!vif->bss_conf.assoc)
 		smps_mode = IEEE80211_SMPS_AUTOMATIC;
 
-	if (IWL_COEX_IS_RRC_ON(mvm->last_bt_notif.ttc_rrc_status,
+	if (mvmvif->phy_ctxt &&
+	    IWL_COEX_IS_RRC_ON(mvm->last_bt_notif.ttc_rrc_status,
 			       mvmvif->phy_ctxt->id))
 		smps_mode = IEEE80211_SMPS_AUTOMATIC;
 
diff --git a/drivers/net/wireless/iwlwifi/mvm/coex_legacy.c b/drivers/net/wireless/iwlwifi/mvm/coex_legacy.c
index d530ef3da107..542ee74f290a 100644
--- a/drivers/net/wireless/iwlwifi/mvm/coex_legacy.c
+++ b/drivers/net/wireless/iwlwifi/mvm/coex_legacy.c
@@ -832,7 +832,8 @@ static void iwl_mvm_bt_notif_iterator(void *_data, u8 *mac,
 	if (!vif->bss_conf.assoc)
 		smps_mode = IEEE80211_SMPS_AUTOMATIC;
 
-	if (data->notif->rrc_enabled & BIT(mvmvif->phy_ctxt->id))
+	if (mvmvif->phy_ctxt &&
+	    data->notif->rrc_enabled & BIT(mvmvif->phy_ctxt->id))
 		smps_mode = IEEE80211_SMPS_AUTOMATIC;
 
 	IWL_DEBUG_COEX(data->mvm,
diff --git a/drivers/net/wireless/iwlwifi/mvm/mac80211.c b/drivers/net/wireless/iwlwifi/mvm/mac80211.c
index 1ff7ec08532d..09654e73a533 100644
--- a/drivers/net/wireless/iwlwifi/mvm/mac80211.c
+++ b/drivers/net/wireless/iwlwifi/mvm/mac80211.c
@@ -405,7 +405,10 @@ int iwl_mvm_mac_setup_register(struct iwl_mvm *mvm)
 		hw->wiphy->bands[IEEE80211_BAND_5GHZ] =
 			&mvm->nvm_data->bands[IEEE80211_BAND_5GHZ];
 
-		if (mvm->fw->ucode_capa.capa[0] & IWL_UCODE_TLV_CAPA_BEAMFORMER)
+		if ((mvm->fw->ucode_capa.capa[0] &
+		     IWL_UCODE_TLV_CAPA_BEAMFORMER) &&
+		    (mvm->fw->ucode_capa.api[0] &
+		     IWL_UCODE_TLV_API_LQ_SS_PARAMS))
 			hw->wiphy->bands[IEEE80211_BAND_5GHZ]->vht_cap.cap |=
 				IEEE80211_VHT_CAP_SU_BEAMFORMER_CAPABLE;
 	}
@@ -2215,7 +2218,19 @@ static void iwl_mvm_mac_cancel_hw_scan(struct ieee80211_hw *hw,
 
 	mutex_lock(&mvm->mutex);
 
-	iwl_mvm_cancel_scan(mvm);
+	/* Due to a race condition, it's possible that mac80211 asks
+	 * us to stop a hw_scan when it's already stopped.  This can
+	 * happen, for instance, if we stopped the scan ourselves,
+	 * called ieee80211_scan_completed() and the userspace called
+	 * cancel scan scan before ieee80211_scan_work() could run.
+	 * To handle that, simply return if the scan is not running.
+	*/
+	/* FIXME: for now, we ignore this race for UMAC scans, since
+	 * they don't set the scan_status.
+	 */
+	if ((mvm->scan_status == IWL_MVM_SCAN_OS) ||
+	    (mvm->fw->ucode_capa.capa[0] & IWL_UCODE_TLV_CAPA_UMAC_SCAN))
+		iwl_mvm_cancel_scan(mvm);
 
 	mutex_unlock(&mvm->mutex);
 }
@@ -2559,12 +2574,29 @@ static int iwl_mvm_mac_sched_scan_stop(struct ieee80211_hw *hw,
 	int ret;
 
 	mutex_lock(&mvm->mutex);
+
+	/* Due to a race condition, it's possible that mac80211 asks
+	 * us to stop a sched_scan when it's already stopped.  This
+	 * can happen, for instance, if we stopped the scan ourselves,
+	 * called ieee80211_sched_scan_stopped() and the userspace called
+	 * stop sched scan scan before ieee80211_sched_scan_stopped_work()
+	 * could run.  To handle this, simply return if the scan is
+	 * not running.
+	*/
+	/* FIXME: for now, we ignore this race for UMAC scans, since
+	 * they don't set the scan_status.
+	 */
+	if (mvm->scan_status != IWL_MVM_SCAN_SCHED &&
+	    !(mvm->fw->ucode_capa.capa[0] & IWL_UCODE_TLV_CAPA_UMAC_SCAN)) {
+		mutex_unlock(&mvm->mutex);
+		return 0;
+	}
+
 	ret = iwl_mvm_scan_offload_stop(mvm, false);
 	mutex_unlock(&mvm->mutex);
 	iwl_mvm_wait_for_async_handlers(mvm);
 
 	return ret;
-
 }
 
 static int iwl_mvm_mac_set_key(struct ieee80211_hw *hw,
diff --git a/drivers/net/wireless/iwlwifi/mvm/rs.c b/drivers/net/wireless/iwlwifi/mvm/rs.c
index 194bd1f939ca..078f24cf4af3 100644
--- a/drivers/net/wireless/iwlwifi/mvm/rs.c
+++ b/drivers/net/wireless/iwlwifi/mvm/rs.c
@@ -134,9 +134,12 @@ enum rs_column_mode {
 #define MAX_NEXT_COLUMNS 7
 #define MAX_COLUMN_CHECKS 3
 
+struct rs_tx_column;
+
 typedef bool (*allow_column_func_t) (struct iwl_mvm *mvm,
 				     struct ieee80211_sta *sta,
-				     struct iwl_scale_tbl_info *tbl);
+				     struct iwl_scale_tbl_info *tbl,
+				     const struct rs_tx_column *next_col);
 
 struct rs_tx_column {
 	enum rs_column_mode mode;
@@ -147,13 +150,15 @@ struct rs_tx_column {
 };
 
 static bool rs_ant_allow(struct iwl_mvm *mvm, struct ieee80211_sta *sta,
-			 struct iwl_scale_tbl_info *tbl)
+			 struct iwl_scale_tbl_info *tbl,
+			 const struct rs_tx_column *next_col)
 {
-	return iwl_mvm_bt_coex_is_ant_avail(mvm, tbl->rate.ant);
+	return iwl_mvm_bt_coex_is_ant_avail(mvm, next_col->ant);
 }
 
 static bool rs_mimo_allow(struct iwl_mvm *mvm, struct ieee80211_sta *sta,
-			  struct iwl_scale_tbl_info *tbl)
+			  struct iwl_scale_tbl_info *tbl,
+			  const struct rs_tx_column *next_col)
 {
 	if (!sta->ht_cap.ht_supported)
 		return false;
@@ -171,7 +176,8 @@ static bool rs_mimo_allow(struct iwl_mvm *mvm, struct ieee80211_sta *sta,
 }
 
 static bool rs_siso_allow(struct iwl_mvm *mvm, struct ieee80211_sta *sta,
-			  struct iwl_scale_tbl_info *tbl)
+			  struct iwl_scale_tbl_info *tbl,
+			  const struct rs_tx_column *next_col)
 {
 	if (!sta->ht_cap.ht_supported)
 		return false;
@@ -180,7 +186,8 @@ static bool rs_siso_allow(struct iwl_mvm *mvm, struct ieee80211_sta *sta,
 }
 
 static bool rs_sgi_allow(struct iwl_mvm *mvm, struct ieee80211_sta *sta,
-			 struct iwl_scale_tbl_info *tbl)
+			 struct iwl_scale_tbl_info *tbl,
+			 const struct rs_tx_column *next_col)
 {
 	struct rs_rate *rate = &tbl->rate;
 	struct ieee80211_sta_ht_cap *ht_cap = &sta->ht_cap;
@@ -1271,6 +1278,9 @@ static void rs_mac80211_tx_status(void *mvm_r,
 	struct iwl_mvm *mvm = IWL_OP_MODE_GET_MVM(op_mode);
 	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
 
+	if (!iwl_mvm_sta_from_mac80211(sta)->vif)
+		return;
+
 	if (!ieee80211_is_data(hdr->frame_control) ||
 	    info->flags & IEEE80211_TX_CTL_NO_ACK)
 		return;
@@ -1590,7 +1600,7 @@ static enum rs_column rs_get_next_column(struct iwl_mvm *mvm,
 
 		for (j = 0; j < MAX_COLUMN_CHECKS; j++) {
 			allow_func = next_col->checks[j];
-			if (allow_func && !allow_func(mvm, sta, tbl))
+			if (allow_func && !allow_func(mvm, sta, tbl, next_col))
 				break;
 		}
 
@@ -2504,6 +2514,14 @@ static void rs_get_rate(void *mvm_r, struct ieee80211_sta *sta, void *mvm_sta,
 	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
 	struct iwl_lq_sta *lq_sta = mvm_sta;
 
+	if (sta && !iwl_mvm_sta_from_mac80211(sta)->vif) {
+		/* if vif isn't initialized mvm doesn't know about
+		 * this station, so don't do anything with the it
+		 */
+		sta = NULL;
+		mvm_sta = NULL;
+	}
+
 	/* TODO: handle rate_idx_mask and rate_idx_mcs_mask */
 
 	/* Treat uninitialized rate scaling data same as non-existing. */
@@ -2820,6 +2838,9 @@ static void rs_rate_update(void *mvm_r,
 			(struct iwl_op_mode *)mvm_r;
 	struct iwl_mvm *mvm = IWL_OP_MODE_GET_MVM(op_mode);
 
+	if (!iwl_mvm_sta_from_mac80211(sta)->vif)
+		return;
+
 	/* Stop any ongoing aggregations as rs starts off assuming no agg */
 	for (tid = 0; tid < IWL_MAX_TID_COUNT; tid++)
 		ieee80211_stop_tx_ba_session(sta, tid);
@@ -3580,9 +3601,15 @@ static ssize_t iwl_dbgfs_ss_force_write(struct iwl_lq_sta *lq_sta, char *buf,
 
 MVM_DEBUGFS_READ_WRITE_FILE_OPS(ss_force, 32);
 
-static void rs_add_debugfs(void *mvm, void *mvm_sta, struct dentry *dir)
+static void rs_add_debugfs(void *mvm, void *priv_sta, struct dentry *dir)
 {
-	struct iwl_lq_sta *lq_sta = mvm_sta;
+	struct iwl_lq_sta *lq_sta = priv_sta;
+	struct iwl_mvm_sta *mvmsta;
+
+	mvmsta = container_of(lq_sta, struct iwl_mvm_sta, lq_sta);
+
+	if (!mvmsta->vif)
+		return;
 
 	debugfs_create_file("rate_scale_table", S_IRUSR | S_IWUSR, dir,
 			    lq_sta, &rs_sta_dbgfs_scale_table_ops);
diff --git a/drivers/net/wireless/iwlwifi/mvm/scan.c b/drivers/net/wireless/iwlwifi/mvm/scan.c
index 7e9aa3cb3254..c47c8051da77 100644
--- a/drivers/net/wireless/iwlwifi/mvm/scan.c
+++ b/drivers/net/wireless/iwlwifi/mvm/scan.c
@@ -1128,8 +1128,10 @@ int iwl_mvm_scan_offload_stop(struct iwl_mvm *mvm, bool notify)
 	if (mvm->scan_status == IWL_MVM_SCAN_NONE)
 		return 0;
 
-	if (iwl_mvm_is_radio_killed(mvm))
+	if (iwl_mvm_is_radio_killed(mvm)) {
+		ret = 0;
 		goto out;
+	}
 
 	if (mvm->scan_status != IWL_MVM_SCAN_SCHED &&
 	    (!(mvm->fw->ucode_capa.api[0] & IWL_UCODE_TLV_API_LMAC_SCAN) ||
@@ -1148,16 +1150,14 @@ int iwl_mvm_scan_offload_stop(struct iwl_mvm *mvm, bool notify)
 		IWL_DEBUG_SCAN(mvm, "Send stop %sscan failed %d\n",
 			       sched ? "offloaded " : "", ret);
 		iwl_remove_notification(&mvm->notif_wait, &wait_scan_done);
-		return ret;
+		goto out;
 	}
 
 	IWL_DEBUG_SCAN(mvm, "Successfully sent stop %sscan\n",
 		       sched ? "offloaded " : "");
 
 	ret = iwl_wait_notification(&mvm->notif_wait, &wait_scan_done, 1 * HZ);
-	if (ret)
-		return ret;
-
+out:
 	/*
 	 * Clear the scan status so the next scan requests will succeed. This
 	 * also ensures the Rx handler doesn't do anything, as the scan was
@@ -1167,7 +1167,6 @@ int iwl_mvm_scan_offload_stop(struct iwl_mvm *mvm, bool notify)
 	if (mvm->scan_status == IWL_MVM_SCAN_OS)
 		iwl_mvm_unref(mvm, IWL_MVM_REF_SCAN);
 
-out:
 	mvm->scan_status = IWL_MVM_SCAN_NONE;
 
 	if (notify) {
@@ -1177,7 +1176,7 @@ out:
 			ieee80211_scan_completed(mvm->hw, true);
 	}
 
-	return 0;
+	return ret;
 }
 
 static void iwl_mvm_unified_scan_fill_tx_cmd(struct iwl_mvm *mvm,
diff --git a/drivers/net/wireless/iwlwifi/mvm/time-event.c b/drivers/net/wireless/iwlwifi/mvm/time-event.c
index 54fafbf9a711..4b81c0bf63b0 100644
--- a/drivers/net/wireless/iwlwifi/mvm/time-event.c
+++ b/drivers/net/wireless/iwlwifi/mvm/time-event.c
@@ -197,6 +197,8 @@ iwl_mvm_te_handle_notify_csa(struct iwl_mvm *mvm,
 			     struct iwl_time_event_notif *notif)
 {
 	if (!le32_to_cpu(notif->status)) {
+		if (te_data->vif->type == NL80211_IFTYPE_STATION)
+			ieee80211_connection_loss(te_data->vif);
 		IWL_DEBUG_TE(mvm, "CSA time event failed to start\n");
 		iwl_mvm_te_clear_data(mvm, te_data);
 		return;
@@ -750,8 +752,7 @@ void iwl_mvm_stop_roc(struct iwl_mvm *mvm)
 	 * request
 	 */
 	list_for_each_entry(te_data, &mvm->time_event_list, list) {
-		if (te_data->vif->type == NL80211_IFTYPE_P2P_DEVICE &&
-		    te_data->running) {
+		if (te_data->vif->type == NL80211_IFTYPE_P2P_DEVICE) {
 			mvmvif = iwl_mvm_vif_from_mac80211(te_data->vif);
 			is_p2p = true;
 			goto remove_te;
@@ -766,10 +767,8 @@ void iwl_mvm_stop_roc(struct iwl_mvm *mvm)
 	 * request
 	 */
 	list_for_each_entry(te_data, &mvm->aux_roc_te_list, list) {
-		if (te_data->running) {
-			mvmvif = iwl_mvm_vif_from_mac80211(te_data->vif);
-			goto remove_te;
-		}
+		mvmvif = iwl_mvm_vif_from_mac80211(te_data->vif);
+		goto remove_te;
 	}
 
 remove_te:
diff --git a/drivers/net/wireless/iwlwifi/mvm/tx.c b/drivers/net/wireless/iwlwifi/mvm/tx.c
index 07304e1fd64a..96a05406babf 100644
--- a/drivers/net/wireless/iwlwifi/mvm/tx.c
+++ b/drivers/net/wireless/iwlwifi/mvm/tx.c
@@ -949,8 +949,10 @@ int iwl_mvm_rx_ba_notif(struct iwl_mvm *mvm, struct iwl_rx_cmd_buffer *rxb,
 	mvmsta = iwl_mvm_sta_from_mac80211(sta);
 	tid_data = &mvmsta->tid_data[tid];
 
-	if (WARN_ONCE(tid_data->txq_id != scd_flow, "Q %d, tid %d, flow %d",
-		      tid_data->txq_id, tid, scd_flow)) {
+	if (tid_data->txq_id != scd_flow) {
+		IWL_ERR(mvm,
+			"invalid BA notification: Q %d, tid %d, flow %d\n",
+			tid_data->txq_id, tid, scd_flow);
 		rcu_read_unlock();
 		return 0;
 	}
diff --git a/drivers/net/wireless/iwlwifi/pcie/drv.c b/drivers/net/wireless/iwlwifi/pcie/drv.c
index dbd6bcf52205..686dd301cd53 100644
--- a/drivers/net/wireless/iwlwifi/pcie/drv.c
+++ b/drivers/net/wireless/iwlwifi/pcie/drv.c
@@ -368,10 +368,12 @@ static const struct pci_device_id iwl_hw_card_ids[] = {
 /* 3165 Series */
 	{IWL_PCI_DEVICE(0x3165, 0x4010, iwl3165_2ac_cfg)},
 	{IWL_PCI_DEVICE(0x3165, 0x4012, iwl3165_2ac_cfg)},
-	{IWL_PCI_DEVICE(0x3165, 0x4110, iwl3165_2ac_cfg)},
-	{IWL_PCI_DEVICE(0x3165, 0x4210, iwl3165_2ac_cfg)},
 	{IWL_PCI_DEVICE(0x3165, 0x4410, iwl3165_2ac_cfg)},
 	{IWL_PCI_DEVICE(0x3165, 0x4510, iwl3165_2ac_cfg)},
+	{IWL_PCI_DEVICE(0x3165, 0x4110, iwl3165_2ac_cfg)},
+	{IWL_PCI_DEVICE(0x3166, 0x4310, iwl3165_2ac_cfg)},
+	{IWL_PCI_DEVICE(0x3166, 0x4210, iwl3165_2ac_cfg)},
+	{IWL_PCI_DEVICE(0x3165, 0x8010, iwl3165_2ac_cfg)},
 
 /* 7265 Series */
 	{IWL_PCI_DEVICE(0x095A, 0x5010, iwl7265_2ac_cfg)},
diff --git a/drivers/net/wireless/mac80211_hwsim.c b/drivers/net/wireless/mac80211_hwsim.c
index 4a4c6586a8d2..8908be6dbc48 100644
--- a/drivers/net/wireless/mac80211_hwsim.c
+++ b/drivers/net/wireless/mac80211_hwsim.c
@@ -946,7 +946,8 @@ static void mac80211_hwsim_tx_frame_nl(struct ieee80211_hw *hw,
 		goto nla_put_failure;
 
 	genlmsg_end(skb, msg_head);
-	genlmsg_unicast(&init_net, skb, dst_portid);
+	if (genlmsg_unicast(&init_net, skb, dst_portid))
+		goto err_free_txskb;
 
 	/* Enqueue the packet */
 	skb_queue_tail(&data->pending, my_skb);
@@ -955,6 +956,8 @@ static void mac80211_hwsim_tx_frame_nl(struct ieee80211_hw *hw,
 	return;
 
 nla_put_failure:
+	nlmsg_free(skb);
+err_free_txskb:
 	printk(KERN_DEBUG "mac80211_hwsim: error occurred in %s\n", __func__);
 	ieee80211_free_txskb(hw, my_skb);
 	data->tx_failed++;
diff --git a/drivers/net/wireless/rtlwifi/base.c b/drivers/net/wireless/rtlwifi/base.c
index 1d4677460711..074f716020aa 100644
--- a/drivers/net/wireless/rtlwifi/base.c
+++ b/drivers/net/wireless/rtlwifi/base.c
@@ -1386,8 +1386,11 @@ u8 rtl_is_special_data(struct ieee80211_hw *hw, struct sk_buff *skb, u8 is_tx)
 		}
 
 		return true;
-	} else if (0x86DD == ether_type) {
-		return true;
+	} else if (ETH_P_IPV6 == ether_type) {
+		/* TODO: Handle any IPv6 cases that need special handling.
+		 * For now, always return false
+		 */
+		goto end;
 	}
 
 end:
diff --git a/drivers/net/wireless/rtlwifi/pci.c b/drivers/net/wireless/rtlwifi/pci.c
index a62170ea0481..8c45cf44ce24 100644
--- a/drivers/net/wireless/rtlwifi/pci.c
+++ b/drivers/net/wireless/rtlwifi/pci.c
@@ -1124,12 +1124,22 @@ static void _rtl_pci_prepare_bcn_tasklet(struct ieee80211_hw *hw)
 	/*This is for new trx flow*/
 	struct rtl_tx_buffer_desc *pbuffer_desc = NULL;
 	u8 temp_one = 1;
+	u8 *entry;
 
 	memset(&tcb_desc, 0, sizeof(struct rtl_tcb_desc));
 	ring = &rtlpci->tx_ring[BEACON_QUEUE];
 	pskb = __skb_dequeue(&ring->queue);
-	if (pskb)
+	if (rtlpriv->use_new_trx_flow)
+		entry = (u8 *)(&ring->buffer_desc[ring->idx]);
+	else
+		entry = (u8 *)(&ring->desc[ring->idx]);
+	if (pskb) {
+		pci_unmap_single(rtlpci->pdev,
+				 rtlpriv->cfg->ops->get_desc(
+				 (u8 *)entry, true, HW_DESC_TXBUFF_ADDR),
+				 pskb->len, PCI_DMA_TODEVICE);
 		kfree_skb(pskb);
+	}
 
 	/*NB: the beacon data buffer must be 32-bit aligned. */
 	pskb = ieee80211_beacon_get(hw, mac->vif);
diff --git a/drivers/net/xen-netback/interface.c b/drivers/net/xen-netback/interface.c
index f38227afe099..3aa8648080c8 100644
--- a/drivers/net/xen-netback/interface.c
+++ b/drivers/net/xen-netback/interface.c
@@ -340,12 +340,11 @@ static void xenvif_get_ethtool_stats(struct net_device *dev,
 	unsigned int num_queues = vif->num_queues;
 	int i;
 	unsigned int queue_index;
-	struct xenvif_stats *vif_stats;
 
 	for (i = 0; i < ARRAY_SIZE(xenvif_stats); i++) {
 		unsigned long accum = 0;
 		for (queue_index = 0; queue_index < num_queues; ++queue_index) {
-			vif_stats = &vif->queues[queue_index].stats;
+			void *vif_stats = &vif->queues[queue_index].stats;
 			accum += *(unsigned long *)(vif_stats + xenvif_stats[i].offset);
 		}
 		data[i] = accum;
diff --git a/drivers/net/xen-netback/netback.c b/drivers/net/xen-netback/netback.c
index f7a31d2cb3f1..997cf0901ac2 100644
--- a/drivers/net/xen-netback/netback.c
+++ b/drivers/net/xen-netback/netback.c
@@ -96,6 +96,7 @@ static void xenvif_idx_release(struct xenvif_queue *queue, u16 pending_idx,
 static void make_tx_response(struct xenvif_queue *queue,
 			     struct xen_netif_tx_request *txp,
 			     s8       st);
+static void push_tx_responses(struct xenvif_queue *queue);
 
 static inline int tx_work_todo(struct xenvif_queue *queue);
 
@@ -657,6 +658,7 @@ static void xenvif_tx_err(struct xenvif_queue *queue,
 	do {
 		spin_lock_irqsave(&queue->response_lock, flags);
 		make_tx_response(queue, txp, XEN_NETIF_RSP_ERROR);
+		push_tx_responses(queue);
 		spin_unlock_irqrestore(&queue->response_lock, flags);
 		if (cons == end)
 			break;
@@ -1343,7 +1345,7 @@ static int xenvif_handle_frag_list(struct xenvif_queue *queue, struct sk_buff *s
 {
 	unsigned int offset = skb_headlen(skb);
 	skb_frag_t frags[MAX_SKB_FRAGS];
-	int i;
+	int i, f;
 	struct ubuf_info *uarg;
 	struct sk_buff *nskb = skb_shinfo(skb)->frag_list;
 
@@ -1383,23 +1385,25 @@ static int xenvif_handle_frag_list(struct xenvif_queue *queue, struct sk_buff *s
 		frags[i].page_offset = 0;
 		skb_frag_size_set(&frags[i], len);
 	}
-	/* swap out with old one */
-	memcpy(skb_shinfo(skb)->frags,
-	       frags,
-	       i * sizeof(skb_frag_t));
-	skb_shinfo(skb)->nr_frags = i;
-	skb->truesize += i * PAGE_SIZE;
 
-	/* remove traces of mapped pages and frag_list */
+	/* Copied all the bits from the frag list -- free it. */
 	skb_frag_list_init(skb);
+	xenvif_skb_zerocopy_prepare(queue, nskb);
+	kfree_skb(nskb);
+
+	/* Release all the original (foreign) frags. */
+	for (f = 0; f < skb_shinfo(skb)->nr_frags; f++)
+		skb_frag_unref(skb, f);
 	uarg = skb_shinfo(skb)->destructor_arg;
 	/* increase inflight counter to offset decrement in callback */
 	atomic_inc(&queue->inflight_packets);
 	uarg->callback(uarg, true);
 	skb_shinfo(skb)->destructor_arg = NULL;
 
-	xenvif_skb_zerocopy_prepare(queue, nskb);
-	kfree_skb(nskb);
+	/* Fill the skb with the new (local) frags. */
+	memcpy(skb_shinfo(skb)->frags, frags, i * sizeof(skb_frag_t));
+	skb_shinfo(skb)->nr_frags = i;
+	skb->truesize += i * PAGE_SIZE;
 
 	return 0;
 }
@@ -1652,13 +1656,20 @@ static void xenvif_idx_release(struct xenvif_queue *queue, u16 pending_idx,
 	unsigned long flags;
 
 	pending_tx_info = &queue->pending_tx_info[pending_idx];
+
 	spin_lock_irqsave(&queue->response_lock, flags);
+
 	make_tx_response(queue, &pending_tx_info->req, status);
-	index = pending_index(queue->pending_prod);
+
+	/* Release the pending index before pusing the Tx response so
+	 * its available before a new Tx request is pushed by the
+	 * frontend.
+	 */
+	index = pending_index(queue->pending_prod++);
 	queue->pending_ring[index] = pending_idx;
-	/* TX shouldn't use the index before we give it back here */
-	mb();
-	queue->pending_prod++;
+
+	push_tx_responses(queue);
+
 	spin_unlock_irqrestore(&queue->response_lock, flags);
 }
 
@@ -1669,7 +1680,6 @@ static void make_tx_response(struct xenvif_queue *queue,
 {
 	RING_IDX i = queue->tx.rsp_prod_pvt;
 	struct xen_netif_tx_response *resp;
-	int notify;
 
 	resp = RING_GET_RESPONSE(&queue->tx, i);
 	resp->id     = txp->id;
@@ -1679,6 +1689,12 @@ static void make_tx_response(struct xenvif_queue *queue,
 		RING_GET_RESPONSE(&queue->tx, ++i)->status = XEN_NETIF_RSP_NULL;
 
 	queue->tx.rsp_prod_pvt = ++i;
+}
+
+static void push_tx_responses(struct xenvif_queue *queue)
+{
+	int notify;
+
 	RING_PUSH_RESPONSES_AND_CHECK_NOTIFY(&queue->tx, notify);
 	if (notify)
 		notify_remote_via_irq(queue->tx_irq);
diff --git a/drivers/net/xen-netfront.c b/drivers/net/xen-netfront.c
index e9b960f0ff32..720aaf6313d2 100644
--- a/drivers/net/xen-netfront.c
+++ b/drivers/net/xen-netfront.c
@@ -1008,8 +1008,7 @@ err:
 
 static int xennet_change_mtu(struct net_device *dev, int mtu)
 {
-	int max = xennet_can_sg(dev) ?
-		XEN_NETIF_MAX_TX_SIZE - MAX_TCP_HEADER : ETH_DATA_LEN;
+	int max = xennet_can_sg(dev) ? XEN_NETIF_MAX_TX_SIZE : ETH_DATA_LEN;
 
 	if (mtu > max)
 		return -EINVAL;
@@ -1279,8 +1278,6 @@ static struct net_device *xennet_create_dev(struct xenbus_device *dev)
 	netdev->ethtool_ops = &xennet_ethtool_ops;
 	SET_NETDEV_DEV(netdev, &dev->dev);
 
-	netif_set_gso_max_size(netdev, XEN_NETIF_MAX_TX_SIZE - MAX_TCP_HEADER);
-
 	np->netdev = netdev;
 
 	netif_carrier_off(netdev);
diff --git a/drivers/of/Kconfig b/drivers/of/Kconfig
index 38d1c51f58b1..7bcaeec876c0 100644
--- a/drivers/of/Kconfig
+++ b/drivers/of/Kconfig
@@ -84,8 +84,7 @@ config OF_RESOLVE
 	bool
 
 config OF_OVERLAY
-	bool
-	depends on OF
+	bool "Device Tree overlays"
 	select OF_DYNAMIC
 	select OF_RESOLVE
 
diff --git a/drivers/of/address.c b/drivers/of/address.c
index ad2906919d45..78a7dcbec7d8 100644
--- a/drivers/of/address.c
+++ b/drivers/of/address.c
@@ -450,12 +450,17 @@ static struct of_bus *of_match_bus(struct device_node *np)
 	return NULL;
 }
 
-static int of_empty_ranges_quirk(void)
+static int of_empty_ranges_quirk(struct device_node *np)
 {
 	if (IS_ENABLED(CONFIG_PPC)) {
-		/* To save cycles, we cache the result */
+		/* To save cycles, we cache the result for global "Mac" setting */
 		static int quirk_state = -1;
 
+		/* PA-SEMI sdc DT bug */
+		if (of_device_is_compatible(np, "1682m-sdc"))
+			return true;
+
+		/* Make quirk cached */
 		if (quirk_state < 0)
 			quirk_state =
 				of_machine_is_compatible("Power Macintosh") ||
@@ -490,7 +495,7 @@ static int of_translate_one(struct device_node *parent, struct of_bus *bus,
 	 * This code is only enabled on powerpc. --gcl
 	 */
 	ranges = of_get_property(parent, rprop, &rlen);
-	if (ranges == NULL && !of_empty_ranges_quirk()) {
+	if (ranges == NULL && !of_empty_ranges_quirk(parent)) {
 		pr_debug("OF: no ranges; cannot translate\n");
 		return 1;
 	}
diff --git a/drivers/of/base.c b/drivers/of/base.c
index 0a8aeb8523fe..8f165b112e03 100644
--- a/drivers/of/base.c
+++ b/drivers/of/base.c
@@ -714,16 +714,12 @@ static struct device_node *__of_find_node_by_path(struct device_node *parent,
 						const char *path)
 {
 	struct device_node *child;
-	int len = strchrnul(path, '/') - path;
-	int term;
+	int len;
 
+	len = strcspn(path, "/:");
 	if (!len)
 		return NULL;
 
-	term = strchrnul(path, ':') - path;
-	if (term < len)
-		len = term;
-
 	__for_each_child_of_node(parent, child) {
 		const char *name = strrchr(child->full_name, '/');
 		if (WARN(!name, "malformed device_node %s\n", child->full_name))
@@ -768,8 +764,12 @@ struct device_node *of_find_node_opts_by_path(const char *path, const char **opt
 
 	/* The path could begin with an alias */
 	if (*path != '/') {
-		char *p = strchrnul(path, '/');
-		int len = separator ? separator - path : p - path;
+		int len;
+		const char *p = separator;
+
+		if (!p)
+			p = strchrnul(path, '/');
+		len = p - path;
 
 		/* of_aliases must not be NULL */
 		if (!of_aliases)
@@ -794,6 +794,8 @@ struct device_node *of_find_node_opts_by_path(const char *path, const char **opt
 		path++; /* Increment past '/' delimiter */
 		np = __of_find_node_by_path(np, path);
 		path = strchrnul(path, '/');
+		if (separator && separator < path)
+			break;
 	}
 	raw_spin_unlock_irqrestore(&devtree_lock, flags);
 	return np;
diff --git a/drivers/of/device.c b/drivers/of/device.c
index 46d6c75c1404..20c1332a0018 100644
--- a/drivers/of/device.c
+++ b/drivers/of/device.c
@@ -2,6 +2,9 @@
 #include <linux/kernel.h>
 #include <linux/of.h>
 #include <linux/of_device.h>
+#include <linux/of_address.h>
+#include <linux/of_iommu.h>
+#include <linux/dma-mapping.h>
 #include <linux/init.h>
 #include <linux/module.h>
 #include <linux/mod_devicetable.h>
@@ -66,6 +69,87 @@ int of_device_add(struct platform_device *ofdev)
 	return device_add(&ofdev->dev);
 }
 
+/**
+ * of_dma_configure - Setup DMA configuration
+ * @dev:	Device to apply DMA configuration
+ * @np:		Pointer to OF node having DMA configuration
+ *
+ * Try to get devices's DMA configuration from DT and update it
+ * accordingly.
+ *
+ * If platform code needs to use its own special DMA configuration, it
+ * can use a platform bus notifier and handle BUS_NOTIFY_ADD_DEVICE events
+ * to fix up DMA configuration.
+ */
+void of_dma_configure(struct device *dev, struct device_node *np)
+{
+	u64 dma_addr, paddr, size;
+	int ret;
+	bool coherent;
+	unsigned long offset;
+	struct iommu_ops *iommu;
+
+	/*
+	 * Set default coherent_dma_mask to 32 bit.  Drivers are expected to
+	 * setup the correct supported mask.
+	 */
+	if (!dev->coherent_dma_mask)
+		dev->coherent_dma_mask = DMA_BIT_MASK(32);
+
+	/*
+	 * Set it to coherent_dma_mask by default if the architecture
+	 * code has not set it.
+	 */
+	if (!dev->dma_mask)
+		dev->dma_mask = &dev->coherent_dma_mask;
+
+	ret = of_dma_get_range(np, &dma_addr, &paddr, &size);
+	if (ret < 0) {
+		dma_addr = offset = 0;
+		size = dev->coherent_dma_mask + 1;
+	} else {
+		offset = PFN_DOWN(paddr - dma_addr);
+
+		/*
+		 * Add a work around to treat the size as mask + 1 in case
+		 * it is defined in DT as a mask.
+		 */
+		if (size & 1) {
+			dev_warn(dev, "Invalid size 0x%llx for dma-range\n",
+				 size);
+			size = size + 1;
+		}
+
+		if (!size) {
+			dev_err(dev, "Adjusted size 0x%llx invalid\n", size);
+			return;
+		}
+		dev_dbg(dev, "dma_pfn_offset(%#08lx)\n", offset);
+	}
+
+	dev->dma_pfn_offset = offset;
+
+	/*
+	 * Limit coherent and dma mask based on size and default mask
+	 * set by the driver.
+	 */
+	dev->coherent_dma_mask = min(dev->coherent_dma_mask,
+				     DMA_BIT_MASK(ilog2(dma_addr + size)));
+	*dev->dma_mask = min((*dev->dma_mask),
+			     DMA_BIT_MASK(ilog2(dma_addr + size)));
+
+	coherent = of_dma_is_coherent(np);
+	dev_dbg(dev, "device is%sdma coherent\n",
+		coherent ? " " : " not ");
+
+	iommu = of_iommu_configure(dev, np);
+	dev_dbg(dev, "device is%sbehind an iommu\n",
+		iommu ? " " : " not ");
+
+	arch_setup_dma_ops(dev, dma_addr, size, iommu, coherent);
+}
+EXPORT_SYMBOL_GPL(of_dma_configure);
+
 int of_device_register(struct platform_device *pdev)
 {
 	device_initialize(&pdev->dev);
diff --git a/drivers/of/irq.c b/drivers/of/irq.c
index 0d7765807f49..1a7980692f25 100644
--- a/drivers/of/irq.c
+++ b/drivers/of/irq.c
@@ -290,7 +290,7 @@ int of_irq_parse_one(struct device_node *device, int index, struct of_phandle_ar
 	struct device_node *p;
 	const __be32 *intspec, *tmp, *addr;
 	u32 intsize, intlen;
-	int i, res = -EINVAL;
+	int i, res;
 
 	pr_debug("of_irq_parse_one: dev=%s, index=%d\n", of_node_full_name(device), index);
 
@@ -323,15 +323,19 @@ int of_irq_parse_one(struct device_node *device, int index, struct of_phandle_ar
 
 	/* Get size of interrupt specifier */
 	tmp = of_get_property(p, "#interrupt-cells", NULL);
-	if (tmp == NULL)
+	if (tmp == NULL) {
+		res = -EINVAL;
 		goto out;
+	}
 	intsize = be32_to_cpu(*tmp);
 
 	pr_debug(" intsize=%d intlen=%d\n", intsize, intlen);
 
 	/* Check index */
-	if ((index + 1) * intsize > intlen)
+	if ((index + 1) * intsize > intlen) {
+		res = -EINVAL;
 		goto out;
+	}
 
 	/* Copy intspec into irq structure */
 	intspec += index * intsize;
diff --git a/drivers/of/of_pci.c b/drivers/of/of_pci.c
index 62426d81a4d6..5751dc5b6494 100644
--- a/drivers/of/of_pci.c
+++ b/drivers/of/of_pci.c
@@ -2,6 +2,7 @@
 #include <linux/export.h>
 #include <linux/of.h>
 #include <linux/of_address.h>
+#include <linux/of_device.h>
 #include <linux/of_pci.h>
 #include <linux/slab.h>
 
@@ -116,6 +117,26 @@ int of_get_pci_domain_nr(struct device_node *node)
 }
 EXPORT_SYMBOL_GPL(of_get_pci_domain_nr);
 
+/**
+ * of_pci_dma_configure - Setup DMA configuration
+ * @dev: ptr to pci_dev struct of the PCI device
+ *
+ * Function to update PCI devices's DMA configuration using the same
+ * info from the OF node of host bridge's parent (if any).
+ */
+void of_pci_dma_configure(struct pci_dev *pci_dev)
+{
+	struct device *dev = &pci_dev->dev;
+	struct device *bridge = pci_get_host_bridge_device(pci_dev);
+
+	if (!bridge->parent)
+		return;
+
+	of_dma_configure(dev, bridge->parent->of_node);
+	pci_put_host_bridge_device(bridge);
+}
+EXPORT_SYMBOL_GPL(of_pci_dma_configure);
+
 #if defined(CONFIG_OF_ADDRESS)
 /**
  * of_pci_get_host_bridge_resources - Parse PCI host bridge resources from DT
diff --git a/drivers/of/overlay.c b/drivers/of/overlay.c
index 352b4f28f82c..dee9270ba547 100644
--- a/drivers/of/overlay.c
+++ b/drivers/of/overlay.c
@@ -19,6 +19,7 @@
 #include <linux/string.h>
 #include <linux/slab.h>
 #include <linux/err.h>
+#include <linux/idr.h>
 
 #include "of_private.h"
 
@@ -85,7 +86,7 @@ static int of_overlay_apply_single_device_node(struct of_overlay *ov,
 		struct device_node *target, struct device_node *child)
 {
 	const char *cname;
-	struct device_node *tchild, *grandchild;
+	struct device_node *tchild;
 	int ret = 0;
 
 	cname = kbasename(child->full_name);
diff --git a/drivers/of/platform.c b/drivers/of/platform.c
index b189733a1539..a01f57c9e34e 100644
--- a/drivers/of/platform.c
+++ b/drivers/of/platform.c
@@ -19,7 +19,6 @@
 #include <linux/slab.h>
 #include <linux/of_address.h>
 #include <linux/of_device.h>
-#include <linux/of_iommu.h>
 #include <linux/of_irq.h>
 #include <linux/of_platform.h>
 #include <linux/platform_device.h>
@@ -150,59 +149,6 @@ struct platform_device *of_device_alloc(struct device_node *np,
 }
 EXPORT_SYMBOL(of_device_alloc);
 
-/**
- * of_dma_configure - Setup DMA configuration
- * @dev:	Device to apply DMA configuration
- *
- * Try to get devices's DMA configuration from DT and update it
- * accordingly.
- *
- * In case if platform code need to use own special DMA configuration,it
- * can use Platform bus notifier and handle BUS_NOTIFY_ADD_DEVICE event
- * to fix up DMA configuration.
- */
-static void of_dma_configure(struct device *dev)
-{
-	u64 dma_addr, paddr, size;
-	int ret;
-	bool coherent;
-	unsigned long offset;
-	struct iommu_ops *iommu;
-
-	/*
-	 * Set default dma-mask to 32 bit. Drivers are expected to setup
-	 * the correct supported dma_mask.
-	 */
-	dev->coherent_dma_mask = DMA_BIT_MASK(32);
-
-	/*
-	 * Set it to coherent_dma_mask by default if the architecture
-	 * code has not set it.
-	 */
-	if (!dev->dma_mask)
-		dev->dma_mask = &dev->coherent_dma_mask;
-
-	ret = of_dma_get_range(dev->of_node, &dma_addr, &paddr, &size);
-	if (ret < 0) {
-		dma_addr = offset = 0;
-		size = dev->coherent_dma_mask;
-	} else {
-		offset = PFN_DOWN(paddr - dma_addr);
-		dev_dbg(dev, "dma_pfn_offset(%#08lx)\n", offset);
-	}
-	dev->dma_pfn_offset = offset;
-
-	coherent = of_dma_is_coherent(dev->of_node);
-	dev_dbg(dev, "device is%sdma coherent\n",
-		coherent ? " " : " not ");
-
-	iommu = of_iommu_configure(dev);
-	dev_dbg(dev, "device is%sbehind an iommu\n",
-		iommu ? " " : " not ");
-
-	arch_setup_dma_ops(dev, dma_addr, size, iommu, coherent);
-}
-
 static void of_dma_deconfigure(struct device *dev)
 {
 	arch_teardown_dma_ops(dev);
@@ -236,7 +182,7 @@ static struct platform_device *of_platform_device_create_pdata(
 
 	dev->dev.bus = &platform_bus_type;
 	dev->dev.platform_data = platform_data;
-	of_dma_configure(&dev->dev);
+	of_dma_configure(&dev->dev, dev->dev.of_node);
 
 	if (of_device_add(dev) != 0) {
 		of_dma_deconfigure(&dev->dev);
@@ -299,7 +245,7 @@ static struct amba_device *of_amba_device_create(struct device_node *node,
 		dev_set_name(&dev->dev, "%s", bus_id);
 	else
 		of_device_make_bus_id(&dev->dev);
-	of_dma_configure(&dev->dev);
+	of_dma_configure(&dev->dev, dev->dev.of_node);
 
 	/* Allow the HW Peripheral ID to be overridden */
 	prop = of_get_property(node, "arm,primecell-periphid", NULL);
diff --git a/drivers/of/unittest.c b/drivers/of/unittest.c
index 0cf9a236d438..52c45c7df07f 100644
--- a/drivers/of/unittest.c
+++ b/drivers/of/unittest.c
@@ -92,6 +92,16 @@ static void __init of_selftest_find_node_by_name(void)
 		 "option path test failed\n");
 	of_node_put(np);
 
+	np = of_find_node_opts_by_path("/testcase-data:test/option", &options);
+	selftest(np && !strcmp("test/option", options),
+		 "option path test, subcase #1 failed\n");
+	of_node_put(np);
+
+	np = of_find_node_opts_by_path("/testcase-data/testcase-device1:test/option", &options);
+	selftest(np && !strcmp("test/option", options),
+		 "option path test, subcase #2 failed\n");
+	of_node_put(np);
+
 	np = of_find_node_opts_by_path("/testcase-data:testoption", NULL);
 	selftest(np, "NULL option path test failed\n");
 	of_node_put(np);
@@ -102,6 +112,12 @@ static void __init of_selftest_find_node_by_name(void)
 		 "option alias path test failed\n");
 	of_node_put(np);
 
+	np = of_find_node_opts_by_path("testcase-alias:test/alias/option",
+				       &options);
+	selftest(np && !strcmp("test/alias/option", options),
+		 "option alias path test, subcase #1 failed\n");
+	of_node_put(np);
+
 	np = of_find_node_opts_by_path("testcase-alias:testaliasoption", NULL);
 	selftest(np, "NULL option alias path test failed\n");
 	of_node_put(np);
@@ -378,9 +394,9 @@ static void __init of_selftest_property_string(void)
 	rc = of_property_match_string(np, "phandle-list-names", "first");
 	selftest(rc == 0, "first expected:0 got:%i\n", rc);
 	rc = of_property_match_string(np, "phandle-list-names", "second");
-	selftest(rc == 1, "second expected:0 got:%i\n", rc);
+	selftest(rc == 1, "second expected:1 got:%i\n", rc);
 	rc = of_property_match_string(np, "phandle-list-names", "third");
-	selftest(rc == 2, "third expected:0 got:%i\n", rc);
+	selftest(rc == 2, "third expected:2 got:%i\n", rc);
 	rc = of_property_match_string(np, "phandle-list-names", "fourth");
 	selftest(rc == -ENODATA, "unmatched string; rc=%i\n", rc);
 	rc = of_property_match_string(np, "missing-property", "blah");
@@ -478,7 +494,6 @@ static void __init of_selftest_changeset(void)
 	struct device_node *n1, *n2, *n21, *nremove, *parent, *np;
 	struct of_changeset chgset;
 
-	of_changeset_init(&chgset);
 	n1 = __of_node_dup(NULL, "/testcase-data/changeset/n1");
 	selftest(n1, "testcase setup failure\n");
 	n2 = __of_node_dup(NULL, "/testcase-data/changeset/n2");
@@ -979,7 +994,7 @@ static int of_path_platform_device_exists(const char *path)
 	return pdev != NULL;
 }
 
-#if IS_ENABLED(CONFIG_I2C)
+#if IS_BUILTIN(CONFIG_I2C)
 
 /* get the i2c client device instantiated at the path */
 static struct i2c_client *of_path_to_i2c_client(const char *path)
@@ -1445,7 +1460,7 @@ static void of_selftest_overlay_11(void)
 		return;
 }
 
-#if IS_ENABLED(CONFIG_I2C) && IS_ENABLED(CONFIG_OF_OVERLAY)
+#if IS_BUILTIN(CONFIG_I2C) && IS_ENABLED(CONFIG_OF_OVERLAY)
 
 struct selftest_i2c_bus_data {
 	struct platform_device	*pdev;
@@ -1584,7 +1599,7 @@ static struct i2c_driver selftest_i2c_dev_driver = {
 	.id_table = selftest_i2c_dev_id,
 };
 
-#if IS_ENABLED(CONFIG_I2C_MUX)
+#if IS_BUILTIN(CONFIG_I2C_MUX)
 
 struct selftest_i2c_mux_data {
 	int nchans;
@@ -1695,7 +1710,7 @@ static int of_selftest_overlay_i2c_init(void)
 			"could not register selftest i2c bus driver\n"))
 		return ret;
 
-#if IS_ENABLED(CONFIG_I2C_MUX)
+#if IS_BUILTIN(CONFIG_I2C_MUX)
 	ret = i2c_add_driver(&selftest_i2c_mux_driver);
 	if (selftest(ret == 0,
 			"could not register selftest i2c mux driver\n"))
@@ -1707,7 +1722,7 @@ static int of_selftest_overlay_i2c_init(void)
 
 static void of_selftest_overlay_i2c_cleanup(void)
 {
-#if IS_ENABLED(CONFIG_I2C_MUX)
+#if IS_BUILTIN(CONFIG_I2C_MUX)
 	i2c_del_driver(&selftest_i2c_mux_driver);
 #endif
 	platform_driver_unregister(&selftest_i2c_bus_driver);
@@ -1814,7 +1829,7 @@ static void __init of_selftest_overlay(void)
 	of_selftest_overlay_10();
 	of_selftest_overlay_11();
 
-#if IS_ENABLED(CONFIG_I2C)
+#if IS_BUILTIN(CONFIG_I2C)
 	if (selftest(of_selftest_overlay_i2c_init() == 0, "i2c init failed\n"))
 		goto out;
 
diff --git a/drivers/pci/host-bridge.c b/drivers/pci/host-bridge.c
index 39b2dbe585aa..5f4a2e04c8d7 100644
--- a/drivers/pci/host-bridge.c
+++ b/drivers/pci/host-bridge.c
@@ -16,13 +16,27 @@ static struct pci_bus *find_pci_root_bus(struct pci_bus *bus)
 	return bus;
 }
 
-static struct pci_host_bridge *find_pci_host_bridge(struct pci_bus *bus)
+struct pci_host_bridge *pci_find_host_bridge(struct pci_bus *bus)
 {
 	struct pci_bus *root_bus = find_pci_root_bus(bus);
 
 	return to_pci_host_bridge(root_bus->bridge);
 }
 
+struct device *pci_get_host_bridge_device(struct pci_dev *dev)
+{
+	struct pci_bus *root_bus = find_pci_root_bus(dev->bus);
+	struct device *bridge = root_bus->bridge;
+
+	kobject_get(&bridge->kobj);
+	return bridge;
+}
+
+void  pci_put_host_bridge_device(struct device *dev)
+{
+	kobject_put(&dev->kobj);
+}
+
 void pci_set_host_bridge_release(struct pci_host_bridge *bridge,
 				 void (*release_fn)(struct pci_host_bridge *),
 				 void *release_data)
@@ -34,7 +48,7 @@ void pci_set_host_bridge_release(struct pci_host_bridge *bridge,
 void pcibios_resource_to_bus(struct pci_bus *bus, struct pci_bus_region *region,
 			     struct resource *res)
 {
-	struct pci_host_bridge *bridge = find_pci_host_bridge(bus);
+	struct pci_host_bridge *bridge = pci_find_host_bridge(bus);
 	struct resource_entry *window;
 	resource_size_t offset = 0;
 
@@ -59,7 +73,7 @@ static bool region_contains(struct pci_bus_region *region1,
 void pcibios_bus_to_resource(struct pci_bus *bus, struct resource *res,
 			     struct pci_bus_region *region)
 {
-	struct pci_host_bridge *bridge = find_pci_host_bridge(bus);
+	struct pci_host_bridge *bridge = pci_find_host_bridge(bus);
 	struct resource_entry *window;
 	resource_size_t offset = 0;
 
diff --git a/drivers/pci/host/Kconfig b/drivers/pci/host/Kconfig
index 7b892a9cc4fc..1dfb567b3522 100644
--- a/drivers/pci/host/Kconfig
+++ b/drivers/pci/host/Kconfig
@@ -106,4 +106,23 @@ config PCI_VERSATILE
 	bool "ARM Versatile PB PCI controller"
 	depends on ARCH_VERSATILE
 
+config PCIE_IPROC
+	tristate "Broadcom iProc PCIe controller"
+	depends on OF && ARM
+	default n
+	help
+	  This enables the iProc PCIe core controller support for Broadcom's
+	  iProc family of SoCs. An appropriate bus interface driver also needs
+	  to be enabled
+
+config PCIE_IPROC_PLATFORM
+	tristate "Broadcom iProc PCIe platform bus driver"
+	depends on ARCH_BCM_IPROC || (ARM && COMPILE_TEST)
+	depends on OF
+	select PCIE_IPROC
+	default ARCH_BCM_IPROC
+	help
+	  Say Y here if you want to use the Broadcom iProc PCIe controller
+	  through the generic platform bus interface
+
 endmenu
diff --git a/drivers/pci/host/Makefile b/drivers/pci/host/Makefile
index e61d91c92bf1..f733b4e27642 100644
--- a/drivers/pci/host/Makefile
+++ b/drivers/pci/host/Makefile
@@ -13,3 +13,5 @@ obj-$(CONFIG_PCIE_XILINX) += pcie-xilinx.o
 obj-$(CONFIG_PCI_XGENE) += pci-xgene.o
 obj-$(CONFIG_PCI_LAYERSCAPE) += pci-layerscape.o
 obj-$(CONFIG_PCI_VERSATILE) += pci-versatile.o
+obj-$(CONFIG_PCIE_IPROC) += pcie-iproc.o
+obj-$(CONFIG_PCIE_IPROC_PLATFORM) += pcie-iproc-platform.o
diff --git a/drivers/pci/host/pci-exynos.c b/drivers/pci/host/pci-exynos.c
index d202b37c3698..c139237e0e52 100644
--- a/drivers/pci/host/pci-exynos.c
+++ b/drivers/pci/host/pci-exynos.c
@@ -396,7 +396,7 @@ static void exynos_pcie_enable_irq_pulse(struct pcie_port *pp)
 
 	/* enable INTX interrupt */
 	val = IRQ_INTA_ASSERT | IRQ_INTB_ASSERT |
-		IRQ_INTC_ASSERT | IRQ_INTD_ASSERT,
+		IRQ_INTC_ASSERT | IRQ_INTD_ASSERT;
 	exynos_elb_writel(exynos_pcie, val, PCIE_IRQ_EN_PULSE);
 }
 
diff --git a/drivers/pci/host/pci-keystone-dw.c b/drivers/pci/host/pci-keystone-dw.c
index 66d8ea41b972..f34892e0edb4 100644
--- a/drivers/pci/host/pci-keystone-dw.c
+++ b/drivers/pci/host/pci-keystone-dw.c
@@ -496,11 +496,12 @@ int __init ks_dw_pcie_host_init(struct keystone_pcie *ks_pcie,
 
 	/* Index 1 is the application reg. space address */
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 1);
-	ks_pcie->app = *res;
 	ks_pcie->va_app_base = devm_ioremap_resource(pp->dev, res);
 	if (IS_ERR(ks_pcie->va_app_base))
 		return PTR_ERR(ks_pcie->va_app_base);
 
+	ks_pcie->app = *res;
+
 	/* Create legacy IRQ domain */
 	ks_pcie->legacy_irq_domain =
 			irq_domain_add_linear(ks_pcie->legacy_intc_np,
diff --git a/drivers/pci/host/pci-layerscape.c b/drivers/pci/host/pci-layerscape.c
index 68c9e5e9b0a8..4a6e62f67579 100644
--- a/drivers/pci/host/pci-layerscape.c
+++ b/drivers/pci/host/pci-layerscape.c
@@ -127,14 +127,11 @@ static int __init ls_pcie_probe(struct platform_device *pdev)
 	pcie->dev = &pdev->dev;
 
 	dbi_base = platform_get_resource_byname(pdev, IORESOURCE_MEM, "regs");
-	if (!dbi_base) {
-		dev_err(&pdev->dev, "missing *regs* space\n");
-		return -ENODEV;
-	}
-
 	pcie->dbi = devm_ioremap_resource(&pdev->dev, dbi_base);
-	if (IS_ERR(pcie->dbi))
+	if (IS_ERR(pcie->dbi)) {
+		dev_err(&pdev->dev, "missing *regs* space\n");
 		return PTR_ERR(pcie->dbi);
+	}
 
 	pcie->scfg = syscon_regmap_lookup_by_phandle(pdev->dev.of_node,
 						     "fsl,pcie-scfg");
diff --git a/drivers/pci/host/pci-mvebu.c b/drivers/pci/host/pci-mvebu.c
index 1309cfbaa719..1ab863551920 100644
--- a/drivers/pci/host/pci-mvebu.c
+++ b/drivers/pci/host/pci-mvebu.c
@@ -129,6 +129,7 @@ struct mvebu_pcie_port {
 	size_t memwin_size;
 	phys_addr_t iowin_base;
 	size_t iowin_size;
+	u32 saved_pcie_stat;
 };
 
 static inline void mvebu_writel(struct mvebu_pcie_port *port, u32 val, u32 reg)
@@ -899,6 +900,35 @@ static void mvebu_pcie_msi_enable(struct mvebu_pcie *pcie)
 		pcie->msi->dev = &pcie->pdev->dev;
 }
 
+static int mvebu_pcie_suspend(struct device *dev)
+{
+	struct mvebu_pcie *pcie;
+	int i;
+
+	pcie = dev_get_drvdata(dev);
+	for (i = 0; i < pcie->nports; i++) {
+		struct mvebu_pcie_port *port = pcie->ports + i;
+		port->saved_pcie_stat = mvebu_readl(port, PCIE_STAT_OFF);
+	}
+
+	return 0;
+}
+
+static int mvebu_pcie_resume(struct device *dev)
+{
+	struct mvebu_pcie *pcie;
+	int i;
+
+	pcie = dev_get_drvdata(dev);
+	for (i = 0; i < pcie->nports; i++) {
+		struct mvebu_pcie_port *port = pcie->ports + i;
+		mvebu_writel(port, port->saved_pcie_stat, PCIE_STAT_OFF);
+		mvebu_pcie_setup_hw(port);
+	}
+
+	return 0;
+}
+
 static int mvebu_pcie_probe(struct platform_device *pdev)
 {
 	struct mvebu_pcie *pcie;
@@ -1056,6 +1086,8 @@ static int mvebu_pcie_probe(struct platform_device *pdev)
 	mvebu_pcie_msi_enable(pcie);
 	mvebu_pcie_enable(pcie);
 
+	platform_set_drvdata(pdev, pcie);
+
 	return 0;
 }
 
@@ -1068,12 +1100,18 @@ static const struct of_device_id mvebu_pcie_of_match_table[] = {
 };
 MODULE_DEVICE_TABLE(of, mvebu_pcie_of_match_table);
 
+static struct dev_pm_ops mvebu_pcie_pm_ops = {
+	.suspend_noirq = mvebu_pcie_suspend,
+	.resume_noirq = mvebu_pcie_resume,
+};
+
 static struct platform_driver mvebu_pcie_driver = {
 	.driver = {
 		.name = "mvebu-pcie",
 		.of_match_table = mvebu_pcie_of_match_table,
 		/* driver unloading/unbinding currently not supported */
 		.suppress_bind_attrs = true,
+		.pm = &mvebu_pcie_pm_ops,
 	},
 	.probe = mvebu_pcie_probe,
 };
diff --git a/drivers/pci/host/pci-rcar-gen2.c b/drivers/pci/host/pci-rcar-gen2.c
index dd6b84e6206c..367e28fa7564 100644
--- a/drivers/pci/host/pci-rcar-gen2.c
+++ b/drivers/pci/host/pci-rcar-gen2.c
@@ -301,6 +301,9 @@ static int rcar_pci_probe(struct platform_device *pdev)
 	if (!mem_res || !mem_res->start)
 		return -ENODEV;
 
+	if (mem_res->start & 0xFFFF)
+		return -EINVAL;
+
 	priv = devm_kzalloc(&pdev->dev,
 			    sizeof(struct rcar_pci_priv), GFP_KERNEL);
 	if (!priv)
diff --git a/drivers/pci/host/pci-versatile.c b/drivers/pci/host/pci-versatile.c
index 1ec694a52379..0863d9cc25f8 100644
--- a/drivers/pci/host/pci-versatile.c
+++ b/drivers/pci/host/pci-versatile.c
@@ -80,7 +80,7 @@ static int versatile_pci_parse_request_of_pci_ranges(struct device *dev,
 	if (err)
 		return err;
 
-	resource_list_for_each_entry(win, res, list) {
+	resource_list_for_each_entry(win, res) {
 		struct resource *parent, *res = win->res;
 
 		switch (resource_type(res)) {
@@ -138,19 +138,19 @@ static int versatile_pci_probe(struct platform_device *pdev)
 	LIST_HEAD(pci_res);
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (!res)
-		return -ENODEV;
 	versatile_pci_base = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(versatile_pci_base))
+		return PTR_ERR(versatile_pci_base);
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 1);
-	if (!res)
-		return -ENODEV;
 	versatile_cfg_base[0] = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(versatile_cfg_base[0]))
+		return PTR_ERR(versatile_cfg_base[0]);
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 2);
-	if (!res)
-		return -ENODEV;
 	versatile_cfg_base[1] = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(versatile_cfg_base[1]))
+		return PTR_ERR(versatile_cfg_base[1]);
 
 	ret = versatile_pci_parse_request_of_pci_ranges(&pdev->dev, &pci_res);
 	if (ret)
@@ -214,6 +214,7 @@ static int versatile_pci_probe(struct platform_device *pdev)
 
 	pci_fixup_irqs(pci_common_swizzle, of_irq_parse_and_map_pci);
 	pci_assign_unassigned_bus_resources(bus);
+	pci_bus_add_devices(bus);
 
 	return 0;
 }
diff --git a/drivers/pci/host/pci-xgene.c b/drivers/pci/host/pci-xgene.c
index aab55474dd0d..ee082c0366ec 100644
--- a/drivers/pci/host/pci-xgene.c
+++ b/drivers/pci/host/pci-xgene.c
@@ -127,7 +127,7 @@ static bool xgene_pcie_hide_rc_bars(struct pci_bus *bus, int offset)
 	return false;
 }
 
-static int xgene_pcie_map_bus(struct pci_bus *bus, unsigned int devfn,
+static void __iomem *xgene_pcie_map_bus(struct pci_bus *bus, unsigned int devfn,
 			      int offset)
 {
 	struct xgene_pcie_port *port = bus->sysdata;
@@ -137,7 +137,7 @@ static int xgene_pcie_map_bus(struct pci_bus *bus, unsigned int devfn,
 		return NULL;
 
 	xgene_pcie_set_rtdid_reg(bus, devfn);
-	return xgene_pcie_get_cfg_base(bus);
+	return xgene_pcie_get_cfg_base(bus) + offset;
 }
 
 static struct pci_ops xgene_pcie_ops = {
diff --git a/drivers/pci/host/pcie-designware.c b/drivers/pci/host/pcie-designware.c
index 1f4ea6f2d910..2e9f84fdd9ce 100644
--- a/drivers/pci/host/pcie-designware.c
+++ b/drivers/pci/host/pcie-designware.c
@@ -342,7 +342,7 @@ static const struct irq_domain_ops msi_domain_ops = {
 	.map = dw_pcie_msi_map,
 };
 
-int __init dw_pcie_host_init(struct pcie_port *pp)
+int dw_pcie_host_init(struct pcie_port *pp)
 {
 	struct device_node *np = pp->dev->of_node;
 	struct platform_device *pdev = to_platform_device(pp->dev);
diff --git a/drivers/pci/host/pcie-iproc-platform.c b/drivers/pci/host/pcie-iproc-platform.c
new file mode 100644
index 000000000000..afad6c21fcfa
--- /dev/null
+++ b/drivers/pci/host/pcie-iproc-platform.c
@@ -0,0 +1,108 @@
+/*
+ * Copyright (C) 2015 Broadcom Corporation
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation version 2.
+ *
+ * This program is distributed "as is" WITHOUT ANY WARRANTY of any
+ * kind, whether express or implied; without even the implied warranty
+ * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/kernel.h>
+#include <linux/pci.h>
+#include <linux/clk.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/interrupt.h>
+#include <linux/platform_device.h>
+#include <linux/of_address.h>
+#include <linux/of_pci.h>
+#include <linux/of_irq.h>
+#include <linux/of_platform.h>
+#include <linux/phy/phy.h>
+
+#include "pcie-iproc.h"
+
+static int iproc_pcie_pltfm_probe(struct platform_device *pdev)
+{
+	struct iproc_pcie *pcie;
+	struct device_node *np = pdev->dev.of_node;
+	struct resource reg;
+	resource_size_t iobase = 0;
+	LIST_HEAD(res);
+	int ret;
+
+	pcie = devm_kzalloc(&pdev->dev, sizeof(struct iproc_pcie), GFP_KERNEL);
+	if (!pcie)
+		return -ENOMEM;
+
+	pcie->dev = &pdev->dev;
+	platform_set_drvdata(pdev, pcie);
+
+	ret = of_address_to_resource(np, 0, &reg);
+	if (ret < 0) {
+		dev_err(pcie->dev, "unable to obtain controller resources\n");
+		return ret;
+	}
+
+	pcie->base = devm_ioremap(pcie->dev, reg.start, resource_size(&reg));
+	if (!pcie->base) {
+		dev_err(pcie->dev, "unable to map controller registers\n");
+		return -ENOMEM;
+	}
+
+	/* PHY use is optional */
+	pcie->phy = devm_phy_get(&pdev->dev, "pcie-phy");
+	if (IS_ERR(pcie->phy)) {
+		if (PTR_ERR(pcie->phy) == -EPROBE_DEFER)
+			return -EPROBE_DEFER;
+		pcie->phy = NULL;
+	}
+
+	ret = of_pci_get_host_bridge_resources(np, 0, 0xff, &res, &iobase);
+	if (ret) {
+		dev_err(pcie->dev,
+			"unable to get PCI host bridge resources\n");
+		return ret;
+	}
+
+	pcie->resources = &res;
+
+	ret = iproc_pcie_setup(pcie);
+	if (ret) {
+		dev_err(pcie->dev, "PCIe controller setup failed\n");
+		return ret;
+	}
+
+	return 0;
+}
+
+static int iproc_pcie_pltfm_remove(struct platform_device *pdev)
+{
+	struct iproc_pcie *pcie = platform_get_drvdata(pdev);
+
+	return iproc_pcie_remove(pcie);
+}
+
+static const struct of_device_id iproc_pcie_of_match_table[] = {
+	{ .compatible = "brcm,iproc-pcie", },
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(of, iproc_pcie_of_match_table);
+
+static struct platform_driver iproc_pcie_pltfm_driver = {
+	.driver = {
+		.name = "iproc-pcie",
+		.of_match_table = of_match_ptr(iproc_pcie_of_match_table),
+	},
+	.probe = iproc_pcie_pltfm_probe,
+	.remove = iproc_pcie_pltfm_remove,
+};
+module_platform_driver(iproc_pcie_pltfm_driver);
+
+MODULE_AUTHOR("Ray Jui <rjui@broadcom.com>");
+MODULE_DESCRIPTION("Broadcom iPROC PCIe platform driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/pci/host/pcie-iproc.c b/drivers/pci/host/pcie-iproc.c
new file mode 100644
index 000000000000..329e1b54528b
--- /dev/null
+++ b/drivers/pci/host/pcie-iproc.c
@@ -0,0 +1,268 @@
+/*
+ * Copyright (C) 2014 Hauke Mehrtens <hauke@hauke-m.de>
+ * Copyright (C) 2015 Broadcom Corporatcommon ion
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation version 2.
+ *
+ * This program is distributed "as is" WITHOUT ANY WARRANTY of any
+ * kind, whether express or implied; without even the implied warranty
+ * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/kernel.h>
+#include <linux/pci.h>
+#include <linux/msi.h>
+#include <linux/clk.h>
+#include <linux/module.h>
+#include <linux/mbus.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include <linux/interrupt.h>
+#include <linux/platform_device.h>
+#include <linux/of_address.h>
+#include <linux/of_pci.h>
+#include <linux/of_irq.h>
+#include <linux/of_platform.h>
+#include <linux/phy/phy.h>
+
+#include "pcie-iproc.h"
+
+#define CLK_CONTROL_OFFSET           0x000
+#define EP_MODE_SURVIVE_PERST_SHIFT  1
+#define EP_MODE_SURVIVE_PERST        BIT(EP_MODE_SURVIVE_PERST_SHIFT)
+#define RC_PCIE_RST_OUTPUT_SHIFT     0
+#define RC_PCIE_RST_OUTPUT           BIT(RC_PCIE_RST_OUTPUT_SHIFT)
+
+#define CFG_IND_ADDR_OFFSET          0x120
+#define CFG_IND_ADDR_MASK            0x00001ffc
+
+#define CFG_IND_DATA_OFFSET          0x124
+
+#define CFG_ADDR_OFFSET              0x1f8
+#define CFG_ADDR_BUS_NUM_SHIFT       20
+#define CFG_ADDR_BUS_NUM_MASK        0x0ff00000
+#define CFG_ADDR_DEV_NUM_SHIFT       15
+#define CFG_ADDR_DEV_NUM_MASK        0x000f8000
+#define CFG_ADDR_FUNC_NUM_SHIFT      12
+#define CFG_ADDR_FUNC_NUM_MASK       0x00007000
+#define CFG_ADDR_REG_NUM_SHIFT       2
+#define CFG_ADDR_REG_NUM_MASK        0x00000ffc
+#define CFG_ADDR_CFG_TYPE_SHIFT      0
+#define CFG_ADDR_CFG_TYPE_MASK       0x00000003
+
+#define CFG_DATA_OFFSET              0x1fc
+
+#define SYS_RC_INTX_EN               0x330
+#define SYS_RC_INTX_MASK             0xf
+
+static inline struct iproc_pcie *sys_to_pcie(struct pci_sys_data *sys)
+{
+	return sys->private_data;
+}
+
+/**
+ * Note access to the configuration registers are protected at the higher layer
+ * by 'pci_lock' in drivers/pci/access.c
+ */
+static void __iomem *iproc_pcie_map_cfg_bus(struct pci_bus *bus,
+					    unsigned int devfn,
+					    int where)
+{
+	struct pci_sys_data *sys = bus->sysdata;
+	struct iproc_pcie *pcie = sys_to_pcie(sys);
+	unsigned slot = PCI_SLOT(devfn);
+	unsigned fn = PCI_FUNC(devfn);
+	unsigned busno = bus->number;
+	u32 val;
+
+	/* root complex access */
+	if (busno == 0) {
+		if (slot >= 1)
+			return NULL;
+		writel(where & CFG_IND_ADDR_MASK,
+		       pcie->base + CFG_IND_ADDR_OFFSET);
+		return (pcie->base + CFG_IND_DATA_OFFSET);
+	}
+
+	if (fn > 1)
+		return NULL;
+
+	/* EP device access */
+	val = (busno << CFG_ADDR_BUS_NUM_SHIFT) |
+		(slot << CFG_ADDR_DEV_NUM_SHIFT) |
+		(fn << CFG_ADDR_FUNC_NUM_SHIFT) |
+		(where & CFG_ADDR_REG_NUM_MASK) |
+		(1 & CFG_ADDR_CFG_TYPE_MASK);
+	writel(val, pcie->base + CFG_ADDR_OFFSET);
+
+	return (pcie->base + CFG_DATA_OFFSET);
+}
+
+static struct pci_ops iproc_pcie_ops = {
+	.map_bus = iproc_pcie_map_cfg_bus,
+	.read = pci_generic_config_read32,
+	.write = pci_generic_config_write32,
+};
+
+static void iproc_pcie_reset(struct iproc_pcie *pcie)
+{
+	u32 val;
+
+	/*
+	 * Configure the PCIe controller as root complex and send a downstream
+	 * reset
+	 */
+	val = EP_MODE_SURVIVE_PERST | RC_PCIE_RST_OUTPUT;
+	writel(val, pcie->base + CLK_CONTROL_OFFSET);
+	udelay(250);
+	val &= ~EP_MODE_SURVIVE_PERST;
+	writel(val, pcie->base + CLK_CONTROL_OFFSET);
+	msleep(250);
+}
+
+static int iproc_pcie_check_link(struct iproc_pcie *pcie, struct pci_bus *bus)
+{
+	u8 hdr_type;
+	u32 link_ctrl;
+	u16 pos, link_status;
+	int link_is_active = 0;
+
+	/* make sure we are not in EP mode */
+	pci_bus_read_config_byte(bus, 0, PCI_HEADER_TYPE, &hdr_type);
+	if ((hdr_type & 0x7f) != PCI_HEADER_TYPE_BRIDGE) {
+		dev_err(pcie->dev, "in EP mode, hdr=%#02x\n", hdr_type);
+		return -EFAULT;
+	}
+
+	/* force class to PCI_CLASS_BRIDGE_PCI (0x0604) */
+	pci_bus_write_config_word(bus, 0, PCI_CLASS_DEVICE,
+				  PCI_CLASS_BRIDGE_PCI);
+
+	/* check link status to see if link is active */
+	pos = pci_bus_find_capability(bus, 0, PCI_CAP_ID_EXP);
+	pci_bus_read_config_word(bus, 0, pos + PCI_EXP_LNKSTA, &link_status);
+	if (link_status & PCI_EXP_LNKSTA_NLW)
+		link_is_active = 1;
+
+	if (!link_is_active) {
+		/* try GEN 1 link speed */
+#define PCI_LINK_STATUS_CTRL_2_OFFSET 0x0dc
+#define PCI_TARGET_LINK_SPEED_MASK    0xf
+#define PCI_TARGET_LINK_SPEED_GEN2    0x2
+#define PCI_TARGET_LINK_SPEED_GEN1    0x1
+		pci_bus_read_config_dword(bus, 0,
+					  PCI_LINK_STATUS_CTRL_2_OFFSET,
+					  &link_ctrl);
+		if ((link_ctrl & PCI_TARGET_LINK_SPEED_MASK) ==
+		    PCI_TARGET_LINK_SPEED_GEN2) {
+			link_ctrl &= ~PCI_TARGET_LINK_SPEED_MASK;
+			link_ctrl |= PCI_TARGET_LINK_SPEED_GEN1;
+			pci_bus_write_config_dword(bus, 0,
+					   PCI_LINK_STATUS_CTRL_2_OFFSET,
+					   link_ctrl);
+			msleep(100);
+
+			pos = pci_bus_find_capability(bus, 0, PCI_CAP_ID_EXP);
+			pci_bus_read_config_word(bus, 0, pos + PCI_EXP_LNKSTA,
+						 &link_status);
+			if (link_status & PCI_EXP_LNKSTA_NLW)
+				link_is_active = 1;
+		}
+	}
+
+	dev_info(pcie->dev, "link: %s\n", link_is_active ? "UP" : "DOWN");
+
+	return link_is_active ? 0 : -ENODEV;
+}
+
+static void iproc_pcie_enable(struct iproc_pcie *pcie)
+{
+	writel(SYS_RC_INTX_MASK, pcie->base + SYS_RC_INTX_EN);
+}
+
+int iproc_pcie_setup(struct iproc_pcie *pcie)
+{
+	int ret;
+	struct pci_bus *bus;
+
+	if (!pcie || !pcie->dev || !pcie->base)
+		return -EINVAL;
+
+	if (pcie->phy) {
+		ret = phy_init(pcie->phy);
+		if (ret) {
+			dev_err(pcie->dev, "unable to initialize PCIe PHY\n");
+			return ret;
+		}
+
+		ret = phy_power_on(pcie->phy);
+		if (ret) {
+			dev_err(pcie->dev, "unable to power on PCIe PHY\n");
+			goto err_exit_phy;
+		}
+
+	}
+
+	iproc_pcie_reset(pcie);
+
+	pcie->sysdata.private_data = pcie;
+
+	bus = pci_create_root_bus(pcie->dev, 0, &iproc_pcie_ops,
+				  &pcie->sysdata, pcie->resources);
+	if (!bus) {
+		dev_err(pcie->dev, "unable to create PCI root bus\n");
+		ret = -ENOMEM;
+		goto err_power_off_phy;
+	}
+	pcie->root_bus = bus;
+
+	ret = iproc_pcie_check_link(pcie, bus);
+	if (ret) {
+		dev_err(pcie->dev, "no PCIe EP device detected\n");
+		goto err_rm_root_bus;
+	}
+
+	iproc_pcie_enable(pcie);
+
+	pci_scan_child_bus(bus);
+	pci_assign_unassigned_bus_resources(bus);
+	pci_fixup_irqs(pci_common_swizzle, of_irq_parse_and_map_pci);
+	pci_bus_add_devices(bus);
+
+	return 0;
+
+err_rm_root_bus:
+	pci_stop_root_bus(bus);
+	pci_remove_root_bus(bus);
+
+err_power_off_phy:
+	if (pcie->phy)
+		phy_power_off(pcie->phy);
+err_exit_phy:
+	if (pcie->phy)
+		phy_exit(pcie->phy);
+
+	return ret;
+}
+EXPORT_SYMBOL(iproc_pcie_setup);
+
+int iproc_pcie_remove(struct iproc_pcie *pcie)
+{
+	pci_stop_root_bus(pcie->root_bus);
+	pci_remove_root_bus(pcie->root_bus);
+
+	if (pcie->phy) {
+		phy_power_off(pcie->phy);
+		phy_exit(pcie->phy);
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL(iproc_pcie_remove);
+
+MODULE_AUTHOR("Ray Jui <rjui@broadcom.com>");
+MODULE_DESCRIPTION("Broadcom iPROC PCIe common driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/pci/host/pcie-iproc.h b/drivers/pci/host/pcie-iproc.h
new file mode 100644
index 000000000000..e28075ed1856
--- /dev/null
+++ b/drivers/pci/host/pcie-iproc.h
@@ -0,0 +1,42 @@
+/*
+ * Copyright (C) 2014-2015 Broadcom Corporation
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation version 2.
+ *
+ * This program is distributed "as is" WITHOUT ANY WARRANTY of any
+ * kind, whether express or implied; without even the implied warranty
+ * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#ifndef _PCIE_IPROC_H
+#define _PCIE_IPROC_H
+
+#define IPROC_PCIE_MAX_NUM_IRQS 6
+
+/**
+ * iProc PCIe device
+ * @dev: pointer to device data structure
+ * @base: PCIe host controller I/O register base
+ * @resources: linked list of all PCI resources
+ * @sysdata: Per PCI controller data
+ * @root_bus: pointer to root bus
+ * @phy: optional PHY device that controls the Serdes
+ * @irqs: interrupt IDs
+ */
+struct iproc_pcie {
+	struct device *dev;
+	void __iomem *base;
+	struct list_head *resources;
+	struct pci_sys_data sysdata;
+	struct pci_bus *root_bus;
+	struct phy *phy;
+	int irqs[IPROC_PCIE_MAX_NUM_IRQS];
+};
+
+int iproc_pcie_setup(struct iproc_pcie *pcie);
+int iproc_pcie_remove(struct iproc_pcie *pcie);
+
+#endif /* _PCIE_IPROC_H */
diff --git a/drivers/pci/host/pcie-rcar.c b/drivers/pci/host/pcie-rcar.c
index c57bd0ac39a0..c086210f2ffd 100644
--- a/drivers/pci/host/pcie-rcar.c
+++ b/drivers/pci/host/pcie-rcar.c
@@ -64,8 +64,8 @@
 #define  LAR_ENABLE		(1 << 1)
 
 /* PCIe address reg & mask */
-#define PCIEPARL(x)		(0x03400 + ((x) * 0x20))
-#define PCIEPARH(x)		(0x03404 + ((x) * 0x20))
+#define PCIEPALR(x)		(0x03400 + ((x) * 0x20))
+#define PCIEPAUR(x)		(0x03404 + ((x) * 0x20))
 #define PCIEPAMR(x)		(0x03408 + ((x) * 0x20))
 #define PCIEPTCTLR(x)		(0x0340c + ((x) * 0x20))
 #define  PAR_ENABLE		(1 << 31)
@@ -341,8 +341,9 @@ static void rcar_pcie_setup_window(int win, struct rcar_pcie *pcie)
 	else
 		res_start = res->start;
 
-	rcar_pci_write_reg(pcie, upper_32_bits(res_start), PCIEPARH(win));
-	rcar_pci_write_reg(pcie, lower_32_bits(res_start), PCIEPARL(win));
+	rcar_pci_write_reg(pcie, upper_32_bits(res_start), PCIEPAUR(win));
+	rcar_pci_write_reg(pcie, lower_32_bits(res_start) & ~0x7F,
+			   PCIEPALR(win));
 
 	/* First resource is for IO */
 	mask = PAR_ENABLE;
@@ -501,7 +502,7 @@ static int rcar_pcie_hw_init(struct rcar_pcie *pcie)
 
 	/* Enable MSI */
 	if (IS_ENABLED(CONFIG_PCI_MSI))
-		rcar_pci_write_reg(pcie, 0x101f0000, PCIEMSITXR);
+		rcar_pci_write_reg(pcie, 0x801f0000, PCIEMSITXR);
 
 	/* Finish initialization - establish a PCI Express link */
 	rcar_pci_write_reg(pcie, CFINIT, PCIETCTLR);
diff --git a/drivers/pci/host/pcie-spear13xx.c b/drivers/pci/host/pcie-spear13xx.c
index 866465fd3dbf..020d78890719 100644
--- a/drivers/pci/host/pcie-spear13xx.c
+++ b/drivers/pci/host/pcie-spear13xx.c
@@ -269,7 +269,7 @@ static struct pcie_host_ops spear13xx_pcie_host_ops = {
 	.host_init = spear13xx_pcie_host_init,
 };
 
-static int __init spear13xx_add_pcie_port(struct pcie_port *pp,
+static int spear13xx_add_pcie_port(struct pcie_port *pp,
 					 struct platform_device *pdev)
 {
 	struct device *dev = &pdev->dev;
@@ -299,7 +299,7 @@ static int __init spear13xx_add_pcie_port(struct pcie_port *pp,
 	return 0;
 }
 
-static int __init spear13xx_pcie_probe(struct platform_device *pdev)
+static int spear13xx_pcie_probe(struct platform_device *pdev)
 {
 	struct spear13xx_pcie *spear13xx_pcie;
 	struct pcie_port *pp;
@@ -370,7 +370,7 @@ static const struct of_device_id spear13xx_pcie_of_match[] = {
 };
 MODULE_DEVICE_TABLE(of, spear13xx_pcie_of_match);
 
-static struct platform_driver spear13xx_pcie_driver __initdata = {
+static struct platform_driver spear13xx_pcie_driver = {
 	.probe		= spear13xx_pcie_probe,
 	.driver = {
 		.name	= "spear-pcie",
diff --git a/drivers/pci/hotplug/cpci_hotplug_pci.c b/drivers/pci/hotplug/cpci_hotplug_pci.c
index 7d48ecae6695..788db48dbbad 100644
--- a/drivers/pci/hotplug/cpci_hotplug_pci.c
+++ b/drivers/pci/hotplug/cpci_hotplug_pci.c
@@ -286,11 +286,12 @@ int cpci_configure_slot(struct slot *slot)
 	}
 	parent = slot->dev->bus;
 
-	list_for_each_entry(dev, &parent->devices, bus_list)
+	list_for_each_entry(dev, &parent->devices, bus_list) {
 		if (PCI_SLOT(dev->devfn) != PCI_SLOT(slot->devfn))
 			continue;
 		if (pci_is_bridge(dev))
 			pci_hp_add_bridge(dev);
+	}
 
 
 	pci_assign_unassigned_bridge_resources(parent->self);
diff --git a/drivers/pci/hotplug/ibmphp_core.c b/drivers/pci/hotplug/ibmphp_core.c
index 96c5c729cdbc..15302475f5b7 100644
--- a/drivers/pci/hotplug/ibmphp_core.c
+++ b/drivers/pci/hotplug/ibmphp_core.c
@@ -738,7 +738,7 @@ static void ibm_unconfigure_device(struct pci_func *func)
  */
 static u8 bus_structure_fixup(u8 busno)
 {
-	struct pci_bus *bus;
+	struct pci_bus *bus, *b;
 	struct pci_dev *dev;
 	u16 l;
 
@@ -765,7 +765,11 @@ static u8 bus_structure_fixup(u8 busno)
 					(l != 0x0000) && (l != 0xffff)) {
 			debug("%s - Inside bus_structure_fixup()\n",
 							__func__);
-			pci_scan_bus(busno, ibmphp_pci_bus->ops, NULL);
+			b = pci_scan_bus(busno, ibmphp_pci_bus->ops, NULL);
+			if (!b)
+				continue;
+
+			pci_bus_add_devices(b);
 			break;
 		}
 	}
diff --git a/drivers/pci/pci-acpi.c b/drivers/pci/pci-acpi.c
index 489063987325..6f6f175f51f7 100644
--- a/drivers/pci/pci-acpi.c
+++ b/drivers/pci/pci-acpi.c
@@ -18,6 +18,15 @@
 #include <linux/pm_qos.h>
 #include "pci.h"
 
+/*
+ * The UUID is defined in the PCI Firmware Specification available here:
+ * https://www.pcisig.com/members/downloads/pcifw_r3_1_13Dec10.pdf
+ */
+const u8 pci_acpi_dsm_uuid[] = {
+	0xd0, 0x37, 0xc9, 0xe5, 0x53, 0x35, 0x7a, 0x4d,
+	0x91, 0x17, 0xea, 0x4d, 0x19, 0xc3, 0x43, 0x4d
+};
+
 phys_addr_t acpi_pci_root_get_mcfg_addr(acpi_handle handle)
 {
 	acpi_status status = AE_NOT_EXIST;
@@ -248,6 +257,9 @@ int pci_get_hp_params(struct pci_dev *dev, struct hotplug_params *hpp)
 	acpi_handle handle, phandle;
 	struct pci_bus *pbus;
 
+	if (acpi_pci_disabled)
+		return -ENODEV;
+
 	handle = NULL;
 	for (pbus = dev->bus; pbus; pbus = pbus->parent) {
 		handle = acpi_pci_get_bridge_handle(pbus);
@@ -528,11 +540,32 @@ static struct pci_platform_pm_ops acpi_pci_platform_pm = {
 
 void acpi_pci_add_bus(struct pci_bus *bus)
 {
+	union acpi_object *obj;
+	struct pci_host_bridge *bridge;
+
 	if (acpi_pci_disabled || !bus->bridge)
 		return;
 
 	acpi_pci_slot_enumerate(bus);
 	acpiphp_enumerate_slots(bus);
+
+	/*
+	 * For a host bridge, check its _DSM for function 8 and if
+	 * that is available, mark it in pci_host_bridge.
+	 */
+	if (!pci_is_root_bus(bus))
+		return;
+
+	obj = acpi_evaluate_dsm(ACPI_HANDLE(bus->bridge), pci_acpi_dsm_uuid, 3,
+				RESET_DELAY_DSM, NULL);
+	if (!obj)
+		return;
+
+	if (obj->type == ACPI_TYPE_INTEGER && obj->integer.value == 1) {
+		bridge = pci_find_host_bridge(bus);
+		bridge->ignore_reset_delay = 1;
+	}
+	ACPI_FREE(obj);
 }
 
 void acpi_pci_remove_bus(struct pci_bus *bus)
@@ -558,6 +591,57 @@ static struct acpi_device *acpi_pci_find_companion(struct device *dev)
 				      check_children);
 }
 
+/**
+ * pci_acpi_optimize_delay - optimize PCI D3 and D3cold delay from ACPI
+ * @pdev: the PCI device whose delay is to be updated
+ * @adev: the companion ACPI device of this PCI device
+ *
+ * Update the d3_delay and d3cold_delay of a PCI device from the ACPI _DSM
+ * control method of either the device itself or the PCI host bridge.
+ *
+ * Function 8, "Reset Delay," applies to the entire hierarchy below a PCI
+ * host bridge.  If it returns one, the OS may assume that all devices in
+ * the hierarchy have already completed power-on reset delays.
+ *
+ * Function 9, "Device Readiness Durations," applies only to the object
+ * where it is located.  It returns delay durations required after various
+ * events if the device requires less time than the spec requires.  Delays
+ * from this function take precedence over the Reset Delay function.
+ *
+ * These _DSM functions are defined by the draft ECN of January 28, 2014,
+ * titled "ACPI additions for FW latency optimizations."
+ */
+static void pci_acpi_optimize_delay(struct pci_dev *pdev,
+				    acpi_handle handle)
+{
+	struct pci_host_bridge *bridge = pci_find_host_bridge(pdev->bus);
+	int value;
+	union acpi_object *obj, *elements;
+
+	if (bridge->ignore_reset_delay)
+		pdev->d3cold_delay = 0;
+
+	obj = acpi_evaluate_dsm(handle, pci_acpi_dsm_uuid, 3,
+				FUNCTION_DELAY_DSM, NULL);
+	if (!obj)
+		return;
+
+	if (obj->type == ACPI_TYPE_PACKAGE && obj->package.count == 5) {
+		elements = obj->package.elements;
+		if (elements[0].type == ACPI_TYPE_INTEGER) {
+			value = (int)elements[0].integer.value / 1000;
+			if (value < PCI_PM_D3COLD_WAIT)
+				pdev->d3cold_delay = value;
+		}
+		if (elements[3].type == ACPI_TYPE_INTEGER) {
+			value = (int)elements[3].integer.value / 1000;
+			if (value < PCI_PM_D3_WAIT)
+				pdev->d3_delay = value;
+		}
+	}
+	ACPI_FREE(obj);
+}
+
 static void pci_acpi_setup(struct device *dev)
 {
 	struct pci_dev *pci_dev = to_pci_dev(dev);
@@ -566,6 +650,8 @@ static void pci_acpi_setup(struct device *dev)
 	if (!adev)
 		return;
 
+	pci_acpi_optimize_delay(pci_dev, adev->handle);
+
 	pci_acpi_add_pm_notifier(adev, pci_dev);
 	if (!adev->wakeup.flags.valid)
 		return;
diff --git a/drivers/pci/pci-label.c b/drivers/pci/pci-label.c
index 2ab1b47c7651..024b5c179348 100644
--- a/drivers/pci/pci-label.c
+++ b/drivers/pci/pci-label.c
@@ -31,8 +31,6 @@
 #include <linux/pci-acpi.h>
 #include "pci.h"
 
-#define	DEVICE_LABEL_DSM	0x07
-
 #ifdef CONFIG_DMI
 enum smbios_attr_enum {
 	SMBIOS_ATTR_NONE = 0,
@@ -148,11 +146,6 @@ static inline void pci_remove_smbiosname_file(struct pci_dev *pdev)
 #endif
 
 #ifdef CONFIG_ACPI
-static const char device_label_dsm_uuid[] = {
-	0xD0, 0x37, 0xC9, 0xE5, 0x53, 0x35, 0x7A, 0x4D,
-	0x91, 0x17, 0xEA, 0x4D, 0x19, 0xC3, 0x43, 0x4D
-};
-
 enum acpi_attr_enum {
 	ACPI_ATTR_LABEL_SHOW,
 	ACPI_ATTR_INDEX_SHOW,
@@ -179,7 +172,7 @@ static int dsm_get_label(struct device *dev, char *buf,
 	if (!handle)
 		return -1;
 
-	obj = acpi_evaluate_dsm(handle, device_label_dsm_uuid, 0x2,
+	obj = acpi_evaluate_dsm(handle, pci_acpi_dsm_uuid, 0x2,
 				DEVICE_LABEL_DSM, NULL);
 	if (!obj)
 		return -1;
@@ -219,7 +212,7 @@ static bool device_has_dsm(struct device *dev)
 	if (!handle)
 		return false;
 
-	return !!acpi_check_dsm(handle, device_label_dsm_uuid, 0x2,
+	return !!acpi_check_dsm(handle, pci_acpi_dsm_uuid, 0x2,
 				1 << DEVICE_LABEL_DSM);
 }
 
diff --git a/drivers/pci/pci-sysfs.c b/drivers/pci/pci-sysfs.c
index aa012fb3834b..312f23a8429c 100644
--- a/drivers/pci/pci-sysfs.c
+++ b/drivers/pci/pci-sysfs.c
@@ -521,7 +521,8 @@ static ssize_t driver_override_store(struct device *dev,
 	struct pci_dev *pdev = to_pci_dev(dev);
 	char *driver_override, *old = pdev->driver_override, *cp;
 
-	if (count > PATH_MAX)
+	/* We need to keep extra room for a newline */
+	if (count >= (PAGE_SIZE - 1))
 		return -EINVAL;
 
 	driver_override = kstrndup(buf, count, GFP_KERNEL);
@@ -549,7 +550,7 @@ static ssize_t driver_override_show(struct device *dev,
 {
 	struct pci_dev *pdev = to_pci_dev(dev);
 
-	return sprintf(buf, "%s\n", pdev->driver_override);
+	return snprintf(buf, PAGE_SIZE, "%s\n", pdev->driver_override);
 }
 static DEVICE_ATTR_RW(driver_override);
 
diff --git a/drivers/pci/pci.c b/drivers/pci/pci.c
index 81f06e8dcc04..acc4b6ef78c4 100644
--- a/drivers/pci/pci.c
+++ b/drivers/pci/pci.c
@@ -126,15 +126,16 @@ EXPORT_SYMBOL_GPL(pci_bus_max_busnr);
 #ifdef CONFIG_HAS_IOMEM
 void __iomem *pci_ioremap_bar(struct pci_dev *pdev, int bar)
 {
+	struct resource *res = &pdev->resource[bar];
+
 	/*
 	 * Make sure the BAR is actually a memory resource, not an IO resource
 	 */
-	if (!(pci_resource_flags(pdev, bar) & IORESOURCE_MEM)) {
-		WARN_ON(1);
+	if (res->flags & IORESOURCE_UNSET || !(res->flags & IORESOURCE_MEM)) {
+		dev_warn(&pdev->dev, "can't ioremap BAR %d: %pR\n", bar, res);
 		return NULL;
 	}
-	return ioremap_nocache(pci_resource_start(pdev, bar),
-				     pci_resource_len(pdev, bar));
+	return ioremap_nocache(res->start, resource_size(res));
 }
 EXPORT_SYMBOL_GPL(pci_ioremap_bar);
 #endif
@@ -145,19 +146,22 @@ static int __pci_find_next_cap_ttl(struct pci_bus *bus, unsigned int devfn,
 				   u8 pos, int cap, int *ttl)
 {
 	u8 id;
+	u16 ent;
+
+	pci_bus_read_config_byte(bus, devfn, pos, &pos);
 
 	while ((*ttl)--) {
-		pci_bus_read_config_byte(bus, devfn, pos, &pos);
 		if (pos < 0x40)
 			break;
 		pos &= ~3;
-		pci_bus_read_config_byte(bus, devfn, pos + PCI_CAP_LIST_ID,
-					 &id);
+		pci_bus_read_config_word(bus, devfn, pos, &ent);
+
+		id = ent & 0xff;
 		if (id == 0xff)
 			break;
 		if (id == cap)
 			return pos;
-		pos += PCI_CAP_LIST_NEXT;
+		pos = (ent >> 8);
 	}
 	return 0;
 }
@@ -2492,6 +2496,7 @@ u8 pci_common_swizzle(struct pci_dev *dev, u8 *pinp)
 	*pinp = pin;
 	return PCI_SLOT(dev->devfn);
 }
+EXPORT_SYMBOL_GPL(pci_common_swizzle);
 
 /**
  *	pci_release_region - Release a PCI bar
diff --git a/drivers/pci/pci.h b/drivers/pci/pci.h
index 4091f82239cd..d72f849174a4 100644
--- a/drivers/pci/pci.h
+++ b/drivers/pci/pci.h
@@ -321,4 +321,6 @@ static inline int pci_dev_specific_reset(struct pci_dev *dev, int probe)
 }
 #endif
 
+struct pci_host_bridge *pci_find_host_bridge(struct pci_bus *bus);
+
 #endif /* DRIVERS_PCI_H */
diff --git a/drivers/pci/pcie/aer/aerdrv_errprint.c b/drivers/pci/pcie/aer/aerdrv_errprint.c
index c6849d9e86ce..167fe411ce2e 100644
--- a/drivers/pci/pcie/aer/aerdrv_errprint.c
+++ b/drivers/pci/pcie/aer/aerdrv_errprint.c
@@ -132,16 +132,8 @@ static const char *aer_agent_string[] = {
 static void __print_tlp_header(struct pci_dev *dev,
 			       struct aer_header_log_regs *t)
 {
-	unsigned char *tlp = (unsigned char *)&t;
-
-	dev_err(&dev->dev, "  TLP Header:"
-		" %02x%02x%02x%02x %02x%02x%02x%02x"
-		" %02x%02x%02x%02x %02x%02x%02x%02x\n",
-		*(tlp + 3), *(tlp + 2), *(tlp + 1), *tlp,
-		*(tlp + 7), *(tlp + 6), *(tlp + 5), *(tlp + 4),
-		*(tlp + 11), *(tlp + 10), *(tlp + 9),
-		*(tlp + 8), *(tlp + 15), *(tlp + 14),
-		*(tlp + 13), *(tlp + 12));
+	dev_err(&dev->dev, "  TLP Header: %08x %08x %08x %08x\n",
+		t->dw0, t->dw1, t->dw2, t->dw3);
 }
 
 static void __aer_print_error(struct pci_dev *dev,
diff --git a/drivers/pci/pcie/aspm.c b/drivers/pci/pcie/aspm.c
index 820740a22e94..7d4fcdc512aa 100644
--- a/drivers/pci/pcie/aspm.c
+++ b/drivers/pci/pcie/aspm.c
@@ -782,24 +782,6 @@ void pci_disable_link_state(struct pci_dev *pdev, int state)
 }
 EXPORT_SYMBOL(pci_disable_link_state);
 
-void pcie_clear_aspm(struct pci_bus *bus)
-{
-	struct pci_dev *child;
-
-	if (aspm_force)
-		return;
-
-	/*
-	 * Clear any ASPM setup that the firmware has carried out on this bus
-	 */
-	list_for_each_entry(child, &bus->devices, bus_list) {
-		__pci_disable_link_state(child, PCIE_LINK_STATE_L0S |
-					 PCIE_LINK_STATE_L1 |
-					 PCIE_LINK_STATE_CLKPM,
-					 false, true);
-	}
-}
-
 static int pcie_aspm_set_policy(const char *val, struct kernel_param *kp)
 {
 	int i;
diff --git a/drivers/pci/probe.c b/drivers/pci/probe.c
index 8d2f400e96cb..6675a7a1b9fc 100644
--- a/drivers/pci/probe.c
+++ b/drivers/pci/probe.c
@@ -6,6 +6,7 @@
 #include <linux/delay.h>
 #include <linux/init.h>
 #include <linux/pci.h>
+#include <linux/of_pci.h>
 #include <linux/pci_hotplug.h>
 #include <linux/slab.h>
 #include <linux/module.h>
@@ -1520,6 +1521,7 @@ void pci_device_add(struct pci_dev *dev, struct pci_bus *bus)
 	dev->dev.dma_mask = &dev->dma_mask;
 	dev->dev.dma_parms = &dev->dma_parms;
 	dev->dev.coherent_dma_mask = 0xffffffffull;
+	of_pci_dma_configure(dev);
 
 	pci_set_dma_max_seg_size(dev, 65536);
 	pci_set_dma_seg_boundary(dev, 0xffffffff);
@@ -1993,6 +1995,7 @@ err_out:
 	kfree(b);
 	return NULL;
 }
+EXPORT_SYMBOL_GPL(pci_create_root_bus);
 
 int pci_bus_insert_busn_res(struct pci_bus *b, int bus, int bus_max)
 {
@@ -2087,7 +2090,6 @@ struct pci_bus *pci_scan_root_bus(struct device *parent, int bus,
 	if (!found)
 		pci_bus_update_busn_res_end(b, max);
 
-	pci_bus_add_devices(b);
 	return b;
 }
 EXPORT_SYMBOL(pci_scan_root_bus);
@@ -2123,7 +2125,6 @@ struct pci_bus *pci_scan_bus(int bus, struct pci_ops *ops,
 	b = pci_create_root_bus(NULL, bus, ops, sysdata, &resources);
 	if (b) {
 		pci_scan_child_bus(b);
-		pci_bus_add_devices(b);
 	} else {
 		pci_free_resource_list(&resources);
 	}
diff --git a/drivers/pci/quirks.c b/drivers/pci/quirks.c
index 85f247e28a80..c6dc1dfd25d5 100644
--- a/drivers/pci/quirks.c
+++ b/drivers/pci/quirks.c
@@ -3182,7 +3182,7 @@ static void quirk_apple_wait_for_thunderbolt(struct pci_dev *dev)
 			|| nhi->subsystem_vendor != 0x2222
 			|| nhi->subsystem_device != 0x1111)
 		goto out;
-	dev_info(&dev->dev, "quirk: wating for thunderbolt to reestablish pci tunnels...\n");
+	dev_info(&dev->dev, "quirk: waiting for thunderbolt to reestablish PCI tunnels...\n");
 	device_pm_wait_for_dev(&dev->dev, &nhi->dev);
 out:
 	pci_dev_put(nhi);
@@ -3822,6 +3822,38 @@ static const struct pci_dev_acs_enabled {
 	{ PCI_VENDOR_ID_INTEL, 0x154F, pci_quirk_mf_endpoint_acs },
 	{ PCI_VENDOR_ID_INTEL, 0x1551, pci_quirk_mf_endpoint_acs },
 	{ PCI_VENDOR_ID_INTEL, 0x1558, pci_quirk_mf_endpoint_acs },
+	/* 82580 */
+	{ PCI_VENDOR_ID_INTEL, 0x1509, pci_quirk_mf_endpoint_acs },
+	{ PCI_VENDOR_ID_INTEL, 0x150E, pci_quirk_mf_endpoint_acs },
+	{ PCI_VENDOR_ID_INTEL, 0x150F, pci_quirk_mf_endpoint_acs },
+	{ PCI_VENDOR_ID_INTEL, 0x1510, pci_quirk_mf_endpoint_acs },
+	{ PCI_VENDOR_ID_INTEL, 0x1511, pci_quirk_mf_endpoint_acs },
+	{ PCI_VENDOR_ID_INTEL, 0x1516, pci_quirk_mf_endpoint_acs },
+	{ PCI_VENDOR_ID_INTEL, 0x1527, pci_quirk_mf_endpoint_acs },
+	/* 82576 */
+	{ PCI_VENDOR_ID_INTEL, 0x10C9, pci_quirk_mf_endpoint_acs },
+	{ PCI_VENDOR_ID_INTEL, 0x10E6, pci_quirk_mf_endpoint_acs },
+	{ PCI_VENDOR_ID_INTEL, 0x10E7, pci_quirk_mf_endpoint_acs },
+	{ PCI_VENDOR_ID_INTEL, 0x10E8, pci_quirk_mf_endpoint_acs },
+	{ PCI_VENDOR_ID_INTEL, 0x150A, pci_quirk_mf_endpoint_acs },
+	{ PCI_VENDOR_ID_INTEL, 0x150D, pci_quirk_mf_endpoint_acs },
+	{ PCI_VENDOR_ID_INTEL, 0x1518, pci_quirk_mf_endpoint_acs },
+	{ PCI_VENDOR_ID_INTEL, 0x1526, pci_quirk_mf_endpoint_acs },
+	/* 82575 */
+	{ PCI_VENDOR_ID_INTEL, 0x10A7, pci_quirk_mf_endpoint_acs },
+	{ PCI_VENDOR_ID_INTEL, 0x10A9, pci_quirk_mf_endpoint_acs },
+	{ PCI_VENDOR_ID_INTEL, 0x10D6, pci_quirk_mf_endpoint_acs },
+	/* I350 */
+	{ PCI_VENDOR_ID_INTEL, 0x1521, pci_quirk_mf_endpoint_acs },
+	{ PCI_VENDOR_ID_INTEL, 0x1522, pci_quirk_mf_endpoint_acs },
+	{ PCI_VENDOR_ID_INTEL, 0x1523, pci_quirk_mf_endpoint_acs },
+	{ PCI_VENDOR_ID_INTEL, 0x1524, pci_quirk_mf_endpoint_acs },
+	/* 82571 (Quads omitted due to non-ACS switch) */
+	{ PCI_VENDOR_ID_INTEL, 0x105E, pci_quirk_mf_endpoint_acs },
+	{ PCI_VENDOR_ID_INTEL, 0x105F, pci_quirk_mf_endpoint_acs },
+	{ PCI_VENDOR_ID_INTEL, 0x1060, pci_quirk_mf_endpoint_acs },
+	{ PCI_VENDOR_ID_INTEL, 0x10D9, pci_quirk_mf_endpoint_acs },
+	/* Intel PCH root ports */
 	{ PCI_VENDOR_ID_INTEL, PCI_ANY_ID, pci_quirk_intel_pch_acs },
 	{ 0x19a2, 0x710, pci_quirk_mf_endpoint_acs }, /* Emulex BE3-R */
 	{ 0x10df, 0x720, pci_quirk_mf_endpoint_acs }, /* Emulex Skyhawk-R */
diff --git a/drivers/pci/remove.c b/drivers/pci/remove.c
index 8bd76c9ba21c..8a280e9c2ad1 100644
--- a/drivers/pci/remove.c
+++ b/drivers/pci/remove.c
@@ -139,6 +139,7 @@ void pci_stop_root_bus(struct pci_bus *bus)
 	/* stop the host bridge */
 	device_release_driver(&host_bridge->dev);
 }
+EXPORT_SYMBOL_GPL(pci_stop_root_bus);
 
 void pci_remove_root_bus(struct pci_bus *bus)
 {
@@ -158,3 +159,4 @@ void pci_remove_root_bus(struct pci_bus *bus)
 	/* remove the host bridge */
 	device_unregister(&host_bridge->dev);
 }
+EXPORT_SYMBOL_GPL(pci_remove_root_bus);
diff --git a/drivers/pci/setup-bus.c b/drivers/pci/setup-bus.c
index e3e17f3c0f0f..8169597e47cb 100644
--- a/drivers/pci/setup-bus.c
+++ b/drivers/pci/setup-bus.c
@@ -1750,3 +1750,4 @@ void pci_assign_unassigned_bus_resources(struct pci_bus *bus)
 	__pci_bus_assign_resources(bus, &add_list, NULL);
 	BUG_ON(!list_empty(&add_list));
 }
+EXPORT_SYMBOL_GPL(pci_assign_unassigned_bus_resources);
diff --git a/drivers/pci/setup-irq.c b/drivers/pci/setup-irq.c
index 4e2d595d50ca..95c225be49d1 100644
--- a/drivers/pci/setup-irq.c
+++ b/drivers/pci/setup-irq.c
@@ -65,3 +65,4 @@ void pci_fixup_irqs(u8 (*swizzle)(struct pci_dev *, u8 *),
 	for_each_pci_dev(dev)
 		pdev_fixup_irq(dev, swizzle, map_irq);
 }
+EXPORT_SYMBOL_GPL(pci_fixup_irqs);
diff --git a/drivers/pci/setup-res.c b/drivers/pci/setup-res.c
index b7c3a5ea1fca..232f9254c11a 100644
--- a/drivers/pci/setup-res.c
+++ b/drivers/pci/setup-res.c
@@ -120,6 +120,7 @@ int pci_claim_resource(struct pci_dev *dev, int resource)
 	if (!root) {
 		dev_info(&dev->dev, "can't claim BAR %d %pR: no compatible bridge window\n",
 			 resource, res);
+		res->flags |= IORESOURCE_UNSET;
 		return -EINVAL;
 	}
 
@@ -127,6 +128,7 @@ int pci_claim_resource(struct pci_dev *dev, int resource)
 	if (conflict) {
 		dev_info(&dev->dev, "can't claim BAR %d %pR: address conflict with %s %pR\n",
 			 resource, res, conflict->name, conflict);
+		res->flags |= IORESOURCE_UNSET;
 		return -EBUSY;
 	}
 
diff --git a/drivers/pcmcia/Kconfig b/drivers/pcmcia/Kconfig
index 3bb49252a098..45f67c63d385 100644
--- a/drivers/pcmcia/Kconfig
+++ b/drivers/pcmcia/Kconfig
@@ -69,8 +69,7 @@ config YENTA
 	tristate "CardBus yenta-compatible bridge support"
 	depends on PCI
 	select CARDBUS if !EXPERT
-	select PCCARD_NONSTATIC if PCMCIA != n && ISA
-	select PCCARD_PCI if PCMCIA !=n && !ISA
+	select PCCARD_NONSTATIC if PCMCIA != n
 	---help---
 	  This option enables support for CardBus host bridges.  Virtually
 	  all modern PCMCIA bridges are CardBus compatible.  A "bridge" is
@@ -110,8 +109,7 @@ config YENTA_TOSHIBA
 config PD6729
 	tristate "Cirrus PD6729 compatible bridge support"
 	depends on PCMCIA && PCI
-	select PCCARD_NONSTATIC if PCMCIA != n && ISA
-	select PCCARD_PCI if PCMCIA !=n && !ISA
+	select PCCARD_NONSTATIC
 	help
 	  This provides support for the Cirrus PD6729 PCI-to-PCMCIA bridge
 	  device, found in some older laptops and PCMCIA card readers.
@@ -119,8 +117,7 @@ config PD6729
 config I82092
 	tristate "i82092 compatible bridge support"
 	depends on PCMCIA && PCI
-	select PCCARD_NONSTATIC if PCMCIA != n && ISA
-	select PCCARD_PCI if PCMCIA !=n && !ISA
+	select PCCARD_NONSTATIC
 	help
 	  This provides support for the Intel I82092AA PCI-to-PCMCIA bridge device,
 	  found in some older laptops and more commonly in evaluation boards for the
@@ -291,9 +288,6 @@ config ELECTRA_CF
 	  Say Y here to support the CompactFlash controller on the
 	  PA Semi Electra eval board.
 
-config PCCARD_PCI
-	bool
-
 config PCCARD_NONSTATIC
 	bool
 
diff --git a/drivers/pcmcia/Makefile b/drivers/pcmcia/Makefile
index f1a7ca04d89e..27e94b30cf96 100644
--- a/drivers/pcmcia/Makefile
+++ b/drivers/pcmcia/Makefile
@@ -12,7 +12,6 @@ obj-$(CONFIG_PCMCIA)				+= pcmcia.o
 pcmcia_rsrc-y					+= rsrc_mgr.o
 pcmcia_rsrc-$(CONFIG_PCCARD_NONSTATIC)		+= rsrc_nonstatic.o
 pcmcia_rsrc-$(CONFIG_PCCARD_IODYN)		+= rsrc_iodyn.o
-pcmcia_rsrc-$(CONFIG_PCCARD_PCI)		+= rsrc_pci.o
 obj-$(CONFIG_PCCARD)				+= pcmcia_rsrc.o
 
 
diff --git a/drivers/pcmcia/rsrc_pci.c b/drivers/pcmcia/rsrc_pci.c
deleted file mode 100644
index 1f67b3ba70fb..000000000000
--- a/drivers/pcmcia/rsrc_pci.c
+++ /dev/null
@@ -1,173 +0,0 @@
-#include <linux/slab.h>
-#include <linux/module.h>
-#include <linux/kernel.h>
-#include <linux/pci.h>
-
-#include <pcmcia/ss.h>
-#include <pcmcia/cistpl.h>
-#include "cs_internal.h"
-
-
-struct pcmcia_align_data {
-	unsigned long	mask;
-	unsigned long	offset;
-};
-
-static resource_size_t pcmcia_align(void *align_data,
-				const struct resource *res,
-				resource_size_t size, resource_size_t align)
-{
-	struct pcmcia_align_data *data = align_data;
-	resource_size_t start;
-
-	start = (res->start & ~data->mask) + data->offset;
-	if (start < res->start)
-		start += data->mask + 1;
-	return start;
-}
-
-static struct resource *find_io_region(struct pcmcia_socket *s,
-					unsigned long base, int num,
-					unsigned long align)
-{
-	struct resource *res = pcmcia_make_resource(0, num, IORESOURCE_IO,
-						dev_name(&s->dev));
-	struct pcmcia_align_data data;
-	int ret;
-
-	data.mask = align - 1;
-	data.offset = base & data.mask;
-
-	ret = pci_bus_alloc_resource(s->cb_dev->bus, res, num, 1,
-					     base, 0, pcmcia_align, &data);
-	if (ret != 0) {
-		kfree(res);
-		res = NULL;
-	}
-	return res;
-}
-
-static int res_pci_find_io(struct pcmcia_socket *s, unsigned int attr,
-			unsigned int *base, unsigned int num,
-			unsigned int align, struct resource **parent)
-{
-	int i, ret = 0;
-
-	/* Check for an already-allocated window that must conflict with
-	 * what was asked for.  It is a hack because it does not catch all
-	 * potential conflicts, just the most obvious ones.
-	 */
-	for (i = 0; i < MAX_IO_WIN; i++) {
-		if (!s->io[i].res)
-			continue;
-
-		if (!*base)
-			continue;
-
-		if ((s->io[i].res->start & (align-1)) == *base)
-			return -EBUSY;
-	}
-
-	for (i = 0; i < MAX_IO_WIN; i++) {
-		struct resource *res = s->io[i].res;
-		unsigned int try;
-
-		if (res && (res->flags & IORESOURCE_BITS) !=
-			(attr & IORESOURCE_BITS))
-			continue;
-
-		if (!res) {
-			if (align == 0)
-				align = 0x10000;
-
-			res = s->io[i].res = find_io_region(s, *base, num,
-								align);
-			if (!res)
-				return -EINVAL;
-
-			*base = res->start;
-			s->io[i].res->flags =
-				((res->flags & ~IORESOURCE_BITS) |
-					(attr & IORESOURCE_BITS));
-			s->io[i].InUse = num;
-			*parent = res;
-			return 0;
-		}
-
-		/* Try to extend top of window */
-		try = res->end + 1;
-		if ((*base == 0) || (*base == try)) {
-			ret = adjust_resource(s->io[i].res, res->start,
-					      resource_size(res) + num);
-			if (ret)
-				continue;
-			*base = try;
-			s->io[i].InUse += num;
-			*parent = res;
-			return 0;
-		}
-
-		/* Try to extend bottom of window */
-		try = res->start - num;
-		if ((*base == 0) || (*base == try)) {
-			ret = adjust_resource(s->io[i].res,
-					      res->start - num,
-					      resource_size(res) + num);
-			if (ret)
-				continue;
-			*base = try;
-			s->io[i].InUse += num;
-			*parent = res;
-			return 0;
-		}
-	}
-	return -EINVAL;
-}
-
-static struct resource *res_pci_find_mem(u_long base, u_long num,
-		u_long align, int low, struct pcmcia_socket *s)
-{
-	struct resource *res = pcmcia_make_resource(0, num, IORESOURCE_MEM,
-						dev_name(&s->dev));
-	struct pcmcia_align_data data;
-	unsigned long min;
-	int ret;
-
-	if (align < 0x20000)
-		align = 0x20000;
-	data.mask = align - 1;
-	data.offset = base & data.mask;
-
-	min = 0;
-	if (!low)
-		min = 0x100000UL;
-
-	ret = pci_bus_alloc_resource(s->cb_dev->bus,
-			res, num, 1, min, 0,
-			pcmcia_align, &data);
-
-	if (ret != 0) {
-		kfree(res);
-		res = NULL;
-	}
-	return res;
-}
-
-
-static int res_pci_init(struct pcmcia_socket *s)
-{
-	if (!s->cb_dev || !(s->features & SS_CAP_PAGE_REGS)) {
-		dev_err(&s->dev, "not supported by res_pci\n");
-		return -EOPNOTSUPP;
-	}
-	return 0;
-}
-
-struct pccard_resource_ops pccard_nonstatic_ops = {
-	.validate_mem = NULL,
-	.find_io = res_pci_find_io,
-	.find_mem = res_pci_find_mem,
-	.init = res_pci_init,
-	.exit = NULL,
-};
-EXPORT_SYMBOL(pccard_nonstatic_ops);
diff --git a/drivers/phy/Kconfig b/drivers/phy/Kconfig
index 2962de205ba7..a53bd5b52df9 100644
--- a/drivers/phy/Kconfig
+++ b/drivers/phy/Kconfig
@@ -35,6 +35,13 @@ config ARMADA375_USBCLUSTER_PHY
 	depends on OF
 	select GENERIC_PHY
 
+config PHY_DM816X_USB
+	tristate "TI dm816x USB PHY driver"
+	depends on ARCH_OMAP2PLUS
+	select GENERIC_PHY
+	help
+	  Enable this for dm816x USB to work.
+
 config PHY_EXYNOS_MIPI_VIDEO
 	tristate "S5P/EXYNOS SoC series MIPI CSI-2/DSI PHY driver"
 	depends on HAS_IOMEM
@@ -174,6 +181,17 @@ config PHY_SUN4I_USB
 	  This driver controls the entire USB PHY block, both the USB OTG
 	  parts, as well as the 2 regular USB 2 host PHYs.
 
+config PHY_SUN9I_USB
+	tristate "Allwinner sun9i SoC USB PHY driver"
+	depends on ARCH_SUNXI && HAS_IOMEM && OF
+	depends on RESET_CONTROLLER
+	select GENERIC_PHY
+	help
+	  Enable this to support the transceiver that is part of Allwinner
+	  sun9i SoCs.
+
+	  This driver controls each individual USB 2 host PHY.
+
 config PHY_SAMSUNG_USB2
 	tristate "Samsung USB 2.0 PHY driver"
 	depends on HAS_IOMEM
diff --git a/drivers/phy/Makefile b/drivers/phy/Makefile
index f080e1bb2a74..f12625178780 100644
--- a/drivers/phy/Makefile
+++ b/drivers/phy/Makefile
@@ -5,6 +5,7 @@
 obj-$(CONFIG_GENERIC_PHY)		+= phy-core.o
 obj-$(CONFIG_PHY_BERLIN_USB)		+= phy-berlin-usb.o
 obj-$(CONFIG_PHY_BERLIN_SATA)		+= phy-berlin-sata.o
+obj-$(CONFIG_PHY_DM816X_USB)		+= phy-dm816x-usb.o
 obj-$(CONFIG_ARMADA375_USBCLUSTER_PHY)	+= phy-armada375-usb2.o
 obj-$(CONFIG_BCM_KONA_USB2_PHY)		+= phy-bcm-kona-usb2.o
 obj-$(CONFIG_PHY_EXYNOS_DP_VIDEO)	+= phy-exynos-dp-video.o
@@ -20,6 +21,7 @@ obj-$(CONFIG_TWL4030_USB)		+= phy-twl4030-usb.o
 obj-$(CONFIG_PHY_EXYNOS5250_SATA)	+= phy-exynos5250-sata.o
 obj-$(CONFIG_PHY_HIX5HD2_SATA)		+= phy-hix5hd2-sata.o
 obj-$(CONFIG_PHY_SUN4I_USB)		+= phy-sun4i-usb.o
+obj-$(CONFIG_PHY_SUN9I_USB)		+= phy-sun9i-usb.o
 obj-$(CONFIG_PHY_SAMSUNG_USB2)		+= phy-exynos-usb2.o
 phy-exynos-usb2-y			+= phy-samsung-usb2.o
 phy-exynos-usb2-$(CONFIG_PHY_EXYNOS4210_USB2)	+= phy-exynos4210-usb2.o
diff --git a/drivers/phy/phy-armada375-usb2.c b/drivers/phy/phy-armada375-usb2.c
index 7c99ca256f05..8ccc3952c13d 100644
--- a/drivers/phy/phy-armada375-usb2.c
+++ b/drivers/phy/phy-armada375-usb2.c
@@ -37,7 +37,7 @@ static int armada375_usb_phy_init(struct phy *phy)
 	struct armada375_cluster_phy *cluster_phy;
 	u32 reg;
 
-	cluster_phy = dev_get_drvdata(phy->dev.parent);
+	cluster_phy = phy_get_drvdata(phy);
 	if (!cluster_phy)
 		return -ENODEV;
 
@@ -131,6 +131,7 @@ static int armada375_usb_phy_probe(struct platform_device *pdev)
 	cluster_phy->reg = usb_cluster_base;
 
 	dev_set_drvdata(dev, cluster_phy);
+	phy_set_drvdata(phy, cluster_phy);
 
 	phy_provider = devm_of_phy_provider_register(&pdev->dev,
 						     armada375_usb_phy_xlate);
diff --git a/drivers/phy/phy-berlin-sata.c b/drivers/phy/phy-berlin-sata.c
index 099eee8851e5..6f3e06d687de 100644
--- a/drivers/phy/phy-berlin-sata.c
+++ b/drivers/phy/phy-berlin-sata.c
@@ -218,7 +218,7 @@ static int phy_berlin_sata_probe(struct platform_device *pdev)
 	if (priv->nphys == 0)
 		return -ENODEV;
 
-	priv->phys = devm_kzalloc(dev, priv->nphys * sizeof(*priv->phys),
+	priv->phys = devm_kcalloc(dev, priv->nphys, sizeof(*priv->phys),
 				  GFP_KERNEL);
 	if (!priv->phys)
 		return -ENOMEM;
diff --git a/drivers/phy/phy-berlin-usb.c b/drivers/phy/phy-berlin-usb.c
index c8a8d53a6ece..c6fc95b53083 100644
--- a/drivers/phy/phy-berlin-usb.c
+++ b/drivers/phy/phy-berlin-usb.c
@@ -103,9 +103,6 @@
 #define MODE_TEST_EN		BIT(11)
 #define ANA_TEST_DC_CTRL(x)	((x) << 12)
 
-#define to_phy_berlin_usb_priv(p)	\
-	container_of((p), struct phy_berlin_usb_priv, phy)
-
 static const u32 phy_berlin_pll_dividers[] = {
 	/* Berlin 2 */
 	CLK_REF_DIV(0xc) | FEEDBACK_CLK_DIV(0x54),
@@ -115,14 +112,13 @@ static const u32 phy_berlin_pll_dividers[] = {
 
 struct phy_berlin_usb_priv {
 	void __iomem		*base;
-	struct phy		*phy;
 	struct reset_control	*rst_ctrl;
 	u32			pll_divider;
 };
 
 static int phy_berlin_usb_power_on(struct phy *phy)
 {
-	struct phy_berlin_usb_priv *priv = dev_get_drvdata(phy->dev.parent);
+	struct phy_berlin_usb_priv *priv = phy_get_drvdata(phy);
 
 	reset_control_reset(priv->rst_ctrl);
 
@@ -175,6 +171,7 @@ static int phy_berlin_usb_probe(struct platform_device *pdev)
 		of_match_device(phy_berlin_sata_of_match, &pdev->dev);
 	struct phy_berlin_usb_priv *priv;
 	struct resource *res;
+	struct phy *phy;
 	struct phy_provider *phy_provider;
 
 	priv = devm_kzalloc(&pdev->dev, sizeof(*priv), GFP_KERNEL);
@@ -192,20 +189,18 @@ static int phy_berlin_usb_probe(struct platform_device *pdev)
 
 	priv->pll_divider = *((u32 *)match->data);
 
-	priv->phy = devm_phy_create(&pdev->dev, NULL, &phy_berlin_usb_ops);
-	if (IS_ERR(priv->phy)) {
+	phy = devm_phy_create(&pdev->dev, NULL, &phy_berlin_usb_ops);
+	if (IS_ERR(phy)) {
 		dev_err(&pdev->dev, "failed to create PHY\n");
-		return PTR_ERR(priv->phy);
+		return PTR_ERR(phy);
 	}
 
 	platform_set_drvdata(pdev, priv);
+	phy_set_drvdata(phy, priv);
 
 	phy_provider =
 		devm_of_phy_provider_register(&pdev->dev, of_phy_simple_xlate);
-	if (IS_ERR(phy_provider))
-		return PTR_ERR(phy_provider);
-
-	return 0;
+	return PTR_ERR_OR_ZERO(phy_provider);
 }
 
 static struct platform_driver phy_berlin_usb_driver = {
diff --git a/drivers/phy/phy-core.c b/drivers/phy/phy-core.c
index a12d35338313..3791838f4bd4 100644
--- a/drivers/phy/phy-core.c
+++ b/drivers/phy/phy-core.c
@@ -52,7 +52,9 @@ static void devm_phy_consume(struct device *dev, void *res)
 
 static int devm_phy_match(struct device *dev, void *res, void *match_data)
 {
-	return res == match_data;
+	struct phy **phy = res;
+
+	return *phy == match_data;
 }
 
 /**
@@ -223,6 +225,7 @@ int phy_init(struct phy *phy)
 	ret = phy_pm_runtime_get_sync(phy);
 	if (ret < 0 && ret != -ENOTSUPP)
 		return ret;
+	ret = 0; /* Override possible ret == -ENOTSUPP */
 
 	mutex_lock(&phy->mutex);
 	if (phy->init_count == 0 && phy->ops->init) {
@@ -231,8 +234,6 @@ int phy_init(struct phy *phy)
 			dev_err(&phy->dev, "phy init failed --> %d\n", ret);
 			goto out;
 		}
-	} else {
-		ret = 0; /* Override possible ret == -ENOTSUPP */
 	}
 	++phy->init_count;
 
@@ -253,6 +254,7 @@ int phy_exit(struct phy *phy)
 	ret = phy_pm_runtime_get_sync(phy);
 	if (ret < 0 && ret != -ENOTSUPP)
 		return ret;
+	ret = 0; /* Override possible ret == -ENOTSUPP */
 
 	mutex_lock(&phy->mutex);
 	if (phy->init_count == 1 && phy->ops->exit) {
@@ -287,6 +289,7 @@ int phy_power_on(struct phy *phy)
 	ret = phy_pm_runtime_get_sync(phy);
 	if (ret < 0 && ret != -ENOTSUPP)
 		return ret;
+	ret = 0; /* Override possible ret == -ENOTSUPP */
 
 	mutex_lock(&phy->mutex);
 	if (phy->power_count == 0 && phy->ops->power_on) {
@@ -295,8 +298,6 @@ int phy_power_on(struct phy *phy)
 			dev_err(&phy->dev, "phy poweron failed --> %d\n", ret);
 			goto out;
 		}
-	} else {
-		ret = 0; /* Override possible ret == -ENOTSUPP */
 	}
 	++phy->power_count;
 	mutex_unlock(&phy->mutex);
diff --git a/drivers/phy/phy-dm816x-usb.c b/drivers/phy/phy-dm816x-usb.c
new file mode 100644
index 000000000000..7b42555ddd51
--- /dev/null
+++ b/drivers/phy/phy-dm816x-usb.c
@@ -0,0 +1,290 @@
+/*
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation version 2.
+ *
+ * This program is distributed "as is" WITHOUT ANY WARRANTY of any
+ * kind, whether express or implied; without even the implied warranty
+ * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/regmap.h>
+
+#include <linux/slab.h>
+#include <linux/of.h>
+#include <linux/io.h>
+#include <linux/usb/phy_companion.h>
+#include <linux/clk.h>
+#include <linux/err.h>
+#include <linux/pm_runtime.h>
+#include <linux/delay.h>
+#include <linux/phy/phy.h>
+#include <linux/of_platform.h>
+
+#include <linux/mfd/syscon.h>
+
+/*
+ * TRM has two sets of USB_CTRL registers.. The correct register bits
+ * are in TRM section 24.9.8.2 USB_CTRL Register. The TRM documents the
+ * phy as being SR70LX Synopsys USB 2.0 OTG nanoPHY. It also seems at
+ * least dm816x rev c ignores writes to USB_CTRL register, but the TI
+ * kernel is writing to those so it's possible that later revisions
+ * have worknig USB_CTRL register.
+ *
+ * Also note that At least USB_CTRL register seems to be dm816x specific
+ * according to the TRM. It's possible that USBPHY_CTRL is more generic,
+ * but that would have to be checked against the SR70LX documentation
+ * which does not seem to be publicly available.
+ *
+ * Finally, the phy on dm814x and am335x is different from dm816x.
+ */
+#define DM816X_USB_CTRL_PHYCLKSRC	BIT(8)	/* 1 = PLL ref clock */
+#define DM816X_USB_CTRL_PHYSLEEP1	BIT(1)	/* Enable the first phy */
+#define DM816X_USB_CTRL_PHYSLEEP0	BIT(0)	/* Enable the second phy */
+
+#define DM816X_USBPHY_CTRL_TXRISETUNE	1
+#define DM816X_USBPHY_CTRL_TXVREFTUNE	0xc
+#define DM816X_USBPHY_CTRL_TXPREEMTUNE	0x2
+
+struct dm816x_usb_phy {
+	struct regmap *syscon;
+	struct device *dev;
+	unsigned int instance;
+	struct clk *refclk;
+	struct usb_phy phy;
+	unsigned int usb_ctrl;		/* Shared between phy0 and phy1 */
+	unsigned int usbphy_ctrl;
+};
+
+static int dm816x_usb_phy_set_host(struct usb_otg *otg, struct usb_bus *host)
+{
+	otg->host = host;
+	if (!host)
+		otg->state = OTG_STATE_UNDEFINED;
+
+	return 0;
+}
+
+static int dm816x_usb_phy_set_peripheral(struct usb_otg *otg,
+					 struct usb_gadget *gadget)
+{
+	otg->gadget = gadget;
+	if (!gadget)
+		otg->state = OTG_STATE_UNDEFINED;
+
+	return 0;
+}
+
+static int dm816x_usb_phy_init(struct phy *x)
+{
+	struct dm816x_usb_phy *phy = phy_get_drvdata(x);
+	unsigned int val;
+	int error;
+
+	if (clk_get_rate(phy->refclk) != 24000000)
+		dev_warn(phy->dev, "nonstandard phy refclk\n");
+
+	/* Set PLL ref clock and put phys to sleep */
+	error = regmap_update_bits(phy->syscon, phy->usb_ctrl,
+				   DM816X_USB_CTRL_PHYCLKSRC |
+				   DM816X_USB_CTRL_PHYSLEEP1 |
+				   DM816X_USB_CTRL_PHYSLEEP0,
+				   0);
+	regmap_read(phy->syscon, phy->usb_ctrl, &val);
+	if ((val & 3) != 0)
+		dev_info(phy->dev,
+			 "Working dm816x USB_CTRL! (0x%08x)\n",
+			 val);
+
+	/*
+	 * TI kernel sets these values for "symmetrical eye diagram and
+	 * better signal quality" so let's assume somebody checked the
+	 * values with a scope and set them here too.
+	 */
+	regmap_read(phy->syscon, phy->usbphy_ctrl, &val);
+	val |= DM816X_USBPHY_CTRL_TXRISETUNE |
+		DM816X_USBPHY_CTRL_TXVREFTUNE |
+		DM816X_USBPHY_CTRL_TXPREEMTUNE;
+	regmap_write(phy->syscon, phy->usbphy_ctrl, val);
+
+	return 0;
+}
+
+static struct phy_ops ops = {
+	.init		= dm816x_usb_phy_init,
+	.owner		= THIS_MODULE,
+};
+
+static int dm816x_usb_phy_runtime_suspend(struct device *dev)
+{
+	struct dm816x_usb_phy *phy = dev_get_drvdata(dev);
+	unsigned int mask, val;
+	int error = 0;
+
+	mask = BIT(phy->instance);
+	val = ~BIT(phy->instance);
+	error = regmap_update_bits(phy->syscon, phy->usb_ctrl,
+				   mask, val);
+	if (error)
+		dev_err(phy->dev, "phy%i failed to power off\n",
+			phy->instance);
+	clk_disable(phy->refclk);
+
+	return 0;
+}
+
+static int dm816x_usb_phy_runtime_resume(struct device *dev)
+{
+	struct dm816x_usb_phy *phy = dev_get_drvdata(dev);
+	unsigned int mask, val;
+	int error;
+
+	error = clk_enable(phy->refclk);
+	if (error)
+		return error;
+
+	/*
+	 * Note that at least dm816x rev c does not seem to do
+	 * anything with the USB_CTRL register. But let's follow
+	 * what the TI tree is doing in case later revisions use
+	 * USB_CTRL.
+	 */
+	mask = BIT(phy->instance);
+	val = BIT(phy->instance);
+	error = regmap_update_bits(phy->syscon, phy->usb_ctrl,
+				   mask, val);
+	if (error) {
+		dev_err(phy->dev, "phy%i failed to power on\n",
+			phy->instance);
+		clk_disable(phy->refclk);
+		return error;
+	}
+
+	return 0;
+}
+
+static UNIVERSAL_DEV_PM_OPS(dm816x_usb_phy_pm_ops,
+			    dm816x_usb_phy_runtime_suspend,
+			    dm816x_usb_phy_runtime_resume,
+			    NULL);
+
+#ifdef CONFIG_OF
+static const struct of_device_id dm816x_usb_phy_id_table[] = {
+	{
+		.compatible = "ti,dm8168-usb-phy",
+	},
+	{},
+};
+MODULE_DEVICE_TABLE(of, dm816x_usb_phy_id_table);
+#endif
+
+static int dm816x_usb_phy_probe(struct platform_device *pdev)
+{
+	struct dm816x_usb_phy *phy;
+	struct resource *res;
+	struct phy *generic_phy;
+	struct phy_provider *phy_provider;
+	struct usb_otg *otg;
+	const struct of_device_id *of_id;
+	const struct usb_phy_data *phy_data;
+	int error;
+
+	of_id = of_match_device(of_match_ptr(dm816x_usb_phy_id_table),
+				&pdev->dev);
+	if (!of_id)
+		return -EINVAL;
+
+	phy = devm_kzalloc(&pdev->dev, sizeof(*phy), GFP_KERNEL);
+	if (!phy)
+		return -ENOMEM;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res)
+		return -ENOENT;
+
+	phy->syscon = syscon_regmap_lookup_by_phandle(pdev->dev.of_node,
+						      "syscon");
+	if (IS_ERR(phy->syscon))
+		return PTR_ERR(phy->syscon);
+
+	/*
+	 * According to sprs614e.pdf, the first usb_ctrl is shared and
+	 * the second instance for usb_ctrl is reserved.. Also the
+	 * register bits are different from earlier TRMs.
+	 */
+	phy->usb_ctrl = 0x20;
+	phy->usbphy_ctrl = (res->start & 0xff) + 4;
+	if (phy->usbphy_ctrl == 0x2c)
+		phy->instance = 1;
+
+	phy_data = of_id->data;
+
+	otg = devm_kzalloc(&pdev->dev, sizeof(*otg), GFP_KERNEL);
+	if (!otg)
+		return -ENOMEM;
+
+	phy->dev = &pdev->dev;
+	phy->phy.dev = phy->dev;
+	phy->phy.label = "dm8168_usb_phy";
+	phy->phy.otg = otg;
+	phy->phy.type = USB_PHY_TYPE_USB2;
+	otg->set_host = dm816x_usb_phy_set_host;
+	otg->set_peripheral = dm816x_usb_phy_set_peripheral;
+	otg->usb_phy = &phy->phy;
+
+	platform_set_drvdata(pdev, phy);
+
+	phy->refclk = devm_clk_get(phy->dev, "refclk");
+	if (IS_ERR(phy->refclk))
+		return PTR_ERR(phy->refclk);
+	error = clk_prepare(phy->refclk);
+	if (error)
+		return error;
+
+	pm_runtime_enable(phy->dev);
+	generic_phy = devm_phy_create(phy->dev, NULL, &ops);
+	if (IS_ERR(generic_phy))
+		return PTR_ERR(generic_phy);
+
+	phy_set_drvdata(generic_phy, phy);
+
+	phy_provider = devm_of_phy_provider_register(phy->dev,
+						     of_phy_simple_xlate);
+	if (IS_ERR(phy_provider))
+		return PTR_ERR(phy_provider);
+
+	usb_add_phy_dev(&phy->phy);
+
+	return 0;
+}
+
+static int dm816x_usb_phy_remove(struct platform_device *pdev)
+{
+	struct dm816x_usb_phy *phy = platform_get_drvdata(pdev);
+
+	usb_remove_phy(&phy->phy);
+	pm_runtime_disable(phy->dev);
+	clk_unprepare(phy->refclk);
+
+	return 0;
+}
+
+static struct platform_driver dm816x_usb_phy_driver = {
+	.probe		= dm816x_usb_phy_probe,
+	.remove		= dm816x_usb_phy_remove,
+	.driver		= {
+		.name	= "dm816x-usb-phy",
+		.pm	= &dm816x_usb_phy_pm_ops,
+		.of_match_table = of_match_ptr(dm816x_usb_phy_id_table),
+	},
+};
+
+module_platform_driver(dm816x_usb_phy_driver);
+
+MODULE_ALIAS("platform:dm816x_usb");
+MODULE_AUTHOR("Tony Lindgren <tony@atomide.com>");
+MODULE_DESCRIPTION("dm816x usb phy driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/phy/phy-exynos-dp-video.c b/drivers/phy/phy-exynos-dp-video.c
index f86cbe68ddaf..179cbf9451aa 100644
--- a/drivers/phy/phy-exynos-dp-video.c
+++ b/drivers/phy/phy-exynos-dp-video.c
@@ -30,28 +30,13 @@ struct exynos_dp_video_phy {
 	const struct exynos_dp_video_phy_drvdata *drvdata;
 };
 
-static void exynos_dp_video_phy_pwr_isol(struct exynos_dp_video_phy *state,
-							unsigned int on)
-{
-	unsigned int val;
-
-	if (IS_ERR(state->regs))
-		return;
-
-	val = on ? 0 : EXYNOS5_PHY_ENABLE;
-
-	regmap_update_bits(state->regs, state->drvdata->phy_ctrl_offset,
-			   EXYNOS5_PHY_ENABLE, val);
-}
-
 static int exynos_dp_video_phy_power_on(struct phy *phy)
 {
 	struct exynos_dp_video_phy *state = phy_get_drvdata(phy);
 
 	/* Disable power isolation on DP-PHY */
-	exynos_dp_video_phy_pwr_isol(state, 0);
-
-	return 0;
+	return regmap_update_bits(state->regs, state->drvdata->phy_ctrl_offset,
+				  EXYNOS5_PHY_ENABLE, EXYNOS5_PHY_ENABLE);
 }
 
 static int exynos_dp_video_phy_power_off(struct phy *phy)
@@ -59,9 +44,8 @@ static int exynos_dp_video_phy_power_off(struct phy *phy)
 	struct exynos_dp_video_phy *state = phy_get_drvdata(phy);
 
 	/* Enable power isolation on DP-PHY */
-	exynos_dp_video_phy_pwr_isol(state, 1);
-
-	return 0;
+	return regmap_update_bits(state->regs, state->drvdata->phy_ctrl_offset,
+				  EXYNOS5_PHY_ENABLE, 0);
 }
 
 static struct phy_ops exynos_dp_video_phy_ops = {
diff --git a/drivers/phy/phy-exynos-mipi-video.c b/drivers/phy/phy-exynos-mipi-video.c
index f017b2f2a54e..df7519a39ba0 100644
--- a/drivers/phy/phy-exynos-mipi-video.c
+++ b/drivers/phy/phy-exynos-mipi-video.c
@@ -43,7 +43,6 @@ struct exynos_mipi_video_phy {
 	} phys[EXYNOS_MIPI_PHYS_NUM];
 	spinlock_t slock;
 	void __iomem *regs;
-	struct mutex mutex;
 	struct regmap *regmap;
 };
 
@@ -59,8 +58,9 @@ static int __set_phy_state(struct exynos_mipi_video_phy *state,
 	else
 		reset = EXYNOS4_MIPI_PHY_SRESETN;
 
-	if (state->regmap) {
-		mutex_lock(&state->mutex);
+	spin_lock(&state->slock);
+
+	if (!IS_ERR(state->regmap)) {
 		regmap_read(state->regmap, offset, &val);
 		if (on)
 			val |= reset;
@@ -72,11 +72,9 @@ static int __set_phy_state(struct exynos_mipi_video_phy *state,
 		else if (!(val & EXYNOS4_MIPI_PHY_RESET_MASK))
 			val &= ~EXYNOS4_MIPI_PHY_ENABLE;
 		regmap_write(state->regmap, offset, val);
-		mutex_unlock(&state->mutex);
 	} else {
 		addr = state->regs + EXYNOS_MIPI_PHY_CONTROL(id / 2);
 
-		spin_lock(&state->slock);
 		val = readl(addr);
 		if (on)
 			val |= reset;
@@ -90,9 +88,9 @@ static int __set_phy_state(struct exynos_mipi_video_phy *state,
 			val &= ~EXYNOS4_MIPI_PHY_ENABLE;
 
 		writel(val, addr);
-		spin_unlock(&state->slock);
 	}
 
+	spin_unlock(&state->slock);
 	return 0;
 }
 
@@ -158,7 +156,6 @@ static int exynos_mipi_video_phy_probe(struct platform_device *pdev)
 
 	dev_set_drvdata(dev, state);
 	spin_lock_init(&state->slock);
-	mutex_init(&state->mutex);
 
 	for (i = 0; i < EXYNOS_MIPI_PHYS_NUM; i++) {
 		struct phy *phy = devm_phy_create(dev, NULL,
diff --git a/drivers/phy/phy-exynos4210-usb2.c b/drivers/phy/phy-exynos4210-usb2.c
index 236a52ad94eb..f30bbb0fb3b2 100644
--- a/drivers/phy/phy-exynos4210-usb2.c
+++ b/drivers/phy/phy-exynos4210-usb2.c
@@ -250,7 +250,6 @@ static const struct samsung_usb2_common_phy exynos4210_phys[] = {
 		.power_on	= exynos4210_power_on,
 		.power_off	= exynos4210_power_off,
 	},
-	{},
 };
 
 const struct samsung_usb2_phy_config exynos4210_usb2_phy_config = {
diff --git a/drivers/phy/phy-exynos4x12-usb2.c b/drivers/phy/phy-exynos4x12-usb2.c
index 0b9de88579b1..765da90a536f 100644
--- a/drivers/phy/phy-exynos4x12-usb2.c
+++ b/drivers/phy/phy-exynos4x12-usb2.c
@@ -361,7 +361,6 @@ static const struct samsung_usb2_common_phy exynos4x12_phys[] = {
 		.power_on	= exynos4x12_power_on,
 		.power_off	= exynos4x12_power_off,
 	},
-	{},
 };
 
 const struct samsung_usb2_phy_config exynos3250_usb2_phy_config = {
diff --git a/drivers/phy/phy-exynos5-usbdrd.c b/drivers/phy/phy-exynos5-usbdrd.c
index 04374018425f..d72ef15b0d68 100644
--- a/drivers/phy/phy-exynos5-usbdrd.c
+++ b/drivers/phy/phy-exynos5-usbdrd.c
@@ -531,7 +531,7 @@ static struct phy *exynos5_usbdrd_phy_xlate(struct device *dev,
 {
 	struct exynos5_usbdrd_phy *phy_drd = dev_get_drvdata(dev);
 
-	if (WARN_ON(args->args[0] > EXYNOS5_DRDPHYS_NUM))
+	if (WARN_ON(args->args[0] >= EXYNOS5_DRDPHYS_NUM))
 		return ERR_PTR(-ENODEV);
 
 	return phy_drd->phys[args->args[0]].phy;
@@ -624,6 +624,13 @@ static const struct exynos5_usbdrd_phy_drvdata exynos5250_usbdrd_phy = {
 	.has_common_clk_gate	= true,
 };
 
+static const struct exynos5_usbdrd_phy_drvdata exynos5433_usbdrd_phy = {
+	.phy_cfg		= phy_cfg_exynos5,
+	.pmu_offset_usbdrd0_phy	= EXYNOS5_USBDRD_PHY_CONTROL,
+	.pmu_offset_usbdrd1_phy	= EXYNOS5433_USBHOST30_PHY_CONTROL,
+	.has_common_clk_gate	= false,
+};
+
 static const struct exynos5_usbdrd_phy_drvdata exynos7_usbdrd_phy = {
 	.phy_cfg		= phy_cfg_exynos5,
 	.pmu_offset_usbdrd0_phy	= EXYNOS5_USBDRD_PHY_CONTROL,
@@ -638,6 +645,9 @@ static const struct of_device_id exynos5_usbdrd_phy_of_match[] = {
 		.compatible = "samsung,exynos5420-usbdrd-phy",
 		.data = &exynos5420_usbdrd_phy
 	}, {
+		.compatible = "samsung,exynos5433-usbdrd-phy",
+		.data = &exynos5433_usbdrd_phy
+	}, {
 		.compatible = "samsung,exynos7-usbdrd-phy",
 		.data = &exynos7_usbdrd_phy
 	},
diff --git a/drivers/phy/phy-exynos5250-usb2.c b/drivers/phy/phy-exynos5250-usb2.c
index 1c139aa0d074..2ed1735a076a 100644
--- a/drivers/phy/phy-exynos5250-usb2.c
+++ b/drivers/phy/phy-exynos5250-usb2.c
@@ -391,7 +391,6 @@ static const struct samsung_usb2_common_phy exynos5250_phys[] = {
 		.power_on	= exynos5250_power_on,
 		.power_off	= exynos5250_power_off,
 	},
-	{},
 };
 
 const struct samsung_usb2_phy_config exynos5250_usb2_phy_config = {
diff --git a/drivers/phy/phy-hix5hd2-sata.c b/drivers/phy/phy-hix5hd2-sata.c
index 34915b4202f1..d6b22659cac1 100644
--- a/drivers/phy/phy-hix5hd2-sata.c
+++ b/drivers/phy/phy-hix5hd2-sata.c
@@ -147,6 +147,9 @@ static int hix5hd2_sata_phy_probe(struct platform_device *pdev)
 		return -ENOMEM;
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res)
+		return -EINVAL;
+
 	priv->base = devm_ioremap(dev, res->start, resource_size(res));
 	if (!priv->base)
 		return -ENOMEM;
diff --git a/drivers/phy/phy-miphy28lp.c b/drivers/phy/phy-miphy28lp.c
index 9b2848e6115d..c4cc11dcb2a2 100644
--- a/drivers/phy/phy-miphy28lp.c
+++ b/drivers/phy/phy-miphy28lp.c
@@ -228,6 +228,7 @@ struct miphy28lp_dev {
 	struct regmap *regmap;
 	struct mutex miphy_mutex;
 	struct miphy28lp_phy **phys;
+	int nphys;
 };
 
 struct miphy_initval {
@@ -1116,7 +1117,7 @@ static struct phy *miphy28lp_xlate(struct device *dev,
 		return ERR_PTR(-EINVAL);
 	}
 
-	for (index = 0; index < of_get_child_count(dev->of_node); index++)
+	for (index = 0; index < miphy_dev->nphys; index++)
 		if (phynode == miphy_dev->phys[index]->phy->dev.of_node) {
 			miphy_phy = miphy_dev->phys[index];
 			break;
@@ -1138,6 +1139,7 @@ static struct phy *miphy28lp_xlate(struct device *dev,
 
 static struct phy_ops miphy28lp_ops = {
 	.init = miphy28lp_init,
+	.owner = THIS_MODULE,
 };
 
 static int miphy28lp_probe_resets(struct device_node *node,
@@ -1200,16 +1202,15 @@ static int miphy28lp_probe(struct platform_device *pdev)
 	struct miphy28lp_dev *miphy_dev;
 	struct phy_provider *provider;
 	struct phy *phy;
-	int chancount, port = 0;
-	int ret;
+	int ret, port = 0;
 
 	miphy_dev = devm_kzalloc(&pdev->dev, sizeof(*miphy_dev), GFP_KERNEL);
 	if (!miphy_dev)
 		return -ENOMEM;
 
-	chancount = of_get_child_count(np);
-	miphy_dev->phys = devm_kzalloc(&pdev->dev, sizeof(phy) * chancount,
-				       GFP_KERNEL);
+	miphy_dev->nphys = of_get_child_count(np);
+	miphy_dev->phys = devm_kcalloc(&pdev->dev, miphy_dev->nphys,
+				       sizeof(*miphy_dev->phys), GFP_KERNEL);
 	if (!miphy_dev->phys)
 		return -ENOMEM;
 
@@ -1258,10 +1259,7 @@ static int miphy28lp_probe(struct platform_device *pdev)
 	}
 
 	provider = devm_of_phy_provider_register(&pdev->dev, miphy28lp_xlate);
-	if (IS_ERR(provider))
-		return PTR_ERR(provider);
-
-	return 0;
+	return PTR_ERR_OR_ZERO(provider);
 }
 
 static const struct of_device_id miphy28lp_of_match[] = {
diff --git a/drivers/phy/phy-miphy365x.c b/drivers/phy/phy-miphy365x.c
index 6c80154e8bff..019c2d75344e 100644
--- a/drivers/phy/phy-miphy365x.c
+++ b/drivers/phy/phy-miphy365x.c
@@ -25,7 +25,7 @@
 #include <linux/mfd/syscon.h>
 #include <linux/regmap.h>
 
-#include <dt-bindings/phy/phy-miphy365x.h>
+#include <dt-bindings/phy/phy.h>
 
 #define HFC_TIMEOUT		100
 
@@ -150,6 +150,7 @@ struct miphy365x_dev {
 	struct regmap *regmap;
 	struct mutex miphy_mutex;
 	struct miphy365x_phy **phys;
+	int nphys;
 };
 
 /*
@@ -176,7 +177,7 @@ static u8 rx_tx_spd[] = {
 static int miphy365x_set_path(struct miphy365x_phy *miphy_phy,
 			      struct miphy365x_dev *miphy_dev)
 {
-	bool sata = (miphy_phy->type == MIPHY_TYPE_SATA);
+	bool sata = (miphy_phy->type == PHY_TYPE_SATA);
 
 	return regmap_update_bits(miphy_dev->regmap,
 				  miphy_phy->ctrlreg,
@@ -430,7 +431,7 @@ static int miphy365x_init(struct phy *phy)
 	}
 
 	/* Initialise Miphy for PCIe or SATA */
-	if (miphy_phy->type == MIPHY_TYPE_PCIE)
+	if (miphy_phy->type == PHY_TYPE_PCIE)
 		ret = miphy365x_init_pcie_port(miphy_phy, miphy_dev);
 	else
 		ret = miphy365x_init_sata_port(miphy_phy, miphy_dev);
@@ -454,8 +455,8 @@ int miphy365x_get_addr(struct device *dev, struct miphy365x_phy *miphy_phy,
 		return ret;
 	}
 
-	if (!((!strncmp(name, "sata", 4) && type == MIPHY_TYPE_SATA) ||
-	      (!strncmp(name, "pcie", 4) && type == MIPHY_TYPE_PCIE)))
+	if (!((!strncmp(name, "sata", 4) && type == PHY_TYPE_SATA) ||
+	      (!strncmp(name, "pcie", 4) && type == PHY_TYPE_PCIE)))
 		return 0;
 
 	miphy_phy->base = of_iomap(phynode, index);
@@ -485,7 +486,7 @@ static struct phy *miphy365x_xlate(struct device *dev,
 		return ERR_PTR(-EINVAL);
 	}
 
-	for (index = 0; index < of_get_child_count(dev->of_node); index++)
+	for (index = 0; index < miphy_dev->nphys; index++)
 		if (phynode == miphy_dev->phys[index]->phy->dev.of_node) {
 			miphy_phy = miphy_dev->phys[index];
 			break;
@@ -498,8 +499,8 @@ static struct phy *miphy365x_xlate(struct device *dev,
 
 	miphy_phy->type = args->args[0];
 
-	if (!(miphy_phy->type == MIPHY_TYPE_SATA ||
-	      miphy_phy->type == MIPHY_TYPE_PCIE)) {
+	if (!(miphy_phy->type == PHY_TYPE_SATA ||
+	      miphy_phy->type == PHY_TYPE_PCIE)) {
 		dev_err(dev, "Unsupported device type: %d\n", miphy_phy->type);
 		return ERR_PTR(-EINVAL);
 	}
@@ -541,16 +542,15 @@ static int miphy365x_probe(struct platform_device *pdev)
 	struct miphy365x_dev *miphy_dev;
 	struct phy_provider *provider;
 	struct phy *phy;
-	int chancount, port = 0;
-	int ret;
+	int ret, port = 0;
 
 	miphy_dev = devm_kzalloc(&pdev->dev, sizeof(*miphy_dev), GFP_KERNEL);
 	if (!miphy_dev)
 		return -ENOMEM;
 
-	chancount = of_get_child_count(np);
-	miphy_dev->phys = devm_kzalloc(&pdev->dev, sizeof(phy) * chancount,
-				       GFP_KERNEL);
+	miphy_dev->nphys = of_get_child_count(np);
+	miphy_dev->phys = devm_kcalloc(&pdev->dev, miphy_dev->nphys,
+				       sizeof(*miphy_dev->phys), GFP_KERNEL);
 	if (!miphy_dev->phys)
 		return -ENOMEM;
 
diff --git a/drivers/phy/phy-omap-control.c b/drivers/phy/phy-omap-control.c
index efe724f97e02..e9c41b3fa0ee 100644
--- a/drivers/phy/phy-omap-control.c
+++ b/drivers/phy/phy-omap-control.c
@@ -216,7 +216,6 @@ void omap_control_usb_set_mode(struct device *dev,
 		return;
 
 	ctrl_phy = dev_get_drvdata(dev);
-
 	if (!ctrl_phy) {
 		dev_err(dev, "Invalid control phy device\n");
 		return;
@@ -241,8 +240,6 @@ void omap_control_usb_set_mode(struct device *dev,
 }
 EXPORT_SYMBOL_GPL(omap_control_usb_set_mode);
 
-#ifdef CONFIG_OF
-
 static const enum omap_control_phy_type otghs_data = OMAP_CTRL_TYPE_OTGHS;
 static const enum omap_control_phy_type usb2_data = OMAP_CTRL_TYPE_USB2;
 static const enum omap_control_phy_type pipe3_data = OMAP_CTRL_TYPE_PIPE3;
@@ -278,8 +275,6 @@ static const struct of_device_id omap_control_phy_id_table[] = {
 	{},
 };
 MODULE_DEVICE_TABLE(of, omap_control_phy_id_table);
-#endif
-
 
 static int omap_control_phy_probe(struct platform_device *pdev)
 {
@@ -287,8 +282,7 @@ static int omap_control_phy_probe(struct platform_device *pdev)
 	const struct of_device_id *of_id;
 	struct omap_control_phy *control_phy;
 
-	of_id = of_match_device(of_match_ptr(omap_control_phy_id_table),
-				&pdev->dev);
+	of_id = of_match_device(omap_control_phy_id_table, &pdev->dev);
 	if (!of_id)
 		return -EINVAL;
 
@@ -344,7 +338,7 @@ static struct platform_driver omap_control_phy_driver = {
 	.probe		= omap_control_phy_probe,
 	.driver		= {
 		.name	= "omap-control-phy",
-		.of_match_table = of_match_ptr(omap_control_phy_id_table),
+		.of_match_table = omap_control_phy_id_table,
 	},
 };
 
@@ -360,7 +354,7 @@ static void __exit omap_control_phy_exit(void)
 }
 module_exit(omap_control_phy_exit);
 
-MODULE_ALIAS("platform: omap_control_phy");
+MODULE_ALIAS("platform:omap_control_phy");
 MODULE_AUTHOR("Texas Instruments Inc.");
 MODULE_DESCRIPTION("OMAP Control Module PHY Driver");
 MODULE_LICENSE("GPL v2");
diff --git a/drivers/phy/phy-omap-usb2.c b/drivers/phy/phy-omap-usb2.c
index 6f4aef3db248..183ef4368101 100644
--- a/drivers/phy/phy-omap-usb2.c
+++ b/drivers/phy/phy-omap-usb2.c
@@ -144,7 +144,6 @@ static struct phy_ops ops = {
 	.owner		= THIS_MODULE,
 };
 
-#ifdef CONFIG_OF
 static const struct usb_phy_data omap_usb2_data = {
 	.label = "omap_usb2",
 	.flags = OMAP_USB2_HAS_START_SRP | OMAP_USB2_HAS_SET_VBUS,
@@ -185,7 +184,6 @@ static const struct of_device_id omap_usb2_id_table[] = {
 	{},
 };
 MODULE_DEVICE_TABLE(of, omap_usb2_id_table);
-#endif
 
 static int omap_usb2_probe(struct platform_device *pdev)
 {
@@ -200,7 +198,7 @@ static int omap_usb2_probe(struct platform_device *pdev)
 	const struct of_device_id *of_id;
 	struct usb_phy_data *phy_data;
 
-	of_id = of_match_device(of_match_ptr(omap_usb2_id_table), &pdev->dev);
+	of_id = of_match_device(omap_usb2_id_table, &pdev->dev);
 
 	if (!of_id)
 		return -EINVAL;
@@ -296,10 +294,11 @@ static int omap_usb2_probe(struct platform_device *pdev)
 			dev_warn(&pdev->dev,
 				 "found usb_otg_ss_refclk960m, please fix DTS\n");
 		}
-	} else {
-		clk_prepare(phy->optclk);
 	}
 
+	if (!IS_ERR(phy->optclk))
+		clk_prepare(phy->optclk);
+
 	usb_add_phy_dev(&phy->phy);
 
 	return 0;
@@ -377,13 +376,13 @@ static struct platform_driver omap_usb2_driver = {
 	.driver		= {
 		.name	= "omap-usb2",
 		.pm	= DEV_PM_OPS,
-		.of_match_table = of_match_ptr(omap_usb2_id_table),
+		.of_match_table = omap_usb2_id_table,
 	},
 };
 
 module_platform_driver(omap_usb2_driver);
 
-MODULE_ALIAS("platform: omap_usb2");
+MODULE_ALIAS("platform:omap_usb2");
 MODULE_AUTHOR("Texas Instruments Inc.");
 MODULE_DESCRIPTION("OMAP USB2 phy driver");
 MODULE_LICENSE("GPL v2");
diff --git a/drivers/phy/phy-qcom-ufs.c b/drivers/phy/phy-qcom-ufs.c
index 44ee983d57fe..f9c618f0ab6e 100644
--- a/drivers/phy/phy-qcom-ufs.c
+++ b/drivers/phy/phy-qcom-ufs.c
@@ -73,6 +73,7 @@ int ufs_qcom_phy_calibrate(struct ufs_qcom_phy *ufs_qcom_phy,
 out:
 	return ret;
 }
+EXPORT_SYMBOL_GPL(ufs_qcom_phy_calibrate);
 
 struct phy *ufs_qcom_phy_generic_probe(struct platform_device *pdev,
 				struct ufs_qcom_phy *common_cfg,
@@ -101,6 +102,7 @@ struct phy *ufs_qcom_phy_generic_probe(struct platform_device *pdev,
 	if (IS_ERR(generic_phy)) {
 		err =  PTR_ERR(generic_phy);
 		dev_err(dev, "%s: failed to create phy %d\n", __func__, err);
+		generic_phy = NULL;
 		goto out;
 	}
 
@@ -110,6 +112,7 @@ struct phy *ufs_qcom_phy_generic_probe(struct platform_device *pdev,
 out:
 	return generic_phy;
 }
+EXPORT_SYMBOL_GPL(ufs_qcom_phy_generic_probe);
 
 /*
  * This assumes the embedded phy structure inside generic_phy is of type
@@ -121,6 +124,7 @@ struct ufs_qcom_phy *get_ufs_qcom_phy(struct phy *generic_phy)
 {
 	return (struct ufs_qcom_phy *)phy_get_drvdata(generic_phy);
 }
+EXPORT_SYMBOL_GPL(get_ufs_qcom_phy);
 
 static
 int ufs_qcom_phy_base_init(struct platform_device *pdev,
@@ -131,40 +135,23 @@ int ufs_qcom_phy_base_init(struct platform_device *pdev,
 	int err = 0;
 
 	res = platform_get_resource_byname(pdev, IORESOURCE_MEM, "phy_mem");
-	if (!res) {
-		dev_err(dev, "%s: phy_mem resource not found\n", __func__);
-		err = -ENOMEM;
-		goto out;
-	}
-
 	phy_common->mmio = devm_ioremap_resource(dev, res);
 	if (IS_ERR((void const *)phy_common->mmio)) {
 		err = PTR_ERR((void const *)phy_common->mmio);
 		phy_common->mmio = NULL;
 		dev_err(dev, "%s: ioremap for phy_mem resource failed %d\n",
 			__func__, err);
-		goto out;
+		return err;
 	}
 
 	/* "dev_ref_clk_ctrl_mem" is optional resource */
 	res = platform_get_resource_byname(pdev, IORESOURCE_MEM,
 					   "dev_ref_clk_ctrl_mem");
-	if (!res) {
-		dev_dbg(dev, "%s: dev_ref_clk_ctrl_mem resource not found\n",
-			__func__);
-		goto out;
-	}
-
 	phy_common->dev_ref_clk_ctrl_mmio = devm_ioremap_resource(dev, res);
-	if (IS_ERR((void const *)phy_common->dev_ref_clk_ctrl_mmio)) {
-		err = PTR_ERR((void const *)phy_common->dev_ref_clk_ctrl_mmio);
+	if (IS_ERR((void const *)phy_common->dev_ref_clk_ctrl_mmio))
 		phy_common->dev_ref_clk_ctrl_mmio = NULL;
-		dev_err(dev, "%s: ioremap for dev_ref_clk_ctrl_mem resource failed %d\n",
-			__func__, err);
-	}
 
-out:
-	return err;
+	return 0;
 }
 
 static int __ufs_qcom_phy_clk_get(struct phy *phy,
@@ -228,6 +215,7 @@ ufs_qcom_phy_init_clks(struct phy *generic_phy,
 out:
 	return err;
 }
+EXPORT_SYMBOL_GPL(ufs_qcom_phy_init_clks);
 
 int
 ufs_qcom_phy_init_vregulators(struct phy *generic_phy,
@@ -252,6 +240,7 @@ ufs_qcom_phy_init_vregulators(struct phy *generic_phy,
 out:
 	return err;
 }
+EXPORT_SYMBOL_GPL(ufs_qcom_phy_init_vregulators);
 
 static int __ufs_qcom_phy_init_vreg(struct phy *phy,
 		struct ufs_qcom_phy_vreg *vreg, const char *name, bool optional)
@@ -346,10 +335,10 @@ int ufs_qcom_phy_cfg_vreg(struct phy *phy,
 			goto out;
 		}
 		uA_load = on ? vreg->max_uA : 0;
-		ret = regulator_set_optimum_mode(reg, uA_load);
+		ret = regulator_set_load(reg, uA_load);
 		if (ret >= 0) {
 			/*
-			 * regulator_set_optimum_mode() returns new regulator
+			 * regulator_set_load() returns new regulator
 			 * mode upon success.
 			 */
 			ret = 0;
@@ -647,6 +636,7 @@ int ufs_qcom_phy_remove(struct phy *generic_phy,
 
 	return 0;
 }
+EXPORT_SYMBOL_GPL(ufs_qcom_phy_remove);
 
 int ufs_qcom_phy_exit(struct phy *generic_phy)
 {
@@ -657,6 +647,7 @@ int ufs_qcom_phy_exit(struct phy *generic_phy)
 
 	return 0;
 }
+EXPORT_SYMBOL_GPL(ufs_qcom_phy_exit);
 
 int ufs_qcom_phy_is_pcs_ready(struct phy *generic_phy)
 {
@@ -725,6 +716,7 @@ out_disable_phy:
 out:
 	return err;
 }
+EXPORT_SYMBOL_GPL(ufs_qcom_phy_power_on);
 
 int ufs_qcom_phy_power_off(struct phy *generic_phy)
 {
@@ -743,3 +735,4 @@ int ufs_qcom_phy_power_off(struct phy *generic_phy)
 
 	return 0;
 }
+EXPORT_SYMBOL_GPL(ufs_qcom_phy_power_off);
diff --git a/drivers/phy/phy-rockchip-usb.c b/drivers/phy/phy-rockchip-usb.c
index 22011c3b6a4b..7d4c33643768 100644
--- a/drivers/phy/phy-rockchip-usb.c
+++ b/drivers/phy/phy-rockchip-usb.c
@@ -61,8 +61,6 @@ static int rockchip_usb_phy_power_off(struct phy *_phy)
 		return ret;
 
 	clk_disable_unprepare(phy->clk);
-	if (ret)
-		return ret;
 
 	return 0;
 }
@@ -78,8 +76,10 @@ static int rockchip_usb_phy_power_on(struct phy *_phy)
 
 	/* Power up usb phy analog blocks by set siddq 0 */
 	ret = rockchip_usb_phy_power(phy, 0);
-	if (ret)
+	if (ret) {
+		clk_disable_unprepare(phy->clk);
 		return ret;
+	}
 
 	return 0;
 }
diff --git a/drivers/phy/phy-samsung-usb2.c b/drivers/phy/phy-samsung-usb2.c
index 4a12f66b7fb5..55b6994932e3 100644
--- a/drivers/phy/phy-samsung-usb2.c
+++ b/drivers/phy/phy-samsung-usb2.c
@@ -37,10 +37,14 @@ static int samsung_usb2_phy_power_on(struct phy *phy)
 		spin_lock(&drv->lock);
 		ret = inst->cfg->power_on(inst);
 		spin_unlock(&drv->lock);
+		if (ret)
+			goto err_power_on;
 	}
 
 	return 0;
 
+err_power_on:
+	clk_disable_unprepare(drv->ref_clk);
 err_instance_clk:
 	clk_disable_unprepare(drv->clk);
 err_main_clk:
@@ -51,7 +55,7 @@ static int samsung_usb2_phy_power_off(struct phy *phy)
 {
 	struct samsung_usb2_phy_instance *inst = phy_get_drvdata(phy);
 	struct samsung_usb2_phy_driver *drv = inst->drv;
-	int ret = 0;
+	int ret;
 
 	dev_dbg(drv->dev, "Request to power_off \"%s\" usb phy\n",
 		inst->cfg->label);
@@ -59,10 +63,12 @@ static int samsung_usb2_phy_power_off(struct phy *phy)
 		spin_lock(&drv->lock);
 		ret = inst->cfg->power_off(inst);
 		spin_unlock(&drv->lock);
+		if (ret)
+			return ret;
 	}
 	clk_disable_unprepare(drv->ref_clk);
 	clk_disable_unprepare(drv->clk);
-	return ret;
+	return 0;
 }
 
 static struct phy_ops samsung_usb2_phy_ops = {
diff --git a/drivers/phy/phy-spear1310-miphy.c b/drivers/phy/phy-spear1310-miphy.c
index 9f47fae7eecb..65ae640cfbd1 100644
--- a/drivers/phy/phy-spear1310-miphy.c
+++ b/drivers/phy/phy-spear1310-miphy.c
@@ -192,14 +192,14 @@ static struct phy *spear1310_miphy_xlate(struct device *dev,
 
 	if (args->args_count < 1) {
 		dev_err(dev, "DT did not pass correct no of args\n");
-		return NULL;
+		return ERR_PTR(-ENODEV);
 	}
 
 	priv->mode = args->args[0];
 
 	if (priv->mode != SATA && priv->mode != PCIE) {
 		dev_err(dev, "DT did not pass correct phy mode\n");
-		return NULL;
+		return ERR_PTR(-ENODEV);
 	}
 
 	return priv->phy;
diff --git a/drivers/phy/phy-spear1340-miphy.c b/drivers/phy/phy-spear1340-miphy.c
index e42bc200275f..1a00c2817f34 100644
--- a/drivers/phy/phy-spear1340-miphy.c
+++ b/drivers/phy/phy-spear1340-miphy.c
@@ -229,14 +229,14 @@ static struct phy *spear1340_miphy_xlate(struct device *dev,
 
 	if (args->args_count < 1) {
 		dev_err(dev, "DT did not pass correct no of args\n");
-		return NULL;
+		return ERR_PTR(-ENODEV);
 	}
 
 	priv->mode = args->args[0];
 
 	if (priv->mode != SATA && priv->mode != PCIE) {
 		dev_err(dev, "DT did not pass correct phy mode\n");
-		return NULL;
+		return ERR_PTR(-ENODEV);
 	}
 
 	return priv->phy;
diff --git a/drivers/phy/phy-stih41x-usb.c b/drivers/phy/phy-stih41x-usb.c
index a603801293ff..c093b472b57d 100644
--- a/drivers/phy/phy-stih41x-usb.c
+++ b/drivers/phy/phy-stih41x-usb.c
@@ -87,8 +87,12 @@ static int stih41x_usb_phy_power_on(struct phy *phy)
 		return ret;
 	}
 
-	return regmap_update_bits(phy_dev->regmap, phy_dev->cfg->syscfg,
-			phy_dev->cfg->oscok, phy_dev->cfg->oscok);
+	ret = regmap_update_bits(phy_dev->regmap, phy_dev->cfg->syscfg,
+				 phy_dev->cfg->oscok, phy_dev->cfg->oscok);
+	if (ret)
+		clk_disable_unprepare(phy_dev->clk);
+
+	return ret;
 }
 
 static int stih41x_usb_phy_power_off(struct phy *phy)
diff --git a/drivers/phy/phy-sun9i-usb.c b/drivers/phy/phy-sun9i-usb.c
new file mode 100644
index 000000000000..0095914a662c
--- /dev/null
+++ b/drivers/phy/phy-sun9i-usb.c
@@ -0,0 +1,202 @@
+/*
+ * Allwinner sun9i USB phy driver
+ *
+ * Copyright (C) 2014-2015 Chen-Yu Tsai <wens@csie.org>
+ *
+ * Based on phy-sun4i-usb.c from
+ * Hans de Goede <hdegoede@redhat.com>
+ *
+ * and code from
+ * Allwinner Technology Co., Ltd. <www.allwinnertech.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/clk.h>
+#include <linux/err.h>
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/phy/phy.h>
+#include <linux/usb/of.h>
+#include <linux/platform_device.h>
+#include <linux/reset.h>
+
+#define SUNXI_AHB_INCR16_BURST_EN	BIT(11)
+#define SUNXI_AHB_INCR8_BURST_EN	BIT(10)
+#define SUNXI_AHB_INCR4_BURST_EN	BIT(9)
+#define SUNXI_AHB_INCRX_ALIGN_EN	BIT(8)
+#define SUNXI_ULPI_BYPASS_EN		BIT(0)
+
+/* usb1 HSIC specific bits */
+#define SUNXI_EHCI_HS_FORCE		BIT(20)
+#define SUNXI_HSIC_CONNECT_DET		BIT(17)
+#define SUNXI_HSIC_CONNECT_INT		BIT(16)
+#define SUNXI_HSIC			BIT(1)
+
+struct sun9i_usb_phy {
+	struct phy *phy;
+	void __iomem *pmu;
+	struct reset_control *reset;
+	struct clk *clk;
+	struct clk *hsic_clk;
+	enum usb_phy_interface type;
+};
+
+static void sun9i_usb_phy_passby(struct sun9i_usb_phy *phy, int enable)
+{
+	u32 bits, reg_value;
+
+	bits = SUNXI_AHB_INCR16_BURST_EN | SUNXI_AHB_INCR8_BURST_EN |
+		SUNXI_AHB_INCR4_BURST_EN | SUNXI_AHB_INCRX_ALIGN_EN |
+		SUNXI_ULPI_BYPASS_EN;
+
+	if (phy->type == USBPHY_INTERFACE_MODE_HSIC)
+		bits |= SUNXI_HSIC | SUNXI_EHCI_HS_FORCE |
+			SUNXI_HSIC_CONNECT_DET | SUNXI_HSIC_CONNECT_INT;
+
+	reg_value = readl(phy->pmu);
+
+	if (enable)
+		reg_value |= bits;
+	else
+		reg_value &= ~bits;
+
+	writel(reg_value, phy->pmu);
+}
+
+static int sun9i_usb_phy_init(struct phy *_phy)
+{
+	struct sun9i_usb_phy *phy = phy_get_drvdata(_phy);
+	int ret;
+
+	ret = clk_prepare_enable(phy->clk);
+	if (ret)
+		goto err_clk;
+
+	ret = clk_prepare_enable(phy->hsic_clk);
+	if (ret)
+		goto err_hsic_clk;
+
+	ret = reset_control_deassert(phy->reset);
+	if (ret)
+		goto err_reset;
+
+	sun9i_usb_phy_passby(phy, 1);
+	return 0;
+
+err_reset:
+	clk_disable_unprepare(phy->hsic_clk);
+
+err_hsic_clk:
+	clk_disable_unprepare(phy->clk);
+
+err_clk:
+	return ret;
+}
+
+static int sun9i_usb_phy_exit(struct phy *_phy)
+{
+	struct sun9i_usb_phy *phy = phy_get_drvdata(_phy);
+
+	sun9i_usb_phy_passby(phy, 0);
+	reset_control_assert(phy->reset);
+	clk_disable_unprepare(phy->hsic_clk);
+	clk_disable_unprepare(phy->clk);
+
+	return 0;
+}
+
+static struct phy_ops sun9i_usb_phy_ops = {
+	.init		= sun9i_usb_phy_init,
+	.exit		= sun9i_usb_phy_exit,
+	.owner		= THIS_MODULE,
+};
+
+static int sun9i_usb_phy_probe(struct platform_device *pdev)
+{
+	struct sun9i_usb_phy *phy;
+	struct device *dev = &pdev->dev;
+	struct device_node *np = dev->of_node;
+	struct phy_provider *phy_provider;
+	struct resource *res;
+
+	phy = devm_kzalloc(dev, sizeof(*phy), GFP_KERNEL);
+	if (!phy)
+		return -ENOMEM;
+
+	phy->type = of_usb_get_phy_mode(np);
+	if (phy->type == USBPHY_INTERFACE_MODE_HSIC) {
+		phy->clk = devm_clk_get(dev, "hsic_480M");
+		if (IS_ERR(phy->clk)) {
+			dev_err(dev, "failed to get hsic_480M clock\n");
+			return PTR_ERR(phy->clk);
+		}
+
+		phy->hsic_clk = devm_clk_get(dev, "hsic_12M");
+		if (IS_ERR(phy->clk)) {
+			dev_err(dev, "failed to get hsic_12M clock\n");
+			return PTR_ERR(phy->clk);
+		}
+
+		phy->reset = devm_reset_control_get(dev, "hsic");
+		if (IS_ERR(phy->reset)) {
+			dev_err(dev, "failed to get reset control\n");
+			return PTR_ERR(phy->reset);
+		}
+	} else {
+		phy->clk = devm_clk_get(dev, "phy");
+		if (IS_ERR(phy->clk)) {
+			dev_err(dev, "failed to get phy clock\n");
+			return PTR_ERR(phy->clk);
+		}
+
+		phy->reset = devm_reset_control_get(dev, "phy");
+		if (IS_ERR(phy->reset)) {
+			dev_err(dev, "failed to get reset control\n");
+			return PTR_ERR(phy->reset);
+		}
+	}
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	phy->pmu = devm_ioremap_resource(dev, res);
+	if (IS_ERR(phy->pmu))
+		return PTR_ERR(phy->pmu);
+
+	phy->phy = devm_phy_create(dev, NULL, &sun9i_usb_phy_ops);
+	if (IS_ERR(phy->phy)) {
+		dev_err(dev, "failed to create PHY\n");
+		return PTR_ERR(phy->phy);
+	}
+
+	phy_set_drvdata(phy->phy, phy);
+	phy_provider = devm_of_phy_provider_register(dev, of_phy_simple_xlate);
+
+	return PTR_ERR_OR_ZERO(phy_provider);
+}
+
+static const struct of_device_id sun9i_usb_phy_of_match[] = {
+	{ .compatible = "allwinner,sun9i-a80-usb-phy" },
+	{ },
+};
+MODULE_DEVICE_TABLE(of, sun9i_usb_phy_of_match);
+
+static struct platform_driver sun9i_usb_phy_driver = {
+	.probe	= sun9i_usb_phy_probe,
+	.driver = {
+		.of_match_table	= sun9i_usb_phy_of_match,
+		.name  = "sun9i-usb-phy",
+	}
+};
+module_platform_driver(sun9i_usb_phy_driver);
+
+MODULE_DESCRIPTION("Allwinner sun9i USB phy driver");
+MODULE_AUTHOR("Chen-Yu Tsai <wens@csie.org>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/phy/phy-ti-pipe3.c b/drivers/phy/phy-ti-pipe3.c
index 95c88f929f27..53f295c1bab1 100644
--- a/drivers/phy/phy-ti-pipe3.c
+++ b/drivers/phy/phy-ti-pipe3.c
@@ -165,15 +165,11 @@ static int ti_pipe3_dpll_wait_lock(struct ti_pipe3 *phy)
 		cpu_relax();
 		val = ti_pipe3_readl(phy->pll_ctrl_base, PLL_STATUS);
 		if (val & PLL_LOCK)
-			break;
+			return 0;
 	} while (!time_after(jiffies, timeout));
 
-	if (!(val & PLL_LOCK)) {
-		dev_err(phy->dev, "DPLL failed to lock\n");
-		return -EBUSY;
-	}
-
-	return 0;
+	dev_err(phy->dev, "DPLL failed to lock\n");
+	return -EBUSY;
 }
 
 static int ti_pipe3_dpll_program(struct ti_pipe3 *phy)
@@ -291,9 +287,7 @@ static struct phy_ops ops = {
 	.owner		= THIS_MODULE,
 };
 
-#ifdef CONFIG_OF
 static const struct of_device_id ti_pipe3_id_table[];
-#endif
 
 static int ti_pipe3_probe(struct platform_device *pdev)
 {
@@ -315,8 +309,7 @@ static int ti_pipe3_probe(struct platform_device *pdev)
 	spin_lock_init(&phy->lock);
 
 	if (!of_device_is_compatible(node, "ti,phy-pipe3-pcie")) {
-		match = of_match_device(of_match_ptr(ti_pipe3_id_table),
-					&pdev->dev);
+		match = of_match_device(ti_pipe3_id_table, &pdev->dev);
 		if (!match)
 			return -EINVAL;
 
@@ -574,7 +567,6 @@ static const struct dev_pm_ops ti_pipe3_pm_ops = {
 	SET_SYSTEM_SLEEP_PM_OPS(ti_pipe3_suspend, ti_pipe3_resume)
 };
 
-#ifdef CONFIG_OF
 static const struct of_device_id ti_pipe3_id_table[] = {
 	{
 		.compatible = "ti,phy-usb3",
@@ -594,7 +586,6 @@ static const struct of_device_id ti_pipe3_id_table[] = {
 	{}
 };
 MODULE_DEVICE_TABLE(of, ti_pipe3_id_table);
-#endif
 
 static struct platform_driver ti_pipe3_driver = {
 	.probe		= ti_pipe3_probe,
@@ -602,13 +593,13 @@ static struct platform_driver ti_pipe3_driver = {
 	.driver		= {
 		.name	= "ti-pipe3",
 		.pm	= &ti_pipe3_pm_ops,
-		.of_match_table = of_match_ptr(ti_pipe3_id_table),
+		.of_match_table = ti_pipe3_id_table,
 	},
 };
 
 module_platform_driver(ti_pipe3_driver);
 
-MODULE_ALIAS("platform: ti_pipe3");
+MODULE_ALIAS("platform:ti_pipe3");
 MODULE_AUTHOR("Texas Instruments Inc.");
 MODULE_DESCRIPTION("TI PIPE3 phy driver");
 MODULE_LICENSE("GPL v2");
diff --git a/drivers/phy/phy-twl4030-usb.c b/drivers/phy/phy-twl4030-usb.c
index 8e87f54671f3..bc42d6a8939f 100644
--- a/drivers/phy/phy-twl4030-usb.c
+++ b/drivers/phy/phy-twl4030-usb.c
@@ -666,7 +666,6 @@ static int twl4030_usb_probe(struct platform_device *pdev)
 	twl->dev		= &pdev->dev;
 	twl->irq		= platform_get_irq(pdev, 0);
 	twl->vbus_supplied	= false;
-	twl->linkstat		= -EINVAL;
 	twl->linkstat		= OMAP_MUSB_UNKNOWN;
 
 	twl->phy.dev		= twl->dev;
diff --git a/drivers/phy/phy-xgene.c b/drivers/phy/phy-xgene.c
index 29214a36ea28..385362e5b2f6 100644
--- a/drivers/phy/phy-xgene.c
+++ b/drivers/phy/phy-xgene.c
@@ -1657,7 +1657,6 @@ static int xgene_phy_probe(struct platform_device *pdev)
 	struct phy_provider *phy_provider;
 	struct xgene_phy_ctx *ctx;
 	struct resource *res;
-	int rc = 0;
 	u32 default_spd[] = DEFAULT_SATA_SPD_SEL;
 	u32 default_txboost_gain[] = DEFAULT_SATA_TXBOOST_GAIN;
 	u32 default_txeye_direction[] = DEFAULT_SATA_TXEYEDIRECTION;
@@ -1676,10 +1675,8 @@ static int xgene_phy_probe(struct platform_device *pdev)
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	ctx->sds_base = devm_ioremap_resource(&pdev->dev, res);
-	if (IS_ERR(ctx->sds_base)) {
-		rc = PTR_ERR(ctx->sds_base);
-		goto error;
-	}
+	if (IS_ERR(ctx->sds_base))
+		return PTR_ERR(ctx->sds_base);
 
 	/* Retrieve optional clock */
 	ctx->clk = clk_get(&pdev->dev, NULL);
@@ -1704,28 +1701,17 @@ static int xgene_phy_probe(struct platform_device *pdev)
 	for (i = 0; i < MAX_LANE; i++)
 		ctx->sata_param.speed[i] = 2; /* Default to Gen3 */
 
-	ctx->dev = &pdev->dev;
 	platform_set_drvdata(pdev, ctx);
 
 	ctx->phy = devm_phy_create(ctx->dev, NULL, &xgene_phy_ops);
 	if (IS_ERR(ctx->phy)) {
 		dev_dbg(&pdev->dev, "Failed to create PHY\n");
-		rc = PTR_ERR(ctx->phy);
-		goto error;
+		return PTR_ERR(ctx->phy);
 	}
 	phy_set_drvdata(ctx->phy, ctx);
 
-	phy_provider = devm_of_phy_provider_register(ctx->dev,
-						     xgene_phy_xlate);
-	if (IS_ERR(phy_provider)) {
-		rc = PTR_ERR(phy_provider);
-		goto error;
-	}
-
-	return 0;
-
-error:
-	return rc;
+	phy_provider = devm_of_phy_provider_register(ctx->dev, xgene_phy_xlate);
+	return PTR_ERR_OR_ZERO(phy_provider);
 }
 
 static const struct of_device_id xgene_phy_of_match[] = {
diff --git a/drivers/pinctrl/intel/pinctrl-baytrail.c b/drivers/pinctrl/intel/pinctrl-baytrail.c
index 5afe03e28b91..2062c224e32f 100644
--- a/drivers/pinctrl/intel/pinctrl-baytrail.c
+++ b/drivers/pinctrl/intel/pinctrl-baytrail.c
@@ -66,6 +66,10 @@
 #define BYT_DIR_MASK		(BIT(1) | BIT(2))
 #define BYT_TRIG_MASK		(BIT(26) | BIT(25) | BIT(24))
 
+#define BYT_CONF0_RESTORE_MASK	(BYT_DIRECT_IRQ_EN | BYT_TRIG_MASK | \
+				 BYT_PIN_MUX)
+#define BYT_VAL_RESTORE_MASK	(BYT_DIR_MASK | BYT_LEVEL)
+
 #define BYT_NGPIO_SCORE		102
 #define BYT_NGPIO_NCORE		28
 #define BYT_NGPIO_SUS		44
@@ -134,12 +138,18 @@ static struct pinctrl_gpio_range byt_ranges[] = {
 	},
 };
 
+struct byt_gpio_pin_context {
+	u32 conf0;
+	u32 val;
+};
+
 struct byt_gpio {
 	struct gpio_chip		chip;
 	struct platform_device		*pdev;
 	spinlock_t			lock;
 	void __iomem			*reg_base;
 	struct pinctrl_gpio_range	*range;
+	struct byt_gpio_pin_context	*saved_context;
 };
 
 #define to_byt_gpio(c)	container_of(c, struct byt_gpio, chip)
@@ -158,40 +168,62 @@ static void __iomem *byt_gpio_reg(struct gpio_chip *chip, unsigned offset,
 	return vg->reg_base + reg_offset + reg;
 }
 
-static bool is_special_pin(struct byt_gpio *vg, unsigned offset)
+static void byt_gpio_clear_triggering(struct byt_gpio *vg, unsigned offset)
+{
+	void __iomem *reg = byt_gpio_reg(&vg->chip, offset, BYT_CONF0_REG);
+	unsigned long flags;
+	u32 value;
+
+	spin_lock_irqsave(&vg->lock, flags);
+	value = readl(reg);
+	value &= ~(BYT_TRIG_POS | BYT_TRIG_NEG | BYT_TRIG_LVL);
+	writel(value, reg);
+	spin_unlock_irqrestore(&vg->lock, flags);
+}
+
+static u32 byt_get_gpio_mux(struct byt_gpio *vg, unsigned offset)
 {
 	/* SCORE pin 92-93 */
 	if (!strcmp(vg->range->name, BYT_SCORE_ACPI_UID) &&
 		offset >= 92 && offset <= 93)
-		return true;
+		return 1;
 
 	/* SUS pin 11-21 */
 	if (!strcmp(vg->range->name, BYT_SUS_ACPI_UID) &&
 		offset >= 11 && offset <= 21)
-		return true;
+		return 1;
 
-	return false;
+	return 0;
 }
 
 static int byt_gpio_request(struct gpio_chip *chip, unsigned offset)
 {
 	struct byt_gpio *vg = to_byt_gpio(chip);
 	void __iomem *reg = byt_gpio_reg(chip, offset, BYT_CONF0_REG);
-	u32 value;
-	bool special;
+	u32 value, gpio_mux;
 
 	/*
 	 * In most cases, func pin mux 000 means GPIO function.
 	 * But, some pins may have func pin mux 001 represents
-	 * GPIO function. Only allow user to export pin with
-	 * func pin mux preset as GPIO function by BIOS/FW.
+	 * GPIO function.
+	 *
+	 * Because there are devices out there where some pins were not
+	 * configured correctly we allow changing the mux value from
+	 * request (but print out warning about that).
 	 */
 	value = readl(reg) & BYT_PIN_MUX;
-	special = is_special_pin(vg, offset);
-	if ((special && value != 1) || (!special && value)) {
-		dev_err(&vg->pdev->dev,
-			"pin %u cannot be used as GPIO.\n", offset);
-		return -EINVAL;
+	gpio_mux = byt_get_gpio_mux(vg, offset);
+	if (WARN_ON(gpio_mux != value)) {
+		unsigned long flags;
+
+		spin_lock_irqsave(&vg->lock, flags);
+		value = readl(reg) & ~BYT_PIN_MUX;
+		value |= gpio_mux;
+		writel(value, reg);
+		spin_unlock_irqrestore(&vg->lock, flags);
+
+		dev_warn(&vg->pdev->dev,
+			 "pin %u forcibly re-configured as GPIO\n", offset);
 	}
 
 	pm_runtime_get(&vg->pdev->dev);
@@ -202,14 +234,8 @@ static int byt_gpio_request(struct gpio_chip *chip, unsigned offset)
 static void byt_gpio_free(struct gpio_chip *chip, unsigned offset)
 {
 	struct byt_gpio *vg = to_byt_gpio(chip);
-	void __iomem *reg = byt_gpio_reg(&vg->chip, offset, BYT_CONF0_REG);
-	u32 value;
-
-	/* clear interrupt triggering */
-	value = readl(reg);
-	value &= ~(BYT_TRIG_POS | BYT_TRIG_NEG | BYT_TRIG_LVL);
-	writel(value, reg);
 
+	byt_gpio_clear_triggering(vg, offset);
 	pm_runtime_put(&vg->pdev->dev);
 }
 
@@ -236,23 +262,13 @@ static int byt_irq_type(struct irq_data *d, unsigned type)
 	value &= ~(BYT_DIRECT_IRQ_EN | BYT_TRIG_POS | BYT_TRIG_NEG |
 		   BYT_TRIG_LVL);
 
-	switch (type) {
-	case IRQ_TYPE_LEVEL_HIGH:
-		value |= BYT_TRIG_LVL;
-	case IRQ_TYPE_EDGE_RISING:
-		value |= BYT_TRIG_POS;
-		break;
-	case IRQ_TYPE_LEVEL_LOW:
-		value |= BYT_TRIG_LVL;
-	case IRQ_TYPE_EDGE_FALLING:
-		value |= BYT_TRIG_NEG;
-		break;
-	case IRQ_TYPE_EDGE_BOTH:
-		value |= (BYT_TRIG_NEG | BYT_TRIG_POS);
-		break;
-	}
 	writel(value, reg);
 
+	if (type & IRQ_TYPE_EDGE_BOTH)
+		__irq_set_handler_locked(d->irq, handle_edge_irq);
+	else if (type & IRQ_TYPE_LEVEL_MASK)
+		__irq_set_handler_locked(d->irq, handle_level_irq);
+
 	spin_unlock_irqrestore(&vg->lock, flags);
 
 	return 0;
@@ -410,58 +426,80 @@ static void byt_gpio_irq_handler(unsigned irq, struct irq_desc *desc)
 	struct irq_data *data = irq_desc_get_irq_data(desc);
 	struct byt_gpio *vg = to_byt_gpio(irq_desc_get_handler_data(desc));
 	struct irq_chip *chip = irq_data_get_irq_chip(data);
-	u32 base, pin, mask;
+	u32 base, pin;
 	void __iomem *reg;
-	u32 pending;
+	unsigned long pending;
 	unsigned virq;
-	int looplimit = 0;
 
 	/* check from GPIO controller which pin triggered the interrupt */
 	for (base = 0; base < vg->chip.ngpio; base += 32) {
-
 		reg = byt_gpio_reg(&vg->chip, base, BYT_INT_STAT_REG);
-
-		while ((pending = readl(reg))) {
-			pin = __ffs(pending);
-			mask = BIT(pin);
-			/* Clear before handling so we can't lose an edge */
-			writel(mask, reg);
-
+		pending = readl(reg);
+		for_each_set_bit(pin, &pending, 32) {
 			virq = irq_find_mapping(vg->chip.irqdomain, base + pin);
 			generic_handle_irq(virq);
-
-			/* In case bios or user sets triggering incorretly a pin
-			 * might remain in "interrupt triggered" state.
-			 */
-			if (looplimit++ > 32) {
-				dev_err(&vg->pdev->dev,
-					"Gpio %d interrupt flood, disabling\n",
-					base + pin);
-
-				reg = byt_gpio_reg(&vg->chip, base + pin,
-						   BYT_CONF0_REG);
-				mask = readl(reg);
-				mask &= ~(BYT_TRIG_NEG | BYT_TRIG_POS |
-					  BYT_TRIG_LVL);
-				writel(mask, reg);
-				mask = readl(reg); /* flush */
-				break;
-			}
 		}
 	}
 	chip->irq_eoi(data);
 }
 
+static void byt_irq_ack(struct irq_data *d)
+{
+	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
+	struct byt_gpio *vg = to_byt_gpio(gc);
+	unsigned offset = irqd_to_hwirq(d);
+	void __iomem *reg;
+
+	reg = byt_gpio_reg(&vg->chip, offset, BYT_INT_STAT_REG);
+	writel(BIT(offset % 32), reg);
+}
+
 static void byt_irq_unmask(struct irq_data *d)
 {
+	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
+	struct byt_gpio *vg = to_byt_gpio(gc);
+	unsigned offset = irqd_to_hwirq(d);
+	unsigned long flags;
+	void __iomem *reg;
+	u32 value;
+
+	spin_lock_irqsave(&vg->lock, flags);
+
+	reg = byt_gpio_reg(&vg->chip, offset, BYT_CONF0_REG);
+	value = readl(reg);
+
+	switch (irqd_get_trigger_type(d)) {
+	case IRQ_TYPE_LEVEL_HIGH:
+		value |= BYT_TRIG_LVL;
+	case IRQ_TYPE_EDGE_RISING:
+		value |= BYT_TRIG_POS;
+		break;
+	case IRQ_TYPE_LEVEL_LOW:
+		value |= BYT_TRIG_LVL;
+	case IRQ_TYPE_EDGE_FALLING:
+		value |= BYT_TRIG_NEG;
+		break;
+	case IRQ_TYPE_EDGE_BOTH:
+		value |= (BYT_TRIG_NEG | BYT_TRIG_POS);
+		break;
+	}
+
+	writel(value, reg);
+
+	spin_unlock_irqrestore(&vg->lock, flags);
 }
 
 static void byt_irq_mask(struct irq_data *d)
 {
+	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
+	struct byt_gpio *vg = to_byt_gpio(gc);
+
+	byt_gpio_clear_triggering(vg, irqd_to_hwirq(d));
 }
 
 static struct irq_chip byt_irqchip = {
 	.name = "BYT-GPIO",
+	.irq_ack = byt_irq_ack,
 	.irq_mask = byt_irq_mask,
 	.irq_unmask = byt_irq_unmask,
 	.irq_set_type = byt_irq_type,
@@ -472,6 +510,21 @@ static void byt_gpio_irq_init_hw(struct byt_gpio *vg)
 {
 	void __iomem *reg;
 	u32 base, value;
+	int i;
+
+	/*
+	 * Clear interrupt triggers for all pins that are GPIOs and
+	 * do not use direct IRQ mode. This will prevent spurious
+	 * interrupts from misconfigured pins.
+	 */
+	for (i = 0; i < vg->chip.ngpio; i++) {
+		value = readl(byt_gpio_reg(&vg->chip, i, BYT_CONF0_REG));
+		if ((value & BYT_PIN_MUX) == byt_get_gpio_mux(vg, i) &&
+		    !(value & BYT_DIRECT_IRQ_EN)) {
+			byt_gpio_clear_triggering(vg, i);
+			dev_dbg(&vg->pdev->dev, "disabling GPIO %d\n", i);
+		}
+	}
 
 	/* clear interrupt status trigger registers */
 	for (base = 0; base < vg->chip.ngpio; base += 32) {
@@ -541,6 +594,11 @@ static int byt_gpio_probe(struct platform_device *pdev)
 	gc->can_sleep = false;
 	gc->dev = dev;
 
+#ifdef CONFIG_PM_SLEEP
+	vg->saved_context = devm_kcalloc(&pdev->dev, gc->ngpio,
+				       sizeof(*vg->saved_context), GFP_KERNEL);
+#endif
+
 	ret = gpiochip_add(gc);
 	if (ret) {
 		dev_err(&pdev->dev, "failed adding byt-gpio chip\n");
@@ -569,6 +627,69 @@ static int byt_gpio_probe(struct platform_device *pdev)
 	return 0;
 }
 
+#ifdef CONFIG_PM_SLEEP
+static int byt_gpio_suspend(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct byt_gpio *vg = platform_get_drvdata(pdev);
+	int i;
+
+	for (i = 0; i < vg->chip.ngpio; i++) {
+		void __iomem *reg;
+		u32 value;
+
+		reg = byt_gpio_reg(&vg->chip, i, BYT_CONF0_REG);
+		value = readl(reg) & BYT_CONF0_RESTORE_MASK;
+		vg->saved_context[i].conf0 = value;
+
+		reg = byt_gpio_reg(&vg->chip, i, BYT_VAL_REG);
+		value = readl(reg) & BYT_VAL_RESTORE_MASK;
+		vg->saved_context[i].val = value;
+	}
+
+	return 0;
+}
+
+static int byt_gpio_resume(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct byt_gpio *vg = platform_get_drvdata(pdev);
+	int i;
+
+	for (i = 0; i < vg->chip.ngpio; i++) {
+		void __iomem *reg;
+		u32 value;
+
+		reg = byt_gpio_reg(&vg->chip, i, BYT_CONF0_REG);
+		value = readl(reg);
+		if ((value & BYT_CONF0_RESTORE_MASK) !=
+		     vg->saved_context[i].conf0) {
+			value &= ~BYT_CONF0_RESTORE_MASK;
+			value |= vg->saved_context[i].conf0;
+			writel(value, reg);
+			dev_info(dev, "restored pin %d conf0 %#08x", i, value);
+		}
+
+		reg = byt_gpio_reg(&vg->chip, i, BYT_VAL_REG);
+		value = readl(reg);
+		if ((value & BYT_VAL_RESTORE_MASK) !=
+		     vg->saved_context[i].val) {
+			u32 v;
+
+			v = value & ~BYT_VAL_RESTORE_MASK;
+			v |= vg->saved_context[i].val;
+			if (v != value) {
+				writel(v, reg);
+				dev_dbg(dev, "restored pin %d val %#08x\n",
+					i, v);
+			}
+		}
+	}
+
+	return 0;
+}
+#endif
+
 static int byt_gpio_runtime_suspend(struct device *dev)
 {
 	return 0;
@@ -580,8 +701,9 @@ static int byt_gpio_runtime_resume(struct device *dev)
 }
 
 static const struct dev_pm_ops byt_gpio_pm_ops = {
-	.runtime_suspend = byt_gpio_runtime_suspend,
-	.runtime_resume = byt_gpio_runtime_resume,
+	SET_LATE_SYSTEM_SLEEP_PM_OPS(byt_gpio_suspend, byt_gpio_resume)
+	SET_RUNTIME_PM_OPS(byt_gpio_runtime_suspend, byt_gpio_runtime_resume,
+			   NULL)
 };
 
 static const struct acpi_device_id byt_gpio_acpi_match[] = {
diff --git a/drivers/pinctrl/intel/pinctrl-cherryview.c b/drivers/pinctrl/intel/pinctrl-cherryview.c
index 3034fd03bced..82f691eeeec4 100644
--- a/drivers/pinctrl/intel/pinctrl-cherryview.c
+++ b/drivers/pinctrl/intel/pinctrl-cherryview.c
@@ -1226,6 +1226,7 @@ static int chv_gpio_direction_input(struct gpio_chip *chip, unsigned offset)
 static int chv_gpio_direction_output(struct gpio_chip *chip, unsigned offset,
 				     int value)
 {
+	chv_gpio_set(chip, offset, value);
 	return pinctrl_gpio_direction_output(chip->base + offset);
 }
 
diff --git a/drivers/pinctrl/pinctrl-at91.c b/drivers/pinctrl/pinctrl-at91.c
index f4cd0b9b2438..a4814066ea08 100644
--- a/drivers/pinctrl/pinctrl-at91.c
+++ b/drivers/pinctrl/pinctrl-at91.c
@@ -1477,28 +1477,25 @@ static void gpio_irq_ack(struct irq_data *d)
 	/* the interrupt is already cleared before by reading ISR */
 }
 
-static unsigned int gpio_irq_startup(struct irq_data *d)
+static int gpio_irq_request_res(struct irq_data *d)
 {
 	struct at91_gpio_chip *at91_gpio = irq_data_get_irq_chip_data(d);
 	unsigned	pin = d->hwirq;
 	int ret;
 
 	ret = gpiochip_lock_as_irq(&at91_gpio->chip, pin);
-	if (ret) {
+	if (ret)
 		dev_err(at91_gpio->chip.dev, "unable to lock pind %lu IRQ\n",
 			d->hwirq);
-		return ret;
-	}
-	gpio_irq_unmask(d);
-	return 0;
+
+	return ret;
 }
 
-static void gpio_irq_shutdown(struct irq_data *d)
+static void gpio_irq_release_res(struct irq_data *d)
 {
 	struct at91_gpio_chip *at91_gpio = irq_data_get_irq_chip_data(d);
 	unsigned	pin = d->hwirq;
 
-	gpio_irq_mask(d);
 	gpiochip_unlock_as_irq(&at91_gpio->chip, pin);
 }
 
@@ -1577,8 +1574,8 @@ void at91_pinctrl_gpio_resume(void)
 static struct irq_chip gpio_irqchip = {
 	.name		= "GPIO",
 	.irq_ack	= gpio_irq_ack,
-	.irq_startup	= gpio_irq_startup,
-	.irq_shutdown	= gpio_irq_shutdown,
+	.irq_request_resources = gpio_irq_request_res,
+	.irq_release_resources = gpio_irq_release_res,
 	.irq_disable	= gpio_irq_mask,
 	.irq_mask	= gpio_irq_mask,
 	.irq_unmask	= gpio_irq_unmask,
diff --git a/drivers/pinctrl/sunxi/pinctrl-sun4i-a10.c b/drivers/pinctrl/sunxi/pinctrl-sun4i-a10.c
index 24c5d88f943f..3c68a8e5e0dd 100644
--- a/drivers/pinctrl/sunxi/pinctrl-sun4i-a10.c
+++ b/drivers/pinctrl/sunxi/pinctrl-sun4i-a10.c
@@ -1011,6 +1011,7 @@ static const struct sunxi_pinctrl_desc sun4i_a10_pinctrl_data = {
 	.pins = sun4i_a10_pins,
 	.npins = ARRAY_SIZE(sun4i_a10_pins),
 	.irq_banks = 1,
+	.irq_read_needs_mux = true,
 };
 
 static int sun4i_a10_pinctrl_probe(struct platform_device *pdev)
diff --git a/drivers/pinctrl/sunxi/pinctrl-sunxi.c b/drivers/pinctrl/sunxi/pinctrl-sunxi.c
index 3d0744337736..f8e171b76693 100644
--- a/drivers/pinctrl/sunxi/pinctrl-sunxi.c
+++ b/drivers/pinctrl/sunxi/pinctrl-sunxi.c
@@ -29,6 +29,7 @@
 #include <linux/slab.h>
 
 #include "../core.h"
+#include "../../gpio/gpiolib.h"
 #include "pinctrl-sunxi.h"
 
 static struct irq_chip sunxi_pinctrl_edge_irq_chip;
@@ -464,10 +465,19 @@ static int sunxi_pinctrl_gpio_direction_input(struct gpio_chip *chip,
 static int sunxi_pinctrl_gpio_get(struct gpio_chip *chip, unsigned offset)
 {
 	struct sunxi_pinctrl *pctl = dev_get_drvdata(chip->dev);
-
 	u32 reg = sunxi_data_reg(offset);
 	u8 index = sunxi_data_offset(offset);
-	u32 val = (readl(pctl->membase + reg) >> index) & DATA_PINS_MASK;
+	u32 set_mux = pctl->desc->irq_read_needs_mux &&
+			test_bit(FLAG_USED_AS_IRQ, &chip->desc[offset].flags);
+	u32 val;
+
+	if (set_mux)
+		sunxi_pmx_set(pctl->pctl_dev, offset, SUN4I_FUNC_INPUT);
+
+	val = (readl(pctl->membase + reg) >> index) & DATA_PINS_MASK;
+
+	if (set_mux)
+		sunxi_pmx_set(pctl->pctl_dev, offset, SUN4I_FUNC_IRQ);
 
 	return val;
 }
diff --git a/drivers/pinctrl/sunxi/pinctrl-sunxi.h b/drivers/pinctrl/sunxi/pinctrl-sunxi.h
index 5a51523a3459..e248e81a0f9e 100644
--- a/drivers/pinctrl/sunxi/pinctrl-sunxi.h
+++ b/drivers/pinctrl/sunxi/pinctrl-sunxi.h
@@ -77,6 +77,9 @@
 #define IRQ_LEVEL_LOW		0x03
 #define IRQ_EDGE_BOTH		0x04
 
+#define SUN4I_FUNC_INPUT	0
+#define SUN4I_FUNC_IRQ		6
+
 struct sunxi_desc_function {
 	const char	*name;
 	u8		muxval;
@@ -94,6 +97,7 @@ struct sunxi_pinctrl_desc {
 	int				npins;
 	unsigned			pin_base;
 	unsigned			irq_banks;
+	bool				irq_read_needs_mux;
 };
 
 struct sunxi_pinctrl_function {
diff --git a/drivers/platform/x86/compal-laptop.c b/drivers/platform/x86/compal-laptop.c
index 15c0fab2bfa1..b4e94471f3d5 100644
--- a/drivers/platform/x86/compal-laptop.c
+++ b/drivers/platform/x86/compal-laptop.c
@@ -177,7 +177,7 @@ struct compal_data{
 	unsigned char curr_pwm;
 
 	/* Power supply */
-	struct power_supply psy;
+	struct power_supply *psy;
 	struct power_supply_info psy_info;
 	char bat_model_name[BAT_MODEL_NAME_LEN + 1];
 	char bat_manufacturer_name[BAT_MANUFACTURER_NAME_LEN + 1];
@@ -565,8 +565,7 @@ static int bat_get_property(struct power_supply *psy,
 				enum power_supply_property psp,
 				union power_supply_propval *val)
 {
-	struct compal_data *data;
-	data = container_of(psy, struct compal_data, psy);
+	struct compal_data *data = power_supply_get_drvdata(psy);
 
 	switch (psp) {
 	case POWER_SUPPLY_PROP_STATUS:
@@ -875,13 +874,16 @@ static struct dmi_system_id __initdata compal_dmi_table[] = {
 };
 MODULE_DEVICE_TABLE(dmi, compal_dmi_table);
 
+static const struct power_supply_desc psy_bat_desc = {
+	.name		= DRIVER_NAME,
+	.type		= POWER_SUPPLY_TYPE_BATTERY,
+	.properties	= compal_bat_properties,
+	.num_properties	= ARRAY_SIZE(compal_bat_properties),
+	.get_property	= bat_get_property,
+};
+
 static void initialize_power_supply_data(struct compal_data *data)
 {
-	data->psy.name = DRIVER_NAME;
-	data->psy.type = POWER_SUPPLY_TYPE_BATTERY;
-	data->psy.properties = compal_bat_properties;
-	data->psy.num_properties = ARRAY_SIZE(compal_bat_properties);
-	data->psy.get_property = bat_get_property;
 
 	ec_read_sequence(BAT_MANUFACTURER_NAME_ADDR,
 					data->bat_manufacturer_name,
@@ -1011,6 +1013,7 @@ static int compal_probe(struct platform_device *pdev)
 	int err;
 	struct compal_data *data;
 	struct device *hwmon_dev;
+	struct power_supply_config psy_cfg = {};
 
 	if (!extra_features)
 		return 0;
@@ -1026,9 +1029,9 @@ static int compal_probe(struct platform_device *pdev)
 	if (err)
 		return err;
 
-	hwmon_dev = hwmon_device_register_with_groups(&pdev->dev,
-						      "compal", data,
-						      compal_hwmon_groups);
+	hwmon_dev = devm_hwmon_device_register_with_groups(&pdev->dev,
+							   "compal", data,
+							   compal_hwmon_groups);
 	if (IS_ERR(hwmon_dev)) {
 		err = PTR_ERR(hwmon_dev);
 		goto remove;
@@ -1036,7 +1039,13 @@ static int compal_probe(struct platform_device *pdev)
 
 	/* Power supply */
 	initialize_power_supply_data(data);
-	power_supply_register(&compal_device->dev, &data->psy);
+	psy_cfg.drv_data = data;
+	data->psy = power_supply_register(&compal_device->dev, &psy_bat_desc,
+					  &psy_cfg);
+	if (IS_ERR(data->psy)) {
+		err = PTR_ERR(data->psy);
+		goto remove;
+	}
 
 	platform_set_drvdata(pdev, data);
 
@@ -1071,7 +1080,7 @@ static int compal_remove(struct platform_device *pdev)
 	pwm_disable_control();
 
 	data = platform_get_drvdata(pdev);
-	power_supply_unregister(&data->psy);
+	power_supply_unregister(data->psy);
 
 	sysfs_remove_group(&pdev->dev.kobj, &compal_platform_attr_group);
 
diff --git a/drivers/pnp/quirks.c b/drivers/pnp/quirks.c
index ebf0d6710b5a..943c1cb9566c 100644
--- a/drivers/pnp/quirks.c
+++ b/drivers/pnp/quirks.c
@@ -246,13 +246,16 @@ static void quirk_system_pci_resources(struct pnp_dev *dev)
 	 */
 	for_each_pci_dev(pdev) {
 		for (i = 0; i < DEVICE_COUNT_RESOURCE; i++) {
-			unsigned long type;
+			unsigned long flags, type;
 
-			type = pci_resource_flags(pdev, i) &
-					(IORESOURCE_IO | IORESOURCE_MEM);
+			flags = pci_resource_flags(pdev, i);
+			type = flags & (IORESOURCE_IO | IORESOURCE_MEM);
 			if (!type || pci_resource_len(pdev, i) == 0)
 				continue;
 
+			if (flags & IORESOURCE_UNSET)
+				continue;
+
 			pci_start = pci_resource_start(pdev, i);
 			pci_end = pci_resource_end(pdev, i);
 			for (j = 0;
diff --git a/drivers/power/88pm860x_battery.c b/drivers/power/88pm860x_battery.c
index bd3c997f4fee..d49579b227ec 100644
--- a/drivers/power/88pm860x_battery.c
+++ b/drivers/power/88pm860x_battery.c
@@ -98,7 +98,7 @@ struct pm860x_battery_info {
 	struct i2c_client *i2c;
 	struct device *dev;
 
-	struct power_supply battery;
+	struct power_supply *battery;
 	struct mutex lock;
 	int status;
 	int irq_cc;
@@ -798,9 +798,8 @@ out:
 
 static void pm860x_external_power_changed(struct power_supply *psy)
 {
-	struct pm860x_battery_info *info;
+	struct pm860x_battery_info *info = dev_get_drvdata(psy->dev.parent);
 
-	info = container_of(psy, struct pm860x_battery_info, battery);
 	calc_resistor(info);
 }
 
@@ -808,7 +807,7 @@ static int pm860x_batt_get_prop(struct power_supply *psy,
 				enum power_supply_property psp,
 				union power_supply_propval *val)
 {
-	struct pm860x_battery_info *info = dev_get_drvdata(psy->dev->parent);
+	struct pm860x_battery_info *info = dev_get_drvdata(psy->dev.parent);
 	int data;
 	int ret;
 
@@ -874,7 +873,7 @@ static int pm860x_batt_set_prop(struct power_supply *psy,
 				       enum power_supply_property psp,
 				       const union power_supply_propval *val)
 {
-	struct pm860x_battery_info *info = dev_get_drvdata(psy->dev->parent);
+	struct pm860x_battery_info *info = dev_get_drvdata(psy->dev.parent);
 
 	switch (psp) {
 	case POWER_SUPPLY_PROP_CHARGE_FULL:
@@ -901,6 +900,16 @@ static enum power_supply_property pm860x_batt_props[] = {
 	POWER_SUPPLY_PROP_TEMP,
 };
 
+static const struct power_supply_desc pm860x_battery_desc = {
+	.name			= "battery-monitor",
+	.type			= POWER_SUPPLY_TYPE_BATTERY,
+	.properties		= pm860x_batt_props,
+	.num_properties		= ARRAY_SIZE(pm860x_batt_props),
+	.get_property		= pm860x_batt_get_prop,
+	.set_property		= pm860x_batt_set_prop,
+	.external_power_changed	= pm860x_external_power_changed,
+};
+
 static int pm860x_battery_probe(struct platform_device *pdev)
 {
 	struct pm860x_chip *chip = dev_get_drvdata(pdev->dev.parent);
@@ -936,14 +945,6 @@ static int pm860x_battery_probe(struct platform_device *pdev)
 
 	pm860x_init_battery(info);
 
-	info->battery.name = "battery-monitor";
-	info->battery.type = POWER_SUPPLY_TYPE_BATTERY;
-	info->battery.properties = pm860x_batt_props;
-	info->battery.num_properties = ARRAY_SIZE(pm860x_batt_props);
-	info->battery.get_property = pm860x_batt_get_prop;
-	info->battery.set_property = pm860x_batt_set_prop;
-	info->battery.external_power_changed = pm860x_external_power_changed;
-
 	if (pdata && pdata->max_capacity)
 		info->max_capacity = pdata->max_capacity;
 	else
@@ -953,10 +954,11 @@ static int pm860x_battery_probe(struct platform_device *pdev)
 	else
 		info->resistor = 300;	/* set default internal resistor */
 
-	ret = power_supply_register(&pdev->dev, &info->battery);
-	if (ret)
-		return ret;
-	info->battery.dev->parent = &pdev->dev;
+	info->battery = power_supply_register(&pdev->dev, &pm860x_battery_desc,
+					      NULL);
+	if (IS_ERR(info->battery))
+		return PTR_ERR(info->battery);
+	info->battery->dev.parent = &pdev->dev;
 
 	ret = request_threaded_irq(info->irq_cc, NULL,
 				pm860x_coulomb_handler, IRQF_ONESHOT,
@@ -981,7 +983,7 @@ static int pm860x_battery_probe(struct platform_device *pdev)
 out_coulomb:
 	free_irq(info->irq_cc, info);
 out_reg:
-	power_supply_unregister(&info->battery);
+	power_supply_unregister(info->battery);
 	return ret;
 }
 
@@ -991,7 +993,7 @@ static int pm860x_battery_remove(struct platform_device *pdev)
 
 	free_irq(info->irq_batt, info);
 	free_irq(info->irq_cc, info);
-	power_supply_unregister(&info->battery);
+	power_supply_unregister(info->battery);
 	return 0;
 }
 
diff --git a/drivers/power/88pm860x_charger.c b/drivers/power/88pm860x_charger.c
index 734ec4afa14d..0e448c68c02b 100644
--- a/drivers/power/88pm860x_charger.c
+++ b/drivers/power/88pm860x_charger.c
@@ -102,7 +102,7 @@ struct pm860x_charger_info {
 	struct i2c_client *i2c_8606;
 	struct device *dev;
 
-	struct power_supply usb;
+	struct power_supply *usb;
 	struct mutex lock;
 	int irq_nums;
 	int irq[7];
@@ -296,14 +296,20 @@ static int set_charging_fsm(struct pm860x_charger_info *info)
 	psy = power_supply_get_by_name(pm860x_supplied_to[0]);
 	if (!psy)
 		return -EINVAL;
-	ret = psy->get_property(psy, POWER_SUPPLY_PROP_VOLTAGE_NOW, &data);
-	if (ret)
+	ret = power_supply_get_property(psy, POWER_SUPPLY_PROP_VOLTAGE_NOW,
+			&data);
+	if (ret) {
+		power_supply_put(psy);
 		return ret;
+	}
 	vbatt = data.intval / 1000;
 
-	ret = psy->get_property(psy, POWER_SUPPLY_PROP_PRESENT, &data);
-	if (ret)
+	ret = power_supply_get_property(psy, POWER_SUPPLY_PROP_PRESENT, &data);
+	if (ret) {
+		power_supply_put(psy);
 		return ret;
+	}
+	power_supply_put(psy);
 
 	mutex_lock(&info->lock);
 	info->present = data.intval;
@@ -414,7 +420,7 @@ static irqreturn_t pm860x_charger_handler(int irq, void *data)
 
 	set_charging_fsm(info);
 
-	power_supply_changed(&info->usb);
+	power_supply_changed(info->usb);
 out:
 	return IRQ_HANDLED;
 }
@@ -430,7 +436,7 @@ static irqreturn_t pm860x_temp_handler(int irq, void *data)
 	psy = power_supply_get_by_name(pm860x_supplied_to[0]);
 	if (!psy)
 		goto out;
-	ret = psy->get_property(psy, POWER_SUPPLY_PROP_TEMP, &temp);
+	ret = power_supply_get_property(psy, POWER_SUPPLY_PROP_TEMP, &temp);
 	if (ret)
 		goto out;
 	value = temp.intval / 10;
@@ -446,6 +452,7 @@ static irqreturn_t pm860x_temp_handler(int irq, void *data)
 
 	set_charging_fsm(info);
 out:
+	power_supply_put(psy);
 	return IRQ_HANDLED;
 }
 
@@ -485,9 +492,10 @@ static irqreturn_t pm860x_done_handler(int irq, void *data)
 	psy = power_supply_get_by_name(pm860x_supplied_to[0]);
 	if (!psy)
 		goto out;
-	ret = psy->get_property(psy, POWER_SUPPLY_PROP_VOLTAGE_NOW, &val);
+	ret = power_supply_get_property(psy, POWER_SUPPLY_PROP_VOLTAGE_NOW,
+			&val);
 	if (ret)
-		goto out;
+		goto out_psy_put;
 	vbatt = val.intval / 1000;
 	/*
 	 * CHG_DONE interrupt is faster than CHG_DET interrupt when
@@ -498,10 +506,13 @@ static irqreturn_t pm860x_done_handler(int irq, void *data)
 	 */
 	ret = pm860x_reg_read(info->i2c, PM8607_STATUS_2);
 	if (ret < 0)
-		goto out;
+		goto out_psy_put;
 	if (vbatt > CHARGE_THRESHOLD && ret & STATUS2_CHG)
-		psy->set_property(psy, POWER_SUPPLY_PROP_CHARGE_FULL, &val);
+		power_supply_set_property(psy, POWER_SUPPLY_PROP_CHARGE_FULL,
+				&val);
 
+out_psy_put:
+	power_supply_put(psy);
 out:
 	mutex_unlock(&info->lock);
 	dev_dbg(info->dev, "%s, Allowed: %d\n", __func__, info->allowed);
@@ -584,8 +595,7 @@ static int pm860x_usb_get_prop(struct power_supply *psy,
 			       enum power_supply_property psp,
 			       union power_supply_propval *val)
 {
-	struct pm860x_charger_info *info =
-	    dev_get_drvdata(psy->dev->parent);
+	struct pm860x_charger_info *info = power_supply_get_drvdata(psy);
 
 	switch (psp) {
 	case POWER_SUPPLY_PROP_STATUS:
@@ -645,9 +655,18 @@ static struct pm860x_irq_desc {
 	{ "vchg", pm860x_vchg_handler },
 };
 
+static const struct power_supply_desc pm860x_charger_desc = {
+	.name		= "usb",
+	.type		= POWER_SUPPLY_TYPE_USB,
+	.properties	= pm860x_usb_props,
+	.num_properties	= ARRAY_SIZE(pm860x_usb_props),
+	.get_property	= pm860x_usb_get_prop,
+};
+
 static int pm860x_charger_probe(struct platform_device *pdev)
 {
 	struct pm860x_chip *chip = dev_get_drvdata(pdev->dev.parent);
+	struct power_supply_config psy_cfg = {};
 	struct pm860x_charger_info *info;
 	int ret;
 	int count;
@@ -685,16 +704,15 @@ static int pm860x_charger_probe(struct platform_device *pdev)
 	mutex_init(&info->lock);
 	platform_set_drvdata(pdev, info);
 
-	info->usb.name = "usb";
-	info->usb.type = POWER_SUPPLY_TYPE_USB;
-	info->usb.supplied_to = pm860x_supplied_to;
-	info->usb.num_supplicants = ARRAY_SIZE(pm860x_supplied_to);
-	info->usb.properties = pm860x_usb_props;
-	info->usb.num_properties = ARRAY_SIZE(pm860x_usb_props);
-	info->usb.get_property = pm860x_usb_get_prop;
-	ret = power_supply_register(&pdev->dev, &info->usb);
-	if (ret)
+	psy_cfg.drv_data = info;
+	psy_cfg.supplied_to = pm860x_supplied_to;
+	psy_cfg.num_supplicants = ARRAY_SIZE(pm860x_supplied_to);
+	info->usb = power_supply_register(&pdev->dev, &pm860x_charger_desc,
+					  &psy_cfg);
+	if (IS_ERR(info->usb)) {
+		ret = PTR_ERR(info->usb);
 		goto out;
+	}
 
 	pm860x_init_charger(info);
 
@@ -711,7 +729,7 @@ static int pm860x_charger_probe(struct platform_device *pdev)
 	return 0;
 
 out_irq:
-	power_supply_unregister(&info->usb);
+	power_supply_unregister(info->usb);
 	while (--i >= 0)
 		free_irq(info->irq[i], info);
 out:
@@ -723,7 +741,7 @@ static int pm860x_charger_remove(struct platform_device *pdev)
 	struct pm860x_charger_info *info = platform_get_drvdata(pdev);
 	int i;
 
-	power_supply_unregister(&info->usb);
+	power_supply_unregister(info->usb);
 	free_irq(info->irq[0], info);
 	for (i = 0; i < info->irq_nums; i++)
 		free_irq(info->irq[i], info);
diff --git a/drivers/power/Kconfig b/drivers/power/Kconfig
index 27b751b995fb..4091fb092d06 100644
--- a/drivers/power/Kconfig
+++ b/drivers/power/Kconfig
@@ -192,6 +192,27 @@ config BATTERY_DA9052
 	  Say Y here to enable support for batteries charger integrated into
 	  DA9052 PMIC.
 
+config CHARGER_DA9150
+	tristate "Dialog Semiconductor DA9150 Charger support"
+	depends on MFD_DA9150
+	depends on DA9150_GPADC
+	depends on IIO
+	help
+	  Say Y here to enable support for charger unit of the DA9150
+	  Integrated Charger & Fuel-Gauge IC.
+
+	  This driver can also be built as a module. If so, the module will be
+	  called da9150-charger.
+
+config AXP288_FUEL_GAUGE
+	tristate "X-Powers AXP288 Fuel Gauge"
+	depends on MFD_AXP20X && IIO
+	help
+	  Say yes here to have support for X-Power power management IC (PMIC)
+	  Fuel Gauge. The device provides battery statistics and status
+	  monitoring as well as alerts for battery over/under voltage and
+	  over/under temperature.
+
 config BATTERY_MAX17040
 	tristate "Maxim MAX17040 Fuel Gauge"
 	depends on I2C
diff --git a/drivers/power/Makefile b/drivers/power/Makefile
index 36f9e0d10111..b7b0181c95e5 100644
--- a/drivers/power/Makefile
+++ b/drivers/power/Makefile
@@ -1,4 +1,4 @@
-ccflags-$(CONFIG_POWER_SUPPLY_DEBUG) := -DDEBUG
+subdir-ccflags-$(CONFIG_POWER_SUPPLY_DEBUG) := -DDEBUG
 
 power_supply-y				:= power_supply_core.o
 power_supply-$(CONFIG_SYSFS)		+= power_supply_sysfs.o
@@ -32,6 +32,7 @@ obj-$(CONFIG_BATTERY_SBS)	+= sbs-battery.o
 obj-$(CONFIG_BATTERY_BQ27x00)	+= bq27x00_battery.o
 obj-$(CONFIG_BATTERY_DA9030)	+= da9030_battery.o
 obj-$(CONFIG_BATTERY_DA9052)	+= da9052-battery.o
+obj-$(CONFIG_CHARGER_DA9150)	+= da9150-charger.o
 obj-$(CONFIG_BATTERY_MAX17040)	+= max17040_battery.o
 obj-$(CONFIG_BATTERY_MAX17042)	+= max17042_battery.o
 obj-$(CONFIG_BATTERY_Z2)	+= z2_battery.o
@@ -62,3 +63,4 @@ obj-$(CONFIG_POWER_AVS)		+= avs/
 obj-$(CONFIG_CHARGER_SMB347)	+= smb347-charger.o
 obj-$(CONFIG_CHARGER_TPS65090)	+= tps65090-charger.o
 obj-$(CONFIG_POWER_RESET)	+= reset/
+obj-$(CONFIG_AXP288_FUEL_GAUGE) += axp288_fuel_gauge.o
diff --git a/drivers/power/ab8500_btemp.c b/drivers/power/ab8500_btemp.c
index 4ebf7b0819f7..8f8044e1acf3 100644
--- a/drivers/power/ab8500_btemp.c
+++ b/drivers/power/ab8500_btemp.c
@@ -45,9 +45,6 @@
 #define BTEMP_BATCTRL_CURR_SRC_60UA	60
 #define BTEMP_BATCTRL_CURR_SRC_120UA	120
 
-#define to_ab8500_btemp_device_info(x) container_of((x), \
-	struct ab8500_btemp, btemp_psy);
-
 /**
  * struct ab8500_btemp_interrupts - ab8500 interrupts
  * @name:	name of the interrupt
@@ -102,7 +99,7 @@ struct ab8500_btemp {
 	struct ab8500_gpadc *gpadc;
 	struct ab8500_fg *fg;
 	struct abx500_bm_data *bm;
-	struct power_supply btemp_psy;
+	struct power_supply *btemp_psy;
 	struct ab8500_btemp_events events;
 	struct ab8500_btemp_ranges btemp_ranges;
 	struct workqueue_struct *btemp_wq;
@@ -654,14 +651,14 @@ static void ab8500_btemp_periodic_work(struct work_struct *work)
 		if ((di->bat_temp != di->prev_bat_temp) || !di->initialized) {
 			di->initialized = true;
 			di->bat_temp = bat_temp;
-			power_supply_changed(&di->btemp_psy);
+			power_supply_changed(di->btemp_psy);
 		}
 	} else if (bat_temp < di->prev_bat_temp) {
 		di->bat_temp--;
-		power_supply_changed(&di->btemp_psy);
+		power_supply_changed(di->btemp_psy);
 	} else if (bat_temp > di->prev_bat_temp) {
 		di->bat_temp++;
-		power_supply_changed(&di->btemp_psy);
+		power_supply_changed(di->btemp_psy);
 	}
 	di->prev_bat_temp = bat_temp;
 
@@ -689,7 +686,7 @@ static irqreturn_t ab8500_btemp_batctrlindb_handler(int irq, void *_di)
 	dev_err(di->dev, "Battery removal detected!\n");
 
 	di->events.batt_rem = true;
-	power_supply_changed(&di->btemp_psy);
+	power_supply_changed(di->btemp_psy);
 
 	return IRQ_HANDLED;
 }
@@ -715,7 +712,7 @@ static irqreturn_t ab8500_btemp_templow_handler(int irq, void *_di)
 		di->events.btemp_high = false;
 		di->events.btemp_medhigh = false;
 		di->events.btemp_lowmed = false;
-		power_supply_changed(&di->btemp_psy);
+		power_supply_changed(di->btemp_psy);
 	}
 
 	return IRQ_HANDLED;
@@ -738,7 +735,7 @@ static irqreturn_t ab8500_btemp_temphigh_handler(int irq, void *_di)
 	di->events.btemp_medhigh = false;
 	di->events.btemp_lowmed = false;
 	di->events.btemp_low = false;
-	power_supply_changed(&di->btemp_psy);
+	power_supply_changed(di->btemp_psy);
 
 	return IRQ_HANDLED;
 }
@@ -760,7 +757,7 @@ static irqreturn_t ab8500_btemp_lowmed_handler(int irq, void *_di)
 	di->events.btemp_medhigh = false;
 	di->events.btemp_high = false;
 	di->events.btemp_low = false;
-	power_supply_changed(&di->btemp_psy);
+	power_supply_changed(di->btemp_psy);
 
 	return IRQ_HANDLED;
 }
@@ -782,7 +779,7 @@ static irqreturn_t ab8500_btemp_medhigh_handler(int irq, void *_di)
 	di->events.btemp_lowmed = false;
 	di->events.btemp_high = false;
 	di->events.btemp_low = false;
-	power_supply_changed(&di->btemp_psy);
+	power_supply_changed(di->btemp_psy);
 
 	return IRQ_HANDLED;
 }
@@ -884,9 +881,7 @@ static int ab8500_btemp_get_property(struct power_supply *psy,
 	enum power_supply_property psp,
 	union power_supply_propval *val)
 {
-	struct ab8500_btemp *di;
-
-	di = to_ab8500_btemp_device_info(psy);
+	struct ab8500_btemp *di = power_supply_get_drvdata(psy);
 
 	switch (psp) {
 	case POWER_SUPPLY_PROP_PRESENT:
@@ -919,14 +914,14 @@ static int ab8500_btemp_get_ext_psy_data(struct device *dev, void *data)
 
 	psy = (struct power_supply *)data;
 	ext = dev_get_drvdata(dev);
-	di = to_ab8500_btemp_device_info(psy);
+	di = power_supply_get_drvdata(psy);
 
 	/*
 	 * For all psy where the name of your driver
 	 * appears in any supplied_to
 	 */
 	for (i = 0; i < ext->num_supplicants; i++) {
-		if (!strcmp(ext->supplied_to[i], psy->name))
+		if (!strcmp(ext->supplied_to[i], psy->desc->name))
 			psy_found = true;
 	}
 
@@ -934,16 +929,16 @@ static int ab8500_btemp_get_ext_psy_data(struct device *dev, void *data)
 		return 0;
 
 	/* Go through all properties for the psy */
-	for (j = 0; j < ext->num_properties; j++) {
+	for (j = 0; j < ext->desc->num_properties; j++) {
 		enum power_supply_property prop;
-		prop = ext->properties[j];
+		prop = ext->desc->properties[j];
 
-		if (ext->get_property(ext, prop, &ret))
+		if (power_supply_get_property(ext, prop, &ret))
 			continue;
 
 		switch (prop) {
 		case POWER_SUPPLY_PROP_PRESENT:
-			switch (ext->type) {
+			switch (ext->desc->type) {
 			case POWER_SUPPLY_TYPE_MAINS:
 				/* AC disconnected */
 				if (!ret.intval && di->events.ac_conn) {
@@ -990,10 +985,10 @@ static int ab8500_btemp_get_ext_psy_data(struct device *dev, void *data)
  */
 static void ab8500_btemp_external_power_changed(struct power_supply *psy)
 {
-	struct ab8500_btemp *di = to_ab8500_btemp_device_info(psy);
+	struct ab8500_btemp *di = power_supply_get_drvdata(psy);
 
 	class_for_each_device(power_supply_class, NULL,
-		&di->btemp_psy, ab8500_btemp_get_ext_psy_data);
+		di->btemp_psy, ab8500_btemp_get_ext_psy_data);
 }
 
 /* ab8500 btemp driver interrupts and their respective isr */
@@ -1044,7 +1039,7 @@ static int ab8500_btemp_remove(struct platform_device *pdev)
 	destroy_workqueue(di->btemp_wq);
 
 	flush_scheduled_work();
-	power_supply_unregister(&di->btemp_psy);
+	power_supply_unregister(di->btemp_psy);
 
 	return 0;
 }
@@ -1054,10 +1049,20 @@ static char *supply_interface[] = {
 	"ab8500_fg",
 };
 
+static const struct power_supply_desc ab8500_btemp_desc = {
+	.name			= "ab8500_btemp",
+	.type			= POWER_SUPPLY_TYPE_BATTERY,
+	.properties		= ab8500_btemp_props,
+	.num_properties		= ARRAY_SIZE(ab8500_btemp_props),
+	.get_property		= ab8500_btemp_get_property,
+	.external_power_changed	= ab8500_btemp_external_power_changed,
+};
+
 static int ab8500_btemp_probe(struct platform_device *pdev)
 {
 	struct device_node *np = pdev->dev.of_node;
 	struct abx500_bm_data *plat = pdev->dev.platform_data;
+	struct power_supply_config psy_cfg = {};
 	struct ab8500_btemp *di;
 	int irq, i, ret = 0;
 	u8 val;
@@ -1089,17 +1094,9 @@ static int ab8500_btemp_probe(struct platform_device *pdev)
 
 	di->initialized = false;
 
-	/* BTEMP supply */
-	di->btemp_psy.name = "ab8500_btemp";
-	di->btemp_psy.type = POWER_SUPPLY_TYPE_BATTERY;
-	di->btemp_psy.properties = ab8500_btemp_props;
-	di->btemp_psy.num_properties = ARRAY_SIZE(ab8500_btemp_props);
-	di->btemp_psy.get_property = ab8500_btemp_get_property;
-	di->btemp_psy.supplied_to = supply_interface;
-	di->btemp_psy.num_supplicants = ARRAY_SIZE(supply_interface);
-	di->btemp_psy.external_power_changed =
-		ab8500_btemp_external_power_changed;
-
+	psy_cfg.supplied_to = supply_interface;
+	psy_cfg.num_supplicants = ARRAY_SIZE(supply_interface);
+	psy_cfg.drv_data = di;
 
 	/* Create a work queue for the btemp */
 	di->btemp_wq =
@@ -1140,9 +1137,11 @@ static int ab8500_btemp_probe(struct platform_device *pdev)
 	}
 
 	/* Register BTEMP power supply class */
-	ret = power_supply_register(di->dev, &di->btemp_psy);
-	if (ret) {
+	di->btemp_psy = power_supply_register(di->dev, &ab8500_btemp_desc,
+					      &psy_cfg);
+	if (IS_ERR(di->btemp_psy)) {
 		dev_err(di->dev, "failed to register BTEMP psy\n");
+		ret = PTR_ERR(di->btemp_psy);
 		goto free_btemp_wq;
 	}
 
@@ -1171,7 +1170,7 @@ static int ab8500_btemp_probe(struct platform_device *pdev)
 	return ret;
 
 free_irq:
-	power_supply_unregister(&di->btemp_psy);
+	power_supply_unregister(di->btemp_psy);
 
 	/* We also have to free all successfully registered irqs */
 	for (i = i - 1; i >= 0; i--) {
diff --git a/drivers/power/ab8500_charger.c b/drivers/power/ab8500_charger.c
index 8c8d170ff0f8..e388171f4e58 100644
--- a/drivers/power/ab8500_charger.c
+++ b/drivers/power/ab8500_charger.c
@@ -435,7 +435,7 @@ static void ab8500_charger_set_usb_connected(struct ab8500_charger *di,
 		if (!connected)
 			di->flags.vbus_drop_end = false;
 
-		sysfs_notify(&di->usb_chg.psy.dev->kobj, NULL, "present");
+		sysfs_notify(&di->usb_chg.psy->dev.kobj, NULL, "present");
 
 		if (connected) {
 			mutex_lock(&di->charger_attached_mutex);
@@ -1516,7 +1516,7 @@ static int ab8500_charger_ac_en(struct ux500_charger *charger,
 
 		dev_dbg(di->dev, "%s Disabled AC charging\n", __func__);
 	}
-	ab8500_power_supply_changed(di, &di->ac_chg.psy);
+	ab8500_power_supply_changed(di, di->ac_chg.psy);
 
 	return ret;
 }
@@ -1672,7 +1672,7 @@ static int ab8500_charger_usb_en(struct ux500_charger *charger,
 		cancel_delayed_work(&di->check_vbat_work);
 
 	}
-	ab8500_power_supply_changed(di, &di->usb_chg.psy);
+	ab8500_power_supply_changed(di, di->usb_chg.psy);
 
 	return ret;
 }
@@ -1811,9 +1811,9 @@ static int ab8500_charger_watchdog_kick(struct ux500_charger *charger)
 	int ret;
 	struct ab8500_charger *di;
 
-	if (charger->psy.type == POWER_SUPPLY_TYPE_MAINS)
+	if (charger->psy->desc->type == POWER_SUPPLY_TYPE_MAINS)
 		di = to_ab8500_charger_ac_device_info(charger);
-	else if (charger->psy.type == POWER_SUPPLY_TYPE_USB)
+	else if (charger->psy->desc->type == POWER_SUPPLY_TYPE_USB)
 		di = to_ab8500_charger_usb_device_info(charger);
 	else
 		return -ENXIO;
@@ -1839,9 +1839,9 @@ static int ab8500_charger_update_charger_current(struct ux500_charger *charger,
 	int ret;
 	struct ab8500_charger *di;
 
-	if (charger->psy.type == POWER_SUPPLY_TYPE_MAINS)
+	if (charger->psy->desc->type == POWER_SUPPLY_TYPE_MAINS)
 		di = to_ab8500_charger_ac_device_info(charger);
-	else if (charger->psy.type == POWER_SUPPLY_TYPE_USB)
+	else if (charger->psy->desc->type == POWER_SUPPLY_TYPE_USB)
 		di = to_ab8500_charger_usb_device_info(charger);
 	else
 		return -ENXIO;
@@ -1879,7 +1879,7 @@ static int ab8540_charger_power_path_enable(struct ux500_charger *charger,
 	int ret;
 	struct ab8500_charger *di;
 
-	if (charger->psy.type == POWER_SUPPLY_TYPE_USB)
+	if (charger->psy->desc->type == POWER_SUPPLY_TYPE_USB)
 		di = to_ab8500_charger_usb_device_info(charger);
 	else
 		return -ENXIO;
@@ -1910,7 +1910,7 @@ static int ab8540_charger_usb_pre_chg_enable(struct ux500_charger *charger,
 	int ret;
 	struct ab8500_charger *di;
 
-	if (charger->psy.type == POWER_SUPPLY_TYPE_USB)
+	if (charger->psy->desc->type == POWER_SUPPLY_TYPE_USB)
 		di = to_ab8500_charger_usb_device_info(charger);
 	else
 		return -ENXIO;
@@ -1937,7 +1937,7 @@ static int ab8500_charger_get_ext_psy_data(struct device *dev, void *data)
 	struct ux500_charger *usb_chg;
 
 	usb_chg = (struct ux500_charger *)data;
-	psy = &usb_chg->psy;
+	psy = usb_chg->psy;
 
 	di = to_ab8500_charger_usb_device_info(usb_chg);
 
@@ -1945,7 +1945,7 @@ static int ab8500_charger_get_ext_psy_data(struct device *dev, void *data)
 
 	/* For all psy where the driver name appears in any supplied_to */
 	for (i = 0; i < ext->num_supplicants; i++) {
-		if (!strcmp(ext->supplied_to[i], psy->name))
+		if (!strcmp(ext->supplied_to[i], psy->desc->name))
 			psy_found = true;
 	}
 
@@ -1953,16 +1953,16 @@ static int ab8500_charger_get_ext_psy_data(struct device *dev, void *data)
 		return 0;
 
 	/* Go through all properties for the psy */
-	for (j = 0; j < ext->num_properties; j++) {
+	for (j = 0; j < ext->desc->num_properties; j++) {
 		enum power_supply_property prop;
-		prop = ext->properties[j];
+		prop = ext->desc->properties[j];
 
-		if (ext->get_property(ext, prop, &ret))
+		if (power_supply_get_property(ext, prop, &ret))
 			continue;
 
 		switch (prop) {
 		case POWER_SUPPLY_PROP_VOLTAGE_NOW:
-			switch (ext->type) {
+			switch (ext->desc->type) {
 			case POWER_SUPPLY_TYPE_BATTERY:
 				di->vbat = ret.intval / 1000;
 				break;
@@ -1993,7 +1993,7 @@ static void ab8500_charger_check_vbat_work(struct work_struct *work)
 		struct ab8500_charger, check_vbat_work.work);
 
 	class_for_each_device(power_supply_class, NULL,
-		&di->usb_chg.psy, ab8500_charger_get_ext_psy_data);
+		di->usb_chg.psy, ab8500_charger_get_ext_psy_data);
 
 	/* First run old_vbat is 0. */
 	if (di->old_vbat == 0)
@@ -2009,7 +2009,7 @@ static void ab8500_charger_check_vbat_work(struct work_struct *work)
 			di->vbat, di->old_vbat);
 		ab8500_charger_set_vbus_in_curr(di,
 					di->max_usb_in_curr.usb_type_max);
-		power_supply_changed(&di->usb_chg.psy);
+		power_supply_changed(di->usb_chg.psy);
 	}
 
 	di->old_vbat = di->vbat;
@@ -2049,7 +2049,7 @@ static void ab8500_charger_check_hw_failure_work(struct work_struct *work)
 		}
 		if (!(reg_value & MAIN_CH_NOK)) {
 			di->flags.mainextchnotok = false;
-			ab8500_power_supply_changed(di, &di->ac_chg.psy);
+			ab8500_power_supply_changed(di, di->ac_chg.psy);
 		}
 	}
 	if (di->flags.vbus_ovv) {
@@ -2062,7 +2062,7 @@ static void ab8500_charger_check_hw_failure_work(struct work_struct *work)
 		}
 		if (!(reg_value & VBUS_OVV_TH)) {
 			di->flags.vbus_ovv = false;
-			ab8500_power_supply_changed(di, &di->usb_chg.psy);
+			ab8500_power_supply_changed(di, di->usb_chg.psy);
 		}
 	}
 	/* If we still have a failure, schedule a new check */
@@ -2132,8 +2132,8 @@ static void ab8500_charger_ac_work(struct work_struct *work)
 		di->ac.charger_connected = 0;
 	}
 
-	ab8500_power_supply_changed(di, &di->ac_chg.psy);
-	sysfs_notify(&di->ac_chg.psy.dev->kobj, NULL, "present");
+	ab8500_power_supply_changed(di, di->ac_chg.psy);
+	sysfs_notify(&di->ac_chg.psy->dev.kobj, NULL, "present");
 }
 
 static void ab8500_charger_usb_attached_work(struct work_struct *work)
@@ -2240,7 +2240,7 @@ static void ab8500_charger_detect_usb_type_work(struct work_struct *work)
 		dev_dbg(di->dev, "%s di->vbus_detected = false\n", __func__);
 		di->vbus_detected = false;
 		ab8500_charger_set_usb_connected(di, false);
-		ab8500_power_supply_changed(di, &di->usb_chg.psy);
+		ab8500_power_supply_changed(di, di->usb_chg.psy);
 	} else {
 		dev_dbg(di->dev, "%s di->vbus_detected = true\n", __func__);
 		di->vbus_detected = true;
@@ -2250,7 +2250,7 @@ static void ab8500_charger_detect_usb_type_work(struct work_struct *work)
 			if (!ret) {
 				ab8500_charger_set_usb_connected(di, true);
 				ab8500_power_supply_changed(di,
-							    &di->usb_chg.psy);
+							    di->usb_chg.psy);
 			}
 		} else {
 			/*
@@ -2267,7 +2267,7 @@ static void ab8500_charger_detect_usb_type_work(struct work_struct *work)
 					ab8500_charger_set_usb_connected(di,
 						true);
 					ab8500_power_supply_changed(di,
-						&di->usb_chg.psy);
+						di->usb_chg.psy);
 				}
 			}
 		}
@@ -2295,7 +2295,7 @@ static void ab8500_charger_usb_link_attach_work(struct work_struct *work)
 	}
 
 	ab8500_charger_set_usb_connected(di, true);
-	ab8500_power_supply_changed(di, &di->usb_chg.psy);
+	ab8500_power_supply_changed(di, di->usb_chg.psy);
 }
 
 /**
@@ -2393,7 +2393,7 @@ static void ab8500_charger_usb_link_status_work(struct work_struct *work)
 	if (!(detected_chargers & USB_PW_CONN)) {
 		di->vbus_detected = false;
 		ab8500_charger_set_usb_connected(di, false);
-		ab8500_power_supply_changed(di, &di->usb_chg.psy);
+		ab8500_power_supply_changed(di, di->usb_chg.psy);
 		return;
 	}
 
@@ -2404,7 +2404,7 @@ static void ab8500_charger_usb_link_status_work(struct work_struct *work)
 		if (ret == -ENXIO) {
 			/* No valid charger type detected */
 			ab8500_charger_set_usb_connected(di, false);
-			ab8500_power_supply_changed(di, &di->usb_chg.psy);
+			ab8500_power_supply_changed(di, di->usb_chg.psy);
 		}
 		return;
 	}
@@ -2463,7 +2463,7 @@ static void ab8500_charger_usb_state_changed_work(struct work_struct *work)
 	case AB8500_BM_USB_STATE_SUSPEND:
 	case AB8500_BM_USB_STATE_MAX:
 		ab8500_charger_set_usb_connected(di, false);
-		ab8500_power_supply_changed(di, &di->usb_chg.psy);
+		ab8500_power_supply_changed(di, di->usb_chg.psy);
 		break;
 
 	case AB8500_BM_USB_STATE_RESUME:
@@ -2486,7 +2486,7 @@ static void ab8500_charger_usb_state_changed_work(struct work_struct *work)
 				return;
 
 			ab8500_charger_set_usb_connected(di, true);
-			ab8500_power_supply_changed(di, &di->usb_chg.psy);
+			ab8500_power_supply_changed(di, di->usb_chg.psy);
 		}
 		break;
 
@@ -2530,7 +2530,7 @@ static void ab8500_charger_check_usbchargernotok_work(struct work_struct *work)
 	}
 
 	if (prev_status != di->flags.usbchargernotok)
-		ab8500_power_supply_changed(di, &di->usb_chg.psy);
+		ab8500_power_supply_changed(di, di->usb_chg.psy);
 }
 
 /**
@@ -2560,7 +2560,7 @@ static void ab8500_charger_check_main_thermal_prot_work(
 	else
 		di->flags.main_thermal_prot = false;
 
-	ab8500_power_supply_changed(di, &di->ac_chg.psy);
+	ab8500_power_supply_changed(di, di->ac_chg.psy);
 }
 
 /**
@@ -2590,7 +2590,7 @@ static void ab8500_charger_check_usb_thermal_prot_work(
 	else
 		di->flags.usb_thermal_prot = false;
 
-	ab8500_power_supply_changed(di, &di->usb_chg.psy);
+	ab8500_power_supply_changed(di, di->usb_chg.psy);
 }
 
 /**
@@ -2651,7 +2651,7 @@ static irqreturn_t ab8500_charger_mainextchnotok_handler(int irq, void *_di)
 
 	dev_dbg(di->dev, "Main charger not ok\n");
 	di->flags.mainextchnotok = true;
-	ab8500_power_supply_changed(di, &di->ac_chg.psy);
+	ab8500_power_supply_changed(di, di->ac_chg.psy);
 
 	/* Schedule a new HW failure check */
 	queue_delayed_work(di->charger_wq, &di->check_hw_failure_work, 0);
@@ -2880,11 +2880,11 @@ static irqreturn_t ab8500_charger_chwdexp_handler(int irq, void *_di)
 	 */
 	if (di->ac.charger_online) {
 		di->ac.wd_expired = true;
-		ab8500_power_supply_changed(di, &di->ac_chg.psy);
+		ab8500_power_supply_changed(di, di->ac_chg.psy);
 	}
 	if (di->usb.charger_online) {
 		di->usb.wd_expired = true;
-		ab8500_power_supply_changed(di, &di->usb_chg.psy);
+		ab8500_power_supply_changed(di, di->usb_chg.psy);
 	}
 
 	return IRQ_HANDLED;
@@ -2927,7 +2927,7 @@ static irqreturn_t ab8500_charger_vbusovv_handler(int irq, void *_di)
 
 	dev_dbg(di->dev, "VBUS overvoltage detected\n");
 	di->flags.vbus_ovv = true;
-	ab8500_power_supply_changed(di, &di->usb_chg.psy);
+	ab8500_power_supply_changed(di, di->usb_chg.psy);
 
 	/* Schedule a new HW failure check */
 	queue_delayed_work(di->charger_wq, &di->check_hw_failure_work, 0);
@@ -3428,10 +3428,10 @@ static int ab8500_charger_remove(struct platform_device *pdev)
 
 	flush_scheduled_work();
 	if (di->usb_chg.enabled)
-		power_supply_unregister(&di->usb_chg.psy);
+		power_supply_unregister(di->usb_chg.psy);
 
 	if (di->ac_chg.enabled && !di->ac_chg.external)
-		power_supply_unregister(&di->ac_chg.psy);
+		power_supply_unregister(di->ac_chg.psy);
 
 	return 0;
 }
@@ -3442,10 +3442,27 @@ static char *supply_interface[] = {
 	"ab8500_btemp",
 };
 
+static const struct power_supply_desc ab8500_ac_chg_desc = {
+	.name		= "ab8500_ac",
+	.type		= POWER_SUPPLY_TYPE_MAINS,
+	.properties	= ab8500_charger_ac_props,
+	.num_properties	= ARRAY_SIZE(ab8500_charger_ac_props),
+	.get_property	= ab8500_charger_ac_get_property,
+};
+
+static const struct power_supply_desc ab8500_usb_chg_desc = {
+	.name		= "ab8500_usb",
+	.type		= POWER_SUPPLY_TYPE_USB,
+	.properties	= ab8500_charger_usb_props,
+	.num_properties	= ARRAY_SIZE(ab8500_charger_usb_props),
+	.get_property	= ab8500_charger_usb_get_property,
+};
+
 static int ab8500_charger_probe(struct platform_device *pdev)
 {
 	struct device_node *np = pdev->dev.of_node;
 	struct abx500_bm_data *plat = pdev->dev.platform_data;
+	struct power_supply_config ac_psy_cfg = {}, usb_psy_cfg = {};
 	struct ab8500_charger *di;
 	int irq, i, charger_status, ret = 0, ch_stat;
 
@@ -3483,15 +3500,15 @@ static int ab8500_charger_probe(struct platform_device *pdev)
 	di->autopower = false;
 	di->invalid_charger_detect_state = 0;
 
+	/* AC and USB supply config */
+	ac_psy_cfg.supplied_to = supply_interface;
+	ac_psy_cfg.num_supplicants = ARRAY_SIZE(supply_interface);
+	ac_psy_cfg.drv_data = &di->ac_chg;
+	usb_psy_cfg.supplied_to = supply_interface;
+	usb_psy_cfg.num_supplicants = ARRAY_SIZE(supply_interface);
+	usb_psy_cfg.drv_data = &di->usb_chg;
+
 	/* AC supply */
-	/* power_supply base class */
-	di->ac_chg.psy.name = "ab8500_ac";
-	di->ac_chg.psy.type = POWER_SUPPLY_TYPE_MAINS;
-	di->ac_chg.psy.properties = ab8500_charger_ac_props;
-	di->ac_chg.psy.num_properties = ARRAY_SIZE(ab8500_charger_ac_props);
-	di->ac_chg.psy.get_property = ab8500_charger_ac_get_property;
-	di->ac_chg.psy.supplied_to = supply_interface;
-	di->ac_chg.psy.num_supplicants = ARRAY_SIZE(supply_interface),
 	/* ux500_charger sub-class */
 	di->ac_chg.ops.enable = &ab8500_charger_ac_en;
 	di->ac_chg.ops.check_enable = &ab8500_charger_ac_check_enable;
@@ -3511,14 +3528,6 @@ static int ab8500_charger_probe(struct platform_device *pdev)
 			&charger_notifier_list, &charger_nb);
 
 	/* USB supply */
-	/* power_supply base class */
-	di->usb_chg.psy.name = "ab8500_usb";
-	di->usb_chg.psy.type = POWER_SUPPLY_TYPE_USB;
-	di->usb_chg.psy.properties = ab8500_charger_usb_props;
-	di->usb_chg.psy.num_properties = ARRAY_SIZE(ab8500_charger_usb_props);
-	di->usb_chg.psy.get_property = ab8500_charger_usb_get_property;
-	di->usb_chg.psy.supplied_to = supply_interface;
-	di->usb_chg.psy.num_supplicants = ARRAY_SIZE(supply_interface),
 	/* ux500_charger sub-class */
 	di->usb_chg.ops.enable = &ab8500_charger_usb_en;
 	di->usb_chg.ops.check_enable = &ab8500_charger_usb_check_enable;
@@ -3616,18 +3625,24 @@ static int ab8500_charger_probe(struct platform_device *pdev)
 
 	/* Register AC charger class */
 	if (di->ac_chg.enabled) {
-		ret = power_supply_register(di->dev, &di->ac_chg.psy);
-		if (ret) {
+		di->ac_chg.psy = power_supply_register(di->dev,
+						       &ab8500_ac_chg_desc,
+						       &ac_psy_cfg);
+		if (IS_ERR(di->ac_chg.psy)) {
 			dev_err(di->dev, "failed to register AC charger\n");
+			ret = PTR_ERR(di->ac_chg.psy);
 			goto free_charger_wq;
 		}
 	}
 
 	/* Register USB charger class */
 	if (di->usb_chg.enabled) {
-		ret = power_supply_register(di->dev, &di->usb_chg.psy);
-		if (ret) {
+		di->usb_chg.psy = power_supply_register(di->dev,
+							&ab8500_usb_chg_desc,
+							&usb_psy_cfg);
+		if (IS_ERR(di->usb_chg.psy)) {
 			dev_err(di->dev, "failed to register USB charger\n");
+			ret = PTR_ERR(di->usb_chg.psy);
 			goto free_ac;
 		}
 	}
@@ -3650,8 +3665,8 @@ static int ab8500_charger_probe(struct platform_device *pdev)
 	if (charger_status & AC_PW_CONN) {
 		di->ac.charger_connected = 1;
 		di->ac_conn = true;
-		ab8500_power_supply_changed(di, &di->ac_chg.psy);
-		sysfs_notify(&di->ac_chg.psy.dev->kobj, NULL, "present");
+		ab8500_power_supply_changed(di, di->ac_chg.psy);
+		sysfs_notify(&di->ac_chg.psy->dev.kobj, NULL, "present");
 	}
 
 	if (charger_status & USB_PW_CONN) {
@@ -3712,10 +3727,10 @@ put_usb_phy:
 	usb_put_phy(di->usb_phy);
 free_usb:
 	if (di->usb_chg.enabled)
-		power_supply_unregister(&di->usb_chg.psy);
+		power_supply_unregister(di->usb_chg.psy);
 free_ac:
 	if (di->ac_chg.enabled)
-		power_supply_unregister(&di->ac_chg.psy);
+		power_supply_unregister(di->ac_chg.psy);
 free_charger_wq:
 	destroy_workqueue(di->charger_wq);
 	return ret;
diff --git a/drivers/power/ab8500_fg.c b/drivers/power/ab8500_fg.c
index c908658aa31a..3830dade5d69 100644
--- a/drivers/power/ab8500_fg.c
+++ b/drivers/power/ab8500_fg.c
@@ -57,9 +57,6 @@
 #define interpolate(x, x1, y1, x2, y2) \
 	((y1) + ((((y2) - (y1)) * ((x) - (x1))) / ((x2) - (x1))));
 
-#define to_ab8500_fg_device_info(x) container_of((x), \
-	struct ab8500_fg, fg_psy);
-
 /**
  * struct ab8500_fg_interrupts - ab8500 fg interupts
  * @name:	name of the interrupt
@@ -229,7 +226,7 @@ struct ab8500_fg {
 	struct ab8500 *parent;
 	struct ab8500_gpadc *gpadc;
 	struct abx500_bm_data *bm;
-	struct power_supply fg_psy;
+	struct power_supply *fg_psy;
 	struct workqueue_struct *fg_wq;
 	struct delayed_work fg_periodic_work;
 	struct delayed_work fg_low_bat_work;
@@ -622,14 +619,14 @@ int ab8500_fg_inst_curr_finalize(struct ab8500_fg *di, int *res)
 	u8 low, high;
 	int val;
 	int ret;
-	int timeout;
+	unsigned long timeout;
 
 	if (!completion_done(&di->ab8500_fg_complete)) {
 		timeout = wait_for_completion_timeout(
 			&di->ab8500_fg_complete,
 			INS_CURR_TIMEOUT);
 		dev_dbg(di->dev, "Finalize time: %d ms\n",
-			((INS_CURR_TIMEOUT - timeout) * 1000) / HZ);
+			jiffies_to_msecs(INS_CURR_TIMEOUT - timeout));
 		if (!timeout) {
 			ret = -ETIME;
 			disable_irq(di->irq);
@@ -716,7 +713,7 @@ fail:
 int ab8500_fg_inst_curr_blocking(struct ab8500_fg *di)
 {
 	int ret;
-	int timeout;
+	unsigned long timeout;
 	int res = 0;
 
 	ret = ab8500_fg_inst_curr_start(di);
@@ -731,7 +728,7 @@ int ab8500_fg_inst_curr_blocking(struct ab8500_fg *di)
 			&di->ab8500_fg_started,
 			INS_CURR_TIMEOUT);
 		dev_dbg(di->dev, "Start time: %d ms\n",
-			((INS_CURR_TIMEOUT - timeout) * 1000) / HZ);
+			jiffies_to_msecs(INS_CURR_TIMEOUT - timeout));
 		if (!timeout) {
 			ret = -ETIME;
 			dev_err(di->dev, "completion timed out [%d]\n",
@@ -1391,7 +1388,7 @@ static void ab8500_fg_check_capacity_limits(struct ab8500_fg *di, bool init)
 				di->bat_cap.prev_percent,
 				di->bat_cap.cap_scale.scaled_cap);
 		}
-		power_supply_changed(&di->fg_psy);
+		power_supply_changed(di->fg_psy);
 		if (di->flags.fully_charged && di->flags.force_full) {
 			dev_dbg(di->dev, "Battery full, notifying.\n");
 			di->flags.force_full = false;
@@ -1850,7 +1847,7 @@ static void ab8500_fg_check_hw_failure_work(struct work_struct *work)
 		if (!di->flags.bat_ovv) {
 			dev_dbg(di->dev, "Battery OVV\n");
 			di->flags.bat_ovv = true;
-			power_supply_changed(&di->fg_psy);
+			power_supply_changed(di->fg_psy);
 		}
 		/* Not yet recovered from ovv, reschedule this test */
 		queue_delayed_work(di->fg_wq, &di->fg_check_hw_failure_work,
@@ -1858,7 +1855,7 @@ static void ab8500_fg_check_hw_failure_work(struct work_struct *work)
 		} else {
 			dev_dbg(di->dev, "Battery recovered from OVV\n");
 			di->flags.bat_ovv = false;
-			power_supply_changed(&di->fg_psy);
+			power_supply_changed(di->fg_psy);
 	}
 }
 
@@ -2096,9 +2093,7 @@ static int ab8500_fg_get_property(struct power_supply *psy,
 	enum power_supply_property psp,
 	union power_supply_propval *val)
 {
-	struct ab8500_fg *di;
-
-	di = to_ab8500_fg_device_info(psy);
+	struct ab8500_fg *di = power_supply_get_drvdata(psy);
 
 	/*
 	 * If battery is identified as unknown and charging of unknown
@@ -2181,14 +2176,14 @@ static int ab8500_fg_get_ext_psy_data(struct device *dev, void *data)
 
 	psy = (struct power_supply *)data;
 	ext = dev_get_drvdata(dev);
-	di = to_ab8500_fg_device_info(psy);
+	di = power_supply_get_drvdata(psy);
 
 	/*
 	 * For all psy where the name of your driver
 	 * appears in any supplied_to
 	 */
 	for (i = 0; i < ext->num_supplicants; i++) {
-		if (!strcmp(ext->supplied_to[i], psy->name))
+		if (!strcmp(ext->supplied_to[i], psy->desc->name))
 			psy_found = true;
 	}
 
@@ -2196,16 +2191,16 @@ static int ab8500_fg_get_ext_psy_data(struct device *dev, void *data)
 		return 0;
 
 	/* Go through all properties for the psy */
-	for (j = 0; j < ext->num_properties; j++) {
+	for (j = 0; j < ext->desc->num_properties; j++) {
 		enum power_supply_property prop;
-		prop = ext->properties[j];
+		prop = ext->desc->properties[j];
 
-		if (ext->get_property(ext, prop, &ret))
+		if (power_supply_get_property(ext, prop, &ret))
 			continue;
 
 		switch (prop) {
 		case POWER_SUPPLY_PROP_STATUS:
-			switch (ext->type) {
+			switch (ext->desc->type) {
 			case POWER_SUPPLY_TYPE_BATTERY:
 				switch (ret.intval) {
 				case POWER_SUPPLY_STATUS_UNKNOWN:
@@ -2244,7 +2239,7 @@ static int ab8500_fg_get_ext_psy_data(struct device *dev, void *data)
 			};
 			break;
 		case POWER_SUPPLY_PROP_TECHNOLOGY:
-			switch (ext->type) {
+			switch (ext->desc->type) {
 			case POWER_SUPPLY_TYPE_BATTERY:
 				if (!di->flags.batt_id_received &&
 				    di->bm->batt_id != BATTERY_UNKNOWN) {
@@ -2274,7 +2269,7 @@ static int ab8500_fg_get_ext_psy_data(struct device *dev, void *data)
 			}
 			break;
 		case POWER_SUPPLY_PROP_TEMP:
-			switch (ext->type) {
+			switch (ext->desc->type) {
 			case POWER_SUPPLY_TYPE_BATTERY:
 				if (di->flags.batt_id_received)
 					di->bat_temp = ret.intval;
@@ -2399,10 +2394,10 @@ out:
  */
 static void ab8500_fg_external_power_changed(struct power_supply *psy)
 {
-	struct ab8500_fg *di = to_ab8500_fg_device_info(psy);
+	struct ab8500_fg *di = power_supply_get_drvdata(psy);
 
 	class_for_each_device(power_supply_class, NULL,
-		&di->fg_psy, ab8500_fg_get_ext_psy_data);
+		di->fg_psy, ab8500_fg_get_ext_psy_data);
 }
 
 /**
@@ -2580,9 +2575,7 @@ static ssize_t ab8505_powercut_flagtime_read(struct device *dev,
 	int ret;
 	u8 reg_value;
 	struct power_supply *psy = dev_get_drvdata(dev);
-	struct ab8500_fg *di;
-
-	di = to_ab8500_fg_device_info(psy);
+	struct ab8500_fg *di = power_supply_get_drvdata(psy);
 
 	ret = abx500_get_register_interruptible(di->dev, AB8500_RTC,
 		AB8505_RTC_PCUT_FLAG_TIME_REG, &reg_value);
@@ -2605,9 +2598,7 @@ static ssize_t ab8505_powercut_flagtime_write(struct device *dev,
 	int ret;
 	long unsigned reg_value;
 	struct power_supply *psy = dev_get_drvdata(dev);
-	struct ab8500_fg *di;
-
-	di = to_ab8500_fg_device_info(psy);
+	struct ab8500_fg *di = power_supply_get_drvdata(psy);
 
 	reg_value = simple_strtoul(buf, NULL, 10);
 
@@ -2633,9 +2624,7 @@ static ssize_t ab8505_powercut_maxtime_read(struct device *dev,
 	int ret;
 	u8 reg_value;
 	struct power_supply *psy = dev_get_drvdata(dev);
-	struct ab8500_fg *di;
-
-	di = to_ab8500_fg_device_info(psy);
+	struct ab8500_fg *di = power_supply_get_drvdata(psy);
 
 	ret = abx500_get_register_interruptible(di->dev, AB8500_RTC,
 		AB8505_RTC_PCUT_MAX_TIME_REG, &reg_value);
@@ -2659,9 +2648,7 @@ static ssize_t ab8505_powercut_maxtime_write(struct device *dev,
 	int ret;
 	int reg_value;
 	struct power_supply *psy = dev_get_drvdata(dev);
-	struct ab8500_fg *di;
-
-	di = to_ab8500_fg_device_info(psy);
+	struct ab8500_fg *di = power_supply_get_drvdata(psy);
 
 	reg_value = simple_strtoul(buf, NULL, 10);
 	if (reg_value > 0x7F) {
@@ -2686,9 +2673,7 @@ static ssize_t ab8505_powercut_restart_read(struct device *dev,
 	int ret;
 	u8 reg_value;
 	struct power_supply *psy = dev_get_drvdata(dev);
-	struct ab8500_fg *di;
-
-	di = to_ab8500_fg_device_info(psy);
+	struct ab8500_fg *di = power_supply_get_drvdata(psy);
 
 	ret = abx500_get_register_interruptible(di->dev, AB8500_RTC,
 		AB8505_RTC_PCUT_RESTART_REG, &reg_value);
@@ -2711,9 +2696,7 @@ static ssize_t ab8505_powercut_restart_write(struct device *dev,
 	int ret;
 	int reg_value;
 	struct power_supply *psy = dev_get_drvdata(dev);
-	struct ab8500_fg *di;
-
-	di = to_ab8500_fg_device_info(psy);
+	struct ab8500_fg *di = power_supply_get_drvdata(psy);
 
 	reg_value = simple_strtoul(buf, NULL, 10);
 	if (reg_value > 0xF) {
@@ -2739,9 +2722,7 @@ static ssize_t ab8505_powercut_timer_read(struct device *dev,
 	int ret;
 	u8 reg_value;
 	struct power_supply *psy = dev_get_drvdata(dev);
-	struct ab8500_fg *di;
-
-	di = to_ab8500_fg_device_info(psy);
+	struct ab8500_fg *di = power_supply_get_drvdata(psy);
 
 	ret = abx500_get_register_interruptible(di->dev, AB8500_RTC,
 						AB8505_RTC_PCUT_TIME_REG, &reg_value);
@@ -2764,9 +2745,7 @@ static ssize_t ab8505_powercut_restart_counter_read(struct device *dev,
 	int ret;
 	u8 reg_value;
 	struct power_supply *psy = dev_get_drvdata(dev);
-	struct ab8500_fg *di;
-
-	di = to_ab8500_fg_device_info(psy);
+	struct ab8500_fg *di = power_supply_get_drvdata(psy);
 
 	ret = abx500_get_register_interruptible(di->dev, AB8500_RTC,
 						AB8505_RTC_PCUT_RESTART_REG, &reg_value);
@@ -2789,9 +2768,7 @@ static ssize_t ab8505_powercut_read(struct device *dev,
 	int ret;
 	u8 reg_value;
 	struct power_supply *psy = dev_get_drvdata(dev);
-	struct ab8500_fg *di;
-
-	di = to_ab8500_fg_device_info(psy);
+	struct ab8500_fg *di = power_supply_get_drvdata(psy);
 
 	ret = abx500_get_register_interruptible(di->dev, AB8500_RTC,
 						AB8505_RTC_PCUT_CTL_STATUS_REG, &reg_value);
@@ -2812,9 +2789,7 @@ static ssize_t ab8505_powercut_write(struct device *dev,
 	int ret;
 	int reg_value;
 	struct power_supply *psy = dev_get_drvdata(dev);
-	struct ab8500_fg *di;
-
-	di = to_ab8500_fg_device_info(psy);
+	struct ab8500_fg *di = power_supply_get_drvdata(psy);
 
 	reg_value = simple_strtoul(buf, NULL, 10);
 	if (reg_value > 0x1) {
@@ -2840,9 +2815,7 @@ static ssize_t ab8505_powercut_flag_read(struct device *dev,
 	int ret;
 	u8 reg_value;
 	struct power_supply *psy = dev_get_drvdata(dev);
-	struct ab8500_fg *di;
-
-	di = to_ab8500_fg_device_info(psy);
+	struct ab8500_fg *di = power_supply_get_drvdata(psy);
 
 	ret = abx500_get_register_interruptible(di->dev, AB8500_RTC,
 						AB8505_RTC_PCUT_CTL_STATUS_REG,  &reg_value);
@@ -2865,9 +2838,7 @@ static ssize_t ab8505_powercut_debounce_read(struct device *dev,
 	int ret;
 	u8 reg_value;
 	struct power_supply *psy = dev_get_drvdata(dev);
-	struct ab8500_fg *di;
-
-	di = to_ab8500_fg_device_info(psy);
+	struct ab8500_fg *di = power_supply_get_drvdata(psy);
 
 	ret = abx500_get_register_interruptible(di->dev, AB8500_RTC,
 						AB8505_RTC_PCUT_DEBOUNCE_REG,  &reg_value);
@@ -2890,9 +2861,7 @@ static ssize_t ab8505_powercut_debounce_write(struct device *dev,
 	int ret;
 	int reg_value;
 	struct power_supply *psy = dev_get_drvdata(dev);
-	struct ab8500_fg *di;
-
-	di = to_ab8500_fg_device_info(psy);
+	struct ab8500_fg *di = power_supply_get_drvdata(psy);
 
 	reg_value = simple_strtoul(buf, NULL, 10);
 	if (reg_value > 0x7) {
@@ -2917,9 +2886,7 @@ static ssize_t ab8505_powercut_enable_status_read(struct device *dev,
 	int ret;
 	u8 reg_value;
 	struct power_supply *psy = dev_get_drvdata(dev);
-	struct ab8500_fg *di;
-
-	di = to_ab8500_fg_device_info(psy);
+	struct ab8500_fg *di = power_supply_get_drvdata(psy);
 
 	ret = abx500_get_register_interruptible(di->dev, AB8500_RTC,
 						AB8505_RTC_PCUT_CTL_STATUS_REG, &reg_value);
@@ -2954,44 +2921,38 @@ static struct device_attribute ab8505_fg_sysfs_psy_attrs[] = {
 		ab8505_powercut_enable_status_read, NULL),
 };
 
-static int ab8500_fg_sysfs_psy_create_attrs(struct device *dev)
+static int ab8500_fg_sysfs_psy_create_attrs(struct ab8500_fg *di)
 {
 	unsigned int i;
-	struct power_supply *psy = dev_get_drvdata(dev);
-	struct ab8500_fg *di;
-
-	di = to_ab8500_fg_device_info(psy);
 
 	if (((is_ab8505(di->parent) || is_ab9540(di->parent)) &&
-	     abx500_get_chip_id(dev->parent) >= AB8500_CUT2P0)
+	     abx500_get_chip_id(di->dev) >= AB8500_CUT2P0)
 	    || is_ab8540(di->parent)) {
 		for (i = 0; i < ARRAY_SIZE(ab8505_fg_sysfs_psy_attrs); i++)
-			if (device_create_file(dev,
+			if (device_create_file(&di->fg_psy->dev,
 					       &ab8505_fg_sysfs_psy_attrs[i]))
 				goto sysfs_psy_create_attrs_failed_ab8505;
 	}
 	return 0;
 sysfs_psy_create_attrs_failed_ab8505:
-	dev_err(dev, "Failed creating sysfs psy attrs for ab8505.\n");
+	dev_err(&di->fg_psy->dev, "Failed creating sysfs psy attrs for ab8505.\n");
 	while (i--)
-		device_remove_file(dev, &ab8505_fg_sysfs_psy_attrs[i]);
+		device_remove_file(&di->fg_psy->dev,
+				   &ab8505_fg_sysfs_psy_attrs[i]);
 
 	return -EIO;
 }
 
-static void ab8500_fg_sysfs_psy_remove_attrs(struct device *dev)
+static void ab8500_fg_sysfs_psy_remove_attrs(struct ab8500_fg *di)
 {
 	unsigned int i;
-	struct power_supply *psy = dev_get_drvdata(dev);
-	struct ab8500_fg *di;
-
-	di = to_ab8500_fg_device_info(psy);
 
 	if (((is_ab8505(di->parent) || is_ab9540(di->parent)) &&
-	     abx500_get_chip_id(dev->parent) >= AB8500_CUT2P0)
+	     abx500_get_chip_id(di->dev) >= AB8500_CUT2P0)
 	    || is_ab8540(di->parent)) {
 		for (i = 0; i < ARRAY_SIZE(ab8505_fg_sysfs_psy_attrs); i++)
-			(void)device_remove_file(dev, &ab8505_fg_sysfs_psy_attrs[i]);
+			(void)device_remove_file(&di->fg_psy->dev,
+						 &ab8505_fg_sysfs_psy_attrs[i]);
 	}
 }
 
@@ -3056,17 +3017,20 @@ static int ab8500_fg_remove(struct platform_device *pdev)
 	ab8500_fg_sysfs_exit(di);
 
 	flush_scheduled_work();
-	ab8500_fg_sysfs_psy_remove_attrs(di->fg_psy.dev);
-	power_supply_unregister(&di->fg_psy);
+	ab8500_fg_sysfs_psy_remove_attrs(di);
+	power_supply_unregister(di->fg_psy);
 	return ret;
 }
 
 /* ab8500 fg driver interrupts and their respective isr */
-static struct ab8500_fg_interrupts ab8500_fg_irq[] = {
+static struct ab8500_fg_interrupts ab8500_fg_irq_th[] = {
 	{"NCONV_ACCU", ab8500_fg_cc_convend_handler},
 	{"BATT_OVV", ab8500_fg_batt_ovv_handler},
 	{"LOW_BAT_F", ab8500_fg_lowbatf_handler},
 	{"CC_INT_CALIB", ab8500_fg_cc_int_calib_handler},
+};
+
+static struct ab8500_fg_interrupts ab8500_fg_irq_bh[] = {
 	{"CCEOC", ab8500_fg_cc_data_end_handler},
 };
 
@@ -3075,10 +3039,20 @@ static char *supply_interface[] = {
 	"ab8500_usb",
 };
 
+static const struct power_supply_desc ab8500_fg_desc = {
+	.name			= "ab8500_fg",
+	.type			= POWER_SUPPLY_TYPE_BATTERY,
+	.properties		= ab8500_fg_props,
+	.num_properties		= ARRAY_SIZE(ab8500_fg_props),
+	.get_property		= ab8500_fg_get_property,
+	.external_power_changed	= ab8500_fg_external_power_changed,
+};
+
 static int ab8500_fg_probe(struct platform_device *pdev)
 {
 	struct device_node *np = pdev->dev.of_node;
 	struct abx500_bm_data *plat = pdev->dev.platform_data;
+	struct power_supply_config psy_cfg = {};
 	struct ab8500_fg *di;
 	int i, irq;
 	int ret = 0;
@@ -3110,14 +3084,9 @@ static int ab8500_fg_probe(struct platform_device *pdev)
 	di->parent = dev_get_drvdata(pdev->dev.parent);
 	di->gpadc = ab8500_gpadc_get("ab8500-gpadc.0");
 
-	di->fg_psy.name = "ab8500_fg";
-	di->fg_psy.type = POWER_SUPPLY_TYPE_BATTERY;
-	di->fg_psy.properties = ab8500_fg_props;
-	di->fg_psy.num_properties = ARRAY_SIZE(ab8500_fg_props);
-	di->fg_psy.get_property = ab8500_fg_get_property;
-	di->fg_psy.supplied_to = supply_interface;
-	di->fg_psy.num_supplicants = ARRAY_SIZE(supply_interface),
-	di->fg_psy.external_power_changed = ab8500_fg_external_power_changed;
+	psy_cfg.supplied_to = supply_interface;
+	psy_cfg.num_supplicants = ARRAY_SIZE(supply_interface);
+	psy_cfg.drv_data = di;
 
 	di->bat_cap.max_mah_design = MILLI_TO_MICRO *
 		di->bm->bat_type[di->bm->batt_id].charge_full_design;
@@ -3178,9 +3147,10 @@ static int ab8500_fg_probe(struct platform_device *pdev)
 	di->flags.batt_id_received = false;
 
 	/* Register FG power supply class */
-	ret = power_supply_register(di->dev, &di->fg_psy);
-	if (ret) {
+	di->fg_psy = power_supply_register(di->dev, &ab8500_fg_desc, &psy_cfg);
+	if (IS_ERR(di->fg_psy)) {
 		dev_err(di->dev, "failed to register FG psy\n");
+		ret = PTR_ERR(di->fg_psy);
 		goto free_inst_curr_wq;
 	}
 
@@ -3194,21 +3164,36 @@ static int ab8500_fg_probe(struct platform_device *pdev)
 	init_completion(&di->ab8500_fg_started);
 	init_completion(&di->ab8500_fg_complete);
 
-	/* Register interrupts */
-	for (i = 0; i < ARRAY_SIZE(ab8500_fg_irq); i++) {
-		irq = platform_get_irq_byname(pdev, ab8500_fg_irq[i].name);
-		ret = request_threaded_irq(irq, NULL, ab8500_fg_irq[i].isr,
-			IRQF_SHARED | IRQF_NO_SUSPEND,
-			ab8500_fg_irq[i].name, di);
+	/* Register primary interrupt handlers */
+	for (i = 0; i < ARRAY_SIZE(ab8500_fg_irq_th); i++) {
+		irq = platform_get_irq_byname(pdev, ab8500_fg_irq_th[i].name);
+		ret = request_irq(irq, ab8500_fg_irq_th[i].isr,
+				  IRQF_SHARED | IRQF_NO_SUSPEND,
+				  ab8500_fg_irq_th[i].name, di);
 
 		if (ret != 0) {
-			dev_err(di->dev, "failed to request %s IRQ %d: %d\n"
-				, ab8500_fg_irq[i].name, irq, ret);
+			dev_err(di->dev, "failed to request %s IRQ %d: %d\n",
+				ab8500_fg_irq_th[i].name, irq, ret);
 			goto free_irq;
 		}
 		dev_dbg(di->dev, "Requested %s IRQ %d: %d\n",
-			ab8500_fg_irq[i].name, irq, ret);
+			ab8500_fg_irq_th[i].name, irq, ret);
 	}
+
+	/* Register threaded interrupt handler */
+	irq = platform_get_irq_byname(pdev, ab8500_fg_irq_bh[0].name);
+	ret = request_threaded_irq(irq, NULL, ab8500_fg_irq_bh[0].isr,
+				IRQF_SHARED | IRQF_NO_SUSPEND | IRQF_ONESHOT,
+			ab8500_fg_irq_bh[0].name, di);
+
+	if (ret != 0) {
+		dev_err(di->dev, "failed to request %s IRQ %d: %d\n",
+			ab8500_fg_irq_bh[0].name, irq, ret);
+		goto free_irq;
+	}
+	dev_dbg(di->dev, "Requested %s IRQ %d: %d\n",
+		ab8500_fg_irq_bh[0].name, irq, ret);
+
 	di->irq = platform_get_irq_byname(pdev, "CCEOC");
 	disable_irq(di->irq);
 	di->nbr_cceoc_irq_cnt = 0;
@@ -3221,7 +3206,7 @@ static int ab8500_fg_probe(struct platform_device *pdev)
 		goto free_irq;
 	}
 
-	ret = ab8500_fg_sysfs_psy_create_attrs(di->fg_psy.dev);
+	ret = ab8500_fg_sysfs_psy_create_attrs(di);
 	if (ret) {
 		dev_err(di->dev, "failed to create FG psy\n");
 		ab8500_fg_sysfs_exit(di);
@@ -3243,13 +3228,15 @@ static int ab8500_fg_probe(struct platform_device *pdev)
 	return ret;
 
 free_irq:
-	power_supply_unregister(&di->fg_psy);
+	power_supply_unregister(di->fg_psy);
 
-	/* We also have to free all successfully registered irqs */
-	for (i = i - 1; i >= 0; i--) {
-		irq = platform_get_irq_byname(pdev, ab8500_fg_irq[i].name);
+	/* We also have to free all registered irqs */
+	for (i = 0; i < ARRAY_SIZE(ab8500_fg_irq_th); i++) {
+		irq = platform_get_irq_byname(pdev, ab8500_fg_irq_th[i].name);
 		free_irq(irq, di);
 	}
+	irq = platform_get_irq_byname(pdev, ab8500_fg_irq_bh[0].name);
+	free_irq(irq, di);
 free_inst_curr_wq:
 	destroy_workqueue(di->fg_wq);
 	return ret;
diff --git a/drivers/power/abx500_chargalg.c b/drivers/power/abx500_chargalg.c
index ab54b8dea670..541f702e0451 100644
--- a/drivers/power/abx500_chargalg.c
+++ b/drivers/power/abx500_chargalg.c
@@ -50,9 +50,6 @@
 #define CHARGALG_CURR_STEP_LOW		0
 #define CHARGALG_CURR_STEP_HIGH	100
 
-#define to_abx500_chargalg_device_info(x) container_of((x), \
-	struct abx500_chargalg, chargalg_psy);
-
 enum abx500_chargers {
 	NO_CHG,
 	AC_CHG,
@@ -256,7 +253,7 @@ struct abx500_chargalg {
 	struct ab8500 *parent;
 	struct abx500_chargalg_current_step_status curr_status;
 	struct abx500_bm_data *bm;
-	struct power_supply chargalg_psy;
+	struct power_supply *chargalg_psy;
 	struct ux500_charger *ac_chg;
 	struct ux500_charger *usb_chg;
 	struct abx500_chargalg_events events;
@@ -695,7 +692,7 @@ static void abx500_chargalg_stop_charging(struct abx500_chargalg *di)
 	di->charge_status = POWER_SUPPLY_STATUS_NOT_CHARGING;
 	di->maintenance_chg = false;
 	cancel_delayed_work(&di->chargalg_wd_work);
-	power_supply_changed(&di->chargalg_psy);
+	power_supply_changed(di->chargalg_psy);
 }
 
 /**
@@ -715,7 +712,7 @@ static void abx500_chargalg_hold_charging(struct abx500_chargalg *di)
 	di->charge_status = POWER_SUPPLY_STATUS_CHARGING;
 	di->maintenance_chg = false;
 	cancel_delayed_work(&di->chargalg_wd_work);
-	power_supply_changed(&di->chargalg_psy);
+	power_supply_changed(di->chargalg_psy);
 }
 
 /**
@@ -842,7 +839,7 @@ static void abx500_chargalg_end_of_charge(struct abx500_chargalg *di)
 			di->charge_status = POWER_SUPPLY_STATUS_FULL;
 			di->maintenance_chg = true;
 			dev_dbg(di->dev, "EOC reached!\n");
-			power_supply_changed(&di->chargalg_psy);
+			power_supply_changed(di->chargalg_psy);
 		} else {
 			dev_dbg(di->dev,
 				" EOC limit reached for the %d"
@@ -987,10 +984,10 @@ static int abx500_chargalg_get_ext_psy_data(struct device *dev, void *data)
 
 	psy = (struct power_supply *)data;
 	ext = dev_get_drvdata(dev);
-	di = to_abx500_chargalg_device_info(psy);
+	di = power_supply_get_drvdata(psy);
 	/* For all psy where the driver name appears in any supplied_to */
 	for (i = 0; i < ext->num_supplicants; i++) {
-		if (!strcmp(ext->supplied_to[i], psy->name))
+		if (!strcmp(ext->supplied_to[i], psy->desc->name))
 			psy_found = true;
 	}
 	if (!psy_found)
@@ -1001,29 +998,31 @@ static int abx500_chargalg_get_ext_psy_data(struct device *dev, void *data)
 	 * property because of handling that sysfs entry on its own, this is
 	 * the place to get the battery capacity.
 	 */
-	if (!ext->get_property(ext, POWER_SUPPLY_PROP_CAPACITY, &ret)) {
+	if (!power_supply_get_property(ext, POWER_SUPPLY_PROP_CAPACITY, &ret)) {
 		di->batt_data.percent = ret.intval;
 		capacity_updated = true;
 	}
 
 	/* Go through all properties for the psy */
-	for (j = 0; j < ext->num_properties; j++) {
+	for (j = 0; j < ext->desc->num_properties; j++) {
 		enum power_supply_property prop;
-		prop = ext->properties[j];
+		prop = ext->desc->properties[j];
 
-		/* Initialize chargers if not already done */
+		/*
+		 * Initialize chargers if not already done.
+		 * The ab8500_charger*/
 		if (!di->ac_chg &&
-			ext->type == POWER_SUPPLY_TYPE_MAINS)
+			ext->desc->type == POWER_SUPPLY_TYPE_MAINS)
 			di->ac_chg = psy_to_ux500_charger(ext);
 		else if (!di->usb_chg &&
-			ext->type == POWER_SUPPLY_TYPE_USB)
+			ext->desc->type == POWER_SUPPLY_TYPE_USB)
 			di->usb_chg = psy_to_ux500_charger(ext);
 
-		if (ext->get_property(ext, prop, &ret))
+		if (power_supply_get_property(ext, prop, &ret))
 			continue;
 		switch (prop) {
 		case POWER_SUPPLY_PROP_PRESENT:
-			switch (ext->type) {
+			switch (ext->desc->type) {
 			case POWER_SUPPLY_TYPE_BATTERY:
 				/* Battery present */
 				if (ret.intval)
@@ -1070,7 +1069,7 @@ static int abx500_chargalg_get_ext_psy_data(struct device *dev, void *data)
 			break;
 
 		case POWER_SUPPLY_PROP_ONLINE:
-			switch (ext->type) {
+			switch (ext->desc->type) {
 			case POWER_SUPPLY_TYPE_BATTERY:
 				break;
 			case POWER_SUPPLY_TYPE_MAINS:
@@ -1115,7 +1114,7 @@ static int abx500_chargalg_get_ext_psy_data(struct device *dev, void *data)
 			break;
 
 		case POWER_SUPPLY_PROP_HEALTH:
-			switch (ext->type) {
+			switch (ext->desc->type) {
 			case POWER_SUPPLY_TYPE_BATTERY:
 				break;
 			case POWER_SUPPLY_TYPE_MAINS:
@@ -1198,7 +1197,7 @@ static int abx500_chargalg_get_ext_psy_data(struct device *dev, void *data)
 			break;
 
 		case POWER_SUPPLY_PROP_VOLTAGE_NOW:
-			switch (ext->type) {
+			switch (ext->desc->type) {
 			case POWER_SUPPLY_TYPE_BATTERY:
 				di->batt_data.volt = ret.intval / 1000;
 				break;
@@ -1214,7 +1213,7 @@ static int abx500_chargalg_get_ext_psy_data(struct device *dev, void *data)
 			break;
 
 		case POWER_SUPPLY_PROP_VOLTAGE_AVG:
-			switch (ext->type) {
+			switch (ext->desc->type) {
 			case POWER_SUPPLY_TYPE_MAINS:
 				/* AVG is used to indicate when we are
 				 * in CV mode */
@@ -1239,7 +1238,7 @@ static int abx500_chargalg_get_ext_psy_data(struct device *dev, void *data)
 			break;
 
 		case POWER_SUPPLY_PROP_TECHNOLOGY:
-			switch (ext->type) {
+			switch (ext->desc->type) {
 			case POWER_SUPPLY_TYPE_BATTERY:
 				if (ret.intval)
 					di->events.batt_unknown = false;
@@ -1257,7 +1256,7 @@ static int abx500_chargalg_get_ext_psy_data(struct device *dev, void *data)
 			break;
 
 		case POWER_SUPPLY_PROP_CURRENT_NOW:
-			switch (ext->type) {
+			switch (ext->desc->type) {
 			case POWER_SUPPLY_TYPE_MAINS:
 					di->chg_info.ac_curr =
 						ret.intval / 1000;
@@ -1275,7 +1274,7 @@ static int abx500_chargalg_get_ext_psy_data(struct device *dev, void *data)
 			break;
 
 		case POWER_SUPPLY_PROP_CURRENT_AVG:
-			switch (ext->type) {
+			switch (ext->desc->type) {
 			case POWER_SUPPLY_TYPE_BATTERY:
 				di->batt_data.avg_curr = ret.intval / 1000;
 				break;
@@ -1311,7 +1310,7 @@ static int abx500_chargalg_get_ext_psy_data(struct device *dev, void *data)
  */
 static void abx500_chargalg_external_power_changed(struct power_supply *psy)
 {
-	struct abx500_chargalg *di = to_abx500_chargalg_device_info(psy);
+	struct abx500_chargalg *di = power_supply_get_drvdata(psy);
 
 	/*
 	 * Trigger execution of the algorithm instantly and read
@@ -1336,7 +1335,7 @@ static void abx500_chargalg_algorithm(struct abx500_chargalg *di)
 
 	/* Collect data from all power_supply class devices */
 	class_for_each_device(power_supply_class, NULL,
-		&di->chargalg_psy, abx500_chargalg_get_ext_psy_data);
+		di->chargalg_psy, abx500_chargalg_get_ext_psy_data);
 
 	abx500_chargalg_end_of_charge(di);
 	abx500_chargalg_check_temp(di);
@@ -1478,7 +1477,7 @@ static void abx500_chargalg_algorithm(struct abx500_chargalg *di)
 		di->charge_status = POWER_SUPPLY_STATUS_NOT_CHARGING;
 		di->maintenance_chg = false;
 		abx500_chargalg_state_to(di, STATE_SUSPENDED);
-		power_supply_changed(&di->chargalg_psy);
+		power_supply_changed(di->chargalg_psy);
 		/* Intentional fallthrough */
 
 	case STATE_SUSPENDED:
@@ -1576,7 +1575,7 @@ static void abx500_chargalg_algorithm(struct abx500_chargalg *di)
 		di->charge_status = POWER_SUPPLY_STATUS_CHARGING;
 		di->eoc_cnt = 0;
 		di->maintenance_chg = false;
-		power_supply_changed(&di->chargalg_psy);
+		power_supply_changed(di->chargalg_psy);
 
 		break;
 
@@ -1624,7 +1623,7 @@ static void abx500_chargalg_algorithm(struct abx500_chargalg *di)
 			di->bm->bat_type[
 				di->bm->batt_id].maint_a_cur_lvl);
 		abx500_chargalg_state_to(di, STATE_MAINTENANCE_A);
-		power_supply_changed(&di->chargalg_psy);
+		power_supply_changed(di->chargalg_psy);
 		/* Intentional fallthrough*/
 
 	case STATE_MAINTENANCE_A:
@@ -1644,7 +1643,7 @@ static void abx500_chargalg_algorithm(struct abx500_chargalg *di)
 			di->bm->bat_type[
 				di->bm->batt_id].maint_b_cur_lvl);
 		abx500_chargalg_state_to(di, STATE_MAINTENANCE_B);
-		power_supply_changed(&di->chargalg_psy);
+		power_supply_changed(di->chargalg_psy);
 		/* Intentional fallthrough*/
 
 	case STATE_MAINTENANCE_B:
@@ -1663,7 +1662,7 @@ static void abx500_chargalg_algorithm(struct abx500_chargalg *di)
 		abx500_chargalg_stop_maintenance_timer(di);
 		di->charge_status = POWER_SUPPLY_STATUS_CHARGING;
 		abx500_chargalg_state_to(di, STATE_TEMP_LOWHIGH);
-		power_supply_changed(&di->chargalg_psy);
+		power_supply_changed(di->chargalg_psy);
 		/* Intentional fallthrough */
 
 	case STATE_TEMP_LOWHIGH:
@@ -1779,9 +1778,7 @@ static int abx500_chargalg_get_property(struct power_supply *psy,
 	enum power_supply_property psp,
 	union power_supply_propval *val)
 {
-	struct abx500_chargalg *di;
-
-	di = to_abx500_chargalg_device_info(psy);
+	struct abx500_chargalg *di = power_supply_get_drvdata(psy);
 
 	switch (psp) {
 	case POWER_SUPPLY_PROP_STATUS:
@@ -2034,7 +2031,7 @@ static int abx500_chargalg_remove(struct platform_device *pdev)
 	/* Delete the work queue */
 	destroy_workqueue(di->chargalg_wq);
 
-	power_supply_unregister(&di->chargalg_psy);
+	power_supply_unregister(di->chargalg_psy);
 
 	return 0;
 }
@@ -2043,10 +2040,20 @@ static char *supply_interface[] = {
 	"ab8500_fg",
 };
 
+static const struct power_supply_desc abx500_chargalg_desc = {
+	.name			= "abx500_chargalg",
+	.type			= POWER_SUPPLY_TYPE_BATTERY,
+	.properties		= abx500_chargalg_props,
+	.num_properties		= ARRAY_SIZE(abx500_chargalg_props),
+	.get_property		= abx500_chargalg_get_property,
+	.external_power_changed	= abx500_chargalg_external_power_changed,
+};
+
 static int abx500_chargalg_probe(struct platform_device *pdev)
 {
 	struct device_node *np = pdev->dev.of_node;
 	struct abx500_bm_data *plat = pdev->dev.platform_data;
+	struct power_supply_config psy_cfg = {};
 	struct abx500_chargalg *di;
 	int ret = 0;
 
@@ -2074,16 +2081,9 @@ static int abx500_chargalg_probe(struct platform_device *pdev)
 	di->dev = &pdev->dev;
 	di->parent = dev_get_drvdata(pdev->dev.parent);
 
-	/* chargalg supply */
-	di->chargalg_psy.name = "abx500_chargalg";
-	di->chargalg_psy.type = POWER_SUPPLY_TYPE_BATTERY;
-	di->chargalg_psy.properties = abx500_chargalg_props;
-	di->chargalg_psy.num_properties = ARRAY_SIZE(abx500_chargalg_props);
-	di->chargalg_psy.get_property = abx500_chargalg_get_property;
-	di->chargalg_psy.supplied_to = supply_interface;
-	di->chargalg_psy.num_supplicants = ARRAY_SIZE(supply_interface),
-	di->chargalg_psy.external_power_changed =
-		abx500_chargalg_external_power_changed;
+	psy_cfg.supplied_to = supply_interface;
+	psy_cfg.num_supplicants = ARRAY_SIZE(supply_interface);
+	psy_cfg.drv_data = di;
 
 	/* Initilialize safety timer */
 	hrtimer_init(&di->safety_timer, CLOCK_REALTIME, HRTIMER_MODE_ABS);
@@ -2115,9 +2115,11 @@ static int abx500_chargalg_probe(struct platform_device *pdev)
 	di->chg_info.prev_conn_chg = -1;
 
 	/* Register chargalg power supply class */
-	ret = power_supply_register(di->dev, &di->chargalg_psy);
-	if (ret) {
+	di->chargalg_psy = power_supply_register(di->dev, &abx500_chargalg_desc,
+						 &psy_cfg);
+	if (IS_ERR(di->chargalg_psy)) {
 		dev_err(di->dev, "failed to register chargalg psy\n");
+		ret = PTR_ERR(di->chargalg_psy);
 		goto free_chargalg_wq;
 	}
 
@@ -2138,7 +2140,7 @@ static int abx500_chargalg_probe(struct platform_device *pdev)
 	return ret;
 
 free_psy:
-	power_supply_unregister(&di->chargalg_psy);
+	power_supply_unregister(di->chargalg_psy);
 free_chargalg_wq:
 	destroy_workqueue(di->chargalg_wq);
 	return ret;
diff --git a/drivers/power/apm_power.c b/drivers/power/apm_power.c
index 39763015b360..9d1a7fbcaed4 100644
--- a/drivers/power/apm_power.c
+++ b/drivers/power/apm_power.c
@@ -15,10 +15,10 @@
 #include <linux/apm-emulation.h>
 
 
-#define PSY_PROP(psy, prop, val) (psy->get_property(psy, \
+#define PSY_PROP(psy, prop, val) (power_supply_get_property(psy, \
 			 POWER_SUPPLY_PROP_##prop, val))
 
-#define _MPSY_PROP(prop, val) (main_battery->get_property(main_battery, \
+#define _MPSY_PROP(prop, val) (power_supply_get_property(main_battery, \
 							 prop, val))
 
 #define MPSY_PROP(prop, val) _MPSY_PROP(POWER_SUPPLY_PROP_##prop, val)
@@ -48,7 +48,7 @@ static int __find_main_battery(struct device *dev, void *data)
 
 	bp->bat = dev_get_drvdata(dev);
 
-	if (bp->bat->use_for_apm) {
+	if (bp->bat->desc->use_for_apm) {
 		/* nice, we explicitly asked to report this battery. */
 		bp->main = bp->bat;
 		return 1;
diff --git a/drivers/power/axp288_fuel_gauge.c b/drivers/power/axp288_fuel_gauge.c
new file mode 100644
index 000000000000..ca1cc5a47eb1
--- /dev/null
+++ b/drivers/power/axp288_fuel_gauge.c
@@ -0,0 +1,1154 @@
+/*
+ * axp288_fuel_gauge.c - Xpower AXP288 PMIC Fuel Gauge Driver
+ *
+ * Copyright (C) 2014 Intel Corporation
+ *
+ * ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License 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.
+ *
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/device.h>
+#include <linux/regmap.h>
+#include <linux/jiffies.h>
+#include <linux/interrupt.h>
+#include <linux/device.h>
+#include <linux/workqueue.h>
+#include <linux/mfd/axp20x.h>
+#include <linux/platform_device.h>
+#include <linux/power_supply.h>
+#include <linux/iio/consumer.h>
+#include <linux/debugfs.h>
+#include <linux/seq_file.h>
+
+#define CHRG_STAT_BAT_SAFE_MODE		(1 << 3)
+#define CHRG_STAT_BAT_VALID			(1 << 4)
+#define CHRG_STAT_BAT_PRESENT		(1 << 5)
+#define CHRG_STAT_CHARGING			(1 << 6)
+#define CHRG_STAT_PMIC_OTP			(1 << 7)
+
+#define CHRG_CCCV_CC_MASK			0xf     /* 4 bits */
+#define CHRG_CCCV_CC_BIT_POS		0
+#define CHRG_CCCV_CC_OFFSET			200     /* 200mA */
+#define CHRG_CCCV_CC_LSB_RES		200     /* 200mA */
+#define CHRG_CCCV_ITERM_20P			(1 << 4)    /* 20% of CC */
+#define CHRG_CCCV_CV_MASK			0x60        /* 2 bits */
+#define CHRG_CCCV_CV_BIT_POS		5
+#define CHRG_CCCV_CV_4100MV			0x0     /* 4.10V */
+#define CHRG_CCCV_CV_4150MV			0x1     /* 4.15V */
+#define CHRG_CCCV_CV_4200MV			0x2     /* 4.20V */
+#define CHRG_CCCV_CV_4350MV			0x3     /* 4.35V */
+#define CHRG_CCCV_CHG_EN			(1 << 7)
+
+#define CV_4100						4100    /* 4100mV */
+#define CV_4150						4150    /* 4150mV */
+#define CV_4200						4200    /* 4200mV */
+#define CV_4350						4350    /* 4350mV */
+
+#define TEMP_IRQ_CFG_QWBTU			(1 << 0)
+#define TEMP_IRQ_CFG_WBTU			(1 << 1)
+#define TEMP_IRQ_CFG_QWBTO			(1 << 2)
+#define TEMP_IRQ_CFG_WBTO			(1 << 3)
+#define TEMP_IRQ_CFG_MASK			0xf
+
+#define FG_IRQ_CFG_LOWBATT_WL2		(1 << 0)
+#define FG_IRQ_CFG_LOWBATT_WL1		(1 << 1)
+#define FG_IRQ_CFG_LOWBATT_MASK		0x3
+#define LOWBAT_IRQ_STAT_LOWBATT_WL2	(1 << 0)
+#define LOWBAT_IRQ_STAT_LOWBATT_WL1	(1 << 1)
+
+#define FG_CNTL_OCV_ADJ_STAT		(1 << 2)
+#define FG_CNTL_OCV_ADJ_EN			(1 << 3)
+#define FG_CNTL_CAP_ADJ_STAT		(1 << 4)
+#define FG_CNTL_CAP_ADJ_EN			(1 << 5)
+#define FG_CNTL_CC_EN				(1 << 6)
+#define FG_CNTL_GAUGE_EN			(1 << 7)
+
+#define FG_REP_CAP_VALID			(1 << 7)
+#define FG_REP_CAP_VAL_MASK			0x7F
+
+#define FG_DES_CAP1_VALID			(1 << 7)
+#define FG_DES_CAP1_VAL_MASK		0x7F
+#define FG_DES_CAP0_VAL_MASK		0xFF
+#define FG_DES_CAP_RES_LSB			1456    /* 1.456mAhr */
+
+#define FG_CC_MTR1_VALID			(1 << 7)
+#define FG_CC_MTR1_VAL_MASK			0x7F
+#define FG_CC_MTR0_VAL_MASK			0xFF
+#define FG_DES_CC_RES_LSB			1456    /* 1.456mAhr */
+
+#define FG_OCV_CAP_VALID			(1 << 7)
+#define FG_OCV_CAP_VAL_MASK			0x7F
+#define FG_CC_CAP_VALID				(1 << 7)
+#define FG_CC_CAP_VAL_MASK			0x7F
+
+#define FG_LOW_CAP_THR1_MASK		0xf0    /* 5% tp 20% */
+#define FG_LOW_CAP_THR1_VAL			0xa0    /* 15 perc */
+#define FG_LOW_CAP_THR2_MASK		0x0f    /* 0% to 15% */
+#define FG_LOW_CAP_WARN_THR			14  /* 14 perc */
+#define FG_LOW_CAP_CRIT_THR			4   /* 4 perc */
+#define FG_LOW_CAP_SHDN_THR			0   /* 0 perc */
+
+#define STATUS_MON_DELAY_JIFFIES    (HZ * 60)   /*60 sec */
+#define NR_RETRY_CNT    3
+#define DEV_NAME	"axp288_fuel_gauge"
+
+/* 1.1mV per LSB expressed in uV */
+#define VOLTAGE_FROM_ADC(a)			((a * 11) / 10)
+/* properties converted to tenths of degrees, uV, uA, uW */
+#define PROP_TEMP(a)		((a) * 10)
+#define UNPROP_TEMP(a)		((a) / 10)
+#define PROP_VOLT(a)		((a) * 1000)
+#define PROP_CURR(a)		((a) * 1000)
+
+#define AXP288_FG_INTR_NUM	6
+enum {
+	QWBTU_IRQ = 0,
+	WBTU_IRQ,
+	QWBTO_IRQ,
+	WBTO_IRQ,
+	WL2_IRQ,
+	WL1_IRQ,
+};
+
+struct axp288_fg_info {
+	struct platform_device *pdev;
+	struct axp20x_fg_pdata *pdata;
+	struct regmap *regmap;
+	struct regmap_irq_chip_data *regmap_irqc;
+	int irq[AXP288_FG_INTR_NUM];
+	struct power_supply *bat;
+	struct mutex lock;
+	int status;
+	struct delayed_work status_monitor;
+	struct dentry *debug_file;
+};
+
+static enum power_supply_property fuel_gauge_props[] = {
+	POWER_SUPPLY_PROP_STATUS,
+	POWER_SUPPLY_PROP_PRESENT,
+	POWER_SUPPLY_PROP_HEALTH,
+	POWER_SUPPLY_PROP_VOLTAGE_MAX_DESIGN,
+	POWER_SUPPLY_PROP_VOLTAGE_MIN_DESIGN,
+	POWER_SUPPLY_PROP_VOLTAGE_NOW,
+	POWER_SUPPLY_PROP_VOLTAGE_OCV,
+	POWER_SUPPLY_PROP_CURRENT_NOW,
+	POWER_SUPPLY_PROP_CAPACITY,
+	POWER_SUPPLY_PROP_CAPACITY_ALERT_MIN,
+	POWER_SUPPLY_PROP_TEMP,
+	POWER_SUPPLY_PROP_TEMP_MAX,
+	POWER_SUPPLY_PROP_TEMP_MIN,
+	POWER_SUPPLY_PROP_TEMP_ALERT_MIN,
+	POWER_SUPPLY_PROP_TEMP_ALERT_MAX,
+	POWER_SUPPLY_PROP_TECHNOLOGY,
+	POWER_SUPPLY_PROP_CHARGE_FULL,
+	POWER_SUPPLY_PROP_CHARGE_NOW,
+	POWER_SUPPLY_PROP_CHARGE_FULL_DESIGN,
+	POWER_SUPPLY_PROP_MODEL_NAME,
+};
+
+static int fuel_gauge_reg_readb(struct axp288_fg_info *info, int reg)
+{
+	int ret, i;
+	unsigned int val;
+
+	for (i = 0; i < NR_RETRY_CNT; i++) {
+		ret = regmap_read(info->regmap, reg, &val);
+		if (ret == -EBUSY)
+			continue;
+		else
+			break;
+	}
+
+	if (ret < 0)
+		dev_err(&info->pdev->dev, "axp288 reg read err:%d\n", ret);
+
+	return val;
+}
+
+static int fuel_gauge_reg_writeb(struct axp288_fg_info *info, int reg, u8 val)
+{
+	int ret;
+
+	ret = regmap_write(info->regmap, reg, (unsigned int)val);
+
+	if (ret < 0)
+		dev_err(&info->pdev->dev, "axp288 reg write err:%d\n", ret);
+
+	return ret;
+}
+
+static int pmic_read_adc_val(const char *name, int *raw_val,
+		struct axp288_fg_info *info)
+{
+	int ret, val = 0;
+	struct iio_channel *indio_chan;
+
+	indio_chan = iio_channel_get(NULL, name);
+	if (IS_ERR_OR_NULL(indio_chan)) {
+		ret = PTR_ERR(indio_chan);
+		goto exit;
+	}
+	ret = iio_read_channel_raw(indio_chan, &val);
+	if (ret < 0) {
+		dev_err(&info->pdev->dev,
+			"IIO channel read error: %x, %x\n", ret, val);
+		goto err_exit;
+	}
+
+	dev_dbg(&info->pdev->dev, "adc raw val=%x\n", val);
+	*raw_val = val;
+
+err_exit:
+	iio_channel_release(indio_chan);
+exit:
+	return ret;
+}
+
+#ifdef CONFIG_DEBUG_FS
+static int fuel_gauge_debug_show(struct seq_file *s, void *data)
+{
+	struct axp288_fg_info *info = s->private;
+	int raw_val, ret;
+
+	seq_printf(s, " PWR_STATUS[%02x] : %02x\n",
+		AXP20X_PWR_INPUT_STATUS,
+		fuel_gauge_reg_readb(info, AXP20X_PWR_INPUT_STATUS));
+	seq_printf(s, "PWR_OP_MODE[%02x] : %02x\n",
+		AXP20X_PWR_OP_MODE,
+		fuel_gauge_reg_readb(info, AXP20X_PWR_OP_MODE));
+	seq_printf(s, " CHRG_CTRL1[%02x] : %02x\n",
+		AXP20X_CHRG_CTRL1,
+		fuel_gauge_reg_readb(info, AXP20X_CHRG_CTRL1));
+	seq_printf(s, "       VLTF[%02x] : %02x\n",
+		AXP20X_V_LTF_DISCHRG,
+		fuel_gauge_reg_readb(info, AXP20X_V_LTF_DISCHRG));
+	seq_printf(s, "       VHTF[%02x] : %02x\n",
+		AXP20X_V_HTF_DISCHRG,
+		fuel_gauge_reg_readb(info, AXP20X_V_HTF_DISCHRG));
+	seq_printf(s, "    CC_CTRL[%02x] : %02x\n",
+		AXP20X_CC_CTRL,
+		fuel_gauge_reg_readb(info, AXP20X_CC_CTRL));
+	seq_printf(s, "BATTERY CAP[%02x] : %02x\n",
+		AXP20X_FG_RES,
+		fuel_gauge_reg_readb(info, AXP20X_FG_RES));
+	seq_printf(s, "    FG_RDC1[%02x] : %02x\n",
+		AXP288_FG_RDC1_REG,
+		fuel_gauge_reg_readb(info, AXP288_FG_RDC1_REG));
+	seq_printf(s, "    FG_RDC0[%02x] : %02x\n",
+		AXP288_FG_RDC0_REG,
+		fuel_gauge_reg_readb(info, AXP288_FG_RDC0_REG));
+	seq_printf(s, "    FG_OCVH[%02x] : %02x\n",
+		AXP288_FG_OCVH_REG,
+		fuel_gauge_reg_readb(info, AXP288_FG_OCVH_REG));
+	seq_printf(s, "    FG_OCVL[%02x] : %02x\n",
+		AXP288_FG_OCVL_REG,
+		fuel_gauge_reg_readb(info, AXP288_FG_OCVL_REG));
+	seq_printf(s, "FG_DES_CAP1[%02x] : %02x\n",
+		AXP288_FG_DES_CAP1_REG,
+		fuel_gauge_reg_readb(info, AXP288_FG_DES_CAP1_REG));
+	seq_printf(s, "FG_DES_CAP0[%02x] : %02x\n",
+		AXP288_FG_DES_CAP0_REG,
+		fuel_gauge_reg_readb(info, AXP288_FG_DES_CAP0_REG));
+	seq_printf(s, " FG_CC_MTR1[%02x] : %02x\n",
+		AXP288_FG_CC_MTR1_REG,
+		fuel_gauge_reg_readb(info, AXP288_FG_CC_MTR1_REG));
+	seq_printf(s, " FG_CC_MTR0[%02x] : %02x\n",
+		AXP288_FG_CC_MTR0_REG,
+		fuel_gauge_reg_readb(info, AXP288_FG_CC_MTR0_REG));
+	seq_printf(s, " FG_OCV_CAP[%02x] : %02x\n",
+		AXP288_FG_OCV_CAP_REG,
+		fuel_gauge_reg_readb(info, AXP288_FG_OCV_CAP_REG));
+	seq_printf(s, "  FG_CC_CAP[%02x] : %02x\n",
+		AXP288_FG_CC_CAP_REG,
+		fuel_gauge_reg_readb(info, AXP288_FG_CC_CAP_REG));
+	seq_printf(s, " FG_LOW_CAP[%02x] : %02x\n",
+		AXP288_FG_LOW_CAP_REG,
+		fuel_gauge_reg_readb(info, AXP288_FG_LOW_CAP_REG));
+	seq_printf(s, "TUNING_CTL0[%02x] : %02x\n",
+		AXP288_FG_TUNE0,
+		fuel_gauge_reg_readb(info, AXP288_FG_TUNE0));
+	seq_printf(s, "TUNING_CTL1[%02x] : %02x\n",
+		AXP288_FG_TUNE1,
+		fuel_gauge_reg_readb(info, AXP288_FG_TUNE1));
+	seq_printf(s, "TUNING_CTL2[%02x] : %02x\n",
+		AXP288_FG_TUNE2,
+		fuel_gauge_reg_readb(info, AXP288_FG_TUNE2));
+	seq_printf(s, "TUNING_CTL3[%02x] : %02x\n",
+		AXP288_FG_TUNE3,
+		fuel_gauge_reg_readb(info, AXP288_FG_TUNE3));
+	seq_printf(s, "TUNING_CTL4[%02x] : %02x\n",
+		AXP288_FG_TUNE4,
+		fuel_gauge_reg_readb(info, AXP288_FG_TUNE4));
+	seq_printf(s, "TUNING_CTL5[%02x] : %02x\n",
+		AXP288_FG_TUNE5,
+		fuel_gauge_reg_readb(info, AXP288_FG_TUNE5));
+
+	ret = pmic_read_adc_val("axp288-batt-temp", &raw_val, info);
+	if (ret >= 0)
+		seq_printf(s, "axp288-batttemp : %d\n", raw_val);
+	ret = pmic_read_adc_val("axp288-pmic-temp", &raw_val, info);
+	if (ret >= 0)
+		seq_printf(s, "axp288-pmictemp : %d\n", raw_val);
+	ret = pmic_read_adc_val("axp288-system-temp", &raw_val, info);
+	if (ret >= 0)
+		seq_printf(s, "axp288-systtemp : %d\n", raw_val);
+	ret = pmic_read_adc_val("axp288-chrg-curr", &raw_val, info);
+	if (ret >= 0)
+		seq_printf(s, "axp288-chrgcurr : %d\n", raw_val);
+	ret = pmic_read_adc_val("axp288-chrg-d-curr", &raw_val, info);
+	if (ret >= 0)
+		seq_printf(s, "axp288-dchrgcur : %d\n", raw_val);
+	ret = pmic_read_adc_val("axp288-batt-volt", &raw_val, info);
+	if (ret >= 0)
+		seq_printf(s, "axp288-battvolt : %d\n", raw_val);
+
+	return 0;
+}
+
+static int debug_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, fuel_gauge_debug_show, inode->i_private);
+}
+
+static const struct file_operations fg_debug_fops = {
+	.open       = debug_open,
+	.read       = seq_read,
+	.llseek     = seq_lseek,
+	.release    = single_release,
+};
+
+static void fuel_gauge_create_debugfs(struct axp288_fg_info *info)
+{
+	info->debug_file = debugfs_create_file("fuelgauge", 0666, NULL,
+		info, &fg_debug_fops);
+}
+
+static void fuel_gauge_remove_debugfs(struct axp288_fg_info *info)
+{
+	debugfs_remove(info->debug_file);
+}
+#else
+static inline void fuel_gauge_create_debugfs(struct axp288_fg_info *info)
+{
+}
+static inline void fuel_gauge_remove_debugfs(struct axp288_fg_info *info)
+{
+}
+#endif
+
+static void fuel_gauge_get_status(struct axp288_fg_info *info)
+{
+	int pwr_stat, ret;
+	int charge, discharge;
+
+	pwr_stat = fuel_gauge_reg_readb(info, AXP20X_PWR_INPUT_STATUS);
+	if (pwr_stat < 0) {
+		dev_err(&info->pdev->dev,
+			"PWR STAT read failed:%d\n", pwr_stat);
+		return;
+	}
+	ret = pmic_read_adc_val("axp288-chrg-curr", &charge, info);
+	if (ret < 0) {
+		dev_err(&info->pdev->dev,
+			"ADC charge current read failed:%d\n", ret);
+		return;
+	}
+	ret = pmic_read_adc_val("axp288-chrg-d-curr", &discharge, info);
+	if (ret < 0) {
+		dev_err(&info->pdev->dev,
+			"ADC discharge current read failed:%d\n", ret);
+		return;
+	}
+
+	if (charge > 0)
+		info->status = POWER_SUPPLY_STATUS_CHARGING;
+	else if (discharge > 0)
+		info->status = POWER_SUPPLY_STATUS_DISCHARGING;
+	else {
+		if (pwr_stat & CHRG_STAT_BAT_PRESENT)
+			info->status = POWER_SUPPLY_STATUS_FULL;
+		else
+			info->status = POWER_SUPPLY_STATUS_NOT_CHARGING;
+	}
+}
+
+static int fuel_gauge_get_vbatt(struct axp288_fg_info *info, int *vbatt)
+{
+	int ret = 0, raw_val;
+
+	ret = pmic_read_adc_val("axp288-batt-volt", &raw_val, info);
+	if (ret < 0)
+		goto vbatt_read_fail;
+
+	*vbatt = VOLTAGE_FROM_ADC(raw_val);
+vbatt_read_fail:
+	return ret;
+}
+
+static int fuel_gauge_get_current(struct axp288_fg_info *info, int *cur)
+{
+	int ret, value = 0;
+	int charge, discharge;
+
+	ret = pmic_read_adc_val("axp288-chrg-curr", &charge, info);
+	if (ret < 0)
+		goto current_read_fail;
+	ret = pmic_read_adc_val("axp288-chrg-d-curr", &discharge, info);
+	if (ret < 0)
+		goto current_read_fail;
+
+	if (charge > 0)
+		value = charge;
+	else if (discharge > 0)
+		value = -1 * discharge;
+
+	*cur = value;
+current_read_fail:
+	return ret;
+}
+
+static int temp_to_adc(struct axp288_fg_info *info, int tval)
+{
+	int rntc = 0, i, ret, adc_val;
+	int rmin, rmax, tmin, tmax;
+	int tcsz = info->pdata->tcsz;
+
+	/* get the Rntc resitance value for this temp */
+	if (tval > info->pdata->thermistor_curve[0][1]) {
+		rntc = info->pdata->thermistor_curve[0][0];
+	} else if (tval <= info->pdata->thermistor_curve[tcsz-1][1]) {
+		rntc = info->pdata->thermistor_curve[tcsz-1][0];
+	} else {
+		for (i = 1; i < tcsz; i++) {
+			if (tval > info->pdata->thermistor_curve[i][1]) {
+				rmin = info->pdata->thermistor_curve[i-1][0];
+				rmax = info->pdata->thermistor_curve[i][0];
+				tmin = info->pdata->thermistor_curve[i-1][1];
+				tmax = info->pdata->thermistor_curve[i][1];
+				rntc = rmin + ((rmax - rmin) *
+					(tval - tmin) / (tmax - tmin));
+				break;
+			}
+		}
+	}
+
+	/* we need the current to calculate the proper adc voltage */
+	ret = fuel_gauge_reg_readb(info, AXP20X_ADC_RATE);
+	if (ret < 0) {
+		dev_err(&info->pdev->dev, "%s:read err:%d\n", __func__, ret);
+		ret = 0x30;
+	}
+
+	/*
+	 * temperature is proportional to NTS thermistor resistance
+	 * ADC_RATE[5-4] determines current, 00=20uA,01=40uA,10=60uA,11=80uA
+	 * [12-bit ADC VAL] = R_NTC(Ω) * current / 800
+	 */
+	adc_val = rntc * (20 + (20 * ((ret >> 4) & 0x3))) / 800;
+
+	return adc_val;
+}
+
+static int adc_to_temp(struct axp288_fg_info *info, int adc_val)
+{
+	int ret, r, i, tval = 0;
+	int rmin, rmax, tmin, tmax;
+	int tcsz = info->pdata->tcsz;
+
+	ret = fuel_gauge_reg_readb(info, AXP20X_ADC_RATE);
+	if (ret < 0) {
+		dev_err(&info->pdev->dev, "%s:read err:%d\n", __func__, ret);
+		ret = 0x30;
+	}
+
+	/*
+	 * temperature is proportional to NTS thermistor resistance
+	 * ADC_RATE[5-4] determines current, 00=20uA,01=40uA,10=60uA,11=80uA
+	 * R_NTC(Ω) = [12-bit ADC VAL] * 800 / current
+	 */
+	r = adc_val * 800 / (20 + (20 * ((ret >> 4) & 0x3)));
+
+	if (r < info->pdata->thermistor_curve[0][0]) {
+		tval = info->pdata->thermistor_curve[0][1];
+	} else if (r >= info->pdata->thermistor_curve[tcsz-1][0]) {
+		tval = info->pdata->thermistor_curve[tcsz-1][1];
+	} else {
+		for (i = 1; i < tcsz; i++) {
+			if (r < info->pdata->thermistor_curve[i][0]) {
+				rmin = info->pdata->thermistor_curve[i-1][0];
+				rmax = info->pdata->thermistor_curve[i][0];
+				tmin = info->pdata->thermistor_curve[i-1][1];
+				tmax = info->pdata->thermistor_curve[i][1];
+				tval = tmin + ((tmax - tmin) *
+					(r - rmin) / (rmax - rmin));
+				break;
+			}
+		}
+	}
+
+	return tval;
+}
+
+static int fuel_gauge_get_btemp(struct axp288_fg_info *info, int *btemp)
+{
+	int ret, raw_val = 0;
+
+	ret = pmic_read_adc_val("axp288-batt-temp", &raw_val, info);
+	if (ret < 0)
+		goto temp_read_fail;
+
+	*btemp = adc_to_temp(info, raw_val);
+
+temp_read_fail:
+	return ret;
+}
+
+static int fuel_gauge_get_vocv(struct axp288_fg_info *info, int *vocv)
+{
+	int ret, value;
+
+	/* 12-bit data value, upper 8 in OCVH, lower 4 in OCVL */
+	ret = fuel_gauge_reg_readb(info, AXP288_FG_OCVH_REG);
+	if (ret < 0)
+		goto vocv_read_fail;
+	value = ret << 4;
+
+	ret = fuel_gauge_reg_readb(info, AXP288_FG_OCVL_REG);
+	if (ret < 0)
+		goto vocv_read_fail;
+	value |= (ret & 0xf);
+
+	*vocv = VOLTAGE_FROM_ADC(value);
+vocv_read_fail:
+	return ret;
+}
+
+static int fuel_gauge_battery_health(struct axp288_fg_info *info)
+{
+	int temp, vocv;
+	int ret, health = POWER_SUPPLY_HEALTH_UNKNOWN;
+
+	ret = fuel_gauge_get_btemp(info, &temp);
+	if (ret < 0)
+		goto health_read_fail;
+
+	ret = fuel_gauge_get_vocv(info, &vocv);
+	if (ret < 0)
+		goto health_read_fail;
+
+	if (vocv > info->pdata->max_volt)
+		health = POWER_SUPPLY_HEALTH_OVERVOLTAGE;
+	else if (temp > info->pdata->max_temp)
+		health = POWER_SUPPLY_HEALTH_OVERHEAT;
+	else if (temp < info->pdata->min_temp)
+		health = POWER_SUPPLY_HEALTH_COLD;
+	else if (vocv < info->pdata->min_volt)
+		health = POWER_SUPPLY_HEALTH_DEAD;
+	else
+		health = POWER_SUPPLY_HEALTH_GOOD;
+
+health_read_fail:
+	return health;
+}
+
+static int fuel_gauge_set_high_btemp_alert(struct axp288_fg_info *info)
+{
+	int ret, adc_val;
+
+	/* program temperature threshold as 1/16 ADC value */
+	adc_val = temp_to_adc(info, info->pdata->max_temp);
+	ret = fuel_gauge_reg_writeb(info, AXP20X_V_HTF_DISCHRG, adc_val >> 4);
+
+	return ret;
+}
+
+static int fuel_gauge_set_low_btemp_alert(struct axp288_fg_info *info)
+{
+	int ret, adc_val;
+
+	/* program temperature threshold as 1/16 ADC value */
+	adc_val = temp_to_adc(info, info->pdata->min_temp);
+	ret = fuel_gauge_reg_writeb(info, AXP20X_V_LTF_DISCHRG, adc_val >> 4);
+
+	return ret;
+}
+
+static int fuel_gauge_get_property(struct power_supply *ps,
+		enum power_supply_property prop,
+		union power_supply_propval *val)
+{
+	struct axp288_fg_info *info = power_supply_get_drvdata(ps);
+	int ret = 0, value;
+
+	mutex_lock(&info->lock);
+	switch (prop) {
+	case POWER_SUPPLY_PROP_STATUS:
+		fuel_gauge_get_status(info);
+		val->intval = info->status;
+		break;
+	case POWER_SUPPLY_PROP_HEALTH:
+		val->intval = fuel_gauge_battery_health(info);
+		break;
+	case POWER_SUPPLY_PROP_VOLTAGE_NOW:
+		ret = fuel_gauge_get_vbatt(info, &value);
+		if (ret < 0)
+			goto fuel_gauge_read_err;
+		val->intval = PROP_VOLT(value);
+		break;
+	case POWER_SUPPLY_PROP_VOLTAGE_OCV:
+		ret = fuel_gauge_get_vocv(info, &value);
+		if (ret < 0)
+			goto fuel_gauge_read_err;
+		val->intval = PROP_VOLT(value);
+		break;
+	case POWER_SUPPLY_PROP_CURRENT_NOW:
+		ret = fuel_gauge_get_current(info, &value);
+		if (ret < 0)
+			goto fuel_gauge_read_err;
+		val->intval = PROP_CURR(value);
+		break;
+	case POWER_SUPPLY_PROP_PRESENT:
+		ret = fuel_gauge_reg_readb(info, AXP20X_PWR_OP_MODE);
+		if (ret < 0)
+			goto fuel_gauge_read_err;
+
+		if (ret & CHRG_STAT_BAT_PRESENT)
+			val->intval = 1;
+		else
+			val->intval = 0;
+		break;
+	case POWER_SUPPLY_PROP_CAPACITY:
+		ret = fuel_gauge_reg_readb(info, AXP20X_FG_RES);
+		if (ret < 0)
+			goto fuel_gauge_read_err;
+
+		if (!(ret & FG_REP_CAP_VALID))
+			dev_err(&info->pdev->dev,
+				"capacity measurement not valid\n");
+		val->intval = (ret & FG_REP_CAP_VAL_MASK);
+		break;
+	case POWER_SUPPLY_PROP_CAPACITY_ALERT_MIN:
+		ret = fuel_gauge_reg_readb(info, AXP288_FG_LOW_CAP_REG);
+		if (ret < 0)
+			goto fuel_gauge_read_err;
+		val->intval = (ret & 0x0f);
+		break;
+	case POWER_SUPPLY_PROP_TEMP:
+		ret = fuel_gauge_get_btemp(info, &value);
+		if (ret < 0)
+			goto fuel_gauge_read_err;
+		val->intval = PROP_TEMP(value);
+		break;
+	case POWER_SUPPLY_PROP_TEMP_MAX:
+	case POWER_SUPPLY_PROP_TEMP_ALERT_MAX:
+		val->intval = PROP_TEMP(info->pdata->max_temp);
+		break;
+	case POWER_SUPPLY_PROP_TEMP_MIN:
+	case POWER_SUPPLY_PROP_TEMP_ALERT_MIN:
+		val->intval = PROP_TEMP(info->pdata->min_temp);
+		break;
+	case POWER_SUPPLY_PROP_TECHNOLOGY:
+		val->intval = POWER_SUPPLY_TECHNOLOGY_LION;
+		break;
+	case POWER_SUPPLY_PROP_CHARGE_NOW:
+		ret = fuel_gauge_reg_readb(info, AXP288_FG_CC_MTR1_REG);
+		if (ret < 0)
+			goto fuel_gauge_read_err;
+
+		value = (ret & FG_CC_MTR1_VAL_MASK) << 8;
+		ret = fuel_gauge_reg_readb(info, AXP288_FG_CC_MTR0_REG);
+		if (ret < 0)
+			goto fuel_gauge_read_err;
+		value |= (ret & FG_CC_MTR0_VAL_MASK);
+		val->intval = value * FG_DES_CAP_RES_LSB;
+		break;
+	case POWER_SUPPLY_PROP_CHARGE_FULL:
+		ret = fuel_gauge_reg_readb(info, AXP288_FG_DES_CAP1_REG);
+		if (ret < 0)
+			goto fuel_gauge_read_err;
+
+		value = (ret & FG_DES_CAP1_VAL_MASK) << 8;
+		ret = fuel_gauge_reg_readb(info, AXP288_FG_DES_CAP0_REG);
+		if (ret < 0)
+			goto fuel_gauge_read_err;
+		value |= (ret & FG_DES_CAP0_VAL_MASK);
+		val->intval = value * FG_DES_CAP_RES_LSB;
+		break;
+	case POWER_SUPPLY_PROP_CHARGE_FULL_DESIGN:
+		val->intval = PROP_CURR(info->pdata->design_cap);
+		break;
+	case POWER_SUPPLY_PROP_VOLTAGE_MAX_DESIGN:
+		val->intval = PROP_VOLT(info->pdata->max_volt);
+		break;
+	case POWER_SUPPLY_PROP_VOLTAGE_MIN_DESIGN:
+		val->intval = PROP_VOLT(info->pdata->min_volt);
+		break;
+	case POWER_SUPPLY_PROP_MODEL_NAME:
+		val->strval = info->pdata->battid;
+		break;
+	default:
+		mutex_unlock(&info->lock);
+		return -EINVAL;
+	}
+
+	mutex_unlock(&info->lock);
+	return 0;
+
+fuel_gauge_read_err:
+	mutex_unlock(&info->lock);
+	return ret;
+}
+
+static int fuel_gauge_set_property(struct power_supply *ps,
+		enum power_supply_property prop,
+		const union power_supply_propval *val)
+{
+	struct axp288_fg_info *info = power_supply_get_drvdata(ps);
+	int ret = 0;
+
+	mutex_lock(&info->lock);
+	switch (prop) {
+	case POWER_SUPPLY_PROP_STATUS:
+		info->status = val->intval;
+		break;
+	case POWER_SUPPLY_PROP_TEMP_MIN:
+	case POWER_SUPPLY_PROP_TEMP_ALERT_MIN:
+		if ((val->intval < PD_DEF_MIN_TEMP) ||
+			(val->intval > PD_DEF_MAX_TEMP)) {
+			ret = -EINVAL;
+			break;
+		}
+		info->pdata->min_temp = UNPROP_TEMP(val->intval);
+		ret = fuel_gauge_set_low_btemp_alert(info);
+		if (ret < 0)
+			dev_err(&info->pdev->dev,
+				"temp alert min set fail:%d\n", ret);
+		break;
+	case POWER_SUPPLY_PROP_TEMP_MAX:
+	case POWER_SUPPLY_PROP_TEMP_ALERT_MAX:
+		if ((val->intval < PD_DEF_MIN_TEMP) ||
+			(val->intval > PD_DEF_MAX_TEMP)) {
+			ret = -EINVAL;
+			break;
+		}
+		info->pdata->max_temp = UNPROP_TEMP(val->intval);
+		ret = fuel_gauge_set_high_btemp_alert(info);
+		if (ret < 0)
+			dev_err(&info->pdev->dev,
+				"temp alert max set fail:%d\n", ret);
+		break;
+	case POWER_SUPPLY_PROP_CAPACITY_ALERT_MIN:
+		if ((val->intval < 0) || (val->intval > 15)) {
+			ret = -EINVAL;
+			break;
+		}
+		ret = fuel_gauge_reg_readb(info, AXP288_FG_LOW_CAP_REG);
+		if (ret < 0)
+			break;
+		ret &= 0xf0;
+		ret |= (val->intval & 0xf);
+		ret = fuel_gauge_reg_writeb(info, AXP288_FG_LOW_CAP_REG, ret);
+		break;
+	default:
+		ret = -EINVAL;
+		break;
+	}
+
+	mutex_unlock(&info->lock);
+	return ret;
+}
+
+static int fuel_gauge_property_is_writeable(struct power_supply *psy,
+	enum power_supply_property psp)
+{
+	int ret;
+
+	switch (psp) {
+	case POWER_SUPPLY_PROP_STATUS:
+	case POWER_SUPPLY_PROP_TEMP_MIN:
+	case POWER_SUPPLY_PROP_TEMP_ALERT_MIN:
+	case POWER_SUPPLY_PROP_TEMP_MAX:
+	case POWER_SUPPLY_PROP_TEMP_ALERT_MAX:
+	case POWER_SUPPLY_PROP_CAPACITY_ALERT_MIN:
+		ret = 1;
+		break;
+	default:
+		ret = 0;
+	}
+
+	return ret;
+}
+
+static void fuel_gauge_status_monitor(struct work_struct *work)
+{
+	struct axp288_fg_info *info = container_of(work,
+		struct axp288_fg_info, status_monitor.work);
+
+	fuel_gauge_get_status(info);
+	power_supply_changed(info->bat);
+	schedule_delayed_work(&info->status_monitor, STATUS_MON_DELAY_JIFFIES);
+}
+
+static irqreturn_t fuel_gauge_thread_handler(int irq, void *dev)
+{
+	struct axp288_fg_info *info = dev;
+	int i;
+
+	for (i = 0; i < AXP288_FG_INTR_NUM; i++) {
+		if (info->irq[i] == irq)
+			break;
+	}
+
+	if (i >= AXP288_FG_INTR_NUM) {
+		dev_warn(&info->pdev->dev, "spurious interrupt!!\n");
+		return IRQ_NONE;
+	}
+
+	switch (i) {
+	case QWBTU_IRQ:
+		dev_info(&info->pdev->dev,
+			"Quit Battery under temperature in work mode IRQ (QWBTU)\n");
+		break;
+	case WBTU_IRQ:
+		dev_info(&info->pdev->dev,
+			"Battery under temperature in work mode IRQ (WBTU)\n");
+		break;
+	case QWBTO_IRQ:
+		dev_info(&info->pdev->dev,
+			"Quit Battery over temperature in work mode IRQ (QWBTO)\n");
+		break;
+	case WBTO_IRQ:
+		dev_info(&info->pdev->dev,
+			"Battery over temperature in work mode IRQ (WBTO)\n");
+		break;
+	case WL2_IRQ:
+		dev_info(&info->pdev->dev, "Low Batt Warning(2) INTR\n");
+		break;
+	case WL1_IRQ:
+		dev_info(&info->pdev->dev, "Low Batt Warning(1) INTR\n");
+		break;
+	default:
+		dev_warn(&info->pdev->dev, "Spurious Interrupt!!!\n");
+	}
+
+	power_supply_changed(info->bat);
+	return IRQ_HANDLED;
+}
+
+static void fuel_gauge_external_power_changed(struct power_supply *psy)
+{
+	struct axp288_fg_info *info = power_supply_get_drvdata(psy);
+
+	power_supply_changed(info->bat);
+}
+
+static const struct power_supply_desc fuel_gauge_desc = {
+	.name			= DEV_NAME,
+	.type			= POWER_SUPPLY_TYPE_BATTERY,
+	.properties		= fuel_gauge_props,
+	.num_properties		= ARRAY_SIZE(fuel_gauge_props),
+	.get_property		= fuel_gauge_get_property,
+	.set_property		= fuel_gauge_set_property,
+	.property_is_writeable	= fuel_gauge_property_is_writeable,
+	.external_power_changed	= fuel_gauge_external_power_changed,
+};
+
+static int fuel_gauge_set_lowbatt_thresholds(struct axp288_fg_info *info)
+{
+	int ret;
+	u8 reg_val;
+
+	ret = fuel_gauge_reg_readb(info, AXP20X_FG_RES);
+	if (ret < 0) {
+		dev_err(&info->pdev->dev, "%s:read err:%d\n", __func__, ret);
+		return ret;
+	}
+	ret = (ret & FG_REP_CAP_VAL_MASK);
+
+	if (ret > FG_LOW_CAP_WARN_THR)
+		reg_val = FG_LOW_CAP_WARN_THR;
+	else if (ret > FG_LOW_CAP_CRIT_THR)
+		reg_val = FG_LOW_CAP_CRIT_THR;
+	else
+		reg_val = FG_LOW_CAP_SHDN_THR;
+
+	reg_val |= FG_LOW_CAP_THR1_VAL;
+	ret = fuel_gauge_reg_writeb(info, AXP288_FG_LOW_CAP_REG, reg_val);
+	if (ret < 0)
+		dev_err(&info->pdev->dev, "%s:write err:%d\n", __func__, ret);
+
+	return ret;
+}
+
+static int fuel_gauge_program_vbatt_full(struct axp288_fg_info *info)
+{
+	int ret;
+	u8 val;
+
+	ret = fuel_gauge_reg_readb(info, AXP20X_CHRG_CTRL1);
+	if (ret < 0)
+		goto fg_prog_ocv_fail;
+	else
+		val = (ret & ~CHRG_CCCV_CV_MASK);
+
+	switch (info->pdata->max_volt) {
+	case CV_4100:
+		val |= (CHRG_CCCV_CV_4100MV << CHRG_CCCV_CV_BIT_POS);
+		break;
+	case CV_4150:
+		val |= (CHRG_CCCV_CV_4150MV << CHRG_CCCV_CV_BIT_POS);
+		break;
+	case CV_4200:
+		val |= (CHRG_CCCV_CV_4200MV << CHRG_CCCV_CV_BIT_POS);
+		break;
+	case CV_4350:
+		val |= (CHRG_CCCV_CV_4350MV << CHRG_CCCV_CV_BIT_POS);
+		break;
+	default:
+		val |= (CHRG_CCCV_CV_4200MV << CHRG_CCCV_CV_BIT_POS);
+		break;
+	}
+
+	ret = fuel_gauge_reg_writeb(info, AXP20X_CHRG_CTRL1, val);
+fg_prog_ocv_fail:
+	return ret;
+}
+
+static int fuel_gauge_program_design_cap(struct axp288_fg_info *info)
+{
+	int ret;
+
+	ret = fuel_gauge_reg_writeb(info,
+		AXP288_FG_DES_CAP1_REG, info->pdata->cap1);
+	if (ret < 0)
+		goto fg_prog_descap_fail;
+
+	ret = fuel_gauge_reg_writeb(info,
+		AXP288_FG_DES_CAP0_REG, info->pdata->cap0);
+
+fg_prog_descap_fail:
+	return ret;
+}
+
+static int fuel_gauge_program_ocv_curve(struct axp288_fg_info *info)
+{
+	int ret = 0, i;
+
+	for (i = 0; i < OCV_CURVE_SIZE; i++) {
+		ret = fuel_gauge_reg_writeb(info,
+			AXP288_FG_OCV_CURVE_REG + i, info->pdata->ocv_curve[i]);
+		if (ret < 0)
+			goto fg_prog_ocv_fail;
+	}
+
+fg_prog_ocv_fail:
+	return ret;
+}
+
+static int fuel_gauge_program_rdc_vals(struct axp288_fg_info *info)
+{
+	int ret;
+
+	ret = fuel_gauge_reg_writeb(info,
+		AXP288_FG_RDC1_REG, info->pdata->rdc1);
+	if (ret < 0)
+		goto fg_prog_ocv_fail;
+
+	ret = fuel_gauge_reg_writeb(info,
+		AXP288_FG_RDC0_REG, info->pdata->rdc0);
+
+fg_prog_ocv_fail:
+	return ret;
+}
+
+static void fuel_gauge_init_config_regs(struct axp288_fg_info *info)
+{
+	int ret;
+
+	/*
+	 * check if the config data is already
+	 * programmed and if so just return.
+	 */
+
+	ret = fuel_gauge_reg_readb(info, AXP288_FG_DES_CAP1_REG);
+	if (ret < 0) {
+		dev_warn(&info->pdev->dev, "CAP1 reg read err!!\n");
+	} else if (!(ret & FG_DES_CAP1_VALID)) {
+		dev_info(&info->pdev->dev, "FG data needs to be initialized\n");
+	} else {
+		dev_info(&info->pdev->dev, "FG data is already initialized\n");
+		return;
+	}
+
+	ret = fuel_gauge_program_vbatt_full(info);
+	if (ret < 0)
+		dev_err(&info->pdev->dev, "set vbatt full fail:%d\n", ret);
+
+	ret = fuel_gauge_program_design_cap(info);
+	if (ret < 0)
+		dev_err(&info->pdev->dev, "set design cap fail:%d\n", ret);
+
+	ret = fuel_gauge_program_rdc_vals(info);
+	if (ret < 0)
+		dev_err(&info->pdev->dev, "set rdc fail:%d\n", ret);
+
+	ret = fuel_gauge_program_ocv_curve(info);
+	if (ret < 0)
+		dev_err(&info->pdev->dev, "set ocv curve fail:%d\n", ret);
+
+	ret = fuel_gauge_set_lowbatt_thresholds(info);
+	if (ret < 0)
+		dev_err(&info->pdev->dev, "lowbatt thr set fail:%d\n", ret);
+
+	ret = fuel_gauge_reg_writeb(info, AXP20X_CC_CTRL, 0xef);
+	if (ret < 0)
+		dev_err(&info->pdev->dev, "gauge cntl set fail:%d\n", ret);
+}
+
+static void fuel_gauge_init_irq(struct axp288_fg_info *info)
+{
+	int ret, i, pirq;
+
+	for (i = 0; i < AXP288_FG_INTR_NUM; i++) {
+		pirq = platform_get_irq(info->pdev, i);
+		info->irq[i] = regmap_irq_get_virq(info->regmap_irqc, pirq);
+		if (info->irq[i] < 0) {
+			dev_warn(&info->pdev->dev,
+				"regmap_irq get virq failed for IRQ %d: %d\n",
+				pirq, info->irq[i]);
+			info->irq[i] = -1;
+			goto intr_failed;
+		}
+		ret = request_threaded_irq(info->irq[i],
+				NULL, fuel_gauge_thread_handler,
+				IRQF_ONESHOT, DEV_NAME, info);
+		if (ret) {
+			dev_warn(&info->pdev->dev,
+				"request irq failed for IRQ %d: %d\n",
+				pirq, info->irq[i]);
+			info->irq[i] = -1;
+			goto intr_failed;
+		} else {
+			dev_info(&info->pdev->dev, "HW IRQ %d -> VIRQ %d\n",
+				pirq, info->irq[i]);
+		}
+	}
+	return;
+
+intr_failed:
+	for (; i > 0; i--) {
+		free_irq(info->irq[i - 1], info);
+		info->irq[i - 1] = -1;
+	}
+}
+
+static void fuel_gauge_init_hw_regs(struct axp288_fg_info *info)
+{
+	int ret;
+	unsigned int val;
+
+	ret = fuel_gauge_set_high_btemp_alert(info);
+	if (ret < 0)
+		dev_err(&info->pdev->dev, "high batt temp set fail:%d\n", ret);
+
+	ret = fuel_gauge_set_low_btemp_alert(info);
+	if (ret < 0)
+		dev_err(&info->pdev->dev, "low batt temp set fail:%d\n", ret);
+
+	/* enable interrupts */
+	val = fuel_gauge_reg_readb(info, AXP20X_IRQ3_EN);
+	val |= TEMP_IRQ_CFG_MASK;
+	fuel_gauge_reg_writeb(info, AXP20X_IRQ3_EN, val);
+
+	val = fuel_gauge_reg_readb(info, AXP20X_IRQ4_EN);
+	val |= FG_IRQ_CFG_LOWBATT_MASK;
+	val = fuel_gauge_reg_writeb(info, AXP20X_IRQ4_EN, val);
+}
+
+static int axp288_fuel_gauge_probe(struct platform_device *pdev)
+{
+	int ret = 0;
+	struct axp288_fg_info *info;
+	struct axp20x_dev *axp20x = dev_get_drvdata(pdev->dev.parent);
+	struct power_supply_config psy_cfg = {};
+
+	info = devm_kzalloc(&pdev->dev, sizeof(*info), GFP_KERNEL);
+	if (!info)
+		return -ENOMEM;
+
+	info->pdev = pdev;
+	info->regmap = axp20x->regmap;
+	info->regmap_irqc = axp20x->regmap_irqc;
+	info->status = POWER_SUPPLY_STATUS_UNKNOWN;
+	info->pdata = pdev->dev.platform_data;
+	if (!info->pdata)
+		return -ENODEV;
+
+	platform_set_drvdata(pdev, info);
+
+	mutex_init(&info->lock);
+	INIT_DELAYED_WORK(&info->status_monitor, fuel_gauge_status_monitor);
+
+	psy_cfg.drv_data = info;
+	info->bat = power_supply_register(&pdev->dev, &fuel_gauge_desc, &psy_cfg);
+	if (IS_ERR(info->bat)) {
+		ret = PTR_ERR(info->bat);
+		dev_err(&pdev->dev, "failed to register battery: %d\n", ret);
+		return ret;
+	}
+
+	fuel_gauge_create_debugfs(info);
+	fuel_gauge_init_config_regs(info);
+	fuel_gauge_init_irq(info);
+	fuel_gauge_init_hw_regs(info);
+	schedule_delayed_work(&info->status_monitor, STATUS_MON_DELAY_JIFFIES);
+
+	return ret;
+}
+
+static struct platform_device_id axp288_fg_id_table[] = {
+	{ .name = DEV_NAME },
+	{},
+};
+
+static int axp288_fuel_gauge_remove(struct platform_device *pdev)
+{
+	struct axp288_fg_info *info = platform_get_drvdata(pdev);
+	int i;
+
+	cancel_delayed_work_sync(&info->status_monitor);
+	power_supply_unregister(info->bat);
+	fuel_gauge_remove_debugfs(info);
+
+	for (i = 0; i < AXP288_FG_INTR_NUM; i++)
+		if (info->irq[i] >= 0)
+			free_irq(info->irq[i], info);
+
+	return 0;
+}
+
+static struct platform_driver axp288_fuel_gauge_driver = {
+	.probe = axp288_fuel_gauge_probe,
+	.remove = axp288_fuel_gauge_remove,
+	.id_table = axp288_fg_id_table,
+	.driver = {
+		.name = DEV_NAME,
+	},
+};
+
+module_platform_driver(axp288_fuel_gauge_driver);
+
+MODULE_AUTHOR("Todd Brandt <todd.e.brandt@linux.intel.com>");
+MODULE_DESCRIPTION("Xpower AXP288 Fuel Gauge Driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/power/bq2415x_charger.c b/drivers/power/bq2415x_charger.c
index 1f49986fc605..6c534dcbc19c 100644
--- a/drivers/power/bq2415x_charger.c
+++ b/drivers/power/bq2415x_charger.c
@@ -13,12 +13,6 @@
  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  * GNU General Public License for more details.
  *
- * You should have received a copy of the GNU General Public License along
- * with this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
- */
-
-/*
  * Datasheets:
  * http://www.ti.com/product/bq24150
  * http://www.ti.com/product/bq24150a
@@ -26,6 +20,8 @@
  * http://www.ti.com/product/bq24153
  * http://www.ti.com/product/bq24153a
  * http://www.ti.com/product/bq24155
+ * http://www.ti.com/product/bq24157s
+ * http://www.ti.com/product/bq24158
  */
 
 #include <linux/kernel.h>
@@ -147,6 +143,7 @@ enum bq2415x_chip {
 	BQ24155,
 	BQ24156,
 	BQ24156A,
+	BQ24157S,
 	BQ24158,
 };
 
@@ -162,18 +159,20 @@ static char *bq2415x_chip_name[] = {
 	"bq24155",
 	"bq24156",
 	"bq24156a",
+	"bq24157s",
 	"bq24158",
 };
 
 struct bq2415x_device {
 	struct device *dev;
 	struct bq2415x_platform_data init_data;
-	struct power_supply charger;
+	struct power_supply *charger;
+	struct power_supply_desc charger_desc;
 	struct delayed_work work;
 	struct power_supply *notify_psy;
 	struct notifier_block nb;
 	enum bq2415x_mode reported_mode;/* mode reported by hook function */
-	enum bq2415x_mode mode;		/* current configured mode */
+	enum bq2415x_mode mode;		/* currently configured mode */
 	enum bq2415x_chip chip;
 	const char *timer_error;
 	char *model;
@@ -352,8 +351,7 @@ static int bq2415x_exec_command(struct bq2415x_device *bq,
 			BQ2415X_BIT_CE);
 		if (ret < 0)
 			return ret;
-		else
-			return ret > 0 ? 0 : 1;
+		return ret > 0 ? 0 : 1;
 	case BQ2415X_CHARGER_ENABLE:
 		return bq2415x_i2c_write_bit(bq, BQ2415X_REG_CONTROL,
 				0, BQ2415X_BIT_CE);
@@ -426,20 +424,17 @@ static enum bq2415x_chip bq2415x_detect_chip(struct bq2415x_device *bq)
 		case 0:
 			if (bq->chip == BQ24151A)
 				return bq->chip;
-			else
-				return BQ24151;
+			return BQ24151;
 		case 1:
 			if (bq->chip == BQ24150A ||
 				bq->chip == BQ24152 ||
 				bq->chip == BQ24155)
 				return bq->chip;
-			else
-				return BQ24150;
+			return BQ24150;
 		case 2:
 			if (bq->chip == BQ24153A)
 				return bq->chip;
-			else
-				return BQ24153;
+			return BQ24153;
 		default:
 			return BQUNKNOWN;
 		}
@@ -450,9 +445,10 @@ static enum bq2415x_chip bq2415x_detect_chip(struct bq2415x_device *bq)
 		case 0:
 			if (bq->chip == BQ24156A)
 				return bq->chip;
-			else
-				return BQ24156;
+			return BQ24156;
 		case 2:
+			if (bq->chip == BQ24157S)
+				return bq->chip;
 			return BQ24158;
 		default:
 			return BQUNKNOWN;
@@ -480,24 +476,22 @@ static int bq2415x_detect_revision(struct bq2415x_device *bq)
 	case BQ24152:
 		if (ret >= 0 && ret <= 3)
 			return ret;
-		else
-			return -1;
+		return -1;
 	case BQ24153:
 	case BQ24153A:
 	case BQ24156:
 	case BQ24156A:
+	case BQ24157S:
 	case BQ24158:
 		if (ret == 3)
 			return 0;
 		else if (ret == 1)
 			return 1;
-		else
-			return -1;
+		return -1;
 	case BQ24155:
 		if (ret == 3)
 			return 3;
-		else
-			return -1;
+		return -1;
 	case BQUNKNOWN:
 		return -1;
 	}
@@ -791,7 +785,7 @@ static int bq2415x_set_mode(struct bq2415x_device *bq, enum bq2415x_mode mode)
 	bq2415x_set_default_value(bq, battery_regulation_voltage);
 
 	bq->mode = mode;
-	sysfs_notify(&bq->charger.dev->kobj, NULL, "mode");
+	sysfs_notify(&bq->charger->dev.kobj, NULL, "mode");
 
 	return 0;
 
@@ -816,7 +810,8 @@ static int bq2415x_notifier_call(struct notifier_block *nb,
 
 	dev_dbg(bq->dev, "notifier call was called\n");
 
-	ret = psy->get_property(psy, POWER_SUPPLY_PROP_CURRENT_MAX, &prop);
+	ret = power_supply_get_property(psy, POWER_SUPPLY_PROP_CURRENT_MAX,
+			&prop);
 	if (ret != 0)
 		return NOTIFY_OK;
 
@@ -874,7 +869,7 @@ static void bq2415x_set_autotimer(struct bq2415x_device *bq, int state)
 static void bq2415x_timer_error(struct bq2415x_device *bq, const char *msg)
 {
 	bq->timer_error = msg;
-	sysfs_notify(&bq->charger.dev->kobj, NULL, "timer");
+	sysfs_notify(&bq->charger->dev.kobj, NULL, "timer");
 	dev_err(bq->dev, "%s\n", msg);
 	if (bq->automode > 0)
 		bq->automode = 0;
@@ -892,7 +887,7 @@ static void bq2415x_timer_work(struct work_struct *work)
 	int boost;
 
 	if (bq->automode > 0 && (bq->reported_mode != bq->mode)) {
-		sysfs_notify(&bq->charger.dev->kobj, NULL, "reported_mode");
+		sysfs_notify(&bq->charger->dev.kobj, NULL, "reported_mode");
 		bq2415x_set_mode(bq, bq->reported_mode);
 	}
 
@@ -998,8 +993,7 @@ static int bq2415x_power_supply_get_property(struct power_supply *psy,
 					     enum power_supply_property psp,
 					     union power_supply_propval *val)
 {
-	struct bq2415x_device *bq = container_of(psy, struct bq2415x_device,
-						 charger);
+	struct bq2415x_device *bq = power_supply_get_drvdata(psy);
 	int ret;
 
 	switch (psp) {
@@ -1030,12 +1024,14 @@ static int bq2415x_power_supply_init(struct bq2415x_device *bq)
 	int ret;
 	int chip;
 	char revstr[8];
+	struct power_supply_config psy_cfg = { .drv_data = bq, };
 
-	bq->charger.name = bq->name;
-	bq->charger.type = POWER_SUPPLY_TYPE_USB;
-	bq->charger.properties = bq2415x_power_supply_props;
-	bq->charger.num_properties = ARRAY_SIZE(bq2415x_power_supply_props);
-	bq->charger.get_property = bq2415x_power_supply_get_property;
+	bq->charger_desc.name = bq->name;
+	bq->charger_desc.type = POWER_SUPPLY_TYPE_USB;
+	bq->charger_desc.properties = bq2415x_power_supply_props;
+	bq->charger_desc.num_properties =
+			ARRAY_SIZE(bq2415x_power_supply_props);
+	bq->charger_desc.get_property = bq2415x_power_supply_get_property;
 
 	ret = bq2415x_detect_chip(bq);
 	if (ret < 0)
@@ -1058,10 +1054,11 @@ static int bq2415x_power_supply_init(struct bq2415x_device *bq)
 		return -ENOMEM;
 	}
 
-	ret = power_supply_register(bq->dev, &bq->charger);
-	if (ret) {
+	bq->charger = power_supply_register(bq->dev, &bq->charger_desc,
+					    &psy_cfg);
+	if (IS_ERR(bq->charger)) {
 		kfree(bq->model);
-		return ret;
+		return PTR_ERR(bq->charger);
 	}
 
 	return 0;
@@ -1073,7 +1070,7 @@ static void bq2415x_power_supply_exit(struct bq2415x_device *bq)
 	if (bq->automode > 0)
 		bq->automode = 0;
 	cancel_delayed_work_sync(&bq->work);
-	power_supply_unregister(&bq->charger);
+	power_supply_unregister(bq->charger);
 	kfree(bq->model);
 }
 
@@ -1085,8 +1082,7 @@ static ssize_t bq2415x_sysfs_show_status(struct device *dev,
 					 char *buf)
 {
 	struct power_supply *psy = dev_get_drvdata(dev);
-	struct bq2415x_device *bq = container_of(psy, struct bq2415x_device,
-						charger);
+	struct bq2415x_device *bq = power_supply_get_drvdata(psy);
 	enum bq2415x_command command;
 	int ret;
 
@@ -1119,8 +1115,7 @@ static ssize_t bq2415x_sysfs_set_timer(struct device *dev,
 				       size_t count)
 {
 	struct power_supply *psy = dev_get_drvdata(dev);
-	struct bq2415x_device *bq = container_of(psy, struct bq2415x_device,
-						charger);
+	struct bq2415x_device *bq = power_supply_get_drvdata(psy);
 	int ret = 0;
 
 	if (strncmp(buf, "auto", 4) == 0)
@@ -1141,8 +1136,7 @@ static ssize_t bq2415x_sysfs_show_timer(struct device *dev,
 					char *buf)
 {
 	struct power_supply *psy = dev_get_drvdata(dev);
-	struct bq2415x_device *bq = container_of(psy, struct bq2415x_device,
-						 charger);
+	struct bq2415x_device *bq = power_supply_get_drvdata(psy);
 
 	if (bq->timer_error)
 		return sprintf(buf, "%s\n", bq->timer_error);
@@ -1166,8 +1160,7 @@ static ssize_t bq2415x_sysfs_set_mode(struct device *dev,
 				      size_t count)
 {
 	struct power_supply *psy = dev_get_drvdata(dev);
-	struct bq2415x_device *bq = container_of(psy, struct bq2415x_device,
-						 charger);
+	struct bq2415x_device *bq = power_supply_get_drvdata(psy);
 	enum bq2415x_mode mode;
 	int ret = 0;
 
@@ -1219,8 +1212,7 @@ static ssize_t bq2415x_sysfs_show_mode(struct device *dev,
 				       char *buf)
 {
 	struct power_supply *psy = dev_get_drvdata(dev);
-	struct bq2415x_device *bq = container_of(psy, struct bq2415x_device,
-						charger);
+	struct bq2415x_device *bq = power_supply_get_drvdata(psy);
 	ssize_t ret = 0;
 
 	if (bq->automode > 0)
@@ -1257,8 +1249,7 @@ static ssize_t bq2415x_sysfs_show_reported_mode(struct device *dev,
 						char *buf)
 {
 	struct power_supply *psy = dev_get_drvdata(dev);
-	struct bq2415x_device *bq = container_of(psy, struct bq2415x_device,
-						 charger);
+	struct bq2415x_device *bq = power_supply_get_drvdata(psy);
 
 	if (bq->automode < 0)
 		return -EINVAL;
@@ -1286,8 +1277,7 @@ static ssize_t bq2415x_sysfs_set_registers(struct device *dev,
 					   size_t count)
 {
 	struct power_supply *psy = dev_get_drvdata(dev);
-	struct bq2415x_device *bq = container_of(psy, struct bq2415x_device,
-						 charger);
+	struct bq2415x_device *bq = power_supply_get_drvdata(psy);
 	ssize_t ret = 0;
 	unsigned int reg;
 	unsigned int val;
@@ -1322,8 +1312,7 @@ static ssize_t bq2415x_sysfs_show_registers(struct device *dev,
 					    char *buf)
 {
 	struct power_supply *psy = dev_get_drvdata(dev);
-	struct bq2415x_device *bq = container_of(psy, struct bq2415x_device,
-						 charger);
+	struct bq2415x_device *bq = power_supply_get_drvdata(psy);
 	ssize_t ret = 0;
 
 	ret += bq2415x_sysfs_print_reg(bq, BQ2415X_REG_STATUS, buf+ret);
@@ -1341,8 +1330,7 @@ static ssize_t bq2415x_sysfs_set_limit(struct device *dev,
 				       size_t count)
 {
 	struct power_supply *psy = dev_get_drvdata(dev);
-	struct bq2415x_device *bq = container_of(psy, struct bq2415x_device,
-						 charger);
+	struct bq2415x_device *bq = power_supply_get_drvdata(psy);
 	long val;
 	int ret;
 
@@ -1373,8 +1361,7 @@ static ssize_t bq2415x_sysfs_show_limit(struct device *dev,
 					char *buf)
 {
 	struct power_supply *psy = dev_get_drvdata(dev);
-	struct bq2415x_device *bq = container_of(psy, struct bq2415x_device,
-						 charger);
+	struct bq2415x_device *bq = power_supply_get_drvdata(psy);
 	int ret;
 
 	if (strcmp(attr->attr.name, "current_limit") == 0)
@@ -1402,8 +1389,7 @@ static ssize_t bq2415x_sysfs_set_enable(struct device *dev,
 					size_t count)
 {
 	struct power_supply *psy = dev_get_drvdata(dev);
-	struct bq2415x_device *bq = container_of(psy, struct bq2415x_device,
-						 charger);
+	struct bq2415x_device *bq = power_supply_get_drvdata(psy);
 	enum bq2415x_command command;
 	long val;
 	int ret;
@@ -1438,8 +1424,7 @@ static ssize_t bq2415x_sysfs_show_enable(struct device *dev,
 					 char *buf)
 {
 	struct power_supply *psy = dev_get_drvdata(dev);
-	struct bq2415x_device *bq = container_of(psy, struct bq2415x_device,
-						 charger);
+	struct bq2415x_device *bq = power_supply_get_drvdata(psy);
 	enum bq2415x_command command;
 	int ret;
 
@@ -1530,13 +1515,13 @@ static const struct attribute_group bq2415x_sysfs_attr_group = {
 
 static int bq2415x_sysfs_init(struct bq2415x_device *bq)
 {
-	return sysfs_create_group(&bq->charger.dev->kobj,
+	return sysfs_create_group(&bq->charger->dev.kobj,
 			&bq2415x_sysfs_attr_group);
 }
 
 static void bq2415x_sysfs_exit(struct bq2415x_device *bq)
 {
-	sysfs_remove_group(&bq->charger.dev->kobj, &bq2415x_sysfs_attr_group);
+	sysfs_remove_group(&bq->charger->dev.kobj, &bq2415x_sysfs_attr_group);
 }
 
 /* main bq2415x probe function */
@@ -1609,27 +1594,27 @@ static int bq2415x_probe(struct i2c_client *client,
 		ret = of_property_read_u32(np, "ti,current-limit",
 				&bq->init_data.current_limit);
 		if (ret)
-			goto error_2;
+			goto error_3;
 		ret = of_property_read_u32(np, "ti,weak-battery-voltage",
 				&bq->init_data.weak_battery_voltage);
 		if (ret)
-			goto error_2;
+			goto error_3;
 		ret = of_property_read_u32(np, "ti,battery-regulation-voltage",
 				&bq->init_data.battery_regulation_voltage);
 		if (ret)
-			goto error_2;
+			goto error_3;
 		ret = of_property_read_u32(np, "ti,charge-current",
 				&bq->init_data.charge_current);
 		if (ret)
-			goto error_2;
+			goto error_3;
 		ret = of_property_read_u32(np, "ti,termination-current",
 				&bq->init_data.termination_current);
 		if (ret)
-			goto error_2;
+			goto error_3;
 		ret = of_property_read_u32(np, "ti,resistor-sense",
 				&bq->init_data.resistor_sense);
 		if (ret)
-			goto error_2;
+			goto error_3;
 	} else {
 		memcpy(&bq->init_data, pdata, sizeof(bq->init_data));
 	}
@@ -1639,19 +1624,19 @@ static int bq2415x_probe(struct i2c_client *client,
 	ret = bq2415x_power_supply_init(bq);
 	if (ret) {
 		dev_err(bq->dev, "failed to register power supply: %d\n", ret);
-		goto error_2;
+		goto error_3;
 	}
 
 	ret = bq2415x_sysfs_init(bq);
 	if (ret) {
 		dev_err(bq->dev, "failed to create sysfs entries: %d\n", ret);
-		goto error_3;
+		goto error_4;
 	}
 
 	ret = bq2415x_set_defaults(bq);
 	if (ret) {
 		dev_err(bq->dev, "failed to set default values: %d\n", ret);
-		goto error_4;
+		goto error_5;
 	}
 
 	if (bq->notify_psy) {
@@ -1659,7 +1644,7 @@ static int bq2415x_probe(struct i2c_client *client,
 		ret = power_supply_reg_notifier(&bq->nb);
 		if (ret) {
 			dev_err(bq->dev, "failed to reg notifier: %d\n", ret);
-			goto error_5;
+			goto error_6;
 		}
 
 		/* Query for initial reported_mode and set it */
@@ -1679,11 +1664,14 @@ static int bq2415x_probe(struct i2c_client *client,
 	dev_info(bq->dev, "driver registered\n");
 	return 0;
 
+error_6:
 error_5:
-error_4:
 	bq2415x_sysfs_exit(bq);
-error_3:
+error_4:
 	bq2415x_power_supply_exit(bq);
+error_3:
+	if (bq->notify_psy)
+		power_supply_put(bq->notify_psy);
 error_2:
 	kfree(name);
 error_1:
@@ -1700,8 +1688,10 @@ static int bq2415x_remove(struct i2c_client *client)
 {
 	struct bq2415x_device *bq = i2c_get_clientdata(client);
 
-	if (bq->notify_psy)
+	if (bq->notify_psy) {
 		power_supply_unreg_notifier(&bq->nb);
+		power_supply_put(bq->notify_psy);
+	}
 
 	bq2415x_sysfs_exit(bq);
 	bq2415x_power_supply_exit(bq);
@@ -1731,6 +1721,7 @@ static const struct i2c_device_id bq2415x_i2c_id_table[] = {
 	{ "bq24155", BQ24155 },
 	{ "bq24156", BQ24156 },
 	{ "bq24156a", BQ24156A },
+	{ "bq24157s", BQ24157S },
 	{ "bq24158", BQ24158 },
 	{},
 };
diff --git a/drivers/power/bq24190_charger.c b/drivers/power/bq24190_charger.c
index d0e8236a6404..407c4af83891 100644
--- a/drivers/power/bq24190_charger.c
+++ b/drivers/power/bq24190_charger.c
@@ -152,8 +152,8 @@
 struct bq24190_dev_info {
 	struct i2c_client		*client;
 	struct device			*dev;
-	struct power_supply		charger;
-	struct power_supply		battery;
+	struct power_supply		*charger;
+	struct power_supply		*battery;
 	char				model_name[I2C_NAME_SIZE];
 	kernel_ulong_t			model;
 	unsigned int			gpio_int;
@@ -423,8 +423,7 @@ static ssize_t bq24190_sysfs_show(struct device *dev,
 		struct device_attribute *attr, char *buf)
 {
 	struct power_supply *psy = dev_get_drvdata(dev);
-	struct bq24190_dev_info *bdi =
-			container_of(psy, struct bq24190_dev_info, charger);
+	struct bq24190_dev_info *bdi = power_supply_get_drvdata(psy);
 	struct bq24190_sysfs_field_info *info;
 	int ret;
 	u8 v;
@@ -444,8 +443,7 @@ static ssize_t bq24190_sysfs_store(struct device *dev,
 		struct device_attribute *attr, const char *buf, size_t count)
 {
 	struct power_supply *psy = dev_get_drvdata(dev);
-	struct bq24190_dev_info *bdi =
-			container_of(psy, struct bq24190_dev_info, charger);
+	struct bq24190_dev_info *bdi = power_supply_get_drvdata(psy);
 	struct bq24190_sysfs_field_info *info;
 	int ret;
 	u8 v;
@@ -469,13 +467,13 @@ static int bq24190_sysfs_create_group(struct bq24190_dev_info *bdi)
 {
 	bq24190_sysfs_init_attrs();
 
-	return sysfs_create_group(&bdi->charger.dev->kobj,
+	return sysfs_create_group(&bdi->charger->dev.kobj,
 			&bq24190_sysfs_attr_group);
 }
 
 static void bq24190_sysfs_remove_group(struct bq24190_dev_info *bdi)
 {
-	sysfs_remove_group(&bdi->charger.dev->kobj, &bq24190_sysfs_attr_group);
+	sysfs_remove_group(&bdi->charger->dev.kobj, &bq24190_sysfs_attr_group);
 }
 #else
 static int bq24190_sysfs_create_group(struct bq24190_dev_info *bdi)
@@ -807,8 +805,7 @@ static int bq24190_charger_set_voltage(struct bq24190_dev_info *bdi,
 static int bq24190_charger_get_property(struct power_supply *psy,
 		enum power_supply_property psp, union power_supply_propval *val)
 {
-	struct bq24190_dev_info *bdi =
-			container_of(psy, struct bq24190_dev_info, charger);
+	struct bq24190_dev_info *bdi = power_supply_get_drvdata(psy);
 	int ret;
 
 	dev_dbg(bdi->dev, "prop: %d\n", psp);
@@ -861,8 +858,7 @@ static int bq24190_charger_set_property(struct power_supply *psy,
 		enum power_supply_property psp,
 		const union power_supply_propval *val)
 {
-	struct bq24190_dev_info *bdi =
-			container_of(psy, struct bq24190_dev_info, charger);
+	struct bq24190_dev_info *bdi = power_supply_get_drvdata(psy);
 	int ret;
 
 	dev_dbg(bdi->dev, "prop: %d\n", psp);
@@ -922,18 +918,15 @@ static char *bq24190_charger_supplied_to[] = {
 	"main-battery",
 };
 
-static void bq24190_charger_init(struct power_supply *charger)
-{
-	charger->name = "bq24190-charger";
-	charger->type = POWER_SUPPLY_TYPE_USB;
-	charger->properties = bq24190_charger_properties;
-	charger->num_properties = ARRAY_SIZE(bq24190_charger_properties);
-	charger->supplied_to = bq24190_charger_supplied_to;
-	charger->num_supplicants = ARRAY_SIZE(bq24190_charger_supplied_to);
-	charger->get_property = bq24190_charger_get_property;
-	charger->set_property = bq24190_charger_set_property;
-	charger->property_is_writeable = bq24190_charger_property_is_writeable;
-}
+static const struct power_supply_desc bq24190_charger_desc = {
+	.name			= "bq24190-charger",
+	.type			= POWER_SUPPLY_TYPE_USB,
+	.properties		= bq24190_charger_properties,
+	.num_properties		= ARRAY_SIZE(bq24190_charger_properties),
+	.get_property		= bq24190_charger_get_property,
+	.set_property		= bq24190_charger_set_property,
+	.property_is_writeable	= bq24190_charger_property_is_writeable,
+};
 
 /* Battery power supply property routines */
 
@@ -1102,8 +1095,7 @@ static int bq24190_battery_set_temp_alert_max(struct bq24190_dev_info *bdi,
 static int bq24190_battery_get_property(struct power_supply *psy,
 		enum power_supply_property psp, union power_supply_propval *val)
 {
-	struct bq24190_dev_info *bdi =
-			container_of(psy, struct bq24190_dev_info, battery);
+	struct bq24190_dev_info *bdi = power_supply_get_drvdata(psy);
 	int ret;
 
 	dev_dbg(bdi->dev, "prop: %d\n", psp);
@@ -1144,8 +1136,7 @@ static int bq24190_battery_set_property(struct power_supply *psy,
 		enum power_supply_property psp,
 		const union power_supply_propval *val)
 {
-	struct bq24190_dev_info *bdi =
-			container_of(psy, struct bq24190_dev_info, battery);
+	struct bq24190_dev_info *bdi = power_supply_get_drvdata(psy);
 	int ret;
 
 	dev_dbg(bdi->dev, "prop: %d\n", psp);
@@ -1193,16 +1184,15 @@ static enum power_supply_property bq24190_battery_properties[] = {
 	POWER_SUPPLY_PROP_SCOPE,
 };
 
-static void bq24190_battery_init(struct power_supply *battery)
-{
-	battery->name = "bq24190-battery";
-	battery->type = POWER_SUPPLY_TYPE_BATTERY;
-	battery->properties = bq24190_battery_properties;
-	battery->num_properties = ARRAY_SIZE(bq24190_battery_properties);
-	battery->get_property = bq24190_battery_get_property;
-	battery->set_property = bq24190_battery_set_property;
-	battery->property_is_writeable = bq24190_battery_property_is_writeable;
-}
+static const struct power_supply_desc bq24190_battery_desc = {
+	.name			= "bq24190-battery",
+	.type			= POWER_SUPPLY_TYPE_BATTERY,
+	.properties		= bq24190_battery_properties,
+	.num_properties		= ARRAY_SIZE(bq24190_battery_properties),
+	.get_property		= bq24190_battery_get_property,
+	.set_property		= bq24190_battery_set_property,
+	.property_is_writeable	= bq24190_battery_property_is_writeable,
+};
 
 static irqreturn_t bq24190_irq_handler_thread(int irq, void *data)
 {
@@ -1269,8 +1259,8 @@ static irqreturn_t bq24190_irq_handler_thread(int irq, void *data)
 	 * interrupt received).
 	 */
 	if (alert_userspace && !bdi->first_time) {
-		power_supply_changed(&bdi->charger);
-		power_supply_changed(&bdi->battery);
+		power_supply_changed(bdi->charger);
+		power_supply_changed(bdi->battery);
 		bdi->first_time = false;
 	}
 
@@ -1362,6 +1352,7 @@ static int bq24190_probe(struct i2c_client *client,
 	struct i2c_adapter *adapter = to_i2c_adapter(client->dev.parent);
 	struct device *dev = &client->dev;
 	struct bq24190_platform_data *pdata = client->dev.platform_data;
+	struct power_supply_config charger_cfg = {}, battery_cfg = {};
 	struct bq24190_dev_info *bdi;
 	int ret;
 
@@ -1416,19 +1407,23 @@ static int bq24190_probe(struct i2c_client *client,
 		goto out2;
 	}
 
-	bq24190_charger_init(&bdi->charger);
-
-	ret = power_supply_register(dev, &bdi->charger);
-	if (ret) {
+	charger_cfg.drv_data = bdi;
+	charger_cfg.supplied_to = bq24190_charger_supplied_to;
+	charger_cfg.num_supplicants = ARRAY_SIZE(bq24190_charger_supplied_to),
+	bdi->charger = power_supply_register(dev, &bq24190_charger_desc,
+						&charger_cfg);
+	if (IS_ERR(bdi->charger)) {
 		dev_err(dev, "Can't register charger\n");
+		ret = PTR_ERR(bdi->charger);
 		goto out2;
 	}
 
-	bq24190_battery_init(&bdi->battery);
-
-	ret = power_supply_register(dev, &bdi->battery);
-	if (ret) {
+	battery_cfg.drv_data = bdi;
+	bdi->battery = power_supply_register(dev, &bq24190_battery_desc,
+						&battery_cfg);
+	if (IS_ERR(bdi->battery)) {
 		dev_err(dev, "Can't register battery\n");
+		ret = PTR_ERR(bdi->battery);
 		goto out3;
 	}
 
@@ -1441,9 +1436,9 @@ static int bq24190_probe(struct i2c_client *client,
 	return 0;
 
 out4:
-	power_supply_unregister(&bdi->battery);
+	power_supply_unregister(bdi->battery);
 out3:
-	power_supply_unregister(&bdi->charger);
+	power_supply_unregister(bdi->charger);
 out2:
 	pm_runtime_disable(dev);
 out1:
@@ -1462,8 +1457,8 @@ static int bq24190_remove(struct i2c_client *client)
 	pm_runtime_put_sync(bdi->dev);
 
 	bq24190_sysfs_remove_group(bdi);
-	power_supply_unregister(&bdi->battery);
-	power_supply_unregister(&bdi->charger);
+	power_supply_unregister(bdi->battery);
+	power_supply_unregister(bdi->charger);
 	pm_runtime_disable(bdi->dev);
 
 	if (bdi->gpio_int)
@@ -1499,8 +1494,8 @@ static int bq24190_pm_resume(struct device *dev)
 	pm_runtime_put_sync(bdi->dev);
 
 	/* Things may have changed while suspended so alert upper layer */
-	power_supply_changed(&bdi->charger);
-	power_supply_changed(&bdi->battery);
+	power_supply_changed(bdi->charger);
+	power_supply_changed(bdi->battery);
 
 	return 0;
 }
diff --git a/drivers/power/bq24735-charger.c b/drivers/power/bq24735-charger.c
index d022b823305b..961a18930027 100644
--- a/drivers/power/bq24735-charger.c
+++ b/drivers/power/bq24735-charger.c
@@ -44,14 +44,15 @@
 #define BQ24735_DEVICE_ID		0xff
 
 struct bq24735 {
-	struct power_supply	charger;
-	struct i2c_client	*client;
-	struct bq24735_platform	*pdata;
+	struct power_supply		*charger;
+	struct power_supply_desc	charger_desc;
+	struct i2c_client		*client;
+	struct bq24735_platform		*pdata;
 };
 
 static inline struct bq24735 *to_bq24735(struct power_supply *psy)
 {
-	return container_of(psy, struct bq24735, charger);
+	return power_supply_get_drvdata(psy);
 }
 
 static enum power_supply_property bq24735_charger_properties[] = {
@@ -192,9 +193,7 @@ static int bq24735_charger_get_property(struct power_supply *psy,
 					enum power_supply_property psp,
 					union power_supply_propval *val)
 {
-	struct bq24735 *charger;
-
-	charger = container_of(psy, struct bq24735, charger);
+	struct bq24735 *charger = to_bq24735(psy);
 
 	switch (psp) {
 	case POWER_SUPPLY_PROP_ONLINE:
@@ -248,7 +247,8 @@ static int bq24735_charger_probe(struct i2c_client *client,
 {
 	int ret;
 	struct bq24735 *charger;
-	struct power_supply *supply;
+	struct power_supply_desc *supply_desc;
+	struct power_supply_config psy_cfg = {};
 	char *name;
 
 	charger = devm_kzalloc(&client->dev, sizeof(*charger), GFP_KERNEL);
@@ -277,16 +277,18 @@ static int bq24735_charger_probe(struct i2c_client *client,
 
 	charger->client = client;
 
-	supply = &charger->charger;
+	supply_desc = &charger->charger_desc;
 
-	supply->name = name;
-	supply->type = POWER_SUPPLY_TYPE_MAINS;
-	supply->properties = bq24735_charger_properties;
-	supply->num_properties = ARRAY_SIZE(bq24735_charger_properties);
-	supply->get_property = bq24735_charger_get_property;
-	supply->supplied_to = charger->pdata->supplied_to;
-	supply->num_supplicants = charger->pdata->num_supplicants;
-	supply->of_node = client->dev.of_node;
+	supply_desc->name = name;
+	supply_desc->type = POWER_SUPPLY_TYPE_MAINS;
+	supply_desc->properties = bq24735_charger_properties;
+	supply_desc->num_properties = ARRAY_SIZE(bq24735_charger_properties);
+	supply_desc->get_property = bq24735_charger_get_property;
+
+	psy_cfg.supplied_to = charger->pdata->supplied_to;
+	psy_cfg.num_supplicants = charger->pdata->num_supplicants;
+	psy_cfg.of_node = client->dev.of_node;
+	psy_cfg.drv_data = charger;
 
 	i2c_set_clientdata(client, charger);
 
@@ -341,8 +343,10 @@ static int bq24735_charger_probe(struct i2c_client *client,
 		}
 	}
 
-	ret = power_supply_register(&client->dev, supply);
-	if (ret < 0) {
+	charger->charger = power_supply_register(&client->dev, supply_desc,
+						 &psy_cfg);
+	if (IS_ERR(charger->charger)) {
+		ret = PTR_ERR(charger->charger);
 		dev_err(&client->dev, "Failed to register power supply: %d\n",
 			ret);
 		goto err_free_name;
@@ -354,7 +358,8 @@ static int bq24735_charger_probe(struct i2c_client *client,
 						IRQF_TRIGGER_RISING |
 						IRQF_TRIGGER_FALLING |
 						IRQF_ONESHOT,
-						supply->name, supply);
+						supply_desc->name,
+						charger->charger);
 		if (ret) {
 			dev_err(&client->dev,
 				"Unable to register IRQ %d err %d\n",
@@ -365,7 +370,7 @@ static int bq24735_charger_probe(struct i2c_client *client,
 
 	return 0;
 err_unregister_supply:
-	power_supply_unregister(supply);
+	power_supply_unregister(charger->charger);
 err_free_name:
 	if (name != charger->pdata->name)
 		kfree(name);
@@ -381,10 +386,10 @@ static int bq24735_charger_remove(struct i2c_client *client)
 		devm_free_irq(&charger->client->dev, charger->client->irq,
 			      &charger->charger);
 
-	power_supply_unregister(&charger->charger);
+	power_supply_unregister(charger->charger);
 
-	if (charger->charger.name != charger->pdata->name)
-		kfree(charger->charger.name);
+	if (charger->charger_desc.name != charger->pdata->name)
+		kfree(charger->charger_desc.name);
 
 	return 0;
 }
diff --git a/drivers/power/bq27x00_battery.c b/drivers/power/bq27x00_battery.c
index b72ba7c1bd69..a57433de5c24 100644
--- a/drivers/power/bq27x00_battery.c
+++ b/drivers/power/bq27x00_battery.c
@@ -16,14 +16,12 @@
  * IMPLIED WARRANTIES, INCLUDING, WITHOUT LIMITATION, THE IMPLIED
  * WARRANTIES OF MERCHANTIBILITY AND FITNESS FOR A PARTICULAR PURPOSE.
  *
- */
-
-/*
  * Datasheets:
  * http://focus.ti.com/docs/prod/folders/print/bq27000.html
  * http://focus.ti.com/docs/prod/folders/print/bq27500.html
  * http://www.ti.com/product/bq27425-g1
  * http://www.ti.com/product/BQ27742-G1
+ * http://www.ti.com/product/BQ27510-G3
  */
 
 #include <linux/device.h>
@@ -74,6 +72,10 @@
 
 #define BQ27742_POWER_AVG		0x76
 
+#define BQ27510_REG_SOC			0x20
+#define BQ27510_REG_DCAP		0x2E /* Design capacity */
+#define BQ27510_REG_CYCT		0x1E /* Cycle count total */
+
 /* bq27425 register addresses are same as bq27x00 addresses minus 4 */
 #define BQ27425_REG_OFFSET		0x04
 #define BQ27425_REG_SOC		(0x1C + BQ27425_REG_OFFSET)
@@ -87,7 +89,7 @@ struct bq27x00_access_methods {
 	int (*read)(struct bq27x00_device_info *di, u8 reg, bool single);
 };
 
-enum bq27x00_chip { BQ27000, BQ27500, BQ27425, BQ27742};
+enum bq27x00_chip { BQ27000, BQ27500, BQ27425, BQ27742, BQ27510};
 
 struct bq27x00_reg_cache {
 	int temperature;
@@ -114,7 +116,7 @@ struct bq27x00_device_info {
 	unsigned long last_update;
 	struct delayed_work work;
 
-	struct power_supply	bat;
+	struct power_supply	*bat;
 
 	struct bq27x00_access_methods bus;
 
@@ -174,6 +176,24 @@ static enum power_supply_property bq27742_battery_props[] = {
 	POWER_SUPPLY_PROP_HEALTH,
 };
 
+static enum power_supply_property bq27510_battery_props[] = {
+	POWER_SUPPLY_PROP_STATUS,
+	POWER_SUPPLY_PROP_PRESENT,
+	POWER_SUPPLY_PROP_VOLTAGE_NOW,
+	POWER_SUPPLY_PROP_CURRENT_NOW,
+	POWER_SUPPLY_PROP_CAPACITY,
+	POWER_SUPPLY_PROP_CAPACITY_LEVEL,
+	POWER_SUPPLY_PROP_TEMP,
+	POWER_SUPPLY_PROP_TIME_TO_EMPTY_NOW,
+	POWER_SUPPLY_PROP_TECHNOLOGY,
+	POWER_SUPPLY_PROP_CHARGE_FULL,
+	POWER_SUPPLY_PROP_CHARGE_NOW,
+	POWER_SUPPLY_PROP_CHARGE_FULL_DESIGN,
+	POWER_SUPPLY_PROP_CYCLE_COUNT,
+	POWER_SUPPLY_PROP_POWER_AVG,
+	POWER_SUPPLY_PROP_HEALTH,
+};
+
 static unsigned int poll_interval = 360;
 module_param(poll_interval, uint, 0644);
 MODULE_PARM_DESC(poll_interval, "battery poll interval in seconds - " \
@@ -198,7 +218,8 @@ static inline int bq27x00_read(struct bq27x00_device_info *di, u8 reg,
  */
 static bool bq27xxx_is_chip_version_higher(struct bq27x00_device_info *di)
 {
-	if (di->chip == BQ27425 || di->chip == BQ27500 || di->chip == BQ27742)
+	if (di->chip == BQ27425 || di->chip == BQ27500 || di->chip == BQ27742
+	    || di->chip == BQ27510)
 		return true;
 	return false;
 }
@@ -213,6 +234,8 @@ static int bq27x00_battery_read_rsoc(struct bq27x00_device_info *di)
 
 	if (di->chip == BQ27500 || di->chip == BQ27742)
 		rsoc = bq27x00_read(di, BQ27500_REG_SOC, false);
+	else if (di->chip == BQ27510)
+		rsoc = bq27x00_read(di, BQ27510_REG_SOC, false);
 	else if (di->chip == BQ27425)
 		rsoc = bq27x00_read(di, BQ27425_REG_SOC, false);
 	else
@@ -286,6 +309,8 @@ static int bq27x00_battery_read_ilmd(struct bq27x00_device_info *di)
 	if (bq27xxx_is_chip_version_higher(di)) {
 		if (di->chip == BQ27425)
 			ilmd = bq27x00_read(di, BQ27425_REG_DCAP, false);
+		else if (di->chip == BQ27510)
+			ilmd = bq27x00_read(di, BQ27510_REG_DCAP, false);
 		else
 			ilmd = bq27x00_read(di, BQ27500_REG_DCAP, false);
 	} else
@@ -354,7 +379,10 @@ static int bq27x00_battery_read_cyct(struct bq27x00_device_info *di)
 {
 	int cyct;
 
-	cyct = bq27x00_read(di, BQ27x00_REG_CYCT, false);
+	if (di->chip == BQ27510)
+		cyct = bq27x00_read(di, BQ27510_REG_CYCT, false);
+	else
+		cyct = bq27x00_read(di, BQ27x00_REG_CYCT, false);
 	if (cyct < 0)
 		dev_err(di->dev, "error reading cycle count total\n");
 
@@ -425,6 +453,10 @@ static int bq27x00_battery_read_health(struct bq27x00_device_info *di)
 		else
 			tval = POWER_SUPPLY_HEALTH_GOOD;
 		return tval;
+	} else if (di->chip == BQ27510) {
+		if (tval & BQ27500_FLAG_OTC)
+			return POWER_SUPPLY_HEALTH_OVERHEAT;
+		return POWER_SUPPLY_HEALTH_GOOD;
 	} else {
 		if (tval & BQ27000_FLAG_EDV1)
 			tval = POWER_SUPPLY_HEALTH_DEAD;
@@ -440,6 +472,7 @@ static void bq27x00_update(struct bq27x00_device_info *di)
 {
 	struct bq27x00_reg_cache cache = {0, };
 	bool is_bq27500 = di->chip == BQ27500;
+	bool is_bq27510 = di->chip == BQ27510;
 	bool is_bq27425 = di->chip == BQ27425;
 	bool is_bq27742 = di->chip == BQ27742;
 	bool flags_1b = !(is_bq27500 || is_bq27742);
@@ -449,7 +482,7 @@ static void bq27x00_update(struct bq27x00_device_info *di)
 		/* read error */
 		cache.flags = -1;
 	if (cache.flags >= 0) {
-		if (!is_bq27500 && !is_bq27425 && !is_bq27742
+		if (!is_bq27500 && !is_bq27425 && !is_bq27742 && !is_bq27510
 				&& (cache.flags & BQ27000_FLAG_CI)) {
 			dev_info(di->dev, "battery is not calibrated! ignoring capacity values\n");
 			cache.capacity = -ENODATA;
@@ -461,7 +494,7 @@ static void bq27x00_update(struct bq27x00_device_info *di)
 			cache.health = -ENODATA;
 		} else {
 			cache.capacity = bq27x00_battery_read_rsoc(di);
-			if (is_bq27742)
+			if (is_bq27742 || is_bq27510)
 				cache.time_to_empty =
 					bq27x00_battery_read_time(di,
 							BQ27x00_REG_TTE);
@@ -498,7 +531,7 @@ static void bq27x00_update(struct bq27x00_device_info *di)
 	}
 
 	if (di->cache.capacity != cache.capacity)
-		power_supply_changed(&di->bat);
+		power_supply_changed(di->bat);
 
 	if (memcmp(&di->cache, &cache, sizeof(cache)) != 0)
 		di->cache = cache;
@@ -570,7 +603,7 @@ static int bq27x00_battery_status(struct bq27x00_device_info *di,
 			status = POWER_SUPPLY_STATUS_FULL;
 		else if (di->cache.flags & BQ27000_FLAG_CHGS)
 			status = POWER_SUPPLY_STATUS_CHARGING;
-		else if (power_supply_am_i_supplied(&di->bat))
+		else if (power_supply_am_i_supplied(di->bat))
 			status = POWER_SUPPLY_STATUS_NOT_CHARGING;
 		else
 			status = POWER_SUPPLY_STATUS_DISCHARGING;
@@ -642,15 +675,12 @@ static int bq27x00_simple_value(int value,
 	return 0;
 }
 
-#define to_bq27x00_device_info(x) container_of((x), \
-				struct bq27x00_device_info, bat);
-
 static int bq27x00_battery_get_property(struct power_supply *psy,
 					enum power_supply_property psp,
 					union power_supply_propval *val)
 {
 	int ret = 0;
-	struct bq27x00_device_info *di = to_bq27x00_device_info(psy);
+	struct bq27x00_device_info *di = power_supply_get_drvdata(psy);
 
 	mutex_lock(&di->lock);
 	if (time_is_before_jiffies(di->last_update + 5 * HZ)) {
@@ -728,35 +758,47 @@ static int bq27x00_battery_get_property(struct power_supply *psy,
 
 static void bq27x00_external_power_changed(struct power_supply *psy)
 {
-	struct bq27x00_device_info *di = to_bq27x00_device_info(psy);
+	struct bq27x00_device_info *di = power_supply_get_drvdata(psy);
 
 	cancel_delayed_work_sync(&di->work);
 	schedule_delayed_work(&di->work, 0);
 }
 
-static int bq27x00_powersupply_init(struct bq27x00_device_info *di)
+static int bq27x00_powersupply_init(struct bq27x00_device_info *di,
+				    const char *name)
 {
 	int ret;
+	struct power_supply_desc *psy_desc;
+	struct power_supply_config psy_cfg = { .drv_data = di, };
 
-	di->bat.type = POWER_SUPPLY_TYPE_BATTERY;
+	psy_desc = devm_kzalloc(di->dev, sizeof(*psy_desc), GFP_KERNEL);
+	if (!psy_desc)
+		return -ENOMEM;
+
+	psy_desc->name = name;
+	psy_desc->type = POWER_SUPPLY_TYPE_BATTERY;
 	if (di->chip == BQ27425) {
-		di->bat.properties = bq27425_battery_props;
-		di->bat.num_properties = ARRAY_SIZE(bq27425_battery_props);
+		psy_desc->properties = bq27425_battery_props;
+		psy_desc->num_properties = ARRAY_SIZE(bq27425_battery_props);
 	} else if (di->chip == BQ27742) {
-		di->bat.properties = bq27742_battery_props;
-		di->bat.num_properties = ARRAY_SIZE(bq27742_battery_props);
+		psy_desc->properties = bq27742_battery_props;
+		psy_desc->num_properties = ARRAY_SIZE(bq27742_battery_props);
+	} else if (di->chip == BQ27510) {
+		psy_desc->properties = bq27510_battery_props;
+		psy_desc->num_properties = ARRAY_SIZE(bq27510_battery_props);
 	} else {
-		di->bat.properties = bq27x00_battery_props;
-		di->bat.num_properties = ARRAY_SIZE(bq27x00_battery_props);
+		psy_desc->properties = bq27x00_battery_props;
+		psy_desc->num_properties = ARRAY_SIZE(bq27x00_battery_props);
 	}
-	di->bat.get_property = bq27x00_battery_get_property;
-	di->bat.external_power_changed = bq27x00_external_power_changed;
+	psy_desc->get_property = bq27x00_battery_get_property;
+	psy_desc->external_power_changed = bq27x00_external_power_changed;
 
 	INIT_DELAYED_WORK(&di->work, bq27x00_battery_poll);
 	mutex_init(&di->lock);
 
-	ret = power_supply_register(di->dev, &di->bat);
-	if (ret) {
+	di->bat = power_supply_register_no_ws(di->dev, psy_desc, &psy_cfg);
+	if (IS_ERR(di->bat)) {
+		ret = PTR_ERR(di->bat);
 		dev_err(di->dev, "failed to register battery: %d\n", ret);
 		return ret;
 	}
@@ -780,7 +822,7 @@ static void bq27x00_powersupply_unregister(struct bq27x00_device_info *di)
 
 	cancel_delayed_work_sync(&di->work);
 
-	power_supply_unregister(&di->bat);
+	power_supply_unregister(di->bat);
 
 	mutex_destroy(&di->lock);
 }
@@ -844,37 +886,34 @@ static int bq27x00_battery_probe(struct i2c_client *client,
 	if (num < 0)
 		return num;
 
-	name = kasprintf(GFP_KERNEL, "%s-%d", id->name, num);
+	name = devm_kasprintf(&client->dev, GFP_KERNEL, "%s-%d", id->name, num);
 	if (!name) {
 		dev_err(&client->dev, "failed to allocate device name\n");
 		retval = -ENOMEM;
-		goto batt_failed_1;
+		goto batt_failed;
 	}
 
 	di = devm_kzalloc(&client->dev, sizeof(*di), GFP_KERNEL);
 	if (!di) {
 		dev_err(&client->dev, "failed to allocate device info data\n");
 		retval = -ENOMEM;
-		goto batt_failed_2;
+		goto batt_failed;
 	}
 
 	di->id = num;
 	di->dev = &client->dev;
 	di->chip = id->driver_data;
-	di->bat.name = name;
 	di->bus.read = &bq27x00_read_i2c;
 
-	retval = bq27x00_powersupply_init(di);
+	retval = bq27x00_powersupply_init(di, name);
 	if (retval)
-		goto batt_failed_2;
+		goto batt_failed;
 
 	i2c_set_clientdata(client, di);
 
 	return 0;
 
-batt_failed_2:
-	kfree(name);
-batt_failed_1:
+batt_failed:
 	mutex_lock(&battery_mutex);
 	idr_remove(&battery_id, num);
 	mutex_unlock(&battery_mutex);
@@ -888,8 +927,6 @@ static int bq27x00_battery_remove(struct i2c_client *client)
 
 	bq27x00_powersupply_unregister(di);
 
-	kfree(di->bat.name);
-
 	mutex_lock(&battery_mutex);
 	idr_remove(&battery_id, di->id);
 	mutex_unlock(&battery_mutex);
@@ -902,6 +939,7 @@ static const struct i2c_device_id bq27x00_id[] = {
 	{ "bq27500", BQ27500 },
 	{ "bq27425", BQ27425 },
 	{ "bq27742", BQ27742 },
+	{ "bq27510", BQ27510 },
 	{},
 };
 MODULE_DEVICE_TABLE(i2c, bq27x00_id);
@@ -977,6 +1015,7 @@ static int bq27000_battery_probe(struct platform_device *pdev)
 {
 	struct bq27x00_device_info *di;
 	struct bq27000_platform_data *pdata = pdev->dev.platform_data;
+	const char *name;
 
 	if (!pdata) {
 		dev_err(&pdev->dev, "no platform_data supplied\n");
@@ -999,10 +1038,10 @@ static int bq27000_battery_probe(struct platform_device *pdev)
 	di->dev = &pdev->dev;
 	di->chip = BQ27000;
 
-	di->bat.name = pdata->name ?: dev_name(&pdev->dev);
+	name = pdata->name ?: dev_name(&pdev->dev);
 	di->bus.read = &bq27000_read_platform;
 
-	return bq27x00_powersupply_init(di);
+	return bq27x00_powersupply_init(di, name);
 }
 
 static int bq27000_battery_remove(struct platform_device *pdev)
diff --git a/drivers/power/charger-manager.c b/drivers/power/charger-manager.c
index 14b0d85318eb..0aed13f90891 100644
--- a/drivers/power/charger-manager.c
+++ b/drivers/power/charger-manager.c
@@ -103,10 +103,11 @@ static bool is_batt_present(struct charger_manager *cm)
 		if (!psy)
 			break;
 
-		ret = psy->get_property(psy,
-				POWER_SUPPLY_PROP_PRESENT, &val);
+		ret = power_supply_get_property(psy, POWER_SUPPLY_PROP_PRESENT,
+				&val);
 		if (ret == 0 && val.intval)
 			present = true;
+		power_supply_put(psy);
 		break;
 	case CM_CHARGER_STAT:
 		for (i = 0; cm->desc->psy_charger_stat[i]; i++) {
@@ -118,8 +119,9 @@ static bool is_batt_present(struct charger_manager *cm)
 				continue;
 			}
 
-			ret = psy->get_property(psy, POWER_SUPPLY_PROP_PRESENT,
-					&val);
+			ret = power_supply_get_property(psy,
+				POWER_SUPPLY_PROP_PRESENT, &val);
+			power_supply_put(psy);
 			if (ret == 0 && val.intval) {
 				present = true;
 				break;
@@ -155,7 +157,9 @@ static bool is_ext_pwr_online(struct charger_manager *cm)
 			continue;
 		}
 
-		ret = psy->get_property(psy, POWER_SUPPLY_PROP_ONLINE, &val);
+		ret = power_supply_get_property(psy, POWER_SUPPLY_PROP_ONLINE,
+				&val);
+		power_supply_put(psy);
 		if (ret == 0 && val.intval) {
 			online = true;
 			break;
@@ -183,8 +187,9 @@ static int get_batt_uV(struct charger_manager *cm, int *uV)
 	if (!fuel_gauge)
 		return -ENODEV;
 
-	ret = fuel_gauge->get_property(fuel_gauge,
+	ret = power_supply_get_property(fuel_gauge,
 				POWER_SUPPLY_PROP_VOLTAGE_NOW, &val);
+	power_supply_put(fuel_gauge);
 	if (ret)
 		return ret;
 
@@ -223,20 +228,26 @@ static bool is_charging(struct charger_manager *cm)
 		}
 
 		/* 2. The charger should be online (ext-power) */
-		ret = psy->get_property(psy, POWER_SUPPLY_PROP_ONLINE, &val);
+		ret = power_supply_get_property(psy, POWER_SUPPLY_PROP_ONLINE,
+				&val);
 		if (ret) {
 			dev_warn(cm->dev, "Cannot read ONLINE value from %s\n",
 				 cm->desc->psy_charger_stat[i]);
+			power_supply_put(psy);
 			continue;
 		}
-		if (val.intval == 0)
+		if (val.intval == 0) {
+			power_supply_put(psy);
 			continue;
+		}
 
 		/*
 		 * 3. The charger should not be FULL, DISCHARGING,
 		 * or NOT_CHARGING.
 		 */
-		ret = psy->get_property(psy, POWER_SUPPLY_PROP_STATUS, &val);
+		ret = power_supply_get_property(psy, POWER_SUPPLY_PROP_STATUS,
+				&val);
+		power_supply_put(psy);
 		if (ret) {
 			dev_warn(cm->dev, "Cannot read STATUS value from %s\n",
 				 cm->desc->psy_charger_stat[i]);
@@ -264,6 +275,7 @@ static bool is_full_charged(struct charger_manager *cm)
 	struct charger_desc *desc = cm->desc;
 	union power_supply_propval val;
 	struct power_supply *fuel_gauge;
+	bool is_full = false;
 	int ret = 0;
 	int uV;
 
@@ -279,30 +291,38 @@ static bool is_full_charged(struct charger_manager *cm)
 		val.intval = 0;
 
 		/* Not full if capacity of fuel gauge isn't full */
-		ret = fuel_gauge->get_property(fuel_gauge,
+		ret = power_supply_get_property(fuel_gauge,
 				POWER_SUPPLY_PROP_CHARGE_FULL, &val);
-		if (!ret && val.intval > desc->fullbatt_full_capacity)
-			return true;
+		if (!ret && val.intval > desc->fullbatt_full_capacity) {
+			is_full = true;
+			goto out;
+		}
 	}
 
 	/* Full, if it's over the fullbatt voltage */
 	if (desc->fullbatt_uV > 0) {
 		ret = get_batt_uV(cm, &uV);
-		if (!ret && uV >= desc->fullbatt_uV)
-			return true;
+		if (!ret && uV >= desc->fullbatt_uV) {
+			is_full = true;
+			goto out;
+		}
 	}
 
 	/* Full, if the capacity is more than fullbatt_soc */
 	if (desc->fullbatt_soc > 0) {
 		val.intval = 0;
 
-		ret = fuel_gauge->get_property(fuel_gauge,
+		ret = power_supply_get_property(fuel_gauge,
 				POWER_SUPPLY_PROP_CAPACITY, &val);
-		if (!ret && val.intval >= desc->fullbatt_soc)
-			return true;
+		if (!ret && val.intval >= desc->fullbatt_soc) {
+			is_full = true;
+			goto out;
+		}
 	}
 
-	return false;
+out:
+	power_supply_put(fuel_gauge);
+	return is_full;
 }
 
 /**
@@ -575,14 +595,18 @@ static int cm_get_battery_temperature_by_psy(struct charger_manager *cm,
 					int *temp)
 {
 	struct power_supply *fuel_gauge;
+	int ret;
 
 	fuel_gauge = power_supply_get_by_name(cm->desc->psy_fuel_gauge);
 	if (!fuel_gauge)
 		return -ENODEV;
 
-	return fuel_gauge->get_property(fuel_gauge,
+	ret = power_supply_get_property(fuel_gauge,
 				POWER_SUPPLY_PROP_TEMP,
 				(union power_supply_propval *)temp);
+	power_supply_put(fuel_gauge);
+
+	return ret;
 }
 
 static int cm_get_battery_temperature(struct charger_manager *cm,
@@ -861,10 +885,9 @@ static int charger_get_property(struct power_supply *psy,
 		enum power_supply_property psp,
 		union power_supply_propval *val)
 {
-	struct charger_manager *cm = container_of(psy,
-			struct charger_manager, charger_psy);
+	struct charger_manager *cm = power_supply_get_drvdata(psy);
 	struct charger_desc *desc = cm->desc;
-	struct power_supply *fuel_gauge;
+	struct power_supply *fuel_gauge = NULL;
 	int ret = 0;
 	int uV;
 
@@ -900,26 +923,26 @@ static int charger_get_property(struct power_supply *psy,
 			ret = -ENODEV;
 			break;
 		}
-		ret = fuel_gauge->get_property(fuel_gauge,
+		ret = power_supply_get_property(fuel_gauge,
 				POWER_SUPPLY_PROP_CURRENT_NOW, val);
 		break;
 	case POWER_SUPPLY_PROP_TEMP:
 	case POWER_SUPPLY_PROP_TEMP_AMBIENT:
 		return cm_get_battery_temperature(cm, &val->intval);
 	case POWER_SUPPLY_PROP_CAPACITY:
-		fuel_gauge = power_supply_get_by_name(cm->desc->psy_fuel_gauge);
-		if (!fuel_gauge) {
-			ret = -ENODEV;
-			break;
-		}
-
 		if (!is_batt_present(cm)) {
 			/* There is no battery. Assume 100% */
 			val->intval = 100;
 			break;
 		}
 
-		ret = fuel_gauge->get_property(fuel_gauge,
+		fuel_gauge = power_supply_get_by_name(cm->desc->psy_fuel_gauge);
+		if (!fuel_gauge) {
+			ret = -ENODEV;
+			break;
+		}
+
+		ret = power_supply_get_property(fuel_gauge,
 					POWER_SUPPLY_PROP_CAPACITY, val);
 		if (ret)
 			break;
@@ -975,7 +998,7 @@ static int charger_get_property(struct power_supply *psy,
 				break;
 			}
 
-			ret = fuel_gauge->get_property(fuel_gauge,
+			ret = power_supply_get_property(fuel_gauge,
 						POWER_SUPPLY_PROP_CHARGE_NOW,
 						val);
 			if (ret) {
@@ -993,6 +1016,8 @@ static int charger_get_property(struct power_supply *psy,
 	default:
 		return -EINVAL;
 	}
+	if (fuel_gauge)
+		power_supply_put(fuel_gauge);
 	return ret;
 }
 
@@ -1015,7 +1040,7 @@ static enum power_supply_property default_charger_props[] = {
 	 */
 };
 
-static struct power_supply psy_default = {
+static const struct power_supply_desc psy_default = {
 	.name = "battery",
 	.type = POWER_SUPPLY_TYPE_BATTERY,
 	.properties = default_charger_props,
@@ -1396,7 +1421,7 @@ static int charger_manager_register_sysfs(struct charger_manager *cm)
 		dev_info(cm->dev, "'%s' regulator's externally_control is %d\n",
 			 charger->regulator_name, charger->externally_control);
 
-		ret = sysfs_create_group(&cm->charger_psy.dev->kobj,
+		ret = sysfs_create_group(&cm->charger_psy->dev.kobj,
 					&charger->attr_g);
 		if (ret < 0) {
 			dev_err(cm->dev, "Cannot create sysfs entry of %s regulator\n",
@@ -1424,13 +1449,13 @@ static int cm_init_thermal_data(struct charger_manager *cm,
 	int ret;
 
 	/* Verify whether fuel gauge provides battery temperature */
-	ret = fuel_gauge->get_property(fuel_gauge,
+	ret = power_supply_get_property(fuel_gauge,
 					POWER_SUPPLY_PROP_TEMP, &val);
 
 	if (!ret) {
-		cm->charger_psy.properties[cm->charger_psy.num_properties] =
+		cm->charger_psy_desc.properties[cm->charger_psy_desc.num_properties] =
 				POWER_SUPPLY_PROP_TEMP;
-		cm->charger_psy.num_properties++;
+		cm->charger_psy_desc.num_properties++;
 		cm->desc->measure_battery_temp = true;
 	}
 #ifdef CONFIG_THERMAL
@@ -1441,9 +1466,9 @@ static int cm_init_thermal_data(struct charger_manager *cm,
 			return PTR_ERR(cm->tzd_batt);
 
 		/* Use external thermometer */
-		cm->charger_psy.properties[cm->charger_psy.num_properties] =
+		cm->charger_psy_desc.properties[cm->charger_psy_desc.num_properties] =
 				POWER_SUPPLY_PROP_TEMP_AMBIENT;
-		cm->charger_psy.num_properties++;
+		cm->charger_psy_desc.num_properties++;
 		cm->desc->measure_battery_temp = true;
 		ret = 0;
 	}
@@ -1459,7 +1484,7 @@ static int cm_init_thermal_data(struct charger_manager *cm,
 	return ret;
 }
 
-static struct of_device_id charger_manager_match[] = {
+static const struct of_device_id charger_manager_match[] = {
 	{
 		.compatible = "charger-manager",
 	},
@@ -1603,6 +1628,7 @@ static int charger_manager_probe(struct platform_device *pdev)
 	int j = 0;
 	union power_supply_propval val;
 	struct power_supply *fuel_gauge;
+	struct power_supply_config psy_cfg = {};
 
 	if (IS_ERR(desc)) {
 		dev_err(&pdev->dev, "No platform data (desc) found\n");
@@ -1617,6 +1643,7 @@ static int charger_manager_probe(struct platform_device *pdev)
 	/* Basic Values. Unspecified are Null or 0 */
 	cm->dev = &pdev->dev;
 	cm->desc = desc;
+	psy_cfg.drv_data = cm;
 
 	/* Initialize alarm timer */
 	if (alarmtimer_get_rtcdev()) {
@@ -1672,13 +1699,7 @@ static int charger_manager_probe(struct platform_device *pdev)
 				desc->psy_charger_stat[i]);
 			return -ENODEV;
 		}
-	}
-
-	fuel_gauge = power_supply_get_by_name(desc->psy_fuel_gauge);
-	if (!fuel_gauge) {
-		dev_err(&pdev->dev, "Cannot find power supply \"%s\"\n",
-			desc->psy_fuel_gauge);
-		return -ENODEV;
+		power_supply_put(psy);
 	}
 
 	if (desc->polling_interval_ms == 0 ||
@@ -1696,40 +1717,46 @@ static int charger_manager_probe(struct platform_device *pdev)
 
 	platform_set_drvdata(pdev, cm);
 
-	memcpy(&cm->charger_psy, &psy_default, sizeof(psy_default));
+	memcpy(&cm->charger_psy_desc, &psy_default, sizeof(psy_default));
 
 	if (!desc->psy_name)
 		strncpy(cm->psy_name_buf, psy_default.name, PSY_NAME_MAX);
 	else
 		strncpy(cm->psy_name_buf, desc->psy_name, PSY_NAME_MAX);
-	cm->charger_psy.name = cm->psy_name_buf;
+	cm->charger_psy_desc.name = cm->psy_name_buf;
 
 	/* Allocate for psy properties because they may vary */
-	cm->charger_psy.properties = devm_kzalloc(&pdev->dev,
+	cm->charger_psy_desc.properties = devm_kzalloc(&pdev->dev,
 				sizeof(enum power_supply_property)
 				* (ARRAY_SIZE(default_charger_props) +
 				NUM_CHARGER_PSY_OPTIONAL), GFP_KERNEL);
-	if (!cm->charger_psy.properties)
+	if (!cm->charger_psy_desc.properties)
 		return -ENOMEM;
 
-	memcpy(cm->charger_psy.properties, default_charger_props,
+	memcpy(cm->charger_psy_desc.properties, default_charger_props,
 		sizeof(enum power_supply_property) *
 		ARRAY_SIZE(default_charger_props));
-	cm->charger_psy.num_properties = psy_default.num_properties;
+	cm->charger_psy_desc.num_properties = psy_default.num_properties;
 
 	/* Find which optional psy-properties are available */
-	if (!fuel_gauge->get_property(fuel_gauge,
+	fuel_gauge = power_supply_get_by_name(desc->psy_fuel_gauge);
+	if (!fuel_gauge) {
+		dev_err(&pdev->dev, "Cannot find power supply \"%s\"\n",
+			desc->psy_fuel_gauge);
+		return -ENODEV;
+	}
+	if (!power_supply_get_property(fuel_gauge,
 					  POWER_SUPPLY_PROP_CHARGE_NOW, &val)) {
-		cm->charger_psy.properties[cm->charger_psy.num_properties] =
+		cm->charger_psy_desc.properties[cm->charger_psy_desc.num_properties] =
 				POWER_SUPPLY_PROP_CHARGE_NOW;
-		cm->charger_psy.num_properties++;
+		cm->charger_psy_desc.num_properties++;
 	}
-	if (!fuel_gauge->get_property(fuel_gauge,
+	if (!power_supply_get_property(fuel_gauge,
 					  POWER_SUPPLY_PROP_CURRENT_NOW,
 					  &val)) {
-		cm->charger_psy.properties[cm->charger_psy.num_properties] =
+		cm->charger_psy_desc.properties[cm->charger_psy_desc.num_properties] =
 				POWER_SUPPLY_PROP_CURRENT_NOW;
-		cm->charger_psy.num_properties++;
+		cm->charger_psy_desc.num_properties++;
 	}
 
 	ret = cm_init_thermal_data(cm, fuel_gauge);
@@ -1737,14 +1764,16 @@ static int charger_manager_probe(struct platform_device *pdev)
 		dev_err(&pdev->dev, "Failed to initialize thermal data\n");
 		cm->desc->measure_battery_temp = false;
 	}
+	power_supply_put(fuel_gauge);
 
 	INIT_DELAYED_WORK(&cm->fullbatt_vchk_work, fullbatt_vchk);
 
-	ret = power_supply_register(NULL, &cm->charger_psy);
-	if (ret) {
+	cm->charger_psy = power_supply_register(NULL, &cm->charger_psy_desc,
+						&psy_cfg);
+	if (IS_ERR(cm->charger_psy)) {
 		dev_err(&pdev->dev, "Cannot register charger-manager with name \"%s\"\n",
-			cm->charger_psy.name);
-		return ret;
+			cm->charger_psy_desc.name);
+		return PTR_ERR(cm->charger_psy);
 	}
 
 	/* Register extcon device for charger cable */
@@ -1790,7 +1819,7 @@ err_reg_sysfs:
 		struct charger_regulator *charger;
 
 		charger = &desc->charger_regulators[i];
-		sysfs_remove_group(&cm->charger_psy.dev->kobj,
+		sysfs_remove_group(&cm->charger_psy->dev.kobj,
 				&charger->attr_g);
 	}
 err_reg_extcon:
@@ -1808,7 +1837,7 @@ err_reg_extcon:
 		regulator_put(desc->charger_regulators[i].consumer);
 	}
 
-	power_supply_unregister(&cm->charger_psy);
+	power_supply_unregister(cm->charger_psy);
 
 	return ret;
 }
@@ -1840,7 +1869,7 @@ static int charger_manager_remove(struct platform_device *pdev)
 	for (i = 0 ; i < desc->num_charger_regulators ; i++)
 		regulator_put(desc->charger_regulators[i].consumer);
 
-	power_supply_unregister(&cm->charger_psy);
+	power_supply_unregister(cm->charger_psy);
 
 	try_charger_enable(cm, false);
 
@@ -1999,7 +2028,7 @@ static bool find_power_supply(struct charger_manager *cm,
 	bool found = false;
 
 	for (i = 0; cm->desc->psy_charger_stat[i]; i++) {
-		if (!strcmp(psy->name, cm->desc->psy_charger_stat[i])) {
+		if (!strcmp(psy->desc->name, cm->desc->psy_charger_stat[i])) {
 			found = true;
 			break;
 		}
diff --git a/drivers/power/collie_battery.c b/drivers/power/collie_battery.c
index 594e4dbc2d51..2da9ed8ccbb5 100644
--- a/drivers/power/collie_battery.c
+++ b/drivers/power/collie_battery.c
@@ -30,7 +30,7 @@ static int wakeup_enabled;
 
 struct collie_bat {
 	int status;
-	struct power_supply psy;
+	struct power_supply *psy;
 	int full_chrg;
 
 	struct mutex work_lock; /* protects data */
@@ -98,7 +98,7 @@ static int collie_bat_get_property(struct power_supply *psy,
 			    union power_supply_propval *val)
 {
 	int ret = 0;
-	struct collie_bat *bat = container_of(psy, struct collie_bat, psy);
+	struct collie_bat *bat = power_supply_get_drvdata(psy);
 
 	if (bat->is_present && !bat->is_present(bat)
 			&& psp != POWER_SUPPLY_PROP_PRESENT) {
@@ -155,14 +155,14 @@ static irqreturn_t collie_bat_gpio_isr(int irq, void *data)
 static void collie_bat_update(struct collie_bat *bat)
 {
 	int old;
-	struct power_supply *psy = &bat->psy;
+	struct power_supply *psy = bat->psy;
 
 	mutex_lock(&bat->work_lock);
 
 	old = bat->status;
 
 	if (bat->is_present && !bat->is_present(bat)) {
-		printk(KERN_NOTICE "%s not present\n", psy->name);
+		printk(KERN_NOTICE "%s not present\n", psy->desc->name);
 		bat->status = POWER_SUPPLY_STATUS_UNKNOWN;
 		bat->full_chrg = -1;
 	} else if (power_supply_am_i_supplied(psy)) {
@@ -220,18 +220,20 @@ static enum power_supply_property collie_bat_bu_props[] = {
 	POWER_SUPPLY_PROP_PRESENT,
 };
 
+static const struct power_supply_desc collie_bat_main_desc = {
+	.name		= "main-battery",
+	.type		= POWER_SUPPLY_TYPE_BATTERY,
+	.properties	= collie_bat_main_props,
+	.num_properties	= ARRAY_SIZE(collie_bat_main_props),
+	.get_property	= collie_bat_get_property,
+	.external_power_changed = collie_bat_external_power_changed,
+	.use_for_apm	= 1,
+};
+
 static struct collie_bat collie_bat_main = {
 	.status = POWER_SUPPLY_STATUS_DISCHARGING,
 	.full_chrg = -1,
-	.psy = {
-		.name		= "main-battery",
-		.type		= POWER_SUPPLY_TYPE_BATTERY,
-		.properties	= collie_bat_main_props,
-		.num_properties	= ARRAY_SIZE(collie_bat_main_props),
-		.get_property	= collie_bat_get_property,
-		.external_power_changed = collie_bat_external_power_changed,
-		.use_for_apm	= 1,
-	},
+	.psy = NULL,
 
 	.gpio_full = COLLIE_GPIO_CO,
 	.gpio_charge_on = COLLIE_GPIO_CHARGE_ON,
@@ -249,18 +251,19 @@ static struct collie_bat collie_bat_main = {
 	.adc_temp_divider = 10000,
 };
 
+static const struct power_supply_desc collie_bat_bu_desc = {
+	.name		= "backup-battery",
+	.type		= POWER_SUPPLY_TYPE_BATTERY,
+	.properties	= collie_bat_bu_props,
+	.num_properties	= ARRAY_SIZE(collie_bat_bu_props),
+	.get_property	= collie_bat_get_property,
+	.external_power_changed = collie_bat_external_power_changed,
+};
+
 static struct collie_bat collie_bat_bu = {
 	.status = POWER_SUPPLY_STATUS_UNKNOWN,
 	.full_chrg = -1,
-
-	.psy = {
-		.name		= "backup-battery",
-		.type		= POWER_SUPPLY_TYPE_BATTERY,
-		.properties	= collie_bat_bu_props,
-		.num_properties	= ARRAY_SIZE(collie_bat_bu_props),
-		.get_property	= collie_bat_get_property,
-		.external_power_changed = collie_bat_external_power_changed,
-	},
+	.psy = NULL,
 
 	.gpio_full = -1,
 	.gpio_charge_on = -1,
@@ -319,6 +322,7 @@ static int collie_bat_resume(struct ucb1x00_dev *dev)
 static int collie_bat_probe(struct ucb1x00_dev *dev)
 {
 	int ret;
+	struct power_supply_config psy_main_cfg = {}, psy_bu_cfg = {};
 
 	if (!machine_is_collie())
 		return -ENODEV;
@@ -334,12 +338,23 @@ static int collie_bat_probe(struct ucb1x00_dev *dev)
 
 	INIT_WORK(&bat_work, collie_bat_work);
 
-	ret = power_supply_register(&dev->ucb->dev, &collie_bat_main.psy);
-	if (ret)
+	psy_main_cfg.drv_data = &collie_bat_main;
+	collie_bat_main.psy = power_supply_register(&dev->ucb->dev,
+						    &collie_bat_main_desc,
+						    &psy_main_cfg);
+	if (IS_ERR(collie_bat_main.psy)) {
+		ret = PTR_ERR(collie_bat_main.psy);
 		goto err_psy_reg_main;
-	ret = power_supply_register(&dev->ucb->dev, &collie_bat_bu.psy);
-	if (ret)
+	}
+
+	psy_main_cfg.drv_data = &collie_bat_bu;
+	collie_bat_bu.psy = power_supply_register(&dev->ucb->dev,
+						  &collie_bat_bu_desc,
+						  &psy_bu_cfg);
+	if (IS_ERR(collie_bat_bu.psy)) {
+		ret = PTR_ERR(collie_bat_bu.psy);
 		goto err_psy_reg_bu;
+	}
 
 	ret = request_irq(gpio_to_irq(COLLIE_GPIO_CO),
 				collie_bat_gpio_isr,
@@ -354,9 +369,9 @@ static int collie_bat_probe(struct ucb1x00_dev *dev)
 	return 0;
 
 err_irq:
-	power_supply_unregister(&collie_bat_bu.psy);
+	power_supply_unregister(collie_bat_bu.psy);
 err_psy_reg_bu:
-	power_supply_unregister(&collie_bat_main.psy);
+	power_supply_unregister(collie_bat_main.psy);
 err_psy_reg_main:
 
 	/* see comment in collie_bat_remove */
@@ -369,8 +384,8 @@ static void collie_bat_remove(struct ucb1x00_dev *dev)
 {
 	free_irq(gpio_to_irq(COLLIE_GPIO_CO), &collie_bat_main);
 
-	power_supply_unregister(&collie_bat_bu.psy);
-	power_supply_unregister(&collie_bat_main.psy);
+	power_supply_unregister(collie_bat_bu.psy);
+	power_supply_unregister(collie_bat_main.psy);
 
 	/*
 	 * Now cancel the bat_work.  We won't get any more schedules,
diff --git a/drivers/power/da9030_battery.c b/drivers/power/da9030_battery.c
index 78cd5d66144b..5ca0f4d90792 100644
--- a/drivers/power/da9030_battery.c
+++ b/drivers/power/da9030_battery.c
@@ -89,7 +89,8 @@ struct da9030_battery_thresholds {
 };
 
 struct da9030_charger {
-	struct power_supply psy;
+	struct power_supply *psy;
+	struct power_supply_desc psy_desc;
 
 	struct device *master;
 
@@ -245,7 +246,7 @@ static void da9030_set_charge(struct da9030_charger *charger, int on)
 
 	da903x_write(charger->master, DA9030_CHARGE_CONTROL, val);
 
-	power_supply_changed(&charger->psy);
+	power_supply_changed(charger->psy);
 }
 
 static void da9030_charger_check_state(struct da9030_charger *charger)
@@ -341,8 +342,7 @@ static int da9030_battery_get_property(struct power_supply *psy,
 				   enum power_supply_property psp,
 				   union power_supply_propval *val)
 {
-	struct da9030_charger *charger;
-	charger = container_of(psy, struct da9030_charger, psy);
+	struct da9030_charger *charger = power_supply_get_drvdata(psy);
 
 	switch (psp) {
 	case POWER_SUPPLY_PROP_STATUS:
@@ -447,16 +447,16 @@ static void da9030_battery_convert_thresholds(struct da9030_charger *charger,
 
 static void da9030_battery_setup_psy(struct da9030_charger *charger)
 {
-	struct power_supply *psy = &charger->psy;
+	struct power_supply_desc *psy_desc = &charger->psy_desc;
 	struct power_supply_info *info = charger->battery_info;
 
-	psy->name = info->name;
-	psy->use_for_apm = info->use_for_apm;
-	psy->type = POWER_SUPPLY_TYPE_BATTERY;
-	psy->get_property = da9030_battery_get_property;
+	psy_desc->name = info->name;
+	psy_desc->use_for_apm = info->use_for_apm;
+	psy_desc->type = POWER_SUPPLY_TYPE_BATTERY;
+	psy_desc->get_property = da9030_battery_get_property;
 
-	psy->properties = da9030_battery_props;
-	psy->num_properties = ARRAY_SIZE(da9030_battery_props);
+	psy_desc->properties = da9030_battery_props;
+	psy_desc->num_properties = ARRAY_SIZE(da9030_battery_props);
 };
 
 static int da9030_battery_charger_init(struct da9030_charger *charger)
@@ -494,6 +494,7 @@ static int da9030_battery_charger_init(struct da9030_charger *charger)
 static int da9030_battery_probe(struct platform_device *pdev)
 {
 	struct da9030_charger *charger;
+	struct power_supply_config psy_cfg = {};
 	struct da9030_battery_info *pdata = pdev->dev.platform_data;
 	int ret;
 
@@ -541,9 +542,13 @@ static int da9030_battery_probe(struct platform_device *pdev)
 		goto err_notifier;
 
 	da9030_battery_setup_psy(charger);
-	ret = power_supply_register(&pdev->dev, &charger->psy);
-	if (ret)
+	psy_cfg.drv_data = charger;
+	charger->psy = power_supply_register(&pdev->dev, &charger->psy_desc,
+					     &psy_cfg);
+	if (IS_ERR(charger->psy)) {
+		ret = PTR_ERR(charger->psy);
 		goto err_ps_register;
+	}
 
 	charger->debug_file = da9030_bat_create_debugfs(charger);
 	platform_set_drvdata(pdev, charger);
@@ -571,7 +576,7 @@ static int da9030_battery_remove(struct platform_device *dev)
 				   DA9030_EVENT_CHIOVER | DA9030_EVENT_TBAT);
 	cancel_delayed_work_sync(&charger->work);
 	da9030_set_charge(charger, 0);
-	power_supply_unregister(&charger->psy);
+	power_supply_unregister(charger->psy);
 
 	return 0;
 }
diff --git a/drivers/power/da9052-battery.c b/drivers/power/da9052-battery.c
index d17250f745c2..830ec46fe7d0 100644
--- a/drivers/power/da9052-battery.c
+++ b/drivers/power/da9052-battery.c
@@ -169,7 +169,7 @@ static u32 const vc_tbl[3][68][2] = {
 
 struct da9052_battery {
 	struct da9052 *da9052;
-	struct power_supply psy;
+	struct power_supply *psy;
 	struct notifier_block nb;
 	int charger_type;
 	int status;
@@ -452,7 +452,7 @@ static irqreturn_t da9052_bat_irq(int irq, void *data)
 
 	if (irq == DA9052_IRQ_CHGEND || irq == DA9052_IRQ_DCIN ||
 	    irq == DA9052_IRQ_VBUS || irq == DA9052_IRQ_TBAT) {
-		power_supply_changed(&bat->psy);
+		power_supply_changed(bat->psy);
 	}
 
 	return IRQ_HANDLED;
@@ -499,8 +499,7 @@ static int da9052_bat_get_property(struct power_supply *psy,
 {
 	int ret;
 	int illegal;
-	struct da9052_battery *bat = container_of(psy, struct da9052_battery,
-						  psy);
+	struct da9052_battery *bat = power_supply_get_drvdata(psy);
 
 	ret = da9052_bat_check_presence(bat, &illegal);
 	if (ret < 0)
@@ -561,7 +560,7 @@ static enum power_supply_property da9052_bat_props[] = {
 	POWER_SUPPLY_PROP_TECHNOLOGY,
 };
 
-static struct power_supply template_battery = {
+static struct power_supply_desc psy_desc = {
 	.name		= "da9052-bat",
 	.type		= POWER_SUPPLY_TYPE_BATTERY,
 	.properties	= da9052_bat_props,
@@ -591,6 +590,7 @@ static s32 da9052_bat_probe(struct platform_device *pdev)
 {
 	struct da9052_pdata *pdata;
 	struct da9052_battery *bat;
+	struct power_supply_config psy_cfg = {};
 	int ret;
 	int i;
 
@@ -599,8 +599,9 @@ static s32 da9052_bat_probe(struct platform_device *pdev)
 	if (!bat)
 		return -ENOMEM;
 
+	psy_cfg.drv_data = bat;
+
 	bat->da9052 = dev_get_drvdata(pdev->dev.parent);
-	bat->psy = template_battery;
 	bat->charger_type = DA9052_NOCHARGER;
 	bat->status = POWER_SUPPLY_STATUS_UNKNOWN;
 	bat->health = POWER_SUPPLY_HEALTH_UNKNOWN;
@@ -608,9 +609,9 @@ static s32 da9052_bat_probe(struct platform_device *pdev)
 
 	pdata = bat->da9052->dev->platform_data;
 	if (pdata != NULL && pdata->use_for_apm)
-		bat->psy.use_for_apm = pdata->use_for_apm;
+		psy_desc.use_for_apm = pdata->use_for_apm;
 	else
-		bat->psy.use_for_apm = 1;
+		psy_desc.use_for_apm = 1;
 
 	for (i = 0; i < ARRAY_SIZE(da9052_bat_irqs); i++) {
 		ret = da9052_request_irq(bat->da9052,
@@ -625,9 +626,11 @@ static s32 da9052_bat_probe(struct platform_device *pdev)
 		}
 	}
 
-	ret = power_supply_register(&pdev->dev, &bat->psy);
-	 if (ret)
+	bat->psy = power_supply_register(&pdev->dev, &psy_desc, &psy_cfg);
+	if (IS_ERR(bat->psy)) {
+		ret = PTR_ERR(bat->psy);
 		goto err;
+	}
 
 	platform_set_drvdata(pdev, bat);
 	return 0;
@@ -646,7 +649,7 @@ static int da9052_bat_remove(struct platform_device *pdev)
 	for (i = 0; i < ARRAY_SIZE(da9052_bat_irqs); i++)
 		da9052_free_irq(bat->da9052, da9052_bat_irq_bits[i], bat);
 
-	power_supply_unregister(&bat->psy);
+	power_supply_unregister(bat->psy);
 
 	return 0;
 }
diff --git a/drivers/power/da9150-charger.c b/drivers/power/da9150-charger.c
new file mode 100644
index 000000000000..60099815296e
--- /dev/null
+++ b/drivers/power/da9150-charger.c
@@ -0,0 +1,694 @@
+/*
+ * DA9150 Charger Driver
+ *
+ * Copyright (c) 2014 Dialog Semiconductor
+ *
+ * Author: Adam Thomson <Adam.Thomson.Opensource@diasemi.com>
+ *
+ * This program is free software; you can redistribute  it and/or modify it
+ * under  the terms of  the GNU General  Public License as published by the
+ * Free Software Foundation;  either version 2 of the  License, or (at your
+ * option) any later version.
+ */
+
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/of.h>
+#include <linux/of_platform.h>
+#include <linux/interrupt.h>
+#include <linux/power_supply.h>
+#include <linux/notifier.h>
+#include <linux/usb/phy.h>
+#include <linux/iio/consumer.h>
+#include <linux/mfd/da9150/core.h>
+#include <linux/mfd/da9150/registers.h>
+
+/* Private data */
+struct da9150_charger {
+	struct da9150 *da9150;
+	struct device *dev;
+
+	struct power_supply *usb;
+	struct power_supply *battery;
+	struct power_supply *supply_online;
+
+	struct usb_phy *usb_phy;
+	struct notifier_block otg_nb;
+	struct work_struct otg_work;
+	unsigned long usb_event;
+
+	struct iio_channel *ibus_chan;
+	struct iio_channel *vbus_chan;
+	struct iio_channel *tjunc_chan;
+	struct iio_channel *vbat_chan;
+};
+
+static inline int da9150_charger_supply_online(struct da9150_charger *charger,
+					       struct power_supply *psy,
+					       union power_supply_propval *val)
+{
+	val->intval = (psy == charger->supply_online) ? 1 : 0;
+
+	return 0;
+}
+
+/* Charger Properties */
+static int da9150_charger_vbus_voltage_now(struct da9150_charger *charger,
+					   union power_supply_propval *val)
+{
+	int v_val, ret;
+
+	/* Read processed value - mV units */
+	ret = iio_read_channel_processed(charger->vbus_chan, &v_val);
+	if (ret < 0)
+		return ret;
+
+	/* Convert voltage to expected uV units */
+	val->intval = v_val * 1000;
+
+	return 0;
+}
+
+static int da9150_charger_ibus_current_avg(struct da9150_charger *charger,
+					   union power_supply_propval *val)
+{
+	int i_val, ret;
+
+	/* Read processed value - mA units */
+	ret = iio_read_channel_processed(charger->ibus_chan, &i_val);
+	if (ret < 0)
+		return ret;
+
+	/* Convert current to expected uA units */
+	val->intval = i_val * 1000;
+
+	return 0;
+}
+
+static int da9150_charger_tjunc_temp(struct da9150_charger *charger,
+				     union power_supply_propval *val)
+{
+	int t_val, ret;
+
+	/* Read processed value - 0.001 degrees C units */
+	ret = iio_read_channel_processed(charger->tjunc_chan, &t_val);
+	if (ret < 0)
+		return ret;
+
+	/* Convert temp to expect 0.1 degrees C units */
+	val->intval = t_val / 100;
+
+	return 0;
+}
+
+static enum power_supply_property da9150_charger_props[] = {
+	POWER_SUPPLY_PROP_ONLINE,
+	POWER_SUPPLY_PROP_VOLTAGE_NOW,
+	POWER_SUPPLY_PROP_CURRENT_AVG,
+	POWER_SUPPLY_PROP_TEMP,
+};
+
+static int da9150_charger_get_prop(struct power_supply *psy,
+				   enum power_supply_property psp,
+				   union power_supply_propval *val)
+{
+	struct da9150_charger *charger = dev_get_drvdata(psy->dev.parent);
+	int ret;
+
+	switch (psp) {
+	case POWER_SUPPLY_PROP_ONLINE:
+		ret = da9150_charger_supply_online(charger, psy, val);
+		break;
+	case POWER_SUPPLY_PROP_VOLTAGE_NOW:
+		ret = da9150_charger_vbus_voltage_now(charger, val);
+		break;
+	case POWER_SUPPLY_PROP_CURRENT_AVG:
+		ret = da9150_charger_ibus_current_avg(charger, val);
+		break;
+	case POWER_SUPPLY_PROP_TEMP:
+		ret = da9150_charger_tjunc_temp(charger, val);
+		break;
+	default:
+		ret = -EINVAL;
+		break;
+	}
+
+	return ret;
+}
+
+/* Battery Properties */
+static int da9150_charger_battery_status(struct da9150_charger *charger,
+					 union power_supply_propval *val)
+{
+	u8 reg;
+
+	/* Check to see if battery is discharging */
+	reg = da9150_reg_read(charger->da9150, DA9150_STATUS_H);
+
+	if (((reg & DA9150_VBUS_STAT_MASK) == DA9150_VBUS_STAT_OFF) ||
+	    ((reg & DA9150_VBUS_STAT_MASK) == DA9150_VBUS_STAT_WAIT)) {
+		val->intval = POWER_SUPPLY_STATUS_DISCHARGING;
+
+		return 0;
+	}
+
+	reg = da9150_reg_read(charger->da9150, DA9150_STATUS_J);
+
+	/* Now check for other states */
+	switch (reg & DA9150_CHG_STAT_MASK) {
+	case DA9150_CHG_STAT_ACT:
+	case DA9150_CHG_STAT_PRE:
+	case DA9150_CHG_STAT_CC:
+	case DA9150_CHG_STAT_CV:
+		val->intval = POWER_SUPPLY_STATUS_CHARGING;
+		break;
+	case DA9150_CHG_STAT_OFF:
+	case DA9150_CHG_STAT_SUSP:
+	case DA9150_CHG_STAT_TEMP:
+	case DA9150_CHG_STAT_TIME:
+	case DA9150_CHG_STAT_BAT:
+		val->intval = POWER_SUPPLY_STATUS_NOT_CHARGING;
+		break;
+	case DA9150_CHG_STAT_FULL:
+		val->intval = POWER_SUPPLY_STATUS_FULL;
+		break;
+	default:
+		val->intval = POWER_SUPPLY_STATUS_UNKNOWN;
+		break;
+	}
+
+	return 0;
+}
+
+static int da9150_charger_battery_health(struct da9150_charger *charger,
+					 union power_supply_propval *val)
+{
+	u8 reg;
+
+	reg = da9150_reg_read(charger->da9150, DA9150_STATUS_J);
+
+	/* Check if temperature limit reached */
+	switch (reg & DA9150_CHG_TEMP_MASK) {
+	case DA9150_CHG_TEMP_UNDER:
+		val->intval = POWER_SUPPLY_HEALTH_COLD;
+		return 0;
+	case DA9150_CHG_TEMP_OVER:
+		val->intval = POWER_SUPPLY_HEALTH_OVERHEAT;
+		return 0;
+	default:
+		break;
+	}
+
+	/* Check for other health states */
+	switch (reg & DA9150_CHG_STAT_MASK) {
+	case DA9150_CHG_STAT_ACT:
+	case DA9150_CHG_STAT_PRE:
+		val->intval = POWER_SUPPLY_HEALTH_DEAD;
+		break;
+	case DA9150_CHG_STAT_TIME:
+		val->intval = POWER_SUPPLY_HEALTH_UNSPEC_FAILURE;
+		break;
+	default:
+		val->intval = POWER_SUPPLY_HEALTH_GOOD;
+		break;
+	}
+
+	return 0;
+}
+
+static int da9150_charger_battery_present(struct da9150_charger *charger,
+					  union power_supply_propval *val)
+{
+	u8 reg;
+
+	/* Check if battery present or removed */
+	reg = da9150_reg_read(charger->da9150, DA9150_STATUS_J);
+	if ((reg & DA9150_CHG_STAT_MASK) == DA9150_CHG_STAT_BAT)
+		val->intval = 0;
+	else
+		val->intval = 1;
+
+	return 0;
+}
+
+static int da9150_charger_battery_charge_type(struct da9150_charger *charger,
+					      union power_supply_propval *val)
+{
+	u8 reg;
+
+	reg = da9150_reg_read(charger->da9150, DA9150_STATUS_J);
+
+	switch (reg & DA9150_CHG_STAT_MASK) {
+	case DA9150_CHG_STAT_CC:
+		val->intval = POWER_SUPPLY_CHARGE_TYPE_FAST;
+		break;
+	case DA9150_CHG_STAT_ACT:
+	case DA9150_CHG_STAT_PRE:
+	case DA9150_CHG_STAT_CV:
+		val->intval = POWER_SUPPLY_CHARGE_TYPE_TRICKLE;
+		break;
+	default:
+		val->intval = POWER_SUPPLY_CHARGE_TYPE_NONE;
+		break;
+	}
+
+	return 0;
+}
+
+static int da9150_charger_battery_voltage_min(struct da9150_charger *charger,
+					      union power_supply_propval *val)
+{
+	u8 reg;
+
+	reg = da9150_reg_read(charger->da9150, DA9150_PPR_CHGCTRL_C);
+
+	/* Value starts at 2500 mV, 50 mV increments, presented in uV */
+	val->intval = ((reg & DA9150_CHG_VFAULT_MASK) * 50000) + 2500000;
+
+	return 0;
+}
+
+static int da9150_charger_battery_voltage_now(struct da9150_charger *charger,
+					      union power_supply_propval *val)
+{
+	int v_val, ret;
+
+	/* Read processed value - mV units */
+	ret = iio_read_channel_processed(charger->vbat_chan, &v_val);
+	if (ret < 0)
+		return ret;
+
+	val->intval = v_val * 1000;
+
+	return 0;
+}
+
+static int da9150_charger_battery_current_max(struct da9150_charger *charger,
+					      union power_supply_propval *val)
+{
+	int reg;
+
+	reg = da9150_reg_read(charger->da9150, DA9150_PPR_CHGCTRL_D);
+
+	/* 25mA increments */
+	val->intval = reg * 25000;
+
+	return 0;
+}
+
+static int da9150_charger_battery_voltage_max(struct da9150_charger *charger,
+					      union power_supply_propval *val)
+{
+	u8 reg;
+
+	reg = da9150_reg_read(charger->da9150, DA9150_PPR_CHGCTRL_B);
+
+	/* Value starts at 3650 mV, 25 mV increments, presented in uV */
+	val->intval = ((reg & DA9150_CHG_VBAT_MASK) * 25000) + 3650000;
+	return 0;
+}
+
+static enum power_supply_property da9150_charger_bat_props[] = {
+	POWER_SUPPLY_PROP_STATUS,
+	POWER_SUPPLY_PROP_ONLINE,
+	POWER_SUPPLY_PROP_HEALTH,
+	POWER_SUPPLY_PROP_PRESENT,
+	POWER_SUPPLY_PROP_CHARGE_TYPE,
+	POWER_SUPPLY_PROP_VOLTAGE_MIN_DESIGN,
+	POWER_SUPPLY_PROP_VOLTAGE_NOW,
+	POWER_SUPPLY_PROP_CONSTANT_CHARGE_CURRENT_MAX,
+	POWER_SUPPLY_PROP_CONSTANT_CHARGE_VOLTAGE_MAX,
+};
+
+static int da9150_charger_battery_get_prop(struct power_supply *psy,
+					   enum power_supply_property psp,
+					   union power_supply_propval *val)
+{
+	struct da9150_charger *charger = dev_get_drvdata(psy->dev.parent);
+	int ret;
+
+	switch (psp) {
+	case POWER_SUPPLY_PROP_STATUS:
+		ret = da9150_charger_battery_status(charger, val);
+		break;
+	case POWER_SUPPLY_PROP_ONLINE:
+		ret = da9150_charger_supply_online(charger, psy, val);
+		break;
+	case POWER_SUPPLY_PROP_HEALTH:
+		ret = da9150_charger_battery_health(charger, val);
+		break;
+	case POWER_SUPPLY_PROP_PRESENT:
+		ret = da9150_charger_battery_present(charger, val);
+		break;
+	case POWER_SUPPLY_PROP_CHARGE_TYPE:
+		ret = da9150_charger_battery_charge_type(charger, val);
+		break;
+	case POWER_SUPPLY_PROP_VOLTAGE_MIN_DESIGN:
+		ret = da9150_charger_battery_voltage_min(charger, val);
+		break;
+	case POWER_SUPPLY_PROP_VOLTAGE_NOW:
+		ret = da9150_charger_battery_voltage_now(charger, val);
+		break;
+	case POWER_SUPPLY_PROP_CONSTANT_CHARGE_CURRENT_MAX:
+		ret = da9150_charger_battery_current_max(charger, val);
+		break;
+	case POWER_SUPPLY_PROP_CONSTANT_CHARGE_VOLTAGE_MAX:
+		ret = da9150_charger_battery_voltage_max(charger, val);
+		break;
+	default:
+		ret = -EINVAL;
+		break;
+	}
+
+	return ret;
+}
+
+static irqreturn_t da9150_charger_chg_irq(int irq, void *data)
+{
+	struct da9150_charger *charger = data;
+
+	power_supply_changed(charger->battery);
+
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t da9150_charger_tjunc_irq(int irq, void *data)
+{
+	struct da9150_charger *charger = data;
+
+	/* Nothing we can really do except report this. */
+	dev_crit(charger->dev, "TJunc over temperature!!!\n");
+	power_supply_changed(charger->usb);
+
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t da9150_charger_vfault_irq(int irq, void *data)
+{
+	struct da9150_charger *charger = data;
+
+	/* Nothing we can really do except report this. */
+	dev_crit(charger->dev, "VSYS under voltage!!!\n");
+	power_supply_changed(charger->usb);
+	power_supply_changed(charger->battery);
+
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t da9150_charger_vbus_irq(int irq, void *data)
+{
+	struct da9150_charger *charger = data;
+	u8 reg;
+
+	reg = da9150_reg_read(charger->da9150, DA9150_STATUS_H);
+
+	/* Charger plugged in or battery only */
+	switch (reg & DA9150_VBUS_STAT_MASK) {
+	case DA9150_VBUS_STAT_OFF:
+	case DA9150_VBUS_STAT_WAIT:
+		charger->supply_online = charger->battery;
+		break;
+	case DA9150_VBUS_STAT_CHG:
+		charger->supply_online = charger->usb;
+		break;
+	default:
+		dev_warn(charger->dev, "Unknown VBUS state - reg = 0x%x\n",
+			 reg);
+		charger->supply_online = NULL;
+		break;
+	}
+
+	power_supply_changed(charger->usb);
+	power_supply_changed(charger->battery);
+
+	return IRQ_HANDLED;
+}
+
+static void da9150_charger_otg_work(struct work_struct *data)
+{
+	struct da9150_charger *charger =
+		container_of(data, struct da9150_charger, otg_work);
+
+	switch (charger->usb_event) {
+	case USB_EVENT_ID:
+		/* Enable OTG Boost */
+		da9150_set_bits(charger->da9150, DA9150_PPR_BKCTRL_A,
+				DA9150_VBUS_MODE_MASK, DA9150_VBUS_MODE_OTG);
+		break;
+	case USB_EVENT_NONE:
+		/* Revert to charge mode */
+		power_supply_changed(charger->usb);
+		power_supply_changed(charger->battery);
+		da9150_set_bits(charger->da9150, DA9150_PPR_BKCTRL_A,
+				DA9150_VBUS_MODE_MASK, DA9150_VBUS_MODE_CHG);
+		break;
+	}
+}
+
+static int da9150_charger_otg_ncb(struct notifier_block *nb, unsigned long val,
+				  void *priv)
+{
+	struct da9150_charger *charger =
+		container_of(nb, struct da9150_charger, otg_nb);
+
+	dev_dbg(charger->dev, "DA9150 OTG notify %lu\n", val);
+
+	charger->usb_event = val;
+	schedule_work(&charger->otg_work);
+
+	return NOTIFY_OK;
+}
+
+static int da9150_charger_register_irq(struct platform_device *pdev,
+				       irq_handler_t handler,
+				       const char *irq_name)
+{
+	struct device *dev = &pdev->dev;
+	struct da9150_charger *charger = platform_get_drvdata(pdev);
+	int irq, ret;
+
+	irq = platform_get_irq_byname(pdev, irq_name);
+	if (irq < 0) {
+		dev_err(dev, "Failed to get IRQ CHG_STATUS: %d\n", irq);
+		return irq;
+	}
+
+	ret = request_threaded_irq(irq, NULL, handler, IRQF_ONESHOT, irq_name,
+				   charger);
+	if (ret)
+		dev_err(dev, "Failed to request IRQ %d: %d\n", irq, ret);
+
+	return ret;
+}
+
+static void da9150_charger_unregister_irq(struct platform_device *pdev,
+					  const char *irq_name)
+{
+	struct device *dev = &pdev->dev;
+	struct da9150_charger *charger = platform_get_drvdata(pdev);
+	int irq;
+
+	irq = platform_get_irq_byname(pdev, irq_name);
+	if (irq < 0) {
+		dev_err(dev, "Failed to get IRQ CHG_STATUS: %d\n", irq);
+		return;
+	}
+
+	free_irq(irq, charger);
+}
+
+static const struct power_supply_desc usb_desc = {
+	.name		= "da9150-usb",
+	.type		= POWER_SUPPLY_TYPE_USB,
+	.properties	= da9150_charger_props,
+	.num_properties	= ARRAY_SIZE(da9150_charger_props),
+	.get_property	= da9150_charger_get_prop,
+};
+
+static const struct power_supply_desc battery_desc = {
+	.name		= "da9150-battery",
+	.type		= POWER_SUPPLY_TYPE_BATTERY,
+	.properties	= da9150_charger_bat_props,
+	.num_properties	= ARRAY_SIZE(da9150_charger_bat_props),
+	.get_property	= da9150_charger_battery_get_prop,
+};
+
+static int da9150_charger_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct da9150 *da9150 = dev_get_drvdata(dev->parent);
+	struct da9150_charger *charger;
+	u8 reg;
+	int ret;
+
+	charger = devm_kzalloc(dev, sizeof(struct da9150_charger), GFP_KERNEL);
+	if (!charger)
+		return -ENOMEM;
+
+	platform_set_drvdata(pdev, charger);
+	charger->da9150 = da9150;
+	charger->dev = dev;
+
+	/* Acquire ADC channels */
+	charger->ibus_chan = iio_channel_get(dev, "CHAN_IBUS");
+	if (IS_ERR(charger->ibus_chan)) {
+		ret = PTR_ERR(charger->ibus_chan);
+		goto ibus_chan_fail;
+	}
+
+	charger->vbus_chan = iio_channel_get(dev, "CHAN_VBUS");
+	if (IS_ERR(charger->vbus_chan)) {
+		ret = PTR_ERR(charger->vbus_chan);
+		goto vbus_chan_fail;
+	}
+
+	charger->tjunc_chan = iio_channel_get(dev, "CHAN_TJUNC");
+	if (IS_ERR(charger->tjunc_chan)) {
+		ret = PTR_ERR(charger->tjunc_chan);
+		goto tjunc_chan_fail;
+	}
+
+	charger->vbat_chan = iio_channel_get(dev, "CHAN_VBAT");
+	if (IS_ERR(charger->vbat_chan)) {
+		ret = PTR_ERR(charger->vbat_chan);
+		goto vbat_chan_fail;
+	}
+
+	/* Register power supplies */
+	charger->usb = power_supply_register(dev, &usb_desc, NULL);
+	if (IS_ERR(charger->usb)) {
+		ret = PTR_ERR(charger->usb);
+		goto usb_fail;
+	}
+
+	charger->battery = power_supply_register(dev, &battery_desc, NULL);
+	if (IS_ERR(charger->battery)) {
+		ret = PTR_ERR(charger->battery);
+		goto battery_fail;
+	}
+
+	/* Get initial online supply */
+	reg = da9150_reg_read(da9150, DA9150_STATUS_H);
+
+	switch (reg & DA9150_VBUS_STAT_MASK) {
+	case DA9150_VBUS_STAT_OFF:
+	case DA9150_VBUS_STAT_WAIT:
+		charger->supply_online = charger->battery;
+		break;
+	case DA9150_VBUS_STAT_CHG:
+		charger->supply_online = charger->usb;
+		break;
+	default:
+		dev_warn(dev, "Unknown VBUS state - reg = 0x%x\n", reg);
+		charger->supply_online = NULL;
+		break;
+	}
+
+	/* Setup OTG reporting & configuration */
+	charger->usb_phy = devm_usb_get_phy(dev, USB_PHY_TYPE_USB2);
+	if (!IS_ERR_OR_NULL(charger->usb_phy)) {
+		INIT_WORK(&charger->otg_work, da9150_charger_otg_work);
+		charger->otg_nb.notifier_call = da9150_charger_otg_ncb;
+		usb_register_notifier(charger->usb_phy, &charger->otg_nb);
+	}
+
+	/* Register IRQs */
+	ret = da9150_charger_register_irq(pdev, da9150_charger_chg_irq,
+					  "CHG_STATUS");
+	if (ret < 0)
+		goto chg_irq_fail;
+
+	ret = da9150_charger_register_irq(pdev, da9150_charger_tjunc_irq,
+					  "CHG_TJUNC");
+	if (ret < 0)
+		goto tjunc_irq_fail;
+
+	ret = da9150_charger_register_irq(pdev, da9150_charger_vfault_irq,
+					  "CHG_VFAULT");
+	if (ret < 0)
+		goto vfault_irq_fail;
+
+	ret = da9150_charger_register_irq(pdev, da9150_charger_vbus_irq,
+					  "CHG_VBUS");
+	if (ret < 0)
+		goto vbus_irq_fail;
+
+	return 0;
+
+
+vbus_irq_fail:
+	da9150_charger_unregister_irq(pdev, "CHG_VFAULT");
+vfault_irq_fail:
+	da9150_charger_unregister_irq(pdev, "CHG_TJUNC");
+tjunc_irq_fail:
+	da9150_charger_unregister_irq(pdev, "CHG_STATUS");
+chg_irq_fail:
+	if (!IS_ERR_OR_NULL(charger->usb_phy))
+		usb_unregister_notifier(charger->usb_phy, &charger->otg_nb);
+battery_fail:
+	power_supply_unregister(charger->usb);
+
+usb_fail:
+	iio_channel_release(charger->vbat_chan);
+
+vbat_chan_fail:
+	iio_channel_release(charger->tjunc_chan);
+
+tjunc_chan_fail:
+	iio_channel_release(charger->vbus_chan);
+
+vbus_chan_fail:
+	iio_channel_release(charger->ibus_chan);
+
+ibus_chan_fail:
+	return ret;
+}
+
+static int da9150_charger_remove(struct platform_device *pdev)
+{
+	struct da9150_charger *charger = platform_get_drvdata(pdev);
+	int irq;
+
+	/* Make sure IRQs are released before unregistering power supplies */
+	irq = platform_get_irq_byname(pdev, "CHG_VBUS");
+	free_irq(irq, charger);
+
+	irq = platform_get_irq_byname(pdev, "CHG_VFAULT");
+	free_irq(irq, charger);
+
+	irq = platform_get_irq_byname(pdev, "CHG_TJUNC");
+	free_irq(irq, charger);
+
+	irq = platform_get_irq_byname(pdev, "CHG_STATUS");
+	free_irq(irq, charger);
+
+	if (!IS_ERR_OR_NULL(charger->usb_phy))
+		usb_unregister_notifier(charger->usb_phy, &charger->otg_nb);
+
+	power_supply_unregister(charger->battery);
+	power_supply_unregister(charger->usb);
+
+	/* Release ADC channels */
+	iio_channel_release(charger->ibus_chan);
+	iio_channel_release(charger->vbus_chan);
+	iio_channel_release(charger->tjunc_chan);
+	iio_channel_release(charger->vbat_chan);
+
+	return 0;
+}
+
+static struct platform_driver da9150_charger_driver = {
+	.driver = {
+		.name = "da9150-charger",
+	},
+	.probe = da9150_charger_probe,
+	.remove = da9150_charger_remove,
+};
+
+module_platform_driver(da9150_charger_driver);
+
+MODULE_DESCRIPTION("Charger Driver for DA9150");
+MODULE_AUTHOR("Adam Thomson <Adam.Thomson.Opensource@diasemi.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/power/ds2760_battery.c b/drivers/power/ds2760_battery.c
index 85b4e6eca0b1..80f73ccb77ab 100644
--- a/drivers/power/ds2760_battery.c
+++ b/drivers/power/ds2760_battery.c
@@ -53,7 +53,8 @@ struct ds2760_device_info {
 	int charge_status;		/* POWER_SUPPLY_STATUS_* */
 
 	int full_counter;
-	struct power_supply bat;
+	struct power_supply *bat;
+	struct power_supply_desc bat_desc;
 	struct device *w1_dev;
 	struct workqueue_struct *monitor_wqueue;
 	struct delayed_work monitor_work;
@@ -254,7 +255,7 @@ static void ds2760_battery_update_status(struct ds2760_device_info *di)
 	if (di->charge_status == POWER_SUPPLY_STATUS_UNKNOWN)
 		di->full_counter = 0;
 
-	if (power_supply_am_i_supplied(&di->bat)) {
+	if (power_supply_am_i_supplied(di->bat)) {
 		if (di->current_uA > 10000) {
 			di->charge_status = POWER_SUPPLY_STATUS_CHARGING;
 			di->full_counter = 0;
@@ -287,7 +288,7 @@ static void ds2760_battery_update_status(struct ds2760_device_info *di)
 	}
 
 	if (di->charge_status != old_charge_status)
-		power_supply_changed(&di->bat);
+		power_supply_changed(di->bat);
 }
 
 static void ds2760_battery_write_status(struct ds2760_device_info *di,
@@ -346,12 +347,9 @@ static void ds2760_battery_work(struct work_struct *work)
 	queue_delayed_work(di->monitor_wqueue, &di->monitor_work, interval);
 }
 
-#define to_ds2760_device_info(x) container_of((x), struct ds2760_device_info, \
-					      bat);
-
 static void ds2760_battery_external_power_changed(struct power_supply *psy)
 {
-	struct ds2760_device_info *di = to_ds2760_device_info(psy);
+	struct ds2760_device_info *di = power_supply_get_drvdata(psy);
 
 	dev_dbg(di->dev, "%s\n", __func__);
 
@@ -377,7 +375,7 @@ static void ds2760_battery_set_charged_work(struct work_struct *work)
 	 * that error.
 	 */
 
-	if (!power_supply_am_i_supplied(&di->bat))
+	if (!power_supply_am_i_supplied(di->bat))
 		return;
 
 	bias = (signed char) di->current_raw +
@@ -396,7 +394,7 @@ static void ds2760_battery_set_charged_work(struct work_struct *work)
 
 static void ds2760_battery_set_charged(struct power_supply *psy)
 {
-	struct ds2760_device_info *di = to_ds2760_device_info(psy);
+	struct ds2760_device_info *di = power_supply_get_drvdata(psy);
 
 	/* postpone the actual work by 20 secs. This is for debouncing GPIO
 	 * signals and to let the current value settle. See AN4188. */
@@ -407,7 +405,7 @@ static int ds2760_battery_get_property(struct power_supply *psy,
 				       enum power_supply_property psp,
 				       union power_supply_propval *val)
 {
-	struct ds2760_device_info *di = to_ds2760_device_info(psy);
+	struct ds2760_device_info *di = power_supply_get_drvdata(psy);
 
 	switch (psp) {
 	case POWER_SUPPLY_PROP_STATUS:
@@ -458,7 +456,7 @@ static int ds2760_battery_set_property(struct power_supply *psy,
 				       enum power_supply_property psp,
 				       const union power_supply_propval *val)
 {
-	struct ds2760_device_info *di = to_ds2760_device_info(psy);
+	struct ds2760_device_info *di = power_supply_get_drvdata(psy);
 
 	switch (psp) {
 	case POWER_SUPPLY_PROP_CHARGE_FULL:
@@ -508,6 +506,7 @@ static enum power_supply_property ds2760_battery_props[] = {
 
 static int ds2760_battery_probe(struct platform_device *pdev)
 {
+	struct power_supply_config psy_cfg = {};
 	char status;
 	int retval = 0;
 	struct ds2760_device_info *di;
@@ -520,20 +519,22 @@ static int ds2760_battery_probe(struct platform_device *pdev)
 
 	platform_set_drvdata(pdev, di);
 
-	di->dev			= &pdev->dev;
-	di->w1_dev		= pdev->dev.parent;
-	di->bat.name		= dev_name(&pdev->dev);
-	di->bat.type		= POWER_SUPPLY_TYPE_BATTERY;
-	di->bat.properties	= ds2760_battery_props;
-	di->bat.num_properties	= ARRAY_SIZE(ds2760_battery_props);
-	di->bat.get_property	= ds2760_battery_get_property;
-	di->bat.set_property	= ds2760_battery_set_property;
-	di->bat.property_is_writeable =
+	di->dev				= &pdev->dev;
+	di->w1_dev			= pdev->dev.parent;
+	di->bat_desc.name		= dev_name(&pdev->dev);
+	di->bat_desc.type		= POWER_SUPPLY_TYPE_BATTERY;
+	di->bat_desc.properties		= ds2760_battery_props;
+	di->bat_desc.num_properties	= ARRAY_SIZE(ds2760_battery_props);
+	di->bat_desc.get_property	= ds2760_battery_get_property;
+	di->bat_desc.set_property	= ds2760_battery_set_property;
+	di->bat_desc.property_is_writeable =
 				  ds2760_battery_property_is_writeable;
-	di->bat.set_charged	= ds2760_battery_set_charged;
-	di->bat.external_power_changed =
+	di->bat_desc.set_charged	= ds2760_battery_set_charged;
+	di->bat_desc.external_power_changed =
 				  ds2760_battery_external_power_changed;
 
+	psy_cfg.drv_data		= di;
+
 	di->charge_status = POWER_SUPPLY_STATUS_UNKNOWN;
 
 	/* enable sleep mode feature */
@@ -555,9 +556,10 @@ static int ds2760_battery_probe(struct platform_device *pdev)
 	if (current_accum)
 		ds2760_battery_set_current_accum(di, current_accum);
 
-	retval = power_supply_register(&pdev->dev, &di->bat);
-	if (retval) {
+	di->bat = power_supply_register(&pdev->dev, &di->bat_desc, &psy_cfg);
+	if (IS_ERR(di->bat)) {
 		dev_err(di->dev, "failed to register battery\n");
+		retval = PTR_ERR(di->bat);
 		goto batt_failed;
 	}
 
@@ -574,7 +576,7 @@ static int ds2760_battery_probe(struct platform_device *pdev)
 	goto success;
 
 workqueue_failed:
-	power_supply_unregister(&di->bat);
+	power_supply_unregister(di->bat);
 batt_failed:
 di_alloc_failed:
 success:
@@ -588,7 +590,7 @@ static int ds2760_battery_remove(struct platform_device *pdev)
 	cancel_delayed_work_sync(&di->monitor_work);
 	cancel_delayed_work_sync(&di->set_charged_work);
 	destroy_workqueue(di->monitor_wqueue);
-	power_supply_unregister(&di->bat);
+	power_supply_unregister(di->bat);
 
 	return 0;
 }
@@ -610,7 +612,7 @@ static int ds2760_battery_resume(struct platform_device *pdev)
 	struct ds2760_device_info *di = platform_get_drvdata(pdev);
 
 	di->charge_status = POWER_SUPPLY_STATUS_UNKNOWN;
-	power_supply_changed(&di->bat);
+	power_supply_changed(di->bat);
 
 	mod_delayed_work(di->monitor_wqueue, &di->monitor_work, HZ);
 
diff --git a/drivers/power/ds2780_battery.c b/drivers/power/ds2780_battery.c
index 9f418fa879e5..a7a0427343f3 100644
--- a/drivers/power/ds2780_battery.c
+++ b/drivers/power/ds2780_battery.c
@@ -37,7 +37,8 @@
 
 struct ds2780_device_info {
 	struct device *dev;
-	struct power_supply bat;
+	struct power_supply *bat;
+	struct power_supply_desc bat_desc;
 	struct device *w1_dev;
 };
 
@@ -52,7 +53,7 @@ static const char manufacturer[] = "Maxim/Dallas";
 static inline struct ds2780_device_info *
 to_ds2780_device_info(struct power_supply *psy)
 {
-	return container_of(psy, struct ds2780_device_info, bat);
+	return power_supply_get_drvdata(psy);
 }
 
 static inline struct power_supply *to_power_supply(struct device *dev)
@@ -757,6 +758,7 @@ static const struct attribute_group ds2780_attr_group = {
 
 static int ds2780_battery_probe(struct platform_device *pdev)
 {
+	struct power_supply_config psy_cfg = {};
 	int ret = 0;
 	struct ds2780_device_info *dev_info;
 
@@ -770,25 +772,29 @@ static int ds2780_battery_probe(struct platform_device *pdev)
 
 	dev_info->dev			= &pdev->dev;
 	dev_info->w1_dev		= pdev->dev.parent;
-	dev_info->bat.name		= dev_name(&pdev->dev);
-	dev_info->bat.type		= POWER_SUPPLY_TYPE_BATTERY;
-	dev_info->bat.properties	= ds2780_battery_props;
-	dev_info->bat.num_properties	= ARRAY_SIZE(ds2780_battery_props);
-	dev_info->bat.get_property	= ds2780_battery_get_property;
+	dev_info->bat_desc.name		= dev_name(&pdev->dev);
+	dev_info->bat_desc.type		= POWER_SUPPLY_TYPE_BATTERY;
+	dev_info->bat_desc.properties	= ds2780_battery_props;
+	dev_info->bat_desc.num_properties = ARRAY_SIZE(ds2780_battery_props);
+	dev_info->bat_desc.get_property	= ds2780_battery_get_property;
 
-	ret = power_supply_register(&pdev->dev, &dev_info->bat);
-	if (ret) {
+	psy_cfg.drv_data		= dev_info;
+
+	dev_info->bat = power_supply_register(&pdev->dev, &dev_info->bat_desc,
+					      &psy_cfg);
+	if (IS_ERR(dev_info->bat)) {
 		dev_err(dev_info->dev, "failed to register battery\n");
+		ret = PTR_ERR(dev_info->bat);
 		goto fail;
 	}
 
-	ret = sysfs_create_group(&dev_info->bat.dev->kobj, &ds2780_attr_group);
+	ret = sysfs_create_group(&dev_info->bat->dev.kobj, &ds2780_attr_group);
 	if (ret) {
 		dev_err(dev_info->dev, "failed to create sysfs group\n");
 		goto fail_unregister;
 	}
 
-	ret = sysfs_create_bin_file(&dev_info->bat.dev->kobj,
+	ret = sysfs_create_bin_file(&dev_info->bat->dev.kobj,
 					&ds2780_param_eeprom_bin_attr);
 	if (ret) {
 		dev_err(dev_info->dev,
@@ -796,7 +802,7 @@ static int ds2780_battery_probe(struct platform_device *pdev)
 		goto fail_remove_group;
 	}
 
-	ret = sysfs_create_bin_file(&dev_info->bat.dev->kobj,
+	ret = sysfs_create_bin_file(&dev_info->bat->dev.kobj,
 					&ds2780_user_eeprom_bin_attr);
 	if (ret) {
 		dev_err(dev_info->dev,
@@ -807,12 +813,12 @@ static int ds2780_battery_probe(struct platform_device *pdev)
 	return 0;
 
 fail_remove_bin_file:
-	sysfs_remove_bin_file(&dev_info->bat.dev->kobj,
+	sysfs_remove_bin_file(&dev_info->bat->dev.kobj,
 				&ds2780_param_eeprom_bin_attr);
 fail_remove_group:
-	sysfs_remove_group(&dev_info->bat.dev->kobj, &ds2780_attr_group);
+	sysfs_remove_group(&dev_info->bat->dev.kobj, &ds2780_attr_group);
 fail_unregister:
-	power_supply_unregister(&dev_info->bat);
+	power_supply_unregister(dev_info->bat);
 fail:
 	return ret;
 }
@@ -821,10 +827,13 @@ static int ds2780_battery_remove(struct platform_device *pdev)
 {
 	struct ds2780_device_info *dev_info = platform_get_drvdata(pdev);
 
-	/* remove attributes */
-	sysfs_remove_group(&dev_info->bat.dev->kobj, &ds2780_attr_group);
+	/*
+	 * Remove attributes before unregistering power supply
+	 * because 'bat' will be freed on power_supply_unregister() call.
+	 */
+	sysfs_remove_group(&dev_info->bat->dev.kobj, &ds2780_attr_group);
 
-	power_supply_unregister(&dev_info->bat);
+	power_supply_unregister(dev_info->bat);
 
 	return 0;
 }
diff --git a/drivers/power/ds2781_battery.c b/drivers/power/ds2781_battery.c
index 0a5acc6fc6f0..56d583dae908 100644
--- a/drivers/power/ds2781_battery.c
+++ b/drivers/power/ds2781_battery.c
@@ -35,7 +35,8 @@
 
 struct ds2781_device_info {
 	struct device *dev;
-	struct power_supply bat;
+	struct power_supply *bat;
+	struct power_supply_desc bat_desc;
 	struct device *w1_dev;
 };
 
@@ -50,7 +51,7 @@ static const char manufacturer[] = "Maxim/Dallas";
 static inline struct ds2781_device_info *
 to_ds2781_device_info(struct power_supply *psy)
 {
-	return container_of(psy, struct ds2781_device_info, bat);
+	return power_supply_get_drvdata(psy);
 }
 
 static inline struct power_supply *to_power_supply(struct device *dev)
@@ -328,7 +329,7 @@ static int ds2781_get_status(struct ds2781_device_info *dev_info, int *status)
 	if (ret < 0)
 		return ret;
 
-	if (power_supply_am_i_supplied(&dev_info->bat)) {
+	if (power_supply_am_i_supplied(dev_info->bat)) {
 		if (capacity == 100)
 			*status = POWER_SUPPLY_STATUS_FULL;
 		else if (current_uA > 50000)
@@ -752,6 +753,7 @@ static const struct attribute_group ds2781_attr_group = {
 
 static int ds2781_battery_probe(struct platform_device *pdev)
 {
+	struct power_supply_config psy_cfg = {};
 	int ret = 0;
 	struct ds2781_device_info *dev_info;
 
@@ -763,25 +765,29 @@ static int ds2781_battery_probe(struct platform_device *pdev)
 
 	dev_info->dev			= &pdev->dev;
 	dev_info->w1_dev		= pdev->dev.parent;
-	dev_info->bat.name		= dev_name(&pdev->dev);
-	dev_info->bat.type		= POWER_SUPPLY_TYPE_BATTERY;
-	dev_info->bat.properties	= ds2781_battery_props;
-	dev_info->bat.num_properties	= ARRAY_SIZE(ds2781_battery_props);
-	dev_info->bat.get_property	= ds2781_battery_get_property;
+	dev_info->bat_desc.name		= dev_name(&pdev->dev);
+	dev_info->bat_desc.type		= POWER_SUPPLY_TYPE_BATTERY;
+	dev_info->bat_desc.properties	= ds2781_battery_props;
+	dev_info->bat_desc.num_properties = ARRAY_SIZE(ds2781_battery_props);
+	dev_info->bat_desc.get_property	= ds2781_battery_get_property;
 
-	ret = power_supply_register(&pdev->dev, &dev_info->bat);
-	if (ret) {
+	psy_cfg.drv_data		= dev_info;
+
+	dev_info->bat = power_supply_register(&pdev->dev, &dev_info->bat_desc,
+						&psy_cfg);
+	if (IS_ERR(dev_info->bat)) {
 		dev_err(dev_info->dev, "failed to register battery\n");
+		ret = PTR_ERR(dev_info->bat);
 		goto fail;
 	}
 
-	ret = sysfs_create_group(&dev_info->bat.dev->kobj, &ds2781_attr_group);
+	ret = sysfs_create_group(&dev_info->bat->dev.kobj, &ds2781_attr_group);
 	if (ret) {
 		dev_err(dev_info->dev, "failed to create sysfs group\n");
 		goto fail_unregister;
 	}
 
-	ret = sysfs_create_bin_file(&dev_info->bat.dev->kobj,
+	ret = sysfs_create_bin_file(&dev_info->bat->dev.kobj,
 					&ds2781_param_eeprom_bin_attr);
 	if (ret) {
 		dev_err(dev_info->dev,
@@ -789,7 +795,7 @@ static int ds2781_battery_probe(struct platform_device *pdev)
 		goto fail_remove_group;
 	}
 
-	ret = sysfs_create_bin_file(&dev_info->bat.dev->kobj,
+	ret = sysfs_create_bin_file(&dev_info->bat->dev.kobj,
 					&ds2781_user_eeprom_bin_attr);
 	if (ret) {
 		dev_err(dev_info->dev,
@@ -800,12 +806,12 @@ static int ds2781_battery_probe(struct platform_device *pdev)
 	return 0;
 
 fail_remove_bin_file:
-	sysfs_remove_bin_file(&dev_info->bat.dev->kobj,
+	sysfs_remove_bin_file(&dev_info->bat->dev.kobj,
 				&ds2781_param_eeprom_bin_attr);
 fail_remove_group:
-	sysfs_remove_group(&dev_info->bat.dev->kobj, &ds2781_attr_group);
+	sysfs_remove_group(&dev_info->bat->dev.kobj, &ds2781_attr_group);
 fail_unregister:
-	power_supply_unregister(&dev_info->bat);
+	power_supply_unregister(dev_info->bat);
 fail:
 	return ret;
 }
@@ -814,10 +820,13 @@ static int ds2781_battery_remove(struct platform_device *pdev)
 {
 	struct ds2781_device_info *dev_info = platform_get_drvdata(pdev);
 
-	/* remove attributes */
-	sysfs_remove_group(&dev_info->bat.dev->kobj, &ds2781_attr_group);
+	/*
+	 * Remove attributes before unregistering power supply
+	 * because 'bat' will be freed on power_supply_unregister() call.
+	 */
+	sysfs_remove_group(&dev_info->bat->dev.kobj, &ds2781_attr_group);
 
-	power_supply_unregister(&dev_info->bat);
+	power_supply_unregister(dev_info->bat);
 
 	return 0;
 }
diff --git a/drivers/power/ds2782_battery.c b/drivers/power/ds2782_battery.c
index 39694883d3bf..ed4d756d21e4 100644
--- a/drivers/power/ds2782_battery.c
+++ b/drivers/power/ds2782_battery.c
@@ -53,11 +53,12 @@ struct ds278x_battery_ops {
 	int (*get_battery_capacity)(struct ds278x_info *info, int *capacity);
 };
 
-#define to_ds278x_info(x) container_of(x, struct ds278x_info, battery)
+#define to_ds278x_info(x) power_supply_get_drvdata(x)
 
 struct ds278x_info {
 	struct i2c_client	*client;
-	struct power_supply	battery;
+	struct power_supply	*battery;
+	struct power_supply_desc	battery_desc;
 	struct ds278x_battery_ops  *ops;
 	struct delayed_work	bat_work;
 	int			id;
@@ -285,7 +286,7 @@ static void ds278x_bat_update(struct ds278x_info *info)
 	ds278x_get_status(info, &info->status);
 
 	if ((old_status != info->status) || (old_capacity != info->capacity))
-		power_supply_changed(&info->battery);
+		power_supply_changed(info->battery);
 }
 
 static void ds278x_bat_work(struct work_struct *work)
@@ -306,7 +307,7 @@ static enum power_supply_property ds278x_battery_props[] = {
 	POWER_SUPPLY_PROP_TEMP,
 };
 
-static void ds278x_power_supply_init(struct power_supply *battery)
+static void ds278x_power_supply_init(struct power_supply_desc *battery)
 {
 	battery->type			= POWER_SUPPLY_TYPE_BATTERY;
 	battery->properties		= ds278x_battery_props;
@@ -319,8 +320,8 @@ static int ds278x_battery_remove(struct i2c_client *client)
 {
 	struct ds278x_info *info = i2c_get_clientdata(client);
 
-	power_supply_unregister(&info->battery);
-	kfree(info->battery.name);
+	power_supply_unregister(info->battery);
+	kfree(info->battery_desc.name);
 
 	mutex_lock(&battery_lock);
 	idr_remove(&battery_id, info->id);
@@ -377,6 +378,7 @@ static int ds278x_battery_probe(struct i2c_client *client,
 				const struct i2c_device_id *id)
 {
 	struct ds278x_platform_data *pdata = client->dev.platform_data;
+	struct power_supply_config psy_cfg = {};
 	struct ds278x_info *info;
 	int ret;
 	int num;
@@ -404,8 +406,9 @@ static int ds278x_battery_probe(struct i2c_client *client,
 		goto fail_info;
 	}
 
-	info->battery.name = kasprintf(GFP_KERNEL, "%s-%d", client->name, num);
-	if (!info->battery.name) {
+	info->battery_desc.name = kasprintf(GFP_KERNEL, "%s-%d",
+					    client->name, num);
+	if (!info->battery_desc.name) {
 		ret = -ENOMEM;
 		goto fail_name;
 	}
@@ -417,16 +420,19 @@ static int ds278x_battery_probe(struct i2c_client *client,
 	info->client = client;
 	info->id = num;
 	info->ops  = &ds278x_ops[id->driver_data];
-	ds278x_power_supply_init(&info->battery);
+	ds278x_power_supply_init(&info->battery_desc);
+	psy_cfg.drv_data = info;
 
 	info->capacity = 100;
 	info->status = POWER_SUPPLY_STATUS_FULL;
 
 	INIT_DELAYED_WORK(&info->bat_work, ds278x_bat_work);
 
-	ret = power_supply_register(&client->dev, &info->battery);
-	if (ret) {
+	info->battery = power_supply_register(&client->dev,
+					      &info->battery_desc, &psy_cfg);
+	if (IS_ERR(info->battery)) {
 		dev_err(&client->dev, "failed to register battery\n");
+		ret = PTR_ERR(info->battery);
 		goto fail_register;
 	} else {
 		schedule_delayed_work(&info->bat_work, DS278x_DELAY);
@@ -435,7 +441,7 @@ static int ds278x_battery_probe(struct i2c_client *client,
 	return 0;
 
 fail_register:
-	kfree(info->battery.name);
+	kfree(info->battery_desc.name);
 fail_name:
 	kfree(info);
 fail_info:
diff --git a/drivers/power/generic-adc-battery.c b/drivers/power/generic-adc-battery.c
index d72733e4f93a..fedc5818fab7 100644
--- a/drivers/power/generic-adc-battery.c
+++ b/drivers/power/generic-adc-battery.c
@@ -44,7 +44,8 @@ static const char *const gab_chan_name[] = {
 };
 
 struct gab {
-	struct power_supply	psy;
+	struct power_supply		*psy;
+	struct power_supply_desc	psy_desc;
 	struct iio_channel	*channel[GAB_MAX_CHAN_TYPE];
 	struct gab_platform_data	*pdata;
 	struct delayed_work bat_work;
@@ -55,7 +56,7 @@ struct gab {
 
 static struct gab *to_generic_bat(struct power_supply *psy)
 {
-	return container_of(psy, struct gab, psy);
+	return power_supply_get_drvdata(psy);
 }
 
 static void gab_ext_power_changed(struct power_supply *psy)
@@ -151,7 +152,7 @@ static int gab_get_property(struct power_supply *psy,
 
 	adc_bat = to_generic_bat(psy);
 	if (!adc_bat) {
-		dev_err(psy->dev, "no battery infos ?!\n");
+		dev_err(&psy->dev, "no battery infos ?!\n");
 		return -EINVAL;
 	}
 	pdata = adc_bat->pdata;
@@ -159,7 +160,7 @@ static int gab_get_property(struct power_supply *psy,
 
 	switch (psp) {
 	case POWER_SUPPLY_PROP_STATUS:
-		gab_get_status(adc_bat);
+		val->intval = gab_get_status(adc_bat);
 		break;
 	case POWER_SUPPLY_PROP_CHARGE_EMPTY_DESIGN:
 		val->intval = 0;
@@ -210,7 +211,7 @@ static void gab_work(struct work_struct *work)
 	pdata = adc_bat->pdata;
 	status = adc_bat->status;
 
-	is_plugged = power_supply_am_i_supplied(&adc_bat->psy);
+	is_plugged = power_supply_am_i_supplied(adc_bat->psy);
 	adc_bat->cable_plugged = is_plugged;
 
 	if (!is_plugged)
@@ -221,7 +222,7 @@ static void gab_work(struct work_struct *work)
 		adc_bat->status = POWER_SUPPLY_STATUS_CHARGING;
 
 	if (status != adc_bat->status)
-		power_supply_changed(&adc_bat->psy);
+		power_supply_changed(adc_bat->psy);
 }
 
 static irqreturn_t gab_charged(int irq, void *dev_id)
@@ -239,7 +240,8 @@ static irqreturn_t gab_charged(int irq, void *dev_id)
 static int gab_probe(struct platform_device *pdev)
 {
 	struct gab *adc_bat;
-	struct power_supply *psy;
+	struct power_supply_desc *psy_desc;
+	struct power_supply_config psy_cfg = {};
 	struct gab_platform_data *pdata = pdev->dev.platform_data;
 	enum power_supply_property *properties;
 	int ret = 0;
@@ -252,32 +254,34 @@ static int gab_probe(struct platform_device *pdev)
 		return -ENOMEM;
 	}
 
-	psy = &adc_bat->psy;
-	psy->name = pdata->battery_info.name;
+	psy_cfg.drv_data = adc_bat;
+	psy_desc = &adc_bat->psy_desc;
+	psy_desc->name = pdata->battery_info.name;
 
 	/* bootup default values for the battery */
 	adc_bat->cable_plugged = false;
 	adc_bat->status = POWER_SUPPLY_STATUS_DISCHARGING;
-	psy->type = POWER_SUPPLY_TYPE_BATTERY;
-	psy->get_property = gab_get_property;
-	psy->external_power_changed = gab_ext_power_changed;
+	psy_desc->type = POWER_SUPPLY_TYPE_BATTERY;
+	psy_desc->get_property = gab_get_property;
+	psy_desc->external_power_changed = gab_ext_power_changed;
 	adc_bat->pdata = pdata;
 
 	/*
 	 * copying the static properties and allocating extra memory for holding
 	 * the extra configurable properties received from platform data.
 	 */
-	psy->properties = kcalloc(ARRAY_SIZE(gab_props) +
+	psy_desc->properties = kcalloc(ARRAY_SIZE(gab_props) +
 					ARRAY_SIZE(gab_chan_name),
-					sizeof(*psy->properties), GFP_KERNEL);
-	if (!psy->properties) {
+					sizeof(*psy_desc->properties),
+					GFP_KERNEL);
+	if (!psy_desc->properties) {
 		ret = -ENOMEM;
 		goto first_mem_fail;
 	}
 
-	memcpy(psy->properties, gab_props, sizeof(gab_props));
+	memcpy(psy_desc->properties, gab_props, sizeof(gab_props));
 	properties = (enum power_supply_property *)
-				((char *)psy->properties + sizeof(gab_props));
+			((char *)psy_desc->properties + sizeof(gab_props));
 
 	/*
 	 * getting channel from iio and copying the battery properties
@@ -291,7 +295,7 @@ static int gab_probe(struct platform_device *pdev)
 			adc_bat->channel[chan] = NULL;
 		} else {
 			/* copying properties for supported channels only */
-			memcpy(properties + sizeof(*(psy->properties)) * index,
+			memcpy(properties + sizeof(*(psy_desc->properties)) * index,
 					&gab_dyn_props[chan],
 					sizeof(gab_dyn_props[chan]));
 			index++;
@@ -310,11 +314,13 @@ static int gab_probe(struct platform_device *pdev)
 	 * as come channels may be not be supported by the device.So
 	 * we need to take care of that.
 	 */
-	psy->num_properties = ARRAY_SIZE(gab_props) + index;
+	psy_desc->num_properties = ARRAY_SIZE(gab_props) + index;
 
-	ret = power_supply_register(&pdev->dev, psy);
-	if (ret)
+	adc_bat->psy = power_supply_register(&pdev->dev, psy_desc, &psy_cfg);
+	if (IS_ERR(adc_bat->psy)) {
+		ret = PTR_ERR(adc_bat->psy);
 		goto err_reg_fail;
+	}
 
 	INIT_DELAYED_WORK(&adc_bat->bat_work, gab_work);
 
@@ -342,14 +348,14 @@ static int gab_probe(struct platform_device *pdev)
 err_gpio:
 	gpio_free(pdata->gpio_charge_finished);
 gpio_req_fail:
-	power_supply_unregister(psy);
+	power_supply_unregister(adc_bat->psy);
 err_reg_fail:
 	for (chan = 0; chan < ARRAY_SIZE(gab_chan_name); chan++) {
 		if (adc_bat->channel[chan])
 			iio_channel_release(adc_bat->channel[chan]);
 	}
 second_mem_fail:
-	kfree(psy->properties);
+	kfree(psy_desc->properties);
 first_mem_fail:
 	return ret;
 }
@@ -360,7 +366,7 @@ static int gab_remove(struct platform_device *pdev)
 	struct gab *adc_bat = platform_get_drvdata(pdev);
 	struct gab_platform_data *pdata = adc_bat->pdata;
 
-	power_supply_unregister(&adc_bat->psy);
+	power_supply_unregister(adc_bat->psy);
 
 	if (gpio_is_valid(pdata->gpio_charge_finished)) {
 		free_irq(gpio_to_irq(pdata->gpio_charge_finished), adc_bat);
@@ -372,7 +378,7 @@ static int gab_remove(struct platform_device *pdev)
 			iio_channel_release(adc_bat->channel[chan]);
 	}
 
-	kfree(adc_bat->psy.properties);
+	kfree(adc_bat->psy_desc.properties);
 	cancel_delayed_work(&adc_bat->bat_work);
 	return 0;
 }
diff --git a/drivers/power/goldfish_battery.c b/drivers/power/goldfish_battery.c
index 29eba88a2963..a50bb988c69a 100644
--- a/drivers/power/goldfish_battery.c
+++ b/drivers/power/goldfish_battery.c
@@ -30,8 +30,8 @@ struct goldfish_battery_data {
 	int irq;
 	spinlock_t lock;
 
-	struct power_supply battery;
-	struct power_supply ac;
+	struct power_supply *battery;
+	struct power_supply *ac;
 };
 
 #define GOLDFISH_BATTERY_READ(data, addr) \
@@ -67,8 +67,7 @@ static int goldfish_ac_get_property(struct power_supply *psy,
 			enum power_supply_property psp,
 			union power_supply_propval *val)
 {
-	struct goldfish_battery_data *data = container_of(psy,
-		struct goldfish_battery_data, ac);
+	struct goldfish_battery_data *data = power_supply_get_drvdata(psy);
 	int ret = 0;
 
 	switch (psp) {
@@ -86,8 +85,7 @@ static int goldfish_battery_get_property(struct power_supply *psy,
 				 enum power_supply_property psp,
 				 union power_supply_propval *val)
 {
-	struct goldfish_battery_data *data = container_of(psy,
-		struct goldfish_battery_data, battery);
+	struct goldfish_battery_data *data = power_supply_get_drvdata(psy);
 	int ret = 0;
 
 	switch (psp) {
@@ -139,20 +137,36 @@ static irqreturn_t goldfish_battery_interrupt(int irq, void *dev_id)
 	status &= BATTERY_INT_MASK;
 
 	if (status & BATTERY_STATUS_CHANGED)
-		power_supply_changed(&data->battery);
+		power_supply_changed(data->battery);
 	if (status & AC_STATUS_CHANGED)
-		power_supply_changed(&data->ac);
+		power_supply_changed(data->ac);
 
 	spin_unlock_irqrestore(&data->lock, irq_flags);
 	return status ? IRQ_HANDLED : IRQ_NONE;
 }
 
+static const struct power_supply_desc battery_desc = {
+	.properties	= goldfish_battery_props,
+	.num_properties	= ARRAY_SIZE(goldfish_battery_props),
+	.get_property	= goldfish_battery_get_property,
+	.name		= "battery",
+	.type		= POWER_SUPPLY_TYPE_BATTERY,
+};
+
+static const struct power_supply_desc ac_desc = {
+	.properties	= goldfish_ac_props,
+	.num_properties	= ARRAY_SIZE(goldfish_ac_props),
+	.get_property	= goldfish_ac_get_property,
+	.name		= "ac",
+	.type		= POWER_SUPPLY_TYPE_MAINS,
+};
 
 static int goldfish_battery_probe(struct platform_device *pdev)
 {
 	int ret;
 	struct resource *r;
 	struct goldfish_battery_data *data;
+	struct power_supply_config psy_cfg = {};
 
 	data = devm_kzalloc(&pdev->dev, sizeof(*data), GFP_KERNEL);
 	if (data == NULL)
@@ -160,18 +174,6 @@ static int goldfish_battery_probe(struct platform_device *pdev)
 
 	spin_lock_init(&data->lock);
 
-	data->battery.properties = goldfish_battery_props;
-	data->battery.num_properties = ARRAY_SIZE(goldfish_battery_props);
-	data->battery.get_property = goldfish_battery_get_property;
-	data->battery.name = "battery";
-	data->battery.type = POWER_SUPPLY_TYPE_BATTERY;
-
-	data->ac.properties = goldfish_ac_props;
-	data->ac.num_properties = ARRAY_SIZE(goldfish_ac_props);
-	data->ac.get_property = goldfish_ac_get_property;
-	data->ac.name = "ac";
-	data->ac.type = POWER_SUPPLY_TYPE_MAINS;
-
 	r = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	if (r == NULL) {
 		dev_err(&pdev->dev, "platform_get_resource failed\n");
@@ -195,14 +197,17 @@ static int goldfish_battery_probe(struct platform_device *pdev)
 	if (ret)
 		return ret;
 
-	ret = power_supply_register(&pdev->dev, &data->ac);
-	if (ret)
-		return ret;
+	psy_cfg.drv_data = data;
 
-	ret = power_supply_register(&pdev->dev, &data->battery);
-	if (ret) {
-		power_supply_unregister(&data->ac);
-		return ret;
+	data->ac = power_supply_register(&pdev->dev, &ac_desc, &psy_cfg);
+	if (IS_ERR(data->ac))
+		return PTR_ERR(data->ac);
+
+	data->battery = power_supply_register(&pdev->dev, &battery_desc,
+						&psy_cfg);
+	if (IS_ERR(data->battery)) {
+		power_supply_unregister(data->ac);
+		return PTR_ERR(data->battery);
 	}
 
 	platform_set_drvdata(pdev, data);
@@ -216,8 +221,8 @@ static int goldfish_battery_remove(struct platform_device *pdev)
 {
 	struct goldfish_battery_data *data = platform_get_drvdata(pdev);
 
-	power_supply_unregister(&data->battery);
-	power_supply_unregister(&data->ac);
+	power_supply_unregister(data->battery);
+	power_supply_unregister(data->ac);
 	battery_data = NULL;
 	return 0;
 }
diff --git a/drivers/power/gpio-charger.c b/drivers/power/gpio-charger.c
index b7424c8501f1..c5869b1941ac 100644
--- a/drivers/power/gpio-charger.c
+++ b/drivers/power/gpio-charger.c
@@ -32,7 +32,8 @@ struct gpio_charger {
 	unsigned int irq;
 	bool wakeup_enabled;
 
-	struct power_supply charger;
+	struct power_supply *charger;
+	struct power_supply_desc charger_desc;
 };
 
 static irqreturn_t gpio_charger_irq(int irq, void *devid)
@@ -46,7 +47,7 @@ static irqreturn_t gpio_charger_irq(int irq, void *devid)
 
 static inline struct gpio_charger *psy_to_gpio_charger(struct power_supply *psy)
 {
-	return container_of(psy, struct gpio_charger, charger);
+	return power_supply_get_drvdata(psy);
 }
 
 static int gpio_charger_get_property(struct power_supply *psy,
@@ -127,8 +128,9 @@ struct gpio_charger_platform_data *gpio_charger_parse_dt(struct device *dev)
 static int gpio_charger_probe(struct platform_device *pdev)
 {
 	const struct gpio_charger_platform_data *pdata = pdev->dev.platform_data;
+	struct power_supply_config psy_cfg = {};
 	struct gpio_charger *gpio_charger;
-	struct power_supply *charger;
+	struct power_supply_desc *charger_desc;
 	int ret;
 	int irq;
 
@@ -154,16 +156,18 @@ static int gpio_charger_probe(struct platform_device *pdev)
 		return -ENOMEM;
 	}
 
-	charger = &gpio_charger->charger;
+	charger_desc = &gpio_charger->charger_desc;
+
+	charger_desc->name = pdata->name ? pdata->name : "gpio-charger";
+	charger_desc->type = pdata->type;
+	charger_desc->properties = gpio_charger_properties;
+	charger_desc->num_properties = ARRAY_SIZE(gpio_charger_properties);
+	charger_desc->get_property = gpio_charger_get_property;
 
-	charger->name = pdata->name ? pdata->name : "gpio-charger";
-	charger->type = pdata->type;
-	charger->properties = gpio_charger_properties;
-	charger->num_properties = ARRAY_SIZE(gpio_charger_properties);
-	charger->get_property = gpio_charger_get_property;
-	charger->supplied_to = pdata->supplied_to;
-	charger->num_supplicants = pdata->num_supplicants;
-	charger->of_node = pdev->dev.of_node;
+	psy_cfg.supplied_to = pdata->supplied_to;
+	psy_cfg.num_supplicants = pdata->num_supplicants;
+	psy_cfg.of_node = pdev->dev.of_node;
+	psy_cfg.drv_data = gpio_charger;
 
 	ret = gpio_request(pdata->gpio, dev_name(&pdev->dev));
 	if (ret) {
@@ -178,8 +182,10 @@ static int gpio_charger_probe(struct platform_device *pdev)
 
 	gpio_charger->pdata = pdata;
 
-	ret = power_supply_register(&pdev->dev, charger);
-	if (ret < 0) {
+	gpio_charger->charger = power_supply_register(&pdev->dev,
+						      charger_desc, &psy_cfg);
+	if (IS_ERR(gpio_charger->charger)) {
+		ret = PTR_ERR(gpio_charger->charger);
 		dev_err(&pdev->dev, "Failed to register power supply: %d\n",
 			ret);
 		goto err_gpio_free;
@@ -189,7 +195,7 @@ static int gpio_charger_probe(struct platform_device *pdev)
 	if (irq > 0) {
 		ret = request_any_context_irq(irq, gpio_charger_irq,
 				IRQF_TRIGGER_RISING | IRQF_TRIGGER_FALLING,
-				dev_name(&pdev->dev), charger);
+				dev_name(&pdev->dev), gpio_charger->charger);
 		if (ret < 0)
 			dev_warn(&pdev->dev, "Failed to request irq: %d\n", ret);
 		else
@@ -213,9 +219,9 @@ static int gpio_charger_remove(struct platform_device *pdev)
 	struct gpio_charger *gpio_charger = platform_get_drvdata(pdev);
 
 	if (gpio_charger->irq)
-		free_irq(gpio_charger->irq, &gpio_charger->charger);
+		free_irq(gpio_charger->irq, gpio_charger->charger);
 
-	power_supply_unregister(&gpio_charger->charger);
+	power_supply_unregister(gpio_charger->charger);
 
 	gpio_free(gpio_charger->pdata->gpio);
 
@@ -241,7 +247,7 @@ static int gpio_charger_resume(struct device *dev)
 
 	if (device_may_wakeup(dev) && gpio_charger->wakeup_enabled)
 		disable_irq_wake(gpio_charger->irq);
-	power_supply_changed(&gpio_charger->charger);
+	power_supply_changed(gpio_charger->charger);
 
 	return 0;
 }
diff --git a/drivers/power/intel_mid_battery.c b/drivers/power/intel_mid_battery.c
index de3f39e6fa8e..9fa4acc107ca 100644
--- a/drivers/power/intel_mid_battery.c
+++ b/drivers/power/intel_mid_battery.c
@@ -107,8 +107,8 @@ struct pmic_power_module_info {
 	unsigned int batt_prev_charge_full;	/* in mAS */
 	unsigned int batt_charge_rate;		/* in units per second */
 
-	struct power_supply usb;
-	struct power_supply batt;
+	struct power_supply *usb;
+	struct power_supply *batt;
 	int irq;				/* GPE_ID or IRQ# */
 	struct workqueue_struct *monitor_wqueue;
 	struct delayed_work monitor_battery;
@@ -404,8 +404,7 @@ static int pmic_usb_get_property(struct power_supply *psy,
 				enum power_supply_property psp,
 				union power_supply_propval *val)
 {
-	struct pmic_power_module_info *pbi = container_of(psy,
-				struct pmic_power_module_info, usb);
+	struct pmic_power_module_info *pbi = power_supply_get_drvdata(psy);
 
 	/* update pmic_power_module_info members */
 	pmic_battery_read_status(pbi);
@@ -444,8 +443,7 @@ static int pmic_battery_get_property(struct power_supply *psy,
 				enum power_supply_property psp,
 				union power_supply_propval *val)
 {
-	struct pmic_power_module_info *pbi = container_of(psy,
-				struct pmic_power_module_info, batt);
+	struct pmic_power_module_info *pbi = power_supply_get_drvdata(psy);
 
 	/* update pmic_power_module_info members */
 	pmic_battery_read_status(pbi);
@@ -640,6 +638,25 @@ static void pmic_battery_handle_intrpt(struct work_struct *work)
 			__func__);
 }
 
+/*
+ * Description of power supplies
+ */
+static const struct power_supply_desc pmic_usb_desc = {
+	.name		= "pmic-usb",
+	.type		= POWER_SUPPLY_TYPE_USB,
+	.properties	= pmic_usb_props,
+	.num_properties	= ARRAY_SIZE(pmic_usb_props),
+	.get_property	= pmic_usb_get_property,
+};
+
+static const struct power_supply_desc pmic_batt_desc = {
+	.name		= "pmic-batt",
+	.type		= POWER_SUPPLY_TYPE_BATTERY,
+	.properties	= pmic_battery_props,
+	.num_properties	= ARRAY_SIZE(pmic_battery_props),
+	.get_property	= pmic_battery_get_property,
+};
+
 /**
  * pmic_battery_probe - pmic battery initialize
  * @irq: pmic battery device irq
@@ -653,6 +670,7 @@ static int probe(int irq, struct device *dev)
 {
 	int retval = 0;
 	struct pmic_power_module_info *pbi;
+	struct power_supply_config psy_cfg = {};
 
 	dev_dbg(dev, "pmic-battery: found pmic battery device\n");
 
@@ -666,6 +684,7 @@ static int probe(int irq, struct device *dev)
 	pbi->dev = dev;
 	pbi->irq = irq;
 	dev_set_drvdata(dev, pbi);
+	psy_cfg.drv_data = pbi;
 
 	/* initialize all required framework before enabling interrupts */
 	INIT_WORK(&pbi->handler, pmic_battery_handle_intrpt);
@@ -687,16 +706,12 @@ static int probe(int irq, struct device *dev)
 	}
 
 	/* register pmic-batt with power supply subsystem */
-	pbi->batt.name = "pmic-batt";
-	pbi->batt.type = POWER_SUPPLY_TYPE_BATTERY;
-	pbi->batt.properties = pmic_battery_props;
-	pbi->batt.num_properties = ARRAY_SIZE(pmic_battery_props);
-	pbi->batt.get_property = pmic_battery_get_property;
-	retval = power_supply_register(dev, &pbi->batt);
-	if (retval) {
+	pbi->batt = power_supply_register(dev, &pmic_usb_desc, &psy_cfg);
+	if (IS_ERR(pbi->batt)) {
 		dev_err(dev,
 			"%s(): failed to register pmic battery device with power supply subsystem\n",
 				__func__);
+		retval = PTR_ERR(pbi->batt);
 		goto power_reg_failed;
 	}
 
@@ -707,16 +722,12 @@ static int probe(int irq, struct device *dev)
 	queue_delayed_work(pbi->monitor_wqueue, &pbi->monitor_battery, HZ * 1);
 
 	/* register pmic-usb with power supply subsystem */
-	pbi->usb.name = "pmic-usb";
-	pbi->usb.type = POWER_SUPPLY_TYPE_USB;
-	pbi->usb.properties = pmic_usb_props;
-	pbi->usb.num_properties = ARRAY_SIZE(pmic_usb_props);
-	pbi->usb.get_property = pmic_usb_get_property;
-	retval = power_supply_register(dev, &pbi->usb);
-	if (retval) {
+	pbi->usb = power_supply_register(dev, &pmic_batt_desc, &psy_cfg);
+	if (IS_ERR(pbi->usb)) {
 		dev_err(dev,
 			"%s(): failed to register pmic usb device with power supply subsystem\n",
 				__func__);
+		retval = PTR_ERR(pbi->usb);
 		goto power_reg_failed_1;
 	}
 
@@ -728,7 +739,7 @@ static int probe(int irq, struct device *dev)
 	return retval;
 
 power_reg_failed_1:
-	power_supply_unregister(&pbi->batt);
+	power_supply_unregister(pbi->batt);
 power_reg_failed:
 	cancel_delayed_work_sync(&pbi->monitor_battery);
 requestirq_failed:
@@ -762,8 +773,8 @@ static int platform_pmic_battery_remove(struct platform_device *pdev)
 	cancel_delayed_work_sync(&pbi->monitor_battery);
 	destroy_workqueue(pbi->monitor_wqueue);
 
-	power_supply_unregister(&pbi->usb);
-	power_supply_unregister(&pbi->batt);
+	power_supply_unregister(pbi->usb);
+	power_supply_unregister(pbi->batt);
 
 	cancel_work_sync(&pbi->handler);
 	kfree(pbi);
diff --git a/drivers/power/ipaq_micro_battery.c b/drivers/power/ipaq_micro_battery.c
index 9d694605cdb7..f03014ea1dc4 100644
--- a/drivers/power/ipaq_micro_battery.c
+++ b/drivers/power/ipaq_micro_battery.c
@@ -93,7 +93,7 @@ static void micro_battery_work(struct work_struct *work)
 
 static int get_capacity(struct power_supply *b)
 {
-	struct micro_battery *mb = dev_get_drvdata(b->dev->parent);
+	struct micro_battery *mb = dev_get_drvdata(b->dev.parent);
 
 	switch (mb->flag & 0x07) {
 	case MICRO_BATT_STATUS_HIGH:
@@ -113,7 +113,7 @@ static int get_capacity(struct power_supply *b)
 
 static int get_status(struct power_supply *b)
 {
-	struct micro_battery *mb = dev_get_drvdata(b->dev->parent);
+	struct micro_battery *mb = dev_get_drvdata(b->dev.parent);
 
 	if (mb->flag == MICRO_BATT_STATUS_UNKNOWN)
 		return POWER_SUPPLY_STATUS_UNKNOWN;
@@ -132,7 +132,7 @@ static int micro_batt_get_property(struct power_supply *b,
 					enum power_supply_property psp,
 					union power_supply_propval *val)
 {
-	struct micro_battery *mb = dev_get_drvdata(b->dev->parent);
+	struct micro_battery *mb = dev_get_drvdata(b->dev.parent);
 
 	switch (psp) {
 	case POWER_SUPPLY_PROP_TECHNOLOGY:
@@ -180,7 +180,7 @@ static int micro_ac_get_property(struct power_supply *b,
 				 enum power_supply_property psp,
 				 union power_supply_propval *val)
 {
-	struct micro_battery *mb = dev_get_drvdata(b->dev->parent);
+	struct micro_battery *mb = dev_get_drvdata(b->dev.parent);
 
 	switch (psp) {
 	case POWER_SUPPLY_PROP_ONLINE:
@@ -202,7 +202,7 @@ static enum power_supply_property micro_batt_power_props[] = {
 	POWER_SUPPLY_PROP_VOLTAGE_NOW,
 };
 
-static struct power_supply micro_batt_power = {
+static const struct power_supply_desc micro_batt_power_desc = {
 	.name			= "main-battery",
 	.type			= POWER_SUPPLY_TYPE_BATTERY,
 	.properties		= micro_batt_power_props,
@@ -215,7 +215,7 @@ static enum power_supply_property micro_ac_power_props[] = {
 	POWER_SUPPLY_PROP_ONLINE,
 };
 
-static struct power_supply micro_ac_power = {
+static const struct power_supply_desc micro_ac_power_desc = {
 	.name			= "ac",
 	.type			= POWER_SUPPLY_TYPE_MAINS,
 	.properties		= micro_ac_power_props,
@@ -223,9 +223,12 @@ static struct power_supply micro_ac_power = {
 	.get_property		= micro_ac_get_property,
 };
 
+static struct power_supply *micro_batt_power, *micro_ac_power;
+
 static int micro_batt_probe(struct platform_device *pdev)
 {
 	struct micro_battery *mb;
+	int ret;
 
 	mb = devm_kzalloc(&pdev->dev, sizeof(*mb), GFP_KERNEL);
 	if (!mb)
@@ -233,14 +236,36 @@ static int micro_batt_probe(struct platform_device *pdev)
 
 	mb->micro = dev_get_drvdata(pdev->dev.parent);
 	mb->wq = create_singlethread_workqueue("ipaq-battery-wq");
+	if (!mb->wq)
+		return -ENOMEM;
+
 	INIT_DELAYED_WORK(&mb->update, micro_battery_work);
 	platform_set_drvdata(pdev, mb);
 	queue_delayed_work(mb->wq, &mb->update, 1);
-	power_supply_register(&pdev->dev, &micro_batt_power);
-	power_supply_register(&pdev->dev, &micro_ac_power);
+
+	micro_batt_power = power_supply_register(&pdev->dev,
+						 &micro_batt_power_desc, NULL);
+	if (IS_ERR(micro_batt_power)) {
+		ret = PTR_ERR(micro_batt_power);
+		goto batt_err;
+	}
+
+	micro_ac_power = power_supply_register(&pdev->dev,
+					       &micro_ac_power_desc, NULL);
+	if (IS_ERR(micro_ac_power)) {
+		ret = PTR_ERR(micro_ac_power);
+		goto ac_err;
+	}
 
 	dev_info(&pdev->dev, "iPAQ micro battery driver\n");
 	return 0;
+
+ac_err:
+	power_supply_unregister(micro_ac_power);
+batt_err:
+	cancel_delayed_work_sync(&mb->update);
+	destroy_workqueue(mb->wq);
+	return ret;
 }
 
 static int micro_batt_remove(struct platform_device *pdev)
@@ -248,9 +273,10 @@ static int micro_batt_remove(struct platform_device *pdev)
 {
 	struct micro_battery *mb = platform_get_drvdata(pdev);
 
-	power_supply_unregister(&micro_ac_power);
-	power_supply_unregister(&micro_batt_power);
+	power_supply_unregister(micro_ac_power);
+	power_supply_unregister(micro_batt_power);
 	cancel_delayed_work_sync(&mb->update);
+	destroy_workqueue(mb->wq);
 
 	return 0;
 }
diff --git a/drivers/power/isp1704_charger.c b/drivers/power/isp1704_charger.c
index 0b4cf9d63291..f2a7d970388f 100644
--- a/drivers/power/isp1704_charger.c
+++ b/drivers/power/isp1704_charger.c
@@ -57,11 +57,12 @@ static u16 isp170x_id[] = {
 };
 
 struct isp1704_charger {
-	struct device		*dev;
-	struct power_supply	psy;
-	struct usb_phy		*phy;
-	struct notifier_block	nb;
-	struct work_struct	work;
+	struct device			*dev;
+	struct power_supply		*psy;
+	struct power_supply_desc	psy_desc;
+	struct usb_phy			*phy;
+	struct notifier_block		nb;
+	struct work_struct		work;
 
 	/* properties */
 	char			model[8];
@@ -259,10 +260,10 @@ static void isp1704_charger_work(struct work_struct *data)
 
 			/* detect wall charger */
 			if (isp1704_charger_detect_dcp(isp)) {
-				isp->psy.type = POWER_SUPPLY_TYPE_USB_DCP;
+				isp->psy_desc.type = POWER_SUPPLY_TYPE_USB_DCP;
 				isp->current_max = 1800;
 			} else {
-				isp->psy.type = POWER_SUPPLY_TYPE_USB;
+				isp->psy_desc.type = POWER_SUPPLY_TYPE_USB;
 				isp->current_max = 500;
 			}
 
@@ -271,7 +272,7 @@ static void isp1704_charger_work(struct work_struct *data)
 				usb_gadget_connect(isp->phy->otg->gadget);
 		}
 
-		if (isp->psy.type != POWER_SUPPLY_TYPE_USB_DCP) {
+		if (isp->psy_desc.type != POWER_SUPPLY_TYPE_USB_DCP) {
 			/*
 			 * Only 500mA here or high speed chirp
 			 * handshaking may break
@@ -280,14 +281,14 @@ static void isp1704_charger_work(struct work_struct *data)
 				isp->current_max = 500;
 
 			if (isp->current_max > 100)
-				isp->psy.type = POWER_SUPPLY_TYPE_USB_CDP;
+				isp->psy_desc.type = POWER_SUPPLY_TYPE_USB_CDP;
 		}
 		break;
 	case USB_EVENT_NONE:
 		isp->online = false;
 		isp->present = 0;
 		isp->current_max = 0;
-		isp->psy.type = POWER_SUPPLY_TYPE_USB;
+		isp->psy_desc.type = POWER_SUPPLY_TYPE_USB;
 
 		/*
 		 * Disable data pullups. We need to prevent the controller from
@@ -306,7 +307,7 @@ static void isp1704_charger_work(struct work_struct *data)
 		goto out;
 	}
 
-	power_supply_changed(&isp->psy);
+	power_supply_changed(isp->psy);
 out:
 	mutex_unlock(&lock);
 }
@@ -326,8 +327,7 @@ static int isp1704_charger_get_property(struct power_supply *psy,
 				enum power_supply_property psp,
 				union power_supply_propval *val)
 {
-	struct isp1704_charger *isp =
-		container_of(psy, struct isp1704_charger, psy);
+	struct isp1704_charger *isp = power_supply_get_drvdata(psy);
 
 	switch (psp) {
 	case POWER_SUPPLY_PROP_PRESENT:
@@ -403,6 +403,7 @@ static int isp1704_charger_probe(struct platform_device *pdev)
 {
 	struct isp1704_charger	*isp;
 	int			ret = -ENODEV;
+	struct power_supply_config psy_cfg = {};
 
 	struct isp1704_charger_data *pdata = dev_get_platdata(&pdev->dev);
 	struct device_node *np = pdev->dev.of_node;
@@ -454,15 +455,19 @@ static int isp1704_charger_probe(struct platform_device *pdev)
 	if (ret < 0)
 		goto fail1;
 
-	isp->psy.name		= "isp1704";
-	isp->psy.type		= POWER_SUPPLY_TYPE_USB;
-	isp->psy.properties	= power_props;
-	isp->psy.num_properties	= ARRAY_SIZE(power_props);
-	isp->psy.get_property	= isp1704_charger_get_property;
+	isp->psy_desc.name		= "isp1704";
+	isp->psy_desc.type		= POWER_SUPPLY_TYPE_USB;
+	isp->psy_desc.properties	= power_props;
+	isp->psy_desc.num_properties	= ARRAY_SIZE(power_props);
+	isp->psy_desc.get_property	= isp1704_charger_get_property;
 
-	ret = power_supply_register(isp->dev, &isp->psy);
-	if (ret)
+	psy_cfg.drv_data		= isp;
+
+	isp->psy = power_supply_register(isp->dev, &isp->psy_desc, &psy_cfg);
+	if (IS_ERR(isp->psy)) {
+		ret = PTR_ERR(isp->psy);
 		goto fail1;
+	}
 
 	/*
 	 * REVISIT: using work in order to allow the usb notifications to be
@@ -498,7 +503,7 @@ static int isp1704_charger_probe(struct platform_device *pdev)
 
 	return 0;
 fail2:
-	power_supply_unregister(&isp->psy);
+	power_supply_unregister(isp->psy);
 fail1:
 	isp1704_charger_set_power(isp, 0);
 fail0:
@@ -512,7 +517,7 @@ static int isp1704_charger_remove(struct platform_device *pdev)
 	struct isp1704_charger *isp = platform_get_drvdata(pdev);
 
 	usb_unregister_notifier(isp->phy, &isp->nb);
-	power_supply_unregister(&isp->psy);
+	power_supply_unregister(isp->psy);
 	isp1704_charger_set_power(isp, 0);
 
 	return 0;
diff --git a/drivers/power/jz4740-battery.c b/drivers/power/jz4740-battery.c
index 9cd391d61819..abdfc21ec13f 100644
--- a/drivers/power/jz4740-battery.c
+++ b/drivers/power/jz4740-battery.c
@@ -46,7 +46,8 @@ struct jz_battery {
 
 	struct completion read_completion;
 
-	struct power_supply battery;
+	struct power_supply *battery;
+	struct power_supply_desc battery_desc;
 	struct delayed_work work;
 
 	struct mutex lock;
@@ -54,7 +55,7 @@ struct jz_battery {
 
 static inline struct jz_battery *psy_to_jz_battery(struct power_supply *psy)
 {
-	return container_of(psy, struct jz_battery, battery);
+	return power_supply_get_drvdata(psy);
 }
 
 static irqreturn_t jz_battery_irq_handler(int irq, void *devid)
@@ -213,7 +214,7 @@ static void jz_battery_update(struct jz_battery *jz_battery)
 	}
 
 	if (has_changed)
-		power_supply_changed(&jz_battery->battery);
+		power_supply_changed(jz_battery->battery);
 }
 
 static enum power_supply_property jz_battery_properties[] = {
@@ -242,8 +243,9 @@ static int jz_battery_probe(struct platform_device *pdev)
 {
 	int ret = 0;
 	struct jz_battery_platform_data *pdata = pdev->dev.parent->platform_data;
+	struct power_supply_config psy_cfg = {};
 	struct jz_battery *jz_battery;
-	struct power_supply *battery;
+	struct power_supply_desc *battery_desc;
 	struct resource *mem;
 
 	if (!pdata) {
@@ -271,14 +273,17 @@ static int jz_battery_probe(struct platform_device *pdev)
 	if (IS_ERR(jz_battery->base))
 		return PTR_ERR(jz_battery->base);
 
-	battery = &jz_battery->battery;
-	battery->name = pdata->info.name;
-	battery->type = POWER_SUPPLY_TYPE_BATTERY;
-	battery->properties	= jz_battery_properties;
-	battery->num_properties	= ARRAY_SIZE(jz_battery_properties);
-	battery->get_property = jz_battery_get_property;
-	battery->external_power_changed = jz_battery_external_power_changed;
-	battery->use_for_apm = 1;
+	battery_desc = &jz_battery->battery_desc;
+	battery_desc->name = pdata->info.name;
+	battery_desc->type = POWER_SUPPLY_TYPE_BATTERY;
+	battery_desc->properties	= jz_battery_properties;
+	battery_desc->num_properties	= ARRAY_SIZE(jz_battery_properties);
+	battery_desc->get_property	= jz_battery_get_property;
+	battery_desc->external_power_changed =
+					jz_battery_external_power_changed;
+	battery_desc->use_for_apm	= 1;
+
+	psy_cfg.drv_data = jz_battery;
 
 	jz_battery->pdata = pdata;
 	jz_battery->pdev = pdev;
@@ -330,9 +335,11 @@ static int jz_battery_probe(struct platform_device *pdev)
 	else
 		jz4740_adc_set_config(pdev->dev.parent, JZ_ADC_CONFIG_BAT_MB, 0);
 
-	ret = power_supply_register(&pdev->dev, &jz_battery->battery);
-	if (ret) {
+	jz_battery->battery = power_supply_register(&pdev->dev, battery_desc,
+							&psy_cfg);
+	if (IS_ERR(jz_battery->battery)) {
 		dev_err(&pdev->dev, "power supply battery register failed.\n");
+		ret = PTR_ERR(jz_battery->battery);
 		goto err_free_charge_irq;
 	}
 
@@ -364,7 +371,7 @@ static int jz_battery_remove(struct platform_device *pdev)
 		gpio_free(jz_battery->pdata->gpio_charge);
 	}
 
-	power_supply_unregister(&jz_battery->battery);
+	power_supply_unregister(jz_battery->battery);
 
 	free_irq(jz_battery->irq, jz_battery);
 
diff --git a/drivers/power/lp8727_charger.c b/drivers/power/lp8727_charger.c
index 32de636dcd73..7e741f1d3cd5 100644
--- a/drivers/power/lp8727_charger.c
+++ b/drivers/power/lp8727_charger.c
@@ -80,9 +80,9 @@ enum lp8727_die_temp {
 };
 
 struct lp8727_psy {
-	struct power_supply ac;
-	struct power_supply usb;
-	struct power_supply batt;
+	struct power_supply *ac;
+	struct power_supply *usb;
+	struct power_supply *batt;
 };
 
 struct lp8727_chg {
@@ -242,9 +242,9 @@ static void lp8727_delayed_func(struct work_struct *_work)
 	lp8727_id_detection(pchg, idno, vbus);
 	lp8727_enable_chgdet(pchg);
 
-	power_supply_changed(&pchg->psy->ac);
-	power_supply_changed(&pchg->psy->usb);
-	power_supply_changed(&pchg->psy->batt);
+	power_supply_changed(pchg->psy->ac);
+	power_supply_changed(pchg->psy->usb);
+	power_supply_changed(pchg->psy->batt);
 }
 
 static irqreturn_t lp8727_isr_func(int irq, void *ptr)
@@ -311,12 +311,12 @@ static int lp8727_charger_get_property(struct power_supply *psy,
 				       enum power_supply_property psp,
 				       union power_supply_propval *val)
 {
-	struct lp8727_chg *pchg = dev_get_drvdata(psy->dev->parent);
+	struct lp8727_chg *pchg = dev_get_drvdata(psy->dev.parent);
 
 	if (psp != POWER_SUPPLY_PROP_ONLINE)
 		return -EINVAL;
 
-	val->intval = lp8727_is_charger_attached(psy->name, pchg->devid);
+	val->intval = lp8727_is_charger_attached(psy->desc->name, pchg->devid);
 
 	return 0;
 }
@@ -337,14 +337,14 @@ static int lp8727_battery_get_property(struct power_supply *psy,
 				       enum power_supply_property psp,
 				       union power_supply_propval *val)
 {
-	struct lp8727_chg *pchg = dev_get_drvdata(psy->dev->parent);
+	struct lp8727_chg *pchg = dev_get_drvdata(psy->dev.parent);
 	struct lp8727_platform_data *pdata = pchg->pdata;
 	enum lp8727_die_temp temp;
 	u8 read;
 
 	switch (psp) {
 	case POWER_SUPPLY_PROP_STATUS:
-		if (!lp8727_is_charger_attached(psy->name, pchg->devid)) {
+		if (!lp8727_is_charger_attached(psy->desc->name, pchg->devid)) {
 			val->intval = POWER_SUPPLY_STATUS_DISCHARGING;
 			return 0;
 		}
@@ -400,13 +400,13 @@ static int lp8727_battery_get_property(struct power_supply *psy,
 
 static void lp8727_charger_changed(struct power_supply *psy)
 {
-	struct lp8727_chg *pchg = dev_get_drvdata(psy->dev->parent);
+	struct lp8727_chg *pchg = dev_get_drvdata(psy->dev.parent);
 	u8 eoc_level;
 	u8 ichg;
 	u8 val;
 
 	/* skip if no charger exists */
-	if (!lp8727_is_charger_attached(psy->name, pchg->devid))
+	if (!lp8727_is_charger_attached(psy->desc->name, pchg->devid))
 		return;
 
 	/* update charging parameters */
@@ -418,8 +418,34 @@ static void lp8727_charger_changed(struct power_supply *psy)
 	}
 }
 
+static const struct power_supply_desc lp8727_ac_desc = {
+	.name			= "ac",
+	.type			= POWER_SUPPLY_TYPE_MAINS,
+	.properties		= lp8727_charger_prop,
+	.num_properties		= ARRAY_SIZE(lp8727_charger_prop),
+	.get_property		= lp8727_charger_get_property,
+};
+
+static const struct power_supply_desc lp8727_usb_desc = {
+	.name			= "usb",
+	.type			= POWER_SUPPLY_TYPE_USB,
+	.properties		= lp8727_charger_prop,
+	.num_properties		= ARRAY_SIZE(lp8727_charger_prop),
+	.get_property		= lp8727_charger_get_property,
+};
+
+static const struct power_supply_desc lp8727_batt_desc = {
+	.name			= "main_batt",
+	.type			= POWER_SUPPLY_TYPE_BATTERY,
+	.properties		= lp8727_battery_prop,
+	.num_properties		= ARRAY_SIZE(lp8727_battery_prop),
+	.get_property		= lp8727_battery_get_property,
+	.external_power_changed	= lp8727_charger_changed,
+};
+
 static int lp8727_register_psy(struct lp8727_chg *pchg)
 {
+	struct power_supply_config psy_cfg = {}; /* Only for ac and usb */
 	struct lp8727_psy *psy;
 
 	psy = devm_kzalloc(pchg->dev, sizeof(*psy), GFP_KERNEL);
@@ -428,44 +454,28 @@ static int lp8727_register_psy(struct lp8727_chg *pchg)
 
 	pchg->psy = psy;
 
-	psy->ac.name = "ac";
-	psy->ac.type = POWER_SUPPLY_TYPE_MAINS;
-	psy->ac.properties = lp8727_charger_prop;
-	psy->ac.num_properties = ARRAY_SIZE(lp8727_charger_prop);
-	psy->ac.get_property = lp8727_charger_get_property;
-	psy->ac.supplied_to = battery_supplied_to;
-	psy->ac.num_supplicants = ARRAY_SIZE(battery_supplied_to);
+	psy_cfg.supplied_to = battery_supplied_to;
+	psy_cfg.num_supplicants = ARRAY_SIZE(battery_supplied_to);
 
-	if (power_supply_register(pchg->dev, &psy->ac))
+	psy->ac = power_supply_register(pchg->dev, &lp8727_ac_desc, &psy_cfg);
+	if (IS_ERR(psy->ac))
 		goto err_psy_ac;
 
-	psy->usb.name = "usb";
-	psy->usb.type = POWER_SUPPLY_TYPE_USB;
-	psy->usb.properties = lp8727_charger_prop;
-	psy->usb.num_properties = ARRAY_SIZE(lp8727_charger_prop);
-	psy->usb.get_property = lp8727_charger_get_property;
-	psy->usb.supplied_to = battery_supplied_to;
-	psy->usb.num_supplicants = ARRAY_SIZE(battery_supplied_to);
-
-	if (power_supply_register(pchg->dev, &psy->usb))
+	psy->usb = power_supply_register(pchg->dev, &lp8727_usb_desc,
+					 &psy_cfg);
+	if (IS_ERR(psy->usb))
 		goto err_psy_usb;
 
-	psy->batt.name = "main_batt";
-	psy->batt.type = POWER_SUPPLY_TYPE_BATTERY;
-	psy->batt.properties = lp8727_battery_prop;
-	psy->batt.num_properties = ARRAY_SIZE(lp8727_battery_prop);
-	psy->batt.get_property = lp8727_battery_get_property;
-	psy->batt.external_power_changed = lp8727_charger_changed;
-
-	if (power_supply_register(pchg->dev, &psy->batt))
+	psy->batt = power_supply_register(pchg->dev, &lp8727_batt_desc, NULL);
+	if (IS_ERR(psy->batt))
 		goto err_psy_batt;
 
 	return 0;
 
 err_psy_batt:
-	power_supply_unregister(&psy->usb);
+	power_supply_unregister(psy->usb);
 err_psy_usb:
-	power_supply_unregister(&psy->ac);
+	power_supply_unregister(psy->ac);
 err_psy_ac:
 	return -EPERM;
 }
@@ -477,9 +487,9 @@ static void lp8727_unregister_psy(struct lp8727_chg *pchg)
 	if (!psy)
 		return;
 
-	power_supply_unregister(&psy->ac);
-	power_supply_unregister(&psy->usb);
-	power_supply_unregister(&psy->batt);
+	power_supply_unregister(psy->ac);
+	power_supply_unregister(psy->usb);
+	power_supply_unregister(psy->batt);
 }
 
 #ifdef CONFIG_OF
diff --git a/drivers/power/lp8788-charger.c b/drivers/power/lp8788-charger.c
index 21fc233c7d61..f5a48fd68b01 100644
--- a/drivers/power/lp8788-charger.c
+++ b/drivers/power/lp8788-charger.c
@@ -105,8 +105,8 @@ struct lp8788_chg_irq {
  */
 struct lp8788_charger {
 	struct lp8788 *lp;
-	struct power_supply charger;
-	struct power_supply battery;
+	struct power_supply *charger;
+	struct power_supply *battery;
 	struct work_struct charger_work;
 	struct iio_channel *chan[LP8788_NUM_CHG_ADC];
 	struct lp8788_chg_irq irqs[LP8788_MAX_CHG_IRQS];
@@ -148,7 +148,7 @@ static int lp8788_charger_get_property(struct power_supply *psy,
 					enum power_supply_property psp,
 					union power_supply_propval *val)
 {
-	struct lp8788_charger *pchg = dev_get_drvdata(psy->dev->parent);
+	struct lp8788_charger *pchg = dev_get_drvdata(psy->dev.parent);
 	u8 read;
 
 	switch (psp) {
@@ -337,7 +337,7 @@ static int lp8788_battery_get_property(struct power_supply *psy,
 					enum power_supply_property psp,
 					union power_supply_propval *val)
 {
-	struct lp8788_charger *pchg = dev_get_drvdata(psy->dev->parent);
+	struct lp8788_charger *pchg = dev_get_drvdata(psy->dev.parent);
 
 	switch (psp) {
 	case POWER_SUPPLY_PROP_STATUS:
@@ -397,36 +397,50 @@ static int lp8788_update_charger_params(struct platform_device *pdev,
 	return 0;
 }
 
+static const struct power_supply_desc lp8788_psy_charger_desc = {
+	.name		= LP8788_CHARGER_NAME,
+	.type		= POWER_SUPPLY_TYPE_MAINS,
+	.properties	= lp8788_charger_prop,
+	.num_properties	= ARRAY_SIZE(lp8788_charger_prop),
+	.get_property	= lp8788_charger_get_property,
+};
+
+static const struct power_supply_desc lp8788_psy_battery_desc = {
+	.name		= LP8788_BATTERY_NAME,
+	.type		= POWER_SUPPLY_TYPE_BATTERY,
+	.properties	= lp8788_battery_prop,
+	.num_properties	= ARRAY_SIZE(lp8788_battery_prop),
+	.get_property	= lp8788_battery_get_property,
+};
+
 static int lp8788_psy_register(struct platform_device *pdev,
 				struct lp8788_charger *pchg)
 {
-	pchg->charger.name = LP8788_CHARGER_NAME;
-	pchg->charger.type = POWER_SUPPLY_TYPE_MAINS;
-	pchg->charger.properties = lp8788_charger_prop;
-	pchg->charger.num_properties = ARRAY_SIZE(lp8788_charger_prop);
-	pchg->charger.get_property = lp8788_charger_get_property;
-	pchg->charger.supplied_to = battery_supplied_to;
-	pchg->charger.num_supplicants = ARRAY_SIZE(battery_supplied_to);
-
-	if (power_supply_register(&pdev->dev, &pchg->charger))
-		return -EPERM;
+	struct power_supply_config charger_cfg = {};
+
+	charger_cfg.supplied_to = battery_supplied_to;
+	charger_cfg.num_supplicants = ARRAY_SIZE(battery_supplied_to);
 
-	pchg->battery.name = LP8788_BATTERY_NAME;
-	pchg->battery.type = POWER_SUPPLY_TYPE_BATTERY;
-	pchg->battery.properties = lp8788_battery_prop;
-	pchg->battery.num_properties = ARRAY_SIZE(lp8788_battery_prop);
-	pchg->battery.get_property = lp8788_battery_get_property;
+	pchg->charger = power_supply_register(&pdev->dev,
+					      &lp8788_psy_charger_desc,
+					      &charger_cfg);
+	if (IS_ERR(pchg->charger))
+		return -EPERM;
 
-	if (power_supply_register(&pdev->dev, &pchg->battery))
+	pchg->battery = power_supply_register(&pdev->dev,
+					      &lp8788_psy_battery_desc, NULL);
+	if (IS_ERR(pchg->battery)) {
+		power_supply_unregister(pchg->charger);
 		return -EPERM;
+	}
 
 	return 0;
 }
 
 static void lp8788_psy_unregister(struct lp8788_charger *pchg)
 {
-	power_supply_unregister(&pchg->battery);
-	power_supply_unregister(&pchg->charger);
+	power_supply_unregister(pchg->battery);
+	power_supply_unregister(pchg->charger);
 }
 
 static void lp8788_charger_event(struct work_struct *work)
@@ -470,8 +484,8 @@ static irqreturn_t lp8788_charger_irq_thread(int virq, void *ptr)
 	case LP8788_INT_EOC:
 	case LP8788_INT_BATT_LOW:
 	case LP8788_INT_NO_BATT:
-		power_supply_changed(&pchg->charger);
-		power_supply_changed(&pchg->battery);
+		power_supply_changed(pchg->charger);
+		power_supply_changed(pchg->battery);
 		break;
 	default:
 		break;
diff --git a/drivers/power/ltc2941-battery-gauge.c b/drivers/power/ltc2941-battery-gauge.c
index e31c927a6d16..daeb0860736c 100644
--- a/drivers/power/ltc2941-battery-gauge.c
+++ b/drivers/power/ltc2941-battery-gauge.c
@@ -59,7 +59,8 @@ enum ltc294x_reg {
 
 struct ltc294x_info {
 	struct i2c_client *client;	/* I2C Client pointer */
-	struct power_supply supply;	/* Supply pointer */
+	struct power_supply *supply;	/* Supply pointer */
+	struct power_supply_desc supply_desc;	/* Supply description */
 	struct delayed_work work;	/* Work scheduler */
 	int num_regs;	/* Number of registers (chip type) */
 	int id;		/* Identifier of ltc294x chip */
@@ -294,8 +295,7 @@ static int ltc294x_get_property(struct power_supply *psy,
 				enum power_supply_property prop,
 				union power_supply_propval *val)
 {
-	struct ltc294x_info *info =
-		container_of(psy, struct ltc294x_info, supply);
+	struct ltc294x_info *info = power_supply_get_drvdata(psy);
 
 	switch (prop) {
 	case POWER_SUPPLY_PROP_CHARGE_NOW:
@@ -317,8 +317,7 @@ static int ltc294x_set_property(struct power_supply *psy,
 	enum power_supply_property psp,
 	const union power_supply_propval *val)
 {
-	struct ltc294x_info *info =
-		container_of(psy, struct ltc294x_info, supply);
+	struct ltc294x_info *info = power_supply_get_drvdata(psy);
 
 	switch (psp) {
 	case POWER_SUPPLY_PROP_CHARGE_NOW:
@@ -345,7 +344,7 @@ static void ltc294x_update(struct ltc294x_info *info)
 
 	if (charge != info->charge) {
 		info->charge = charge;
-		power_supply_changed(&info->supply);
+		power_supply_changed(info->supply);
 	}
 }
 
@@ -371,8 +370,8 @@ static int ltc294x_i2c_remove(struct i2c_client *client)
 	struct ltc294x_info *info = i2c_get_clientdata(client);
 
 	cancel_delayed_work(&info->work);
-	power_supply_unregister(&info->supply);
-	kfree(info->supply.name);
+	power_supply_unregister(info->supply);
+	kfree(info->supply_desc.name);
 	mutex_lock(&ltc294x_lock);
 	idr_remove(&ltc294x_id, info->id);
 	mutex_unlock(&ltc294x_lock);
@@ -382,6 +381,7 @@ static int ltc294x_i2c_remove(struct i2c_client *client)
 static int ltc294x_i2c_probe(struct i2c_client *client,
 	const struct i2c_device_id *id)
 {
+	struct power_supply_config psy_cfg = {};
 	struct ltc294x_info *info;
 	int ret;
 	int num;
@@ -406,8 +406,9 @@ static int ltc294x_i2c_probe(struct i2c_client *client,
 	i2c_set_clientdata(client, info);
 
 	info->num_regs = id->driver_data;
-	info->supply.name = kasprintf(GFP_KERNEL, "%s-%d", client->name, num);
-	if (!info->supply.name) {
+	info->supply_desc.name = kasprintf(GFP_KERNEL, "%s-%d", client->name,
+					   num);
+	if (!info->supply_desc.name) {
 		ret = -ENOMEM;
 		goto fail_name;
 	}
@@ -440,30 +441,32 @@ static int ltc294x_i2c_probe(struct i2c_client *client,
 	} else {
 		if (prescaler_exp > LTC2941_MAX_PRESCALER_EXP)
 			prescaler_exp = LTC2941_MAX_PRESCALER_EXP;
-		info->Qlsb = ((58 * 50000) / r_sense) /
+		info->Qlsb = ((85 * 50000) / r_sense) /
 				(128 / (1 << prescaler_exp));
 	}
 
 	info->client = client;
 	info->id = num;
-	info->supply.type = POWER_SUPPLY_TYPE_BATTERY;
-	info->supply.properties = ltc294x_properties;
+	info->supply_desc.type = POWER_SUPPLY_TYPE_BATTERY;
+	info->supply_desc.properties = ltc294x_properties;
 	if (info->num_regs >= LTC294X_REG_TEMPERATURE_LSB)
-		info->supply.num_properties =
+		info->supply_desc.num_properties =
 			ARRAY_SIZE(ltc294x_properties);
 	else if (info->num_regs >= LTC294X_REG_CURRENT_LSB)
-		info->supply.num_properties =
+		info->supply_desc.num_properties =
 			ARRAY_SIZE(ltc294x_properties) - 1;
 	else if (info->num_regs >= LTC294X_REG_VOLTAGE_LSB)
-		info->supply.num_properties =
+		info->supply_desc.num_properties =
 			ARRAY_SIZE(ltc294x_properties) - 2;
 	else
-		info->supply.num_properties =
+		info->supply_desc.num_properties =
 			ARRAY_SIZE(ltc294x_properties) - 3;
-	info->supply.get_property = ltc294x_get_property;
-	info->supply.set_property = ltc294x_set_property;
-	info->supply.property_is_writeable = ltc294x_property_is_writeable;
-	info->supply.external_power_changed	= NULL;
+	info->supply_desc.get_property = ltc294x_get_property;
+	info->supply_desc.set_property = ltc294x_set_property;
+	info->supply_desc.property_is_writeable = ltc294x_property_is_writeable;
+	info->supply_desc.external_power_changed	= NULL;
+
+	psy_cfg.drv_data = info;
 
 	INIT_DELAYED_WORK(&info->work, ltc294x_work);
 
@@ -473,9 +476,11 @@ static int ltc294x_i2c_probe(struct i2c_client *client,
 		goto fail_comm;
 	}
 
-	ret = power_supply_register(&client->dev, &info->supply);
-	if (ret) {
+	info->supply = power_supply_register(&client->dev, &info->supply_desc,
+					     &psy_cfg);
+	if (IS_ERR(info->supply)) {
 		dev_err(&client->dev, "failed to register ltc2941\n");
+		ret = PTR_ERR(info->supply);
 		goto fail_register;
 	} else {
 		schedule_delayed_work(&info->work, LTC294X_WORK_DELAY * HZ);
@@ -484,7 +489,7 @@ static int ltc294x_i2c_probe(struct i2c_client *client,
 	return 0;
 
 fail_register:
-	kfree(info->supply.name);
+	kfree(info->supply_desc.name);
 fail_comm:
 fail_name:
 fail_info:
diff --git a/drivers/power/max14577_charger.c b/drivers/power/max14577_charger.c
index ef4103ee6021..a36bcaf62dd4 100644
--- a/drivers/power/max14577_charger.c
+++ b/drivers/power/max14577_charger.c
@@ -22,12 +22,9 @@
 #include <linux/mfd/max14577.h>
 
 struct max14577_charger {
-	struct device *dev;
-	struct max14577	*max14577;
-	struct power_supply	charger;
-
-	unsigned int		charging_state;
-	unsigned int		battery_state;
+	struct device		*dev;
+	struct max14577		*max14577;
+	struct power_supply	*charger;
 
 	struct max14577_charger_platform_data	*pdata;
 };
@@ -57,10 +54,10 @@ static enum max14577_muic_charger_type maxim_get_charger_type(
 	}
 }
 
-static int max14577_get_charger_state(struct max14577_charger *chg)
+static int max14577_get_charger_state(struct max14577_charger *chg, int *val)
 {
 	struct regmap *rmap = chg->max14577->regmap;
-	int state = POWER_SUPPLY_STATUS_DISCHARGING;
+	int ret;
 	u8 reg_data;
 
 	/*
@@ -74,23 +71,32 @@ static int max14577_get_charger_state(struct max14577_charger *chg)
 	 *  - handle properly dead-battery charging (respect timer)
 	 *  - handle timers (fast-charge and prequal) /MBCCHGERR/
 	 */
-	max14577_read_reg(rmap, MAX14577_CHG_REG_CHG_CTRL2, &reg_data);
-	if ((reg_data & CHGCTRL2_MBCHOSTEN_MASK) == 0)
-		goto state_set;
+	ret = max14577_read_reg(rmap, MAX14577_CHG_REG_CHG_CTRL2, &reg_data);
+	if (ret < 0)
+		goto out;
+
+	if ((reg_data & CHGCTRL2_MBCHOSTEN_MASK) == 0) {
+		*val = POWER_SUPPLY_STATUS_DISCHARGING;
+		goto out;
+	}
+
+	ret = max14577_read_reg(rmap, MAX14577_CHG_REG_STATUS3, &reg_data);
+	if (ret < 0)
+		goto out;
 
-	max14577_read_reg(rmap, MAX14577_CHG_REG_STATUS3, &reg_data);
 	if (reg_data & STATUS3_CGMBC_MASK) {
 		/* Charger or USB-cable is connected */
 		if (reg_data & STATUS3_EOC_MASK)
-			state = POWER_SUPPLY_STATUS_FULL;
+			*val = POWER_SUPPLY_STATUS_FULL;
 		else
-			state = POWER_SUPPLY_STATUS_CHARGING;
-		goto state_set;
+			*val = POWER_SUPPLY_STATUS_CHARGING;
+		goto out;
 	}
 
-state_set:
-	chg->charging_state = state;
-	return state;
+	*val = POWER_SUPPLY_STATUS_DISCHARGING;
+
+out:
+	return ret;
 }
 
 /*
@@ -98,8 +104,10 @@ state_set:
  *  - POWER_SUPPLY_CHARGE_TYPE_NONE
  *  - POWER_SUPPLY_CHARGE_TYPE_FAST
  */
-static int max14577_get_charge_type(struct max14577_charger *chg)
+static int max14577_get_charge_type(struct max14577_charger *chg, int *val)
 {
+	int ret, charging;
+
 	/*
 	 * TODO: CHARGE_TYPE_TRICKLE (VCHGR_RC or EOC)?
 	 * As spec says:
@@ -108,18 +116,29 @@ static int max14577_get_charge_type(struct max14577_charger *chg)
 	 *  top-off timer starts. The device continues to trickle
 	 *  charge the battery until the top-off timer runs out."
 	 */
-	if (max14577_get_charger_state(chg) == POWER_SUPPLY_STATUS_CHARGING)
-		return POWER_SUPPLY_CHARGE_TYPE_FAST;
-	return POWER_SUPPLY_CHARGE_TYPE_NONE;
+	ret = max14577_get_charger_state(chg, &charging);
+	if (ret < 0)
+		return ret;
+
+	if (charging == POWER_SUPPLY_STATUS_CHARGING)
+		*val = POWER_SUPPLY_CHARGE_TYPE_FAST;
+	else
+		*val = POWER_SUPPLY_CHARGE_TYPE_NONE;
+
+	return 0;
 }
 
-static int max14577_get_online(struct max14577_charger *chg)
+static int max14577_get_online(struct max14577_charger *chg, int *val)
 {
 	struct regmap *rmap = chg->max14577->regmap;
 	u8 reg_data;
+	int ret;
 	enum max14577_muic_charger_type chg_type;
 
-	max14577_read_reg(rmap, MAX14577_MUIC_REG_STATUS2, &reg_data);
+	ret = max14577_read_reg(rmap, MAX14577_MUIC_REG_STATUS2, &reg_data);
+	if (ret < 0)
+		return ret;
+
 	reg_data = ((reg_data & STATUS2_CHGTYP_MASK) >> STATUS2_CHGTYP_SHIFT);
 	chg_type = maxim_get_charger_type(chg->max14577->dev_type, reg_data);
 	switch (chg_type) {
@@ -129,14 +148,17 @@ static int max14577_get_online(struct max14577_charger *chg)
 	case MAX14577_CHARGER_TYPE_SPECIAL_1A:
 	case MAX14577_CHARGER_TYPE_DEAD_BATTERY:
 	case MAX77836_CHARGER_TYPE_SPECIAL_BIAS:
-		return 1;
+		*val = 1;
+		break;
 	case MAX14577_CHARGER_TYPE_NONE:
 	case MAX14577_CHARGER_TYPE_DOWNSTREAM_PORT:
 	case MAX14577_CHARGER_TYPE_RESERVED:
 	case MAX77836_CHARGER_TYPE_RESERVED:
 	default:
-		return 0;
+		*val = 0;
 	}
+
+	return 0;
 }
 
 /*
@@ -145,30 +167,38 @@ static int max14577_get_online(struct max14577_charger *chg)
  *  - POWER_SUPPLY_HEALTH_OVERVOLTAGE
  *  - POWER_SUPPLY_HEALTH_GOOD
  */
-static int max14577_get_battery_health(struct max14577_charger *chg)
+static int max14577_get_battery_health(struct max14577_charger *chg, int *val)
 {
 	struct regmap *rmap = chg->max14577->regmap;
-	int state = POWER_SUPPLY_HEALTH_GOOD;
+	int ret;
 	u8 reg_data;
 	enum max14577_muic_charger_type chg_type;
 
-	max14577_read_reg(rmap, MAX14577_MUIC_REG_STATUS2, &reg_data);
+	ret = max14577_read_reg(rmap, MAX14577_MUIC_REG_STATUS2, &reg_data);
+	if (ret < 0)
+		goto out;
+
 	reg_data = ((reg_data & STATUS2_CHGTYP_MASK) >> STATUS2_CHGTYP_SHIFT);
 	chg_type = maxim_get_charger_type(chg->max14577->dev_type, reg_data);
 	if (chg_type == MAX14577_CHARGER_TYPE_DEAD_BATTERY) {
-		state = POWER_SUPPLY_HEALTH_DEAD;
-		goto state_set;
+		*val = POWER_SUPPLY_HEALTH_DEAD;
+		goto out;
 	}
 
-	max14577_read_reg(rmap, MAX14577_CHG_REG_STATUS3, &reg_data);
+	ret = max14577_read_reg(rmap, MAX14577_CHG_REG_STATUS3, &reg_data);
+	if (ret < 0)
+		goto out;
+
 	if (reg_data & STATUS3_OVP_MASK) {
-		state = POWER_SUPPLY_HEALTH_OVERVOLTAGE;
-		goto state_set;
+		*val = POWER_SUPPLY_HEALTH_OVERVOLTAGE;
+		goto out;
 	}
 
-state_set:
-	chg->battery_state = state;
-	return state;
+	/* Not dead, not overvoltage */
+	*val = POWER_SUPPLY_HEALTH_GOOD;
+
+out:
+	return ret;
 }
 
 /*
@@ -176,9 +206,11 @@ state_set:
  * The max14577 chip doesn't report any status of battery presence.
  * Lets assume that it will always be used with some battery.
  */
-static int max14577_get_present(struct max14577_charger *chg)
+static int max14577_get_present(struct max14577_charger *chg, int *val)
 {
-	return 1;
+	*val = 1;
+
+	return 0;
 }
 
 static int max14577_set_fast_charge_timer(struct max14577_charger *chg,
@@ -389,26 +421,24 @@ static int max14577_charger_get_property(struct power_supply *psy,
 			    enum power_supply_property psp,
 			    union power_supply_propval *val)
 {
-	struct max14577_charger *chg = container_of(psy,
-						  struct max14577_charger,
-						  charger);
+	struct max14577_charger *chg = power_supply_get_drvdata(psy);
 	int ret = 0;
 
 	switch (psp) {
 	case POWER_SUPPLY_PROP_STATUS:
-		val->intval = max14577_get_charger_state(chg);
+		ret = max14577_get_charger_state(chg, &val->intval);
 		break;
 	case POWER_SUPPLY_PROP_CHARGE_TYPE:
-		val->intval = max14577_get_charge_type(chg);
+		ret = max14577_get_charge_type(chg, &val->intval);
 		break;
 	case POWER_SUPPLY_PROP_HEALTH:
-		val->intval = max14577_get_battery_health(chg);
+		ret = max14577_get_battery_health(chg, &val->intval);
 		break;
 	case POWER_SUPPLY_PROP_PRESENT:
-		val->intval = max14577_get_present(chg);
+		ret = max14577_get_present(chg, &val->intval);
 		break;
 	case POWER_SUPPLY_PROP_ONLINE:
-		val->intval = max14577_get_online(chg);
+		ret = max14577_get_online(chg, &val->intval);
 		break;
 	case POWER_SUPPLY_PROP_MODEL_NAME:
 		BUILD_BUG_ON(ARRAY_SIZE(model_names) != MAXIM_DEVICE_TYPE_NUM);
@@ -424,6 +454,14 @@ static int max14577_charger_get_property(struct power_supply *psy,
 	return ret;
 }
 
+static const struct power_supply_desc max14577_charger_desc = {
+	.name = "max14577-charger",
+	.type = POWER_SUPPLY_TYPE_BATTERY,
+	.properties = max14577_charger_props,
+	.num_properties = ARRAY_SIZE(max14577_charger_props),
+	.get_property = max14577_charger_get_property,
+};
+
 #ifdef CONFIG_OF
 static struct max14577_charger_platform_data *max14577_charger_dt_init(
 		struct platform_device *pdev)
@@ -531,6 +569,7 @@ static DEVICE_ATTR(fast_charge_timer, S_IRUGO | S_IWUSR,
 static int max14577_charger_probe(struct platform_device *pdev)
 {
 	struct max14577_charger *chg;
+	struct power_supply_config psy_cfg = {};
 	struct max14577 *max14577 = dev_get_drvdata(pdev->dev.parent);
 	int ret;
 
@@ -550,21 +589,18 @@ static int max14577_charger_probe(struct platform_device *pdev)
 	if (ret)
 		return ret;
 
-	chg->charger.name = "max14577-charger",
-	chg->charger.type = POWER_SUPPLY_TYPE_BATTERY,
-	chg->charger.properties = max14577_charger_props,
-	chg->charger.num_properties = ARRAY_SIZE(max14577_charger_props),
-	chg->charger.get_property = max14577_charger_get_property,
-
 	ret = device_create_file(&pdev->dev, &dev_attr_fast_charge_timer);
 	if (ret) {
 		dev_err(&pdev->dev, "failed: create sysfs entry\n");
 		return ret;
 	}
 
-	ret = power_supply_register(&pdev->dev, &chg->charger);
-	if (ret) {
+	psy_cfg.drv_data = chg;
+	chg->charger = power_supply_register(&pdev->dev, &max14577_charger_desc,
+						&psy_cfg);
+	if (IS_ERR(chg->charger)) {
 		dev_err(&pdev->dev, "failed: power supply register\n");
+		ret = PTR_ERR(chg->charger);
 		goto err;
 	}
 
@@ -585,7 +621,7 @@ static int max14577_charger_remove(struct platform_device *pdev)
 	struct max14577_charger *chg = platform_get_drvdata(pdev);
 
 	device_remove_file(&pdev->dev, &dev_attr_fast_charge_timer);
-	power_supply_unregister(&chg->charger);
+	power_supply_unregister(chg->charger);
 
 	return 0;
 }
diff --git a/drivers/power/max17040_battery.c b/drivers/power/max17040_battery.c
index 14d44706327b..8689c80202b5 100644
--- a/drivers/power/max17040_battery.c
+++ b/drivers/power/max17040_battery.c
@@ -40,7 +40,7 @@
 struct max17040_chip {
 	struct i2c_client		*client;
 	struct delayed_work		work;
-	struct power_supply		battery;
+	struct power_supply		*battery;
 	struct max17040_platform_data	*pdata;
 
 	/* State Of Connect */
@@ -57,8 +57,7 @@ static int max17040_get_property(struct power_supply *psy,
 			    enum power_supply_property psp,
 			    union power_supply_propval *val)
 {
-	struct max17040_chip *chip = container_of(psy,
-				struct max17040_chip, battery);
+	struct max17040_chip *chip = power_supply_get_drvdata(psy);
 
 	switch (psp) {
 	case POWER_SUPPLY_PROP_STATUS:
@@ -188,7 +187,8 @@ static void max17040_work(struct work_struct *work)
 	max17040_get_online(chip->client);
 	max17040_get_status(chip->client);
 
-	schedule_delayed_work(&chip->work, MAX17040_DELAY);
+	queue_delayed_work(system_power_efficient_wq, &chip->work,
+			   MAX17040_DELAY);
 }
 
 static enum power_supply_property max17040_battery_props[] = {
@@ -198,12 +198,20 @@ static enum power_supply_property max17040_battery_props[] = {
 	POWER_SUPPLY_PROP_CAPACITY,
 };
 
+static const struct power_supply_desc max17040_battery_desc = {
+	.name		= "battery",
+	.type		= POWER_SUPPLY_TYPE_BATTERY,
+	.get_property	= max17040_get_property,
+	.properties	= max17040_battery_props,
+	.num_properties	= ARRAY_SIZE(max17040_battery_props),
+};
+
 static int max17040_probe(struct i2c_client *client,
 			const struct i2c_device_id *id)
 {
 	struct i2c_adapter *adapter = to_i2c_adapter(client->dev.parent);
+	struct power_supply_config psy_cfg = {};
 	struct max17040_chip *chip;
-	int ret;
 
 	if (!i2c_check_functionality(adapter, I2C_FUNC_SMBUS_BYTE))
 		return -EIO;
@@ -216,24 +224,21 @@ static int max17040_probe(struct i2c_client *client,
 	chip->pdata = client->dev.platform_data;
 
 	i2c_set_clientdata(client, chip);
+	psy_cfg.drv_data = chip;
 
-	chip->battery.name		= "battery";
-	chip->battery.type		= POWER_SUPPLY_TYPE_BATTERY;
-	chip->battery.get_property	= max17040_get_property;
-	chip->battery.properties	= max17040_battery_props;
-	chip->battery.num_properties	= ARRAY_SIZE(max17040_battery_props);
-
-	ret = power_supply_register(&client->dev, &chip->battery);
-	if (ret) {
+	chip->battery = power_supply_register(&client->dev,
+				&max17040_battery_desc, &psy_cfg);
+	if (IS_ERR(chip->battery)) {
 		dev_err(&client->dev, "failed: power supply register\n");
-		return ret;
+		return PTR_ERR(chip->battery);
 	}
 
 	max17040_reset(client);
 	max17040_get_version(client);
 
 	INIT_DEFERRABLE_WORK(&chip->work, max17040_work);
-	schedule_delayed_work(&chip->work, MAX17040_DELAY);
+	queue_delayed_work(system_power_efficient_wq, &chip->work,
+			   MAX17040_DELAY);
 
 	return 0;
 }
@@ -242,7 +247,7 @@ static int max17040_remove(struct i2c_client *client)
 {
 	struct max17040_chip *chip = i2c_get_clientdata(client);
 
-	power_supply_unregister(&chip->battery);
+	power_supply_unregister(chip->battery);
 	cancel_delayed_work(&chip->work);
 	return 0;
 }
@@ -263,7 +268,8 @@ static int max17040_resume(struct device *dev)
 	struct i2c_client *client = to_i2c_client(dev);
 	struct max17040_chip *chip = i2c_get_clientdata(client);
 
-	schedule_delayed_work(&chip->work, MAX17040_DELAY);
+	queue_delayed_work(system_power_efficient_wq, &chip->work,
+			   MAX17040_DELAY);
 	return 0;
 }
 
diff --git a/drivers/power/max17042_battery.c b/drivers/power/max17042_battery.c
index 1da6c5fbdff5..6cc5e87ec031 100644
--- a/drivers/power/max17042_battery.c
+++ b/drivers/power/max17042_battery.c
@@ -63,13 +63,10 @@
 #define dP_ACC_100	0x1900
 #define dP_ACC_200	0x3200
 
-#define MAX17042_IC_VERSION	0x0092
-#define MAX17047_IC_VERSION	0x00AC	/* same for max17050 */
-
 struct max17042_chip {
 	struct i2c_client *client;
 	struct regmap *regmap;
-	struct power_supply battery;
+	struct power_supply *battery;
 	enum max170xx_chip_type chip_type;
 	struct max17042_platform_data *pdata;
 	struct work_struct work;
@@ -96,8 +93,7 @@ static int max17042_get_property(struct power_supply *psy,
 			    enum power_supply_property psp,
 			    union power_supply_propval *val)
 {
-	struct max17042_chip *chip = container_of(psy,
-				struct max17042_chip, battery);
+	struct max17042_chip *chip = power_supply_get_drvdata(psy);
 	struct regmap *map = chip->regmap;
 	int ret;
 	u32 data;
@@ -132,7 +128,7 @@ static int max17042_get_property(struct power_supply *psy,
 		val->intval *= 20000; /* Units of LSB = 20mV */
 		break;
 	case POWER_SUPPLY_PROP_VOLTAGE_MIN_DESIGN:
-		if (chip->chip_type == MAX17042)
+		if (chip->chip_type == MAXIM_DEVICE_TYPE_MAX17042)
 			ret = regmap_read(map, MAX17042_V_empty, &data);
 		else
 			ret = regmap_read(map, MAX17047_V_empty, &data);
@@ -272,6 +268,7 @@ static inline void max17042_override_por(struct regmap *map,
 static inline void max10742_unlock_model(struct max17042_chip *chip)
 {
 	struct regmap *map = chip->regmap;
+
 	regmap_write(map, MAX17042_MLOCKReg1, MODEL_UNLOCK1);
 	regmap_write(map, MAX17042_MLOCKReg2, MODEL_UNLOCK2);
 }
@@ -289,6 +286,7 @@ static inline void max17042_write_model_data(struct max17042_chip *chip,
 {
 	struct regmap *map = chip->regmap;
 	int i;
+
 	for (i = 0; i < size; i++)
 		regmap_write(map, addr + i,
 			chip->pdata->config_data->cell_char_tbl[i]);
@@ -379,7 +377,8 @@ static void max17042_write_config_regs(struct max17042_chip *chip)
 	regmap_write(map, MAX17042_FilterCFG,
 			config->filter_cfg);
 	regmap_write(map, MAX17042_RelaxCFG, config->relax_cfg);
-	if (chip->chip_type == MAX17047)
+	if (chip->chip_type == MAXIM_DEVICE_TYPE_MAX17047 ||
+			chip->chip_type == MAXIM_DEVICE_TYPE_MAX17050)
 		regmap_write(map, MAX17047_FullSOCThr,
 						config->full_soc_thresh);
 }
@@ -392,7 +391,7 @@ static void  max17042_write_custom_regs(struct max17042_chip *chip)
 	max17042_write_verify_reg(map, MAX17042_RCOMP0, config->rcomp0);
 	max17042_write_verify_reg(map, MAX17042_TempCo,	config->tcompc0);
 	max17042_write_verify_reg(map, MAX17042_ICHGTerm, config->ichgt_term);
-	if (chip->chip_type == MAX17042) {
+	if (chip->chip_type == MAXIM_DEVICE_TYPE_MAX17042) {
 		regmap_write(map, MAX17042_EmptyTempCo,	config->empty_tempco);
 		max17042_write_verify_reg(map, MAX17042_K_empty0,
 					config->kempty0);
@@ -501,14 +500,14 @@ static inline void max17042_override_por_values(struct max17042_chip *chip)
 
 	max17042_override_por(map, MAX17042_FullCAP, config->fullcap);
 	max17042_override_por(map, MAX17042_FullCAPNom, config->fullcapnom);
-	if (chip->chip_type == MAX17042)
+	if (chip->chip_type == MAXIM_DEVICE_TYPE_MAX17042)
 		max17042_override_por(map, MAX17042_SOC_empty,
 						config->socempty);
 	max17042_override_por(map, MAX17042_LAvg_empty, config->lavg_empty);
 	max17042_override_por(map, MAX17042_dQacc, config->dqacc);
 	max17042_override_por(map, MAX17042_dPacc, config->dpacc);
 
-	if (chip->chip_type == MAX17042)
+	if (chip->chip_type == MAXIM_DEVICE_TYPE_MAX17042)
 		max17042_override_por(map, MAX17042_V_empty, config->vempty);
 	else
 		max17042_override_por(map, MAX17047_V_empty, config->vempty);
@@ -529,7 +528,6 @@ static int max17042_init_chip(struct max17042_chip *chip)
 {
 	struct regmap *map = chip->regmap;
 	int ret;
-	int val;
 
 	max17042_override_por_values(chip);
 	/* After Power up, the MAX17042 requires 500mS in order
@@ -572,8 +570,7 @@ static int max17042_init_chip(struct max17042_chip *chip)
 	max17042_load_new_capacity_params(chip);
 
 	/* Init complete, Clear the POR bit */
-	regmap_read(map, MAX17042_STATUS, &val);
-	regmap_write(map, MAX17042_STATUS, val & (~STATUS_POR_BIT));
+	regmap_update_bits(map, MAX17042_STATUS, STATUS_POR_BIT, 0x0);
 	return 0;
 }
 
@@ -604,7 +601,7 @@ static irqreturn_t max17042_thread_handler(int id, void *dev)
 		max17042_set_soc_threshold(chip, 1);
 	}
 
-	power_supply_changed(&chip->battery);
+	power_supply_changed(chip->battery);
 	return IRQ_HANDLED;
 }
 
@@ -664,10 +661,28 @@ static const struct regmap_config max17042_regmap_config = {
 	.val_format_endian = REGMAP_ENDIAN_NATIVE,
 };
 
+static const struct power_supply_desc max17042_psy_desc = {
+	.name		= "max170xx_battery",
+	.type		= POWER_SUPPLY_TYPE_BATTERY,
+	.get_property	= max17042_get_property,
+	.properties	= max17042_battery_props,
+	.num_properties	= ARRAY_SIZE(max17042_battery_props),
+};
+
+static const struct power_supply_desc max17042_no_current_sense_psy_desc = {
+	.name		= "max170xx_battery",
+	.type		= POWER_SUPPLY_TYPE_BATTERY,
+	.get_property	= max17042_get_property,
+	.properties	= max17042_battery_props,
+	.num_properties	= ARRAY_SIZE(max17042_battery_props) - 2,
+};
+
 static int max17042_probe(struct i2c_client *client,
 			const struct i2c_device_id *id)
 {
 	struct i2c_adapter *adapter = to_i2c_adapter(client->dev.parent);
+	const struct power_supply_desc *max17042_desc = &max17042_psy_desc;
+	struct power_supply_config psy_cfg = {};
 	struct max17042_chip *chip;
 	int ret;
 	int i;
@@ -694,29 +709,13 @@ static int max17042_probe(struct i2c_client *client,
 	}
 
 	i2c_set_clientdata(client, chip);
-
-	regmap_read(chip->regmap, MAX17042_DevName, &val);
-	if (val == MAX17042_IC_VERSION) {
-		dev_dbg(&client->dev, "chip type max17042 detected\n");
-		chip->chip_type = MAX17042;
-	} else if (val == MAX17047_IC_VERSION) {
-		dev_dbg(&client->dev, "chip type max17047/50 detected\n");
-		chip->chip_type = MAX17047;
-	} else {
-		dev_err(&client->dev, "device version mismatch: %x\n", val);
-		return -EIO;
-	}
-
-	chip->battery.name		= "max170xx_battery";
-	chip->battery.type		= POWER_SUPPLY_TYPE_BATTERY;
-	chip->battery.get_property	= max17042_get_property;
-	chip->battery.properties	= max17042_battery_props;
-	chip->battery.num_properties	= ARRAY_SIZE(max17042_battery_props);
+	chip->chip_type = id->driver_data;
+	psy_cfg.drv_data = chip;
 
 	/* When current is not measured,
 	 * CURRENT_NOW and CURRENT_AVG properties should be invisible. */
 	if (!chip->pdata->enable_current_sense)
-		chip->battery.num_properties -= 2;
+		max17042_desc = &max17042_no_current_sense_psy_desc;
 
 	if (chip->pdata->r_sns == 0)
 		chip->pdata->r_sns = MAX17042_DEFAULT_SNS_RESISTOR;
@@ -733,21 +732,22 @@ static int max17042_probe(struct i2c_client *client,
 		regmap_write(chip->regmap, MAX17042_LearnCFG, 0x0007);
 	}
 
-	ret = power_supply_register(&client->dev, &chip->battery);
-	if (ret) {
+	chip->battery = power_supply_register(&client->dev, max17042_desc,
+						&psy_cfg);
+	if (IS_ERR(chip->battery)) {
 		dev_err(&client->dev, "failed: power supply register\n");
-		return ret;
+		return PTR_ERR(chip->battery);
 	}
 
 	if (client->irq) {
 		ret = request_threaded_irq(client->irq, NULL,
 					max17042_thread_handler,
 					IRQF_TRIGGER_FALLING | IRQF_ONESHOT,
-					chip->battery.name, chip);
+					chip->battery->desc->name, chip);
 		if (!ret) {
-			regmap_read(chip->regmap, MAX17042_CONFIG, &val);
-			val |= CONFIG_ALRT_BIT_ENBL;
-			regmap_write(chip->regmap, MAX17042_CONFIG, val);
+			regmap_update_bits(chip->regmap, MAX17042_CONFIG,
+					CONFIG_ALRT_BIT_ENBL,
+					CONFIG_ALRT_BIT_ENBL);
 			max17042_set_soc_threshold(chip, 1);
 		} else {
 			client->irq = 0;
@@ -773,7 +773,7 @@ static int max17042_remove(struct i2c_client *client)
 
 	if (client->irq)
 		free_irq(client->irq, chip);
-	power_supply_unregister(&chip->battery);
+	power_supply_unregister(chip->battery);
 	return 0;
 }
 
@@ -823,9 +823,9 @@ MODULE_DEVICE_TABLE(of, max17042_dt_match);
 #endif
 
 static const struct i2c_device_id max17042_id[] = {
-	{ "max17042", 0 },
-	{ "max17047", 1 },
-	{ "max17050", 2 },
+	{ "max17042", MAXIM_DEVICE_TYPE_MAX17042 },
+	{ "max17047", MAXIM_DEVICE_TYPE_MAX17047 },
+	{ "max17050", MAXIM_DEVICE_TYPE_MAX17050 },
 	{ }
 };
 MODULE_DEVICE_TABLE(i2c, max17042_id);
diff --git a/drivers/power/max77693_charger.c b/drivers/power/max77693_charger.c
index b042970fdeaf..754879eb59f6 100644
--- a/drivers/power/max77693_charger.c
+++ b/drivers/power/max77693_charger.c
@@ -22,14 +22,14 @@
 #include <linux/mfd/max77693.h>
 #include <linux/mfd/max77693-private.h>
 
-static const char *max77693_charger_name		= "max77693-charger";
+#define MAX77693_CHARGER_NAME				"max77693-charger"
 static const char *max77693_charger_model		= "MAX77693";
 static const char *max77693_charger_manufacturer	= "Maxim Integrated";
 
 struct max77693_charger {
 	struct device		*dev;
 	struct max77693_dev	*max77693;
-	struct power_supply	charger;
+	struct power_supply	*charger;
 
 	u32 constant_volt;
 	u32 min_system_volt;
@@ -38,13 +38,14 @@ struct max77693_charger {
 	u32 charge_input_threshold_volt;
 };
 
-static int max77693_get_charger_state(struct regmap *regmap)
+static int max77693_get_charger_state(struct regmap *regmap, int *val)
 {
-	int state;
+	int ret;
 	unsigned int data;
 
-	if (regmap_read(regmap, MAX77693_CHG_REG_CHG_DETAILS_01, &data) < 0)
-		return POWER_SUPPLY_STATUS_UNKNOWN;
+	ret = regmap_read(regmap, MAX77693_CHG_REG_CHG_DETAILS_01, &data);
+	if (ret < 0)
+		return ret;
 
 	data &= CHG_DETAILS_01_CHG_MASK;
 	data >>= CHG_DETAILS_01_CHG_SHIFT;
@@ -56,35 +57,36 @@ static int max77693_get_charger_state(struct regmap *regmap)
 	case MAX77693_CHARGING_TOP_OFF:
 	/* In high temp the charging current is reduced, but still charging */
 	case MAX77693_CHARGING_HIGH_TEMP:
-		state = POWER_SUPPLY_STATUS_CHARGING;
+		*val = POWER_SUPPLY_STATUS_CHARGING;
 		break;
 	case MAX77693_CHARGING_DONE:
-		state = POWER_SUPPLY_STATUS_FULL;
+		*val = POWER_SUPPLY_STATUS_FULL;
 		break;
 	case MAX77693_CHARGING_TIMER_EXPIRED:
 	case MAX77693_CHARGING_THERMISTOR_SUSPEND:
-		state = POWER_SUPPLY_STATUS_NOT_CHARGING;
+		*val = POWER_SUPPLY_STATUS_NOT_CHARGING;
 		break;
 	case MAX77693_CHARGING_OFF:
 	case MAX77693_CHARGING_OVER_TEMP:
 	case MAX77693_CHARGING_WATCHDOG_EXPIRED:
-		state = POWER_SUPPLY_STATUS_DISCHARGING;
+		*val = POWER_SUPPLY_STATUS_DISCHARGING;
 		break;
 	case MAX77693_CHARGING_RESERVED:
 	default:
-		state = POWER_SUPPLY_STATUS_UNKNOWN;
+		*val = POWER_SUPPLY_STATUS_UNKNOWN;
 	}
 
-	return state;
+	return 0;
 }
 
-static int max77693_get_charge_type(struct regmap *regmap)
+static int max77693_get_charge_type(struct regmap *regmap, int *val)
 {
-	int state;
+	int ret;
 	unsigned int data;
 
-	if (regmap_read(regmap, MAX77693_CHG_REG_CHG_DETAILS_01, &data) < 0)
-		return POWER_SUPPLY_CHARGE_TYPE_UNKNOWN;
+	ret = regmap_read(regmap, MAX77693_CHG_REG_CHG_DETAILS_01, &data);
+	if (ret < 0)
+		return ret;
 
 	data &= CHG_DETAILS_01_CHG_MASK;
 	data >>= CHG_DETAILS_01_CHG_SHIFT;
@@ -96,13 +98,13 @@ static int max77693_get_charge_type(struct regmap *regmap)
 	 * 100 and 250 mA. It is higher than prequalification current.
 	 */
 	case MAX77693_CHARGING_TOP_OFF:
-		state = POWER_SUPPLY_CHARGE_TYPE_TRICKLE;
+		*val = POWER_SUPPLY_CHARGE_TYPE_TRICKLE;
 		break;
 	case MAX77693_CHARGING_FAST_CONST_CURRENT:
 	case MAX77693_CHARGING_FAST_CONST_VOLTAGE:
 	/* In high temp the charging current is reduced, but still charging */
 	case MAX77693_CHARGING_HIGH_TEMP:
-		state = POWER_SUPPLY_CHARGE_TYPE_FAST;
+		*val = POWER_SUPPLY_CHARGE_TYPE_FAST;
 		break;
 	case MAX77693_CHARGING_DONE:
 	case MAX77693_CHARGING_TIMER_EXPIRED:
@@ -110,14 +112,14 @@ static int max77693_get_charge_type(struct regmap *regmap)
 	case MAX77693_CHARGING_OFF:
 	case MAX77693_CHARGING_OVER_TEMP:
 	case MAX77693_CHARGING_WATCHDOG_EXPIRED:
-		state = POWER_SUPPLY_CHARGE_TYPE_NONE;
+		*val = POWER_SUPPLY_CHARGE_TYPE_NONE;
 		break;
 	case MAX77693_CHARGING_RESERVED:
 	default:
-		state = POWER_SUPPLY_CHARGE_TYPE_UNKNOWN;
+		*val = POWER_SUPPLY_CHARGE_TYPE_UNKNOWN;
 	}
 
-	return state;
+	return 0;
 }
 
 /*
@@ -129,69 +131,78 @@ static int max77693_get_charge_type(struct regmap *regmap)
  *  - POWER_SUPPLY_HEALTH_UNKNOWN
  *  - POWER_SUPPLY_HEALTH_UNSPEC_FAILURE
  */
-static int max77693_get_battery_health(struct regmap *regmap)
+static int max77693_get_battery_health(struct regmap *regmap, int *val)
 {
-	int state;
+	int ret;
 	unsigned int data;
 
-	if (regmap_read(regmap, MAX77693_CHG_REG_CHG_DETAILS_01, &data) < 0)
-		return POWER_SUPPLY_HEALTH_UNKNOWN;
+	ret = regmap_read(regmap, MAX77693_CHG_REG_CHG_DETAILS_01, &data);
+	if (ret < 0)
+		return ret;
 
 	data &= CHG_DETAILS_01_BAT_MASK;
 	data >>= CHG_DETAILS_01_BAT_SHIFT;
 
 	switch (data) {
 	case MAX77693_BATTERY_NOBAT:
-		state = POWER_SUPPLY_HEALTH_DEAD;
+		*val = POWER_SUPPLY_HEALTH_DEAD;
 		break;
 	case MAX77693_BATTERY_PREQUALIFICATION:
 	case MAX77693_BATTERY_GOOD:
 	case MAX77693_BATTERY_LOWVOLTAGE:
-		state = POWER_SUPPLY_HEALTH_GOOD;
+		*val = POWER_SUPPLY_HEALTH_GOOD;
 		break;
 	case MAX77693_BATTERY_TIMER_EXPIRED:
 		/*
 		 * Took longer to charge than expected, charging suspended.
 		 * Damaged battery?
 		 */
-		state = POWER_SUPPLY_HEALTH_SAFETY_TIMER_EXPIRE;
+		*val = POWER_SUPPLY_HEALTH_SAFETY_TIMER_EXPIRE;
 		break;
 	case MAX77693_BATTERY_OVERVOLTAGE:
-		state = POWER_SUPPLY_HEALTH_OVERVOLTAGE;
+		*val = POWER_SUPPLY_HEALTH_OVERVOLTAGE;
 		break;
 	case MAX77693_BATTERY_OVERCURRENT:
-		state = POWER_SUPPLY_HEALTH_UNSPEC_FAILURE;
+		*val = POWER_SUPPLY_HEALTH_UNSPEC_FAILURE;
 		break;
 	case MAX77693_BATTERY_RESERVED:
 	default:
-		state = POWER_SUPPLY_HEALTH_UNKNOWN;
+		*val = POWER_SUPPLY_HEALTH_UNKNOWN;
 		break;
 	}
 
-	return state;
+	return 0;
 }
 
-static int max77693_get_present(struct regmap *regmap)
+static int max77693_get_present(struct regmap *regmap, int *val)
 {
 	unsigned int data;
+	int ret;
 
 	/*
 	 * Read CHG_INT_OK register. High DETBAT bit here should be
 	 * equal to value 0x0 in CHG_DETAILS_01/BAT field.
 	 */
-	regmap_read(regmap, MAX77693_CHG_REG_CHG_INT_OK, &data);
-	if (data & CHG_INT_OK_DETBAT_MASK)
-		return 0;
-	return 1;
+	ret = regmap_read(regmap, MAX77693_CHG_REG_CHG_INT_OK, &data);
+	if (ret < 0)
+		return ret;
+
+	*val = (data & CHG_INT_OK_DETBAT_MASK) ? 0 : 1;
+
+	return 0;
 }
 
-static int max77693_get_online(struct regmap *regmap)
+static int max77693_get_online(struct regmap *regmap, int *val)
 {
 	unsigned int data;
+	int ret;
+
+	ret = regmap_read(regmap, MAX77693_CHG_REG_CHG_INT_OK, &data);
+	if (ret < 0)
+		return ret;
+
+	*val = (data & CHG_INT_OK_CHGIN_MASK) ? 1 : 0;
 
-	regmap_read(regmap, MAX77693_CHG_REG_CHG_INT_OK, &data);
-	if (data & CHG_INT_OK_CHGIN_MASK)
-		return 1;
 	return 0;
 }
 
@@ -209,27 +220,25 @@ static int max77693_charger_get_property(struct power_supply *psy,
 			    enum power_supply_property psp,
 			    union power_supply_propval *val)
 {
-	struct max77693_charger *chg = container_of(psy,
-						  struct max77693_charger,
-						  charger);
+	struct max77693_charger *chg = power_supply_get_drvdata(psy);
 	struct regmap *regmap = chg->max77693->regmap;
 	int ret = 0;
 
 	switch (psp) {
 	case POWER_SUPPLY_PROP_STATUS:
-		val->intval = max77693_get_charger_state(regmap);
+		ret = max77693_get_charger_state(regmap, &val->intval);
 		break;
 	case POWER_SUPPLY_PROP_CHARGE_TYPE:
-		val->intval = max77693_get_charge_type(regmap);
+		ret = max77693_get_charge_type(regmap, &val->intval);
 		break;
 	case POWER_SUPPLY_PROP_HEALTH:
-		val->intval = max77693_get_battery_health(regmap);
+		ret = max77693_get_battery_health(regmap, &val->intval);
 		break;
 	case POWER_SUPPLY_PROP_PRESENT:
-		val->intval = max77693_get_present(regmap);
+		ret = max77693_get_present(regmap, &val->intval);
 		break;
 	case POWER_SUPPLY_PROP_ONLINE:
-		val->intval = max77693_get_online(regmap);
+		ret = max77693_get_online(regmap, &val->intval);
 		break;
 	case POWER_SUPPLY_PROP_MODEL_NAME:
 		val->strval = max77693_charger_model;
@@ -244,6 +253,14 @@ static int max77693_charger_get_property(struct power_supply *psy,
 	return ret;
 }
 
+static const struct power_supply_desc max77693_charger_desc = {
+	.name		= MAX77693_CHARGER_NAME,
+	.type		= POWER_SUPPLY_TYPE_BATTERY,
+	.properties	= max77693_charger_props,
+	.num_properties	= ARRAY_SIZE(max77693_charger_props),
+	.get_property	= max77693_charger_get_property,
+};
+
 static ssize_t device_attr_store(struct device *dev,
 		struct device_attribute *attr, const char *buf, size_t count,
 		int (*fn)(struct max77693_charger *, unsigned long))
@@ -659,6 +676,7 @@ static int max77693_dt_init(struct device *dev, struct max77693_charger *chg)
 static int max77693_charger_probe(struct platform_device *pdev)
 {
 	struct max77693_charger *chg;
+	struct power_supply_config psy_cfg = {};
 	struct max77693_dev *max77693 = dev_get_drvdata(pdev->dev.parent);
 	int ret;
 
@@ -678,11 +696,7 @@ static int max77693_charger_probe(struct platform_device *pdev)
 	if (ret)
 		return ret;
 
-	chg->charger.name = max77693_charger_name;
-	chg->charger.type = POWER_SUPPLY_TYPE_BATTERY;
-	chg->charger.properties = max77693_charger_props;
-	chg->charger.num_properties = ARRAY_SIZE(max77693_charger_props);
-	chg->charger.get_property = max77693_charger_get_property;
+	psy_cfg.drv_data = chg;
 
 	ret = device_create_file(&pdev->dev, &dev_attr_fast_charge_timer);
 	if (ret) {
@@ -703,9 +717,12 @@ static int max77693_charger_probe(struct platform_device *pdev)
 		goto err;
 	}
 
-	ret = power_supply_register(&pdev->dev, &chg->charger);
-	if (ret) {
+	chg->charger = power_supply_register(&pdev->dev,
+						&max77693_charger_desc,
+						&psy_cfg);
+	if (IS_ERR(chg->charger)) {
 		dev_err(&pdev->dev, "failed: power supply register\n");
+		ret = PTR_ERR(chg->charger);
 		goto err;
 	}
 
@@ -727,7 +744,7 @@ static int max77693_charger_remove(struct platform_device *pdev)
 	device_remove_file(&pdev->dev, &dev_attr_top_off_threshold_current);
 	device_remove_file(&pdev->dev, &dev_attr_fast_charge_timer);
 
-	power_supply_unregister(&chg->charger);
+	power_supply_unregister(chg->charger);
 
 	return 0;
 }
diff --git a/drivers/power/max8903_charger.c b/drivers/power/max8903_charger.c
index 99e3cdcd3e11..bf2b4b3a7cae 100644
--- a/drivers/power/max8903_charger.c
+++ b/drivers/power/max8903_charger.c
@@ -31,7 +31,8 @@
 struct max8903_data {
 	struct max8903_pdata pdata;
 	struct device *dev;
-	struct power_supply psy;
+	struct power_supply *psy;
+	struct power_supply_desc psy_desc;
 	bool fault;
 	bool usb_in;
 	bool ta_in;
@@ -47,8 +48,7 @@ static int max8903_get_property(struct power_supply *psy,
 		enum power_supply_property psp,
 		union power_supply_propval *val)
 {
-	struct max8903_data *data = container_of(psy,
-			struct max8903_data, psy);
+	struct max8903_data *data = power_supply_get_drvdata(psy);
 
 	switch (psp) {
 	case POWER_SUPPLY_PROP_STATUS:
@@ -104,17 +104,17 @@ static irqreturn_t max8903_dcin(int irq, void *_data)
 	dev_dbg(data->dev, "TA(DC-IN) Charger %s.\n", ta_in ?
 			"Connected" : "Disconnected");
 
-	old_type = data->psy.type;
+	old_type = data->psy_desc.type;
 
 	if (data->ta_in)
-		data->psy.type = POWER_SUPPLY_TYPE_MAINS;
+		data->psy_desc.type = POWER_SUPPLY_TYPE_MAINS;
 	else if (data->usb_in)
-		data->psy.type = POWER_SUPPLY_TYPE_USB;
+		data->psy_desc.type = POWER_SUPPLY_TYPE_USB;
 	else
-		data->psy.type = POWER_SUPPLY_TYPE_BATTERY;
+		data->psy_desc.type = POWER_SUPPLY_TYPE_BATTERY;
 
-	if (old_type != data->psy.type)
-		power_supply_changed(&data->psy);
+	if (old_type != data->psy_desc.type)
+		power_supply_changed(data->psy);
 
 	return IRQ_HANDLED;
 }
@@ -143,17 +143,17 @@ static irqreturn_t max8903_usbin(int irq, void *_data)
 	dev_dbg(data->dev, "USB Charger %s.\n", usb_in ?
 			"Connected" : "Disconnected");
 
-	old_type = data->psy.type;
+	old_type = data->psy_desc.type;
 
 	if (data->ta_in)
-		data->psy.type = POWER_SUPPLY_TYPE_MAINS;
+		data->psy_desc.type = POWER_SUPPLY_TYPE_MAINS;
 	else if (data->usb_in)
-		data->psy.type = POWER_SUPPLY_TYPE_USB;
+		data->psy_desc.type = POWER_SUPPLY_TYPE_USB;
 	else
-		data->psy.type = POWER_SUPPLY_TYPE_BATTERY;
+		data->psy_desc.type = POWER_SUPPLY_TYPE_BATTERY;
 
-	if (old_type != data->psy.type)
-		power_supply_changed(&data->psy);
+	if (old_type != data->psy_desc.type)
+		power_supply_changed(data->psy);
 
 	return IRQ_HANDLED;
 }
@@ -184,6 +184,7 @@ static int max8903_probe(struct platform_device *pdev)
 	struct max8903_data *data;
 	struct device *dev = &pdev->dev;
 	struct max8903_pdata *pdata = pdev->dev.platform_data;
+	struct power_supply_config psy_cfg = {};
 	int ret = 0;
 	int gpio;
 	int ta_in = 0;
@@ -280,17 +281,20 @@ static int max8903_probe(struct platform_device *pdev)
 	data->ta_in = ta_in;
 	data->usb_in = usb_in;
 
-	data->psy.name = "max8903_charger";
-	data->psy.type = (ta_in) ? POWER_SUPPLY_TYPE_MAINS :
+	data->psy_desc.name = "max8903_charger";
+	data->psy_desc.type = (ta_in) ? POWER_SUPPLY_TYPE_MAINS :
 			((usb_in) ? POWER_SUPPLY_TYPE_USB :
 			 POWER_SUPPLY_TYPE_BATTERY);
-	data->psy.get_property = max8903_get_property;
-	data->psy.properties = max8903_charger_props;
-	data->psy.num_properties = ARRAY_SIZE(max8903_charger_props);
+	data->psy_desc.get_property = max8903_get_property;
+	data->psy_desc.properties = max8903_charger_props;
+	data->psy_desc.num_properties = ARRAY_SIZE(max8903_charger_props);
 
-	ret = power_supply_register(dev, &data->psy);
-	if (ret) {
+	psy_cfg.drv_data = data;
+
+	data->psy = power_supply_register(dev, &data->psy_desc, &psy_cfg);
+	if (IS_ERR(data->psy)) {
 		dev_err(dev, "failed: power supply register.\n");
+		ret = PTR_ERR(data->psy);
 		goto err;
 	}
 
@@ -339,7 +343,7 @@ err_dc_irq:
 	if (pdata->dc_valid)
 		free_irq(gpio_to_irq(pdata->dok), data);
 err_psy:
-	power_supply_unregister(&data->psy);
+	power_supply_unregister(data->psy);
 err:
 	return ret;
 }
@@ -357,7 +361,7 @@ static int max8903_remove(struct platform_device *pdev)
 			free_irq(gpio_to_irq(pdata->uok), data);
 		if (pdata->dc_valid)
 			free_irq(gpio_to_irq(pdata->dok), data);
-		power_supply_unregister(&data->psy);
+		power_supply_unregister(data->psy);
 	}
 
 	return 0;
diff --git a/drivers/power/max8925_power.c b/drivers/power/max8925_power.c
index a6d45eef64dd..57eb5c2bfc21 100644
--- a/drivers/power/max8925_power.c
+++ b/drivers/power/max8925_power.c
@@ -68,9 +68,9 @@ struct max8925_power_info {
 	struct i2c_client	*gpm;
 	struct i2c_client	*adc;
 
-	struct power_supply	ac;
-	struct power_supply	usb;
-	struct power_supply	battery;
+	struct power_supply	*ac;
+	struct power_supply	*usb;
+	struct power_supply	*battery;
 	int			irq_base;
 	unsigned		ac_online:1;
 	unsigned		usb_online:1;
@@ -196,7 +196,7 @@ static int max8925_ac_get_prop(struct power_supply *psy,
 			       enum power_supply_property psp,
 			       union power_supply_propval *val)
 {
-	struct max8925_power_info *info = dev_get_drvdata(psy->dev->parent);
+	struct max8925_power_info *info = dev_get_drvdata(psy->dev.parent);
 	int ret = 0;
 
 	switch (psp) {
@@ -230,7 +230,7 @@ static int max8925_usb_get_prop(struct power_supply *psy,
 				enum power_supply_property psp,
 				union power_supply_propval *val)
 {
-	struct max8925_power_info *info = dev_get_drvdata(psy->dev->parent);
+	struct max8925_power_info *info = dev_get_drvdata(psy->dev.parent);
 	int ret = 0;
 
 	switch (psp) {
@@ -264,7 +264,7 @@ static int max8925_bat_get_prop(struct power_supply *psy,
 				enum power_supply_property psp,
 				union power_supply_propval *val)
 {
-	struct max8925_power_info *info = dev_get_drvdata(psy->dev->parent);
+	struct max8925_power_info *info = dev_get_drvdata(psy->dev.parent);
 	int ret = 0;
 
 	switch (psp) {
@@ -347,6 +347,30 @@ static enum power_supply_property max8925_battery_props[] = {
 	POWER_SUPPLY_PROP_STATUS,
 };
 
+static const struct power_supply_desc ac_desc = {
+	.name		= "max8925-ac",
+	.type		= POWER_SUPPLY_TYPE_MAINS,
+	.properties	= max8925_ac_props,
+	.num_properties	= ARRAY_SIZE(max8925_ac_props),
+	.get_property	= max8925_ac_get_prop,
+};
+
+static const struct power_supply_desc usb_desc = {
+	.name		= "max8925-usb",
+	.type		= POWER_SUPPLY_TYPE_USB,
+	.properties	= max8925_usb_props,
+	.num_properties	= ARRAY_SIZE(max8925_usb_props),
+	.get_property	= max8925_usb_get_prop,
+};
+
+static const struct power_supply_desc battery_desc = {
+	.name		= "max8925-battery",
+	.type		= POWER_SUPPLY_TYPE_BATTERY,
+	.properties	= max8925_battery_props,
+	.num_properties	= ARRAY_SIZE(max8925_battery_props),
+	.get_property	= max8925_bat_get_prop,
+};
+
 #define REQUEST_IRQ(_irq, _name)					\
 do {									\
 	ret = request_threaded_irq(chip->irq_base + _irq, NULL,		\
@@ -482,6 +506,7 @@ max8925_power_dt_init(struct platform_device *pdev)
 static int max8925_power_probe(struct platform_device *pdev)
 {
 	struct max8925_chip *chip = dev_get_drvdata(pdev->dev.parent);
+	struct power_supply_config psy_cfg = {}; /* Only for ac and usb */
 	struct max8925_power_pdata *pdata = NULL;
 	struct max8925_power_info *info;
 	int ret;
@@ -502,40 +527,29 @@ static int max8925_power_probe(struct platform_device *pdev)
 	info->adc = chip->adc;
 	platform_set_drvdata(pdev, info);
 
-	info->ac.name = "max8925-ac";
-	info->ac.type = POWER_SUPPLY_TYPE_MAINS;
-	info->ac.properties = max8925_ac_props;
-	info->ac.num_properties = ARRAY_SIZE(max8925_ac_props);
-	info->ac.get_property = max8925_ac_get_prop;
-	info->ac.supplied_to = pdata->supplied_to;
-	info->ac.num_supplicants = pdata->num_supplicants;
-	ret = power_supply_register(&pdev->dev, &info->ac);
-	if (ret)
+	psy_cfg.supplied_to = pdata->supplied_to;
+	psy_cfg.num_supplicants = pdata->num_supplicants;
+
+	info->ac = power_supply_register(&pdev->dev, &ac_desc, &psy_cfg);
+	if (IS_ERR(info->ac)) {
+		ret = PTR_ERR(info->ac);
 		goto out;
-	info->ac.dev->parent = &pdev->dev;
-
-	info->usb.name = "max8925-usb";
-	info->usb.type = POWER_SUPPLY_TYPE_USB;
-	info->usb.properties = max8925_usb_props;
-	info->usb.num_properties = ARRAY_SIZE(max8925_usb_props);
-	info->usb.get_property = max8925_usb_get_prop;
-	info->usb.supplied_to = pdata->supplied_to;
-	info->usb.num_supplicants = pdata->num_supplicants;
-
-	ret = power_supply_register(&pdev->dev, &info->usb);
-	if (ret)
+	}
+	info->ac->dev.parent = &pdev->dev;
+
+	info->usb = power_supply_register(&pdev->dev, &usb_desc, &psy_cfg);
+	if (IS_ERR(info->usb)) {
+		ret = PTR_ERR(info->usb);
 		goto out_usb;
-	info->usb.dev->parent = &pdev->dev;
-
-	info->battery.name = "max8925-battery";
-	info->battery.type = POWER_SUPPLY_TYPE_BATTERY;
-	info->battery.properties = max8925_battery_props;
-	info->battery.num_properties = ARRAY_SIZE(max8925_battery_props);
-	info->battery.get_property = max8925_bat_get_prop;
-	ret = power_supply_register(&pdev->dev, &info->battery);
-	if (ret)
+	}
+	info->usb->dev.parent = &pdev->dev;
+
+	info->battery = power_supply_register(&pdev->dev, &battery_desc, NULL);
+	if (IS_ERR(info->battery)) {
+		ret = PTR_ERR(info->battery);
 		goto out_battery;
-	info->battery.dev->parent = &pdev->dev;
+	}
+	info->battery->dev.parent = &pdev->dev;
 
 	info->batt_detect = pdata->batt_detect;
 	info->topoff_threshold = pdata->topoff_threshold;
@@ -547,9 +561,9 @@ static int max8925_power_probe(struct platform_device *pdev)
 	max8925_init_charger(chip, info);
 	return 0;
 out_battery:
-	power_supply_unregister(&info->battery);
+	power_supply_unregister(info->battery);
 out_usb:
-	power_supply_unregister(&info->ac);
+	power_supply_unregister(info->ac);
 out:
 	return ret;
 }
@@ -559,9 +573,9 @@ static int max8925_power_remove(struct platform_device *pdev)
 	struct max8925_power_info *info = platform_get_drvdata(pdev);
 
 	if (info) {
-		power_supply_unregister(&info->ac);
-		power_supply_unregister(&info->usb);
-		power_supply_unregister(&info->battery);
+		power_supply_unregister(info->ac);
+		power_supply_unregister(info->usb);
+		power_supply_unregister(info->battery);
 		max8925_deinit_charger(info);
 	}
 	return 0;
diff --git a/drivers/power/max8997_charger.c b/drivers/power/max8997_charger.c
index aefa0c9a3007..0b2eab571528 100644
--- a/drivers/power/max8997_charger.c
+++ b/drivers/power/max8997_charger.c
@@ -30,7 +30,7 @@
 struct charger_data {
 	struct device *dev;
 	struct max8997_dev *iodev;
-	struct power_supply battery;
+	struct power_supply *battery;
 };
 
 static enum power_supply_property max8997_battery_props[] = {
@@ -44,8 +44,7 @@ static int max8997_battery_get_property(struct power_supply *psy,
 		enum power_supply_property psp,
 		union power_supply_propval *val)
 {
-	struct charger_data *charger = container_of(psy,
-			struct charger_data, battery);
+	struct charger_data *charger = power_supply_get_drvdata(psy);
 	struct i2c_client *i2c = charger->iodev->i2c;
 	int ret;
 	u8 reg;
@@ -86,12 +85,21 @@ static int max8997_battery_get_property(struct power_supply *psy,
 	return 0;
 }
 
+static const struct power_supply_desc max8997_battery_desc = {
+	.name		= "max8997_pmic",
+	.type		= POWER_SUPPLY_TYPE_BATTERY,
+	.get_property	= max8997_battery_get_property,
+	.properties	= max8997_battery_props,
+	.num_properties	= ARRAY_SIZE(max8997_battery_props),
+};
+
 static int max8997_battery_probe(struct platform_device *pdev)
 {
 	int ret = 0;
 	struct charger_data *charger;
 	struct max8997_dev *iodev = dev_get_drvdata(pdev->dev.parent);
 	struct max8997_platform_data *pdata = dev_get_platdata(iodev->dev);
+	struct power_supply_config psy_cfg = {};
 
 	if (!pdata)
 		return -EINVAL;
@@ -147,19 +155,18 @@ static int max8997_battery_probe(struct platform_device *pdev)
 
 	platform_set_drvdata(pdev, charger);
 
-	charger->battery.name = "max8997_pmic";
-	charger->battery.type = POWER_SUPPLY_TYPE_BATTERY;
-	charger->battery.get_property = max8997_battery_get_property;
-	charger->battery.properties = max8997_battery_props;
-	charger->battery.num_properties = ARRAY_SIZE(max8997_battery_props);
 
 	charger->dev = &pdev->dev;
 	charger->iodev = iodev;
 
-	ret = power_supply_register(&pdev->dev, &charger->battery);
-	if (ret) {
+	psy_cfg.drv_data = charger;
+
+	charger->battery = power_supply_register(&pdev->dev,
+						 &max8997_battery_desc,
+						 &psy_cfg);
+	if (IS_ERR(charger->battery)) {
 		dev_err(&pdev->dev, "failed: power supply register\n");
-		return ret;
+		return PTR_ERR(charger->battery);
 	}
 
 	return 0;
@@ -169,7 +176,7 @@ static int max8997_battery_remove(struct platform_device *pdev)
 {
 	struct charger_data *charger = platform_get_drvdata(pdev);
 
-	power_supply_unregister(&charger->battery);
+	power_supply_unregister(charger->battery);
 	return 0;
 }
 
diff --git a/drivers/power/max8998_charger.c b/drivers/power/max8998_charger.c
index 08694c7a9f38..47448d4bc6cd 100644
--- a/drivers/power/max8998_charger.c
+++ b/drivers/power/max8998_charger.c
@@ -30,7 +30,7 @@
 struct max8998_battery_data {
 	struct device *dev;
 	struct max8998_dev *iodev;
-	struct power_supply battery;
+	struct power_supply *battery;
 };
 
 static enum power_supply_property max8998_battery_props[] = {
@@ -43,8 +43,7 @@ static int max8998_battery_get_property(struct power_supply *psy,
 		enum power_supply_property psp,
 		union power_supply_propval *val)
 {
-	struct max8998_battery_data *max8998 = container_of(psy,
-			struct max8998_battery_data, battery);
+	struct max8998_battery_data *max8998 = power_supply_get_drvdata(psy);
 	struct i2c_client *i2c = max8998->iodev->i2c;
 	int ret;
 	u8 reg;
@@ -75,10 +74,19 @@ static int max8998_battery_get_property(struct power_supply *psy,
 	return 0;
 }
 
+static const struct power_supply_desc max8998_battery_desc = {
+	.name		= "max8998_pmic",
+	.type		= POWER_SUPPLY_TYPE_BATTERY,
+	.get_property	= max8998_battery_get_property,
+	.properties	= max8998_battery_props,
+	.num_properties	= ARRAY_SIZE(max8998_battery_props),
+};
+
 static int max8998_battery_probe(struct platform_device *pdev)
 {
 	struct max8998_dev *iodev = dev_get_drvdata(pdev->dev.parent);
 	struct max8998_platform_data *pdata = dev_get_platdata(iodev->dev);
+	struct power_supply_config psy_cfg = {};
 	struct max8998_battery_data *max8998;
 	struct i2c_client *i2c;
 	int ret = 0;
@@ -161,15 +169,15 @@ static int max8998_battery_probe(struct platform_device *pdev)
 		goto err;
 	}
 
-	max8998->battery.name = "max8998_pmic";
-	max8998->battery.type = POWER_SUPPLY_TYPE_BATTERY;
-	max8998->battery.get_property = max8998_battery_get_property;
-	max8998->battery.properties = max8998_battery_props;
-	max8998->battery.num_properties = ARRAY_SIZE(max8998_battery_props);
+	psy_cfg.drv_data = max8998;
 
-	ret = power_supply_register(max8998->dev, &max8998->battery);
-	if (ret) {
-		dev_err(max8998->dev, "failed: power supply register\n");
+	max8998->battery = power_supply_register(max8998->dev,
+						 &max8998_battery_desc,
+						 &psy_cfg);
+	if (IS_ERR(max8998->battery)) {
+		ret = PTR_ERR(max8998->battery);
+		dev_err(max8998->dev, "failed: power supply register: %d\n",
+			ret);
 		goto err;
 	}
 
@@ -182,7 +190,7 @@ static int max8998_battery_remove(struct platform_device *pdev)
 {
 	struct max8998_battery_data *max8998 = platform_get_drvdata(pdev);
 
-	power_supply_unregister(&max8998->battery);
+	power_supply_unregister(max8998->battery);
 
 	return 0;
 }
diff --git a/drivers/power/olpc_battery.c b/drivers/power/olpc_battery.c
index ad9cde705de1..a944338a39de 100644
--- a/drivers/power/olpc_battery.c
+++ b/drivers/power/olpc_battery.c
@@ -81,7 +81,7 @@ static enum power_supply_property olpc_ac_props[] = {
 	POWER_SUPPLY_PROP_ONLINE,
 };
 
-static struct power_supply olpc_ac = {
+static const struct power_supply_desc olpc_ac_desc = {
 	.name = "olpc-ac",
 	.type = POWER_SUPPLY_TYPE_MAINS,
 	.properties = olpc_ac_props,
@@ -89,6 +89,8 @@ static struct power_supply olpc_ac = {
 	.get_property = olpc_ac_get_prop,
 };
 
+static struct power_supply *olpc_ac;
+
 static char bat_serial[17]; /* Ick */
 
 static int olpc_bat_get_status(union power_supply_propval *val, uint8_t ec_byte)
@@ -574,21 +576,23 @@ static struct device_attribute olpc_bat_error = {
  *		Initialisation
  *********************************************************************/
 
-static struct power_supply olpc_bat = {
+static struct power_supply_desc olpc_bat_desc = {
 	.name = "olpc-battery",
 	.get_property = olpc_bat_get_property,
 	.use_for_apm = 1,
 };
 
+static struct power_supply *olpc_bat;
+
 static int olpc_battery_suspend(struct platform_device *pdev,
 				pm_message_t state)
 {
-	if (device_may_wakeup(olpc_ac.dev))
+	if (device_may_wakeup(&olpc_ac->dev))
 		olpc_ec_wakeup_set(EC_SCI_SRC_ACPWR);
 	else
 		olpc_ec_wakeup_clear(EC_SCI_SRC_ACPWR);
 
-	if (device_may_wakeup(olpc_bat.dev))
+	if (device_may_wakeup(&olpc_bat->dev))
 		olpc_ec_wakeup_set(EC_SCI_SRC_BATTERY | EC_SCI_SRC_BATSOC
 				   | EC_SCI_SRC_BATERR);
 	else
@@ -619,52 +623,54 @@ static int olpc_battery_probe(struct platform_device *pdev)
 
 	/* Ignore the status. It doesn't actually matter */
 
-	ret = power_supply_register(&pdev->dev, &olpc_ac);
-	if (ret)
-		return ret;
+	olpc_ac = power_supply_register(&pdev->dev, &olpc_ac_desc, NULL);
+	if (IS_ERR(olpc_ac))
+		return PTR_ERR(olpc_ac);
 
 	if (olpc_board_at_least(olpc_board_pre(0xd0))) { /* XO-1.5 */
-		olpc_bat.properties = olpc_xo15_bat_props;
-		olpc_bat.num_properties = ARRAY_SIZE(olpc_xo15_bat_props);
+		olpc_bat_desc.properties = olpc_xo15_bat_props;
+		olpc_bat_desc.num_properties = ARRAY_SIZE(olpc_xo15_bat_props);
 	} else { /* XO-1 */
-		olpc_bat.properties = olpc_xo1_bat_props;
-		olpc_bat.num_properties = ARRAY_SIZE(olpc_xo1_bat_props);
+		olpc_bat_desc.properties = olpc_xo1_bat_props;
+		olpc_bat_desc.num_properties = ARRAY_SIZE(olpc_xo1_bat_props);
 	}
 
-	ret = power_supply_register(&pdev->dev, &olpc_bat);
-	if (ret)
+	olpc_bat = power_supply_register(&pdev->dev, &olpc_bat_desc, NULL);
+	if (IS_ERR(olpc_bat)) {
+		ret = PTR_ERR(olpc_bat);
 		goto battery_failed;
+	}
 
-	ret = device_create_bin_file(olpc_bat.dev, &olpc_bat_eeprom);
+	ret = device_create_bin_file(&olpc_bat->dev, &olpc_bat_eeprom);
 	if (ret)
 		goto eeprom_failed;
 
-	ret = device_create_file(olpc_bat.dev, &olpc_bat_error);
+	ret = device_create_file(&olpc_bat->dev, &olpc_bat_error);
 	if (ret)
 		goto error_failed;
 
 	if (olpc_ec_wakeup_available()) {
-		device_set_wakeup_capable(olpc_ac.dev, true);
-		device_set_wakeup_capable(olpc_bat.dev, true);
+		device_set_wakeup_capable(&olpc_ac->dev, true);
+		device_set_wakeup_capable(&olpc_bat->dev, true);
 	}
 
 	return 0;
 
 error_failed:
-	device_remove_bin_file(olpc_bat.dev, &olpc_bat_eeprom);
+	device_remove_bin_file(&olpc_bat->dev, &olpc_bat_eeprom);
 eeprom_failed:
-	power_supply_unregister(&olpc_bat);
+	power_supply_unregister(olpc_bat);
 battery_failed:
-	power_supply_unregister(&olpc_ac);
+	power_supply_unregister(olpc_ac);
 	return ret;
 }
 
 static int olpc_battery_remove(struct platform_device *pdev)
 {
-	device_remove_file(olpc_bat.dev, &olpc_bat_error);
-	device_remove_bin_file(olpc_bat.dev, &olpc_bat_eeprom);
-	power_supply_unregister(&olpc_bat);
-	power_supply_unregister(&olpc_ac);
+	device_remove_file(&olpc_bat->dev, &olpc_bat_error);
+	device_remove_bin_file(&olpc_bat->dev, &olpc_bat_eeprom);
+	power_supply_unregister(olpc_bat);
+	power_supply_unregister(olpc_ac);
 	return 0;
 }
 
diff --git a/drivers/power/pcf50633-charger.c b/drivers/power/pcf50633-charger.c
index 771c4f0fb8ac..d05597b4e40f 100644
--- a/drivers/power/pcf50633-charger.c
+++ b/drivers/power/pcf50633-charger.c
@@ -33,9 +33,9 @@ struct pcf50633_mbc {
 	int adapter_online;
 	int usb_online;
 
-	struct power_supply usb;
-	struct power_supply adapter;
-	struct power_supply ac;
+	struct power_supply *usb;
+	struct power_supply *adapter;
+	struct power_supply *ac;
 };
 
 int pcf50633_mbc_usb_curlim_set(struct pcf50633 *pcf, int ma)
@@ -104,7 +104,7 @@ int pcf50633_mbc_usb_curlim_set(struct pcf50633 *pcf, int ma)
 				PCF50633_MBCC1_CHGENA, PCF50633_MBCC1_CHGENA);
 	}
 
-	power_supply_changed(&mbc->usb);
+	power_supply_changed(mbc->usb);
 
 	return ret;
 }
@@ -278,9 +278,9 @@ pcf50633_mbc_irq_handler(int irq, void *data)
 	else if (irq == PCF50633_IRQ_ADPREM)
 		mbc->adapter_online = 0;
 
-	power_supply_changed(&mbc->ac);
-	power_supply_changed(&mbc->usb);
-	power_supply_changed(&mbc->adapter);
+	power_supply_changed(mbc->ac);
+	power_supply_changed(mbc->usb);
+	power_supply_changed(mbc->adapter);
 
 	if (mbc->pcf->pdata->mbc_event_callback)
 		mbc->pcf->pdata->mbc_event_callback(mbc->pcf, irq);
@@ -290,8 +290,7 @@ static int adapter_get_property(struct power_supply *psy,
 			enum power_supply_property psp,
 			union power_supply_propval *val)
 {
-	struct pcf50633_mbc *mbc = container_of(psy,
-				struct pcf50633_mbc, adapter);
+	struct pcf50633_mbc *mbc = power_supply_get_drvdata(psy);
 	int ret = 0;
 
 	switch (psp) {
@@ -309,7 +308,7 @@ static int usb_get_property(struct power_supply *psy,
 			enum power_supply_property psp,
 			union power_supply_propval *val)
 {
-	struct pcf50633_mbc *mbc = container_of(psy, struct pcf50633_mbc, usb);
+	struct pcf50633_mbc *mbc = power_supply_get_drvdata(psy);
 	int ret = 0;
 	u8 usblim = pcf50633_reg_read(mbc->pcf, PCF50633_REG_MBCC7) &
 						PCF50633_MBCC7_USB_MASK;
@@ -330,7 +329,7 @@ static int ac_get_property(struct power_supply *psy,
 			enum power_supply_property psp,
 			union power_supply_propval *val)
 {
-	struct pcf50633_mbc *mbc = container_of(psy, struct pcf50633_mbc, ac);
+	struct pcf50633_mbc *mbc = power_supply_get_drvdata(psy);
 	int ret = 0;
 	u8 usblim = pcf50633_reg_read(mbc->pcf, PCF50633_REG_MBCC7) &
 						PCF50633_MBCC7_USB_MASK;
@@ -366,8 +365,33 @@ static const u8 mbc_irq_handlers[] = {
 	PCF50633_IRQ_LOWBAT,
 };
 
+static const struct power_supply_desc pcf50633_mbc_adapter_desc = {
+	.name		= "adapter",
+	.type		= POWER_SUPPLY_TYPE_MAINS,
+	.properties	= power_props,
+	.num_properties	= ARRAY_SIZE(power_props),
+	.get_property	= &adapter_get_property,
+};
+
+static const struct power_supply_desc pcf50633_mbc_usb_desc = {
+	.name		= "usb",
+	.type		= POWER_SUPPLY_TYPE_USB,
+	.properties	= power_props,
+	.num_properties	= ARRAY_SIZE(power_props),
+	.get_property	= usb_get_property,
+};
+
+static const struct power_supply_desc pcf50633_mbc_ac_desc = {
+	.name		= "ac",
+	.type		= POWER_SUPPLY_TYPE_MAINS,
+	.properties	= power_props,
+	.num_properties	= ARRAY_SIZE(power_props),
+	.get_property	= ac_get_property,
+};
+
 static int pcf50633_mbc_probe(struct platform_device *pdev)
 {
+	struct power_supply_config psy_cfg = {};
 	struct pcf50633_mbc *mbc;
 	int ret;
 	int i;
@@ -385,49 +409,36 @@ static int pcf50633_mbc_probe(struct platform_device *pdev)
 		pcf50633_register_irq(mbc->pcf, mbc_irq_handlers[i],
 					pcf50633_mbc_irq_handler, mbc);
 
+	psy_cfg.supplied_to		= mbc->pcf->pdata->batteries;
+	psy_cfg.num_supplicants		= mbc->pcf->pdata->num_batteries;
+	psy_cfg.drv_data		= mbc;
+
 	/* Create power supplies */
-	mbc->adapter.name		= "adapter";
-	mbc->adapter.type		= POWER_SUPPLY_TYPE_MAINS;
-	mbc->adapter.properties		= power_props;
-	mbc->adapter.num_properties	= ARRAY_SIZE(power_props);
-	mbc->adapter.get_property	= &adapter_get_property;
-	mbc->adapter.supplied_to	= mbc->pcf->pdata->batteries;
-	mbc->adapter.num_supplicants	= mbc->pcf->pdata->num_batteries;
-
-	mbc->usb.name			= "usb";
-	mbc->usb.type			= POWER_SUPPLY_TYPE_USB;
-	mbc->usb.properties		= power_props;
-	mbc->usb.num_properties		= ARRAY_SIZE(power_props);
-	mbc->usb.get_property		= usb_get_property;
-	mbc->usb.supplied_to		= mbc->pcf->pdata->batteries;
-	mbc->usb.num_supplicants	= mbc->pcf->pdata->num_batteries;
-
-	mbc->ac.name			= "ac";
-	mbc->ac.type			= POWER_SUPPLY_TYPE_MAINS;
-	mbc->ac.properties		= power_props;
-	mbc->ac.num_properties		= ARRAY_SIZE(power_props);
-	mbc->ac.get_property		= ac_get_property;
-	mbc->ac.supplied_to		= mbc->pcf->pdata->batteries;
-	mbc->ac.num_supplicants		= mbc->pcf->pdata->num_batteries;
-
-	ret = power_supply_register(&pdev->dev, &mbc->adapter);
-	if (ret) {
+	mbc->adapter = power_supply_register(&pdev->dev,
+					     &pcf50633_mbc_adapter_desc,
+					     &psy_cfg);
+	if (IS_ERR(mbc->adapter)) {
 		dev_err(mbc->pcf->dev, "failed to register adapter\n");
+		ret = PTR_ERR(mbc->adapter);
 		return ret;
 	}
 
-	ret = power_supply_register(&pdev->dev, &mbc->usb);
-	if (ret) {
+	mbc->usb = power_supply_register(&pdev->dev, &pcf50633_mbc_usb_desc,
+					 &psy_cfg);
+	if (IS_ERR(mbc->usb)) {
 		dev_err(mbc->pcf->dev, "failed to register usb\n");
-		power_supply_unregister(&mbc->adapter);
+		power_supply_unregister(mbc->adapter);
+		ret = PTR_ERR(mbc->usb);
 		return ret;
 	}
 
-	ret = power_supply_register(&pdev->dev, &mbc->ac);
-	if (ret) {
+	mbc->ac = power_supply_register(&pdev->dev, &pcf50633_mbc_ac_desc,
+					&psy_cfg);
+	if (IS_ERR(mbc->ac)) {
 		dev_err(mbc->pcf->dev, "failed to register ac\n");
-		power_supply_unregister(&mbc->adapter);
-		power_supply_unregister(&mbc->usb);
+		power_supply_unregister(mbc->adapter);
+		power_supply_unregister(mbc->usb);
+		ret = PTR_ERR(mbc->ac);
 		return ret;
 	}
 
@@ -454,9 +465,9 @@ static int pcf50633_mbc_remove(struct platform_device *pdev)
 		pcf50633_free_irq(mbc->pcf, mbc_irq_handlers[i]);
 
 	sysfs_remove_group(&pdev->dev.kobj, &mbc_attr_group);
-	power_supply_unregister(&mbc->usb);
-	power_supply_unregister(&mbc->adapter);
-	power_supply_unregister(&mbc->ac);
+	power_supply_unregister(mbc->usb);
+	power_supply_unregister(mbc->adapter);
+	power_supply_unregister(mbc->ac);
 
 	return 0;
 }
diff --git a/drivers/power/pda_power.c b/drivers/power/pda_power.c
index 0c52e2a0d90c..dfe1ee89f7c7 100644
--- a/drivers/power/pda_power.c
+++ b/drivers/power/pda_power.c
@@ -34,6 +34,7 @@ static struct timer_list charger_timer;
 static struct timer_list supply_timer;
 static struct timer_list polling_timer;
 static int polling;
+static struct power_supply *pda_psy_ac, *pda_psy_usb;
 
 #if IS_ENABLED(CONFIG_USB_PHY)
 static struct usb_phy *transceiver;
@@ -58,7 +59,7 @@ static int pda_power_get_property(struct power_supply *psy,
 {
 	switch (psp) {
 	case POWER_SUPPLY_PROP_ONLINE:
-		if (psy->type == POWER_SUPPLY_TYPE_MAINS)
+		if (psy->desc->type == POWER_SUPPLY_TYPE_MAINS)
 			val->intval = pdata->is_ac_online ?
 				      pdata->is_ac_online() : 0;
 		else
@@ -80,21 +81,17 @@ static char *pda_power_supplied_to[] = {
 	"backup-battery",
 };
 
-static struct power_supply pda_psy_ac = {
+static const struct power_supply_desc pda_psy_ac_desc = {
 	.name = "ac",
 	.type = POWER_SUPPLY_TYPE_MAINS,
-	.supplied_to = pda_power_supplied_to,
-	.num_supplicants = ARRAY_SIZE(pda_power_supplied_to),
 	.properties = pda_power_props,
 	.num_properties = ARRAY_SIZE(pda_power_props),
 	.get_property = pda_power_get_property,
 };
 
-static struct power_supply pda_psy_usb = {
+static const struct power_supply_desc pda_psy_usb_desc = {
 	.name = "usb",
 	.type = POWER_SUPPLY_TYPE_USB,
-	.supplied_to = pda_power_supplied_to,
-	.num_supplicants = ARRAY_SIZE(pda_power_supplied_to),
 	.properties = pda_power_props,
 	.num_properties = ARRAY_SIZE(pda_power_props),
 	.get_property = pda_power_get_property,
@@ -147,12 +144,12 @@ static void supply_timer_func(unsigned long unused)
 {
 	if (ac_status == PDA_PSY_TO_CHANGE) {
 		ac_status = new_ac_status;
-		power_supply_changed(&pda_psy_ac);
+		power_supply_changed(pda_psy_ac);
 	}
 
 	if (usb_status == PDA_PSY_TO_CHANGE) {
 		usb_status = new_usb_status;
-		power_supply_changed(&pda_psy_usb);
+		power_supply_changed(pda_psy_usb);
 	}
 }
 
@@ -176,9 +173,9 @@ static void charger_timer_func(unsigned long unused)
 
 static irqreturn_t power_changed_isr(int irq, void *power_supply)
 {
-	if (power_supply == &pda_psy_ac)
+	if (power_supply == pda_psy_ac)
 		ac_status = PDA_PSY_TO_CHANGE;
-	else if (power_supply == &pda_psy_usb)
+	else if (power_supply == pda_psy_usb)
 		usb_status = PDA_PSY_TO_CHANGE;
 	else
 		return IRQ_NONE;
@@ -262,6 +259,7 @@ static int otg_handle_notification(struct notifier_block *nb,
 
 static int pda_power_probe(struct platform_device *pdev)
 {
+	struct power_supply_config psy_cfg = {};
 	int ret = 0;
 
 	dev = &pdev->dev;
@@ -309,10 +307,11 @@ static int pda_power_probe(struct platform_device *pdev)
 	usb_irq = platform_get_resource_byname(pdev, IORESOURCE_IRQ, "usb");
 
 	if (pdata->supplied_to) {
-		pda_psy_ac.supplied_to = pdata->supplied_to;
-		pda_psy_ac.num_supplicants = pdata->num_supplicants;
-		pda_psy_usb.supplied_to = pdata->supplied_to;
-		pda_psy_usb.num_supplicants = pdata->num_supplicants;
+		psy_cfg.supplied_to = pdata->supplied_to;
+		psy_cfg.num_supplicants = pdata->num_supplicants;
+	} else {
+		psy_cfg.supplied_to = pda_power_supplied_to;
+		psy_cfg.num_supplicants = ARRAY_SIZE(pda_power_supplied_to);
 	}
 
 #if IS_ENABLED(CONFIG_USB_PHY)
@@ -326,17 +325,19 @@ static int pda_power_probe(struct platform_device *pdev)
 #endif
 
 	if (pdata->is_ac_online) {
-		ret = power_supply_register(&pdev->dev, &pda_psy_ac);
-		if (ret) {
+		pda_psy_ac = power_supply_register(&pdev->dev,
+						   &pda_psy_ac_desc, &psy_cfg);
+		if (IS_ERR(pda_psy_ac)) {
 			dev_err(dev, "failed to register %s power supply\n",
-				pda_psy_ac.name);
+				pda_psy_ac_desc.name);
+			ret = PTR_ERR(pda_psy_ac);
 			goto ac_supply_failed;
 		}
 
 		if (ac_irq) {
 			ret = request_irq(ac_irq->start, power_changed_isr,
 					  get_irq_flags(ac_irq), ac_irq->name,
-					  &pda_psy_ac);
+					  pda_psy_ac);
 			if (ret) {
 				dev_err(dev, "request ac irq failed\n");
 				goto ac_irq_failed;
@@ -347,17 +348,20 @@ static int pda_power_probe(struct platform_device *pdev)
 	}
 
 	if (pdata->is_usb_online) {
-		ret = power_supply_register(&pdev->dev, &pda_psy_usb);
-		if (ret) {
+		pda_psy_usb = power_supply_register(&pdev->dev,
+						    &pda_psy_usb_desc,
+						    &psy_cfg);
+		if (IS_ERR(pda_psy_usb)) {
 			dev_err(dev, "failed to register %s power supply\n",
-				pda_psy_usb.name);
+				pda_psy_usb_desc.name);
+			ret = PTR_ERR(pda_psy_usb);
 			goto usb_supply_failed;
 		}
 
 		if (usb_irq) {
 			ret = request_irq(usb_irq->start, power_changed_isr,
 					  get_irq_flags(usb_irq),
-					  usb_irq->name, &pda_psy_usb);
+					  usb_irq->name, pda_psy_usb);
 			if (ret) {
 				dev_err(dev, "request usb irq failed\n");
 				goto usb_irq_failed;
@@ -394,21 +398,21 @@ static int pda_power_probe(struct platform_device *pdev)
 #if IS_ENABLED(CONFIG_USB_PHY)
 otg_reg_notifier_failed:
 	if (pdata->is_usb_online && usb_irq)
-		free_irq(usb_irq->start, &pda_psy_usb);
+		free_irq(usb_irq->start, pda_psy_usb);
 #endif
 usb_irq_failed:
 	if (pdata->is_usb_online)
-		power_supply_unregister(&pda_psy_usb);
+		power_supply_unregister(pda_psy_usb);
 usb_supply_failed:
 	if (pdata->is_ac_online && ac_irq)
-		free_irq(ac_irq->start, &pda_psy_ac);
+		free_irq(ac_irq->start, pda_psy_ac);
 #if IS_ENABLED(CONFIG_USB_PHY)
 	if (!IS_ERR_OR_NULL(transceiver))
 		usb_put_phy(transceiver);
 #endif
 ac_irq_failed:
 	if (pdata->is_ac_online)
-		power_supply_unregister(&pda_psy_ac);
+		power_supply_unregister(pda_psy_ac);
 ac_supply_failed:
 	if (ac_draw) {
 		regulator_put(ac_draw);
@@ -424,9 +428,9 @@ wrongid:
 static int pda_power_remove(struct platform_device *pdev)
 {
 	if (pdata->is_usb_online && usb_irq)
-		free_irq(usb_irq->start, &pda_psy_usb);
+		free_irq(usb_irq->start, pda_psy_usb);
 	if (pdata->is_ac_online && ac_irq)
-		free_irq(ac_irq->start, &pda_psy_ac);
+		free_irq(ac_irq->start, pda_psy_ac);
 
 	if (polling)
 		del_timer_sync(&polling_timer);
@@ -434,9 +438,9 @@ static int pda_power_remove(struct platform_device *pdev)
 	del_timer_sync(&supply_timer);
 
 	if (pdata->is_usb_online)
-		power_supply_unregister(&pda_psy_usb);
+		power_supply_unregister(pda_psy_usb);
 	if (pdata->is_ac_online)
-		power_supply_unregister(&pda_psy_ac);
+		power_supply_unregister(pda_psy_ac);
 #if IS_ENABLED(CONFIG_USB_PHY)
 	if (!IS_ERR_OR_NULL(transceiver))
 		usb_put_phy(transceiver);
diff --git a/drivers/power/pm2301_charger.c b/drivers/power/pm2301_charger.c
index 777324992c59..cc0893ffbf7e 100644
--- a/drivers/power/pm2301_charger.c
+++ b/drivers/power/pm2301_charger.c
@@ -216,7 +216,7 @@ static int pm2xxx_charger_ovv_mngt(struct pm2xxx_charger *pm2, int val)
 {
 	dev_err(pm2->dev, "Overvoltage detected\n");
 	pm2->flags.ovv = true;
-	power_supply_changed(&pm2->ac_chg.psy);
+	power_supply_changed(pm2->ac_chg.psy);
 
 	/* Schedule a new HW failure check */
 	queue_delayed_work(pm2->charger_wq, &pm2->check_hw_failure_work, 0);
@@ -229,7 +229,7 @@ static int pm2xxx_charger_wd_exp_mngt(struct pm2xxx_charger *pm2, int val)
 	dev_dbg(pm2->dev , "20 minutes watchdog expired\n");
 
 	pm2->ac.wd_expired = true;
-	power_supply_changed(&pm2->ac_chg.psy);
+	power_supply_changed(pm2->ac_chg.psy);
 
 	return 0;
 }
@@ -573,7 +573,7 @@ static int pm2xxx_charger_update_charger_current(struct ux500_charger *charger,
 	struct pm2xxx_charger *pm2;
 	u8 val;
 
-	if (charger->psy.type == POWER_SUPPLY_TYPE_MAINS)
+	if (charger->psy->desc->type == POWER_SUPPLY_TYPE_MAINS)
 		pm2 = to_pm2xxx_charger_ac_device_info(charger);
 	else
 		return -ENXIO;
@@ -816,7 +816,7 @@ static int pm2xxx_charger_ac_en(struct ux500_charger *charger,
 
 		dev_dbg(pm2->dev, "PM2301: " "Disabled AC charging\n");
 	}
-	power_supply_changed(&pm2->ac_chg.psy);
+	power_supply_changed(pm2->ac_chg.psy);
 
 error_occured:
 	return ret;
@@ -827,7 +827,7 @@ static int pm2xxx_charger_watchdog_kick(struct ux500_charger *charger)
 	int ret;
 	struct pm2xxx_charger *pm2;
 
-	if (charger->psy.type == POWER_SUPPLY_TYPE_MAINS)
+	if (charger->psy->desc->type == POWER_SUPPLY_TYPE_MAINS)
 		pm2 = to_pm2xxx_charger_ac_device_info(charger);
 	else
 		return -ENXIO;
@@ -845,8 +845,8 @@ static void pm2xxx_charger_ac_work(struct work_struct *work)
 		struct pm2xxx_charger, ac_work);
 
 
-	power_supply_changed(&pm2->ac_chg.psy);
-	sysfs_notify(&pm2->ac_chg.psy.dev->kobj, NULL, "present");
+	power_supply_changed(pm2->ac_chg.psy);
+	sysfs_notify(&pm2->ac_chg.psy->dev.kobj, NULL, "present");
 };
 
 static void pm2xxx_charger_check_hw_failure_work(struct work_struct *work)
@@ -862,7 +862,7 @@ static void pm2xxx_charger_check_hw_failure_work(struct work_struct *work)
 		if (!(reg_value & (PM2XXX_INT4_S_ITVPWR1OVV |
 					PM2XXX_INT4_S_ITVPWR2OVV))) {
 			pm2->flags.ovv = false;
-			power_supply_changed(&pm2->ac_chg.psy);
+			power_supply_changed(pm2->ac_chg.psy);
 		}
 	}
 
@@ -895,7 +895,7 @@ static void pm2xxx_charger_check_main_thermal_prot_work(
 				| PM2XXX_INT5_S_ITTHERMALSHUTDOWNFALL))
 		pm2->flags.main_thermal_prot = false;
 
-	power_supply_changed(&pm2->ac_chg.psy);
+	power_supply_changed(pm2->ac_chg.psy);
 }
 
 static struct pm2xxx_interrupts pm2xxx_int = {
@@ -989,6 +989,7 @@ static int pm2xxx_wall_charger_probe(struct i2c_client *i2c_client,
 		const struct i2c_device_id *id)
 {
 	struct pm2xxx_platform_data *pl_data = i2c_client->dev.platform_data;
+	struct power_supply_config psy_cfg = {};
 	struct pm2xxx_charger *pm2;
 	int ret = 0;
 	u8 val;
@@ -1042,13 +1043,14 @@ static int pm2xxx_wall_charger_probe(struct i2c_client *i2c_client,
 
 	/* AC supply */
 	/* power_supply base class */
-	pm2->ac_chg.psy.name = pm2->pdata->label;
-	pm2->ac_chg.psy.type = POWER_SUPPLY_TYPE_MAINS;
-	pm2->ac_chg.psy.properties = pm2xxx_charger_ac_props;
-	pm2->ac_chg.psy.num_properties = ARRAY_SIZE(pm2xxx_charger_ac_props);
-	pm2->ac_chg.psy.get_property = pm2xxx_charger_ac_get_property;
-	pm2->ac_chg.psy.supplied_to = pm2->pdata->supplied_to;
-	pm2->ac_chg.psy.num_supplicants = pm2->pdata->num_supplicants;
+	pm2->ac_chg_desc.name = pm2->pdata->label;
+	pm2->ac_chg_desc.type = POWER_SUPPLY_TYPE_MAINS;
+	pm2->ac_chg_desc.properties = pm2xxx_charger_ac_props;
+	pm2->ac_chg_desc.num_properties = ARRAY_SIZE(pm2xxx_charger_ac_props);
+	pm2->ac_chg_desc.get_property = pm2xxx_charger_ac_get_property;
+
+	psy_cfg.supplied_to = pm2->pdata->supplied_to;
+	psy_cfg.num_supplicants = pm2->pdata->num_supplicants;
 	/* pm2xxx_charger sub-class */
 	pm2->ac_chg.ops.enable = &pm2xxx_charger_ac_en;
 	pm2->ac_chg.ops.kick_wd = &pm2xxx_charger_watchdog_kick;
@@ -1093,9 +1095,11 @@ static int pm2xxx_wall_charger_probe(struct i2c_client *i2c_client,
 	}
 
 	/* Register AC charger class */
-	ret = power_supply_register(pm2->dev, &pm2->ac_chg.psy);
-	if (ret) {
+	pm2->ac_chg.psy = power_supply_register(pm2->dev, &pm2->ac_chg_desc,
+						&psy_cfg);
+	if (IS_ERR(pm2->ac_chg.psy)) {
 		dev_err(pm2->dev, "failed to register AC charger\n");
+		ret = PTR_ERR(pm2->ac_chg.psy);
 		goto free_regulator;
 	}
 
@@ -1167,8 +1171,8 @@ static int pm2xxx_wall_charger_probe(struct i2c_client *i2c_client,
 		ab8500_override_turn_on_stat(~AB8500_POW_KEY_1_ON,
 					     AB8500_MAIN_CH_DET);
 		pm2->ac_conn = true;
-		power_supply_changed(&pm2->ac_chg.psy);
-		sysfs_notify(&pm2->ac_chg.psy.dev->kobj, NULL, "present");
+		power_supply_changed(pm2->ac_chg.psy);
+		sysfs_notify(&pm2->ac_chg.psy->dev.kobj, NULL, "present");
 	}
 
 	return 0;
@@ -1183,7 +1187,7 @@ unregister_pm2xxx_interrupt:
 	free_irq(gpio_to_irq(pm2->pdata->gpio_irq_number), pm2);
 unregister_pm2xxx_charger:
 	/* unregister power supply */
-	power_supply_unregister(&pm2->ac_chg.psy);
+	power_supply_unregister(pm2->ac_chg.psy);
 free_regulator:
 	/* disable the regulator */
 	regulator_put(pm2->regu);
@@ -1218,7 +1222,7 @@ static int pm2xxx_wall_charger_remove(struct i2c_client *i2c_client)
 	/* disable the regulator */
 	regulator_put(pm2->regu);
 
-	power_supply_unregister(&pm2->ac_chg.psy);
+	power_supply_unregister(pm2->ac_chg.psy);
 
 	if (gpio_is_valid(pm2->lpn_pin))
 		gpio_free(pm2->lpn_pin);
diff --git a/drivers/power/pm2301_charger.h b/drivers/power/pm2301_charger.h
index 8ce3cc0195df..24181cf9717b 100644
--- a/drivers/power/pm2301_charger.h
+++ b/drivers/power/pm2301_charger.h
@@ -486,6 +486,7 @@ struct pm2xxx_charger {
 	struct work_struct check_main_thermal_prot_work;
 	struct delayed_work check_hw_failure_work;
 	struct ux500_charger ac_chg;
+	struct power_supply_desc ac_chg_desc;
 	struct pm2xxx_charger_event_flags flags;
 };
 
diff --git a/drivers/power/pmu_battery.c b/drivers/power/pmu_battery.c
index 023d24993b87..9c8d5253812c 100644
--- a/drivers/power/pmu_battery.c
+++ b/drivers/power/pmu_battery.c
@@ -17,13 +17,14 @@
 #include <linux/slab.h>
 
 static struct pmu_battery_dev {
-	struct power_supply bat;
+	struct power_supply *bat;
+	struct power_supply_desc bat_desc;
 	struct pmu_battery_info *pbi;
 	char name[16];
 	int propval;
 } *pbats[PMU_MAX_BATTERIES];
 
-#define to_pmu_battery_dev(x) container_of(x, struct pmu_battery_dev, bat)
+#define to_pmu_battery_dev(x) power_supply_get_drvdata(x)
 
 /*********************************************************************
  *		Power
@@ -49,7 +50,7 @@ static enum power_supply_property pmu_ac_props[] = {
 	POWER_SUPPLY_PROP_ONLINE,
 };
 
-static struct power_supply pmu_ac = {
+static const struct power_supply_desc pmu_ac_desc = {
 	.name = "pmu-ac",
 	.type = POWER_SUPPLY_TYPE_MAINS,
 	.properties = pmu_ac_props,
@@ -57,6 +58,8 @@ static struct power_supply pmu_ac = {
 	.get_property = pmu_get_ac_prop,
 };
 
+static struct power_supply *pmu_ac;
+
 /*********************************************************************
  *		Battery properties
  *********************************************************************/
@@ -142,7 +145,7 @@ static struct platform_device *bat_pdev;
 
 static int __init pmu_bat_init(void)
 {
-	int ret;
+	int ret = 0;
 	int i;
 
 	bat_pdev = platform_device_register_simple("pmu-battery",
@@ -152,25 +155,32 @@ static int __init pmu_bat_init(void)
 		goto pdev_register_failed;
 	}
 
-	ret = power_supply_register(&bat_pdev->dev, &pmu_ac);
-	if (ret)
+	pmu_ac = power_supply_register(&bat_pdev->dev, &pmu_ac_desc, NULL);
+	if (IS_ERR(pmu_ac)) {
+		ret = PTR_ERR(pmu_ac);
 		goto ac_register_failed;
+	}
 
 	for (i = 0; i < pmu_battery_count; i++) {
+		struct power_supply_config psy_cfg = {};
 		struct pmu_battery_dev *pbat = kzalloc(sizeof(*pbat),
 						       GFP_KERNEL);
 		if (!pbat)
 			break;
 
 		sprintf(pbat->name, "PMU_battery_%d", i);
-		pbat->bat.name = pbat->name;
-		pbat->bat.properties = pmu_bat_props;
-		pbat->bat.num_properties = ARRAY_SIZE(pmu_bat_props);
-		pbat->bat.get_property = pmu_bat_get_property;
+		pbat->bat_desc.name = pbat->name;
+		pbat->bat_desc.properties = pmu_bat_props;
+		pbat->bat_desc.num_properties = ARRAY_SIZE(pmu_bat_props);
+		pbat->bat_desc.get_property = pmu_bat_get_property;
 		pbat->pbi = &pmu_batteries[i];
+		psy_cfg.drv_data = pbat;
 
-		ret = power_supply_register(&bat_pdev->dev, &pbat->bat);
-		if (ret) {
+		pbat->bat = power_supply_register(&bat_pdev->dev,
+						  &pbat->bat_desc,
+						  &psy_cfg);
+		if (IS_ERR(pbat->bat)) {
+			ret = PTR_ERR(pbat->bat);
 			kfree(pbat);
 			goto battery_register_failed;
 		}
@@ -183,10 +193,10 @@ battery_register_failed:
 	while (i--) {
 		if (!pbats[i])
 			continue;
-		power_supply_unregister(&pbats[i]->bat);
+		power_supply_unregister(pbats[i]->bat);
 		kfree(pbats[i]);
 	}
-	power_supply_unregister(&pmu_ac);
+	power_supply_unregister(pmu_ac);
 ac_register_failed:
 	platform_device_unregister(bat_pdev);
 pdev_register_failed:
@@ -201,10 +211,10 @@ static void __exit pmu_bat_exit(void)
 	for (i = 0; i < PMU_MAX_BATTERIES; i++) {
 		if (!pbats[i])
 			continue;
-		power_supply_unregister(&pbats[i]->bat);
+		power_supply_unregister(pbats[i]->bat);
 		kfree(pbats[i]);
 	}
-	power_supply_unregister(&pmu_ac);
+	power_supply_unregister(pmu_ac);
 	platform_device_unregister(bat_pdev);
 }
 
diff --git a/drivers/power/power_supply_core.c b/drivers/power/power_supply_core.c
index 694e8cddd5c1..2ed4a4a6b3c5 100644
--- a/drivers/power/power_supply_core.c
+++ b/drivers/power/power_supply_core.c
@@ -40,16 +40,16 @@ static bool __power_supply_is_supplied_by(struct power_supply *supplier,
 
 	/* Support both supplied_to and supplied_from modes */
 	if (supply->supplied_from) {
-		if (!supplier->name)
+		if (!supplier->desc->name)
 			return false;
 		for (i = 0; i < supply->num_supplies; i++)
-			if (!strcmp(supplier->name, supply->supplied_from[i]))
+			if (!strcmp(supplier->desc->name, supply->supplied_from[i]))
 				return true;
 	} else {
-		if (!supply->name)
+		if (!supply->desc->name)
 			return false;
 		for (i = 0; i < supplier->num_supplicants; i++)
-			if (!strcmp(supplier->supplied_to[i], supply->name))
+			if (!strcmp(supplier->supplied_to[i], supply->desc->name))
 				return true;
 	}
 
@@ -62,8 +62,8 @@ static int __power_supply_changed_work(struct device *dev, void *data)
 	struct power_supply *pst = dev_get_drvdata(dev);
 
 	if (__power_supply_is_supplied_by(psy, pst)) {
-		if (pst->external_power_changed)
-			pst->external_power_changed(pst);
+		if (pst->desc->external_power_changed)
+			pst->desc->external_power_changed(pst);
 	}
 
 	return 0;
@@ -75,7 +75,7 @@ static void power_supply_changed_work(struct work_struct *work)
 	struct power_supply *psy = container_of(work, struct power_supply,
 						changed_work);
 
-	dev_dbg(psy->dev, "%s\n", __func__);
+	dev_dbg(&psy->dev, "%s\n", __func__);
 
 	spin_lock_irqsave(&psy->changed_lock, flags);
 	/*
@@ -93,7 +93,7 @@ static void power_supply_changed_work(struct work_struct *work)
 		power_supply_update_leds(psy);
 		atomic_notifier_call_chain(&power_supply_notifier,
 				PSY_EVENT_PROP_CHANGED, psy);
-		kobject_uevent(&psy->dev->kobj, KOBJ_CHANGE);
+		kobject_uevent(&psy->dev.kobj, KOBJ_CHANGE);
 		spin_lock_irqsave(&psy->changed_lock, flags);
 	}
 
@@ -103,7 +103,7 @@ static void power_supply_changed_work(struct work_struct *work)
 	 * to true.
 	 */
 	if (likely(!psy->changed))
-		pm_relax(psy->dev);
+		pm_relax(&psy->dev);
 	spin_unlock_irqrestore(&psy->changed_lock, flags);
 }
 
@@ -111,11 +111,11 @@ void power_supply_changed(struct power_supply *psy)
 {
 	unsigned long flags;
 
-	dev_dbg(psy->dev, "%s\n", __func__);
+	dev_dbg(&psy->dev, "%s\n", __func__);
 
 	spin_lock_irqsave(&psy->changed_lock, flags);
 	psy->changed = true;
-	pm_stay_awake(psy->dev);
+	pm_stay_awake(&psy->dev);
 	spin_unlock_irqrestore(&psy->changed_lock, flags);
 	schedule_work(&psy->changed_work);
 }
@@ -138,9 +138,9 @@ static int __power_supply_populate_supplied_from(struct device *dev,
 			break;
 
 		if (np == epsy->of_node) {
-			dev_info(psy->dev, "%s: Found supply : %s\n",
-				psy->name, epsy->name);
-			psy->supplied_from[i-1] = (char *)epsy->name;
+			dev_info(&psy->dev, "%s: Found supply : %s\n",
+				psy->desc->name, epsy->desc->name);
+			psy->supplied_from[i-1] = (char *)epsy->desc->name;
 			psy->num_supplies++;
 			of_node_put(np);
 			break;
@@ -158,7 +158,7 @@ static int power_supply_populate_supplied_from(struct power_supply *psy)
 	error = class_for_each_device(power_supply_class, NULL, psy,
 				      __power_supply_populate_supplied_from);
 
-	dev_dbg(psy->dev, "%s %d\n", __func__, error);
+	dev_dbg(&psy->dev, "%s %d\n", __func__, error);
 
 	return error;
 }
@@ -220,7 +220,7 @@ static int power_supply_check_supplies(struct power_supply *psy)
 		of_node_put(np);
 
 		if (ret) {
-			dev_dbg(psy->dev, "Failed to find supply!\n");
+			dev_dbg(&psy->dev, "Failed to find supply!\n");
 			return ret;
 		}
 	} while (np);
@@ -230,17 +230,18 @@ static int power_supply_check_supplies(struct power_supply *psy)
 		return 0;
 
 	/* All supplies found, allocate char ** array for filling */
-	psy->supplied_from = devm_kzalloc(psy->dev, sizeof(psy->supplied_from),
+	psy->supplied_from = devm_kzalloc(&psy->dev, sizeof(psy->supplied_from),
 					  GFP_KERNEL);
 	if (!psy->supplied_from) {
-		dev_err(psy->dev, "Couldn't allocate memory for supply list\n");
+		dev_err(&psy->dev, "Couldn't allocate memory for supply list\n");
 		return -ENOMEM;
 	}
 
-	*psy->supplied_from = devm_kzalloc(psy->dev, sizeof(char *) * (cnt - 1),
+	*psy->supplied_from = devm_kzalloc(&psy->dev,
+					   sizeof(char *) * (cnt - 1),
 					   GFP_KERNEL);
 	if (!*psy->supplied_from) {
-		dev_err(psy->dev, "Couldn't allocate memory for supply list\n");
+		dev_err(&psy->dev, "Couldn't allocate memory for supply list\n");
 		return -ENOMEM;
 	}
 
@@ -260,7 +261,8 @@ static int __power_supply_am_i_supplied(struct device *dev, void *data)
 	struct power_supply *epsy = dev_get_drvdata(dev);
 
 	if (__power_supply_is_supplied_by(epsy, psy))
-		if (!epsy->get_property(epsy, POWER_SUPPLY_PROP_ONLINE, &ret))
+		if (!epsy->desc->get_property(epsy, POWER_SUPPLY_PROP_ONLINE,
+					&ret))
 			return ret.intval;
 
 	return 0;
@@ -273,7 +275,7 @@ int power_supply_am_i_supplied(struct power_supply *psy)
 	error = class_for_each_device(power_supply_class, NULL, psy,
 				      __power_supply_am_i_supplied);
 
-	dev_dbg(psy->dev, "%s %d\n", __func__, error);
+	dev_dbg(&psy->dev, "%s %d\n", __func__, error);
 
 	return error;
 }
@@ -286,8 +288,9 @@ static int __power_supply_is_system_supplied(struct device *dev, void *data)
 	unsigned int *count = data;
 
 	(*count)++;
-	if (psy->type != POWER_SUPPLY_TYPE_BATTERY)
-		if (!psy->get_property(psy, POWER_SUPPLY_PROP_ONLINE, &ret))
+	if (psy->desc->type != POWER_SUPPLY_TYPE_BATTERY)
+		if (!psy->desc->get_property(psy, POWER_SUPPLY_PROP_ONLINE,
+					&ret))
 			return ret.intval;
 
 	return 0;
@@ -314,8 +317,10 @@ EXPORT_SYMBOL_GPL(power_supply_is_system_supplied);
 
 int power_supply_set_battery_charged(struct power_supply *psy)
 {
-	if (psy->type == POWER_SUPPLY_TYPE_BATTERY && psy->set_charged) {
-		psy->set_charged(psy);
+	if (atomic_read(&psy->use_cnt) >= 0 &&
+			psy->desc->type == POWER_SUPPLY_TYPE_BATTERY &&
+			psy->desc->set_charged) {
+		psy->desc->set_charged(psy);
 		return 0;
 	}
 
@@ -328,28 +333,74 @@ static int power_supply_match_device_by_name(struct device *dev, const void *dat
 	const char *name = data;
 	struct power_supply *psy = dev_get_drvdata(dev);
 
-	return strcmp(psy->name, name) == 0;
+	return strcmp(psy->desc->name, name) == 0;
 }
 
+/**
+ * power_supply_get_by_name() - Search for a power supply and returns its ref
+ * @name: Power supply name to fetch
+ *
+ * If power supply was found, it increases reference count for the
+ * internal power supply's device. The user should power_supply_put()
+ * after usage.
+ *
+ * Return: On success returns a reference to a power supply with
+ * matching name equals to @name, a NULL otherwise.
+ */
 struct power_supply *power_supply_get_by_name(const char *name)
 {
+	struct power_supply *psy = NULL;
 	struct device *dev = class_find_device(power_supply_class, NULL, name,
 					power_supply_match_device_by_name);
 
-	return dev ? dev_get_drvdata(dev) : NULL;
+	if (dev) {
+		psy = dev_get_drvdata(dev);
+		atomic_inc(&psy->use_cnt);
+	}
+
+	return psy;
 }
 EXPORT_SYMBOL_GPL(power_supply_get_by_name);
 
+/**
+ * power_supply_put() - Drop reference obtained with power_supply_get_by_name
+ * @psy: Reference to put
+ *
+ * The reference to power supply should be put before unregistering
+ * the power supply.
+ */
+void power_supply_put(struct power_supply *psy)
+{
+	might_sleep();
+
+	atomic_dec(&psy->use_cnt);
+	put_device(&psy->dev);
+}
+EXPORT_SYMBOL_GPL(power_supply_put);
+
 #ifdef CONFIG_OF
 static int power_supply_match_device_node(struct device *dev, const void *data)
 {
 	return dev->parent && dev->parent->of_node == data;
 }
 
+/**
+ * power_supply_get_by_phandle() - Search for a power supply and returns its ref
+ * @np: Pointer to device node holding phandle property
+ * @phandle_name: Name of property holding a power supply name
+ *
+ * If power supply was found, it increases reference count for the
+ * internal power supply's device. The user should power_supply_put()
+ * after usage.
+ *
+ * Return: On success returns a reference to a power supply with
+ * matching name equals to value under @property, NULL or ERR_PTR otherwise.
+ */
 struct power_supply *power_supply_get_by_phandle(struct device_node *np,
 							const char *property)
 {
 	struct device_node *power_supply_np;
+	struct power_supply *psy = NULL;
 	struct device *dev;
 
 	power_supply_np = of_parse_phandle(np, property, 0);
@@ -361,21 +412,70 @@ struct power_supply *power_supply_get_by_phandle(struct device_node *np,
 
 	of_node_put(power_supply_np);
 
-	return dev ? dev_get_drvdata(dev) : NULL;
+	if (dev) {
+		psy = dev_get_drvdata(dev);
+		atomic_inc(&psy->use_cnt);
+	}
+
+	return psy;
 }
 EXPORT_SYMBOL_GPL(power_supply_get_by_phandle);
 #endif /* CONFIG_OF */
 
+int power_supply_get_property(struct power_supply *psy,
+			    enum power_supply_property psp,
+			    union power_supply_propval *val)
+{
+	if (atomic_read(&psy->use_cnt) <= 0)
+		return -ENODEV;
+
+	return psy->desc->get_property(psy, psp, val);
+}
+EXPORT_SYMBOL_GPL(power_supply_get_property);
+
+int power_supply_set_property(struct power_supply *psy,
+			    enum power_supply_property psp,
+			    const union power_supply_propval *val)
+{
+	if (atomic_read(&psy->use_cnt) <= 0 || !psy->desc->set_property)
+		return -ENODEV;
+
+	return psy->desc->set_property(psy, psp, val);
+}
+EXPORT_SYMBOL_GPL(power_supply_set_property);
+
+int power_supply_property_is_writeable(struct power_supply *psy,
+					enum power_supply_property psp)
+{
+	if (atomic_read(&psy->use_cnt) <= 0 ||
+			!psy->desc->property_is_writeable)
+		return -ENODEV;
+
+	return psy->desc->property_is_writeable(psy, psp);
+}
+EXPORT_SYMBOL_GPL(power_supply_property_is_writeable);
+
+void power_supply_external_power_changed(struct power_supply *psy)
+{
+	if (atomic_read(&psy->use_cnt) <= 0 ||
+			!psy->desc->external_power_changed)
+		return;
+
+	psy->desc->external_power_changed(psy);
+}
+EXPORT_SYMBOL_GPL(power_supply_external_power_changed);
+
 int power_supply_powers(struct power_supply *psy, struct device *dev)
 {
-	return sysfs_create_link(&psy->dev->kobj, &dev->kobj, "powers");
+	return sysfs_create_link(&psy->dev.kobj, &dev->kobj, "powers");
 }
 EXPORT_SYMBOL_GPL(power_supply_powers);
 
 static void power_supply_dev_release(struct device *dev)
 {
+	struct power_supply *psy = container_of(dev, struct power_supply, dev);
 	pr_debug("device: '%s': %s\n", dev_name(dev), __func__);
-	kfree(dev);
+	kfree(psy);
 }
 
 int power_supply_reg_notifier(struct notifier_block *nb)
@@ -400,7 +500,7 @@ static int power_supply_read_temp(struct thermal_zone_device *tzd,
 
 	WARN_ON(tzd == NULL);
 	psy = tzd->devdata;
-	ret = psy->get_property(psy, POWER_SUPPLY_PROP_TEMP, &val);
+	ret = psy->desc->get_property(psy, POWER_SUPPLY_PROP_TEMP, &val);
 
 	/* Convert tenths of degree Celsius to milli degree Celsius. */
 	if (!ret)
@@ -417,14 +517,14 @@ static int psy_register_thermal(struct power_supply *psy)
 {
 	int i;
 
-	if (psy->no_thermal)
+	if (psy->desc->no_thermal)
 		return 0;
 
 	/* Register battery zone device psy reports temperature */
-	for (i = 0; i < psy->num_properties; i++) {
-		if (psy->properties[i] == POWER_SUPPLY_PROP_TEMP) {
-			psy->tzd = thermal_zone_device_register(psy->name, 0, 0,
-					psy, &psy_tzd_ops, NULL, 0, 0);
+	for (i = 0; i < psy->desc->num_properties; i++) {
+		if (psy->desc->properties[i] == POWER_SUPPLY_PROP_TEMP) {
+			psy->tzd = thermal_zone_device_register(psy->desc->name,
+					0, 0, psy, &psy_tzd_ops, NULL, 0, 0);
 			return PTR_ERR_OR_ZERO(psy->tzd);
 		}
 	}
@@ -447,7 +547,7 @@ static int ps_get_max_charge_cntl_limit(struct thermal_cooling_device *tcd,
 	int ret;
 
 	psy = tcd->devdata;
-	ret = psy->get_property(psy,
+	ret = psy->desc->get_property(psy,
 		POWER_SUPPLY_PROP_CHARGE_CONTROL_LIMIT_MAX, &val);
 	if (!ret)
 		*state = val.intval;
@@ -463,7 +563,7 @@ static int ps_get_cur_chrage_cntl_limit(struct thermal_cooling_device *tcd,
 	int ret;
 
 	psy = tcd->devdata;
-	ret = psy->get_property(psy,
+	ret = psy->desc->get_property(psy,
 		POWER_SUPPLY_PROP_CHARGE_CONTROL_LIMIT, &val);
 	if (!ret)
 		*state = val.intval;
@@ -480,7 +580,7 @@ static int ps_set_cur_charge_cntl_limit(struct thermal_cooling_device *tcd,
 
 	psy = tcd->devdata;
 	val.intval = state;
-	ret = psy->set_property(psy,
+	ret = psy->desc->set_property(psy,
 		POWER_SUPPLY_PROP_CHARGE_CONTROL_LIMIT, &val);
 
 	return ret;
@@ -497,11 +597,11 @@ static int psy_register_cooler(struct power_supply *psy)
 	int i;
 
 	/* Register for cooling device if psy can control charging */
-	for (i = 0; i < psy->num_properties; i++) {
-		if (psy->properties[i] ==
+	for (i = 0; i < psy->desc->num_properties; i++) {
+		if (psy->desc->properties[i] ==
 				POWER_SUPPLY_PROP_CHARGE_CONTROL_LIMIT) {
 			psy->tcd = thermal_cooling_device_register(
-							(char *)psy->name,
+							(char *)psy->desc->name,
 							psy, &psy_tcd_ops);
 			return PTR_ERR_OR_ZERO(psy->tcd);
 		}
@@ -535,15 +635,21 @@ static void psy_unregister_cooler(struct power_supply *psy)
 }
 #endif
 
-static int __power_supply_register(struct device *parent,
-				   struct power_supply *psy, bool ws)
+static struct power_supply *__must_check
+__power_supply_register(struct device *parent,
+				   const struct power_supply_desc *desc,
+				   const struct power_supply_config *cfg,
+				   bool ws)
 {
 	struct device *dev;
+	struct power_supply *psy;
 	int rc;
 
-	dev = kzalloc(sizeof(*dev), GFP_KERNEL);
-	if (!dev)
-		return -ENOMEM;
+	psy = kzalloc(sizeof(*psy), GFP_KERNEL);
+	if (!psy)
+		return ERR_PTR(-ENOMEM);
+
+	dev = &psy->dev;
 
 	device_initialize(dev);
 
@@ -552,9 +658,16 @@ static int __power_supply_register(struct device *parent,
 	dev->parent = parent;
 	dev->release = power_supply_dev_release;
 	dev_set_drvdata(dev, psy);
-	psy->dev = dev;
+	psy->desc = desc;
+	atomic_inc(&psy->use_cnt);
+	if (cfg) {
+		psy->drv_data = cfg->drv_data;
+		psy->of_node = cfg->of_node;
+		psy->supplied_to = cfg->supplied_to;
+		psy->num_supplicants = cfg->num_supplicants;
+	}
 
-	rc = dev_set_name(dev, "%s", psy->name);
+	rc = dev_set_name(dev, "%s", desc->name);
 	if (rc)
 		goto dev_set_name_failed;
 
@@ -589,7 +702,7 @@ static int __power_supply_register(struct device *parent,
 
 	power_supply_changed(psy);
 
-	return 0;
+	return psy;
 
 create_triggers_failed:
 	psy_unregister_cooler(psy);
@@ -602,33 +715,155 @@ wakeup_init_failed:
 check_supplies_failed:
 dev_set_name_failed:
 	put_device(dev);
-	return rc;
+	return ERR_PTR(rc);
 }
 
-int power_supply_register(struct device *parent, struct power_supply *psy)
+/**
+ * power_supply_register() - Register new power supply
+ * @parent:	Device to be a parent of power supply's device
+ * @desc:	Description of power supply, must be valid through whole
+ *		lifetime of this power supply
+ * @cfg:	Run-time specific configuration accessed during registering,
+ *		may be NULL
+ *
+ * Return: A pointer to newly allocated power_supply on success
+ * or ERR_PTR otherwise.
+ * Use power_supply_unregister() on returned power_supply pointer to release
+ * resources.
+ */
+struct power_supply *__must_check power_supply_register(struct device *parent,
+		const struct power_supply_desc *desc,
+		const struct power_supply_config *cfg)
 {
-	return __power_supply_register(parent, psy, true);
+	return __power_supply_register(parent, desc, cfg, true);
 }
 EXPORT_SYMBOL_GPL(power_supply_register);
 
-int power_supply_register_no_ws(struct device *parent, struct power_supply *psy)
+/**
+ * power_supply_register() - Register new non-waking-source power supply
+ * @parent:	Device to be a parent of power supply's device
+ * @desc:	Description of power supply, must be valid through whole
+ *		lifetime of this power supply
+ * @cfg:	Run-time specific configuration accessed during registering,
+ *		may be NULL
+ *
+ * Return: A pointer to newly allocated power_supply on success
+ * or ERR_PTR otherwise.
+ * Use power_supply_unregister() on returned power_supply pointer to release
+ * resources.
+ */
+struct power_supply *__must_check
+power_supply_register_no_ws(struct device *parent,
+		const struct power_supply_desc *desc,
+		const struct power_supply_config *cfg)
 {
-	return __power_supply_register(parent, psy, false);
+	return __power_supply_register(parent, desc, cfg, false);
 }
 EXPORT_SYMBOL_GPL(power_supply_register_no_ws);
 
+static void devm_power_supply_release(struct device *dev, void *res)
+{
+	struct power_supply **psy = res;
+
+	power_supply_unregister(*psy);
+}
+
+/**
+ * power_supply_register() - Register managed power supply
+ * @parent:	Device to be a parent of power supply's device
+ * @desc:	Description of power supply, must be valid through whole
+ *		lifetime of this power supply
+ * @cfg:	Run-time specific configuration accessed during registering,
+ *		may be NULL
+ *
+ * Return: A pointer to newly allocated power_supply on success
+ * or ERR_PTR otherwise.
+ * The returned power_supply pointer will be automatically unregistered
+ * on driver detach.
+ */
+struct power_supply *__must_check
+devm_power_supply_register(struct device *parent,
+		const struct power_supply_desc *desc,
+		const struct power_supply_config *cfg)
+{
+	struct power_supply **ptr, *psy;
+
+	ptr = devres_alloc(devm_power_supply_release, sizeof(*ptr), GFP_KERNEL);
+
+	if (!ptr)
+		return ERR_PTR(-ENOMEM);
+	psy = __power_supply_register(parent, desc, cfg, true);
+	if (IS_ERR(psy)) {
+		devres_free(ptr);
+	} else {
+		*ptr = psy;
+		devres_add(parent, ptr);
+	}
+	return psy;
+}
+EXPORT_SYMBOL_GPL(devm_power_supply_register);
+
+/**
+ * power_supply_register() - Register managed non-waking-source power supply
+ * @parent:	Device to be a parent of power supply's device
+ * @desc:	Description of power supply, must be valid through whole
+ *		lifetime of this power supply
+ * @cfg:	Run-time specific configuration accessed during registering,
+ *		may be NULL
+ *
+ * Return: A pointer to newly allocated power_supply on success
+ * or ERR_PTR otherwise.
+ * The returned power_supply pointer will be automatically unregistered
+ * on driver detach.
+ */
+struct power_supply *__must_check
+devm_power_supply_register_no_ws(struct device *parent,
+		const struct power_supply_desc *desc,
+		const struct power_supply_config *cfg)
+{
+	struct power_supply **ptr, *psy;
+
+	ptr = devres_alloc(devm_power_supply_release, sizeof(*ptr), GFP_KERNEL);
+
+	if (!ptr)
+		return ERR_PTR(-ENOMEM);
+	psy = __power_supply_register(parent, desc, cfg, false);
+	if (IS_ERR(psy)) {
+		devres_free(ptr);
+	} else {
+		*ptr = psy;
+		devres_add(parent, ptr);
+	}
+	return psy;
+}
+EXPORT_SYMBOL_GPL(devm_power_supply_register_no_ws);
+
+/**
+ * power_supply_unregister() - Remove this power supply from system
+ * @psy:	Pointer to power supply to unregister
+ *
+ * Remove this power supply from the system. The resources of power supply
+ * will be freed here or on last power_supply_put() call.
+ */
 void power_supply_unregister(struct power_supply *psy)
 {
+	WARN_ON(atomic_dec_return(&psy->use_cnt));
 	cancel_work_sync(&psy->changed_work);
-	sysfs_remove_link(&psy->dev->kobj, "powers");
+	sysfs_remove_link(&psy->dev.kobj, "powers");
 	power_supply_remove_triggers(psy);
 	psy_unregister_cooler(psy);
 	psy_unregister_thermal(psy);
-	device_init_wakeup(psy->dev, false);
-	device_unregister(psy->dev);
+	device_init_wakeup(&psy->dev, false);
+	device_unregister(&psy->dev);
 }
 EXPORT_SYMBOL_GPL(power_supply_unregister);
 
+void *power_supply_get_drvdata(struct power_supply *psy)
+{
+	return psy->drv_data;
+}
+EXPORT_SYMBOL_GPL(power_supply_get_drvdata);
+
 static int __init power_supply_class_init(void)
 {
 	power_supply_class = class_create(THIS_MODULE, "power_supply");
diff --git a/drivers/power/power_supply_leds.c b/drivers/power/power_supply_leds.c
index effa093c37b0..2d41a43fc81a 100644
--- a/drivers/power/power_supply_leds.c
+++ b/drivers/power/power_supply_leds.c
@@ -25,10 +25,10 @@ static void power_supply_update_bat_leds(struct power_supply *psy)
 	unsigned long delay_on = 0;
 	unsigned long delay_off = 0;
 
-	if (psy->get_property(psy, POWER_SUPPLY_PROP_STATUS, &status))
+	if (psy->desc->get_property(psy, POWER_SUPPLY_PROP_STATUS, &status))
 		return;
 
-	dev_dbg(psy->dev, "%s %d\n", __func__, status.intval);
+	dev_dbg(&psy->dev, "%s %d\n", __func__, status.intval);
 
 	switch (status.intval) {
 	case POWER_SUPPLY_STATUS_FULL:
@@ -58,21 +58,21 @@ static void power_supply_update_bat_leds(struct power_supply *psy)
 static int power_supply_create_bat_triggers(struct power_supply *psy)
 {
 	psy->charging_full_trig_name = kasprintf(GFP_KERNEL,
-					"%s-charging-or-full", psy->name);
+					"%s-charging-or-full", psy->desc->name);
 	if (!psy->charging_full_trig_name)
 		goto charging_full_failed;
 
 	psy->charging_trig_name = kasprintf(GFP_KERNEL,
-					"%s-charging", psy->name);
+					"%s-charging", psy->desc->name);
 	if (!psy->charging_trig_name)
 		goto charging_failed;
 
-	psy->full_trig_name = kasprintf(GFP_KERNEL, "%s-full", psy->name);
+	psy->full_trig_name = kasprintf(GFP_KERNEL, "%s-full", psy->desc->name);
 	if (!psy->full_trig_name)
 		goto full_failed;
 
 	psy->charging_blink_full_solid_trig_name = kasprintf(GFP_KERNEL,
-		"%s-charging-blink-full-solid", psy->name);
+		"%s-charging-blink-full-solid", psy->desc->name);
 	if (!psy->charging_blink_full_solid_trig_name)
 		goto charging_blink_full_solid_failed;
 
@@ -115,10 +115,10 @@ static void power_supply_update_gen_leds(struct power_supply *psy)
 {
 	union power_supply_propval online;
 
-	if (psy->get_property(psy, POWER_SUPPLY_PROP_ONLINE, &online))
+	if (psy->desc->get_property(psy, POWER_SUPPLY_PROP_ONLINE, &online))
 		return;
 
-	dev_dbg(psy->dev, "%s %d\n", __func__, online.intval);
+	dev_dbg(&psy->dev, "%s %d\n", __func__, online.intval);
 
 	if (online.intval)
 		led_trigger_event(psy->online_trig, LED_FULL);
@@ -128,7 +128,8 @@ static void power_supply_update_gen_leds(struct power_supply *psy)
 
 static int power_supply_create_gen_triggers(struct power_supply *psy)
 {
-	psy->online_trig_name = kasprintf(GFP_KERNEL, "%s-online", psy->name);
+	psy->online_trig_name = kasprintf(GFP_KERNEL, "%s-online",
+					  psy->desc->name);
 	if (!psy->online_trig_name)
 		return -ENOMEM;
 
@@ -147,7 +148,7 @@ static void power_supply_remove_gen_triggers(struct power_supply *psy)
 
 void power_supply_update_leds(struct power_supply *psy)
 {
-	if (psy->type == POWER_SUPPLY_TYPE_BATTERY)
+	if (psy->desc->type == POWER_SUPPLY_TYPE_BATTERY)
 		power_supply_update_bat_leds(psy);
 	else
 		power_supply_update_gen_leds(psy);
@@ -155,14 +156,14 @@ void power_supply_update_leds(struct power_supply *psy)
 
 int power_supply_create_triggers(struct power_supply *psy)
 {
-	if (psy->type == POWER_SUPPLY_TYPE_BATTERY)
+	if (psy->desc->type == POWER_SUPPLY_TYPE_BATTERY)
 		return power_supply_create_bat_triggers(psy);
 	return power_supply_create_gen_triggers(psy);
 }
 
 void power_supply_remove_triggers(struct power_supply *psy)
 {
-	if (psy->type == POWER_SUPPLY_TYPE_BATTERY)
+	if (psy->desc->type == POWER_SUPPLY_TYPE_BATTERY)
 		power_supply_remove_bat_triggers(psy);
 	else
 		power_supply_remove_gen_triggers(psy);
diff --git a/drivers/power/power_supply_sysfs.c b/drivers/power/power_supply_sysfs.c
index 62653f50a524..9134e3d2d95e 100644
--- a/drivers/power/power_supply_sysfs.c
+++ b/drivers/power/power_supply_sysfs.c
@@ -74,9 +74,9 @@ static ssize_t power_supply_show_property(struct device *dev,
 	union power_supply_propval value;
 
 	if (off == POWER_SUPPLY_PROP_TYPE) {
-		value.intval = psy->type;
+		value.intval = psy->desc->type;
 	} else {
-		ret = psy->get_property(psy, off, &value);
+		ret = power_supply_get_property(psy, off, &value);
 
 		if (ret < 0) {
 			if (ret == -ENODATA)
@@ -125,7 +125,7 @@ static ssize_t power_supply_store_property(struct device *dev,
 
 	value.intval = long_val;
 
-	ret = psy->set_property(psy, off, &value);
+	ret = psy->desc->set_property(psy, off, &value);
 	if (ret < 0)
 		return ret;
 
@@ -218,12 +218,12 @@ static umode_t power_supply_attr_is_visible(struct kobject *kobj,
 	if (attrno == POWER_SUPPLY_PROP_TYPE)
 		return mode;
 
-	for (i = 0; i < psy->num_properties; i++) {
-		int property = psy->properties[i];
+	for (i = 0; i < psy->desc->num_properties; i++) {
+		int property = psy->desc->properties[i];
 
 		if (property == attrno) {
-			if (psy->property_is_writeable &&
-			    psy->property_is_writeable(psy, property) > 0)
+			if (psy->desc->property_is_writeable &&
+			    power_supply_property_is_writeable(psy, property) > 0)
 				mode |= S_IWUSR;
 
 			return mode;
@@ -279,14 +279,14 @@ int power_supply_uevent(struct device *dev, struct kobj_uevent_env *env)
 
 	dev_dbg(dev, "uevent\n");
 
-	if (!psy || !psy->dev) {
+	if (!psy || !psy->desc) {
 		dev_dbg(dev, "No power supply yet\n");
 		return ret;
 	}
 
-	dev_dbg(dev, "POWER_SUPPLY_NAME=%s\n", psy->name);
+	dev_dbg(dev, "POWER_SUPPLY_NAME=%s\n", psy->desc->name);
 
-	ret = add_uevent_var(env, "POWER_SUPPLY_NAME=%s", psy->name);
+	ret = add_uevent_var(env, "POWER_SUPPLY_NAME=%s", psy->desc->name);
 	if (ret)
 		return ret;
 
@@ -294,11 +294,11 @@ int power_supply_uevent(struct device *dev, struct kobj_uevent_env *env)
 	if (!prop_buf)
 		return -ENOMEM;
 
-	for (j = 0; j < psy->num_properties; j++) {
+	for (j = 0; j < psy->desc->num_properties; j++) {
 		struct device_attribute *attr;
 		char *line;
 
-		attr = &power_supply_attrs[psy->properties[j]];
+		attr = &power_supply_attrs[psy->desc->properties[j]];
 
 		ret = power_supply_show_property(dev, attr, prop_buf);
 		if (ret == -ENODEV || ret == -ENODATA) {
diff --git a/drivers/power/reset/Kconfig b/drivers/power/reset/Kconfig
index 27f6646731b0..aad9c3318c02 100644
--- a/drivers/power/reset/Kconfig
+++ b/drivers/power/reset/Kconfig
@@ -151,9 +151,17 @@ config POWER_RESET_SYSCON
 	help
 	  Reboot support for generic SYSCON mapped register reset.
 
+config POWER_RESET_SYSCON_POWEROFF
+	bool "Generic SYSCON regmap poweroff driver"
+	depends on OF
+	select MFD_SYSCON
+	help
+	  Poweroff support for generic SYSCON mapped register poweroff.
+
 config POWER_RESET_RMOBILE
 	tristate "Renesas R-Mobile reset driver"
 	depends on ARCH_RMOBILE || COMPILE_TEST
+	depends on HAS_IOMEM
 	help
 	  Reboot support for Renesas R-Mobile and SH-Mobile SoCs.
 
diff --git a/drivers/power/reset/Makefile b/drivers/power/reset/Makefile
index 11de15bae52e..dbe06c368743 100644
--- a/drivers/power/reset/Makefile
+++ b/drivers/power/reset/Makefile
@@ -17,4 +17,5 @@ obj-$(CONFIG_POWER_RESET_VEXPRESS) += vexpress-poweroff.o
 obj-$(CONFIG_POWER_RESET_XGENE) += xgene-reboot.o
 obj-$(CONFIG_POWER_RESET_KEYSTONE) += keystone-reset.o
 obj-$(CONFIG_POWER_RESET_SYSCON) += syscon-reboot.o
+obj-$(CONFIG_POWER_RESET_SYSCON_POWEROFF) += syscon-poweroff.o
 obj-$(CONFIG_POWER_RESET_RMOBILE) += rmobile-reset.o
diff --git a/drivers/power/reset/at91-poweroff.c b/drivers/power/reset/at91-poweroff.c
index 4b72ea51c364..9847cfb7e23d 100644
--- a/drivers/power/reset/at91-poweroff.c
+++ b/drivers/power/reset/at91-poweroff.c
@@ -140,7 +140,7 @@ static int at91_poweroff_probe(struct platform_device *pdev)
 	return 0;
 }
 
-static struct of_device_id at91_poweroff_of_match[] = {
+static const struct of_device_id at91_poweroff_of_match[] = {
 	{ .compatible = "atmel,at91sam9260-shdwc", },
 	{ .compatible = "atmel,at91sam9rl-shdwc", },
 	{ .compatible = "atmel,at91sam9x5-shdwc", },
diff --git a/drivers/power/reset/at91-reset.c b/drivers/power/reset/at91-reset.c
index 13584e24736a..01c7055c4200 100644
--- a/drivers/power/reset/at91-reset.c
+++ b/drivers/power/reset/at91-reset.c
@@ -73,8 +73,8 @@ static int at91sam9260_restart(struct notifier_block *this, unsigned long mode,
 		: "r" (at91_ramc_base[0]),
 		  "r" (at91_rstc_base),
 		  "r" (1),
-		  "r" (AT91_SDRAMC_LPCB_POWER_DOWN),
-		  "r" (AT91_RSTC_KEY | AT91_RSTC_PERRST | AT91_RSTC_PROCRST));
+		  "r" cpu_to_le32(AT91_SDRAMC_LPCB_POWER_DOWN),
+		  "r" cpu_to_le32(AT91_RSTC_KEY | AT91_RSTC_PERRST | AT91_RSTC_PROCRST));
 
 	return NOTIFY_DONE;
 }
@@ -116,8 +116,8 @@ static int at91sam9g45_restart(struct notifier_block *this, unsigned long mode,
 		  "r" (at91_ramc_base[1]),
 		  "r" (at91_rstc_base),
 		  "r" (1),
-		  "r" (AT91_DDRSDRC_LPCB_POWER_DOWN),
-		  "r" (AT91_RSTC_KEY | AT91_RSTC_PERRST | AT91_RSTC_PROCRST)
+		  "r" cpu_to_le32(AT91_DDRSDRC_LPCB_POWER_DOWN),
+		  "r" cpu_to_le32(AT91_RSTC_KEY | AT91_RSTC_PERRST | AT91_RSTC_PROCRST)
 		: "r0");
 
 	return NOTIFY_DONE;
@@ -152,14 +152,14 @@ static void __init at91_reset_status(struct platform_device *pdev)
 	pr_info("AT91: Starting after %s\n", reason);
 }
 
-static struct of_device_id at91_ramc_of_match[] = {
+static const struct of_device_id at91_ramc_of_match[] = {
 	{ .compatible = "atmel,at91sam9260-sdramc", },
 	{ .compatible = "atmel,at91sam9g45-ddramc", },
 	{ .compatible = "atmel,sama5d3-ddramc", },
 	{ /* sentinel */ }
 };
 
-static struct of_device_id at91_reset_of_match[] = {
+static const struct of_device_id at91_reset_of_match[] = {
 	{ .compatible = "atmel,at91sam9260-rstc", .data = at91sam9260_restart },
 	{ .compatible = "atmel,at91sam9g45-rstc", .data = at91sam9g45_restart },
 	{ /* sentinel */ }
diff --git a/drivers/power/reset/hisi-reboot.c b/drivers/power/reset/hisi-reboot.c
index 5385460e23bb..9ab7f562a83b 100644
--- a/drivers/power/reset/hisi-reboot.c
+++ b/drivers/power/reset/hisi-reboot.c
@@ -64,7 +64,7 @@ static int hisi_reboot_probe(struct platform_device *pdev)
 	return err;
 }
 
-static struct of_device_id hisi_reboot_of_match[] = {
+static const struct of_device_id hisi_reboot_of_match[] = {
 	{ .compatible = "hisilicon,sysctrl" },
 	{}
 };
diff --git a/drivers/power/reset/keystone-reset.c b/drivers/power/reset/keystone-reset.c
index faedf16c8111..c70f1bffe038 100644
--- a/drivers/power/reset/keystone-reset.c
+++ b/drivers/power/reset/keystone-reset.c
@@ -70,7 +70,7 @@ static struct notifier_block rsctrl_restart_nb = {
 	.priority = 128,
 };
 
-static struct of_device_id rsctrl_of_match[] = {
+static const struct of_device_id rsctrl_of_match[] = {
 	{.compatible = "ti,keystone-reset", },
 	{},
 };
diff --git a/drivers/power/reset/st-poweroff.c b/drivers/power/reset/st-poweroff.c
index 27383de9caa8..a488877a3538 100644
--- a/drivers/power/reset/st-poweroff.c
+++ b/drivers/power/reset/st-poweroff.c
@@ -97,7 +97,7 @@ static struct notifier_block st_restart_nb = {
 	.priority = 192,
 };
 
-static struct of_device_id st_reset_of_match[] = {
+static const struct of_device_id st_reset_of_match[] = {
 	{
 		.compatible = "st,stih415-restart",
 		.data = (void *)&stih415_reset,
diff --git a/drivers/power/reset/syscon-poweroff.c b/drivers/power/reset/syscon-poweroff.c
new file mode 100644
index 000000000000..5560b0dbc180
--- /dev/null
+++ b/drivers/power/reset/syscon-poweroff.c
@@ -0,0 +1,102 @@
+/*
+ * Generic Syscon Poweroff Driver
+ *
+ * Copyright (c) 2015, National Instruments Corp.
+ * Author: Moritz Fischer <moritz.fischer@ettus.com>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/kallsyms.h>
+#include <linux/delay.h>
+#include <linux/io.h>
+#include <linux/notifier.h>
+#include <linux/mfd/syscon.h>
+#include <linux/of_address.h>
+#include <linux/of_device.h>
+#include <linux/platform_device.h>
+#include <linux/pm.h>
+#include <linux/regmap.h>
+
+static struct regmap *map;
+static u32 offset;
+static u32 mask;
+
+void syscon_poweroff(void)
+{
+	/* Issue the poweroff */
+	regmap_write(map, offset, mask);
+
+	mdelay(1000);
+
+	pr_emerg("Unable to poweroff system\n");
+}
+
+static int syscon_poweroff_probe(struct platform_device *pdev)
+{
+	char symname[KSYM_NAME_LEN];
+
+	map = syscon_regmap_lookup_by_phandle(pdev->dev.of_node, "regmap");
+	if (IS_ERR(map)) {
+		dev_err(&pdev->dev, "unable to get syscon");
+		return PTR_ERR(map);
+	}
+
+	if (of_property_read_u32(pdev->dev.of_node, "offset", &offset)) {
+		dev_err(&pdev->dev, "unable to read 'offset'");
+		return -EINVAL;
+	}
+
+	if (of_property_read_u32(pdev->dev.of_node, "mask", &mask)) {
+		dev_err(&pdev->dev, "unable to read 'mask'");
+		return -EINVAL;
+	}
+
+	if (pm_power_off) {
+		lookup_symbol_name((ulong)pm_power_off, symname);
+		dev_err(&pdev->dev,
+		"pm_power_off already claimed %p %s",
+		pm_power_off, symname);
+		return -EBUSY;
+	}
+
+	pm_power_off = syscon_poweroff;
+
+	return 0;
+}
+
+static int syscon_poweroff_remove(struct platform_device *pdev)
+{
+	if (pm_power_off == syscon_poweroff)
+		pm_power_off = NULL;
+
+	return 0;
+}
+
+static const struct of_device_id syscon_poweroff_of_match[] = {
+	{ .compatible = "syscon-poweroff" },
+	{}
+};
+
+static struct platform_driver syscon_poweroff_driver = {
+	.probe = syscon_poweroff_probe,
+	.remove = syscon_poweroff_remove,
+	.driver = {
+		.name = "syscon-poweroff",
+		.of_match_table = syscon_poweroff_of_match,
+	},
+};
+
+static int __init syscon_poweroff_register(void)
+{
+	return platform_driver_register(&syscon_poweroff_driver);
+}
+device_initcall(syscon_poweroff_register);
diff --git a/drivers/power/reset/syscon-reboot.c b/drivers/power/reset/syscon-reboot.c
index c4049f45663f..d3c7d245ae63 100644
--- a/drivers/power/reset/syscon-reboot.c
+++ b/drivers/power/reset/syscon-reboot.c
@@ -76,7 +76,7 @@ static int syscon_reboot_probe(struct platform_device *pdev)
 	return err;
 }
 
-static struct of_device_id syscon_reboot_of_match[] = {
+static const struct of_device_id syscon_reboot_of_match[] = {
 	{ .compatible = "syscon-reboot" },
 	{}
 };
diff --git a/drivers/power/reset/vexpress-poweroff.c b/drivers/power/reset/vexpress-poweroff.c
index be12d9b92957..6a9bf7089373 100644
--- a/drivers/power/reset/vexpress-poweroff.c
+++ b/drivers/power/reset/vexpress-poweroff.c
@@ -80,7 +80,7 @@ DEVICE_ATTR(active, S_IRUGO | S_IWUSR, vexpress_reset_active_show,
 
 enum vexpress_reset_func { FUNC_RESET, FUNC_SHUTDOWN, FUNC_REBOOT };
 
-static struct of_device_id vexpress_reset_of_match[] = {
+static const struct of_device_id vexpress_reset_of_match[] = {
 	{
 		.compatible = "arm,vexpress-reset",
 		.data = (void *)FUNC_RESET,
diff --git a/drivers/power/reset/xgene-reboot.c b/drivers/power/reset/xgene-reboot.c
index b0e5002f8deb..f07e93c97ba3 100644
--- a/drivers/power/reset/xgene-reboot.c
+++ b/drivers/power/reset/xgene-reboot.c
@@ -87,7 +87,7 @@ static int xgene_reboot_probe(struct platform_device *pdev)
 	return err;
 }
 
-static struct of_device_id xgene_reboot_of_match[] = {
+static const struct of_device_id xgene_reboot_of_match[] = {
 	{ .compatible = "apm,xgene-reboot" },
 	{}
 };
diff --git a/drivers/power/rt5033_battery.c b/drivers/power/rt5033_battery.c
index 7b898f41c595..a7a6877b4e16 100644
--- a/drivers/power/rt5033_battery.c
+++ b/drivers/power/rt5033_battery.c
@@ -72,8 +72,7 @@ static int rt5033_battery_get_property(struct power_supply *psy,
 		enum power_supply_property psp,
 		union power_supply_propval *val)
 {
-	struct rt5033_battery *battery = container_of(psy,
-				struct rt5033_battery, psy);
+	struct rt5033_battery *battery = power_supply_get_drvdata(psy);
 
 	switch (psp) {
 	case POWER_SUPPLY_PROP_VOLTAGE_NOW:
@@ -102,16 +101,25 @@ static enum power_supply_property rt5033_battery_props[] = {
 	POWER_SUPPLY_PROP_CAPACITY,
 };
 
-static struct regmap_config rt5033_battery_regmap_config = {
+static const struct regmap_config rt5033_battery_regmap_config = {
 	.reg_bits	= 8,
 	.val_bits	= 8,
 	.max_register	= RT5033_FUEL_REG_END,
 };
 
+static const struct power_supply_desc rt5033_battery_desc = {
+	.name		= "rt5033-battery",
+	.type		= POWER_SUPPLY_TYPE_BATTERY,
+	.get_property	= rt5033_battery_get_property,
+	.properties	= rt5033_battery_props,
+	.num_properties	= ARRAY_SIZE(rt5033_battery_props),
+};
+
 static int rt5033_battery_probe(struct i2c_client *client,
 		const struct i2c_device_id *id)
 {
 	struct i2c_adapter *adapter = to_i2c_adapter(client->dev.parent);
+	struct power_supply_config psy_cfg = {};
 	struct rt5033_battery *battery;
 	u32 ret;
 
@@ -131,16 +139,13 @@ static int rt5033_battery_probe(struct i2c_client *client,
 	}
 
 	i2c_set_clientdata(client, battery);
+	psy_cfg.drv_data = battery;
 
-	battery->psy.name		= "rt5033-battery";
-	battery->psy.type		= POWER_SUPPLY_TYPE_BATTERY;
-	battery->psy.get_property	= rt5033_battery_get_property;
-	battery->psy.properties		= rt5033_battery_props;
-	battery->psy.num_properties	= ARRAY_SIZE(rt5033_battery_props);
-
-	ret = power_supply_register(&client->dev, &battery->psy);
-	if (ret) {
+	battery->psy = power_supply_register(&client->dev,
+					     &rt5033_battery_desc, &psy_cfg);
+	if (IS_ERR(battery->psy)) {
 		dev_err(&client->dev, "Failed to register power supply\n");
+		ret = PTR_ERR(battery->psy);
 		return ret;
 	}
 
@@ -151,7 +156,7 @@ static int rt5033_battery_remove(struct i2c_client *client)
 {
 	struct rt5033_battery *battery = i2c_get_clientdata(client);
 
-	power_supply_unregister(&battery->psy);
+	power_supply_unregister(battery->psy);
 
 	return 0;
 }
diff --git a/drivers/power/rx51_battery.c b/drivers/power/rx51_battery.c
index a01aacb32f59..ac6206951d58 100644
--- a/drivers/power/rx51_battery.c
+++ b/drivers/power/rx51_battery.c
@@ -29,7 +29,8 @@
 
 struct rx51_device_info {
 	struct device *dev;
-	struct power_supply bat;
+	struct power_supply *bat;
+	struct power_supply_desc bat_desc;
 	struct iio_channel *channel_temp;
 	struct iio_channel *channel_bsi;
 	struct iio_channel *channel_vbat;
@@ -161,8 +162,7 @@ static int rx51_battery_get_property(struct power_supply *psy,
 					enum power_supply_property psp,
 					union power_supply_propval *val)
 {
-	struct rx51_device_info *di = container_of((psy),
-				struct rx51_device_info, bat);
+	struct rx51_device_info *di = power_supply_get_drvdata(psy);
 
 	switch (psp) {
 	case POWER_SUPPLY_PROP_TECHNOLOGY:
@@ -204,6 +204,7 @@ static enum power_supply_property rx51_battery_props[] = {
 
 static int rx51_battery_probe(struct platform_device *pdev)
 {
+	struct power_supply_config psy_cfg = {};
 	struct rx51_device_info *di;
 	int ret;
 
@@ -214,11 +215,13 @@ static int rx51_battery_probe(struct platform_device *pdev)
 	platform_set_drvdata(pdev, di);
 
 	di->dev = &pdev->dev;
-	di->bat.name = dev_name(&pdev->dev);
-	di->bat.type = POWER_SUPPLY_TYPE_BATTERY;
-	di->bat.properties = rx51_battery_props;
-	di->bat.num_properties = ARRAY_SIZE(rx51_battery_props);
-	di->bat.get_property = rx51_battery_get_property;
+	di->bat_desc.name = dev_name(&pdev->dev);
+	di->bat_desc.type = POWER_SUPPLY_TYPE_BATTERY;
+	di->bat_desc.properties = rx51_battery_props;
+	di->bat_desc.num_properties = ARRAY_SIZE(rx51_battery_props);
+	di->bat_desc.get_property = rx51_battery_get_property;
+
+	psy_cfg.drv_data = di;
 
 	di->channel_temp = iio_channel_get(di->dev, "temp");
 	if (IS_ERR(di->channel_temp)) {
@@ -238,9 +241,11 @@ static int rx51_battery_probe(struct platform_device *pdev)
 		goto error_channel_bsi;
 	}
 
-	ret = power_supply_register(di->dev, &di->bat);
-	if (ret)
+	di->bat = power_supply_register(di->dev, &di->bat_desc, &psy_cfg);
+	if (IS_ERR(di->bat)) {
+		ret = PTR_ERR(di->bat);
 		goto error_channel_vbat;
+	}
 
 	return 0;
 
@@ -259,7 +264,7 @@ static int rx51_battery_remove(struct platform_device *pdev)
 {
 	struct rx51_device_info *di = platform_get_drvdata(pdev);
 
-	power_supply_unregister(&di->bat);
+	power_supply_unregister(di->bat);
 
 	iio_channel_release(di->channel_vbat);
 	iio_channel_release(di->channel_bsi);
diff --git a/drivers/power/s3c_adc_battery.c b/drivers/power/s3c_adc_battery.c
index 5948ce058bdd..0ffe5cd3abf6 100644
--- a/drivers/power/s3c_adc_battery.c
+++ b/drivers/power/s3c_adc_battery.c
@@ -28,7 +28,7 @@
 #define JITTER_DELAY			500 /* ms */
 
 struct s3c_adc_bat {
-	struct power_supply		psy;
+	struct power_supply		*psy;
 	struct s3c_adc_client		*client;
 	struct s3c_adc_bat_pdata	*pdata;
 	int				volt_value;
@@ -73,10 +73,10 @@ static int s3c_adc_backup_bat_get_property(struct power_supply *psy,
 				enum power_supply_property psp,
 				union power_supply_propval *val)
 {
-	struct s3c_adc_bat *bat = container_of(psy, struct s3c_adc_bat, psy);
+	struct s3c_adc_bat *bat = power_supply_get_drvdata(psy);
 
 	if (!bat) {
-		dev_err(psy->dev, "%s: no battery infos ?!\n", __func__);
+		dev_err(&psy->dev, "%s: no battery infos ?!\n", __func__);
 		return -EINVAL;
 	}
 
@@ -105,17 +105,17 @@ static int s3c_adc_backup_bat_get_property(struct power_supply *psy,
 	}
 }
 
-static struct s3c_adc_bat backup_bat = {
-	.psy = {
-		.name		= "backup-battery",
-		.type		= POWER_SUPPLY_TYPE_BATTERY,
-		.properties	= s3c_adc_backup_bat_props,
-		.num_properties = ARRAY_SIZE(s3c_adc_backup_bat_props),
-		.get_property	= s3c_adc_backup_bat_get_property,
-		.use_for_apm	= 1,
-	},
+static const struct power_supply_desc backup_bat_desc = {
+	.name		= "backup-battery",
+	.type		= POWER_SUPPLY_TYPE_BATTERY,
+	.properties	= s3c_adc_backup_bat_props,
+	.num_properties = ARRAY_SIZE(s3c_adc_backup_bat_props),
+	.get_property	= s3c_adc_backup_bat_get_property,
+	.use_for_apm	= 1,
 };
 
+static struct s3c_adc_bat backup_bat;
+
 static enum power_supply_property s3c_adc_main_bat_props[] = {
 	POWER_SUPPLY_PROP_STATUS,
 	POWER_SUPPLY_PROP_CHARGE_FULL_DESIGN,
@@ -141,7 +141,7 @@ static int s3c_adc_bat_get_property(struct power_supply *psy,
 				    enum power_supply_property psp,
 				    union power_supply_propval *val)
 {
-	struct s3c_adc_bat *bat = container_of(psy, struct s3c_adc_bat, psy);
+	struct s3c_adc_bat *bat = power_supply_get_drvdata(psy);
 
 	int new_level;
 	int full_volt;
@@ -149,7 +149,7 @@ static int s3c_adc_bat_get_property(struct power_supply *psy,
 	unsigned int lut_size;
 
 	if (!bat) {
-		dev_err(psy->dev, "no battery infos ?!\n");
+		dev_err(&psy->dev, "no battery infos ?!\n");
 		return -EINVAL;
 	}
 
@@ -232,18 +232,18 @@ static int s3c_adc_bat_get_property(struct power_supply *psy,
 	}
 }
 
-static struct s3c_adc_bat main_bat = {
-	.psy = {
-		.name			= "main-battery",
-		.type			= POWER_SUPPLY_TYPE_BATTERY,
-		.properties		= s3c_adc_main_bat_props,
-		.num_properties		= ARRAY_SIZE(s3c_adc_main_bat_props),
-		.get_property		= s3c_adc_bat_get_property,
-		.external_power_changed = s3c_adc_bat_ext_power_changed,
-		.use_for_apm		= 1,
-	},
+static const struct power_supply_desc main_bat_desc = {
+	.name			= "main-battery",
+	.type			= POWER_SUPPLY_TYPE_BATTERY,
+	.properties		= s3c_adc_main_bat_props,
+	.num_properties		= ARRAY_SIZE(s3c_adc_main_bat_props),
+	.get_property		= s3c_adc_bat_get_property,
+	.external_power_changed = s3c_adc_bat_ext_power_changed,
+	.use_for_apm		= 1,
 };
 
+static struct s3c_adc_bat main_bat;
+
 static void s3c_adc_bat_work(struct work_struct *work)
 {
 	struct s3c_adc_bat *bat = &main_bat;
@@ -251,7 +251,7 @@ static void s3c_adc_bat_work(struct work_struct *work)
 	int is_plugged;
 	static int was_plugged;
 
-	is_plugged = power_supply_am_i_supplied(&bat->psy);
+	is_plugged = power_supply_am_i_supplied(bat->psy);
 	bat->cable_plugged = is_plugged;
 	if (is_plugged != was_plugged) {
 		was_plugged = is_plugged;
@@ -279,7 +279,7 @@ static void s3c_adc_bat_work(struct work_struct *work)
 		}
 	}
 
-	power_supply_changed(&bat->psy);
+	power_supply_changed(bat->psy);
 }
 
 static irqreturn_t s3c_adc_bat_charged(int irq, void *dev_id)
@@ -310,16 +310,25 @@ static int s3c_adc_bat_probe(struct platform_device *pdev)
 	main_bat.cable_plugged = 0;
 	main_bat.status = POWER_SUPPLY_STATUS_DISCHARGING;
 
-	ret = power_supply_register(&pdev->dev, &main_bat.psy);
-	if (ret)
+	main_bat.psy = power_supply_register(&pdev->dev, &main_bat_desc, NULL);
+	if (IS_ERR(main_bat.psy)) {
+		ret = PTR_ERR(main_bat.psy);
 		goto err_reg_main;
+	}
 	if (pdata->backup_volt_mult) {
+		const struct power_supply_config psy_cfg
+						= { .drv_data = &backup_bat, };
+
 		backup_bat.client = client;
 		backup_bat.pdata = pdev->dev.platform_data;
 		backup_bat.volt_value = -1;
-		ret = power_supply_register(&pdev->dev, &backup_bat.psy);
-		if (ret)
+		backup_bat.psy = power_supply_register(&pdev->dev,
+						       &backup_bat_desc,
+						       &psy_cfg);
+		if (IS_ERR(backup_bat.psy)) {
+			ret = PTR_ERR(backup_bat.psy);
 			goto err_reg_backup;
+		}
 	}
 
 	INIT_DELAYED_WORK(&bat_work, s3c_adc_bat_work);
@@ -360,9 +369,9 @@ err_irq:
 		gpio_free(pdata->gpio_charge_finished);
 err_gpio:
 	if (pdata->backup_volt_mult)
-		power_supply_unregister(&backup_bat.psy);
+		power_supply_unregister(backup_bat.psy);
 err_reg_backup:
-	power_supply_unregister(&main_bat.psy);
+	power_supply_unregister(main_bat.psy);
 err_reg_main:
 	return ret;
 }
@@ -372,9 +381,9 @@ static int s3c_adc_bat_remove(struct platform_device *pdev)
 	struct s3c_adc_client *client = platform_get_drvdata(pdev);
 	struct s3c_adc_bat_pdata *pdata = pdev->dev.platform_data;
 
-	power_supply_unregister(&main_bat.psy);
+	power_supply_unregister(main_bat.psy);
 	if (pdata->backup_volt_mult)
-		power_supply_unregister(&backup_bat.psy);
+		power_supply_unregister(backup_bat.psy);
 
 	s3c_adc_release(client);
 
diff --git a/drivers/power/sbs-battery.c b/drivers/power/sbs-battery.c
index c7b7b4018df3..de1178659d4b 100644
--- a/drivers/power/sbs-battery.c
+++ b/drivers/power/sbs-battery.c
@@ -156,7 +156,7 @@ static enum power_supply_property sbs_properties[] = {
 
 struct sbs_info {
 	struct i2c_client		*client;
-	struct power_supply		power_supply;
+	struct power_supply		*power_supply;
 	struct sbs_platform_data	*pdata;
 	bool				is_present;
 	bool				gpio_detect;
@@ -391,7 +391,7 @@ static int sbs_get_battery_property(struct i2c_client *client,
 			chip->last_state = val->intval;
 		else if (chip->last_state != val->intval) {
 			cancel_delayed_work_sync(&chip->work);
-			power_supply_changed(&chip->power_supply);
+			power_supply_changed(chip->power_supply);
 			chip->poll_time = 0;
 		}
 	} else {
@@ -556,8 +556,7 @@ static int sbs_get_property(struct power_supply *psy,
 	union power_supply_propval *val)
 {
 	int ret = 0;
-	struct sbs_info *chip = container_of(psy,
-				struct sbs_info, power_supply);
+	struct sbs_info *chip = power_supply_get_drvdata(psy);
 	struct i2c_client *client = chip->client;
 
 	switch (psp) {
@@ -638,7 +637,7 @@ static int sbs_get_property(struct power_supply *psy,
 	if (!chip->gpio_detect &&
 		chip->is_present != (ret >= 0)) {
 		chip->is_present = (ret >= 0);
-		power_supply_changed(&chip->power_supply);
+		power_supply_changed(chip->power_supply);
 	}
 
 done:
@@ -671,9 +670,7 @@ static irqreturn_t sbs_irq(int irq, void *devid)
 
 static void sbs_external_power_changed(struct power_supply *psy)
 {
-	struct sbs_info *chip;
-
-	chip = container_of(psy, struct sbs_info, power_supply);
+	struct sbs_info *chip = power_supply_get_drvdata(psy);
 
 	if (chip->ignore_changes > 0) {
 		chip->ignore_changes--;
@@ -712,7 +709,7 @@ static void sbs_delayed_work(struct work_struct *work)
 
 	if (chip->last_state != ret) {
 		chip->poll_time = 0;
-		power_supply_changed(&chip->power_supply);
+		power_supply_changed(chip->power_supply);
 		return;
 	}
 	if (chip->poll_time > 0) {
@@ -796,42 +793,48 @@ static struct sbs_platform_data *sbs_of_populate_pdata(
 }
 #endif
 
+static const struct power_supply_desc sbs_default_desc = {
+	.type = POWER_SUPPLY_TYPE_BATTERY,
+	.properties = sbs_properties,
+	.num_properties = ARRAY_SIZE(sbs_properties),
+	.get_property = sbs_get_property,
+	.external_power_changed = sbs_external_power_changed,
+};
+
 static int sbs_probe(struct i2c_client *client,
 	const struct i2c_device_id *id)
 {
 	struct sbs_info *chip;
+	struct power_supply_desc *sbs_desc;
 	struct sbs_platform_data *pdata = client->dev.platform_data;
+	struct power_supply_config psy_cfg = {};
 	int rc;
 	int irq;
-	char *name;
 
-	name = kasprintf(GFP_KERNEL, "sbs-%s", dev_name(&client->dev));
-	if (!name) {
-		dev_err(&client->dev, "Failed to allocate device name\n");
+	sbs_desc = devm_kmemdup(&client->dev, &sbs_default_desc,
+			sizeof(*sbs_desc), GFP_KERNEL);
+	if (!sbs_desc)
+		return -ENOMEM;
+
+	sbs_desc->name = devm_kasprintf(&client->dev, GFP_KERNEL, "sbs-%s",
+			dev_name(&client->dev));
+	if (!sbs_desc->name)
 		return -ENOMEM;
-	}
 
 	chip = kzalloc(sizeof(struct sbs_info), GFP_KERNEL);
-	if (!chip) {
-		rc = -ENOMEM;
-		goto exit_free_name;
-	}
+	if (!chip)
+		return -ENOMEM;
 
 	chip->client = client;
 	chip->enable_detection = false;
 	chip->gpio_detect = false;
-	chip->power_supply.name = name;
-	chip->power_supply.type = POWER_SUPPLY_TYPE_BATTERY;
-	chip->power_supply.properties = sbs_properties;
-	chip->power_supply.num_properties = ARRAY_SIZE(sbs_properties);
-	chip->power_supply.get_property = sbs_get_property;
-	chip->power_supply.of_node = client->dev.of_node;
+	psy_cfg.of_node = client->dev.of_node;
+	psy_cfg.drv_data = chip;
 	/* ignore first notification of external change, it is generated
 	 * from the power_supply_register call back
 	 */
 	chip->ignore_changes = 1;
 	chip->last_state = POWER_SUPPLY_STATUS_UNKNOWN;
-	chip->power_supply.external_power_changed = sbs_external_power_changed;
 
 	pdata = sbs_of_populate_pdata(client);
 
@@ -870,7 +873,7 @@ static int sbs_probe(struct i2c_client *client,
 
 	rc = request_irq(irq, sbs_irq,
 		IRQF_TRIGGER_RISING | IRQF_TRIGGER_FALLING,
-		dev_name(&client->dev), &chip->power_supply);
+		dev_name(&client->dev), chip->power_supply);
 	if (rc) {
 		dev_warn(&client->dev, "Failed to request irq: %d\n", rc);
 		gpio_free(pdata->battery_detect);
@@ -892,10 +895,12 @@ skip_gpio:
 		goto exit_psupply;
 	}
 
-	rc = power_supply_register(&client->dev, &chip->power_supply);
-	if (rc) {
+	chip->power_supply = power_supply_register(&client->dev, sbs_desc,
+						   &psy_cfg);
+	if (IS_ERR(chip->power_supply)) {
 		dev_err(&client->dev,
 			"%s: Failed to register power supply\n", __func__);
+		rc = PTR_ERR(chip->power_supply);
 		goto exit_psupply;
 	}
 
@@ -910,15 +915,12 @@ skip_gpio:
 
 exit_psupply:
 	if (chip->irq)
-		free_irq(chip->irq, &chip->power_supply);
+		free_irq(chip->irq, chip->power_supply);
 	if (chip->gpio_detect)
 		gpio_free(pdata->battery_detect);
 
 	kfree(chip);
 
-exit_free_name:
-	kfree(name);
-
 	return rc;
 }
 
@@ -927,15 +929,14 @@ static int sbs_remove(struct i2c_client *client)
 	struct sbs_info *chip = i2c_get_clientdata(client);
 
 	if (chip->irq)
-		free_irq(chip->irq, &chip->power_supply);
+		free_irq(chip->irq, chip->power_supply);
 	if (chip->gpio_detect)
 		gpio_free(chip->pdata->battery_detect);
 
-	power_supply_unregister(&chip->power_supply);
+	power_supply_unregister(chip->power_supply);
 
 	cancel_delayed_work_sync(&chip->work);
 
-	kfree(chip->power_supply.name);
 	kfree(chip);
 	chip = NULL;
 
diff --git a/drivers/power/smb347-charger.c b/drivers/power/smb347-charger.c
index acf84e80fe98..0b60a0b5878b 100644
--- a/drivers/power/smb347-charger.c
+++ b/drivers/power/smb347-charger.c
@@ -139,9 +139,9 @@ struct smb347_charger {
 	struct mutex		lock;
 	struct device		*dev;
 	struct regmap		*regmap;
-	struct power_supply	mains;
-	struct power_supply	usb;
-	struct power_supply	battery;
+	struct power_supply	*mains;
+	struct power_supply	*usb;
+	struct power_supply	*battery;
 	bool			mains_online;
 	bool			usb_online;
 	bool			charging_enabled;
@@ -741,7 +741,7 @@ static irqreturn_t smb347_interrupt(int irq, void *data)
 	 */
 	if (stat_c & STAT_C_CHARGER_ERROR) {
 		dev_err(smb->dev, "charging stopped due to charger error\n");
-		power_supply_changed(&smb->battery);
+		power_supply_changed(smb->battery);
 		handled = true;
 	}
 
@@ -752,7 +752,7 @@ static irqreturn_t smb347_interrupt(int irq, void *data)
 	 */
 	if (irqstat_c & (IRQSTAT_C_TERMINATION_IRQ | IRQSTAT_C_TAPER_IRQ)) {
 		if (irqstat_c & IRQSTAT_C_TERMINATION_STAT)
-			power_supply_changed(&smb->battery);
+			power_supply_changed(smb->battery);
 		dev_dbg(smb->dev, "going to HW maintenance mode\n");
 		handled = true;
 	}
@@ -766,7 +766,7 @@ static irqreturn_t smb347_interrupt(int irq, void *data)
 
 		if (irqstat_d & IRQSTAT_D_CHARGE_TIMEOUT_STAT)
 			dev_warn(smb->dev, "charging stopped due to timeout\n");
-		power_supply_changed(&smb->battery);
+		power_supply_changed(smb->battery);
 		handled = true;
 	}
 
@@ -778,9 +778,9 @@ static irqreturn_t smb347_interrupt(int irq, void *data)
 		if (smb347_update_ps_status(smb) > 0) {
 			smb347_start_stop_charging(smb);
 			if (smb->pdata->use_mains)
-				power_supply_changed(&smb->mains);
+				power_supply_changed(smb->mains);
 			if (smb->pdata->use_usb)
-				power_supply_changed(&smb->usb);
+				power_supply_changed(smb->usb);
 		}
 		handled = true;
 	}
@@ -842,7 +842,8 @@ static int smb347_irq_init(struct smb347_charger *smb,
 		goto fail;
 
 	ret = request_threaded_irq(irq, NULL, smb347_interrupt,
-				   IRQF_TRIGGER_FALLING, client->name, smb);
+				   IRQF_TRIGGER_FALLING | IRQF_ONESHOT,
+				   client->name, smb);
 	if (ret < 0)
 		goto fail_gpio;
 
@@ -934,8 +935,7 @@ static int smb347_mains_get_property(struct power_supply *psy,
 				     enum power_supply_property prop,
 				     union power_supply_propval *val)
 {
-	struct smb347_charger *smb =
-		container_of(psy, struct smb347_charger, mains);
+	struct smb347_charger *smb = power_supply_get_drvdata(psy);
 	int ret;
 
 	switch (prop) {
@@ -976,8 +976,7 @@ static int smb347_usb_get_property(struct power_supply *psy,
 				   enum power_supply_property prop,
 				   union power_supply_propval *val)
 {
-	struct smb347_charger *smb =
-		container_of(psy, struct smb347_charger, usb);
+	struct smb347_charger *smb = power_supply_get_drvdata(psy);
 	int ret;
 
 	switch (prop) {
@@ -1063,8 +1062,7 @@ static int smb347_battery_get_property(struct power_supply *psy,
 				       enum power_supply_property prop,
 				       union power_supply_propval *val)
 {
-	struct smb347_charger *smb =
-			container_of(psy, struct smb347_charger, battery);
+	struct smb347_charger *smb = power_supply_get_drvdata(psy);
 	const struct smb347_charger_platform_data *pdata = smb->pdata;
 	int ret;
 
@@ -1188,11 +1186,36 @@ static const struct regmap_config smb347_regmap = {
 	.readable_reg	= smb347_readable_reg,
 };
 
+static const struct power_supply_desc smb347_mains_desc = {
+	.name		= "smb347-mains",
+	.type		= POWER_SUPPLY_TYPE_MAINS,
+	.get_property	= smb347_mains_get_property,
+	.properties	= smb347_mains_properties,
+	.num_properties	= ARRAY_SIZE(smb347_mains_properties),
+};
+
+static const struct power_supply_desc smb347_usb_desc = {
+	.name		= "smb347-usb",
+	.type		= POWER_SUPPLY_TYPE_USB,
+	.get_property	= smb347_usb_get_property,
+	.properties	= smb347_usb_properties,
+	.num_properties	= ARRAY_SIZE(smb347_usb_properties),
+};
+
+static const struct power_supply_desc smb347_battery_desc = {
+	.name		= "smb347-battery",
+	.type		= POWER_SUPPLY_TYPE_BATTERY,
+	.get_property	= smb347_battery_get_property,
+	.properties	= smb347_battery_properties,
+	.num_properties	= ARRAY_SIZE(smb347_battery_properties),
+};
+
 static int smb347_probe(struct i2c_client *client,
 			const struct i2c_device_id *id)
 {
 	static char *battery[] = { "smb347-battery" };
 	const struct smb347_charger_platform_data *pdata;
+	struct power_supply_config mains_usb_cfg = {}, battery_cfg = {};
 	struct device *dev = &client->dev;
 	struct smb347_charger *smb;
 	int ret;
@@ -1222,49 +1245,35 @@ static int smb347_probe(struct i2c_client *client,
 	if (ret < 0)
 		return ret;
 
+	mains_usb_cfg.supplied_to = battery;
+	mains_usb_cfg.num_supplicants = ARRAY_SIZE(battery);
+	mains_usb_cfg.drv_data = smb;
 	if (smb->pdata->use_mains) {
-		smb->mains.name = "smb347-mains";
-		smb->mains.type = POWER_SUPPLY_TYPE_MAINS;
-		smb->mains.get_property = smb347_mains_get_property;
-		smb->mains.properties = smb347_mains_properties;
-		smb->mains.num_properties = ARRAY_SIZE(smb347_mains_properties);
-		smb->mains.supplied_to = battery;
-		smb->mains.num_supplicants = ARRAY_SIZE(battery);
-		ret = power_supply_register(dev, &smb->mains);
-		if (ret < 0)
-			return ret;
+		smb->mains = power_supply_register(dev, &smb347_mains_desc,
+						   &mains_usb_cfg);
+		if (IS_ERR(smb->mains))
+			return PTR_ERR(smb->mains);
 	}
 
 	if (smb->pdata->use_usb) {
-		smb->usb.name = "smb347-usb";
-		smb->usb.type = POWER_SUPPLY_TYPE_USB;
-		smb->usb.get_property = smb347_usb_get_property;
-		smb->usb.properties = smb347_usb_properties;
-		smb->usb.num_properties = ARRAY_SIZE(smb347_usb_properties);
-		smb->usb.supplied_to = battery;
-		smb->usb.num_supplicants = ARRAY_SIZE(battery);
-		ret = power_supply_register(dev, &smb->usb);
-		if (ret < 0) {
+		smb->usb = power_supply_register(dev, &smb347_usb_desc,
+						 &mains_usb_cfg);
+		if (IS_ERR(smb->usb)) {
 			if (smb->pdata->use_mains)
-				power_supply_unregister(&smb->mains);
-			return ret;
+				power_supply_unregister(smb->mains);
+			return PTR_ERR(smb->usb);
 		}
 	}
 
-	smb->battery.name = "smb347-battery";
-	smb->battery.type = POWER_SUPPLY_TYPE_BATTERY;
-	smb->battery.get_property = smb347_battery_get_property;
-	smb->battery.properties = smb347_battery_properties;
-	smb->battery.num_properties = ARRAY_SIZE(smb347_battery_properties);
-
-
-	ret = power_supply_register(dev, &smb->battery);
-	if (ret < 0) {
+	battery_cfg.drv_data = smb;
+	smb->battery = power_supply_register(dev, &smb347_battery_desc,
+					     &battery_cfg);
+	if (IS_ERR(smb->battery)) {
 		if (smb->pdata->use_usb)
-			power_supply_unregister(&smb->usb);
+			power_supply_unregister(smb->usb);
 		if (smb->pdata->use_mains)
-			power_supply_unregister(&smb->mains);
-		return ret;
+			power_supply_unregister(smb->mains);
+		return PTR_ERR(smb->battery);
 	}
 
 	/*
@@ -1294,11 +1303,11 @@ static int smb347_remove(struct i2c_client *client)
 		gpio_free(smb->pdata->irq_gpio);
 	}
 
-	power_supply_unregister(&smb->battery);
+	power_supply_unregister(smb->battery);
 	if (smb->pdata->use_usb)
-		power_supply_unregister(&smb->usb);
+		power_supply_unregister(smb->usb);
 	if (smb->pdata->use_mains)
-		power_supply_unregister(&smb->mains);
+		power_supply_unregister(smb->mains);
 	return 0;
 }
 
diff --git a/drivers/power/test_power.c b/drivers/power/test_power.c
index f26b1fa00fe1..f986e0cca7ac 100644
--- a/drivers/power/test_power.c
+++ b/drivers/power/test_power.c
@@ -153,12 +153,12 @@ static char *test_power_ac_supplied_to[] = {
 	"test_battery",
 };
 
-static struct power_supply test_power_supplies[] = {
+static struct power_supply *test_power_supplies[TEST_POWER_NUM];
+
+static const struct power_supply_desc test_power_desc[] = {
 	[TEST_AC] = {
 		.name = "test_ac",
 		.type = POWER_SUPPLY_TYPE_MAINS,
-		.supplied_to = test_power_ac_supplied_to,
-		.num_supplicants = ARRAY_SIZE(test_power_ac_supplied_to),
 		.properties = test_power_ac_props,
 		.num_properties = ARRAY_SIZE(test_power_ac_props),
 		.get_property = test_power_get_ac_property,
@@ -173,14 +173,25 @@ static struct power_supply test_power_supplies[] = {
 	[TEST_USB] = {
 		.name = "test_usb",
 		.type = POWER_SUPPLY_TYPE_USB,
-		.supplied_to = test_power_ac_supplied_to,
-		.num_supplicants = ARRAY_SIZE(test_power_ac_supplied_to),
 		.properties = test_power_ac_props,
 		.num_properties = ARRAY_SIZE(test_power_ac_props),
 		.get_property = test_power_get_usb_property,
 	},
 };
 
+static const struct power_supply_config test_power_configs[] = {
+	{
+		/* test_ac */
+		.supplied_to = test_power_ac_supplied_to,
+		.num_supplicants = ARRAY_SIZE(test_power_ac_supplied_to),
+	}, {
+		/* test_battery */
+	}, {
+		/* test_usb */
+		.supplied_to = test_power_ac_supplied_to,
+		.num_supplicants = ARRAY_SIZE(test_power_ac_supplied_to),
+	},
+};
 
 static int __init test_power_init(void)
 {
@@ -188,12 +199,16 @@ static int __init test_power_init(void)
 	int ret;
 
 	BUILD_BUG_ON(TEST_POWER_NUM != ARRAY_SIZE(test_power_supplies));
+	BUILD_BUG_ON(TEST_POWER_NUM != ARRAY_SIZE(test_power_configs));
 
 	for (i = 0; i < ARRAY_SIZE(test_power_supplies); i++) {
-		ret = power_supply_register(NULL, &test_power_supplies[i]);
-		if (ret) {
+		test_power_supplies[i] = power_supply_register(NULL,
+						&test_power_desc[i],
+						&test_power_configs[i]);
+		if (IS_ERR(test_power_supplies[i])) {
 			pr_err("%s: failed to register %s\n", __func__,
-				test_power_supplies[i].name);
+				test_power_desc[i].name);
+			ret = PTR_ERR(test_power_supplies[i]);
 			goto failed;
 		}
 	}
@@ -202,7 +217,7 @@ static int __init test_power_init(void)
 	return 0;
 failed:
 	while (--i >= 0)
-		power_supply_unregister(&test_power_supplies[i]);
+		power_supply_unregister(test_power_supplies[i]);
 	return ret;
 }
 module_init(test_power_init);
@@ -216,13 +231,13 @@ static void __exit test_power_exit(void)
 	usb_online = 0;
 	battery_status = POWER_SUPPLY_STATUS_DISCHARGING;
 	for (i = 0; i < ARRAY_SIZE(test_power_supplies); i++)
-		power_supply_changed(&test_power_supplies[i]);
+		power_supply_changed(test_power_supplies[i]);
 	pr_info("%s: 'changed' event sent, sleeping for 10 seconds...\n",
 		__func__);
 	ssleep(10);
 
 	for (i = 0; i < ARRAY_SIZE(test_power_supplies); i++)
-		power_supply_unregister(&test_power_supplies[i]);
+		power_supply_unregister(test_power_supplies[i]);
 
 	module_initialized = false;
 }
@@ -320,7 +335,7 @@ static inline void signal_power_supply_changed(struct power_supply *psy)
 static int param_set_ac_online(const char *key, const struct kernel_param *kp)
 {
 	ac_online = map_get_value(map_ac_online, key, ac_online);
-	signal_power_supply_changed(&test_power_supplies[TEST_AC]);
+	signal_power_supply_changed(test_power_supplies[TEST_AC]);
 	return 0;
 }
 
@@ -333,7 +348,7 @@ static int param_get_ac_online(char *buffer, const struct kernel_param *kp)
 static int param_set_usb_online(const char *key, const struct kernel_param *kp)
 {
 	usb_online = map_get_value(map_ac_online, key, usb_online);
-	signal_power_supply_changed(&test_power_supplies[TEST_USB]);
+	signal_power_supply_changed(test_power_supplies[TEST_USB]);
 	return 0;
 }
 
@@ -347,7 +362,7 @@ static int param_set_battery_status(const char *key,
 					const struct kernel_param *kp)
 {
 	battery_status = map_get_value(map_status, key, battery_status);
-	signal_power_supply_changed(&test_power_supplies[TEST_BATTERY]);
+	signal_power_supply_changed(test_power_supplies[TEST_BATTERY]);
 	return 0;
 }
 
@@ -361,7 +376,7 @@ static int param_set_battery_health(const char *key,
 					const struct kernel_param *kp)
 {
 	battery_health = map_get_value(map_health, key, battery_health);
-	signal_power_supply_changed(&test_power_supplies[TEST_BATTERY]);
+	signal_power_supply_changed(test_power_supplies[TEST_BATTERY]);
 	return 0;
 }
 
@@ -375,7 +390,7 @@ static int param_set_battery_present(const char *key,
 					const struct kernel_param *kp)
 {
 	battery_present = map_get_value(map_present, key, battery_present);
-	signal_power_supply_changed(&test_power_supplies[TEST_AC]);
+	signal_power_supply_changed(test_power_supplies[TEST_AC]);
 	return 0;
 }
 
@@ -391,7 +406,7 @@ static int param_set_battery_technology(const char *key,
 {
 	battery_technology = map_get_value(map_technology, key,
 						battery_technology);
-	signal_power_supply_changed(&test_power_supplies[TEST_BATTERY]);
+	signal_power_supply_changed(test_power_supplies[TEST_BATTERY]);
 	return 0;
 }
 
@@ -412,7 +427,7 @@ static int param_set_battery_capacity(const char *key,
 		return -EINVAL;
 
 	battery_capacity = tmp;
-	signal_power_supply_changed(&test_power_supplies[TEST_BATTERY]);
+	signal_power_supply_changed(test_power_supplies[TEST_BATTERY]);
 	return 0;
 }
 
@@ -427,7 +442,7 @@ static int param_set_battery_voltage(const char *key,
 		return -EINVAL;
 
 	battery_voltage = tmp;
-	signal_power_supply_changed(&test_power_supplies[TEST_BATTERY]);
+	signal_power_supply_changed(test_power_supplies[TEST_BATTERY]);
 	return 0;
 }
 
diff --git a/drivers/power/tosa_battery.c b/drivers/power/tosa_battery.c
index f4d80df627c7..6e88c1b37945 100644
--- a/drivers/power/tosa_battery.c
+++ b/drivers/power/tosa_battery.c
@@ -26,7 +26,7 @@ static struct work_struct bat_work;
 
 struct tosa_bat {
 	int status;
-	struct power_supply psy;
+	struct power_supply *psy;
 	int full_chrg;
 
 	struct mutex work_lock; /* protects data */
@@ -61,7 +61,7 @@ static unsigned long tosa_read_bat(struct tosa_bat *bat)
 	mutex_lock(&bat_lock);
 	gpio_set_value(bat->gpio_bat, 1);
 	msleep(5);
-	value = wm97xx_read_aux_adc(dev_get_drvdata(bat->psy.dev->parent),
+	value = wm97xx_read_aux_adc(dev_get_drvdata(bat->psy->dev.parent),
 			bat->adc_bat);
 	gpio_set_value(bat->gpio_bat, 0);
 	mutex_unlock(&bat_lock);
@@ -81,7 +81,7 @@ static unsigned long tosa_read_temp(struct tosa_bat *bat)
 	mutex_lock(&bat_lock);
 	gpio_set_value(bat->gpio_temp, 1);
 	msleep(5);
-	value = wm97xx_read_aux_adc(dev_get_drvdata(bat->psy.dev->parent),
+	value = wm97xx_read_aux_adc(dev_get_drvdata(bat->psy->dev.parent),
 			bat->adc_temp);
 	gpio_set_value(bat->gpio_temp, 0);
 	mutex_unlock(&bat_lock);
@@ -96,7 +96,7 @@ static int tosa_bat_get_property(struct power_supply *psy,
 			    union power_supply_propval *val)
 {
 	int ret = 0;
-	struct tosa_bat *bat = container_of(psy, struct tosa_bat, psy);
+	struct tosa_bat *bat = power_supply_get_drvdata(psy);
 
 	if (bat->is_present && !bat->is_present(bat)
 			&& psp != POWER_SUPPLY_PROP_PRESENT) {
@@ -158,14 +158,14 @@ static irqreturn_t tosa_bat_gpio_isr(int irq, void *data)
 static void tosa_bat_update(struct tosa_bat *bat)
 {
 	int old;
-	struct power_supply *psy = &bat->psy;
+	struct power_supply *psy = bat->psy;
 
 	mutex_lock(&bat->work_lock);
 
 	old = bat->status;
 
 	if (bat->is_present && !bat->is_present(bat)) {
-		printk(KERN_NOTICE "%s not present\n", psy->name);
+		printk(KERN_NOTICE "%s not present\n", psy->desc->name);
 		bat->status = POWER_SUPPLY_STATUS_UNKNOWN;
 		bat->full_chrg = -1;
 	} else if (power_supply_am_i_supplied(psy)) {
@@ -222,18 +222,38 @@ static enum power_supply_property tosa_bat_bu_props[] = {
 	POWER_SUPPLY_PROP_PRESENT,
 };
 
+static const struct power_supply_desc tosa_bat_main_desc = {
+	.name		= "main-battery",
+	.type		= POWER_SUPPLY_TYPE_BATTERY,
+	.properties	= tosa_bat_main_props,
+	.num_properties	= ARRAY_SIZE(tosa_bat_main_props),
+	.get_property	= tosa_bat_get_property,
+	.external_power_changed = tosa_bat_external_power_changed,
+	.use_for_apm	= 1,
+};
+
+static const struct power_supply_desc tosa_bat_jacket_desc = {
+	.name		= "jacket-battery",
+	.type		= POWER_SUPPLY_TYPE_BATTERY,
+	.properties	= tosa_bat_main_props,
+	.num_properties	= ARRAY_SIZE(tosa_bat_main_props),
+	.get_property	= tosa_bat_get_property,
+	.external_power_changed = tosa_bat_external_power_changed,
+};
+
+static const struct power_supply_desc tosa_bat_bu_desc = {
+	.name		= "backup-battery",
+	.type		= POWER_SUPPLY_TYPE_BATTERY,
+	.properties	= tosa_bat_bu_props,
+	.num_properties	= ARRAY_SIZE(tosa_bat_bu_props),
+	.get_property	= tosa_bat_get_property,
+	.external_power_changed = tosa_bat_external_power_changed,
+};
+
 static struct tosa_bat tosa_bat_main = {
 	.status = POWER_SUPPLY_STATUS_DISCHARGING,
 	.full_chrg = -1,
-	.psy = {
-		.name		= "main-battery",
-		.type		= POWER_SUPPLY_TYPE_BATTERY,
-		.properties	= tosa_bat_main_props,
-		.num_properties	= ARRAY_SIZE(tosa_bat_main_props),
-		.get_property	= tosa_bat_get_property,
-		.external_power_changed = tosa_bat_external_power_changed,
-		.use_for_apm	= 1,
-	},
+	.psy = NULL,
 
 	.gpio_full = TOSA_GPIO_BAT0_CRG,
 	.gpio_charge_off = TOSA_GPIO_CHARGE_OFF,
@@ -254,14 +274,7 @@ static struct tosa_bat tosa_bat_main = {
 static struct tosa_bat tosa_bat_jacket = {
 	.status = POWER_SUPPLY_STATUS_DISCHARGING,
 	.full_chrg = -1,
-	.psy = {
-		.name		= "jacket-battery",
-		.type		= POWER_SUPPLY_TYPE_BATTERY,
-		.properties	= tosa_bat_main_props,
-		.num_properties	= ARRAY_SIZE(tosa_bat_main_props),
-		.get_property	= tosa_bat_get_property,
-		.external_power_changed = tosa_bat_external_power_changed,
-	},
+	.psy = NULL,
 
 	.is_present = tosa_jacket_bat_is_present,
 	.gpio_full = TOSA_GPIO_BAT1_CRG,
@@ -283,15 +296,7 @@ static struct tosa_bat tosa_bat_jacket = {
 static struct tosa_bat tosa_bat_bu = {
 	.status = POWER_SUPPLY_STATUS_UNKNOWN,
 	.full_chrg = -1,
-
-	.psy = {
-		.name		= "backup-battery",
-		.type		= POWER_SUPPLY_TYPE_BATTERY,
-		.properties	= tosa_bat_bu_props,
-		.num_properties	= ARRAY_SIZE(tosa_bat_bu_props),
-		.get_property	= tosa_bat_get_property,
-		.external_power_changed = tosa_bat_external_power_changed,
-	},
+	.psy = NULL,
 
 	.gpio_full = -1,
 	.gpio_charge_off = -1,
@@ -345,6 +350,9 @@ static int tosa_bat_resume(struct platform_device *dev)
 static int tosa_bat_probe(struct platform_device *dev)
 {
 	int ret;
+	struct power_supply_config main_psy_cfg = {},
+				   jacket_psy_cfg = {},
+				   bu_psy_cfg = {};
 
 	if (!machine_is_tosa())
 		return -ENODEV;
@@ -358,15 +366,31 @@ static int tosa_bat_probe(struct platform_device *dev)
 
 	INIT_WORK(&bat_work, tosa_bat_work);
 
-	ret = power_supply_register(&dev->dev, &tosa_bat_main.psy);
-	if (ret)
+	main_psy_cfg.drv_data = &tosa_bat_main;
+	tosa_bat_main.psy = power_supply_register(&dev->dev,
+						  &tosa_bat_main_desc,
+						  &main_psy_cfg);
+	if (IS_ERR(tosa_bat_main.psy)) {
+		ret = PTR_ERR(tosa_bat_main.psy);
 		goto err_psy_reg_main;
-	ret = power_supply_register(&dev->dev, &tosa_bat_jacket.psy);
-	if (ret)
+	}
+
+	jacket_psy_cfg.drv_data = &tosa_bat_jacket;
+	tosa_bat_jacket.psy = power_supply_register(&dev->dev,
+						    &tosa_bat_jacket_desc,
+						    &jacket_psy_cfg);
+	if (IS_ERR(tosa_bat_jacket.psy)) {
+		ret = PTR_ERR(tosa_bat_jacket.psy);
 		goto err_psy_reg_jacket;
-	ret = power_supply_register(&dev->dev, &tosa_bat_bu.psy);
-	if (ret)
+	}
+
+	bu_psy_cfg.drv_data = &tosa_bat_bu;
+	tosa_bat_bu.psy = power_supply_register(&dev->dev, &tosa_bat_bu_desc,
+						&bu_psy_cfg);
+	if (IS_ERR(tosa_bat_bu.psy)) {
+		ret = PTR_ERR(tosa_bat_bu.psy);
 		goto err_psy_reg_bu;
+	}
 
 	ret = request_irq(gpio_to_irq(TOSA_GPIO_BAT0_CRG),
 				tosa_bat_gpio_isr,
@@ -395,11 +419,11 @@ static int tosa_bat_probe(struct platform_device *dev)
 err_req_jacket:
 	free_irq(gpio_to_irq(TOSA_GPIO_BAT0_CRG), &tosa_bat_main);
 err_req_main:
-	power_supply_unregister(&tosa_bat_bu.psy);
+	power_supply_unregister(tosa_bat_bu.psy);
 err_psy_reg_bu:
-	power_supply_unregister(&tosa_bat_jacket.psy);
+	power_supply_unregister(tosa_bat_jacket.psy);
 err_psy_reg_jacket:
-	power_supply_unregister(&tosa_bat_main.psy);
+	power_supply_unregister(tosa_bat_main.psy);
 err_psy_reg_main:
 
 	/* see comment in tosa_bat_remove */
@@ -415,9 +439,9 @@ static int tosa_bat_remove(struct platform_device *dev)
 	free_irq(gpio_to_irq(TOSA_GPIO_BAT1_CRG), &tosa_bat_jacket);
 	free_irq(gpio_to_irq(TOSA_GPIO_BAT0_CRG), &tosa_bat_main);
 
-	power_supply_unregister(&tosa_bat_bu.psy);
-	power_supply_unregister(&tosa_bat_jacket.psy);
-	power_supply_unregister(&tosa_bat_main.psy);
+	power_supply_unregister(tosa_bat_bu.psy);
+	power_supply_unregister(tosa_bat_jacket.psy);
+	power_supply_unregister(tosa_bat_main.psy);
 
 	/*
 	 * Now cancel the bat_work.  We won't get any more schedules,
diff --git a/drivers/power/tps65090-charger.c b/drivers/power/tps65090-charger.c
index 0f4e5971dff5..7e8fbd29c30e 100644
--- a/drivers/power/tps65090-charger.c
+++ b/drivers/power/tps65090-charger.c
@@ -43,7 +43,7 @@ struct tps65090_charger {
 	int	irq;
 	struct task_struct	*poll_task;
 	bool			passive_mode;
-	struct power_supply	ac;
+	struct power_supply	*ac;
 	struct tps65090_platform_data *pdata;
 };
 
@@ -135,8 +135,7 @@ static int tps65090_ac_get_property(struct power_supply *psy,
 			enum power_supply_property psp,
 			union power_supply_propval *val)
 {
-	struct tps65090_charger *charger = container_of(psy,
-					struct tps65090_charger, ac);
+	struct tps65090_charger *charger = power_supply_get_drvdata(psy);
 
 	if (psp == POWER_SUPPLY_PROP_ONLINE) {
 		val->intval = charger->ac_online;
@@ -190,7 +189,7 @@ static irqreturn_t tps65090_charger_isr(int irq, void *dev_id)
 	}
 
 	if (charger->prev_ac_online != charger->ac_online)
-		power_supply_changed(&charger->ac);
+		power_supply_changed(charger->ac);
 
 	return IRQ_HANDLED;
 }
@@ -229,10 +228,19 @@ static int tps65090_charger_poll_task(void *data)
 	return 0;
 }
 
+static const struct power_supply_desc tps65090_charger_desc = {
+	.name			= "tps65090-ac",
+	.type			= POWER_SUPPLY_TYPE_MAINS,
+	.get_property		= tps65090_ac_get_property,
+	.properties		= tps65090_ac_props,
+	.num_properties		= ARRAY_SIZE(tps65090_ac_props),
+};
+
 static int tps65090_charger_probe(struct platform_device *pdev)
 {
 	struct tps65090_charger *cdata;
 	struct tps65090_platform_data *pdata;
+	struct power_supply_config psy_cfg = {};
 	uint8_t status1 = 0;
 	int ret;
 	int irq;
@@ -259,19 +267,16 @@ static int tps65090_charger_probe(struct platform_device *pdev)
 	cdata->dev			= &pdev->dev;
 	cdata->pdata			= pdata;
 
-	cdata->ac.name			= "tps65090-ac";
-	cdata->ac.type			= POWER_SUPPLY_TYPE_MAINS;
-	cdata->ac.get_property		= tps65090_ac_get_property;
-	cdata->ac.properties		= tps65090_ac_props;
-	cdata->ac.num_properties	= ARRAY_SIZE(tps65090_ac_props);
-	cdata->ac.supplied_to		= pdata->supplied_to;
-	cdata->ac.num_supplicants	= pdata->num_supplicants;
-	cdata->ac.of_node		= pdev->dev.of_node;
-
-	ret = power_supply_register(&pdev->dev, &cdata->ac);
-	if (ret) {
+	psy_cfg.supplied_to		= pdata->supplied_to;
+	psy_cfg.num_supplicants		= pdata->num_supplicants;
+	psy_cfg.of_node			= pdev->dev.of_node;
+	psy_cfg.drv_data		= cdata;
+
+	cdata->ac = power_supply_register(&pdev->dev, &tps65090_charger_desc,
+			&psy_cfg);
+	if (IS_ERR(cdata->ac)) {
 		dev_err(&pdev->dev, "failed: power supply register\n");
-		return ret;
+		return PTR_ERR(cdata->ac);
 	}
 
 	irq = platform_get_irq(pdev, 0);
@@ -301,7 +306,7 @@ static int tps65090_charger_probe(struct platform_device *pdev)
 			goto fail_unregister_supply;
 		}
 		cdata->ac_online = 1;
-		power_supply_changed(&cdata->ac);
+		power_supply_changed(cdata->ac);
 	}
 
 	if (irq != -ENXIO) {
@@ -328,7 +333,7 @@ static int tps65090_charger_probe(struct platform_device *pdev)
 	return 0;
 
 fail_unregister_supply:
-	power_supply_unregister(&cdata->ac);
+	power_supply_unregister(cdata->ac);
 
 	return ret;
 }
@@ -339,12 +344,12 @@ static int tps65090_charger_remove(struct platform_device *pdev)
 
 	if (cdata->irq == -ENXIO)
 		kthread_stop(cdata->poll_task);
-	power_supply_unregister(&cdata->ac);
+	power_supply_unregister(cdata->ac);
 
 	return 0;
 }
 
-static struct of_device_id of_tps65090_charger_match[] = {
+static const struct of_device_id of_tps65090_charger_match[] = {
 	{ .compatible = "ti,tps65090-charger", },
 	{ /* end */ }
 };
diff --git a/drivers/power/twl4030_charger.c b/drivers/power/twl4030_charger.c
index d35b83e635b5..02a522cb7753 100644
--- a/drivers/power/twl4030_charger.c
+++ b/drivers/power/twl4030_charger.c
@@ -87,8 +87,8 @@ MODULE_PARM_DESC(allow_usb, "Allow USB charge drawing default current");
 
 struct twl4030_bci {
 	struct device		*dev;
-	struct power_supply	ac;
-	struct power_supply	usb;
+	struct power_supply	*ac;
+	struct power_supply	*usb;
 	struct usb_phy		*transceiver;
 	struct notifier_block	usb_nb;
 	struct work_struct	work;
@@ -318,8 +318,8 @@ static irqreturn_t twl4030_charger_interrupt(int irq, void *arg)
 	struct twl4030_bci *bci = arg;
 
 	dev_dbg(bci->dev, "CHG_PRES irq\n");
-	power_supply_changed(&bci->ac);
-	power_supply_changed(&bci->usb);
+	power_supply_changed(bci->ac);
+	power_supply_changed(bci->usb);
 
 	return IRQ_HANDLED;
 }
@@ -347,8 +347,8 @@ static irqreturn_t twl4030_bci_interrupt(int irq, void *arg)
 
 	if (irqs1 & (TWL4030_ICHGLOW | TWL4030_ICHGEOC)) {
 		/* charger state change, inform the core */
-		power_supply_changed(&bci->ac);
-		power_supply_changed(&bci->usb);
+		power_supply_changed(bci->ac);
+		power_supply_changed(bci->usb);
 	}
 
 	/* various monitoring events, for now we just log them here */
@@ -463,7 +463,7 @@ static int twl4030_bci_get_property(struct power_supply *psy,
 				    enum power_supply_property psp,
 				    union power_supply_propval *val)
 {
-	struct twl4030_bci *bci = dev_get_drvdata(psy->dev->parent);
+	struct twl4030_bci *bci = dev_get_drvdata(psy->dev.parent);
 	int is_charging;
 	int state;
 	int ret;
@@ -472,7 +472,7 @@ static int twl4030_bci_get_property(struct power_supply *psy,
 	if (state < 0)
 		return state;
 
-	if (psy->type == POWER_SUPPLY_TYPE_USB)
+	if (psy->desc->type == POWER_SUPPLY_TYPE_USB)
 		is_charging = state & TWL4030_MSTATEC_USB;
 	else
 		is_charging = state & TWL4030_MSTATEC_AC;
@@ -488,7 +488,7 @@ static int twl4030_bci_get_property(struct power_supply *psy,
 		/* charging must be active for meaningful result */
 		if (!is_charging)
 			return -ENODATA;
-		if (psy->type == POWER_SUPPLY_TYPE_USB) {
+		if (psy->desc->type == POWER_SUPPLY_TYPE_USB) {
 			ret = twl4030bci_read_adc_val(TWL4030_BCIVBUS);
 			if (ret < 0)
 				return ret;
@@ -558,6 +558,22 @@ twl4030_bci_parse_dt(struct device *dev)
 }
 #endif
 
+static const struct power_supply_desc twl4030_bci_ac_desc = {
+	.name		= "twl4030_ac",
+	.type		= POWER_SUPPLY_TYPE_MAINS,
+	.properties	= twl4030_charger_props,
+	.num_properties	= ARRAY_SIZE(twl4030_charger_props),
+	.get_property	= twl4030_bci_get_property,
+};
+
+static const struct power_supply_desc twl4030_bci_usb_desc = {
+	.name		= "twl4030_usb",
+	.type		= POWER_SUPPLY_TYPE_USB,
+	.properties	= twl4030_charger_props,
+	.num_properties	= ARRAY_SIZE(twl4030_charger_props),
+	.get_property	= twl4030_bci_get_property,
+};
+
 static int __init twl4030_bci_probe(struct platform_device *pdev)
 {
 	struct twl4030_bci *bci;
@@ -584,28 +600,21 @@ static int __init twl4030_bci_probe(struct platform_device *pdev)
 	}
 
 	platform_set_drvdata(pdev, bci);
-	bci->ac.name = "twl4030_ac";
-	bci->ac.type = POWER_SUPPLY_TYPE_MAINS;
-	bci->ac.properties = twl4030_charger_props;
-	bci->ac.num_properties = ARRAY_SIZE(twl4030_charger_props);
-	bci->ac.get_property = twl4030_bci_get_property;
 
-	ret = power_supply_register(&pdev->dev, &bci->ac);
-	if (ret) {
+	bci->ac = power_supply_register(&pdev->dev, &twl4030_bci_ac_desc,
+					NULL);
+	if (IS_ERR(bci->ac)) {
+		ret = PTR_ERR(bci->ac);
 		dev_err(&pdev->dev, "failed to register ac: %d\n", ret);
 		goto fail_register_ac;
 	}
 
-	bci->usb.name = "twl4030_usb";
-	bci->usb.type = POWER_SUPPLY_TYPE_USB;
-	bci->usb.properties = twl4030_charger_props;
-	bci->usb.num_properties = ARRAY_SIZE(twl4030_charger_props);
-	bci->usb.get_property = twl4030_bci_get_property;
-
 	bci->usb_reg = regulator_get(bci->dev, "bci3v1");
 
-	ret = power_supply_register(&pdev->dev, &bci->usb);
-	if (ret) {
+	bci->usb = power_supply_register(&pdev->dev, &twl4030_bci_usb_desc,
+					 NULL);
+	if (IS_ERR(bci->usb)) {
+		ret = PTR_ERR(bci->usb);
 		dev_err(&pdev->dev, "failed to register usb: %d\n", ret);
 		goto fail_register_usb;
 	}
@@ -670,9 +679,9 @@ fail_unmask_interrupts:
 fail_bci_irq:
 	free_irq(bci->irq_chg, bci);
 fail_chg_irq:
-	power_supply_unregister(&bci->usb);
+	power_supply_unregister(bci->usb);
 fail_register_usb:
-	power_supply_unregister(&bci->ac);
+	power_supply_unregister(bci->ac);
 fail_register_ac:
 fail_no_battery:
 	kfree(bci);
@@ -700,8 +709,8 @@ static int __exit twl4030_bci_remove(struct platform_device *pdev)
 	}
 	free_irq(bci->irq_bci, bci);
 	free_irq(bci->irq_chg, bci);
-	power_supply_unregister(&bci->usb);
-	power_supply_unregister(&bci->ac);
+	power_supply_unregister(bci->usb);
+	power_supply_unregister(bci->ac);
 	kfree(bci);
 
 	return 0;
diff --git a/drivers/power/twl4030_madc_battery.c b/drivers/power/twl4030_madc_battery.c
index 7ef445a6cfa6..f5817e422d64 100644
--- a/drivers/power/twl4030_madc_battery.c
+++ b/drivers/power/twl4030_madc_battery.c
@@ -19,10 +19,14 @@
 #include <linux/sort.h>
 #include <linux/i2c/twl4030-madc.h>
 #include <linux/power/twl4030_madc_battery.h>
+#include <linux/iio/consumer.h>
 
 struct twl4030_madc_battery {
-	struct power_supply psy;
+	struct power_supply *psy;
 	struct twl4030_madc_bat_platform_data *pdata;
+	struct iio_channel *channel_temp;
+	struct iio_channel *channel_ichg;
+	struct iio_channel *channel_vbat;
 };
 
 static enum power_supply_property twl4030_madc_bat_props[] = {
@@ -38,43 +42,34 @@ static enum power_supply_property twl4030_madc_bat_props[] = {
 	POWER_SUPPLY_PROP_TIME_TO_EMPTY_NOW,
 };
 
-static int madc_read(int index)
+static int madc_read(struct iio_channel *channel)
 {
-	struct twl4030_madc_request req;
-	int val;
+	int val, err;
+	err = iio_read_channel_processed(channel, &val);
+	if (err < 0)
+		return err;
 
-	req.channels = index;
-	req.method = TWL4030_MADC_SW2;
-	req.type = TWL4030_MADC_WAIT;
-	req.do_avg = 0;
-	req.raw = false;
-	req.func_cb = NULL;
-
-	val = twl4030_madc_conversion(&req);
-	if (val < 0)
-		return val;
-
-	return req.rbuf[ffs(index) - 1];
+	return val;
 }
 
-static int twl4030_madc_bat_get_charging_status(void)
+static int twl4030_madc_bat_get_charging_status(struct twl4030_madc_battery *bt)
 {
-	return (madc_read(TWL4030_MADC_ICHG) > 0) ? 1 : 0;
+	return (madc_read(bt->channel_ichg) > 0) ? 1 : 0;
 }
 
-static int twl4030_madc_bat_get_voltage(void)
+static int twl4030_madc_bat_get_voltage(struct twl4030_madc_battery *bt)
 {
-	return madc_read(TWL4030_MADC_VBAT);
+	return madc_read(bt->channel_vbat);
 }
 
-static int twl4030_madc_bat_get_current(void)
+static int twl4030_madc_bat_get_current(struct twl4030_madc_battery *bt)
 {
-	return madc_read(TWL4030_MADC_ICHG) * 1000;
+	return madc_read(bt->channel_ichg) * 1000;
 }
 
-static int twl4030_madc_bat_get_temp(void)
+static int twl4030_madc_bat_get_temp(struct twl4030_madc_battery *bt)
 {
-	return madc_read(TWL4030_MADC_BTEMP) * 10;
+	return madc_read(bt->channel_temp) * 10;
 }
 
 static int twl4030_madc_bat_voltscale(struct twl4030_madc_battery *bat,
@@ -84,7 +79,7 @@ static int twl4030_madc_bat_voltscale(struct twl4030_madc_battery *bat,
 	int i, res = 0;
 
 	/* choose charging curve */
-	if (twl4030_madc_bat_get_charging_status())
+	if (twl4030_madc_bat_get_charging_status(bat))
 		calibration = bat->pdata->charging;
 	else
 		calibration = bat->pdata->discharging;
@@ -113,29 +108,28 @@ static int twl4030_madc_bat_get_property(struct power_supply *psy,
 					enum power_supply_property psp,
 					union power_supply_propval *val)
 {
-	struct twl4030_madc_battery *bat = container_of(psy,
-					struct twl4030_madc_battery, psy);
+	struct twl4030_madc_battery *bat = power_supply_get_drvdata(psy);
 
 	switch (psp) {
 	case POWER_SUPPLY_PROP_STATUS:
 		if (twl4030_madc_bat_voltscale(bat,
-				twl4030_madc_bat_get_voltage()) > 95)
+				twl4030_madc_bat_get_voltage(bat)) > 95)
 			val->intval = POWER_SUPPLY_STATUS_FULL;
 		else {
-			if (twl4030_madc_bat_get_charging_status())
+			if (twl4030_madc_bat_get_charging_status(bat))
 				val->intval = POWER_SUPPLY_STATUS_CHARGING;
 			else
 				val->intval = POWER_SUPPLY_STATUS_DISCHARGING;
 		}
 		break;
 	case POWER_SUPPLY_PROP_VOLTAGE_NOW:
-		val->intval = twl4030_madc_bat_get_voltage() * 1000;
+		val->intval = twl4030_madc_bat_get_voltage(bat) * 1000;
 		break;
 	case POWER_SUPPLY_PROP_TECHNOLOGY:
 		val->intval = POWER_SUPPLY_TECHNOLOGY_LION;
 		break;
 	case POWER_SUPPLY_PROP_CURRENT_NOW:
-		val->intval = twl4030_madc_bat_get_current();
+		val->intval = twl4030_madc_bat_get_current(bat);
 		break;
 	case POWER_SUPPLY_PROP_PRESENT:
 		/* assume battery is always present */
@@ -143,23 +137,23 @@ static int twl4030_madc_bat_get_property(struct power_supply *psy,
 		break;
 	case POWER_SUPPLY_PROP_CHARGE_NOW: {
 			int percent = twl4030_madc_bat_voltscale(bat,
-					twl4030_madc_bat_get_voltage());
+					twl4030_madc_bat_get_voltage(bat));
 			val->intval = (percent * bat->pdata->capacity) / 100;
 			break;
 		}
 	case POWER_SUPPLY_PROP_CAPACITY:
 		val->intval = twl4030_madc_bat_voltscale(bat,
-					twl4030_madc_bat_get_voltage());
+					twl4030_madc_bat_get_voltage(bat));
 		break;
 	case POWER_SUPPLY_PROP_CHARGE_FULL:
 		val->intval = bat->pdata->capacity;
 		break;
 	case POWER_SUPPLY_PROP_TEMP:
-		val->intval = twl4030_madc_bat_get_temp();
+		val->intval = twl4030_madc_bat_get_temp(bat);
 		break;
 	case POWER_SUPPLY_PROP_TIME_TO_EMPTY_NOW: {
 			int percent = twl4030_madc_bat_voltscale(bat,
-					twl4030_madc_bat_get_voltage());
+					twl4030_madc_bat_get_voltage(bat));
 			/* in mAh */
 			int chg = (percent * (bat->pdata->capacity/1000))/100;
 
@@ -176,12 +170,19 @@ static int twl4030_madc_bat_get_property(struct power_supply *psy,
 
 static void twl4030_madc_bat_ext_changed(struct power_supply *psy)
 {
-	struct twl4030_madc_battery *bat = container_of(psy,
-					struct twl4030_madc_battery, psy);
-
-	power_supply_changed(&bat->psy);
+	power_supply_changed(psy);
 }
 
+static const struct power_supply_desc twl4030_madc_bat_desc = {
+	.name			= "twl4030_battery",
+	.type			= POWER_SUPPLY_TYPE_BATTERY,
+	.properties		= twl4030_madc_bat_props,
+	.num_properties		= ARRAY_SIZE(twl4030_madc_bat_props),
+	.get_property		= twl4030_madc_bat_get_property,
+	.external_power_changed	= twl4030_madc_bat_ext_changed,
+
+};
+
 static int twl4030_cmp(const void *a, const void *b)
 {
 	return ((struct twl4030_madc_bat_calibration *)b)->voltage -
@@ -192,19 +193,31 @@ static int twl4030_madc_battery_probe(struct platform_device *pdev)
 {
 	struct twl4030_madc_battery *twl4030_madc_bat;
 	struct twl4030_madc_bat_platform_data *pdata = pdev->dev.platform_data;
+	struct power_supply_config psy_cfg = {};
+	int ret = 0;
 
-	twl4030_madc_bat = kzalloc(sizeof(*twl4030_madc_bat), GFP_KERNEL);
+	twl4030_madc_bat = devm_kzalloc(&pdev->dev, sizeof(*twl4030_madc_bat),
+				GFP_KERNEL);
 	if (!twl4030_madc_bat)
 		return -ENOMEM;
 
-	twl4030_madc_bat->psy.name = "twl4030_battery";
-	twl4030_madc_bat->psy.type = POWER_SUPPLY_TYPE_BATTERY;
-	twl4030_madc_bat->psy.properties = twl4030_madc_bat_props;
-	twl4030_madc_bat->psy.num_properties =
-					ARRAY_SIZE(twl4030_madc_bat_props);
-	twl4030_madc_bat->psy.get_property = twl4030_madc_bat_get_property;
-	twl4030_madc_bat->psy.external_power_changed =
-					twl4030_madc_bat_ext_changed;
+	twl4030_madc_bat->channel_temp = iio_channel_get(&pdev->dev, "temp");
+	if (IS_ERR(twl4030_madc_bat->channel_temp)) {
+		ret = PTR_ERR(twl4030_madc_bat->channel_temp);
+		goto err;
+	}
+
+	twl4030_madc_bat->channel_ichg = iio_channel_get(&pdev->dev, "ichg");
+	if (IS_ERR(twl4030_madc_bat->channel_ichg)) {
+		ret = PTR_ERR(twl4030_madc_bat->channel_ichg);
+		goto err_temp;
+	}
+
+	twl4030_madc_bat->channel_vbat = iio_channel_get(&pdev->dev, "vbat");
+	if (IS_ERR(twl4030_madc_bat->channel_vbat)) {
+		ret = PTR_ERR(twl4030_madc_bat->channel_vbat);
+		goto err_ichg;
+	}
 
 	/* sort charging and discharging calibration data */
 	sort(pdata->charging, pdata->charging_size,
@@ -216,17 +229,36 @@ static int twl4030_madc_battery_probe(struct platform_device *pdev)
 
 	twl4030_madc_bat->pdata = pdata;
 	platform_set_drvdata(pdev, twl4030_madc_bat);
-	power_supply_register(&pdev->dev, &twl4030_madc_bat->psy);
+	psy_cfg.drv_data = twl4030_madc_bat;
+	twl4030_madc_bat->psy = power_supply_register(&pdev->dev,
+						      &twl4030_madc_bat_desc,
+						      &psy_cfg);
+	if (IS_ERR(twl4030_madc_bat->psy)) {
+		ret = PTR_ERR(twl4030_madc_bat->psy);
+		goto err_vbat;
+	}
 
 	return 0;
+
+err_vbat:
+	iio_channel_release(twl4030_madc_bat->channel_vbat);
+err_ichg:
+	iio_channel_release(twl4030_madc_bat->channel_ichg);
+err_temp:
+	iio_channel_release(twl4030_madc_bat->channel_temp);
+err:
+	return ret;
 }
 
 static int twl4030_madc_battery_remove(struct platform_device *pdev)
 {
 	struct twl4030_madc_battery *bat = platform_get_drvdata(pdev);
 
-	power_supply_unregister(&bat->psy);
-	kfree(bat);
+	power_supply_unregister(bat->psy);
+
+	iio_channel_release(bat->channel_vbat);
+	iio_channel_release(bat->channel_ichg);
+	iio_channel_release(bat->channel_temp);
 
 	return 0;
 }
@@ -243,3 +275,4 @@ module_platform_driver(twl4030_madc_battery_driver);
 MODULE_LICENSE("GPL");
 MODULE_AUTHOR("Lukas Märdian <lukas@goldelico.com>");
 MODULE_DESCRIPTION("twl4030_madc battery driver");
+MODULE_ALIAS("platform:twl4030_madc_battery");
diff --git a/drivers/power/wm831x_backup.c b/drivers/power/wm831x_backup.c
index 56fb509f4be0..2e33109ca8c7 100644
--- a/drivers/power/wm831x_backup.c
+++ b/drivers/power/wm831x_backup.c
@@ -21,7 +21,8 @@
 
 struct wm831x_backup {
 	struct wm831x *wm831x;
-	struct power_supply backup;
+	struct power_supply *backup;
+	struct power_supply_desc backup_desc;
 	char name[20];
 };
 
@@ -115,7 +116,7 @@ static int wm831x_backup_get_prop(struct power_supply *psy,
 				  enum power_supply_property psp,
 				  union power_supply_propval *val)
 {
-	struct wm831x_backup *devdata = dev_get_drvdata(psy->dev->parent);
+	struct wm831x_backup *devdata = dev_get_drvdata(psy->dev.parent);
 	struct wm831x *wm831x = devdata->wm831x;
 	int ret = 0;
 
@@ -166,8 +167,6 @@ static int wm831x_backup_probe(struct platform_device *pdev)
 	struct wm831x *wm831x = dev_get_drvdata(pdev->dev.parent);
 	struct wm831x_pdata *wm831x_pdata = wm831x->dev->platform_data;
 	struct wm831x_backup *devdata;
-	struct power_supply *backup;
-	int ret;
 
 	devdata = devm_kzalloc(&pdev->dev, sizeof(struct wm831x_backup),
 				GFP_KERNEL);
@@ -177,8 +176,6 @@ static int wm831x_backup_probe(struct platform_device *pdev)
 	devdata->wm831x = wm831x;
 	platform_set_drvdata(pdev, devdata);
 
-	backup = &devdata->backup;
-
 	/* We ignore configuration failures since we can still read
 	 * back the status without enabling the charger (which may
 	 * already be enabled anyway).
@@ -192,21 +189,22 @@ static int wm831x_backup_probe(struct platform_device *pdev)
 		snprintf(devdata->name, sizeof(devdata->name),
 			 "wm831x-backup");
 
-	backup->name = devdata->name;
-	backup->type = POWER_SUPPLY_TYPE_BATTERY;
-	backup->properties = wm831x_backup_props;
-	backup->num_properties = ARRAY_SIZE(wm831x_backup_props);
-	backup->get_property = wm831x_backup_get_prop;
-	ret = power_supply_register(&pdev->dev, backup);
+	devdata->backup_desc.name = devdata->name;
+	devdata->backup_desc.type = POWER_SUPPLY_TYPE_BATTERY;
+	devdata->backup_desc.properties = wm831x_backup_props;
+	devdata->backup_desc.num_properties = ARRAY_SIZE(wm831x_backup_props);
+	devdata->backup_desc.get_property = wm831x_backup_get_prop;
+	devdata->backup = power_supply_register(&pdev->dev,
+						&devdata->backup_desc, NULL);
 
-	return ret;
+	return PTR_ERR_OR_ZERO(devdata->backup);
 }
 
 static int wm831x_backup_remove(struct platform_device *pdev)
 {
 	struct wm831x_backup *devdata = platform_get_drvdata(pdev);
 
-	power_supply_unregister(&devdata->backup);
+	power_supply_unregister(devdata->backup);
 
 	return 0;
 }
diff --git a/drivers/power/wm831x_power.c b/drivers/power/wm831x_power.c
index 3bed2f55cf7d..0161bdabd5a3 100644
--- a/drivers/power/wm831x_power.c
+++ b/drivers/power/wm831x_power.c
@@ -21,9 +21,12 @@
 
 struct wm831x_power {
 	struct wm831x *wm831x;
-	struct power_supply wall;
-	struct power_supply usb;
-	struct power_supply battery;
+	struct power_supply *wall;
+	struct power_supply *usb;
+	struct power_supply *battery;
+	struct power_supply_desc wall_desc;
+	struct power_supply_desc usb_desc;
+	struct power_supply_desc battery_desc;
 	char wall_name[20];
 	char usb_name[20];
 	char battery_name[20];
@@ -67,7 +70,7 @@ static int wm831x_wall_get_prop(struct power_supply *psy,
 				enum power_supply_property psp,
 				union power_supply_propval *val)
 {
-	struct wm831x_power *wm831x_power = dev_get_drvdata(psy->dev->parent);
+	struct wm831x_power *wm831x_power = dev_get_drvdata(psy->dev.parent);
 	struct wm831x *wm831x = wm831x_power->wm831x;
 	int ret = 0;
 
@@ -98,7 +101,7 @@ static int wm831x_usb_get_prop(struct power_supply *psy,
 			       enum power_supply_property psp,
 			       union power_supply_propval *val)
 {
-	struct wm831x_power *wm831x_power = dev_get_drvdata(psy->dev->parent);
+	struct wm831x_power *wm831x_power = dev_get_drvdata(psy->dev.parent);
 	struct wm831x *wm831x = wm831x_power->wm831x;
 	int ret = 0;
 
@@ -393,7 +396,7 @@ static int wm831x_bat_get_prop(struct power_supply *psy,
 			       enum power_supply_property psp,
 			       union power_supply_propval *val)
 {
-	struct wm831x_power *wm831x_power = dev_get_drvdata(psy->dev->parent);
+	struct wm831x_power *wm831x_power = dev_get_drvdata(psy->dev.parent);
 	struct wm831x *wm831x = wm831x_power->wm831x;
 	int ret = 0;
 
@@ -451,7 +454,7 @@ static irqreturn_t wm831x_bat_irq(int irq, void *data)
 	/* The battery charger is autonomous so we don't need to do
 	 * anything except kick user space */
 	if (wm831x_power->have_battery)
-		power_supply_changed(&wm831x_power->battery);
+		power_supply_changed(wm831x_power->battery);
 
 	return IRQ_HANDLED;
 }
@@ -482,9 +485,9 @@ static irqreturn_t wm831x_pwr_src_irq(int irq, void *data)
 
 	/* Just notify for everything - little harm in overnotifying. */
 	if (wm831x_power->have_battery)
-		power_supply_changed(&wm831x_power->battery);
-	power_supply_changed(&wm831x_power->usb);
-	power_supply_changed(&wm831x_power->wall);
+		power_supply_changed(wm831x_power->battery);
+	power_supply_changed(wm831x_power->usb);
+	power_supply_changed(wm831x_power->wall);
 
 	return IRQ_HANDLED;
 }
@@ -494,9 +497,6 @@ static int wm831x_power_probe(struct platform_device *pdev)
 	struct wm831x *wm831x = dev_get_drvdata(pdev->dev.parent);
 	struct wm831x_pdata *wm831x_pdata = wm831x->dev->platform_data;
 	struct wm831x_power *power;
-	struct power_supply *usb;
-	struct power_supply *battery;
-	struct power_supply *wall;
 	int ret, irq, i;
 
 	power = kzalloc(sizeof(struct wm831x_power), GFP_KERNEL);
@@ -506,10 +506,6 @@ static int wm831x_power_probe(struct platform_device *pdev)
 	power->wm831x = wm831x;
 	platform_set_drvdata(pdev, power);
 
-	usb = &power->usb;
-	battery = &power->battery;
-	wall = &power->wall;
-
 	if (wm831x_pdata && wm831x_pdata->wm831x_num) {
 		snprintf(power->wall_name, sizeof(power->wall_name),
 			 "wm831x-wall.%d", wm831x_pdata->wm831x_num);
@@ -531,23 +527,28 @@ static int wm831x_power_probe(struct platform_device *pdev)
 	 */
 	wm831x_config_battery(wm831x);
 
-	wall->name = power->wall_name;
-	wall->type = POWER_SUPPLY_TYPE_MAINS;
-	wall->properties = wm831x_wall_props;
-	wall->num_properties = ARRAY_SIZE(wm831x_wall_props);
-	wall->get_property = wm831x_wall_get_prop;
-	ret = power_supply_register(&pdev->dev, wall);
-	if (ret)
+	power->wall_desc.name = power->wall_name;
+	power->wall_desc.type = POWER_SUPPLY_TYPE_MAINS;
+	power->wall_desc.properties = wm831x_wall_props;
+	power->wall_desc.num_properties = ARRAY_SIZE(wm831x_wall_props);
+	power->wall_desc.get_property = wm831x_wall_get_prop;
+	power->wall = power_supply_register(&pdev->dev, &power->wall_desc,
+					    NULL);
+	if (IS_ERR(power->wall)) {
+		ret = PTR_ERR(power->wall);
 		goto err_kmalloc;
+	}
 
-	usb->name = power->usb_name,
-	usb->type = POWER_SUPPLY_TYPE_USB;
-	usb->properties = wm831x_usb_props;
-	usb->num_properties = ARRAY_SIZE(wm831x_usb_props);
-	usb->get_property = wm831x_usb_get_prop;
-	ret = power_supply_register(&pdev->dev, usb);
-	if (ret)
+	power->usb_desc.name = power->usb_name,
+	power->usb_desc.type = POWER_SUPPLY_TYPE_USB;
+	power->usb_desc.properties = wm831x_usb_props;
+	power->usb_desc.num_properties = ARRAY_SIZE(wm831x_usb_props);
+	power->usb_desc.get_property = wm831x_usb_get_prop;
+	power->usb = power_supply_register(&pdev->dev, &power->usb_desc, NULL);
+	if (IS_ERR(power->usb)) {
+		ret = PTR_ERR(power->usb);
 		goto err_wall;
+	}
 
 	ret = wm831x_reg_read(wm831x, WM831X_CHARGER_CONTROL_1);
 	if (ret < 0)
@@ -555,14 +556,18 @@ static int wm831x_power_probe(struct platform_device *pdev)
 	power->have_battery = ret & WM831X_CHG_ENA;
 
 	if (power->have_battery) {
-		    battery->name = power->battery_name;
-		    battery->properties = wm831x_bat_props;
-		    battery->num_properties = ARRAY_SIZE(wm831x_bat_props);
-		    battery->get_property = wm831x_bat_get_prop;
-		    battery->use_for_apm = 1;
-		    ret = power_supply_register(&pdev->dev, battery);
-		    if (ret)
-			    goto err_usb;
+		power->battery_desc.name = power->battery_name;
+		power->battery_desc.properties = wm831x_bat_props;
+		power->battery_desc.num_properties = ARRAY_SIZE(wm831x_bat_props);
+		power->battery_desc.get_property = wm831x_bat_get_prop;
+		power->battery_desc.use_for_apm = 1;
+		power->battery = power_supply_register(&pdev->dev,
+						       &power->battery_desc,
+						       NULL);
+		if (IS_ERR(power->battery)) {
+			ret = PTR_ERR(power->battery);
+			goto err_usb;
+		}
 	}
 
 	irq = wm831x_irq(wm831x, platform_get_irq_byname(pdev, "SYSLO"));
@@ -615,11 +620,11 @@ err_syslo:
 	free_irq(irq, power);
 err_battery:
 	if (power->have_battery)
-		power_supply_unregister(battery);
+		power_supply_unregister(power->battery);
 err_usb:
-	power_supply_unregister(usb);
+	power_supply_unregister(power->usb);
 err_wall:
-	power_supply_unregister(wall);
+	power_supply_unregister(power->wall);
 err_kmalloc:
 	kfree(power);
 	return ret;
@@ -645,9 +650,9 @@ static int wm831x_power_remove(struct platform_device *pdev)
 	free_irq(irq, wm831x_power);
 
 	if (wm831x_power->have_battery)
-		power_supply_unregister(&wm831x_power->battery);
-	power_supply_unregister(&wm831x_power->wall);
-	power_supply_unregister(&wm831x_power->usb);
+		power_supply_unregister(wm831x_power->battery);
+	power_supply_unregister(wm831x_power->wall);
+	power_supply_unregister(wm831x_power->usb);
 	kfree(wm831x_power);
 	return 0;
 }
diff --git a/drivers/power/wm8350_power.c b/drivers/power/wm8350_power.c
index b3607e2906d2..5c5880664e09 100644
--- a/drivers/power/wm8350_power.c
+++ b/drivers/power/wm8350_power.c
@@ -196,14 +196,14 @@ static irqreturn_t wm8350_charger_handler(int irq, void *data)
 		break;
 	case WM8350_IRQ_CHG_TO:
 		dev_err(wm8350->dev, "charger timeout\n");
-		power_supply_changed(&power->battery);
+		power_supply_changed(power->battery);
 		break;
 
 	case WM8350_IRQ_CHG_BAT_HOT:
 	case WM8350_IRQ_CHG_BAT_COLD:
 	case WM8350_IRQ_CHG_START:
 	case WM8350_IRQ_CHG_END:
-		power_supply_changed(&power->battery);
+		power_supply_changed(power->battery);
 		break;
 
 	case WM8350_IRQ_CHG_FAST_RDY:
@@ -231,9 +231,9 @@ static irqreturn_t wm8350_charger_handler(int irq, void *data)
 	case WM8350_IRQ_EXT_WALL_FB:
 		wm8350_charger_config(wm8350, policy);
 	case WM8350_IRQ_EXT_BAT_FB:   /* Fall through */
-		power_supply_changed(&power->battery);
-		power_supply_changed(&power->usb);
-		power_supply_changed(&power->ac);
+		power_supply_changed(power->battery);
+		power_supply_changed(power->usb);
+		power_supply_changed(power->ac);
 		break;
 
 	default:
@@ -250,7 +250,7 @@ static int wm8350_ac_get_prop(struct power_supply *psy,
 			      enum power_supply_property psp,
 			      union power_supply_propval *val)
 {
-	struct wm8350 *wm8350 = dev_get_drvdata(psy->dev->parent);
+	struct wm8350 *wm8350 = dev_get_drvdata(psy->dev.parent);
 	int ret = 0;
 
 	switch (psp) {
@@ -280,7 +280,7 @@ static int wm8350_usb_get_prop(struct power_supply *psy,
 			       enum power_supply_property psp,
 			       union power_supply_propval *val)
 {
-	struct wm8350 *wm8350 = dev_get_drvdata(psy->dev->parent);
+	struct wm8350 *wm8350 = dev_get_drvdata(psy->dev.parent);
 	int ret = 0;
 
 	switch (psp) {
@@ -346,7 +346,7 @@ static int wm8350_bat_get_property(struct power_supply *psy,
 				   enum power_supply_property psp,
 				   union power_supply_propval *val)
 {
-	struct wm8350 *wm8350 = dev_get_drvdata(psy->dev->parent);
+	struct wm8350 *wm8350 = dev_get_drvdata(psy->dev.parent);
 	int ret = 0;
 
 	switch (psp) {
@@ -382,6 +382,30 @@ static enum power_supply_property wm8350_bat_props[] = {
 	POWER_SUPPLY_PROP_CHARGE_TYPE,
 };
 
+static const struct power_supply_desc wm8350_ac_desc = {
+	.name		= "wm8350-ac",
+	.type		= POWER_SUPPLY_TYPE_MAINS,
+	.properties	= wm8350_ac_props,
+	.num_properties	= ARRAY_SIZE(wm8350_ac_props),
+	.get_property	= wm8350_ac_get_prop,
+};
+
+static const struct power_supply_desc wm8350_battery_desc = {
+	.name		= "wm8350-battery",
+	.properties	= wm8350_bat_props,
+	.num_properties	= ARRAY_SIZE(wm8350_bat_props),
+	.get_property	= wm8350_bat_get_property,
+	.use_for_apm	= 1,
+};
+
+static const struct power_supply_desc wm8350_usb_desc = {
+	.name		= "wm8350-usb",
+	.type		= POWER_SUPPLY_TYPE_USB,
+	.properties	= wm8350_usb_props,
+	.num_properties	= ARRAY_SIZE(wm8350_usb_props),
+	.get_property	= wm8350_usb_get_prop,
+};
+
 /*********************************************************************
  *		Initialisation
  *********************************************************************/
@@ -447,37 +471,24 @@ static int wm8350_power_probe(struct platform_device *pdev)
 	struct wm8350 *wm8350 = platform_get_drvdata(pdev);
 	struct wm8350_power *power = &wm8350->power;
 	struct wm8350_charger_policy *policy = power->policy;
-	struct power_supply *usb = &power->usb;
-	struct power_supply *battery = &power->battery;
-	struct power_supply *ac = &power->ac;
 	int ret;
 
-	ac->name = "wm8350-ac";
-	ac->type = POWER_SUPPLY_TYPE_MAINS;
-	ac->properties = wm8350_ac_props;
-	ac->num_properties = ARRAY_SIZE(wm8350_ac_props);
-	ac->get_property = wm8350_ac_get_prop;
-	ret = power_supply_register(&pdev->dev, ac);
-	if (ret)
-		return ret;
-
-	battery->name = "wm8350-battery";
-	battery->properties = wm8350_bat_props;
-	battery->num_properties = ARRAY_SIZE(wm8350_bat_props);
-	battery->get_property = wm8350_bat_get_property;
-	battery->use_for_apm = 1;
-	ret = power_supply_register(&pdev->dev, battery);
-	if (ret)
+	power->ac = power_supply_register(&pdev->dev, &wm8350_ac_desc, NULL);
+	if (IS_ERR(power->ac))
+		return PTR_ERR(power->ac);
+
+	power->battery = power_supply_register(&pdev->dev, &wm8350_battery_desc,
+					       NULL);
+	if (IS_ERR(power->battery)) {
+		ret = PTR_ERR(power->battery);
 		goto battery_failed;
+	}
 
-	usb->name = "wm8350-usb",
-	usb->type = POWER_SUPPLY_TYPE_USB;
-	usb->properties = wm8350_usb_props;
-	usb->num_properties = ARRAY_SIZE(wm8350_usb_props);
-	usb->get_property = wm8350_usb_get_prop;
-	ret = power_supply_register(&pdev->dev, usb);
-	if (ret)
+	power->usb = power_supply_register(&pdev->dev, &wm8350_usb_desc, NULL);
+	if (IS_ERR(power->usb)) {
+		ret = PTR_ERR(power->usb);
 		goto usb_failed;
+	}
 
 	ret = device_create_file(&pdev->dev, &dev_attr_charger_state);
 	if (ret < 0)
@@ -494,9 +505,9 @@ static int wm8350_power_probe(struct platform_device *pdev)
 	return ret;
 
 usb_failed:
-	power_supply_unregister(battery);
+	power_supply_unregister(power->battery);
 battery_failed:
-	power_supply_unregister(ac);
+	power_supply_unregister(power->ac);
 
 	return ret;
 }
@@ -508,9 +519,9 @@ static int wm8350_power_remove(struct platform_device *pdev)
 
 	free_charger_irq(wm8350);
 	device_remove_file(&pdev->dev, &dev_attr_charger_state);
-	power_supply_unregister(&power->battery);
-	power_supply_unregister(&power->ac);
-	power_supply_unregister(&power->usb);
+	power_supply_unregister(power->battery);
+	power_supply_unregister(power->ac);
+	power_supply_unregister(power->usb);
 	return 0;
 }
 
diff --git a/drivers/power/wm97xx_battery.c b/drivers/power/wm97xx_battery.c
index a8e6203673ad..c2f09ed35050 100644
--- a/drivers/power/wm97xx_battery.c
+++ b/drivers/power/wm97xx_battery.c
@@ -32,20 +32,20 @@ static enum power_supply_property *prop;
 
 static unsigned long wm97xx_read_bat(struct power_supply *bat_ps)
 {
-	struct wm97xx_pdata *wmdata = bat_ps->dev->parent->platform_data;
+	struct wm97xx_pdata *wmdata = bat_ps->dev.parent->platform_data;
 	struct wm97xx_batt_pdata *pdata = wmdata->batt_pdata;
 
-	return wm97xx_read_aux_adc(dev_get_drvdata(bat_ps->dev->parent),
+	return wm97xx_read_aux_adc(dev_get_drvdata(bat_ps->dev.parent),
 					pdata->batt_aux) * pdata->batt_mult /
 					pdata->batt_div;
 }
 
 static unsigned long wm97xx_read_temp(struct power_supply *bat_ps)
 {
-	struct wm97xx_pdata *wmdata = bat_ps->dev->parent->platform_data;
+	struct wm97xx_pdata *wmdata = bat_ps->dev.parent->platform_data;
 	struct wm97xx_batt_pdata *pdata = wmdata->batt_pdata;
 
-	return wm97xx_read_aux_adc(dev_get_drvdata(bat_ps->dev->parent),
+	return wm97xx_read_aux_adc(dev_get_drvdata(bat_ps->dev.parent),
 					pdata->temp_aux) * pdata->temp_mult /
 					pdata->temp_div;
 }
@@ -54,7 +54,7 @@ static int wm97xx_bat_get_property(struct power_supply *bat_ps,
 			    enum power_supply_property psp,
 			    union power_supply_propval *val)
 {
-	struct wm97xx_pdata *wmdata = bat_ps->dev->parent->platform_data;
+	struct wm97xx_pdata *wmdata = bat_ps->dev.parent->platform_data;
 	struct wm97xx_batt_pdata *pdata = wmdata->batt_pdata;
 
 	switch (psp) {
@@ -105,7 +105,7 @@ static void wm97xx_bat_external_power_changed(struct power_supply *bat_ps)
 static void wm97xx_bat_update(struct power_supply *bat_ps)
 {
 	int old_status = bat_status;
-	struct wm97xx_pdata *wmdata = bat_ps->dev->parent->platform_data;
+	struct wm97xx_pdata *wmdata = bat_ps->dev.parent->platform_data;
 	struct wm97xx_batt_pdata *pdata = wmdata->batt_pdata;
 
 	mutex_lock(&work_lock);
@@ -117,7 +117,7 @@ static void wm97xx_bat_update(struct power_supply *bat_ps)
 			POWER_SUPPLY_STATUS_UNKNOWN;
 
 	if (old_status != bat_status) {
-		pr_debug("%s: %i -> %i\n", bat_ps->name, old_status,
+		pr_debug("%s: %i -> %i\n", bat_ps->desc->name, old_status,
 					bat_status);
 		power_supply_changed(bat_ps);
 	}
@@ -125,7 +125,8 @@ static void wm97xx_bat_update(struct power_supply *bat_ps)
 	mutex_unlock(&work_lock);
 }
 
-static struct power_supply bat_ps = {
+static struct power_supply *bat_psy;
+static struct power_supply_desc bat_psy_desc = {
 	.type			= POWER_SUPPLY_TYPE_BATTERY,
 	.get_property		= wm97xx_bat_get_property,
 	.external_power_changed = wm97xx_bat_external_power_changed,
@@ -134,7 +135,7 @@ static struct power_supply bat_ps = {
 
 static void wm97xx_bat_work(struct work_struct *work)
 {
-	wm97xx_bat_update(&bat_ps);
+	wm97xx_bat_update(bat_psy);
 }
 
 static irqreturn_t wm97xx_chrg_irq(int irq, void *data)
@@ -237,18 +238,20 @@ static int wm97xx_bat_probe(struct platform_device *dev)
 		dev_info(&dev->dev, "Please consider setting proper battery "
 				"name in platform definition file, falling "
 				"back to name \"wm97xx-batt\"\n");
-		bat_ps.name = "wm97xx-batt";
+		bat_psy_desc.name = "wm97xx-batt";
 	} else
-		bat_ps.name = pdata->batt_name;
+		bat_psy_desc.name = pdata->batt_name;
 
-	bat_ps.properties = prop;
-	bat_ps.num_properties = props;
+	bat_psy_desc.properties = prop;
+	bat_psy_desc.num_properties = props;
 
-	ret = power_supply_register(&dev->dev, &bat_ps);
-	if (!ret)
+	bat_psy = power_supply_register(&dev->dev, &bat_psy_desc, NULL);
+	if (!IS_ERR(bat_psy)) {
 		schedule_work(&bat_work);
-	else
+	} else {
+		ret = PTR_ERR(bat_psy);
 		goto err4;
+	}
 
 	return 0;
 err4:
@@ -273,7 +276,7 @@ static int wm97xx_bat_remove(struct platform_device *dev)
 		gpio_free(pdata->charge_gpio);
 	}
 	cancel_work_sync(&bat_work);
-	power_supply_unregister(&bat_ps);
+	power_supply_unregister(bat_psy);
 	kfree(prop);
 	return 0;
 }
diff --git a/drivers/power/z2_battery.c b/drivers/power/z2_battery.c
index 814d2e31f0c9..b201e3facf73 100644
--- a/drivers/power/z2_battery.c
+++ b/drivers/power/z2_battery.c
@@ -21,12 +21,13 @@
 #define	Z2_DEFAULT_NAME	"Z2"
 
 struct z2_charger {
-	struct z2_battery_info	*info;
-	int			bat_status;
-	struct i2c_client	*client;
-	struct power_supply	batt_ps;
-	struct mutex		work_lock;
-	struct work_struct	bat_work;
+	struct z2_battery_info		*info;
+	int				bat_status;
+	struct i2c_client		*client;
+	struct power_supply		*batt_ps;
+	struct power_supply_desc	batt_ps_desc;
+	struct mutex			work_lock;
+	struct work_struct		bat_work;
 };
 
 static unsigned long z2_read_bat(struct z2_charger *charger)
@@ -44,8 +45,7 @@ static int z2_batt_get_property(struct power_supply *batt_ps,
 			    enum power_supply_property psp,
 			    union power_supply_propval *val)
 {
-	struct z2_charger *charger = container_of(batt_ps, struct z2_charger,
-						batt_ps);
+	struct z2_charger *charger = power_supply_get_drvdata(batt_ps);
 	struct z2_battery_info *info = charger->info;
 
 	switch (psp) {
@@ -85,8 +85,8 @@ static int z2_batt_get_property(struct power_supply *batt_ps,
 
 static void z2_batt_ext_power_changed(struct power_supply *batt_ps)
 {
-	struct z2_charger *charger = container_of(batt_ps, struct z2_charger,
-						batt_ps);
+	struct z2_charger *charger = power_supply_get_drvdata(batt_ps);
+
 	schedule_work(&charger->bat_work);
 }
 
@@ -106,9 +106,10 @@ static void z2_batt_update(struct z2_charger *charger)
 		POWER_SUPPLY_STATUS_UNKNOWN;
 
 	if (old_status != charger->bat_status) {
-		pr_debug("%s: %i -> %i\n", charger->batt_ps.name, old_status,
-			charger->bat_status);
-		power_supply_changed(&charger->batt_ps);
+		pr_debug("%s: %i -> %i\n", charger->batt_ps->desc->name,
+				old_status,
+				charger->bat_status);
+		power_supply_changed(charger->batt_ps);
 	}
 
 	mutex_unlock(&charger->work_lock);
@@ -166,16 +167,17 @@ static int z2_batt_ps_init(struct z2_charger *charger, int props)
 				"Please consider setting proper battery "
 				"name in platform definition file, falling "
 				"back to name \" Z2_DEFAULT_NAME \"\n");
-		charger->batt_ps.name = Z2_DEFAULT_NAME;
+		charger->batt_ps_desc.name = Z2_DEFAULT_NAME;
 	} else
-		charger->batt_ps.name = info->batt_name;
+		charger->batt_ps_desc.name = info->batt_name;
 
-	charger->batt_ps.properties		= prop;
-	charger->batt_ps.num_properties		= props;
-	charger->batt_ps.type			= POWER_SUPPLY_TYPE_BATTERY;
-	charger->batt_ps.get_property		= z2_batt_get_property;
-	charger->batt_ps.external_power_changed	= z2_batt_ext_power_changed;
-	charger->batt_ps.use_for_apm		= 1;
+	charger->batt_ps_desc.properties	= prop;
+	charger->batt_ps_desc.num_properties	= props;
+	charger->batt_ps_desc.type		= POWER_SUPPLY_TYPE_BATTERY;
+	charger->batt_ps_desc.get_property	= z2_batt_get_property;
+	charger->batt_ps_desc.external_power_changed =
+						z2_batt_ext_power_changed;
+	charger->batt_ps_desc.use_for_apm	= 1;
 
 	return 0;
 }
@@ -187,6 +189,7 @@ static int z2_batt_probe(struct i2c_client *client,
 	int props = 1;	/* POWER_SUPPLY_PROP_PRESENT */
 	struct z2_charger *charger;
 	struct z2_battery_info *info = client->dev.platform_data;
+	struct power_supply_config psy_cfg = {};
 
 	if (info == NULL) {
 		dev_err(&client->dev,
@@ -203,6 +206,7 @@ static int z2_batt_probe(struct i2c_client *client,
 	charger->info = info;
 	charger->client = client;
 	i2c_set_clientdata(client, charger);
+	psy_cfg.drv_data = charger;
 
 	mutex_init(&charger->work_lock);
 
@@ -230,16 +234,20 @@ static int z2_batt_probe(struct i2c_client *client,
 
 	INIT_WORK(&charger->bat_work, z2_batt_work);
 
-	ret = power_supply_register(&client->dev, &charger->batt_ps);
-	if (ret)
+	charger->batt_ps = power_supply_register(&client->dev,
+						 &charger->batt_ps_desc,
+						 &psy_cfg);
+	if (IS_ERR(charger->batt_ps)) {
+		ret = PTR_ERR(charger->batt_ps);
 		goto err4;
+	}
 
 	schedule_work(&charger->bat_work);
 
 	return 0;
 
 err4:
-	kfree(charger->batt_ps.properties);
+	kfree(charger->batt_ps_desc.properties);
 err3:
 	if (info->charge_gpio >= 0 && gpio_is_valid(info->charge_gpio))
 		free_irq(gpio_to_irq(info->charge_gpio), charger);
@@ -257,9 +265,9 @@ static int z2_batt_remove(struct i2c_client *client)
 	struct z2_battery_info *info = charger->info;
 
 	cancel_work_sync(&charger->bat_work);
-	power_supply_unregister(&charger->batt_ps);
+	power_supply_unregister(charger->batt_ps);
 
-	kfree(charger->batt_ps.properties);
+	kfree(charger->batt_ps_desc.properties);
 	if (info->charge_gpio >= 0 && gpio_is_valid(info->charge_gpio)) {
 		free_irq(gpio_to_irq(info->charge_gpio), charger);
 		gpio_free(info->charge_gpio);
diff --git a/drivers/powercap/intel_rapl.c b/drivers/powercap/intel_rapl.c
index 97b5e4ee1ca4..63d4033eb683 100644
--- a/drivers/powercap/intel_rapl.c
+++ b/drivers/powercap/intel_rapl.c
@@ -73,7 +73,7 @@
 
 #define TIME_WINDOW_MAX_MSEC 40000
 #define TIME_WINDOW_MIN_MSEC 250
-
+#define ENERGY_UNIT_SCALE    1000 /* scale from driver unit to powercap unit */
 enum unit_type {
 	ARBITRARY_UNIT, /* no translation */
 	POWER_UNIT,
@@ -158,6 +158,7 @@ struct rapl_domain {
 	struct rapl_power_limit rpl[NR_POWER_LIMITS];
 	u64 attr_map; /* track capabilities */
 	unsigned int state;
+	unsigned int domain_energy_unit;
 	int package_id;
 };
 #define power_zone_to_rapl_domain(_zone) \
@@ -190,6 +191,7 @@ struct rapl_defaults {
 	void (*set_floor_freq)(struct rapl_domain *rd, bool mode);
 	u64 (*compute_time_window)(struct rapl_package *rp, u64 val,
 				bool to_raw);
+	unsigned int dram_domain_energy_unit;
 };
 static struct rapl_defaults *rapl_defaults;
 
@@ -227,7 +229,8 @@ static int rapl_read_data_raw(struct rapl_domain *rd,
 static int rapl_write_data_raw(struct rapl_domain *rd,
 			enum rapl_primitives prim,
 			unsigned long long value);
-static u64 rapl_unit_xlate(int package, enum unit_type type, u64 value,
+static u64 rapl_unit_xlate(struct rapl_domain *rd, int package,
+			enum unit_type type, u64 value,
 			int to_raw);
 static void package_power_limit_irq_save(int package_id);
 
@@ -305,7 +308,9 @@ static int get_energy_counter(struct powercap_zone *power_zone, u64 *energy_raw)
 
 static int get_max_energy_counter(struct powercap_zone *pcd_dev, u64 *energy)
 {
-	*energy = rapl_unit_xlate(0, ENERGY_UNIT, ENERGY_STATUS_MASK, 0);
+	struct rapl_domain *rd = power_zone_to_rapl_domain(pcd_dev);
+
+	*energy = rapl_unit_xlate(rd, 0, ENERGY_UNIT, ENERGY_STATUS_MASK, 0);
 	return 0;
 }
 
@@ -639,6 +644,11 @@ static void rapl_init_domains(struct rapl_package *rp)
 			rd->msrs[4] = MSR_DRAM_POWER_INFO;
 			rd->rpl[0].prim_id = PL1_ENABLE;
 			rd->rpl[0].name = pl1_name;
+			rd->domain_energy_unit =
+				rapl_defaults->dram_domain_energy_unit;
+			if (rd->domain_energy_unit)
+				pr_info("DRAM domain energy unit %dpj\n",
+					rd->domain_energy_unit);
 			break;
 		}
 		if (mask) {
@@ -648,11 +658,13 @@ static void rapl_init_domains(struct rapl_package *rp)
 	}
 }
 
-static u64 rapl_unit_xlate(int package, enum unit_type type, u64 value,
+static u64 rapl_unit_xlate(struct rapl_domain *rd, int package,
+			enum unit_type type, u64 value,
 			int to_raw)
 {
 	u64 units = 1;
 	struct rapl_package *rp;
+	u64 scale = 1;
 
 	rp = find_package_by_id(package);
 	if (!rp)
@@ -663,7 +675,12 @@ static u64 rapl_unit_xlate(int package, enum unit_type type, u64 value,
 		units = rp->power_unit;
 		break;
 	case ENERGY_UNIT:
-		units = rp->energy_unit;
+		scale = ENERGY_UNIT_SCALE;
+		/* per domain unit takes precedence */
+		if (rd && rd->domain_energy_unit)
+			units = rd->domain_energy_unit;
+		else
+			units = rp->energy_unit;
 		break;
 	case TIME_UNIT:
 		return rapl_defaults->compute_time_window(rp, value, to_raw);
@@ -673,11 +690,11 @@ static u64 rapl_unit_xlate(int package, enum unit_type type, u64 value,
 	};
 
 	if (to_raw)
-		return div64_u64(value, units);
+		return div64_u64(value, units) * scale;
 
 	value *= units;
 
-	return value;
+	return div64_u64(value, scale);
 }
 
 /* in the order of enum rapl_primitives */
@@ -773,7 +790,7 @@ static int rapl_read_data_raw(struct rapl_domain *rd,
 	final = value & rp->mask;
 	final = final >> rp->shift;
 	if (xlate)
-		*data = rapl_unit_xlate(rd->package_id, rp->unit, final, 0);
+		*data = rapl_unit_xlate(rd, rd->package_id, rp->unit, final, 0);
 	else
 		*data = final;
 
@@ -799,7 +816,7 @@ static int rapl_write_data_raw(struct rapl_domain *rd,
 			"failed to read msr 0x%x on cpu %d\n", msr, cpu);
 		return -EIO;
 	}
-	value = rapl_unit_xlate(rd->package_id, rp->unit, value, 1);
+	value = rapl_unit_xlate(rd, rd->package_id, rp->unit, value, 1);
 	msr_val &= ~rp->mask;
 	msr_val |= value << rp->shift;
 	if (wrmsrl_safe_on_cpu(cpu, msr, msr_val)) {
@@ -818,7 +835,7 @@ static int rapl_write_data_raw(struct rapl_domain *rd,
  * calculate units differ on different CPUs.
  * We convert the units to below format based on CPUs.
  * i.e.
- * energy unit: microJoules : Represented in microJoules by default
+ * energy unit: picoJoules  : Represented in picoJoules by default
  * power unit : microWatts  : Represented in milliWatts by default
  * time unit  : microseconds: Represented in seconds by default
  */
@@ -834,7 +851,7 @@ static int rapl_check_unit_core(struct rapl_package *rp, int cpu)
 	}
 
 	value = (msr_val & ENERGY_UNIT_MASK) >> ENERGY_UNIT_OFFSET;
-	rp->energy_unit = 1000000 / (1 << value);
+	rp->energy_unit = ENERGY_UNIT_SCALE * 1000000 / (1 << value);
 
 	value = (msr_val & POWER_UNIT_MASK) >> POWER_UNIT_OFFSET;
 	rp->power_unit = 1000000 / (1 << value);
@@ -842,7 +859,7 @@ static int rapl_check_unit_core(struct rapl_package *rp, int cpu)
 	value = (msr_val & TIME_UNIT_MASK) >> TIME_UNIT_OFFSET;
 	rp->time_unit = 1000000 / (1 << value);
 
-	pr_debug("Core CPU package %d energy=%duJ, time=%dus, power=%duW\n",
+	pr_debug("Core CPU package %d energy=%dpJ, time=%dus, power=%duW\n",
 		rp->id, rp->energy_unit, rp->time_unit, rp->power_unit);
 
 	return 0;
@@ -859,7 +876,7 @@ static int rapl_check_unit_atom(struct rapl_package *rp, int cpu)
 		return -ENODEV;
 	}
 	value = (msr_val & ENERGY_UNIT_MASK) >> ENERGY_UNIT_OFFSET;
-	rp->energy_unit = 1 << value;
+	rp->energy_unit = ENERGY_UNIT_SCALE * 1 << value;
 
 	value = (msr_val & POWER_UNIT_MASK) >> POWER_UNIT_OFFSET;
 	rp->power_unit = (1 << value) * 1000;
@@ -867,7 +884,7 @@ static int rapl_check_unit_atom(struct rapl_package *rp, int cpu)
 	value = (msr_val & TIME_UNIT_MASK) >> TIME_UNIT_OFFSET;
 	rp->time_unit = 1000000 / (1 << value);
 
-	pr_debug("Atom package %d energy=%duJ, time=%dus, power=%duW\n",
+	pr_debug("Atom package %d energy=%dpJ, time=%dus, power=%duW\n",
 		rp->id, rp->energy_unit, rp->time_unit, rp->power_unit);
 
 	return 0;
@@ -1017,6 +1034,13 @@ static const struct rapl_defaults rapl_defaults_core = {
 	.compute_time_window = rapl_compute_time_window_core,
 };
 
+static const struct rapl_defaults rapl_defaults_hsw_server = {
+	.check_unit = rapl_check_unit_core,
+	.set_floor_freq = set_floor_freq_default,
+	.compute_time_window = rapl_compute_time_window_core,
+	.dram_domain_energy_unit = 15300,
+};
+
 static const struct rapl_defaults rapl_defaults_atom = {
 	.check_unit = rapl_check_unit_atom,
 	.set_floor_freq = set_floor_freq_atom,
@@ -1037,7 +1061,7 @@ static const struct x86_cpu_id rapl_ids[] = {
 	RAPL_CPU(0x3a, rapl_defaults_core),/* Ivy Bridge */
 	RAPL_CPU(0x3c, rapl_defaults_core),/* Haswell */
 	RAPL_CPU(0x3d, rapl_defaults_core),/* Broadwell */
-	RAPL_CPU(0x3f, rapl_defaults_core),/* Haswell */
+	RAPL_CPU(0x3f, rapl_defaults_hsw_server),/* Haswell servers */
 	RAPL_CPU(0x45, rapl_defaults_core),/* Haswell ULT */
 	RAPL_CPU(0x4C, rapl_defaults_atom),/* Braswell */
 	RAPL_CPU(0x4A, rapl_defaults_atom),/* Tangier */
diff --git a/drivers/regulator/act8865-regulator.c b/drivers/regulator/act8865-regulator.c
index 9eec453b745d..2ff73d72ca34 100644
--- a/drivers/regulator/act8865-regulator.c
+++ b/drivers/regulator/act8865-regulator.c
@@ -29,6 +29,35 @@
 #include <linux/regmap.h>
 
 /*
+ * ACT8600 Global Register Map.
+ */
+#define ACT8600_SYS_MODE	0x00
+#define ACT8600_SYS_CTRL	0x01
+#define ACT8600_DCDC1_VSET	0x10
+#define ACT8600_DCDC1_CTRL	0x12
+#define ACT8600_DCDC2_VSET	0x20
+#define ACT8600_DCDC2_CTRL	0x22
+#define ACT8600_DCDC3_VSET	0x30
+#define ACT8600_DCDC3_CTRL	0x32
+#define ACT8600_SUDCDC4_VSET	0x40
+#define ACT8600_SUDCDC4_CTRL	0x41
+#define ACT8600_LDO5_VSET	0x50
+#define ACT8600_LDO5_CTRL	0x51
+#define ACT8600_LDO6_VSET	0x60
+#define ACT8600_LDO6_CTRL	0x61
+#define ACT8600_LDO7_VSET	0x70
+#define ACT8600_LDO7_CTRL	0x71
+#define ACT8600_LDO8_VSET	0x80
+#define ACT8600_LDO8_CTRL	0x81
+#define ACT8600_LDO910_CTRL	0x91
+#define ACT8600_APCH0		0xA1
+#define ACT8600_APCH1		0xA8
+#define ACT8600_APCH2		0xA9
+#define ACT8600_APCH_STAT	0xAA
+#define ACT8600_OTG0		0xB0
+#define ACT8600_OTG1		0xB2
+
+/*
  * ACT8846 Global Register Map.
  */
 #define	ACT8846_SYS0		0x00
@@ -94,10 +123,15 @@
 #define	ACT8865_ENA		0x80	/* ON - [7] */
 #define	ACT8865_VSEL_MASK	0x3F	/* VSET - [5:0] */
 
+
+#define ACT8600_LDO10_ENA		0x40	/* ON - [6] */
+#define ACT8600_SUDCDC_VSEL_MASK	0xFF	/* SUDCDC VSET - [7:0] */
+
 /*
  * ACT8865 voltage number
  */
 #define	ACT8865_VOLTAGE_NUM	64
+#define ACT8600_SUDCDC_VOLTAGE_NUM	255
 
 struct act8865 {
 	struct regmap *regmap;
@@ -116,6 +150,13 @@ static const struct regulator_linear_range act8865_voltage_ranges[] = {
 	REGULATOR_LINEAR_RANGE(2400000, 48, 63, 100000),
 };
 
+static const struct regulator_linear_range act8600_sudcdc_voltage_ranges[] = {
+	REGULATOR_LINEAR_RANGE(3000000, 0, 63, 0),
+	REGULATOR_LINEAR_RANGE(3000000, 64, 159, 100000),
+	REGULATOR_LINEAR_RANGE(12600000, 160, 191, 200000),
+	REGULATOR_LINEAR_RANGE(19000000, 191, 255, 400000),
+};
+
 static struct regulator_ops act8865_ops = {
 	.list_voltage		= regulator_list_voltage_linear_range,
 	.map_voltage		= regulator_map_voltage_linear_range,
@@ -126,9 +167,16 @@ static struct regulator_ops act8865_ops = {
 	.is_enabled		= regulator_is_enabled_regmap,
 };
 
-#define ACT88xx_REG(_name, _family, _id, _vsel_reg)			\
+static struct regulator_ops act8865_ldo_ops = {
+	.enable			= regulator_enable_regmap,
+	.disable		= regulator_disable_regmap,
+	.is_enabled		= regulator_is_enabled_regmap,
+};
+
+#define ACT88xx_REG(_name, _family, _id, _vsel_reg, _supply)		\
 	[_family##_ID_##_id] = {					\
 		.name			= _name,			\
+		.supply_name		= _supply,			\
 		.id			= _family##_ID_##_id,		\
 		.type			= REGULATOR_VOLTAGE,		\
 		.ops			= &act8865_ops,			\
@@ -142,33 +190,80 @@ static struct regulator_ops act8865_ops = {
 		.owner			= THIS_MODULE,			\
 	}
 
+static const struct regulator_desc act8600_regulators[] = {
+	ACT88xx_REG("DCDC1", ACT8600, DCDC1, VSET, "vp1"),
+	ACT88xx_REG("DCDC2", ACT8600, DCDC2, VSET, "vp2"),
+	ACT88xx_REG("DCDC3", ACT8600, DCDC3, VSET, "vp3"),
+	{
+		.name = "SUDCDC_REG4",
+		.id = ACT8600_ID_SUDCDC4,
+		.ops = &act8865_ops,
+		.type = REGULATOR_VOLTAGE,
+		.n_voltages = ACT8600_SUDCDC_VOLTAGE_NUM,
+		.linear_ranges = act8600_sudcdc_voltage_ranges,
+		.n_linear_ranges = ARRAY_SIZE(act8600_sudcdc_voltage_ranges),
+		.vsel_reg = ACT8600_SUDCDC4_VSET,
+		.vsel_mask = ACT8600_SUDCDC_VSEL_MASK,
+		.enable_reg = ACT8600_SUDCDC4_CTRL,
+		.enable_mask = ACT8865_ENA,
+		.owner = THIS_MODULE,
+	},
+	ACT88xx_REG("LDO5", ACT8600, LDO5, VSET, "inl"),
+	ACT88xx_REG("LDO6", ACT8600, LDO6, VSET, "inl"),
+	ACT88xx_REG("LDO7", ACT8600, LDO7, VSET, "inl"),
+	ACT88xx_REG("LDO8", ACT8600, LDO8, VSET, "inl"),
+	{
+		.name = "LDO_REG9",
+		.id = ACT8600_ID_LDO9,
+		.ops = &act8865_ldo_ops,
+		.type = REGULATOR_VOLTAGE,
+		.n_voltages = 1,
+		.fixed_uV = 1800000,
+		.enable_reg = ACT8600_LDO910_CTRL,
+		.enable_mask = ACT8865_ENA,
+		.owner = THIS_MODULE,
+	},
+	{
+		.name = "LDO_REG10",
+		.id = ACT8600_ID_LDO10,
+		.ops = &act8865_ldo_ops,
+		.type = REGULATOR_VOLTAGE,
+		.n_voltages = 1,
+		.fixed_uV = 1200000,
+		.enable_reg = ACT8600_LDO910_CTRL,
+		.enable_mask = ACT8600_LDO10_ENA,
+		.owner = THIS_MODULE,
+	},
+};
+
 static const struct regulator_desc act8846_regulators[] = {
-	ACT88xx_REG("REG1", ACT8846, REG1, VSET),
-	ACT88xx_REG("REG2", ACT8846, REG2, VSET0),
-	ACT88xx_REG("REG3", ACT8846, REG3, VSET0),
-	ACT88xx_REG("REG4", ACT8846, REG4, VSET0),
-	ACT88xx_REG("REG5", ACT8846, REG5, VSET),
-	ACT88xx_REG("REG6", ACT8846, REG6, VSET),
-	ACT88xx_REG("REG7", ACT8846, REG7, VSET),
-	ACT88xx_REG("REG8", ACT8846, REG8, VSET),
-	ACT88xx_REG("REG9", ACT8846, REG9, VSET),
-	ACT88xx_REG("REG10", ACT8846, REG10, VSET),
-	ACT88xx_REG("REG11", ACT8846, REG11, VSET),
-	ACT88xx_REG("REG12", ACT8846, REG12, VSET),
+	ACT88xx_REG("REG1", ACT8846, REG1, VSET, "vp1"),
+	ACT88xx_REG("REG2", ACT8846, REG2, VSET0, "vp2"),
+	ACT88xx_REG("REG3", ACT8846, REG3, VSET0, "vp3"),
+	ACT88xx_REG("REG4", ACT8846, REG4, VSET0, "vp4"),
+	ACT88xx_REG("REG5", ACT8846, REG5, VSET, "inl1"),
+	ACT88xx_REG("REG6", ACT8846, REG6, VSET, "inl1"),
+	ACT88xx_REG("REG7", ACT8846, REG7, VSET, "inl1"),
+	ACT88xx_REG("REG8", ACT8846, REG8, VSET, "inl2"),
+	ACT88xx_REG("REG9", ACT8846, REG9, VSET, "inl2"),
+	ACT88xx_REG("REG10", ACT8846, REG10, VSET, "inl3"),
+	ACT88xx_REG("REG11", ACT8846, REG11, VSET, "inl3"),
+	ACT88xx_REG("REG12", ACT8846, REG12, VSET, "inl3"),
 };
 
 static const struct regulator_desc act8865_regulators[] = {
-	ACT88xx_REG("DCDC_REG1", ACT8865, DCDC1, VSET1),
-	ACT88xx_REG("DCDC_REG2", ACT8865, DCDC2, VSET1),
-	ACT88xx_REG("DCDC_REG3", ACT8865, DCDC3, VSET1),
-	ACT88xx_REG("LDO_REG1", ACT8865, LDO1, VSET),
-	ACT88xx_REG("LDO_REG2", ACT8865, LDO2, VSET),
-	ACT88xx_REG("LDO_REG3", ACT8865, LDO3, VSET),
-	ACT88xx_REG("LDO_REG4", ACT8865, LDO4, VSET),
+	ACT88xx_REG("DCDC_REG1", ACT8865, DCDC1, VSET1, "vp1"),
+	ACT88xx_REG("DCDC_REG2", ACT8865, DCDC2, VSET1, "vp2"),
+	ACT88xx_REG("DCDC_REG3", ACT8865, DCDC3, VSET1, "vp3"),
+	ACT88xx_REG("LDO_REG1", ACT8865, LDO1, VSET, "inl45"),
+	ACT88xx_REG("LDO_REG2", ACT8865, LDO2, VSET, "inl45"),
+	ACT88xx_REG("LDO_REG3", ACT8865, LDO3, VSET, "inl67"),
+	ACT88xx_REG("LDO_REG4", ACT8865, LDO4, VSET, "inl67"),
 };
 
 #ifdef CONFIG_OF
 static const struct of_device_id act8865_dt_ids[] = {
+	{ .compatible = "active-semi,act8600", .data = (void *)ACT8600 },
 	{ .compatible = "active-semi,act8846", .data = (void *)ACT8846 },
 	{ .compatible = "active-semi,act8865", .data = (void *)ACT8865 },
 	{ }
@@ -200,6 +295,19 @@ static struct of_regulator_match act8865_matches[] = {
 	[ACT8865_ID_LDO4]	= { .name = "LDO_REG4"},
 };
 
+static struct of_regulator_match act8600_matches[] = {
+	[ACT8600_ID_DCDC1]	= { .name = "DCDC_REG1"},
+	[ACT8600_ID_DCDC2]	= { .name = "DCDC_REG2"},
+	[ACT8600_ID_DCDC3]	= { .name = "DCDC_REG3"},
+	[ACT8600_ID_SUDCDC4]	= { .name = "SUDCDC_REG4"},
+	[ACT8600_ID_LDO5]	= { .name = "LDO_REG5"},
+	[ACT8600_ID_LDO6]	= { .name = "LDO_REG6"},
+	[ACT8600_ID_LDO7]	= { .name = "LDO_REG7"},
+	[ACT8600_ID_LDO8]	= { .name = "LDO_REG8"},
+	[ACT8600_ID_LDO9]	= { .name = "LDO_REG9"},
+	[ACT8600_ID_LDO10]	= { .name = "LDO_REG10"},
+};
+
 static int act8865_pdata_from_dt(struct device *dev,
 				 struct device_node **of_node,
 				 struct act8865_platform_data *pdata,
@@ -217,6 +325,10 @@ static int act8865_pdata_from_dt(struct device *dev,
 	}
 
 	switch (type) {
+	case ACT8600:
+		matches = act8600_matches;
+		num_matches = ARRAY_SIZE(act8600_matches);
+		break;
 	case ACT8846:
 		matches = act8846_matches;
 		num_matches = ARRAY_SIZE(act8846_matches);
@@ -317,6 +429,12 @@ static int act8865_pmic_probe(struct i2c_client *client,
 	}
 
 	switch (type) {
+	case ACT8600:
+		regulators = act8600_regulators;
+		num_regulators = ARRAY_SIZE(act8600_regulators);
+		off_reg = -1;
+		off_mask = -1;
+		break;
 	case ACT8846:
 		regulators = act8846_regulators;
 		num_regulators = ARRAY_SIZE(act8846_regulators);
@@ -366,7 +484,7 @@ static int act8865_pmic_probe(struct i2c_client *client,
 	}
 
 	if (of_device_is_system_power_controller(dev->of_node)) {
-		if (!pm_power_off) {
+		if (!pm_power_off && (off_reg > 0)) {
 			act8865_i2c_client = client;
 			act8865->off_reg = off_reg;
 			act8865->off_mask = off_mask;
@@ -402,6 +520,7 @@ static int act8865_pmic_probe(struct i2c_client *client,
 }
 
 static const struct i2c_device_id act8865_ids[] = {
+	{ .name = "act8600", .driver_data = ACT8600 },
 	{ .name = "act8846", .driver_data = ACT8846 },
 	{ .name = "act8865", .driver_data = ACT8865 },
 	{ },
diff --git a/drivers/regulator/arizona-ldo1.c b/drivers/regulator/arizona-ldo1.c
index 8169165904c0..a1d07d347c20 100644
--- a/drivers/regulator/arizona-ldo1.c
+++ b/drivers/regulator/arizona-ldo1.c
@@ -282,6 +282,9 @@ static int arizona_ldo1_probe(struct platform_device *pdev)
 		arizona->external_dcvdd = true;
 
 	ldo1->regulator = devm_regulator_register(&pdev->dev, desc, &config);
+
+	of_node_put(config.of_node);
+
 	if (IS_ERR(ldo1->regulator)) {
 		ret = PTR_ERR(ldo1->regulator);
 		dev_err(arizona->dev, "Failed to register LDO1 supply: %d\n",
@@ -289,8 +292,6 @@ static int arizona_ldo1_probe(struct platform_device *pdev)
 		return ret;
 	}
 
-	of_node_put(config.of_node);
-
 	platform_set_drvdata(pdev, ldo1);
 
 	return 0;
diff --git a/drivers/regulator/arizona-micsupp.c b/drivers/regulator/arizona-micsupp.c
index 20079006459a..4116e74effa4 100644
--- a/drivers/regulator/arizona-micsupp.c
+++ b/drivers/regulator/arizona-micsupp.c
@@ -284,6 +284,9 @@ static int arizona_micsupp_probe(struct platform_device *pdev)
 	micsupp->regulator = devm_regulator_register(&pdev->dev,
 						     desc,
 						     &config);
+
+	of_node_put(config.of_node);
+
 	if (IS_ERR(micsupp->regulator)) {
 		ret = PTR_ERR(micsupp->regulator);
 		dev_err(arizona->dev, "Failed to register mic supply: %d\n",
@@ -291,8 +294,6 @@ static int arizona_micsupp_probe(struct platform_device *pdev)
 		return ret;
 	}
 
-	of_node_put(config.of_node);
-
 	platform_set_drvdata(pdev, micsupp);
 
 	return 0;
diff --git a/drivers/regulator/core.c b/drivers/regulator/core.c
index b899947d839d..443eaab933fc 100644
--- a/drivers/regulator/core.c
+++ b/drivers/regulator/core.c
@@ -648,10 +648,12 @@ static int drms_uA_update(struct regulator_dev *rdev)
 	if (err < 0)
 		return 0;
 
-	if (!rdev->desc->ops->get_optimum_mode)
+	if (!rdev->desc->ops->get_optimum_mode &&
+	    !rdev->desc->ops->set_load)
 		return 0;
 
-	if (!rdev->desc->ops->set_mode)
+	if (!rdev->desc->ops->set_mode &&
+	    !rdev->desc->ops->set_load)
 		return -EINVAL;
 
 	/* get output voltage */
@@ -676,22 +678,29 @@ static int drms_uA_update(struct regulator_dev *rdev)
 	list_for_each_entry(sibling, &rdev->consumer_list, list)
 		current_uA += sibling->uA_load;
 
-	/* now get the optimum mode for our new total regulator load */
-	mode = rdev->desc->ops->get_optimum_mode(rdev, input_uV,
-						  output_uV, current_uA);
+	if (rdev->desc->ops->set_load) {
+		/* set the optimum mode for our new total regulator load */
+		err = rdev->desc->ops->set_load(rdev, current_uA);
+		if (err < 0)
+			rdev_err(rdev, "failed to set load %d\n", current_uA);
+	} else {
+		/* now get the optimum mode for our new total regulator load */
+		mode = rdev->desc->ops->get_optimum_mode(rdev, input_uV,
+							 output_uV, current_uA);
+
+		/* check the new mode is allowed */
+		err = regulator_mode_constrain(rdev, &mode);
+		if (err < 0) {
+			rdev_err(rdev, "failed to get optimum mode @ %d uA %d -> %d uV\n",
+				 current_uA, input_uV, output_uV);
+			return err;
+		}
 
-	/* check the new mode is allowed */
-	err = regulator_mode_constrain(rdev, &mode);
-	if (err < 0) {
-		rdev_err(rdev, "failed to get optimum mode @ %d uA %d -> %d uV\n",
-			 current_uA, input_uV, output_uV);
-		return err;
+		err = rdev->desc->ops->set_mode(rdev, mode);
+		if (err < 0)
+			rdev_err(rdev, "failed to set optimum mode %x\n", mode);
 	}
 
-	err = rdev->desc->ops->set_mode(rdev, mode);
-	if (err < 0)
-		rdev_err(rdev, "failed to set optimum mode %x\n", mode);
-
 	return err;
 }
 
@@ -1316,6 +1325,54 @@ static struct regulator_dev *regulator_dev_lookup(struct device *dev,
 	return NULL;
 }
 
+static int regulator_resolve_supply(struct regulator_dev *rdev)
+{
+	struct regulator_dev *r;
+	struct device *dev = rdev->dev.parent;
+	int ret;
+
+	/* No supply to resovle? */
+	if (!rdev->supply_name)
+		return 0;
+
+	/* Supply already resolved? */
+	if (rdev->supply)
+		return 0;
+
+	r = regulator_dev_lookup(dev, rdev->supply_name, &ret);
+	if (ret == -ENODEV) {
+		/*
+		 * No supply was specified for this regulator and
+		 * there will never be one.
+		 */
+		return 0;
+	}
+
+	if (!r) {
+		dev_err(dev, "Failed to resolve %s-supply for %s\n",
+			rdev->supply_name, rdev->desc->name);
+		return -EPROBE_DEFER;
+	}
+
+	/* Recursively resolve the supply of the supply */
+	ret = regulator_resolve_supply(r);
+	if (ret < 0)
+		return ret;
+
+	ret = set_supply(rdev, r);
+	if (ret < 0)
+		return ret;
+
+	/* Cascade always-on state to supply */
+	if (_regulator_is_enabled(rdev)) {
+		ret = regulator_enable(rdev->supply);
+		if (ret < 0)
+			return ret;
+	}
+
+	return 0;
+}
+
 /* Internal regulator request function */
 static struct regulator *_regulator_get(struct device *dev, const char *id,
 					bool exclusive, bool allow_dummy)
@@ -1385,6 +1442,12 @@ found:
 		goto out;
 	}
 
+	ret = regulator_resolve_supply(rdev);
+	if (ret < 0) {
+		regulator = ERR_PTR(ret);
+		goto out;
+	}
+
 	if (!try_module_get(rdev->owner))
 		goto out;
 
@@ -1839,10 +1902,12 @@ static int _regulator_do_enable(struct regulator_dev *rdev)
 	}
 
 	if (rdev->ena_pin) {
-		ret = regulator_ena_gpio_ctrl(rdev, true);
-		if (ret < 0)
-			return ret;
-		rdev->ena_gpio_state = 1;
+		if (!rdev->ena_gpio_state) {
+			ret = regulator_ena_gpio_ctrl(rdev, true);
+			if (ret < 0)
+				return ret;
+			rdev->ena_gpio_state = 1;
+		}
 	} else if (rdev->desc->ops->enable) {
 		ret = rdev->desc->ops->enable(rdev);
 		if (ret < 0)
@@ -1939,10 +2004,12 @@ static int _regulator_do_disable(struct regulator_dev *rdev)
 	trace_regulator_disable(rdev_get_name(rdev));
 
 	if (rdev->ena_pin) {
-		ret = regulator_ena_gpio_ctrl(rdev, false);
-		if (ret < 0)
-			return ret;
-		rdev->ena_gpio_state = 0;
+		if (rdev->ena_gpio_state) {
+			ret = regulator_ena_gpio_ctrl(rdev, false);
+			if (ret < 0)
+				return ret;
+			rdev->ena_gpio_state = 0;
+		}
 
 	} else if (rdev->desc->ops->disable) {
 		ret = rdev->desc->ops->disable(rdev);
@@ -2994,7 +3061,7 @@ unsigned int regulator_get_mode(struct regulator *regulator)
 EXPORT_SYMBOL_GPL(regulator_get_mode);
 
 /**
- * regulator_set_optimum_mode - set regulator optimum operating mode
+ * regulator_set_load - set regulator load
  * @regulator: regulator source
  * @uA_load: load current
  *
@@ -3017,9 +3084,9 @@ EXPORT_SYMBOL_GPL(regulator_get_mode);
  * DRMS will sum the total requested load on the regulator and change
  * to the most efficient operating mode if platform constraints allow.
  *
- * Returns the new regulator mode or error.
+ * On error a negative errno is returned.
  */
-int regulator_set_optimum_mode(struct regulator *regulator, int uA_load)
+int regulator_set_load(struct regulator *regulator, int uA_load)
 {
 	struct regulator_dev *rdev = regulator->rdev;
 	int ret;
@@ -3031,7 +3098,7 @@ int regulator_set_optimum_mode(struct regulator *regulator, int uA_load)
 
 	return ret;
 }
-EXPORT_SYMBOL_GPL(regulator_set_optimum_mode);
+EXPORT_SYMBOL_GPL(regulator_set_load);
 
 /**
  * regulator_allow_bypass - allow the regulator to go into bypass mode
@@ -3444,13 +3511,6 @@ static umode_t regulator_attr_is_visible(struct kobject *kobj,
 	if (attr == &dev_attr_requested_microamps.attr)
 		return rdev->desc->type == REGULATOR_CURRENT ? mode : 0;
 
-	/* all the other attributes exist to support constraints;
-	 * don't show them if there are no constraints, or if the
-	 * relevant supporting methods are missing.
-	 */
-	if (!rdev->constraints)
-		return 0;
-
 	/* constraints need specific supporting methods */
 	if (attr == &dev_attr_min_microvolts.attr ||
 	    attr == &dev_attr_max_microvolts.attr)
@@ -3502,7 +3562,18 @@ static struct class regulator_class = {
 
 static void rdev_init_debugfs(struct regulator_dev *rdev)
 {
-	rdev->debugfs = debugfs_create_dir(rdev_get_name(rdev), debugfs_root);
+	struct device *parent = rdev->dev.parent;
+	const char *rname = rdev_get_name(rdev);
+	char name[NAME_MAX];
+
+	/* Avoid duplicate debugfs directory names */
+	if (parent && rname == rdev->desc->name) {
+		snprintf(name, sizeof(name), "%s-%s", dev_name(parent),
+			 rname);
+		rname = name;
+	}
+
+	rdev->debugfs = debugfs_create_dir(rname, debugfs_root);
 	if (!rdev->debugfs) {
 		rdev_warn(rdev, "Failed to create debugfs directory\n");
 		return;
@@ -3536,7 +3607,6 @@ regulator_register(const struct regulator_desc *regulator_desc,
 	struct regulator_dev *rdev;
 	struct device *dev;
 	int ret, i;
-	const char *supply = NULL;
 
 	if (regulator_desc == NULL || cfg == NULL)
 		return ERR_PTR(-EINVAL);
@@ -3633,12 +3703,6 @@ regulator_register(const struct regulator_desc *regulator_desc,
 				 config->ena_gpio, ret);
 			goto wash;
 		}
-
-		if (config->ena_gpio_flags & GPIOF_OUT_INIT_HIGH)
-			rdev->ena_gpio_state = 1;
-
-		if (config->ena_gpio_invert)
-			rdev->ena_gpio_state = !rdev->ena_gpio_state;
 	}
 
 	/* set regulator constraints */
@@ -3650,41 +3714,10 @@ regulator_register(const struct regulator_desc *regulator_desc,
 		goto scrub;
 
 	if (init_data && init_data->supply_regulator)
-		supply = init_data->supply_regulator;
+		rdev->supply_name = init_data->supply_regulator;
 	else if (regulator_desc->supply_name)
-		supply = regulator_desc->supply_name;
-
-	if (supply) {
-		struct regulator_dev *r;
-
-		r = regulator_dev_lookup(dev, supply, &ret);
-
-		if (ret == -ENODEV) {
-			/*
-			 * No supply was specified for this regulator and
-			 * there will never be one.
-			 */
-			ret = 0;
-			goto add_dev;
-		} else if (!r) {
-			dev_err(dev, "Failed to find supply %s\n", supply);
-			ret = -EPROBE_DEFER;
-			goto scrub;
-		}
-
-		ret = set_supply(rdev, r);
-		if (ret < 0)
-			goto scrub;
-
-		/* Enable supply if rail is enabled */
-		if (_regulator_is_enabled(rdev)) {
-			ret = regulator_enable(rdev->supply);
-			if (ret < 0)
-				goto scrub;
-		}
-	}
+		rdev->supply_name = regulator_desc->supply_name;
 
-add_dev:
 	/* add consumers devices */
 	if (init_data) {
 		for (i = 0; i < init_data->num_consumer_supplies; i++) {
@@ -3711,8 +3744,6 @@ unset_supplies:
 	unset_regulator_supplies(rdev);
 
 scrub:
-	if (rdev->supply)
-		_regulator_put(rdev->supply);
 	regulator_ena_gpio_free(rdev);
 	kfree(rdev->constraints);
 wash:
@@ -3807,9 +3838,11 @@ int regulator_suspend_finish(void)
 	list_for_each_entry(rdev, &regulator_list, list) {
 		mutex_lock(&rdev->mutex);
 		if (rdev->use_count > 0  || rdev->constraints->always_on) {
-			error = _regulator_do_enable(rdev);
-			if (error)
-				ret = error;
+			if (!_regulator_is_enabled(rdev)) {
+				error = _regulator_do_enable(rdev);
+				if (error)
+					ret = error;
+			}
 		} else {
 			if (!have_full_constraints())
 				goto unlock;
@@ -3943,6 +3976,110 @@ static const struct file_operations supply_map_fops = {
 #endif
 };
 
+#ifdef CONFIG_DEBUG_FS
+static void regulator_summary_show_subtree(struct seq_file *s,
+					   struct regulator_dev *rdev,
+					   int level)
+{
+	struct list_head *list = s->private;
+	struct regulator_dev *child;
+	struct regulation_constraints *c;
+	struct regulator *consumer;
+
+	if (!rdev)
+		return;
+
+	seq_printf(s, "%*s%-*s %3d %4d %6d ",
+		   level * 3 + 1, "",
+		   30 - level * 3, rdev_get_name(rdev),
+		   rdev->use_count, rdev->open_count, rdev->bypass_count);
+
+	seq_printf(s, "%5dmV ", _regulator_get_voltage(rdev) / 1000);
+	seq_printf(s, "%5dmA ", _regulator_get_current_limit(rdev) / 1000);
+
+	c = rdev->constraints;
+	if (c) {
+		switch (rdev->desc->type) {
+		case REGULATOR_VOLTAGE:
+			seq_printf(s, "%5dmV %5dmV ",
+				   c->min_uV / 1000, c->max_uV / 1000);
+			break;
+		case REGULATOR_CURRENT:
+			seq_printf(s, "%5dmA %5dmA ",
+				   c->min_uA / 1000, c->max_uA / 1000);
+			break;
+		}
+	}
+
+	seq_puts(s, "\n");
+
+	list_for_each_entry(consumer, &rdev->consumer_list, list) {
+		if (consumer->dev->class == &regulator_class)
+			continue;
+
+		seq_printf(s, "%*s%-*s ",
+			   (level + 1) * 3 + 1, "",
+			   30 - (level + 1) * 3, dev_name(consumer->dev));
+
+		switch (rdev->desc->type) {
+		case REGULATOR_VOLTAGE:
+			seq_printf(s, "%37dmV %5dmV",
+				   consumer->min_uV / 1000,
+				   consumer->max_uV / 1000);
+			break;
+		case REGULATOR_CURRENT:
+			break;
+		}
+
+		seq_puts(s, "\n");
+	}
+
+	list_for_each_entry(child, list, list) {
+		/* handle only non-root regulators supplied by current rdev */
+		if (!child->supply || child->supply->rdev != rdev)
+			continue;
+
+		regulator_summary_show_subtree(s, child, level + 1);
+	}
+}
+
+static int regulator_summary_show(struct seq_file *s, void *data)
+{
+	struct list_head *list = s->private;
+	struct regulator_dev *rdev;
+
+	seq_puts(s, " regulator                      use open bypass voltage current     min     max\n");
+	seq_puts(s, "-------------------------------------------------------------------------------\n");
+
+	mutex_lock(&regulator_list_mutex);
+
+	list_for_each_entry(rdev, list, list) {
+		if (rdev->supply)
+			continue;
+
+		regulator_summary_show_subtree(s, rdev, 0);
+	}
+
+	mutex_unlock(&regulator_list_mutex);
+
+	return 0;
+}
+
+static int regulator_summary_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, regulator_summary_show, inode->i_private);
+}
+#endif
+
+static const struct file_operations regulator_summary_fops = {
+#ifdef CONFIG_DEBUG_FS
+	.open		= regulator_summary_open,
+	.read		= seq_read,
+	.llseek		= seq_lseek,
+	.release	= single_release,
+#endif
+};
+
 static int __init regulator_init(void)
 {
 	int ret;
@@ -3956,6 +4093,9 @@ static int __init regulator_init(void)
 	debugfs_create_file("supply_map", 0444, debugfs_root, NULL,
 			    &supply_map_fops);
 
+	debugfs_create_file("regulator_summary", 0444, debugfs_root,
+			    &regulator_list, &regulator_summary_fops);
+
 	regulator_dummy_init();
 
 	return ret;
diff --git a/drivers/regulator/da9210-regulator.c b/drivers/regulator/da9210-regulator.c
index bc6100103f7f..f0489cb9018b 100644
--- a/drivers/regulator/da9210-regulator.c
+++ b/drivers/regulator/da9210-regulator.c
@@ -152,6 +152,15 @@ static int da9210_i2c_probe(struct i2c_client *i2c,
 	config.regmap = chip->regmap;
 	config.of_node = dev->of_node;
 
+	/* Mask all interrupt sources to deassert interrupt line */
+	error = regmap_write(chip->regmap, DA9210_REG_MASK_A, ~0);
+	if (!error)
+		error = regmap_write(chip->regmap, DA9210_REG_MASK_B, ~0);
+	if (error) {
+		dev_err(&i2c->dev, "Failed to write to mask reg: %d\n", error);
+		return error;
+	}
+
 	rdev = devm_regulator_register(&i2c->dev, &da9210_reg, &config);
 	if (IS_ERR(rdev)) {
 		dev_err(&i2c->dev, "Failed to register DA9210 regulator\n");
diff --git a/drivers/regulator/da9211-regulator.c b/drivers/regulator/da9211-regulator.c
index 01343419555e..df79e4b1946e 100644
--- a/drivers/regulator/da9211-regulator.c
+++ b/drivers/regulator/da9211-regulator.c
@@ -305,8 +305,7 @@ static irqreturn_t da9211_irq_handler(int irq, void *data)
 
 	if (reg_val & DA9211_E_OV_CURR_A) {
 		regulator_notifier_call_chain(chip->rdev[0],
-			REGULATOR_EVENT_OVER_CURRENT,
-			rdev_get_drvdata(chip->rdev[0]));
+			REGULATOR_EVENT_OVER_CURRENT, NULL);
 
 		err = regmap_write(chip->regmap, DA9211_REG_EVENT_B,
 			DA9211_E_OV_CURR_A);
@@ -318,8 +317,7 @@ static irqreturn_t da9211_irq_handler(int irq, void *data)
 
 	if (reg_val & DA9211_E_OV_CURR_B) {
 		regulator_notifier_call_chain(chip->rdev[1],
-			REGULATOR_EVENT_OVER_CURRENT,
-			rdev_get_drvdata(chip->rdev[1]));
+			REGULATOR_EVENT_OVER_CURRENT, NULL);
 
 		err = regmap_write(chip->regmap, DA9211_REG_EVENT_B,
 			DA9211_E_OV_CURR_B);
@@ -344,7 +342,7 @@ static int da9211_regulator_init(struct da9211 *chip)
 
 	ret = regmap_read(chip->regmap, DA9211_REG_CONFIG_E, &data);
 	if (ret < 0) {
-		dev_err(chip->dev, "Failed to read CONTROL_E reg: %d\n", ret);
+		dev_err(chip->dev, "Failed to read CONFIG_E reg: %d\n", ret);
 		return ret;
 	}
 
diff --git a/drivers/regulator/dbx500-prcmu.c b/drivers/regulator/dbx500-prcmu.c
index 2d16b9f16de7..3963dfad766c 100644
--- a/drivers/regulator/dbx500-prcmu.c
+++ b/drivers/regulator/dbx500-prcmu.c
@@ -95,14 +95,9 @@ void ux500_regulator_resume_debug(void)
 
 static int ux500_regulator_power_state_cnt_print(struct seq_file *s, void *p)
 {
-	struct device *dev = s->private;
-	int err;
-
 	/* print power state count */
-	err = seq_printf(s, "ux500-regulator power state count: %i\n",
-		power_state_active_get());
-	if (err < 0)
-		dev_err(dev, "seq_printf overflow\n");
+	seq_printf(s, "ux500-regulator power state count: %i\n",
+		   power_state_active_get());
 
 	return 0;
 }
@@ -124,19 +119,11 @@ static const struct file_operations ux500_regulator_power_state_cnt_fops = {
 
 static int ux500_regulator_status_print(struct seq_file *s, void *p)
 {
-	struct device *dev = s->private;
-	int err;
 	int i;
 
 	/* print dump header */
-	err = seq_puts(s, "ux500-regulator status:\n");
-	if (err < 0)
-		dev_err(dev, "seq_puts overflow\n");
-
-	err = seq_printf(s, "%31s : %8s : %8s\n", "current",
-		"before", "after");
-	if (err < 0)
-		dev_err(dev, "seq_printf overflow\n");
+	seq_puts(s, "ux500-regulator status:\n");
+	seq_printf(s, "%31s : %8s : %8s\n", "current", "before", "after");
 
 	for (i = 0; i < rdebug.num_regulators; i++) {
 		struct dbx500_regulator_info *info;
@@ -144,12 +131,11 @@ static int ux500_regulator_status_print(struct seq_file *s, void *p)
 		info = &rdebug.regulator_array[i];
 
 		/* print status */
-		err = seq_printf(s, "%20s : %8s : %8s : %8s\n", info->desc.name,
-			info->is_enabled ? "enabled" : "disabled",
-			rdebug.state_before_suspend[i] ? "enabled" : "disabled",
-			rdebug.state_after_suspend[i] ? "enabled" : "disabled");
-		if (err < 0)
-			dev_err(dev, "seq_printf overflow\n");
+		seq_printf(s, "%20s : %8s : %8s : %8s\n",
+			   info->desc.name,
+			   info->is_enabled ? "enabled" : "disabled",
+			   rdebug.state_before_suspend[i] ? "enabled" : "disabled",
+			   rdebug.state_after_suspend[i] ? "enabled" : "disabled");
 	}
 
 	return 0;
diff --git a/drivers/regulator/devres.c b/drivers/regulator/devres.c
index 8f785bc9e510..6ec1d400adae 100644
--- a/drivers/regulator/devres.c
+++ b/drivers/regulator/devres.c
@@ -413,3 +413,88 @@ void devm_regulator_bulk_unregister_supply_alias(struct device *dev,
 		devm_regulator_unregister_supply_alias(dev, id[i]);
 }
 EXPORT_SYMBOL_GPL(devm_regulator_bulk_unregister_supply_alias);
+
+struct regulator_notifier_match {
+	struct regulator *regulator;
+	struct notifier_block *nb;
+};
+
+static int devm_regulator_match_notifier(struct device *dev, void *res,
+					 void *data)
+{
+	struct regulator_notifier_match *match = res;
+	struct regulator_notifier_match *target = data;
+
+	return match->regulator == target->regulator && match->nb == target->nb;
+}
+
+static void devm_regulator_destroy_notifier(struct device *dev, void *res)
+{
+	struct regulator_notifier_match *match = res;
+
+	regulator_unregister_notifier(match->regulator, match->nb);
+}
+
+/**
+ * devm_regulator_register_notifier - Resource managed
+ * regulator_register_notifier
+ *
+ * @regulator: regulator source
+ * @nb: notifier block
+ *
+ * The notifier will be registers under the consumer device and be
+ * automatically be unregistered when the source device is unbound.
+ */
+int devm_regulator_register_notifier(struct regulator *regulator,
+				     struct notifier_block *nb)
+{
+	struct regulator_notifier_match *match;
+	int ret;
+
+	match = devres_alloc(devm_regulator_destroy_notifier,
+			     sizeof(struct regulator_notifier_match),
+			     GFP_KERNEL);
+	if (!match)
+		return -ENOMEM;
+
+	match->regulator = regulator;
+	match->nb = nb;
+
+	ret = regulator_register_notifier(regulator, nb);
+	if (ret < 0) {
+		devres_free(match);
+		return ret;
+	}
+
+	devres_add(regulator->dev, match);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(devm_regulator_register_notifier);
+
+/**
+ * devm_regulator_unregister_notifier - Resource managed
+ * regulator_unregister_notifier()
+ *
+ * @regulator: regulator source
+ * @nb: notifier block
+ *
+ * Unregister a notifier registered with devm_regulator_register_notifier().
+ * Normally this function will not need to be called and the resource
+ * management code will ensure that the resource is freed.
+ */
+void devm_regulator_unregister_notifier(struct regulator *regulator,
+					struct notifier_block *nb)
+{
+	struct regulator_notifier_match match;
+	int rc;
+
+	match.regulator = regulator;
+	match.nb = nb;
+
+	rc = devres_release(regulator->dev, devm_regulator_destroy_notifier,
+			    devm_regulator_match_notifier, &match);
+	if (rc != 0)
+		WARN_ON(rc);
+}
+EXPORT_SYMBOL_GPL(devm_regulator_unregister_notifier);
diff --git a/drivers/regulator/max77693.c b/drivers/regulator/max77693.c
index 07b313e51b21..9665a488e2f1 100644
--- a/drivers/regulator/max77693.c
+++ b/drivers/regulator/max77693.c
@@ -128,6 +128,8 @@ static struct regulator_ops max77693_charger_ops = {
 #define regulator_desc_esafeout(_num)	{			\
 	.name		= "ESAFEOUT"#_num,			\
 	.id		= MAX77693_ESAFEOUT##_num,		\
+	.of_match	= of_match_ptr("ESAFEOUT"#_num),	\
+	.regulators_node	= of_match_ptr("regulators"),	\
 	.n_voltages	= 4,					\
 	.ops		= &max77693_safeout_ops,		\
 	.type		= REGULATOR_VOLTAGE,			\
@@ -145,6 +147,8 @@ static const struct regulator_desc regulators[] = {
 	{
 		.name = "CHARGER",
 		.id = MAX77693_CHARGER,
+		.of_match = of_match_ptr("CHARGER"),
+		.regulators_node = of_match_ptr("regulators"),
 		.ops = &max77693_charger_ops,
 		.type = REGULATOR_CURRENT,
 		.owner = THIS_MODULE,
@@ -154,102 +158,23 @@ static const struct regulator_desc regulators[] = {
 	},
 };
 
-#ifdef CONFIG_OF
-static int max77693_pmic_dt_parse_rdata(struct device *dev,
-					struct max77693_regulator_data **rdata)
-{
-	struct device_node *np;
-	struct of_regulator_match *rmatch;
-	struct max77693_regulator_data *tmp;
-	int i, matched = 0;
-
-	np = of_get_child_by_name(dev->parent->of_node, "regulators");
-	if (!np)
-		return -EINVAL;
-
-	rmatch = devm_kzalloc(dev,
-		 sizeof(*rmatch) * ARRAY_SIZE(regulators), GFP_KERNEL);
-	if (!rmatch) {
-		of_node_put(np);
-		return -ENOMEM;
-	}
-
-	for (i = 0; i < ARRAY_SIZE(regulators); i++)
-		rmatch[i].name = regulators[i].name;
-
-	matched = of_regulator_match(dev, np, rmatch, ARRAY_SIZE(regulators));
-	of_node_put(np);
-	if (matched <= 0)
-		return matched;
-	*rdata = devm_kzalloc(dev, sizeof(**rdata) * matched, GFP_KERNEL);
-	if (!(*rdata))
-		return -ENOMEM;
-
-	tmp = *rdata;
-
-	for (i = 0; i < matched; i++) {
-		tmp->initdata = rmatch[i].init_data;
-		tmp->of_node = rmatch[i].of_node;
-		tmp->id = regulators[i].id;
-		tmp++;
-	}
-
-	return matched;
-}
-#else
-static int max77693_pmic_dt_parse_rdata(struct device *dev,
-					struct max77693_regulator_data **rdata)
-{
-	return 0;
-}
-#endif /* CONFIG_OF */
-
-static int max77693_pmic_init_rdata(struct device *dev,
-				    struct max77693_regulator_data **rdata)
-{
-	struct max77693_platform_data *pdata;
-	int num_regulators = 0;
-
-	pdata = dev_get_platdata(dev->parent);
-	if (pdata) {
-		*rdata = pdata->regulators;
-		num_regulators = pdata->num_regulators;
-	}
-
-	if (!(*rdata) && dev->parent->of_node)
-		num_regulators = max77693_pmic_dt_parse_rdata(dev, rdata);
-
-	return num_regulators;
-}
-
 static int max77693_pmic_probe(struct platform_device *pdev)
 {
 	struct max77693_dev *iodev = dev_get_drvdata(pdev->dev.parent);
-	struct max77693_regulator_data *rdata = NULL;
-	int num_rdata, i;
+	int i;
 	struct regulator_config config = { };
 
-	num_rdata = max77693_pmic_init_rdata(&pdev->dev, &rdata);
-	if (!rdata || num_rdata <= 0) {
-		dev_err(&pdev->dev, "No init data supplied.\n");
-		return -ENODEV;
-	}
-
-	config.dev = &pdev->dev;
+	config.dev = iodev->dev;
 	config.regmap = iodev->regmap;
 
-	for (i = 0; i < num_rdata; i++) {
-		int id = rdata[i].id;
+	for (i = 0; i < ARRAY_SIZE(regulators); i++) {
 		struct regulator_dev *rdev;
 
-		config.init_data = rdata[i].initdata;
-		config.of_node = rdata[i].of_node;
-
 		rdev = devm_regulator_register(&pdev->dev,
-						&regulators[id], &config);
+						&regulators[i], &config);
 		if (IS_ERR(rdev)) {
 			dev_err(&pdev->dev,
-				"Failed to initialize regulator-%d\n", id);
+				"Failed to initialize regulator-%d\n", i);
 			return PTR_ERR(rdev);
 		}
 	}
diff --git a/drivers/regulator/max8660.c b/drivers/regulator/max8660.c
index 7eee2ca18541..4071d74fa828 100644
--- a/drivers/regulator/max8660.c
+++ b/drivers/regulator/max8660.c
@@ -382,7 +382,7 @@ static int max8660_probe(struct i2c_client *client,
 				   const struct i2c_device_id *i2c_id)
 {
 	struct device *dev = &client->dev;
-	struct max8660_platform_data *pdata = dev_get_platdata(dev);
+	struct max8660_platform_data pdata_of, *pdata = dev_get_platdata(dev);
 	struct regulator_config config = { };
 	struct max8660 *max8660;
 	int boot_on, i, id, ret = -EINVAL;
@@ -391,7 +391,6 @@ static int max8660_probe(struct i2c_client *client,
 
 	if (dev->of_node && !pdata) {
 		const struct of_device_id *id;
-		struct max8660_platform_data pdata_of;
 
 		id = of_match_device(of_match_ptr(max8660_dt_ids), dev);
 		if (!id)
@@ -443,9 +442,9 @@ static int max8660_probe(struct i2c_client *client,
 	for (i = 0; i < pdata->num_subdevs; i++) {
 
 		if (!pdata->subdevs[i].platform_data)
-			return ret;
-
-		boot_on = pdata->subdevs[i].platform_data->constraints.boot_on;
+			boot_on = false;
+		else
+			boot_on = pdata->subdevs[i].platform_data->constraints.boot_on;
 
 		switch (pdata->subdevs[i].id) {
 		case MAX8660_V3:
diff --git a/drivers/regulator/palmas-regulator.c b/drivers/regulator/palmas-regulator.c
index 9205f433573c..8217613807d3 100644
--- a/drivers/regulator/palmas-regulator.c
+++ b/drivers/regulator/palmas-regulator.c
@@ -916,6 +916,9 @@ static int palmas_ldo_registration(struct palmas_pmic *pmic,
 			    (id == PALMAS_REG_LDO6))
 				desc->enable_time = 2000;
 		} else {
+			if (!ddata->has_regen3 && id == PALMAS_REG_REGEN3)
+				continue;
+
 			desc->n_voltages = 1;
 			if (reg_init && reg_init->roof_floor)
 				desc->ops = &palmas_ops_ext_control_extreg;
@@ -1398,6 +1401,7 @@ static struct palmas_pmic_driver_data palmas_ddata = {
 	.ldo_begin = PALMAS_REG_LDO1,
 	.ldo_end = PALMAS_REG_LDOUSB,
 	.max_reg = PALMAS_NUM_REGS,
+	.has_regen3 = true,
 	.palmas_regs_info = palmas_generic_regs_info,
 	.palmas_matches = palmas_matches,
 	.sleep_req_info = palma_sleep_req_info,
@@ -1411,6 +1415,7 @@ static struct palmas_pmic_driver_data tps65917_ddata = {
 	.ldo_begin = TPS65917_REG_LDO1,
 	.ldo_end = TPS65917_REG_LDO5,
 	.max_reg = TPS65917_NUM_REGS,
+	.has_regen3 = true,
 	.palmas_regs_info = tps65917_regs_info,
 	.palmas_matches = tps65917_matches,
 	.sleep_req_info = tps65917_sleep_req_info,
@@ -1505,7 +1510,7 @@ static void palmas_dt_to_pdata(struct device *dev,
 	pdata->ldo6_vibrator = of_property_read_bool(node, "ti,ldo6-vibrator");
 }
 
-static struct of_device_id of_palmas_match_tbl[] = {
+static const struct of_device_id of_palmas_match_tbl[] = {
 	{
 		.compatible = "ti,palmas-pmic",
 		.data = &palmas_ddata,
@@ -1572,6 +1577,12 @@ static int palmas_regulators_probe(struct platform_device *pdev)
 	if (!pmic)
 		return -ENOMEM;
 
+	if (of_device_is_compatible(node, "ti,tps659038-pmic")) {
+		palmas_generic_regs_info[PALMAS_REG_REGEN2].ctrl_addr =
+							TPS659038_REGEN2_CTRL;
+		palmas_ddata.has_regen3 = false;
+	}
+
 	pmic->dev = &pdev->dev;
 	pmic->palmas = palmas;
 	palmas->pmic = pmic;
diff --git a/drivers/regulator/qcom_rpm-regulator.c b/drivers/regulator/qcom_rpm-regulator.c
index 00c5cc3d9546..e254272585b2 100644
--- a/drivers/regulator/qcom_rpm-regulator.c
+++ b/drivers/regulator/qcom_rpm-regulator.c
@@ -393,6 +393,28 @@ static int rpm_reg_is_enabled(struct regulator_dev *rdev)
 	return vreg->is_enabled;
 }
 
+static int rpm_reg_set_load(struct regulator_dev *rdev, int load_uA)
+{
+	struct qcom_rpm_reg *vreg = rdev_get_drvdata(rdev);
+	const struct rpm_reg_parts *parts = vreg->parts;
+	const struct request_member *req = &parts->ia;
+	int load_mA = load_uA / 1000;
+	int max_mA = req->mask >> req->shift;
+	int ret;
+
+	if (req->mask == 0)
+		return -EINVAL;
+
+	if (load_mA > max_mA)
+		load_mA = max_mA;
+
+	mutex_lock(&vreg->lock);
+	ret = rpm_reg_write(vreg, req, load_mA);
+	mutex_unlock(&vreg->lock);
+
+	return ret;
+}
+
 static struct regulator_ops uV_ops = {
 	.list_voltage = regulator_list_voltage_linear_range,
 
@@ -402,6 +424,8 @@ static struct regulator_ops uV_ops = {
 	.enable = rpm_reg_uV_enable,
 	.disable = rpm_reg_uV_disable,
 	.is_enabled = rpm_reg_is_enabled,
+
+	.set_load = rpm_reg_set_load,
 };
 
 static struct regulator_ops mV_ops = {
@@ -413,6 +437,8 @@ static struct regulator_ops mV_ops = {
 	.enable = rpm_reg_mV_enable,
 	.disable = rpm_reg_mV_disable,
 	.is_enabled = rpm_reg_is_enabled,
+
+	.set_load = rpm_reg_set_load,
 };
 
 static struct regulator_ops switch_ops = {
@@ -581,31 +607,6 @@ static const struct qcom_rpm_reg smb208_smps = {
 	.supports_force_mode_bypass = false,
 };
 
-static const struct of_device_id rpm_of_match[] = {
-	{ .compatible = "qcom,rpm-pm8058-pldo",     .data = &pm8058_pldo },
-	{ .compatible = "qcom,rpm-pm8058-nldo",     .data = &pm8058_nldo },
-	{ .compatible = "qcom,rpm-pm8058-smps",     .data = &pm8058_smps },
-	{ .compatible = "qcom,rpm-pm8058-ncp",      .data = &pm8058_ncp },
-	{ .compatible = "qcom,rpm-pm8058-switch",   .data = &pm8058_switch },
-
-	{ .compatible = "qcom,rpm-pm8901-pldo",     .data = &pm8901_pldo },
-	{ .compatible = "qcom,rpm-pm8901-nldo",     .data = &pm8901_nldo },
-	{ .compatible = "qcom,rpm-pm8901-ftsmps",   .data = &pm8901_ftsmps },
-	{ .compatible = "qcom,rpm-pm8901-switch",   .data = &pm8901_switch },
-
-	{ .compatible = "qcom,rpm-pm8921-pldo",     .data = &pm8921_pldo },
-	{ .compatible = "qcom,rpm-pm8921-nldo",     .data = &pm8921_nldo },
-	{ .compatible = "qcom,rpm-pm8921-nldo1200", .data = &pm8921_nldo1200 },
-	{ .compatible = "qcom,rpm-pm8921-smps",     .data = &pm8921_smps },
-	{ .compatible = "qcom,rpm-pm8921-ftsmps",   .data = &pm8921_ftsmps },
-	{ .compatible = "qcom,rpm-pm8921-ncp",      .data = &pm8921_ncp },
-	{ .compatible = "qcom,rpm-pm8921-switch",   .data = &pm8921_switch },
-
-	{ .compatible = "qcom,rpm-smb208", .data = &smb208_smps },
-	{ }
-};
-MODULE_DEVICE_TABLE(of, rpm_of_match);
-
 static int rpm_reg_set(struct qcom_rpm_reg *vreg,
 		       const struct request_member *req,
 		       const int value)
@@ -619,7 +620,9 @@ static int rpm_reg_set(struct qcom_rpm_reg *vreg,
 	return 0;
 }
 
-static int rpm_reg_of_parse_freq(struct device *dev, struct qcom_rpm_reg *vreg)
+static int rpm_reg_of_parse_freq(struct device *dev,
+				 struct device_node *node,
+				 struct qcom_rpm_reg *vreg)
 {
 	static const int freq_table[] = {
 		19200000, 9600000, 6400000, 4800000, 3840000, 3200000, 2740000,
@@ -633,7 +636,7 @@ static int rpm_reg_of_parse_freq(struct device *dev, struct qcom_rpm_reg *vreg)
 	int i;
 
 	key = "qcom,switch-mode-frequency";
-	ret = of_property_read_u32(dev->of_node, key, &freq);
+	ret = of_property_read_u32(node, key, &freq);
 	if (ret) {
 		dev_err(dev, "regulator requires %s property\n", key);
 		return -EINVAL;
@@ -650,84 +653,40 @@ static int rpm_reg_of_parse_freq(struct device *dev, struct qcom_rpm_reg *vreg)
 	return -EINVAL;
 }
 
-static int rpm_reg_probe(struct platform_device *pdev)
+static int rpm_reg_of_parse(struct device_node *node,
+			    const struct regulator_desc *desc,
+			    struct regulator_config *config)
 {
-	struct regulator_init_data *initdata;
-	const struct qcom_rpm_reg *template;
-	const struct of_device_id *match;
-	struct regulator_config config = { };
-	struct regulator_dev *rdev;
-	struct qcom_rpm_reg *vreg;
+	struct qcom_rpm_reg *vreg = config->driver_data;
+	struct device *dev = config->dev;
 	const char *key;
 	u32 force_mode;
 	bool pwm;
 	u32 val;
 	int ret;
 
-	match = of_match_device(rpm_of_match, &pdev->dev);
-	template = match->data;
-
-	vreg = devm_kmalloc(&pdev->dev, sizeof(*vreg), GFP_KERNEL);
-	if (!vreg) {
-		dev_err(&pdev->dev, "failed to allocate vreg\n");
-		return -ENOMEM;
-	}
-	memcpy(vreg, template, sizeof(*vreg));
-	mutex_init(&vreg->lock);
-	vreg->dev = &pdev->dev;
-	vreg->desc.id = -1;
-	vreg->desc.owner = THIS_MODULE;
-	vreg->desc.type = REGULATOR_VOLTAGE;
-	vreg->desc.name = pdev->dev.of_node->name;
-	vreg->desc.supply_name = "vin";
-
-	vreg->rpm = dev_get_drvdata(pdev->dev.parent);
-	if (!vreg->rpm) {
-		dev_err(&pdev->dev, "unable to retrieve handle to rpm\n");
-		return -ENODEV;
-	}
-
-	initdata = of_get_regulator_init_data(&pdev->dev, pdev->dev.of_node,
-					      &vreg->desc);
-	if (!initdata)
-		return -EINVAL;
-
-	key = "reg";
-	ret = of_property_read_u32(pdev->dev.of_node, key, &val);
-	if (ret) {
-		dev_err(&pdev->dev, "failed to read %s\n", key);
-		return ret;
-	}
-	vreg->resource = val;
-
-	if ((vreg->parts->uV.mask || vreg->parts->mV.mask) &&
-	    (!initdata->constraints.min_uV || !initdata->constraints.max_uV)) {
-		dev_err(&pdev->dev, "no voltage specified for regulator\n");
-		return -EINVAL;
-	}
-
 	key = "bias-pull-down";
-	if (of_property_read_bool(pdev->dev.of_node, key)) {
+	if (of_property_read_bool(node, key)) {
 		ret = rpm_reg_set(vreg, &vreg->parts->pd, 1);
 		if (ret) {
-			dev_err(&pdev->dev, "%s is invalid", key);
+			dev_err(dev, "%s is invalid", key);
 			return ret;
 		}
 	}
 
 	if (vreg->parts->freq.mask) {
-		ret = rpm_reg_of_parse_freq(&pdev->dev, vreg);
+		ret = rpm_reg_of_parse_freq(dev, node, vreg);
 		if (ret < 0)
 			return ret;
 	}
 
 	if (vreg->parts->pm.mask) {
 		key = "qcom,power-mode-hysteretic";
-		pwm = !of_property_read_bool(pdev->dev.of_node, key);
+		pwm = !of_property_read_bool(node, key);
 
 		ret = rpm_reg_set(vreg, &vreg->parts->pm, pwm);
 		if (ret) {
-			dev_err(&pdev->dev, "failed to set power mode\n");
+			dev_err(dev, "failed to set power mode\n");
 			return ret;
 		}
 	}
@@ -736,11 +695,11 @@ static int rpm_reg_probe(struct platform_device *pdev)
 		force_mode = -1;
 
 		key = "qcom,force-mode";
-		ret = of_property_read_u32(pdev->dev.of_node, key, &val);
+		ret = of_property_read_u32(node, key, &val);
 		if (ret == -EINVAL) {
 			val = QCOM_RPM_FORCE_MODE_NONE;
 		} else if (ret < 0) {
-			dev_err(&pdev->dev, "failed to read %s\n", key);
+			dev_err(dev, "failed to read %s\n", key);
 			return ret;
 		}
 
@@ -775,25 +734,192 @@ static int rpm_reg_probe(struct platform_device *pdev)
 		}
 
 		if (force_mode == -1) {
-			dev_err(&pdev->dev, "invalid force mode\n");
+			dev_err(dev, "invalid force mode\n");
 			return -EINVAL;
 		}
 
 		ret = rpm_reg_set(vreg, &vreg->parts->fm, force_mode);
 		if (ret) {
-			dev_err(&pdev->dev, "failed to set force mode\n");
+			dev_err(dev, "failed to set force mode\n");
 			return ret;
 		}
 	}
 
-	config.dev = &pdev->dev;
-	config.init_data = initdata;
-	config.driver_data = vreg;
-	config.of_node = pdev->dev.of_node;
-	rdev = devm_regulator_register(&pdev->dev, &vreg->desc, &config);
-	if (IS_ERR(rdev)) {
-		dev_err(&pdev->dev, "can't register regulator\n");
-		return PTR_ERR(rdev);
+	return 0;
+}
+
+struct rpm_regulator_data {
+	const char *name;
+	int resource;
+	const struct qcom_rpm_reg *template;
+	const char *supply;
+};
+
+static const struct rpm_regulator_data rpm_pm8058_regulators[] = {
+	{ "l0",   QCOM_RPM_PM8058_LDO0,   &pm8058_nldo, "vdd_l0_l1_lvs"	},
+	{ "l1",   QCOM_RPM_PM8058_LDO1,   &pm8058_nldo, "vdd_l0_l1_lvs" },
+	{ "l2",   QCOM_RPM_PM8058_LDO2,   &pm8058_pldo, "vdd_l2_l11_l12" },
+	{ "l3",   QCOM_RPM_PM8058_LDO3,   &pm8058_pldo, "vdd_l3_l4_l5" },
+	{ "l4",   QCOM_RPM_PM8058_LDO4,   &pm8058_pldo, "vdd_l3_l4_l5" },
+	{ "l5",   QCOM_RPM_PM8058_LDO5,   &pm8058_pldo, "vdd_l3_l4_l5" },
+	{ "l6",   QCOM_RPM_PM8058_LDO6,   &pm8058_pldo, "vdd_l6_l7" },
+	{ "l7",   QCOM_RPM_PM8058_LDO7,   &pm8058_pldo, "vdd_l6_l7" },
+	{ "l8",   QCOM_RPM_PM8058_LDO8,   &pm8058_pldo, "vdd_l8" },
+	{ "l9",   QCOM_RPM_PM8058_LDO9,   &pm8058_pldo, "vdd_l9" },
+	{ "l10",  QCOM_RPM_PM8058_LDO10,  &pm8058_pldo, "vdd_l10" },
+	{ "l11",  QCOM_RPM_PM8058_LDO11,  &pm8058_pldo, "vdd_l2_l11_l12" },
+	{ "l12",  QCOM_RPM_PM8058_LDO12,  &pm8058_pldo, "vdd_l2_l11_l12" },
+	{ "l13",  QCOM_RPM_PM8058_LDO13,  &pm8058_pldo, "vdd_l13_l16" },
+	{ "l14",  QCOM_RPM_PM8058_LDO14,  &pm8058_pldo, "vdd_l14_l15" },
+	{ "l15",  QCOM_RPM_PM8058_LDO15,  &pm8058_pldo, "vdd_l14_l15" },
+	{ "l16",  QCOM_RPM_PM8058_LDO16,  &pm8058_pldo, "vdd_l13_l16" },
+	{ "l17",  QCOM_RPM_PM8058_LDO17,  &pm8058_pldo, "vdd_l17_l18" },
+	{ "l18",  QCOM_RPM_PM8058_LDO18,  &pm8058_pldo, "vdd_l17_l18" },
+	{ "l19",  QCOM_RPM_PM8058_LDO19,  &pm8058_pldo, "vdd_l19_l20" },
+	{ "l20",  QCOM_RPM_PM8058_LDO20,  &pm8058_pldo, "vdd_l19_l20" },
+	{ "l21",  QCOM_RPM_PM8058_LDO21,  &pm8058_nldo, "vdd_l21" },
+	{ "l22",  QCOM_RPM_PM8058_LDO22,  &pm8058_nldo, "vdd_l22" },
+	{ "l23",  QCOM_RPM_PM8058_LDO23,  &pm8058_nldo, "vdd_l23_l24_l25" },
+	{ "l24",  QCOM_RPM_PM8058_LDO24,  &pm8058_nldo, "vdd_l23_l24_l25" },
+	{ "l25",  QCOM_RPM_PM8058_LDO25,  &pm8058_nldo, "vdd_l23_l24_l25" },
+
+	{ "s0",   QCOM_RPM_PM8058_SMPS0,  &pm8058_smps, "vdd_s0" },
+	{ "s1",   QCOM_RPM_PM8058_SMPS1,  &pm8058_smps, "vdd_s1" },
+	{ "s2",   QCOM_RPM_PM8058_SMPS2,  &pm8058_smps, "vdd_s2" },
+	{ "s3",   QCOM_RPM_PM8058_SMPS3,  &pm8058_smps, "vdd_s3" },
+	{ "s4",   QCOM_RPM_PM8058_SMPS4,  &pm8058_smps, "vdd_s4" },
+
+	{ "lvs0", QCOM_RPM_PM8058_LVS0, &pm8058_switch, "vdd_l0_l1_lvs" },
+	{ "lvs1", QCOM_RPM_PM8058_LVS1, &pm8058_switch, "vdd_l0_l1_lvs" },
+
+	{ "ncp",  QCOM_RPM_PM8058_NCP, &pm8058_ncp, "vdd_ncp" },
+	{ }
+};
+
+static const struct rpm_regulator_data rpm_pm8901_regulators[] = {
+	{ "l0",   QCOM_RPM_PM8901_LDO0, &pm8901_nldo, "vdd_l0" },
+	{ "l1",   QCOM_RPM_PM8901_LDO1, &pm8901_pldo, "vdd_l1" },
+	{ "l2",   QCOM_RPM_PM8901_LDO2, &pm8901_pldo, "vdd_l2" },
+	{ "l3",   QCOM_RPM_PM8901_LDO3, &pm8901_pldo, "vdd_l3" },
+	{ "l4",   QCOM_RPM_PM8901_LDO4, &pm8901_pldo, "vdd_l4" },
+	{ "l5",   QCOM_RPM_PM8901_LDO5, &pm8901_pldo, "vdd_l5" },
+	{ "l6",   QCOM_RPM_PM8901_LDO6, &pm8901_pldo, "vdd_l6" },
+
+	{ "s0",   QCOM_RPM_PM8901_SMPS0, &pm8901_ftsmps, "vdd_s0" },
+	{ "s1",   QCOM_RPM_PM8901_SMPS1, &pm8901_ftsmps, "vdd_s1" },
+	{ "s2",   QCOM_RPM_PM8901_SMPS2, &pm8901_ftsmps, "vdd_s2" },
+	{ "s3",   QCOM_RPM_PM8901_SMPS3, &pm8901_ftsmps, "vdd_s3" },
+	{ "s4",   QCOM_RPM_PM8901_SMPS4, &pm8901_ftsmps, "vdd_s4" },
+
+	{ "lvs0", QCOM_RPM_PM8901_LVS0, &pm8901_switch, "lvs0_in" },
+	{ "lvs1", QCOM_RPM_PM8901_LVS1, &pm8901_switch, "lvs1_in" },
+	{ "lvs2", QCOM_RPM_PM8901_LVS2, &pm8901_switch, "lvs2_in" },
+	{ "lvs3", QCOM_RPM_PM8901_LVS3, &pm8901_switch, "lvs3_in" },
+
+	{ "mvs", QCOM_RPM_PM8901_MVS, &pm8901_switch, "mvs_in" },
+	{ }
+};
+
+static const struct rpm_regulator_data rpm_pm8921_regulators[] = {
+	{ "s1",  QCOM_RPM_PM8921_SMPS1, &pm8921_smps, "vdd_s1" },
+	{ "s2",  QCOM_RPM_PM8921_SMPS2, &pm8921_smps, "vdd_s2" },
+	{ "s3",  QCOM_RPM_PM8921_SMPS3, &pm8921_smps },
+	{ "s4",  QCOM_RPM_PM8921_SMPS4, &pm8921_smps, "vdd_s4" },
+	{ "s7",  QCOM_RPM_PM8921_SMPS7, &pm8921_smps, "vdd_s7" },
+	{ "s8",  QCOM_RPM_PM8921_SMPS8, &pm8921_smps, "vdd_s8"  },
+
+	{ "l1",  QCOM_RPM_PM8921_LDO1, &pm8921_nldo, "vdd_l1_l2_l12_l18" },
+	{ "l2",  QCOM_RPM_PM8921_LDO2, &pm8921_nldo, "vdd_l1_l2_l12_l18" },
+	{ "l3",  QCOM_RPM_PM8921_LDO3, &pm8921_pldo, "vdd_l3_l15_l17" },
+	{ "l4",  QCOM_RPM_PM8921_LDO4, &pm8921_pldo, "vdd_l4_l14" },
+	{ "l5",  QCOM_RPM_PM8921_LDO5, &pm8921_pldo, "vdd_l5_l8_l16" },
+	{ "l6",  QCOM_RPM_PM8921_LDO6, &pm8921_pldo, "vdd_l6_l7" },
+	{ "l7",  QCOM_RPM_PM8921_LDO7, &pm8921_pldo, "vdd_l6_l7" },
+	{ "l8",  QCOM_RPM_PM8921_LDO8, &pm8921_pldo, "vdd_l5_l8_l16" },
+	{ "l9",  QCOM_RPM_PM8921_LDO9, &pm8921_pldo, "vdd_l9_l11" },
+	{ "l10", QCOM_RPM_PM8921_LDO10, &pm8921_pldo, "vdd_l10_l22" },
+	{ "l11", QCOM_RPM_PM8921_LDO11, &pm8921_pldo, "vdd_l9_l11" },
+	{ "l12", QCOM_RPM_PM8921_LDO12, &pm8921_nldo, "vdd_l1_l2_l12_l18" },
+	{ "l14", QCOM_RPM_PM8921_LDO14, &pm8921_pldo, "vdd_l4_l14" },
+	{ "l15", QCOM_RPM_PM8921_LDO15, &pm8921_pldo, "vdd_l3_l15_l17" },
+	{ "l16", QCOM_RPM_PM8921_LDO16, &pm8921_pldo, "vdd_l5_l8_l16" },
+	{ "l17", QCOM_RPM_PM8921_LDO17, &pm8921_pldo, "vdd_l3_l15_l17" },
+	{ "l18", QCOM_RPM_PM8921_LDO18, &pm8921_nldo, "vdd_l1_l2_l12_l18" },
+	{ "l21", QCOM_RPM_PM8921_LDO21, &pm8921_pldo, "vdd_l21_l23_l29" },
+	{ "l22", QCOM_RPM_PM8921_LDO22, &pm8921_pldo, "vdd_l10_l22" },
+	{ "l23", QCOM_RPM_PM8921_LDO23, &pm8921_pldo, "vdd_l21_l23_l29" },
+	{ "l24", QCOM_RPM_PM8921_LDO24, &pm8921_nldo1200, "vdd_l24" },
+	{ "l25", QCOM_RPM_PM8921_LDO25, &pm8921_nldo1200, "vdd_l25" },
+	{ "l26", QCOM_RPM_PM8921_LDO26, &pm8921_nldo1200, "vdd_l26" },
+	{ "l27", QCOM_RPM_PM8921_LDO27, &pm8921_nldo1200, "vdd_l27" },
+	{ "l28", QCOM_RPM_PM8921_LDO28, &pm8921_nldo1200, "vdd_l28" },
+	{ "l29", QCOM_RPM_PM8921_LDO29, &pm8921_pldo, "vdd_l21_l23_l29" },
+
+	{ "lvs1", QCOM_RPM_PM8921_LVS1, &pm8921_switch, "vin_lvs1_3_6" },
+	{ "lvs2", QCOM_RPM_PM8921_LVS2, &pm8921_switch, "vin_lvs2" },
+	{ "lvs3", QCOM_RPM_PM8921_LVS3, &pm8921_switch, "vin_lvs1_3_6" },
+	{ "lvs4", QCOM_RPM_PM8921_LVS4, &pm8921_switch, "vin_lvs4_5_7" },
+	{ "lvs5", QCOM_RPM_PM8921_LVS5, &pm8921_switch, "vin_lvs4_5_7" },
+	{ "lvs6", QCOM_RPM_PM8921_LVS6, &pm8921_switch, "vin_lvs1_3_6" },
+	{ "lvs7", QCOM_RPM_PM8921_LVS7, &pm8921_switch, "vin_lvs4_5_7" },
+
+	{ "usb-switch", QCOM_RPM_USB_OTG_SWITCH, &pm8921_switch, "vin_5vs" },
+	{ "hdmi-switch", QCOM_RPM_HDMI_SWITCH, &pm8921_switch, "vin_5vs" },
+	{ "ncp", QCOM_RPM_PM8921_NCP, &pm8921_ncp, "vdd_ncp" },
+	{ }
+};
+
+static const struct of_device_id rpm_of_match[] = {
+	{ .compatible = "qcom,rpm-pm8058-regulators", .data = &rpm_pm8058_regulators },
+	{ .compatible = "qcom,rpm-pm8901-regulators", .data = &rpm_pm8901_regulators },
+	{ .compatible = "qcom,rpm-pm8921-regulators", .data = &rpm_pm8921_regulators },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, rpm_of_match);
+
+static int rpm_reg_probe(struct platform_device *pdev)
+{
+	const struct rpm_regulator_data *reg;
+	const struct of_device_id *match;
+	struct regulator_config config = { };
+	struct regulator_dev *rdev;
+	struct qcom_rpm_reg *vreg;
+	struct qcom_rpm *rpm;
+
+	rpm = dev_get_drvdata(pdev->dev.parent);
+	if (!rpm) {
+		dev_err(&pdev->dev, "unable to retrieve handle to rpm\n");
+		return -ENODEV;
+	}
+
+	match = of_match_device(rpm_of_match, &pdev->dev);
+	for (reg = match->data; reg->name; reg++) {
+		vreg = devm_kmalloc(&pdev->dev, sizeof(*vreg), GFP_KERNEL);
+		if (!vreg)
+			return -ENOMEM;
+
+		memcpy(vreg, reg->template, sizeof(*vreg));
+		mutex_init(&vreg->lock);
+
+		vreg->dev = &pdev->dev;
+		vreg->resource = reg->resource;
+		vreg->rpm = rpm;
+
+		vreg->desc.id = -1;
+		vreg->desc.owner = THIS_MODULE;
+		vreg->desc.type = REGULATOR_VOLTAGE;
+		vreg->desc.name = reg->name;
+		vreg->desc.supply_name = reg->supply;
+		vreg->desc.of_match = reg->name;
+		vreg->desc.of_parse_cb = rpm_reg_of_parse;
+
+		config.dev = &pdev->dev;
+		config.driver_data = vreg;
+		rdev = devm_regulator_register(&pdev->dev, &vreg->desc, &config);
+		if (IS_ERR(rdev)) {
+			dev_err(&pdev->dev, "failed to register %s\n", reg->name);
+			return PTR_ERR(rdev);
+		}
 	}
 
 	return 0;
diff --git a/drivers/regulator/rk808-regulator.c b/drivers/regulator/rk808-regulator.c
index 1f93b752a81c..3fd44353cc80 100644
--- a/drivers/regulator/rk808-regulator.c
+++ b/drivers/regulator/rk808-regulator.c
@@ -235,6 +235,7 @@ static const struct regulator_desc rk808_reg[] = {
 		.vsel_mask = RK808_LDO_VSEL_MASK,
 		.enable_reg = RK808_LDO_EN_REG,
 		.enable_mask = BIT(0),
+		.enable_time = 400,
 		.owner = THIS_MODULE,
 	}, {
 		.name = "LDO_REG2",
@@ -249,6 +250,7 @@ static const struct regulator_desc rk808_reg[] = {
 		.vsel_mask = RK808_LDO_VSEL_MASK,
 		.enable_reg = RK808_LDO_EN_REG,
 		.enable_mask = BIT(1),
+		.enable_time = 400,
 		.owner = THIS_MODULE,
 	}, {
 		.name = "LDO_REG3",
@@ -263,6 +265,7 @@ static const struct regulator_desc rk808_reg[] = {
 		.vsel_mask = RK808_BUCK4_VSEL_MASK,
 		.enable_reg = RK808_LDO_EN_REG,
 		.enable_mask = BIT(2),
+		.enable_time = 400,
 		.owner = THIS_MODULE,
 	}, {
 		.name = "LDO_REG4",
@@ -277,6 +280,7 @@ static const struct regulator_desc rk808_reg[] = {
 		.vsel_mask = RK808_LDO_VSEL_MASK,
 		.enable_reg = RK808_LDO_EN_REG,
 		.enable_mask = BIT(3),
+		.enable_time = 400,
 		.owner = THIS_MODULE,
 	}, {
 		.name = "LDO_REG5",
@@ -291,6 +295,7 @@ static const struct regulator_desc rk808_reg[] = {
 		.vsel_mask = RK808_LDO_VSEL_MASK,
 		.enable_reg = RK808_LDO_EN_REG,
 		.enable_mask = BIT(4),
+		.enable_time = 400,
 		.owner = THIS_MODULE,
 	}, {
 		.name = "LDO_REG6",
@@ -305,6 +310,7 @@ static const struct regulator_desc rk808_reg[] = {
 		.vsel_mask = RK808_LDO_VSEL_MASK,
 		.enable_reg = RK808_LDO_EN_REG,
 		.enable_mask = BIT(5),
+		.enable_time = 400,
 		.owner = THIS_MODULE,
 	}, {
 		.name = "LDO_REG7",
@@ -319,6 +325,7 @@ static const struct regulator_desc rk808_reg[] = {
 		.vsel_mask = RK808_LDO_VSEL_MASK,
 		.enable_reg = RK808_LDO_EN_REG,
 		.enable_mask = BIT(6),
+		.enable_time = 400,
 		.owner = THIS_MODULE,
 	}, {
 		.name = "LDO_REG8",
@@ -333,6 +340,7 @@ static const struct regulator_desc rk808_reg[] = {
 		.vsel_mask = RK808_LDO_VSEL_MASK,
 		.enable_reg = RK808_LDO_EN_REG,
 		.enable_mask = BIT(7),
+		.enable_time = 400,
 		.owner = THIS_MODULE,
 	}, {
 		.name = "SWITCH_REG1",
diff --git a/drivers/regulator/stw481x-vmmc.c b/drivers/regulator/stw481x-vmmc.c
index 89025f560259..7d2ae3e9e942 100644
--- a/drivers/regulator/stw481x-vmmc.c
+++ b/drivers/regulator/stw481x-vmmc.c
@@ -56,6 +56,7 @@ static int stw481x_vmmc_regulator_probe(struct platform_device *pdev)
 {
 	struct stw481x *stw481x = dev_get_platdata(&pdev->dev);
 	struct regulator_config config = { };
+	struct regulator_dev *rdev;
 	int ret;
 
 	/* First disable the external VMMC if it's active */
@@ -75,12 +76,11 @@ static int stw481x_vmmc_regulator_probe(struct platform_device *pdev)
 						      pdev->dev.of_node,
 						      &vmmc_regulator);
 
-	stw481x->vmmc_regulator = devm_regulator_register(&pdev->dev,
-						&vmmc_regulator, &config);
-	if (IS_ERR(stw481x->vmmc_regulator)) {
+	rdev = devm_regulator_register(&pdev->dev, &vmmc_regulator, &config);
+	if (IS_ERR(rdev)) {
 		dev_err(&pdev->dev,
 			"error initializing STw481x VMMC regulator\n");
-		return PTR_ERR(stw481x->vmmc_regulator);
+		return PTR_ERR(rdev);
 	}
 
 	dev_info(&pdev->dev, "initialized STw481x VMMC regulator\n");
diff --git a/drivers/regulator/tps65910-regulator.c b/drivers/regulator/tps65910-regulator.c
index e2cffe01b807..fb991ec76423 100644
--- a/drivers/regulator/tps65910-regulator.c
+++ b/drivers/regulator/tps65910-regulator.c
@@ -17,6 +17,7 @@
 #include <linux/module.h>
 #include <linux/init.h>
 #include <linux/err.h>
+#include <linux/of.h>
 #include <linux/platform_device.h>
 #include <linux/regulator/driver.h>
 #include <linux/regulator/machine.h>
diff --git a/drivers/regulator/wm8350-regulator.c b/drivers/regulator/wm8350-regulator.c
index 7ec7c390eeda..95f6b040186e 100644
--- a/drivers/regulator/wm8350-regulator.c
+++ b/drivers/regulator/wm8350-regulator.c
@@ -1151,17 +1151,16 @@ static const struct regulator_desc wm8350_reg[NUM_WM8350_REGULATORS] = {
 static irqreturn_t pmic_uv_handler(int irq, void *data)
 {
 	struct regulator_dev *rdev = (struct regulator_dev *)data;
-	struct wm8350 *wm8350 = rdev_get_drvdata(rdev);
 
 	mutex_lock(&rdev->mutex);
 	if (irq == WM8350_IRQ_CS1 || irq == WM8350_IRQ_CS2)
 		regulator_notifier_call_chain(rdev,
 					      REGULATOR_EVENT_REGULATION_OUT,
-					      wm8350);
+					      NULL);
 	else
 		regulator_notifier_call_chain(rdev,
 					      REGULATOR_EVENT_UNDER_VOLTAGE,
-					      wm8350);
+					      NULL);
 	mutex_unlock(&rdev->mutex);
 
 	return IRQ_HANDLED;
diff --git a/drivers/rpmsg/virtio_rpmsg_bus.c b/drivers/rpmsg/virtio_rpmsg_bus.c
index 92f6af6da699..73354ee27877 100644
--- a/drivers/rpmsg/virtio_rpmsg_bus.c
+++ b/drivers/rpmsg/virtio_rpmsg_bus.c
@@ -951,6 +951,7 @@ static int rpmsg_probe(struct virtio_device *vdev)
 	void *bufs_va;
 	int err = 0, i;
 	size_t total_buf_space;
+	bool notify;
 
 	vrp = kzalloc(sizeof(*vrp), GFP_KERNEL);
 	if (!vrp)
@@ -1030,8 +1031,22 @@ static int rpmsg_probe(struct virtio_device *vdev)
 		}
 	}
 
+	/*
+	 * Prepare to kick but don't notify yet - we can't do this before
+	 * device is ready.
+	 */
+	notify = virtqueue_kick_prepare(vrp->rvq);
+
+	/* From this point on, we can notify and get callbacks. */
+	virtio_device_ready(vdev);
+
 	/* tell the remote processor it can start sending messages */
-	virtqueue_kick(vrp->rvq);
+	/*
+	 * this might be concurrent with callbacks, but we are only
+	 * doing notify, not a full kick here, so that's ok.
+	 */
+	if (notify)
+		virtqueue_notify(vrp->rvq);
 
 	dev_info(&vdev->dev, "rpmsg host is online\n");
 
diff --git a/drivers/rtc/class.c b/drivers/rtc/class.c
index 472a5adc4642..c29ba7e14304 100644
--- a/drivers/rtc/class.c
+++ b/drivers/rtc/class.c
@@ -55,7 +55,7 @@ static int rtc_suspend(struct device *dev)
 	struct timespec64	delta, delta_delta;
 	int err;
 
-	if (has_persistent_clock())
+	if (timekeeping_rtc_skipsuspend())
 		return 0;
 
 	if (strcmp(dev_name(&rtc->dev), CONFIG_RTC_HCTOSYS_DEVICE) != 0)
@@ -102,7 +102,7 @@ static int rtc_resume(struct device *dev)
 	struct timespec64	sleep_time;
 	int err;
 
-	if (has_persistent_clock())
+	if (timekeeping_rtc_skipresume())
 		return 0;
 
 	rtc_hctosys_ret = -ENODEV;
@@ -117,10 +117,6 @@ static int rtc_resume(struct device *dev)
 		return 0;
 	}
 
-	if (rtc_valid_tm(&tm) != 0) {
-		pr_debug("%s:  bogus resume time\n", dev_name(&rtc->dev));
-		return 0;
-	}
 	new_rtc.tv_sec = rtc_tm_to_time64(&tm);
 	new_rtc.tv_nsec = 0;
 
diff --git a/drivers/rtc/interface.c b/drivers/rtc/interface.c
index 37215cf983e9..d43ee409a5f2 100644
--- a/drivers/rtc/interface.c
+++ b/drivers/rtc/interface.c
@@ -72,7 +72,11 @@ int rtc_set_time(struct rtc_device *rtc, struct rtc_time *tm)
 		err = -ENODEV;
 	else if (rtc->ops->set_time)
 		err = rtc->ops->set_time(rtc->dev.parent, tm);
-	else if (rtc->ops->set_mmss) {
+	else if (rtc->ops->set_mmss64) {
+		time64_t secs64 = rtc_tm_to_time64(tm);
+
+		err = rtc->ops->set_mmss64(rtc->dev.parent, secs64);
+	} else if (rtc->ops->set_mmss) {
 		time64_t secs64 = rtc_tm_to_time64(tm);
 		err = rtc->ops->set_mmss(rtc->dev.parent, secs64);
 	} else
@@ -96,6 +100,8 @@ int rtc_set_mmss(struct rtc_device *rtc, unsigned long secs)
 
 	if (!rtc->ops)
 		err = -ENODEV;
+	else if (rtc->ops->set_mmss64)
+		err = rtc->ops->set_mmss64(rtc->dev.parent, secs);
 	else if (rtc->ops->set_mmss)
 		err = rtc->ops->set_mmss(rtc->dev.parent, secs);
 	else if (rtc->ops->read_time && rtc->ops->set_time) {
diff --git a/drivers/rtc/rtc-ab3100.c b/drivers/rtc/rtc-ab3100.c
index 1d0340fdb820..9b725c553058 100644
--- a/drivers/rtc/rtc-ab3100.c
+++ b/drivers/rtc/rtc-ab3100.c
@@ -43,21 +43,21 @@
 /*
  * RTC clock functions and device struct declaration
  */
-static int ab3100_rtc_set_mmss(struct device *dev, unsigned long secs)
+static int ab3100_rtc_set_mmss(struct device *dev, time64_t secs)
 {
 	u8 regs[] = {AB3100_TI0, AB3100_TI1, AB3100_TI2,
 		     AB3100_TI3, AB3100_TI4, AB3100_TI5};
 	unsigned char buf[6];
-	u64 fat_time = (u64) secs * AB3100_RTC_CLOCK_RATE * 2;
+	u64 hw_counter = secs * AB3100_RTC_CLOCK_RATE * 2;
 	int err = 0;
 	int i;
 
-	buf[0] = (fat_time) & 0xFF;
-	buf[1] = (fat_time >> 8) & 0xFF;
-	buf[2] = (fat_time >> 16) & 0xFF;
-	buf[3] = (fat_time >> 24) & 0xFF;
-	buf[4] = (fat_time >> 32) & 0xFF;
-	buf[5] = (fat_time >> 40) & 0xFF;
+	buf[0] = (hw_counter) & 0xFF;
+	buf[1] = (hw_counter >> 8) & 0xFF;
+	buf[2] = (hw_counter >> 16) & 0xFF;
+	buf[3] = (hw_counter >> 24) & 0xFF;
+	buf[4] = (hw_counter >> 32) & 0xFF;
+	buf[5] = (hw_counter >> 40) & 0xFF;
 
 	for (i = 0; i < 6; i++) {
 		err = abx500_set_register_interruptible(dev, 0,
@@ -75,7 +75,7 @@ static int ab3100_rtc_set_mmss(struct device *dev, unsigned long secs)
 
 static int ab3100_rtc_read_time(struct device *dev, struct rtc_time *tm)
 {
-	unsigned long time;
+	time64_t time;
 	u8 rtcval;
 	int err;
 
@@ -88,7 +88,7 @@ static int ab3100_rtc_read_time(struct device *dev, struct rtc_time *tm)
 		dev_info(dev, "clock not set (lost power)");
 		return -EINVAL;
 	} else {
-		u64 fat_time;
+		u64 hw_counter;
 		u8 buf[6];
 
 		/* Read out time registers */
@@ -98,22 +98,21 @@ static int ab3100_rtc_read_time(struct device *dev, struct rtc_time *tm)
 		if (err != 0)
 			return err;
 
-		fat_time = ((u64) buf[5] << 40) | ((u64) buf[4] << 32) |
+		hw_counter = ((u64) buf[5] << 40) | ((u64) buf[4] << 32) |
 			((u64) buf[3] << 24) | ((u64) buf[2] << 16) |
 			((u64) buf[1] << 8) | (u64) buf[0];
-		time = (unsigned long) (fat_time /
-					(u64) (AB3100_RTC_CLOCK_RATE * 2));
+		time = hw_counter / (u64) (AB3100_RTC_CLOCK_RATE * 2);
 	}
 
-	rtc_time_to_tm(time, tm);
+	rtc_time64_to_tm(time, tm);
 
 	return rtc_valid_tm(tm);
 }
 
 static int ab3100_rtc_read_alarm(struct device *dev, struct rtc_wkalrm *alarm)
 {
-	unsigned long time;
-	u64 fat_time;
+	time64_t time;
+	u64 hw_counter;
 	u8 buf[6];
 	u8 rtcval;
 	int err;
@@ -134,11 +133,11 @@ static int ab3100_rtc_read_alarm(struct device *dev, struct rtc_wkalrm *alarm)
 						     AB3100_AL0, buf, 4);
 	if (err)
 		return err;
-	fat_time = ((u64) buf[3] << 40) | ((u64) buf[2] << 32) |
+	hw_counter = ((u64) buf[3] << 40) | ((u64) buf[2] << 32) |
 		((u64) buf[1] << 24) | ((u64) buf[0] << 16);
-	time = (unsigned long) (fat_time / (u64) (AB3100_RTC_CLOCK_RATE * 2));
+	time = hw_counter / (u64) (AB3100_RTC_CLOCK_RATE * 2);
 
-	rtc_time_to_tm(time, &alarm->time);
+	rtc_time64_to_tm(time, &alarm->time);
 
 	return rtc_valid_tm(&alarm->time);
 }
@@ -147,17 +146,17 @@ static int ab3100_rtc_set_alarm(struct device *dev, struct rtc_wkalrm *alarm)
 {
 	u8 regs[] = {AB3100_AL0, AB3100_AL1, AB3100_AL2, AB3100_AL3};
 	unsigned char buf[4];
-	unsigned long secs;
-	u64 fat_time;
+	time64_t secs;
+	u64 hw_counter;
 	int err;
 	int i;
 
-	rtc_tm_to_time(&alarm->time, &secs);
-	fat_time = (u64) secs * AB3100_RTC_CLOCK_RATE * 2;
-	buf[0] = (fat_time >> 16) & 0xFF;
-	buf[1] = (fat_time >> 24) & 0xFF;
-	buf[2] = (fat_time >> 32) & 0xFF;
-	buf[3] = (fat_time >> 40) & 0xFF;
+	secs = rtc_tm_to_time64(&alarm->time);
+	hw_counter = secs * AB3100_RTC_CLOCK_RATE * 2;
+	buf[0] = (hw_counter >> 16) & 0xFF;
+	buf[1] = (hw_counter >> 24) & 0xFF;
+	buf[2] = (hw_counter >> 32) & 0xFF;
+	buf[3] = (hw_counter >> 40) & 0xFF;
 
 	/* Set the alarm */
 	for (i = 0; i < 4; i++) {
@@ -193,7 +192,7 @@ static int ab3100_rtc_irq_enable(struct device *dev, unsigned int enabled)
 
 static const struct rtc_class_ops ab3100_rtc_ops = {
 	.read_time	= ab3100_rtc_read_time,
-	.set_mmss	= ab3100_rtc_set_mmss,
+	.set_mmss64	= ab3100_rtc_set_mmss,
 	.read_alarm	= ab3100_rtc_read_alarm,
 	.set_alarm	= ab3100_rtc_set_alarm,
 	.alarm_irq_enable = ab3100_rtc_irq_enable,
diff --git a/drivers/rtc/rtc-at91rm9200.c b/drivers/rtc/rtc-at91rm9200.c
index 70a5d94cc766..b283a1a573b3 100644
--- a/drivers/rtc/rtc-at91rm9200.c
+++ b/drivers/rtc/rtc-at91rm9200.c
@@ -31,6 +31,7 @@
 #include <linux/io.h>
 #include <linux/of.h>
 #include <linux/of_device.h>
+#include <linux/suspend.h>
 #include <linux/uaccess.h>
 
 #include "rtc-at91rm9200.h"
@@ -54,6 +55,10 @@ static void __iomem *at91_rtc_regs;
 static int irq;
 static DEFINE_SPINLOCK(at91_rtc_lock);
 static u32 at91_rtc_shadow_imr;
+static bool suspended;
+static DEFINE_SPINLOCK(suspended_lock);
+static unsigned long cached_events;
+static u32 at91_rtc_imr;
 
 static void at91_rtc_write_ier(u32 mask)
 {
@@ -290,7 +295,9 @@ static irqreturn_t at91_rtc_interrupt(int irq, void *dev_id)
 	struct rtc_device *rtc = platform_get_drvdata(pdev);
 	unsigned int rtsr;
 	unsigned long events = 0;
+	int ret = IRQ_NONE;
 
+	spin_lock(&suspended_lock);
 	rtsr = at91_rtc_read(AT91_RTC_SR) & at91_rtc_read_imr();
 	if (rtsr) {		/* this interrupt is shared!  Is it ours? */
 		if (rtsr & AT91_RTC_ALARM)
@@ -304,14 +311,22 @@ static irqreturn_t at91_rtc_interrupt(int irq, void *dev_id)
 
 		at91_rtc_write(AT91_RTC_SCCR, rtsr);	/* clear status reg */
 
-		rtc_update_irq(rtc, 1, events);
+		if (!suspended) {
+			rtc_update_irq(rtc, 1, events);
 
-		dev_dbg(&pdev->dev, "%s(): num=%ld, events=0x%02lx\n", __func__,
-			events >> 8, events & 0x000000FF);
+			dev_dbg(&pdev->dev, "%s(): num=%ld, events=0x%02lx\n",
+				__func__, events >> 8, events & 0x000000FF);
+		} else {
+			cached_events |= events;
+			at91_rtc_write_idr(at91_rtc_imr);
+			pm_system_wakeup();
+		}
 
-		return IRQ_HANDLED;
+		ret = IRQ_HANDLED;
 	}
-	return IRQ_NONE;		/* not handled */
+	spin_unlock(&suspended_lock);
+
+	return ret;
 }
 
 static const struct at91_rtc_config at91rm9200_config = {
@@ -401,8 +416,8 @@ static int __init at91_rtc_probe(struct platform_device *pdev)
 					AT91_RTC_CALEV);
 
 	ret = devm_request_irq(&pdev->dev, irq, at91_rtc_interrupt,
-				IRQF_SHARED,
-				"at91_rtc", pdev);
+			       IRQF_SHARED | IRQF_COND_SUSPEND,
+			       "at91_rtc", pdev);
 	if (ret) {
 		dev_err(&pdev->dev, "IRQ %d already in use.\n", irq);
 		return ret;
@@ -454,8 +469,6 @@ static void at91_rtc_shutdown(struct platform_device *pdev)
 
 /* AT91RM9200 RTC Power management control */
 
-static u32 at91_rtc_imr;
-
 static int at91_rtc_suspend(struct device *dev)
 {
 	/* this IRQ is shared with DBGU and other hardware which isn't
@@ -464,21 +477,42 @@ static int at91_rtc_suspend(struct device *dev)
 	at91_rtc_imr = at91_rtc_read_imr()
 			& (AT91_RTC_ALARM|AT91_RTC_SECEV);
 	if (at91_rtc_imr) {
-		if (device_may_wakeup(dev))
+		if (device_may_wakeup(dev)) {
+			unsigned long flags;
+
 			enable_irq_wake(irq);
-		else
+
+			spin_lock_irqsave(&suspended_lock, flags);
+			suspended = true;
+			spin_unlock_irqrestore(&suspended_lock, flags);
+		} else {
 			at91_rtc_write_idr(at91_rtc_imr);
+		}
 	}
 	return 0;
 }
 
 static int at91_rtc_resume(struct device *dev)
 {
+	struct rtc_device *rtc = dev_get_drvdata(dev);
+
 	if (at91_rtc_imr) {
-		if (device_may_wakeup(dev))
+		if (device_may_wakeup(dev)) {
+			unsigned long flags;
+
+			spin_lock_irqsave(&suspended_lock, flags);
+
+			if (cached_events) {
+				rtc_update_irq(rtc, 1, cached_events);
+				cached_events = 0;
+			}
+
+			suspended = false;
+			spin_unlock_irqrestore(&suspended_lock, flags);
+
 			disable_irq_wake(irq);
-		else
-			at91_rtc_write_ier(at91_rtc_imr);
+		}
+		at91_rtc_write_ier(at91_rtc_imr);
 	}
 	return 0;
 }
diff --git a/drivers/rtc/rtc-at91sam9.c b/drivers/rtc/rtc-at91sam9.c
index 2183fd2750ab..5ccaee32df72 100644
--- a/drivers/rtc/rtc-at91sam9.c
+++ b/drivers/rtc/rtc-at91sam9.c
@@ -23,6 +23,7 @@
 #include <linux/io.h>
 #include <linux/mfd/syscon.h>
 #include <linux/regmap.h>
+#include <linux/suspend.h>
 #include <linux/clk.h>
 
 /*
@@ -77,6 +78,9 @@ struct sam9_rtc {
 	unsigned int		gpbr_offset;
 	int 			irq;
 	struct clk		*sclk;
+	bool			suspended;
+	unsigned long		events;
+	spinlock_t		lock;
 };
 
 #define rtt_readl(rtc, field) \
@@ -271,14 +275,9 @@ static int at91_rtc_proc(struct device *dev, struct seq_file *seq)
 	return 0;
 }
 
-/*
- * IRQ handler for the RTC
- */
-static irqreturn_t at91_rtc_interrupt(int irq, void *_rtc)
+static irqreturn_t at91_rtc_cache_events(struct sam9_rtc *rtc)
 {
-	struct sam9_rtc *rtc = _rtc;
 	u32 sr, mr;
-	unsigned long events = 0;
 
 	/* Shared interrupt may be for another device.  Note: reading
 	 * SR clears it, so we must only read it in this irq handler!
@@ -290,18 +289,54 @@ static irqreturn_t at91_rtc_interrupt(int irq, void *_rtc)
 
 	/* alarm status */
 	if (sr & AT91_RTT_ALMS)
-		events |= (RTC_AF | RTC_IRQF);
+		rtc->events |= (RTC_AF | RTC_IRQF);
 
 	/* timer update/increment */
 	if (sr & AT91_RTT_RTTINC)
-		events |= (RTC_UF | RTC_IRQF);
+		rtc->events |= (RTC_UF | RTC_IRQF);
+
+	return IRQ_HANDLED;
+}
+
+static void at91_rtc_flush_events(struct sam9_rtc *rtc)
+{
+	if (!rtc->events)
+		return;
 
-	rtc_update_irq(rtc->rtcdev, 1, events);
+	rtc_update_irq(rtc->rtcdev, 1, rtc->events);
+	rtc->events = 0;
 
 	pr_debug("%s: num=%ld, events=0x%02lx\n", __func__,
-		events >> 8, events & 0x000000FF);
+		rtc->events >> 8, rtc->events & 0x000000FF);
+}
 
-	return IRQ_HANDLED;
+/*
+ * IRQ handler for the RTC
+ */
+static irqreturn_t at91_rtc_interrupt(int irq, void *_rtc)
+{
+	struct sam9_rtc *rtc = _rtc;
+	int ret;
+
+	spin_lock(&rtc->lock);
+
+	ret = at91_rtc_cache_events(rtc);
+
+	/* We're called in suspended state */
+	if (rtc->suspended) {
+		/* Mask irqs coming from this peripheral */
+		rtt_writel(rtc, MR,
+			   rtt_readl(rtc, MR) &
+			   ~(AT91_RTT_ALMIEN | AT91_RTT_RTTINCIEN));
+		/* Trigger a system wakeup */
+		pm_system_wakeup();
+	} else {
+		at91_rtc_flush_events(rtc);
+	}
+
+	spin_unlock(&rtc->lock);
+
+	return ret;
 }
 
 static const struct rtc_class_ops at91_rtc_ops = {
@@ -421,7 +456,8 @@ static int at91_rtc_probe(struct platform_device *pdev)
 
 	/* register irq handler after we know what name we'll use */
 	ret = devm_request_irq(&pdev->dev, rtc->irq, at91_rtc_interrupt,
-				IRQF_SHARED, dev_name(&rtc->rtcdev->dev), rtc);
+			       IRQF_SHARED | IRQF_COND_SUSPEND,
+			       dev_name(&rtc->rtcdev->dev), rtc);
 	if (ret) {
 		dev_dbg(&pdev->dev, "can't share IRQ %d?\n", rtc->irq);
 		return ret;
@@ -482,7 +518,12 @@ static int at91_rtc_suspend(struct device *dev)
 	rtc->imr = mr & (AT91_RTT_ALMIEN | AT91_RTT_RTTINCIEN);
 	if (rtc->imr) {
 		if (device_may_wakeup(dev) && (mr & AT91_RTT_ALMIEN)) {
+			unsigned long flags;
+
 			enable_irq_wake(rtc->irq);
+			spin_lock_irqsave(&rtc->lock, flags);
+			rtc->suspended = true;
+			spin_unlock_irqrestore(&rtc->lock, flags);
 			/* don't let RTTINC cause wakeups */
 			if (mr & AT91_RTT_RTTINCIEN)
 				rtt_writel(rtc, MR, mr & ~AT91_RTT_RTTINCIEN);
@@ -499,10 +540,18 @@ static int at91_rtc_resume(struct device *dev)
 	u32		mr;
 
 	if (rtc->imr) {
+		unsigned long flags;
+
 		if (device_may_wakeup(dev))
 			disable_irq_wake(rtc->irq);
 		mr = rtt_readl(rtc, MR);
 		rtt_writel(rtc, MR, mr | rtc->imr);
+
+		spin_lock_irqsave(&rtc->lock, flags);
+		rtc->suspended = false;
+		at91_rtc_cache_events(rtc);
+		at91_rtc_flush_events(rtc);
+		spin_unlock_irqrestore(&rtc->lock, flags);
 	}
 
 	return 0;
diff --git a/drivers/rtc/rtc-ds1685.c b/drivers/rtc/rtc-ds1685.c
index 8c3bfcb115b7..803869c7d7c2 100644
--- a/drivers/rtc/rtc-ds1685.c
+++ b/drivers/rtc/rtc-ds1685.c
@@ -399,21 +399,21 @@ ds1685_rtc_read_alarm(struct device *dev, struct rtc_wkalrm *alrm)
 	 * of this RTC chip.  We check for it anyways in case support is
 	 * added in the future.
 	 */
-	if (unlikely((seconds >= 0xc0) && (seconds <= 0xff)))
+	if (unlikely(seconds >= 0xc0))
 		alrm->time.tm_sec = -1;
 	else
 		alrm->time.tm_sec = ds1685_rtc_bcd2bin(rtc, seconds,
 						       RTC_SECS_BCD_MASK,
 						       RTC_SECS_BIN_MASK);
 
-	if (unlikely((minutes >= 0xc0) && (minutes <= 0xff)))
+	if (unlikely(minutes >= 0xc0))
 		alrm->time.tm_min = -1;
 	else
 		alrm->time.tm_min = ds1685_rtc_bcd2bin(rtc, minutes,
 						       RTC_MINS_BCD_MASK,
 						       RTC_MINS_BIN_MASK);
 
-	if (unlikely((hours >= 0xc0) && (hours <= 0xff)))
+	if (unlikely(hours >= 0xc0))
 		alrm->time.tm_hour = -1;
 	else
 		alrm->time.tm_hour = ds1685_rtc_bcd2bin(rtc, hours,
@@ -472,13 +472,13 @@ ds1685_rtc_set_alarm(struct device *dev, struct rtc_wkalrm *alrm)
 	 * field, and we only support four fields.  We put the support
 	 * here anyways for the future.
 	 */
-	if (unlikely((seconds >= 0xc0) && (seconds <= 0xff)))
+	if (unlikely(seconds >= 0xc0))
 		seconds = 0xff;
 
-	if (unlikely((minutes >= 0xc0) && (minutes <= 0xff)))
+	if (unlikely(minutes >= 0xc0))
 		minutes = 0xff;
 
-	if (unlikely((hours >= 0xc0) && (hours <= 0xff)))
+	if (unlikely(hours >= 0xc0))
 		hours = 0xff;
 
 	alrm->time.tm_mon	= -1;
@@ -528,7 +528,6 @@ ds1685_rtc_set_alarm(struct device *dev, struct rtc_wkalrm *alrm)
 /* ----------------------------------------------------------------------- */
 /* /dev/rtcX Interface functions */
 
-#ifdef CONFIG_RTC_INTF_DEV
 /**
  * ds1685_rtc_alarm_irq_enable - replaces ioctl() RTC_AIE on/off.
  * @dev: pointer to device structure.
@@ -557,7 +556,6 @@ ds1685_rtc_alarm_irq_enable(struct device *dev, unsigned int enabled)
 
 	return 0;
 }
-#endif
 /* ----------------------------------------------------------------------- */
 
 
@@ -1612,7 +1610,7 @@ ds1685_rtc_sysfs_time_regs_show(struct device *dev,
 		ds1685_rtc_sysfs_time_regs_lookup(attr->attr.name, false);
 
 	/* Make sure we actually matched something. */
-	if (!bcd_reg_info && !bin_reg_info)
+	if (!bcd_reg_info || !bin_reg_info)
 		return -EINVAL;
 
 	/* bcd_reg_info->reg == bin_reg_info->reg. */
@@ -1650,7 +1648,7 @@ ds1685_rtc_sysfs_time_regs_store(struct device *dev,
 		return -EINVAL;
 
 	/* Make sure we actually matched something. */
-	if (!bcd_reg_info && !bin_reg_info)
+	if (!bcd_reg_info || !bin_reg_info)
 		return -EINVAL;
 
 	/* Check for a valid range. */
diff --git a/drivers/rtc/rtc-mc13xxx.c b/drivers/rtc/rtc-mc13xxx.c
index 5bce904b7ee6..32df1d812367 100644
--- a/drivers/rtc/rtc-mc13xxx.c
+++ b/drivers/rtc/rtc-mc13xxx.c
@@ -83,20 +83,19 @@ static int mc13xxx_rtc_read_time(struct device *dev, struct rtc_time *tm)
 			return ret;
 	} while (days1 != days2);
 
-	rtc_time_to_tm(days1 * SEC_PER_DAY + seconds, tm);
+	rtc_time64_to_tm((time64_t)days1 * SEC_PER_DAY + seconds, tm);
 
 	return rtc_valid_tm(tm);
 }
 
-static int mc13xxx_rtc_set_mmss(struct device *dev, unsigned long secs)
+static int mc13xxx_rtc_set_mmss(struct device *dev, time64_t secs)
 {
 	struct mc13xxx_rtc *priv = dev_get_drvdata(dev);
 	unsigned int seconds, days;
 	unsigned int alarmseconds;
 	int ret;
 
-	seconds = secs % SEC_PER_DAY;
-	days = secs / SEC_PER_DAY;
+	days = div_s64_rem(secs, SEC_PER_DAY, &seconds);
 
 	mc13xxx_lock(priv->mc13xxx);
 
@@ -159,7 +158,7 @@ static int mc13xxx_rtc_read_alarm(struct device *dev, struct rtc_wkalrm *alarm)
 {
 	struct mc13xxx_rtc *priv = dev_get_drvdata(dev);
 	unsigned seconds, days;
-	unsigned long s1970;
+	time64_t s1970;
 	int enabled, pending;
 	int ret;
 
@@ -189,10 +188,10 @@ out:
 	alarm->enabled = enabled;
 	alarm->pending = pending;
 
-	s1970 = days * SEC_PER_DAY + seconds;
+	s1970 = (time64_t)days * SEC_PER_DAY + seconds;
 
-	rtc_time_to_tm(s1970, &alarm->time);
-	dev_dbg(dev, "%s: %lu\n", __func__, s1970);
+	rtc_time64_to_tm(s1970, &alarm->time);
+	dev_dbg(dev, "%s: %lld\n", __func__, (long long)s1970);
 
 	return 0;
 }
@@ -200,8 +199,8 @@ out:
 static int mc13xxx_rtc_set_alarm(struct device *dev, struct rtc_wkalrm *alarm)
 {
 	struct mc13xxx_rtc *priv = dev_get_drvdata(dev);
-	unsigned long s1970;
-	unsigned seconds, days;
+	time64_t s1970;
+	u32 seconds, days;
 	int ret;
 
 	mc13xxx_lock(priv->mc13xxx);
@@ -215,20 +214,17 @@ static int mc13xxx_rtc_set_alarm(struct device *dev, struct rtc_wkalrm *alarm)
 	if (unlikely(ret))
 		goto out;
 
-	ret = rtc_tm_to_time(&alarm->time, &s1970);
-	if (unlikely(ret))
-		goto out;
+	s1970 = rtc_tm_to_time64(&alarm->time);
 
-	dev_dbg(dev, "%s: o%2.s %lu\n", __func__, alarm->enabled ? "n" : "ff",
-			s1970);
+	dev_dbg(dev, "%s: o%2.s %lld\n", __func__, alarm->enabled ? "n" : "ff",
+			(long long)s1970);
 
 	ret = mc13xxx_rtc_irq_enable_unlocked(dev, alarm->enabled,
 			MC13XXX_IRQ_TODA);
 	if (unlikely(ret))
 		goto out;
 
-	seconds = s1970 % SEC_PER_DAY;
-	days = s1970 / SEC_PER_DAY;
+	days = div_s64_rem(s1970, SEC_PER_DAY, &seconds);
 
 	ret = mc13xxx_reg_write(priv->mc13xxx, MC13XXX_RTCDAYA, days);
 	if (unlikely(ret))
@@ -268,7 +264,7 @@ static irqreturn_t mc13xxx_rtc_update_handler(int irq, void *dev)
 
 static const struct rtc_class_ops mc13xxx_rtc_ops = {
 	.read_time = mc13xxx_rtc_read_time,
-	.set_mmss = mc13xxx_rtc_set_mmss,
+	.set_mmss64 = mc13xxx_rtc_set_mmss,
 	.read_alarm = mc13xxx_rtc_read_alarm,
 	.set_alarm = mc13xxx_rtc_set_alarm,
 	.alarm_irq_enable = mc13xxx_rtc_alarm_irq_enable,
diff --git a/drivers/rtc/rtc-mrst.c b/drivers/rtc/rtc-mrst.c
index e2436d140175..3a6fd3a8a2ec 100644
--- a/drivers/rtc/rtc-mrst.c
+++ b/drivers/rtc/rtc-mrst.c
@@ -413,8 +413,8 @@ static void rtc_mrst_do_remove(struct device *dev)
 	mrst->dev = NULL;
 }
 
-#ifdef	CONFIG_PM
-static int mrst_suspend(struct device *dev, pm_message_t mesg)
+#ifdef CONFIG_PM_SLEEP
+static int mrst_suspend(struct device *dev)
 {
 	struct mrst_rtc	*mrst = dev_get_drvdata(dev);
 	unsigned char	tmp;
@@ -453,7 +453,7 @@ static int mrst_suspend(struct device *dev, pm_message_t mesg)
  */
 static inline int mrst_poweroff(struct device *dev)
 {
-	return mrst_suspend(dev, PMSG_HIBERNATE);
+	return mrst_suspend(dev);
 }
 
 static int mrst_resume(struct device *dev)
@@ -490,9 +490,11 @@ static int mrst_resume(struct device *dev)
 	return 0;
 }
 
+static SIMPLE_DEV_PM_OPS(mrst_pm_ops, mrst_suspend, mrst_resume);
+#define MRST_PM_OPS (&mrst_pm_ops)
+
 #else
-#define	mrst_suspend	NULL
-#define	mrst_resume	NULL
+#define MRST_PM_OPS NULL
 
 static inline int mrst_poweroff(struct device *dev)
 {
@@ -529,9 +531,8 @@ static struct platform_driver vrtc_mrst_platform_driver = {
 	.remove		= vrtc_mrst_platform_remove,
 	.shutdown	= vrtc_mrst_platform_shutdown,
 	.driver = {
-		.name		= (char *) driver_name,
-		.suspend	= mrst_suspend,
-		.resume		= mrst_resume,
+		.name	= driver_name,
+		.pm	= MRST_PM_OPS,
 	}
 };
 
diff --git a/drivers/rtc/rtc-mxc.c b/drivers/rtc/rtc-mxc.c
index 3c3f8d10ab43..09d422b9f7f7 100644
--- a/drivers/rtc/rtc-mxc.c
+++ b/drivers/rtc/rtc-mxc.c
@@ -106,7 +106,7 @@ static inline int is_imx1_rtc(struct rtc_plat_data *data)
  * This function is used to obtain the RTC time or the alarm value in
  * second.
  */
-static u32 get_alarm_or_time(struct device *dev, int time_alarm)
+static time64_t get_alarm_or_time(struct device *dev, int time_alarm)
 {
 	struct platform_device *pdev = to_platform_device(dev);
 	struct rtc_plat_data *pdata = platform_get_drvdata(pdev);
@@ -129,29 +129,28 @@ static u32 get_alarm_or_time(struct device *dev, int time_alarm)
 	hr = hr_min >> 8;
 	min = hr_min & 0xff;
 
-	return (((day * 24 + hr) * 60) + min) * 60 + sec;
+	return ((((time64_t)day * 24 + hr) * 60) + min) * 60 + sec;
 }
 
 /*
  * This function sets the RTC alarm value or the time value.
  */
-static void set_alarm_or_time(struct device *dev, int time_alarm, u32 time)
+static void set_alarm_or_time(struct device *dev, int time_alarm, time64_t time)
 {
-	u32 day, hr, min, sec, temp;
+	u32 tod, day, hr, min, sec, temp;
 	struct platform_device *pdev = to_platform_device(dev);
 	struct rtc_plat_data *pdata = platform_get_drvdata(pdev);
 	void __iomem *ioaddr = pdata->ioaddr;
 
-	day = time / 86400;
-	time -= day * 86400;
+	day = div_s64_rem(time, 86400, &tod);
 
 	/* time is within a day now */
-	hr = time / 3600;
-	time -= hr * 3600;
+	hr = tod / 3600;
+	tod -= hr * 3600;
 
 	/* time is within an hour now */
-	min = time / 60;
-	sec = time - min * 60;
+	min = tod / 60;
+	sec = tod - min * 60;
 
 	temp = (hr << 8) + min;
 
@@ -173,29 +172,18 @@ static void set_alarm_or_time(struct device *dev, int time_alarm, u32 time)
  * This function updates the RTC alarm registers and then clears all the
  * interrupt status bits.
  */
-static int rtc_update_alarm(struct device *dev, struct rtc_time *alrm)
+static void rtc_update_alarm(struct device *dev, struct rtc_time *alrm)
 {
-	struct rtc_time alarm_tm, now_tm;
-	unsigned long now, time;
+	time64_t time;
 	struct platform_device *pdev = to_platform_device(dev);
 	struct rtc_plat_data *pdata = platform_get_drvdata(pdev);
 	void __iomem *ioaddr = pdata->ioaddr;
 
-	now = get_alarm_or_time(dev, MXC_RTC_TIME);
-	rtc_time_to_tm(now, &now_tm);
-	alarm_tm.tm_year = now_tm.tm_year;
-	alarm_tm.tm_mon = now_tm.tm_mon;
-	alarm_tm.tm_mday = now_tm.tm_mday;
-	alarm_tm.tm_hour = alrm->tm_hour;
-	alarm_tm.tm_min = alrm->tm_min;
-	alarm_tm.tm_sec = alrm->tm_sec;
-	rtc_tm_to_time(&alarm_tm, &time);
+	time = rtc_tm_to_time64(alrm);
 
 	/* clear all the interrupt status bits */
 	writew(readw(ioaddr + RTC_RTCISR), ioaddr + RTC_RTCISR);
 	set_alarm_or_time(dev, MXC_RTC_ALARM, time);
-
-	return 0;
 }
 
 static void mxc_rtc_irq_enable(struct device *dev, unsigned int bit,
@@ -283,14 +271,14 @@ static int mxc_rtc_alarm_irq_enable(struct device *dev, unsigned int enabled)
  */
 static int mxc_rtc_read_time(struct device *dev, struct rtc_time *tm)
 {
-	u32 val;
+	time64_t val;
 
 	/* Avoid roll-over from reading the different registers */
 	do {
 		val = get_alarm_or_time(dev, MXC_RTC_TIME);
 	} while (val != get_alarm_or_time(dev, MXC_RTC_TIME));
 
-	rtc_time_to_tm(val, tm);
+	rtc_time64_to_tm(val, tm);
 
 	return 0;
 }
@@ -298,7 +286,7 @@ static int mxc_rtc_read_time(struct device *dev, struct rtc_time *tm)
 /*
  * This function sets the internal RTC time based on tm in Gregorian date.
  */
-static int mxc_rtc_set_mmss(struct device *dev, unsigned long time)
+static int mxc_rtc_set_mmss(struct device *dev, time64_t time)
 {
 	struct platform_device *pdev = to_platform_device(dev);
 	struct rtc_plat_data *pdata = platform_get_drvdata(pdev);
@@ -309,9 +297,9 @@ static int mxc_rtc_set_mmss(struct device *dev, unsigned long time)
 	if (is_imx1_rtc(pdata)) {
 		struct rtc_time tm;
 
-		rtc_time_to_tm(time, &tm);
+		rtc_time64_to_tm(time, &tm);
 		tm.tm_year = 70;
-		rtc_tm_to_time(&tm, &time);
+		time = rtc_tm_to_time64(&tm);
 	}
 
 	/* Avoid roll-over from reading the different registers */
@@ -333,7 +321,7 @@ static int mxc_rtc_read_alarm(struct device *dev, struct rtc_wkalrm *alrm)
 	struct rtc_plat_data *pdata = platform_get_drvdata(pdev);
 	void __iomem *ioaddr = pdata->ioaddr;
 
-	rtc_time_to_tm(get_alarm_or_time(dev, MXC_RTC_ALARM), &alrm->time);
+	rtc_time64_to_tm(get_alarm_or_time(dev, MXC_RTC_ALARM), &alrm->time);
 	alrm->pending = ((readw(ioaddr + RTC_RTCISR) & RTC_ALM_BIT)) ? 1 : 0;
 
 	return 0;
@@ -346,11 +334,8 @@ static int mxc_rtc_set_alarm(struct device *dev, struct rtc_wkalrm *alrm)
 {
 	struct platform_device *pdev = to_platform_device(dev);
 	struct rtc_plat_data *pdata = platform_get_drvdata(pdev);
-	int ret;
 
-	ret = rtc_update_alarm(dev, &alrm->time);
-	if (ret)
-		return ret;
+	rtc_update_alarm(dev, &alrm->time);
 
 	memcpy(&pdata->g_rtc_alarm, &alrm->time, sizeof(struct rtc_time));
 	mxc_rtc_irq_enable(dev, RTC_ALM_BIT, alrm->enabled);
@@ -362,7 +347,7 @@ static int mxc_rtc_set_alarm(struct device *dev, struct rtc_wkalrm *alrm)
 static struct rtc_class_ops mxc_rtc_ops = {
 	.release		= mxc_rtc_release,
 	.read_time		= mxc_rtc_read_time,
-	.set_mmss		= mxc_rtc_set_mmss,
+	.set_mmss64		= mxc_rtc_set_mmss,
 	.read_alarm		= mxc_rtc_read_alarm,
 	.set_alarm		= mxc_rtc_set_alarm,
 	.alarm_irq_enable	= mxc_rtc_alarm_irq_enable,
diff --git a/drivers/rtc/rtc-s3c.c b/drivers/rtc/rtc-s3c.c
index 4241eeab3386..f4cf6851fae9 100644
--- a/drivers/rtc/rtc-s3c.c
+++ b/drivers/rtc/rtc-s3c.c
@@ -849,6 +849,7 @@ static struct s3c_rtc_data const s3c2443_rtc_data = {
 
 static struct s3c_rtc_data const s3c6410_rtc_data = {
 	.max_user_freq		= 32768,
+	.needs_src_clk		= true,
 	.irq_handler		= s3c6410_rtc_irq,
 	.set_freq		= s3c6410_rtc_setfreq,
 	.enable_tick		= s3c6410_rtc_enable_tick,
diff --git a/drivers/rtc/rtc-test.c b/drivers/rtc/rtc-test.c
index 8f86fa91de1a..3a2da4c892d6 100644
--- a/drivers/rtc/rtc-test.c
+++ b/drivers/rtc/rtc-test.c
@@ -13,6 +13,10 @@
 #include <linux/rtc.h>
 #include <linux/platform_device.h>
 
+static int test_mmss64;
+module_param(test_mmss64, int, 0644);
+MODULE_PARM_DESC(test_mmss64, "Test struct rtc_class_ops.set_mmss64().");
+
 static struct platform_device *test0 = NULL, *test1 = NULL;
 
 static int test_rtc_read_alarm(struct device *dev,
@@ -30,7 +34,13 @@ static int test_rtc_set_alarm(struct device *dev,
 static int test_rtc_read_time(struct device *dev,
 	struct rtc_time *tm)
 {
-	rtc_time_to_tm(get_seconds(), tm);
+	rtc_time64_to_tm(ktime_get_real_seconds(), tm);
+	return 0;
+}
+
+static int test_rtc_set_mmss64(struct device *dev, time64_t secs)
+{
+	dev_info(dev, "%s, secs = %lld\n", __func__, (long long)secs);
 	return 0;
 }
 
@@ -55,7 +65,7 @@ static int test_rtc_alarm_irq_enable(struct device *dev, unsigned int enable)
 	return 0;
 }
 
-static const struct rtc_class_ops test_rtc_ops = {
+static struct rtc_class_ops test_rtc_ops = {
 	.proc = test_rtc_proc,
 	.read_time = test_rtc_read_time,
 	.read_alarm = test_rtc_read_alarm,
@@ -101,6 +111,11 @@ static int test_probe(struct platform_device *plat_dev)
 	int err;
 	struct rtc_device *rtc;
 
+	if (test_mmss64) {
+		test_rtc_ops.set_mmss64 = test_rtc_set_mmss64;
+		test_rtc_ops.set_mmss = NULL;
+	}
+
 	rtc = devm_rtc_device_register(&plat_dev->dev, "test",
 				&test_rtc_ops, THIS_MODULE);
 	if (IS_ERR(rtc)) {
diff --git a/drivers/rtc/systohc.c b/drivers/rtc/systohc.c
index eb71872d0361..7728d5e32bf4 100644
--- a/drivers/rtc/systohc.c
+++ b/drivers/rtc/systohc.c
@@ -11,7 +11,7 @@
  * rtc_set_ntp_time - Save NTP synchronized time to the RTC
  * @now: Current time of day
  *
- * Replacement for the NTP platform function update_persistent_clock
+ * Replacement for the NTP platform function update_persistent_clock64
  * that stores time for later retrieval by rtc_hctosys.
  *
  * Returns 0 on successful RTC update, -ENODEV if a RTC update is not
@@ -35,7 +35,10 @@ int rtc_set_ntp_time(struct timespec64 now)
 	if (rtc) {
 		/* rtc_hctosys exclusively uses UTC, so we call set_time here,
 		 * not set_mmss. */
-		if (rtc->ops && (rtc->ops->set_time || rtc->ops->set_mmss))
+		if (rtc->ops &&
+		    (rtc->ops->set_time ||
+		     rtc->ops->set_mmss64 ||
+		     rtc->ops->set_mmss))
 			err = rtc_set_time(rtc, &tm);
 		rtc_class_close(rtc);
 	}
diff --git a/drivers/s390/block/dcssblk.c b/drivers/s390/block/dcssblk.c
index 96128cb009f3..da212813f2d5 100644
--- a/drivers/s390/block/dcssblk.c
+++ b/drivers/s390/block/dcssblk.c
@@ -547,7 +547,7 @@ dcssblk_add_store(struct device *dev, struct device_attribute *attr, const char
 	 * parse input
 	 */
 	num_of_segments = 0;
-	for (i = 0; ((buf[i] != '\0') && (buf[i] != '\n') && i < count); i++) {
+	for (i = 0; (i < count && (buf[i] != '\0') && (buf[i] != '\n')); i++) {
 		for (j = i; (buf[j] != ':') &&
 			(buf[j] != '\0') &&
 			(buf[j] != '\n') &&
diff --git a/drivers/s390/block/scm_blk_cluster.c b/drivers/s390/block/scm_blk_cluster.c
index 09db45296eed..7497ddde2dd6 100644
--- a/drivers/s390/block/scm_blk_cluster.c
+++ b/drivers/s390/block/scm_blk_cluster.c
@@ -92,7 +92,7 @@ bool scm_reserve_cluster(struct scm_request *scmrq)
 			add = 0;
 			continue;
 		}
-		for (pos = 0; pos <= iter->aob->request.msb_count; pos++) {
+		for (pos = 0; pos < iter->aob->request.msb_count; pos++) {
 			if (clusters_intersect(req, iter->request[pos]) &&
 			    (rq_data_dir(req) == WRITE ||
 			     rq_data_dir(iter->request[pos]) == WRITE)) {
diff --git a/drivers/scsi/be2iscsi/be_main.c b/drivers/scsi/be2iscsi/be_main.c
index a7cc61837818..923a2b5a2439 100644
--- a/drivers/scsi/be2iscsi/be_main.c
+++ b/drivers/scsi/be2iscsi/be_main.c
@@ -5734,9 +5734,9 @@ free_port:
 hba_free:
 	if (phba->msix_enabled)
 		pci_disable_msix(phba->pcidev);
-	iscsi_host_remove(phba->shost);
 	pci_dev_put(phba->pcidev);
 	iscsi_host_free(phba->shost);
+	pci_set_drvdata(pcidev, NULL);
 disable_pci:
 	pci_disable_device(pcidev);
 	return ret;
diff --git a/drivers/scsi/ipr.c b/drivers/scsi/ipr.c
index 9219953ee949..d9afc51af7d3 100644
--- a/drivers/scsi/ipr.c
+++ b/drivers/scsi/ipr.c
@@ -6815,7 +6815,8 @@ static struct ata_port_operations ipr_sata_ops = {
 };
 
 static struct ata_port_info sata_port_info = {
-	.flags		= ATA_FLAG_SATA | ATA_FLAG_PIO_DMA,
+	.flags		= ATA_FLAG_SATA | ATA_FLAG_PIO_DMA |
+			  ATA_FLAG_SAS_HOST,
 	.pio_mask	= ATA_PIO4_ONLY,
 	.mwdma_mask	= ATA_MWDMA2,
 	.udma_mask	= ATA_UDMA6,
diff --git a/drivers/scsi/libsas/sas_ata.c b/drivers/scsi/libsas/sas_ata.c
index 932d9cc98d2f..9c706d8c1441 100644
--- a/drivers/scsi/libsas/sas_ata.c
+++ b/drivers/scsi/libsas/sas_ata.c
@@ -547,7 +547,8 @@ static struct ata_port_operations sas_sata_ops = {
 };
 
 static struct ata_port_info sata_port_info = {
-	.flags = ATA_FLAG_SATA | ATA_FLAG_PIO_DMA | ATA_FLAG_NCQ,
+	.flags = ATA_FLAG_SATA | ATA_FLAG_PIO_DMA | ATA_FLAG_NCQ |
+		 ATA_FLAG_SAS_HOST,
 	.pio_mask = ATA_PIO4,
 	.mwdma_mask = ATA_MWDMA2,
 	.udma_mask = ATA_UDMA6,
diff --git a/drivers/scsi/libsas/sas_discover.c b/drivers/scsi/libsas/sas_discover.c
index 62b58d38ce2e..60de66252fa2 100644
--- a/drivers/scsi/libsas/sas_discover.c
+++ b/drivers/scsi/libsas/sas_discover.c
@@ -500,6 +500,7 @@ static void sas_revalidate_domain(struct work_struct *work)
 	struct sas_discovery_event *ev = to_sas_discovery_event(work);
 	struct asd_sas_port *port = ev->port;
 	struct sas_ha_struct *ha = port->ha;
+	struct domain_device *ddev = port->port_dev;
 
 	/* prevent revalidation from finding sata links in recovery */
 	mutex_lock(&ha->disco_mutex);
@@ -514,8 +515,9 @@ static void sas_revalidate_domain(struct work_struct *work)
 	SAS_DPRINTK("REVALIDATING DOMAIN on port %d, pid:%d\n", port->id,
 		    task_pid_nr(current));
 
-	if (port->port_dev)
-		res = sas_ex_revalidate_domain(port->port_dev);
+	if (ddev && (ddev->dev_type == SAS_FANOUT_EXPANDER_DEVICE ||
+		     ddev->dev_type == SAS_EDGE_EXPANDER_DEVICE))
+		res = sas_ex_revalidate_domain(ddev);
 
 	SAS_DPRINTK("done REVALIDATING DOMAIN on port %d, pid:%d, res 0x%x\n",
 		    port->id, task_pid_nr(current), res);
diff --git a/drivers/scsi/qla2xxx/tcm_qla2xxx.c b/drivers/scsi/qla2xxx/tcm_qla2xxx.c
index 99f43b7fc9ab..ab4879e12ea7 100644
--- a/drivers/scsi/qla2xxx/tcm_qla2xxx.c
+++ b/drivers/scsi/qla2xxx/tcm_qla2xxx.c
@@ -1596,7 +1596,7 @@ static int tcm_qla2xxx_check_initiator_node_acl(
 	/*
 	 * Finally register the new FC Nexus with TCM
 	 */
-	__transport_register_session(se_nacl->se_tpg, se_nacl, se_sess, sess);
+	transport_register_session(se_nacl->se_tpg, se_nacl, se_sess, sess);
 
 	return 0;
 }
diff --git a/drivers/scsi/scsi_error.c b/drivers/scsi/scsi_error.c
index 4cdaffca17fc..c95a4e943fc6 100644
--- a/drivers/scsi/scsi_error.c
+++ b/drivers/scsi/scsi_error.c
@@ -26,6 +26,7 @@
 #include <linux/blkdev.h>
 #include <linux/delay.h>
 #include <linux/jiffies.h>
+#include <asm/unaligned.h>
 
 #include <scsi/scsi.h>
 #include <scsi/scsi_cmnd.h>
@@ -2586,3 +2587,33 @@ void scsi_build_sense_buffer(int desc, u8 *buf, u8 key, u8 asc, u8 ascq)
 	}
 }
 EXPORT_SYMBOL(scsi_build_sense_buffer);
+
+/**
+ * scsi_set_sense_information - set the information field in a
+ *		formatted sense data buffer
+ * @buf:	Where to build sense data
+ * @info:	64-bit information value to be set
+ *
+ **/
+void scsi_set_sense_information(u8 *buf, u64 info)
+{
+	if ((buf[0] & 0x7f) == 0x72) {
+		u8 *ucp, len;
+
+		len = buf[7];
+		ucp = (char *)scsi_sense_desc_find(buf, len + 8, 0);
+		if (!ucp) {
+			buf[7] = len + 0xa;
+			ucp = buf + 8 + len;
+		}
+		ucp[0] = 0;
+		ucp[1] = 0xa;
+		ucp[2] = 0x80; /* Valid bit */
+		ucp[3] = 0;
+		put_unaligned_be64(info, &ucp[4]);
+	} else if ((buf[0] & 0x7f) == 0x70) {
+		buf[0] |= 0x80;
+		put_unaligned_be64(info, &buf[3]);
+	}
+}
+EXPORT_SYMBOL(scsi_set_sense_information);
diff --git a/drivers/scsi/scsi_lib.c b/drivers/scsi/scsi_lib.c
index 54d7a6cbb98a..b1a263137a23 100644
--- a/drivers/scsi/scsi_lib.c
+++ b/drivers/scsi/scsi_lib.c
@@ -1311,9 +1311,11 @@ scsi_prep_state_check(struct scsi_device *sdev, struct request *req)
 				    "rejecting I/O to dead device\n");
 			ret = BLKPREP_KILL;
 			break;
-		case SDEV_QUIESCE:
 		case SDEV_BLOCK:
 		case SDEV_CREATED_BLOCK:
+			ret = BLKPREP_DEFER;
+			break;
+		case SDEV_QUIESCE:
 			/*
 			 * If the devices is blocked we defer normal commands.
 			 */
diff --git a/drivers/scsi/ufs/ufshcd.c b/drivers/scsi/ufs/ufshcd.c
index 5d60a868830d..2aa85e398f76 100644
--- a/drivers/scsi/ufs/ufshcd.c
+++ b/drivers/scsi/ufs/ufshcd.c
@@ -4225,22 +4225,15 @@ static struct scsi_host_template ufshcd_driver_template = {
 static int ufshcd_config_vreg_load(struct device *dev, struct ufs_vreg *vreg,
 				   int ua)
 {
-	int ret = 0;
-	struct regulator *reg = vreg->reg;
-	const char *name = vreg->name;
+	int ret;
 
-	BUG_ON(!vreg);
+	if (!vreg)
+		return 0;
 
-	ret = regulator_set_optimum_mode(reg, ua);
-	if (ret >= 0) {
-		/*
-		 * regulator_set_optimum_mode() returns new regulator
-		 * mode upon success.
-		 */
-		ret = 0;
-	} else {
-		dev_err(dev, "%s: %s set optimum mode(ua=%d) failed, err=%d\n",
-				__func__, name, ua, ret);
+	ret = regulator_set_load(vreg->reg, ua);
+	if (ret < 0) {
+		dev_err(dev, "%s: %s set load (ua=%d) failed, err=%d\n",
+				__func__, vreg->name, ua, ret);
 	}
 
 	return ret;
@@ -4249,18 +4242,12 @@ static int ufshcd_config_vreg_load(struct device *dev, struct ufs_vreg *vreg,
 static inline int ufshcd_config_vreg_lpm(struct ufs_hba *hba,
 					 struct ufs_vreg *vreg)
 {
-	if (!vreg)
-		return 0;
-
 	return ufshcd_config_vreg_load(hba->dev, vreg, UFS_VREG_LPM_LOAD_UA);
 }
 
 static inline int ufshcd_config_vreg_hpm(struct ufs_hba *hba,
 					 struct ufs_vreg *vreg)
 {
-	if (!vreg)
-		return 0;
-
 	return ufshcd_config_vreg_load(hba->dev, vreg, vreg->max_uA);
 }
 
diff --git a/drivers/sh/pm_runtime.c b/drivers/sh/pm_runtime.c
index f3ee439d6f0e..cd4c293f0dd0 100644
--- a/drivers/sh/pm_runtime.c
+++ b/drivers/sh/pm_runtime.c
@@ -81,7 +81,9 @@ static int __init sh_pm_runtime_init(void)
 		if (!of_machine_is_compatible("renesas,emev2") &&
 		    !of_machine_is_compatible("renesas,r7s72100") &&
 		    !of_machine_is_compatible("renesas,r8a73a4") &&
+#ifndef CONFIG_PM_GENERIC_DOMAINS_OF
 		    !of_machine_is_compatible("renesas,r8a7740") &&
+#endif
 		    !of_machine_is_compatible("renesas,r8a7778") &&
 		    !of_machine_is_compatible("renesas,r8a7779") &&
 		    !of_machine_is_compatible("renesas,r8a7790") &&
diff --git a/drivers/spi/Kconfig b/drivers/spi/Kconfig
index ab8dfbef6f1b..198f96b7fb45 100644
--- a/drivers/spi/Kconfig
+++ b/drivers/spi/Kconfig
@@ -159,10 +159,9 @@ config SPI_BUTTERFLY
 
 config SPI_CADENCE
 	tristate "Cadence SPI controller"
-	depends on ARM
 	help
 	  This selects the Cadence SPI controller master driver
-	  used by Xilinx Zynq.
+	  used by Xilinx Zynq and ZynqMP.
 
 config SPI_CLPS711X
 	tristate "CLPS711X host SPI controller"
@@ -632,7 +631,7 @@ config SPI_DW_PCI
 
 config SPI_DW_MID_DMA
 	bool "DMA support for DW SPI controller on Intel MID platform"
-	depends on SPI_DW_PCI && INTEL_MID_DMAC
+	depends on SPI_DW_PCI && DW_DMAC_PCI
 
 config SPI_DW_MMIO
 	tristate "Memory-mapped io interface driver for DW SPI core"
diff --git a/drivers/spi/spi-atmel.c b/drivers/spi/spi-atmel.c
index 9af7841f2e8c..a2f40b1b2225 100644
--- a/drivers/spi/spi-atmel.c
+++ b/drivers/spi/spi-atmel.c
@@ -180,11 +180,17 @@
 	  | SPI_BF(name, value))
 
 /* Register access macros */
+#ifdef CONFIG_AVR32
 #define spi_readl(port, reg) \
 	__raw_readl((port)->regs + SPI_##reg)
 #define spi_writel(port, reg, value) \
 	__raw_writel((value), (port)->regs + SPI_##reg)
-
+#else
+#define spi_readl(port, reg) \
+	readl_relaxed((port)->regs + SPI_##reg)
+#define spi_writel(port, reg, value) \
+	writel_relaxed((value), (port)->regs + SPI_##reg)
+#endif
 /* use PIO for small transfers, avoiding DMA setup/teardown overhead and
  * cache operations; better heuristics consider wordsize and bitrate.
  */
@@ -764,17 +770,17 @@ static void atmel_spi_pdc_next_xfer(struct spi_master *master,
 			(unsigned long long)xfer->rx_dma);
 	}
 
-	/* REVISIT: We're waiting for ENDRX before we start the next
+	/* REVISIT: We're waiting for RXBUFF before we start the next
 	 * transfer because we need to handle some difficult timing
-	 * issues otherwise. If we wait for ENDTX in one transfer and
-	 * then starts waiting for ENDRX in the next, it's difficult
-	 * to tell the difference between the ENDRX interrupt we're
-	 * actually waiting for and the ENDRX interrupt of the
+	 * issues otherwise. If we wait for TXBUFE in one transfer and
+	 * then starts waiting for RXBUFF in the next, it's difficult
+	 * to tell the difference between the RXBUFF interrupt we're
+	 * actually waiting for and the RXBUFF interrupt of the
 	 * previous transfer.
 	 *
 	 * It should be doable, though. Just not now...
 	 */
-	spi_writel(as, IER, SPI_BIT(ENDRX) | SPI_BIT(OVRES));
+	spi_writel(as, IER, SPI_BIT(RXBUFF) | SPI_BIT(OVRES));
 	spi_writel(as, PTCR, SPI_BIT(TXTEN) | SPI_BIT(RXTEN));
 }
 
diff --git a/drivers/spi/spi-bcm2835.c b/drivers/spi/spi-bcm2835.c
index 419a782ab6d5..f63864a893c5 100644
--- a/drivers/spi/spi-bcm2835.c
+++ b/drivers/spi/spi-bcm2835.c
@@ -3,6 +3,7 @@
  *
  * Copyright (C) 2012 Chris Boot
  * Copyright (C) 2013 Stephen Warren
+ * Copyright (C) 2015 Martin Sperl
  *
  * This driver is inspired by:
  * spi-ath79.c, Copyright (C) 2009-2011 Gabor Juhos <juhosg@openwrt.org>
@@ -29,6 +30,7 @@
 #include <linux/module.h>
 #include <linux/of.h>
 #include <linux/of_irq.h>
+#include <linux/of_gpio.h>
 #include <linux/of_device.h>
 #include <linux/spi/spi.h>
 
@@ -66,8 +68,10 @@
 #define BCM2835_SPI_CS_CS_10		0x00000002
 #define BCM2835_SPI_CS_CS_01		0x00000001
 
-#define BCM2835_SPI_TIMEOUT_MS	30000
-#define BCM2835_SPI_MODE_BITS	(SPI_CPOL | SPI_CPHA | SPI_CS_HIGH | SPI_NO_CS)
+#define BCM2835_SPI_POLLING_LIMIT_US	30
+#define BCM2835_SPI_TIMEOUT_MS		30000
+#define BCM2835_SPI_MODE_BITS	(SPI_CPOL | SPI_CPHA | SPI_CS_HIGH \
+				| SPI_NO_CS | SPI_3WIRE)
 
 #define DRV_NAME	"spi-bcm2835"
 
@@ -75,10 +79,10 @@ struct bcm2835_spi {
 	void __iomem *regs;
 	struct clk *clk;
 	int irq;
-	struct completion done;
 	const u8 *tx_buf;
 	u8 *rx_buf;
-	int len;
+	int tx_len;
+	int rx_len;
 };
 
 static inline u32 bcm2835_rd(struct bcm2835_spi *bs, unsigned reg)
@@ -91,205 +95,315 @@ static inline void bcm2835_wr(struct bcm2835_spi *bs, unsigned reg, u32 val)
 	writel(val, bs->regs + reg);
 }
 
-static inline void bcm2835_rd_fifo(struct bcm2835_spi *bs, int len)
+static inline void bcm2835_rd_fifo(struct bcm2835_spi *bs)
 {
 	u8 byte;
 
-	while (len--) {
+	while ((bs->rx_len) &&
+	       (bcm2835_rd(bs, BCM2835_SPI_CS) & BCM2835_SPI_CS_RXD)) {
 		byte = bcm2835_rd(bs, BCM2835_SPI_FIFO);
 		if (bs->rx_buf)
 			*bs->rx_buf++ = byte;
+		bs->rx_len--;
 	}
 }
 
-static inline void bcm2835_wr_fifo(struct bcm2835_spi *bs, int len)
+static inline void bcm2835_wr_fifo(struct bcm2835_spi *bs)
 {
 	u8 byte;
 
-	if (len > bs->len)
-		len = bs->len;
-
-	while (len--) {
+	while ((bs->tx_len) &&
+	       (bcm2835_rd(bs, BCM2835_SPI_CS) & BCM2835_SPI_CS_TXD)) {
 		byte = bs->tx_buf ? *bs->tx_buf++ : 0;
 		bcm2835_wr(bs, BCM2835_SPI_FIFO, byte);
-		bs->len--;
+		bs->tx_len--;
 	}
 }
 
+static void bcm2835_spi_reset_hw(struct spi_master *master)
+{
+	struct bcm2835_spi *bs = spi_master_get_devdata(master);
+	u32 cs = bcm2835_rd(bs, BCM2835_SPI_CS);
+
+	/* Disable SPI interrupts and transfer */
+	cs &= ~(BCM2835_SPI_CS_INTR |
+		BCM2835_SPI_CS_INTD |
+		BCM2835_SPI_CS_TA);
+	/* and reset RX/TX FIFOS */
+	cs |= BCM2835_SPI_CS_CLEAR_RX | BCM2835_SPI_CS_CLEAR_TX;
+
+	/* and reset the SPI_HW */
+	bcm2835_wr(bs, BCM2835_SPI_CS, cs);
+}
+
 static irqreturn_t bcm2835_spi_interrupt(int irq, void *dev_id)
 {
 	struct spi_master *master = dev_id;
 	struct bcm2835_spi *bs = spi_master_get_devdata(master);
-	u32 cs = bcm2835_rd(bs, BCM2835_SPI_CS);
 
-	/*
-	 * RXR - RX needs Reading. This means 12 (or more) bytes have been
-	 * transmitted and hence 12 (or more) bytes have been received.
-	 *
-	 * The FIFO is 16-bytes deep. We check for this interrupt to keep the
-	 * FIFO full; we have a 4-byte-time buffer for IRQ latency. We check
-	 * this before DONE (TX empty) just in case we delayed processing this
-	 * interrupt for some reason.
-	 *
-	 * We only check for this case if we have more bytes to TX; at the end
-	 * of the transfer, we ignore this pipelining optimization, and let
-	 * bcm2835_spi_finish_transfer() drain the RX FIFO.
+	/* Read as many bytes as possible from FIFO */
+	bcm2835_rd_fifo(bs);
+	/* Write as many bytes as possible to FIFO */
+	bcm2835_wr_fifo(bs);
+
+	/* based on flags decide if we can finish the transfer */
+	if (bcm2835_rd(bs, BCM2835_SPI_CS) & BCM2835_SPI_CS_DONE) {
+		/* Transfer complete - reset SPI HW */
+		bcm2835_spi_reset_hw(master);
+		/* wake up the framework */
+		complete(&master->xfer_completion);
+	}
+
+	return IRQ_HANDLED;
+}
+
+static int bcm2835_spi_transfer_one_poll(struct spi_master *master,
+					 struct spi_device *spi,
+					 struct spi_transfer *tfr,
+					 u32 cs,
+					 unsigned long xfer_time_us)
+{
+	struct bcm2835_spi *bs = spi_master_get_devdata(master);
+	unsigned long timeout = jiffies +
+		max(4 * xfer_time_us * HZ / 1000000, 2uL);
+
+	/* enable HW block without interrupts */
+	bcm2835_wr(bs, BCM2835_SPI_CS, cs | BCM2835_SPI_CS_TA);
+
+	/* set timeout to 4x the expected time, or 2 jiffies */
+	/* loop until finished the transfer */
+	while (bs->rx_len) {
+		/* read from fifo as much as possible */
+		bcm2835_rd_fifo(bs);
+		/* fill in tx fifo as much as possible */
+		bcm2835_wr_fifo(bs);
+		/* if we still expect some data after the read,
+		 * check for a possible timeout
+		 */
+		if (bs->rx_len && time_after(jiffies, timeout)) {
+			/* Transfer complete - reset SPI HW */
+			bcm2835_spi_reset_hw(master);
+			/* and return timeout */
+			return -ETIMEDOUT;
+		}
+	}
+
+	/* Transfer complete - reset SPI HW */
+	bcm2835_spi_reset_hw(master);
+	/* and return without waiting for completion */
+	return 0;
+}
+
+static int bcm2835_spi_transfer_one_irq(struct spi_master *master,
+					struct spi_device *spi,
+					struct spi_transfer *tfr,
+					u32 cs)
+{
+	struct bcm2835_spi *bs = spi_master_get_devdata(master);
+
+	/* fill in fifo if we have gpio-cs
+	 * note that there have been rare events where the native-CS
+	 * flapped for <1us which may change the behaviour
+	 * with gpio-cs this does not happen, so it is implemented
+	 * only for this case
 	 */
-	if (bs->len && (cs & BCM2835_SPI_CS_RXR)) {
-		/* Read 12 bytes of data */
-		bcm2835_rd_fifo(bs, 12);
-
-		/* Write up to 12 bytes */
-		bcm2835_wr_fifo(bs, 12);
-
-		/*
-		 * We must have written something to the TX FIFO due to the
-		 * bs->len check above, so cannot be DONE. Hence, return
-		 * early. Note that DONE could also be set if we serviced an
-		 * RXR interrupt really late.
+	if (gpio_is_valid(spi->cs_gpio)) {
+		/* enable HW block, but without interrupts enabled
+		 * this would triggern an immediate interrupt
 		 */
-		return IRQ_HANDLED;
+		bcm2835_wr(bs, BCM2835_SPI_CS,
+			   cs | BCM2835_SPI_CS_TA);
+		/* fill in tx fifo as much as possible */
+		bcm2835_wr_fifo(bs);
 	}
 
 	/*
-	 * DONE - TX empty. This occurs when we first enable the transfer
-	 * since we do not pre-fill the TX FIFO. At any other time, given that
-	 * we refill the TX FIFO above based on RXR, and hence ignore DONE if
-	 * RXR is set, DONE really does mean end-of-transfer.
+	 * Enable the HW block. This will immediately trigger a DONE (TX
+	 * empty) interrupt, upon which we will fill the TX FIFO with the
+	 * first TX bytes. Pre-filling the TX FIFO here to avoid the
+	 * interrupt doesn't work:-(
 	 */
-	if (cs & BCM2835_SPI_CS_DONE) {
-		if (bs->len) { /* First interrupt in a transfer */
-			bcm2835_wr_fifo(bs, 16);
-		} else { /* Transfer complete */
-			/* Disable SPI interrupts */
-			cs &= ~(BCM2835_SPI_CS_INTR | BCM2835_SPI_CS_INTD);
-			bcm2835_wr(bs, BCM2835_SPI_CS, cs);
-
-			/*
-			 * Wake up bcm2835_spi_transfer_one(), which will call
-			 * bcm2835_spi_finish_transfer(), to drain the RX FIFO.
-			 */
-			complete(&bs->done);
-		}
-
-		return IRQ_HANDLED;
-	}
+	cs |= BCM2835_SPI_CS_INTR | BCM2835_SPI_CS_INTD | BCM2835_SPI_CS_TA;
+	bcm2835_wr(bs, BCM2835_SPI_CS, cs);
 
-	return IRQ_NONE;
+	/* signal that we need to wait for completion */
+	return 1;
 }
 
-static int bcm2835_spi_start_transfer(struct spi_device *spi,
-		struct spi_transfer *tfr)
+static int bcm2835_spi_transfer_one(struct spi_master *master,
+				    struct spi_device *spi,
+				    struct spi_transfer *tfr)
 {
-	struct bcm2835_spi *bs = spi_master_get_devdata(spi->master);
+	struct bcm2835_spi *bs = spi_master_get_devdata(master);
 	unsigned long spi_hz, clk_hz, cdiv;
-	u32 cs = BCM2835_SPI_CS_INTR | BCM2835_SPI_CS_INTD | BCM2835_SPI_CS_TA;
+	unsigned long spi_used_hz, xfer_time_us;
+	u32 cs = bcm2835_rd(bs, BCM2835_SPI_CS);
 
+	/* set clock */
 	spi_hz = tfr->speed_hz;
 	clk_hz = clk_get_rate(bs->clk);
 
 	if (spi_hz >= clk_hz / 2) {
 		cdiv = 2; /* clk_hz/2 is the fastest we can go */
 	} else if (spi_hz) {
-		/* CDIV must be a power of two */
-		cdiv = roundup_pow_of_two(DIV_ROUND_UP(clk_hz, spi_hz));
+		/* CDIV must be a multiple of two */
+		cdiv = DIV_ROUND_UP(clk_hz, spi_hz);
+		cdiv += (cdiv % 2);
 
 		if (cdiv >= 65536)
 			cdiv = 0; /* 0 is the slowest we can go */
-	} else
+	} else {
 		cdiv = 0; /* 0 is the slowest we can go */
+	}
+	spi_used_hz = cdiv ? (clk_hz / cdiv) : (clk_hz / 65536);
+	bcm2835_wr(bs, BCM2835_SPI_CLK, cdiv);
 
+	/* handle all the modes */
+	if ((spi->mode & SPI_3WIRE) && (tfr->rx_buf))
+		cs |= BCM2835_SPI_CS_REN;
 	if (spi->mode & SPI_CPOL)
 		cs |= BCM2835_SPI_CS_CPOL;
 	if (spi->mode & SPI_CPHA)
 		cs |= BCM2835_SPI_CS_CPHA;
 
-	if (!(spi->mode & SPI_NO_CS)) {
-		if (spi->mode & SPI_CS_HIGH) {
-			cs |= BCM2835_SPI_CS_CSPOL;
-			cs |= BCM2835_SPI_CS_CSPOL0 << spi->chip_select;
-		}
-
-		cs |= spi->chip_select;
-	}
+	/* for gpio_cs set dummy CS so that no HW-CS get changed
+	 * we can not run this in bcm2835_spi_set_cs, as it does
+	 * not get called for cs_gpio cases, so we need to do it here
+	 */
+	if (gpio_is_valid(spi->cs_gpio) || (spi->mode & SPI_NO_CS))
+		cs |= BCM2835_SPI_CS_CS_10 | BCM2835_SPI_CS_CS_01;
 
-	reinit_completion(&bs->done);
+	/* set transmit buffers and length */
 	bs->tx_buf = tfr->tx_buf;
 	bs->rx_buf = tfr->rx_buf;
-	bs->len = tfr->len;
+	bs->tx_len = tfr->len;
+	bs->rx_len = tfr->len;
 
-	bcm2835_wr(bs, BCM2835_SPI_CLK, cdiv);
-	/*
-	 * Enable the HW block. This will immediately trigger a DONE (TX
-	 * empty) interrupt, upon which we will fill the TX FIFO with the
-	 * first TX bytes. Pre-filling the TX FIFO here to avoid the
-	 * interrupt doesn't work:-(
-	 */
-	bcm2835_wr(bs, BCM2835_SPI_CS, cs);
+	/* calculate the estimated time in us the transfer runs */
+	xfer_time_us = tfr->len
+		* 9 /* clocks/byte - SPI-HW waits 1 clock after each byte */
+		* 1000000 / spi_used_hz;
 
-	return 0;
+	/* for short requests run polling*/
+	if (xfer_time_us <= BCM2835_SPI_POLLING_LIMIT_US)
+		return bcm2835_spi_transfer_one_poll(master, spi, tfr,
+						     cs, xfer_time_us);
+
+	return bcm2835_spi_transfer_one_irq(master, spi, tfr, cs);
 }
 
-static int bcm2835_spi_finish_transfer(struct spi_device *spi,
-		struct spi_transfer *tfr, bool cs_change)
+static void bcm2835_spi_handle_err(struct spi_master *master,
+				   struct spi_message *msg)
 {
-	struct bcm2835_spi *bs = spi_master_get_devdata(spi->master);
-	u32 cs = bcm2835_rd(bs, BCM2835_SPI_CS);
+	bcm2835_spi_reset_hw(master);
+}
+
+static void bcm2835_spi_set_cs(struct spi_device *spi, bool gpio_level)
+{
+	/*
+	 * we can assume that we are "native" as per spi_set_cs
+	 *   calling us ONLY when cs_gpio is not set
+	 * we can also assume that we are CS < 3 as per bcm2835_spi_setup
+	 *   we would not get called because of error handling there.
+	 * the level passed is the electrical level not enabled/disabled
+	 *   so it has to get translated back to enable/disable
+	 *   see spi_set_cs in spi.c for the implementation
+	 */
 
-	/* Drain RX FIFO */
-	while (cs & BCM2835_SPI_CS_RXD) {
-		bcm2835_rd_fifo(bs, 1);
-		cs = bcm2835_rd(bs, BCM2835_SPI_CS);
+	struct spi_master *master = spi->master;
+	struct bcm2835_spi *bs = spi_master_get_devdata(master);
+	u32 cs = bcm2835_rd(bs, BCM2835_SPI_CS);
+	bool enable;
+
+	/* calculate the enable flag from the passed gpio_level */
+	enable = (spi->mode & SPI_CS_HIGH) ? gpio_level : !gpio_level;
+
+	/* set flags for "reverse" polarity in the registers */
+	if (spi->mode & SPI_CS_HIGH) {
+		/* set the correct CS-bits */
+		cs |= BCM2835_SPI_CS_CSPOL;
+		cs |= BCM2835_SPI_CS_CSPOL0 << spi->chip_select;
+	} else {
+		/* clean the CS-bits */
+		cs &= ~BCM2835_SPI_CS_CSPOL;
+		cs &= ~(BCM2835_SPI_CS_CSPOL0 << spi->chip_select);
 	}
 
-	if (tfr->delay_usecs)
-		udelay(tfr->delay_usecs);
+	/* select the correct chip_select depending on disabled/enabled */
+	if (enable) {
+		/* set cs correctly */
+		if (spi->mode & SPI_NO_CS) {
+			/* use the "undefined" chip-select */
+			cs |= BCM2835_SPI_CS_CS_10 | BCM2835_SPI_CS_CS_01;
+		} else {
+			/* set the chip select */
+			cs &= ~(BCM2835_SPI_CS_CS_10 | BCM2835_SPI_CS_CS_01);
+			cs |= spi->chip_select;
+		}
+	} else {
+		/* disable CSPOL which puts HW-CS into deselected state */
+		cs &= ~BCM2835_SPI_CS_CSPOL;
+		/* use the "undefined" chip-select as precaution */
+		cs |= BCM2835_SPI_CS_CS_10 | BCM2835_SPI_CS_CS_01;
+	}
 
-	if (cs_change)
-		/* Clear TA flag */
-		bcm2835_wr(bs, BCM2835_SPI_CS, cs & ~BCM2835_SPI_CS_TA);
+	/* finally set the calculated flags in SPI_CS */
+	bcm2835_wr(bs, BCM2835_SPI_CS, cs);
+}
 
-	return 0;
+static int chip_match_name(struct gpio_chip *chip, void *data)
+{
+	return !strcmp(chip->label, data);
 }
 
-static int bcm2835_spi_transfer_one(struct spi_master *master,
-		struct spi_message *mesg)
+static int bcm2835_spi_setup(struct spi_device *spi)
 {
-	struct bcm2835_spi *bs = spi_master_get_devdata(master);
-	struct spi_transfer *tfr;
-	struct spi_device *spi = mesg->spi;
-	int err = 0;
-	unsigned int timeout;
-	bool cs_change;
-
-	list_for_each_entry(tfr, &mesg->transfers, transfer_list) {
-		err = bcm2835_spi_start_transfer(spi, tfr);
-		if (err)
-			goto out;
-
-		timeout = wait_for_completion_timeout(&bs->done,
-				msecs_to_jiffies(BCM2835_SPI_TIMEOUT_MS));
-		if (!timeout) {
-			err = -ETIMEDOUT;
-			goto out;
-		}
+	int err;
+	struct gpio_chip *chip;
+	/*
+	 * sanity checking the native-chipselects
+	 */
+	if (spi->mode & SPI_NO_CS)
+		return 0;
+	if (gpio_is_valid(spi->cs_gpio))
+		return 0;
+	if (spi->chip_select > 1) {
+		/* error in the case of native CS requested with CS > 1
+		 * officially there is a CS2, but it is not documented
+		 * which GPIO is connected with that...
+		 */
+		dev_err(&spi->dev,
+			"setup: only two native chip-selects are supported\n");
+		return -EINVAL;
+	}
+	/* now translate native cs to GPIO */
 
-		cs_change = tfr->cs_change ||
-			list_is_last(&tfr->transfer_list, &mesg->transfers);
+	/* get the gpio chip for the base */
+	chip = gpiochip_find("pinctrl-bcm2835", chip_match_name);
+	if (!chip)
+		return 0;
 
-		err = bcm2835_spi_finish_transfer(spi, tfr, cs_change);
-		if (err)
-			goto out;
+	/* and calculate the real CS */
+	spi->cs_gpio = chip->base + 8 - spi->chip_select;
 
-		mesg->actual_length += (tfr->len - bs->len);
-	}
+	/* and set up the "mode" and level */
+	dev_info(&spi->dev, "setting up native-CS%i as GPIO %i\n",
+		 spi->chip_select, spi->cs_gpio);
 
-out:
-	/* Clear FIFOs, and disable the HW block */
-	bcm2835_wr(bs, BCM2835_SPI_CS,
-		   BCM2835_SPI_CS_CLEAR_RX | BCM2835_SPI_CS_CLEAR_TX);
-	mesg->status = err;
-	spi_finalize_current_message(master);
+	/* set up GPIO as output and pull to the correct level */
+	err = gpio_direction_output(spi->cs_gpio,
+				    (spi->mode & SPI_CS_HIGH) ? 0 : 1);
+	if (err) {
+		dev_err(&spi->dev,
+			"could not set CS%i gpio %i as output: %i",
+			spi->chip_select, spi->cs_gpio, err);
+		return err;
+	}
+	/* the implementation of pinctrl-bcm2835 currently does not
+	 * set the GPIO value when using gpio_direction_output
+	 * so we are setting it here explicitly
+	 */
+	gpio_set_value(spi->cs_gpio, (spi->mode & SPI_CS_HIGH) ? 0 : 1);
 
 	return 0;
 }
@@ -312,13 +426,14 @@ static int bcm2835_spi_probe(struct platform_device *pdev)
 	master->mode_bits = BCM2835_SPI_MODE_BITS;
 	master->bits_per_word_mask = SPI_BPW_MASK(8);
 	master->num_chipselect = 3;
-	master->transfer_one_message = bcm2835_spi_transfer_one;
+	master->setup = bcm2835_spi_setup;
+	master->set_cs = bcm2835_spi_set_cs;
+	master->transfer_one = bcm2835_spi_transfer_one;
+	master->handle_err = bcm2835_spi_handle_err;
 	master->dev.of_node = pdev->dev.of_node;
 
 	bs = spi_master_get_devdata(master);
 
-	init_completion(&bs->done);
-
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	bs->regs = devm_ioremap_resource(&pdev->dev, res);
 	if (IS_ERR(bs->regs)) {
@@ -343,13 +458,13 @@ static int bcm2835_spi_probe(struct platform_device *pdev)
 	clk_prepare_enable(bs->clk);
 
 	err = devm_request_irq(&pdev->dev, bs->irq, bcm2835_spi_interrupt, 0,
-				dev_name(&pdev->dev), master);
+			       dev_name(&pdev->dev), master);
 	if (err) {
 		dev_err(&pdev->dev, "could not request IRQ: %d\n", err);
 		goto out_clk_disable;
 	}
 
-	/* initialise the hardware */
+	/* initialise the hardware with the default polarities */
 	bcm2835_wr(bs, BCM2835_SPI_CS,
 		   BCM2835_SPI_CS_CLEAR_RX | BCM2835_SPI_CS_CLEAR_TX);
 
diff --git a/drivers/spi/spi-bcm53xx.c b/drivers/spi/spi-bcm53xx.c
index 3fb91c81015a..1520554978a3 100644
--- a/drivers/spi/spi-bcm53xx.c
+++ b/drivers/spi/spi-bcm53xx.c
@@ -44,7 +44,7 @@ static int bcm53xxspi_wait(struct bcm53xxspi *b53spi, unsigned int timeout_ms)
 	u32 tmp;
 
 	/* SPE bit has to be 0 before we read MSPI STATUS */
-	deadline = jiffies + BCM53XXSPI_SPE_TIMEOUT_MS * HZ / 1000;
+	deadline = jiffies + msecs_to_jiffies(BCM53XXSPI_SPE_TIMEOUT_MS);
 	do {
 		tmp = bcm53xxspi_read(b53spi, B53SPI_MSPI_SPCR2);
 		if (!(tmp & B53SPI_MSPI_SPCR2_SPE))
@@ -56,7 +56,7 @@ static int bcm53xxspi_wait(struct bcm53xxspi *b53spi, unsigned int timeout_ms)
 		goto spi_timeout;
 
 	/* Check status */
-	deadline = jiffies + timeout_ms * HZ / 1000;
+	deadline = jiffies + msecs_to_jiffies(timeout_ms);
 	do {
 		tmp = bcm53xxspi_read(b53spi, B53SPI_MSPI_MSPI_STATUS);
 		if (tmp & B53SPI_MSPI_MSPI_STATUS_SPIF) {
diff --git a/drivers/spi/spi-bfin5xx.c b/drivers/spi/spi-bfin5xx.c
index 37079937d2f7..a3d65b4f4944 100644
--- a/drivers/spi/spi-bfin5xx.c
+++ b/drivers/spi/spi-bfin5xx.c
@@ -559,7 +559,7 @@ static void bfin_spi_pump_transfers(unsigned long data)
 	struct spi_transfer *previous = NULL;
 	struct bfin_spi_slave_data *chip = NULL;
 	unsigned int bits_per_word;
-	u16 cr, cr_width, dma_width, dma_config;
+	u16 cr, cr_width = 0, dma_width, dma_config;
 	u32 tranf_success = 1;
 	u8 full_duplex = 0;
 
@@ -648,7 +648,6 @@ static void bfin_spi_pump_transfers(unsigned long data)
 	} else if (bits_per_word == 8) {
 		drv_data->n_bytes = bits_per_word/8;
 		drv_data->len = transfer->len;
-		cr_width = 0;
 		drv_data->ops = &bfin_bfin_spi_transfer_ops_u8;
 	}
 	cr = bfin_read(&drv_data->regs->ctl) & ~(BIT_CTL_TIMOD | BIT_CTL_WORDSIZE);
diff --git a/drivers/spi/spi-bitbang-txrx.h b/drivers/spi/spi-bitbang-txrx.h
index c616e41521be..06b34e5bcfa3 100644
--- a/drivers/spi/spi-bitbang-txrx.h
+++ b/drivers/spi/spi-bitbang-txrx.h
@@ -49,12 +49,17 @@ bitbang_txrx_be_cpha0(struct spi_device *spi,
 {
 	/* if (cpol == 0) this is SPI_MODE_0; else this is SPI_MODE_2 */
 
+	bool oldbit = !(word & 1);
 	/* clock starts at inactive polarity */
 	for (word <<= (32 - bits); likely(bits); bits--) {
 
 		/* setup MSB (to slave) on trailing edge */
-		if ((flags & SPI_MASTER_NO_TX) == 0)
-			setmosi(spi, word & (1 << 31));
+		if ((flags & SPI_MASTER_NO_TX) == 0) {
+			if ((word & (1 << 31)) != oldbit) {
+				setmosi(spi, word & (1 << 31));
+				oldbit = word & (1 << 31);
+			}
+		}
 		spidelay(nsecs);	/* T(setup) */
 
 		setsck(spi, !cpol);
@@ -76,13 +81,18 @@ bitbang_txrx_be_cpha1(struct spi_device *spi,
 {
 	/* if (cpol == 0) this is SPI_MODE_1; else this is SPI_MODE_3 */
 
+	bool oldbit = !(word & (1 << 31));
 	/* clock starts at inactive polarity */
 	for (word <<= (32 - bits); likely(bits); bits--) {
 
 		/* setup MSB (to slave) on leading edge */
 		setsck(spi, !cpol);
-		if ((flags & SPI_MASTER_NO_TX) == 0)
-			setmosi(spi, word & (1 << 31));
+		if ((flags & SPI_MASTER_NO_TX) == 0) {
+			if ((word & (1 << 31)) != oldbit) {
+				setmosi(spi, word & (1 << 31));
+				oldbit = word & (1 << 31);
+			}
+		}
 		spidelay(nsecs); /* T(setup) */
 
 		setsck(spi, cpol);
diff --git a/drivers/spi/spi-dw-mid.c b/drivers/spi/spi-dw-mid.c
index a0197fd4e95c..bb1052e748f2 100644
--- a/drivers/spi/spi-dw-mid.c
+++ b/drivers/spi/spi-dw-mid.c
@@ -23,29 +23,31 @@
 #include "spi-dw.h"
 
 #ifdef CONFIG_SPI_DW_MID_DMA
-#include <linux/intel_mid_dma.h>
 #include <linux/pci.h>
+#include <linux/platform_data/dma-dw.h>
 
 #define RX_BUSY		0
 #define TX_BUSY		1
 
-struct mid_dma {
-	struct intel_mid_dma_slave	dmas_tx;
-	struct intel_mid_dma_slave	dmas_rx;
-};
+static struct dw_dma_slave mid_dma_tx = { .dst_id = 1 };
+static struct dw_dma_slave mid_dma_rx = { .src_id = 0 };
 
 static bool mid_spi_dma_chan_filter(struct dma_chan *chan, void *param)
 {
-	struct dw_spi *dws = param;
+	struct dw_dma_slave *s = param;
+
+	if (s->dma_dev != chan->device->dev)
+		return false;
 
-	return dws->dma_dev == chan->device->dev;
+	chan->private = s;
+	return true;
 }
 
 static int mid_spi_dma_init(struct dw_spi *dws)
 {
-	struct mid_dma *dw_dma = dws->dma_priv;
 	struct pci_dev *dma_dev;
-	struct intel_mid_dma_slave *rxs, *txs;
+	struct dw_dma_slave *tx = dws->dma_tx;
+	struct dw_dma_slave *rx = dws->dma_rx;
 	dma_cap_mask_t mask;
 
 	/*
@@ -56,28 +58,22 @@ static int mid_spi_dma_init(struct dw_spi *dws)
 	if (!dma_dev)
 		return -ENODEV;
 
-	dws->dma_dev = &dma_dev->dev;
-
 	dma_cap_zero(mask);
 	dma_cap_set(DMA_SLAVE, mask);
 
 	/* 1. Init rx channel */
-	dws->rxchan = dma_request_channel(mask, mid_spi_dma_chan_filter, dws);
+	rx->dma_dev = &dma_dev->dev;
+	dws->rxchan = dma_request_channel(mask, mid_spi_dma_chan_filter, rx);
 	if (!dws->rxchan)
 		goto err_exit;
-	rxs = &dw_dma->dmas_rx;
-	rxs->hs_mode = LNW_DMA_HW_HS;
-	rxs->cfg_mode = LNW_DMA_PER_TO_MEM;
-	dws->rxchan->private = rxs;
+	dws->master->dma_rx = dws->rxchan;
 
 	/* 2. Init tx channel */
-	dws->txchan = dma_request_channel(mask, mid_spi_dma_chan_filter, dws);
+	tx->dma_dev = &dma_dev->dev;
+	dws->txchan = dma_request_channel(mask, mid_spi_dma_chan_filter, tx);
 	if (!dws->txchan)
 		goto free_rxchan;
-	txs = &dw_dma->dmas_tx;
-	txs->hs_mode = LNW_DMA_HW_HS;
-	txs->cfg_mode = LNW_DMA_MEM_TO_PER;
-	dws->txchan->private = txs;
+	dws->master->dma_tx = dws->txchan;
 
 	dws->dma_inited = 1;
 	return 0;
@@ -100,6 +96,42 @@ static void mid_spi_dma_exit(struct dw_spi *dws)
 	dma_release_channel(dws->rxchan);
 }
 
+static irqreturn_t dma_transfer(struct dw_spi *dws)
+{
+	u16 irq_status = dw_readl(dws, DW_SPI_ISR);
+
+	if (!irq_status)
+		return IRQ_NONE;
+
+	dw_readl(dws, DW_SPI_ICR);
+	spi_reset_chip(dws);
+
+	dev_err(&dws->master->dev, "%s: FIFO overrun/underrun\n", __func__);
+	dws->master->cur_msg->status = -EIO;
+	spi_finalize_current_transfer(dws->master);
+	return IRQ_HANDLED;
+}
+
+static bool mid_spi_can_dma(struct spi_master *master, struct spi_device *spi,
+		struct spi_transfer *xfer)
+{
+	struct dw_spi *dws = spi_master_get_devdata(master);
+
+	if (!dws->dma_inited)
+		return false;
+
+	return xfer->len > dws->fifo_len;
+}
+
+static enum dma_slave_buswidth convert_dma_width(u32 dma_width) {
+	if (dma_width == 1)
+		return DMA_SLAVE_BUSWIDTH_1_BYTE;
+	else if (dma_width == 2)
+		return DMA_SLAVE_BUSWIDTH_2_BYTES;
+
+	return DMA_SLAVE_BUSWIDTH_UNDEFINED;
+}
+
 /*
  * dws->dma_chan_busy is set before the dma transfer starts, callback for tx
  * channel will clear a corresponding bit.
@@ -108,37 +140,38 @@ static void dw_spi_dma_tx_done(void *arg)
 {
 	struct dw_spi *dws = arg;
 
-	if (test_and_clear_bit(TX_BUSY, &dws->dma_chan_busy) & BIT(RX_BUSY))
+	clear_bit(TX_BUSY, &dws->dma_chan_busy);
+	if (test_bit(RX_BUSY, &dws->dma_chan_busy))
 		return;
-	dw_spi_xfer_done(dws);
+	spi_finalize_current_transfer(dws->master);
 }
 
-static struct dma_async_tx_descriptor *dw_spi_dma_prepare_tx(struct dw_spi *dws)
+static struct dma_async_tx_descriptor *dw_spi_dma_prepare_tx(struct dw_spi *dws,
+		struct spi_transfer *xfer)
 {
 	struct dma_slave_config txconf;
 	struct dma_async_tx_descriptor *txdesc;
 
-	if (!dws->tx_dma)
+	if (!xfer->tx_buf)
 		return NULL;
 
 	txconf.direction = DMA_MEM_TO_DEV;
 	txconf.dst_addr = dws->dma_addr;
-	txconf.dst_maxburst = LNW_DMA_MSIZE_16;
+	txconf.dst_maxburst = 16;
 	txconf.src_addr_width = DMA_SLAVE_BUSWIDTH_4_BYTES;
-	txconf.dst_addr_width = dws->dma_width;
+	txconf.dst_addr_width = convert_dma_width(dws->dma_width);
 	txconf.device_fc = false;
 
 	dmaengine_slave_config(dws->txchan, &txconf);
 
-	memset(&dws->tx_sgl, 0, sizeof(dws->tx_sgl));
-	dws->tx_sgl.dma_address = dws->tx_dma;
-	dws->tx_sgl.length = dws->len;
-
 	txdesc = dmaengine_prep_slave_sg(dws->txchan,
-				&dws->tx_sgl,
-				1,
+				xfer->tx_sg.sgl,
+				xfer->tx_sg.nents,
 				DMA_MEM_TO_DEV,
 				DMA_PREP_INTERRUPT | DMA_CTRL_ACK);
+	if (!txdesc)
+		return NULL;
+
 	txdesc->callback = dw_spi_dma_tx_done;
 	txdesc->callback_param = dws;
 
@@ -153,74 +186,74 @@ static void dw_spi_dma_rx_done(void *arg)
 {
 	struct dw_spi *dws = arg;
 
-	if (test_and_clear_bit(RX_BUSY, &dws->dma_chan_busy) & BIT(TX_BUSY))
+	clear_bit(RX_BUSY, &dws->dma_chan_busy);
+	if (test_bit(TX_BUSY, &dws->dma_chan_busy))
 		return;
-	dw_spi_xfer_done(dws);
+	spi_finalize_current_transfer(dws->master);
 }
 
-static struct dma_async_tx_descriptor *dw_spi_dma_prepare_rx(struct dw_spi *dws)
+static struct dma_async_tx_descriptor *dw_spi_dma_prepare_rx(struct dw_spi *dws,
+		struct spi_transfer *xfer)
 {
 	struct dma_slave_config rxconf;
 	struct dma_async_tx_descriptor *rxdesc;
 
-	if (!dws->rx_dma)
+	if (!xfer->rx_buf)
 		return NULL;
 
 	rxconf.direction = DMA_DEV_TO_MEM;
 	rxconf.src_addr = dws->dma_addr;
-	rxconf.src_maxburst = LNW_DMA_MSIZE_16;
+	rxconf.src_maxburst = 16;
 	rxconf.dst_addr_width = DMA_SLAVE_BUSWIDTH_4_BYTES;
-	rxconf.src_addr_width = dws->dma_width;
+	rxconf.src_addr_width = convert_dma_width(dws->dma_width);
 	rxconf.device_fc = false;
 
 	dmaengine_slave_config(dws->rxchan, &rxconf);
 
-	memset(&dws->rx_sgl, 0, sizeof(dws->rx_sgl));
-	dws->rx_sgl.dma_address = dws->rx_dma;
-	dws->rx_sgl.length = dws->len;
-
 	rxdesc = dmaengine_prep_slave_sg(dws->rxchan,
-				&dws->rx_sgl,
-				1,
+				xfer->rx_sg.sgl,
+				xfer->rx_sg.nents,
 				DMA_DEV_TO_MEM,
 				DMA_PREP_INTERRUPT | DMA_CTRL_ACK);
+	if (!rxdesc)
+		return NULL;
+
 	rxdesc->callback = dw_spi_dma_rx_done;
 	rxdesc->callback_param = dws;
 
 	return rxdesc;
 }
 
-static void dw_spi_dma_setup(struct dw_spi *dws)
+static int mid_spi_dma_setup(struct dw_spi *dws, struct spi_transfer *xfer)
 {
 	u16 dma_ctrl = 0;
 
-	spi_enable_chip(dws, 0);
-
-	dw_writew(dws, DW_SPI_DMARDLR, 0xf);
-	dw_writew(dws, DW_SPI_DMATDLR, 0x10);
+	dw_writel(dws, DW_SPI_DMARDLR, 0xf);
+	dw_writel(dws, DW_SPI_DMATDLR, 0x10);
 
-	if (dws->tx_dma)
+	if (xfer->tx_buf)
 		dma_ctrl |= SPI_DMA_TDMAE;
-	if (dws->rx_dma)
+	if (xfer->rx_buf)
 		dma_ctrl |= SPI_DMA_RDMAE;
-	dw_writew(dws, DW_SPI_DMACR, dma_ctrl);
+	dw_writel(dws, DW_SPI_DMACR, dma_ctrl);
+
+	/* Set the interrupt mask */
+	spi_umask_intr(dws, SPI_INT_TXOI | SPI_INT_RXUI | SPI_INT_RXOI);
+
+	dws->transfer_handler = dma_transfer;
 
-	spi_enable_chip(dws, 1);
+	return 0;
 }
 
-static int mid_spi_dma_transfer(struct dw_spi *dws, int cs_change)
+static int mid_spi_dma_transfer(struct dw_spi *dws, struct spi_transfer *xfer)
 {
 	struct dma_async_tx_descriptor *txdesc, *rxdesc;
 
-	/* 1. setup DMA related registers */
-	if (cs_change)
-		dw_spi_dma_setup(dws);
-
-	/* 2. Prepare the TX dma transfer */
-	txdesc = dw_spi_dma_prepare_tx(dws);
+	/* Prepare the TX dma transfer */
+	txdesc = dw_spi_dma_prepare_tx(dws, xfer);
 
-	/* 3. Prepare the RX dma transfer */
-	rxdesc = dw_spi_dma_prepare_rx(dws);
+	/* Prepare the RX dma transfer */
+	rxdesc = dw_spi_dma_prepare_rx(dws, xfer);
 
 	/* rx must be started before tx due to spi instinct */
 	if (rxdesc) {
@@ -238,10 +271,25 @@ static int mid_spi_dma_transfer(struct dw_spi *dws, int cs_change)
 	return 0;
 }
 
+static void mid_spi_dma_stop(struct dw_spi *dws)
+{
+	if (test_bit(TX_BUSY, &dws->dma_chan_busy)) {
+		dmaengine_terminate_all(dws->txchan);
+		clear_bit(TX_BUSY, &dws->dma_chan_busy);
+	}
+	if (test_bit(RX_BUSY, &dws->dma_chan_busy)) {
+		dmaengine_terminate_all(dws->rxchan);
+		clear_bit(RX_BUSY, &dws->dma_chan_busy);
+	}
+}
+
 static struct dw_spi_dma_ops mid_dma_ops = {
 	.dma_init	= mid_spi_dma_init,
 	.dma_exit	= mid_spi_dma_exit,
+	.dma_setup	= mid_spi_dma_setup,
+	.can_dma	= mid_spi_can_dma,
 	.dma_transfer	= mid_spi_dma_transfer,
+	.dma_stop	= mid_spi_dma_stop,
 };
 #endif
 
@@ -274,9 +322,8 @@ int dw_spi_mid_init(struct dw_spi *dws)
 	iounmap(clk_reg);
 
 #ifdef CONFIG_SPI_DW_MID_DMA
-	dws->dma_priv = kzalloc(sizeof(struct mid_dma), GFP_KERNEL);
-	if (!dws->dma_priv)
-		return -ENOMEM;
+	dws->dma_tx = &mid_dma_tx;
+	dws->dma_rx = &mid_dma_rx;
 	dws->dma_ops = &mid_dma_ops;
 #endif
 	return 0;
diff --git a/drivers/spi/spi-dw-pci.c b/drivers/spi/spi-dw-pci.c
index 5ba331047cbe..6d331e0db331 100644
--- a/drivers/spi/spi-dw-pci.c
+++ b/drivers/spi/spi-dw-pci.c
@@ -36,13 +36,13 @@ struct spi_pci_desc {
 
 static struct spi_pci_desc spi_pci_mid_desc_1 = {
 	.setup = dw_spi_mid_init,
-	.num_cs = 32,
+	.num_cs = 5,
 	.bus_num = 0,
 };
 
 static struct spi_pci_desc spi_pci_mid_desc_2 = {
 	.setup = dw_spi_mid_init,
-	.num_cs = 4,
+	.num_cs = 2,
 	.bus_num = 1,
 };
 
diff --git a/drivers/spi/spi-dw.c b/drivers/spi/spi-dw.c
index 5a97a62b298a..8d67d03c71eb 100644
--- a/drivers/spi/spi-dw.c
+++ b/drivers/spi/spi-dw.c
@@ -28,11 +28,6 @@
 #include <linux/debugfs.h>
 #endif
 
-#define START_STATE	((void *)0)
-#define RUNNING_STATE	((void *)1)
-#define DONE_STATE	((void *)2)
-#define ERROR_STATE	((void *)-1)
-
 /* Slave spi_dev related */
 struct chip_data {
 	u16 cr0;
@@ -143,13 +138,26 @@ static inline void dw_spi_debugfs_remove(struct dw_spi *dws)
 }
 #endif /* CONFIG_DEBUG_FS */
 
+static void dw_spi_set_cs(struct spi_device *spi, bool enable)
+{
+	struct dw_spi *dws = spi_master_get_devdata(spi->master);
+	struct chip_data *chip = spi_get_ctldata(spi);
+
+	/* Chip select logic is inverted from spi_set_cs() */
+	if (chip && chip->cs_control)
+		chip->cs_control(!enable);
+
+	if (!enable)
+		dw_writel(dws, DW_SPI_SER, BIT(spi->chip_select));
+}
+
 /* Return the max entries we can fill into tx fifo */
 static inline u32 tx_max(struct dw_spi *dws)
 {
 	u32 tx_left, tx_room, rxtx_gap;
 
 	tx_left = (dws->tx_end - dws->tx) / dws->n_bytes;
-	tx_room = dws->fifo_len - dw_readw(dws, DW_SPI_TXFLR);
+	tx_room = dws->fifo_len - dw_readl(dws, DW_SPI_TXFLR);
 
 	/*
 	 * Another concern is about the tx/rx mismatch, we
@@ -170,7 +178,7 @@ static inline u32 rx_max(struct dw_spi *dws)
 {
 	u32 rx_left = (dws->rx_end - dws->rx) / dws->n_bytes;
 
-	return min_t(u32, rx_left, dw_readw(dws, DW_SPI_RXFLR));
+	return min_t(u32, rx_left, dw_readl(dws, DW_SPI_RXFLR));
 }
 
 static void dw_writer(struct dw_spi *dws)
@@ -186,7 +194,7 @@ static void dw_writer(struct dw_spi *dws)
 			else
 				txw = *(u16 *)(dws->tx);
 		}
-		dw_writew(dws, DW_SPI_DR, txw);
+		dw_writel(dws, DW_SPI_DR, txw);
 		dws->tx += dws->n_bytes;
 	}
 }
@@ -197,7 +205,7 @@ static void dw_reader(struct dw_spi *dws)
 	u16 rxw;
 
 	while (max--) {
-		rxw = dw_readw(dws, DW_SPI_DR);
+		rxw = dw_readl(dws, DW_SPI_DR);
 		/* Care rx only if the transfer's original "rx" is not null */
 		if (dws->rx_end - dws->len) {
 			if (dws->n_bytes == 1)
@@ -209,103 +217,22 @@ static void dw_reader(struct dw_spi *dws)
 	}
 }
 
-static void *next_transfer(struct dw_spi *dws)
-{
-	struct spi_message *msg = dws->cur_msg;
-	struct spi_transfer *trans = dws->cur_transfer;
-
-	/* Move to next transfer */
-	if (trans->transfer_list.next != &msg->transfers) {
-		dws->cur_transfer =
-			list_entry(trans->transfer_list.next,
-					struct spi_transfer,
-					transfer_list);
-		return RUNNING_STATE;
-	}
-
-	return DONE_STATE;
-}
-
-/*
- * Note: first step is the protocol driver prepares
- * a dma-capable memory, and this func just need translate
- * the virt addr to physical
- */
-static int map_dma_buffers(struct dw_spi *dws)
-{
-	if (!dws->cur_msg->is_dma_mapped
-		|| !dws->dma_inited
-		|| !dws->cur_chip->enable_dma
-		|| !dws->dma_ops)
-		return 0;
-
-	if (dws->cur_transfer->tx_dma)
-		dws->tx_dma = dws->cur_transfer->tx_dma;
-
-	if (dws->cur_transfer->rx_dma)
-		dws->rx_dma = dws->cur_transfer->rx_dma;
-
-	return 1;
-}
-
-/* Caller already set message->status; dma and pio irqs are blocked */
-static void giveback(struct dw_spi *dws)
-{
-	struct spi_transfer *last_transfer;
-	struct spi_message *msg;
-
-	msg = dws->cur_msg;
-	dws->cur_msg = NULL;
-	dws->cur_transfer = NULL;
-	dws->prev_chip = dws->cur_chip;
-	dws->cur_chip = NULL;
-	dws->dma_mapped = 0;
-
-	last_transfer = list_last_entry(&msg->transfers, struct spi_transfer,
-					transfer_list);
-
-	if (!last_transfer->cs_change)
-		spi_chip_sel(dws, msg->spi, 0);
-
-	spi_finalize_current_message(dws->master);
-}
-
 static void int_error_stop(struct dw_spi *dws, const char *msg)
 {
-	/* Stop the hw */
-	spi_enable_chip(dws, 0);
+	spi_reset_chip(dws);
 
 	dev_err(&dws->master->dev, "%s\n", msg);
-	dws->cur_msg->state = ERROR_STATE;
-	tasklet_schedule(&dws->pump_transfers);
+	dws->master->cur_msg->status = -EIO;
+	spi_finalize_current_transfer(dws->master);
 }
 
-void dw_spi_xfer_done(struct dw_spi *dws)
-{
-	/* Update total byte transferred return count actual bytes read */
-	dws->cur_msg->actual_length += dws->len;
-
-	/* Move to next transfer */
-	dws->cur_msg->state = next_transfer(dws);
-
-	/* Handle end of message */
-	if (dws->cur_msg->state == DONE_STATE) {
-		dws->cur_msg->status = 0;
-		giveback(dws);
-	} else
-		tasklet_schedule(&dws->pump_transfers);
-}
-EXPORT_SYMBOL_GPL(dw_spi_xfer_done);
-
 static irqreturn_t interrupt_transfer(struct dw_spi *dws)
 {
-	u16 irq_status = dw_readw(dws, DW_SPI_ISR);
+	u16 irq_status = dw_readl(dws, DW_SPI_ISR);
 
 	/* Error handling */
 	if (irq_status & (SPI_INT_TXOI | SPI_INT_RXOI | SPI_INT_RXUI)) {
-		dw_readw(dws, DW_SPI_TXOICR);
-		dw_readw(dws, DW_SPI_RXOICR);
-		dw_readw(dws, DW_SPI_RXUICR);
+		dw_readl(dws, DW_SPI_ICR);
 		int_error_stop(dws, "interrupt_transfer: fifo overrun/underrun");
 		return IRQ_HANDLED;
 	}
@@ -313,7 +240,7 @@ static irqreturn_t interrupt_transfer(struct dw_spi *dws)
 	dw_reader(dws);
 	if (dws->rx_end == dws->rx) {
 		spi_mask_intr(dws, SPI_INT_TXEI);
-		dw_spi_xfer_done(dws);
+		spi_finalize_current_transfer(dws->master);
 		return IRQ_HANDLED;
 	}
 	if (irq_status & SPI_INT_TXEI) {
@@ -328,13 +255,14 @@ static irqreturn_t interrupt_transfer(struct dw_spi *dws)
 
 static irqreturn_t dw_spi_irq(int irq, void *dev_id)
 {
-	struct dw_spi *dws = dev_id;
-	u16 irq_status = dw_readw(dws, DW_SPI_ISR) & 0x3f;
+	struct spi_master *master = dev_id;
+	struct dw_spi *dws = spi_master_get_devdata(master);
+	u16 irq_status = dw_readl(dws, DW_SPI_ISR) & 0x3f;
 
 	if (!irq_status)
 		return IRQ_NONE;
 
-	if (!dws->cur_msg) {
+	if (!master->cur_msg) {
 		spi_mask_intr(dws, SPI_INT_TXEI);
 		return IRQ_HANDLED;
 	}
@@ -343,7 +271,7 @@ static irqreturn_t dw_spi_irq(int irq, void *dev_id)
 }
 
 /* Must be called inside pump_transfers() */
-static void poll_transfer(struct dw_spi *dws)
+static int poll_transfer(struct dw_spi *dws)
 {
 	do {
 		dw_writer(dws);
@@ -351,64 +279,32 @@ static void poll_transfer(struct dw_spi *dws)
 		cpu_relax();
 	} while (dws->rx_end > dws->rx);
 
-	dw_spi_xfer_done(dws);
+	return 0;
 }
 
-static void pump_transfers(unsigned long data)
+static int dw_spi_transfer_one(struct spi_master *master,
+		struct spi_device *spi, struct spi_transfer *transfer)
 {
-	struct dw_spi *dws = (struct dw_spi *)data;
-	struct spi_message *message = NULL;
-	struct spi_transfer *transfer = NULL;
-	struct spi_transfer *previous = NULL;
-	struct spi_device *spi = NULL;
-	struct chip_data *chip = NULL;
-	u8 bits = 0;
+	struct dw_spi *dws = spi_master_get_devdata(master);
+	struct chip_data *chip = spi_get_ctldata(spi);
 	u8 imask = 0;
-	u8 cs_change = 0;
-	u16 txint_level = 0;
+	u16 txlevel = 0;
 	u16 clk_div = 0;
 	u32 speed = 0;
 	u32 cr0 = 0;
+	int ret;
 
-	/* Get current state information */
-	message = dws->cur_msg;
-	transfer = dws->cur_transfer;
-	chip = dws->cur_chip;
-	spi = message->spi;
-
-	if (message->state == ERROR_STATE) {
-		message->status = -EIO;
-		goto early_exit;
-	}
-
-	/* Handle end of message */
-	if (message->state == DONE_STATE) {
-		message->status = 0;
-		goto early_exit;
-	}
-
-	/* Delay if requested at end of transfer */
-	if (message->state == RUNNING_STATE) {
-		previous = list_entry(transfer->transfer_list.prev,
-					struct spi_transfer,
-					transfer_list);
-		if (previous->delay_usecs)
-			udelay(previous->delay_usecs);
-	}
-
+	dws->dma_mapped = 0;
 	dws->n_bytes = chip->n_bytes;
 	dws->dma_width = chip->dma_width;
-	dws->cs_control = chip->cs_control;
 
-	dws->rx_dma = transfer->rx_dma;
-	dws->tx_dma = transfer->tx_dma;
 	dws->tx = (void *)transfer->tx_buf;
 	dws->tx_end = dws->tx + transfer->len;
 	dws->rx = transfer->rx_buf;
 	dws->rx_end = dws->rx + transfer->len;
-	dws->len = dws->cur_transfer->len;
-	if (chip != dws->prev_chip)
-		cs_change = 1;
+	dws->len = transfer->len;
+
+	spi_enable_chip(dws, 0);
 
 	cr0 = chip->cr0;
 
@@ -416,32 +312,37 @@ static void pump_transfers(unsigned long data)
 	if (transfer->speed_hz) {
 		speed = chip->speed_hz;
 
-		if ((transfer->speed_hz != speed) || (!chip->clk_div)) {
+		if ((transfer->speed_hz != speed) || !chip->clk_div) {
 			speed = transfer->speed_hz;
 
 			/* clk_div doesn't support odd number */
-			clk_div = dws->max_freq / speed;
-			clk_div = (clk_div + 1) & 0xfffe;
+			clk_div = (dws->max_freq / speed + 1) & 0xfffe;
 
 			chip->speed_hz = speed;
 			chip->clk_div = clk_div;
+
+			spi_set_clk(dws, chip->clk_div);
 		}
 	}
 	if (transfer->bits_per_word) {
-		bits = transfer->bits_per_word;
-		dws->n_bytes = dws->dma_width = bits >> 3;
-		cr0 = (bits - 1)
+		if (transfer->bits_per_word == 8) {
+			dws->n_bytes = 1;
+			dws->dma_width = 1;
+		} else if (transfer->bits_per_word == 16) {
+			dws->n_bytes = 2;
+			dws->dma_width = 2;
+		}
+		cr0 = (transfer->bits_per_word - 1)
 			| (chip->type << SPI_FRF_OFFSET)
 			| (spi->mode << SPI_MODE_OFFSET)
 			| (chip->tmode << SPI_TMOD_OFFSET);
 	}
-	message->state = RUNNING_STATE;
 
 	/*
 	 * Adjust transfer mode if necessary. Requires platform dependent
 	 * chipselect mechanism.
 	 */
-	if (dws->cs_control) {
+	if (chip->cs_control) {
 		if (dws->rx && dws->tx)
 			chip->tmode = SPI_TMOD_TR;
 		else if (dws->rx)
@@ -453,80 +354,60 @@ static void pump_transfers(unsigned long data)
 		cr0 |= (chip->tmode << SPI_TMOD_OFFSET);
 	}
 
+	dw_writel(dws, DW_SPI_CTRL0, cr0);
+
 	/* Check if current transfer is a DMA transaction */
-	dws->dma_mapped = map_dma_buffers(dws);
+	if (master->can_dma && master->can_dma(master, spi, transfer))
+		dws->dma_mapped = master->cur_msg_mapped;
+
+	/* For poll mode just disable all interrupts */
+	spi_mask_intr(dws, 0xff);
 
 	/*
 	 * Interrupt mode
 	 * we only need set the TXEI IRQ, as TX/RX always happen syncronizely
 	 */
-	if (!dws->dma_mapped && !chip->poll_mode) {
-		int templen = dws->len / dws->n_bytes;
-
-		txint_level = dws->fifo_len / 2;
-		txint_level = (templen > txint_level) ? txint_level : templen;
+	if (dws->dma_mapped) {
+		ret = dws->dma_ops->dma_setup(dws, transfer);
+		if (ret < 0) {
+			spi_enable_chip(dws, 1);
+			return ret;
+		}
+	} else if (!chip->poll_mode) {
+		txlevel = min_t(u16, dws->fifo_len / 2, dws->len / dws->n_bytes);
+		dw_writel(dws, DW_SPI_TXFLTR, txlevel);
 
+		/* Set the interrupt mask */
 		imask |= SPI_INT_TXEI | SPI_INT_TXOI |
 			 SPI_INT_RXUI | SPI_INT_RXOI;
+		spi_umask_intr(dws, imask);
+
 		dws->transfer_handler = interrupt_transfer;
 	}
 
-	/*
-	 * Reprogram registers only if
-	 *	1. chip select changes
-	 *	2. clk_div is changed
-	 *	3. control value changes
-	 */
-	if (dw_readw(dws, DW_SPI_CTRL0) != cr0 || cs_change || clk_div || imask) {
-		spi_enable_chip(dws, 0);
-
-		if (dw_readw(dws, DW_SPI_CTRL0) != cr0)
-			dw_writew(dws, DW_SPI_CTRL0, cr0);
-
-		spi_set_clk(dws, clk_div ? clk_div : chip->clk_div);
-		spi_chip_sel(dws, spi, 1);
-
-		/* Set the interrupt mask, for poll mode just disable all int */
-		spi_mask_intr(dws, 0xff);
-		if (imask)
-			spi_umask_intr(dws, imask);
-		if (txint_level)
-			dw_writew(dws, DW_SPI_TXFLTR, txint_level);
+	spi_enable_chip(dws, 1);
 
-		spi_enable_chip(dws, 1);
-		if (cs_change)
-			dws->prev_chip = chip;
+	if (dws->dma_mapped) {
+		ret = dws->dma_ops->dma_transfer(dws, transfer);
+		if (ret < 0)
+			return ret;
 	}
 
-	if (dws->dma_mapped)
-		dws->dma_ops->dma_transfer(dws, cs_change);
-
 	if (chip->poll_mode)
-		poll_transfer(dws);
-
-	return;
+		return poll_transfer(dws);
 
-early_exit:
-	giveback(dws);
+	return 1;
 }
 
-static int dw_spi_transfer_one_message(struct spi_master *master,
+static void dw_spi_handle_err(struct spi_master *master,
 		struct spi_message *msg)
 {
 	struct dw_spi *dws = spi_master_get_devdata(master);
 
-	dws->cur_msg = msg;
-	/* Initial message state */
-	dws->cur_msg->state = START_STATE;
-	dws->cur_transfer = list_entry(dws->cur_msg->transfers.next,
-						struct spi_transfer,
-						transfer_list);
-	dws->cur_chip = spi_get_ctldata(dws->cur_msg->spi);
-
-	/* Launch transfers */
-	tasklet_schedule(&dws->pump_transfers);
+	if (dws->dma_mapped)
+		dws->dma_ops->dma_stop(dws);
 
-	return 0;
+	spi_reset_chip(dws);
 }
 
 /* This may be called twice for each spi dev */
@@ -561,8 +442,6 @@ static int dw_spi_setup(struct spi_device *spi)
 
 		chip->rx_threshold = 0;
 		chip->tx_threshold = 0;
-
-		chip->enable_dma = chip_info->enable_dma;
 	}
 
 	if (spi->bits_per_word == 8) {
@@ -610,9 +489,7 @@ static void dw_spi_cleanup(struct spi_device *spi)
 /* Restart the controller, disable all interrupts, clean rx fifo */
 static void spi_hw_init(struct device *dev, struct dw_spi *dws)
 {
-	spi_enable_chip(dws, 0);
-	spi_mask_intr(dws, 0xff);
-	spi_enable_chip(dws, 1);
+	spi_reset_chip(dws);
 
 	/*
 	 * Try to detect the FIFO depth if not set by interface driver,
@@ -621,14 +498,14 @@ static void spi_hw_init(struct device *dev, struct dw_spi *dws)
 	if (!dws->fifo_len) {
 		u32 fifo;
 
-		for (fifo = 2; fifo <= 256; fifo++) {
-			dw_writew(dws, DW_SPI_TXFLTR, fifo);
-			if (fifo != dw_readw(dws, DW_SPI_TXFLTR))
+		for (fifo = 1; fifo < 256; fifo++) {
+			dw_writel(dws, DW_SPI_TXFLTR, fifo);
+			if (fifo != dw_readl(dws, DW_SPI_TXFLTR))
 				break;
 		}
-		dw_writew(dws, DW_SPI_TXFLTR, 0);
+		dw_writel(dws, DW_SPI_TXFLTR, 0);
 
-		dws->fifo_len = (fifo == 2) ? 0 : fifo - 1;
+		dws->fifo_len = (fifo == 1) ? 0 : fifo;
 		dev_dbg(dev, "Detected FIFO size: %u bytes\n", dws->fifo_len);
 	}
 }
@@ -646,13 +523,12 @@ int dw_spi_add_host(struct device *dev, struct dw_spi *dws)
 
 	dws->master = master;
 	dws->type = SSI_MOTO_SPI;
-	dws->prev_chip = NULL;
 	dws->dma_inited = 0;
 	dws->dma_addr = (dma_addr_t)(dws->paddr + 0x60);
 	snprintf(dws->name, sizeof(dws->name), "dw_spi%d", dws->bus_num);
 
 	ret = devm_request_irq(dev, dws->irq, dw_spi_irq, IRQF_SHARED,
-			dws->name, dws);
+			dws->name, master);
 	if (ret < 0) {
 		dev_err(&master->dev, "can not get IRQ\n");
 		goto err_free_master;
@@ -664,7 +540,9 @@ int dw_spi_add_host(struct device *dev, struct dw_spi *dws)
 	master->num_chipselect = dws->num_cs;
 	master->setup = dw_spi_setup;
 	master->cleanup = dw_spi_cleanup;
-	master->transfer_one_message = dw_spi_transfer_one_message;
+	master->set_cs = dw_spi_set_cs;
+	master->transfer_one = dw_spi_transfer_one;
+	master->handle_err = dw_spi_handle_err;
 	master->max_speed_hz = dws->max_freq;
 	master->dev.of_node = dev->of_node;
 
@@ -676,11 +554,11 @@ int dw_spi_add_host(struct device *dev, struct dw_spi *dws)
 		if (ret) {
 			dev_warn(dev, "DMA init failed\n");
 			dws->dma_inited = 0;
+		} else {
+			master->can_dma = dws->dma_ops->can_dma;
 		}
 	}
 
-	tasklet_init(&dws->pump_transfers, pump_transfers, (unsigned long)dws);
-
 	spi_master_set_devdata(master, dws);
 	ret = devm_spi_register_master(dev, master);
 	if (ret) {
diff --git a/drivers/spi/spi-dw.h b/drivers/spi/spi-dw.h
index 3d32be68c142..6c91391c1a4f 100644
--- a/drivers/spi/spi-dw.h
+++ b/drivers/spi/spi-dw.h
@@ -91,12 +91,15 @@ struct dw_spi;
 struct dw_spi_dma_ops {
 	int (*dma_init)(struct dw_spi *dws);
 	void (*dma_exit)(struct dw_spi *dws);
-	int (*dma_transfer)(struct dw_spi *dws, int cs_change);
+	int (*dma_setup)(struct dw_spi *dws, struct spi_transfer *xfer);
+	bool (*can_dma)(struct spi_master *master, struct spi_device *spi,
+			struct spi_transfer *xfer);
+	int (*dma_transfer)(struct dw_spi *dws, struct spi_transfer *xfer);
+	void (*dma_stop)(struct dw_spi *dws);
 };
 
 struct dw_spi {
 	struct spi_master	*master;
-	struct spi_device	*cur_dev;
 	enum dw_ssi_type	type;
 	char			name[16];
 
@@ -109,41 +112,26 @@ struct dw_spi {
 	u16			bus_num;
 	u16			num_cs;		/* supported slave numbers */
 
-	/* Message Transfer pump */
-	struct tasklet_struct	pump_transfers;
-
 	/* Current message transfer state info */
-	struct spi_message	*cur_msg;
-	struct spi_transfer	*cur_transfer;
-	struct chip_data	*cur_chip;
-	struct chip_data	*prev_chip;
 	size_t			len;
 	void			*tx;
 	void			*tx_end;
 	void			*rx;
 	void			*rx_end;
 	int			dma_mapped;
-	dma_addr_t		rx_dma;
-	dma_addr_t		tx_dma;
-	size_t			rx_map_len;
-	size_t			tx_map_len;
 	u8			n_bytes;	/* current is a 1/2 bytes op */
-	u8			max_bits_per_word;	/* maxim is 16b */
 	u32			dma_width;
 	irqreturn_t		(*transfer_handler)(struct dw_spi *dws);
-	void			(*cs_control)(u32 command);
 
-	/* Dma info */
+	/* DMA info */
 	int			dma_inited;
 	struct dma_chan		*txchan;
-	struct scatterlist	tx_sgl;
 	struct dma_chan		*rxchan;
-	struct scatterlist	rx_sgl;
 	unsigned long		dma_chan_busy;
-	struct device		*dma_dev;
 	dma_addr_t		dma_addr; /* phy address of the Data register */
 	struct dw_spi_dma_ops	*dma_ops;
-	void			*dma_priv; /* platform relate info */
+	void			*dma_tx;
+	void			*dma_rx;
 
 	/* Bus interface info */
 	void			*priv;
@@ -162,16 +150,6 @@ static inline void dw_writel(struct dw_spi *dws, u32 offset, u32 val)
 	__raw_writel(val, dws->regs + offset);
 }
 
-static inline u16 dw_readw(struct dw_spi *dws, u32 offset)
-{
-	return __raw_readw(dws->regs + offset);
-}
-
-static inline void dw_writew(struct dw_spi *dws, u32 offset, u16 val)
-{
-	__raw_writew(val, dws->regs + offset);
-}
-
 static inline void spi_enable_chip(struct dw_spi *dws, int enable)
 {
 	dw_writel(dws, DW_SPI_SSIENR, (enable ? 1 : 0));
@@ -182,22 +160,6 @@ static inline void spi_set_clk(struct dw_spi *dws, u16 div)
 	dw_writel(dws, DW_SPI_BAUDR, div);
 }
 
-static inline void spi_chip_sel(struct dw_spi *dws, struct spi_device *spi,
-		int active)
-{
-	u16 cs = spi->chip_select;
-	int gpio_val = active ? (spi->mode & SPI_CS_HIGH) :
-		!(spi->mode & SPI_CS_HIGH);
-
-	if (dws->cs_control)
-		dws->cs_control(active);
-	if (gpio_is_valid(spi->cs_gpio))
-		gpio_set_value(spi->cs_gpio, gpio_val);
-
-	if (active)
-		dw_writel(dws, DW_SPI_SER, 1 << cs);
-}
-
 /* Disable IRQ bits */
 static inline void spi_mask_intr(struct dw_spi *dws, u32 mask)
 {
@@ -217,15 +179,26 @@ static inline void spi_umask_intr(struct dw_spi *dws, u32 mask)
 }
 
 /*
+ * This does disable the SPI controller, interrupts, and re-enable the
+ * controller back. Transmit and receive FIFO buffers are cleared when the
+ * device is disabled.
+ */
+static inline void spi_reset_chip(struct dw_spi *dws)
+{
+	spi_enable_chip(dws, 0);
+	spi_mask_intr(dws, 0xff);
+	spi_enable_chip(dws, 1);
+}
+
+/*
  * Each SPI slave device to work with dw_api controller should
- * has such a structure claiming its working mode (PIO/DMA etc),
+ * has such a structure claiming its working mode (poll or PIO/DMA),
  * which can be save in the "controller_data" member of the
  * struct spi_device.
  */
 struct dw_spi_chip {
 	u8 poll_mode;	/* 1 for controller polling mode */
 	u8 type;	/* SPI/SSP/MicroWire */
-	u8 enable_dma;
 	void (*cs_control)(u32 command);
 };
 
@@ -233,7 +206,6 @@ extern int dw_spi_add_host(struct device *dev, struct dw_spi *dws);
 extern void dw_spi_remove_host(struct dw_spi *dws);
 extern int dw_spi_suspend_host(struct dw_spi *dws);
 extern int dw_spi_resume_host(struct dw_spi *dws);
-extern void dw_spi_xfer_done(struct dw_spi *dws);
 
 /* platform related setup */
 extern int dw_spi_mid_init(struct dw_spi *dws); /* Intel MID platforms */
diff --git a/drivers/spi/spi-fsl-dspi.c b/drivers/spi/spi-fsl-dspi.c
index d1a39249704a..5fe54cda309f 100644
--- a/drivers/spi/spi-fsl-dspi.c
+++ b/drivers/spi/spi-fsl-dspi.c
@@ -20,6 +20,7 @@
 #include <linux/interrupt.h>
 #include <linux/io.h>
 #include <linux/kernel.h>
+#include <linux/math64.h>
 #include <linux/module.h>
 #include <linux/of.h>
 #include <linux/of_device.h>
@@ -29,6 +30,7 @@
 #include <linux/sched.h>
 #include <linux/spi/spi.h>
 #include <linux/spi/spi_bitbang.h>
+#include <linux/time.h>
 
 #define DRIVER_NAME "fsl-dspi"
 
@@ -51,7 +53,7 @@
 #define SPI_CTAR_CPOL(x)	((x) << 26)
 #define SPI_CTAR_CPHA(x)	((x) << 25)
 #define SPI_CTAR_LSBFE(x)	((x) << 24)
-#define SPI_CTAR_PCSSCR(x)	(((x) & 0x00000003) << 22)
+#define SPI_CTAR_PCSSCK(x)	(((x) & 0x00000003) << 22)
 #define SPI_CTAR_PASC(x)	(((x) & 0x00000003) << 20)
 #define SPI_CTAR_PDT(x)	(((x) & 0x00000003) << 18)
 #define SPI_CTAR_PBR(x)	(((x) & 0x00000003) << 16)
@@ -59,6 +61,7 @@
 #define SPI_CTAR_ASC(x)	(((x) & 0x0000000f) << 8)
 #define SPI_CTAR_DT(x)		(((x) & 0x0000000f) << 4)
 #define SPI_CTAR_BR(x)		((x) & 0x0000000f)
+#define SPI_CTAR_SCALE_BITS	0xf
 
 #define SPI_CTAR0_SLAVE	0x0c
 
@@ -148,23 +151,66 @@ static void hz_to_spi_baud(char *pbr, char *br, int speed_hz,
 		16,	32,	64,	128,
 		256,	512,	1024,	2048,
 		4096,	8192,	16384,	32768 };
-	int temp, i = 0, j = 0;
+	int scale_needed, scale, minscale = INT_MAX;
+	int i, j;
+
+	scale_needed = clkrate / speed_hz;
+	if (clkrate % speed_hz)
+		scale_needed++;
+
+	for (i = 0; i < ARRAY_SIZE(brs); i++)
+		for (j = 0; j < ARRAY_SIZE(pbr_tbl); j++) {
+			scale = brs[i] * pbr_tbl[j];
+			if (scale >= scale_needed) {
+				if (scale < minscale) {
+					minscale = scale;
+					*br = i;
+					*pbr = j;
+				}
+				break;
+			}
+		}
 
-	temp = clkrate / 2 / speed_hz;
+	if (minscale == INT_MAX) {
+		pr_warn("Can not find valid baud rate,speed_hz is %d,clkrate is %ld, we use the max prescaler value.\n",
+			speed_hz, clkrate);
+		*pbr = ARRAY_SIZE(pbr_tbl) - 1;
+		*br =  ARRAY_SIZE(brs) - 1;
+	}
+}
 
-	for (i = 0; i < ARRAY_SIZE(pbr_tbl); i++)
-		for (j = 0; j < ARRAY_SIZE(brs); j++) {
-			if (pbr_tbl[i] * brs[j] >= temp) {
-				*pbr = i;
-				*br = j;
-				return;
+static void ns_delay_scale(char *psc, char *sc, int delay_ns,
+		unsigned long clkrate)
+{
+	int pscale_tbl[4] = {1, 3, 5, 7};
+	int scale_needed, scale, minscale = INT_MAX;
+	int i, j;
+	u32 remainder;
+
+	scale_needed = div_u64_rem((u64)delay_ns * clkrate, NSEC_PER_SEC,
+			&remainder);
+	if (remainder)
+		scale_needed++;
+
+	for (i = 0; i < ARRAY_SIZE(pscale_tbl); i++)
+		for (j = 0; j <= SPI_CTAR_SCALE_BITS; j++) {
+			scale = pscale_tbl[i] * (2 << j);
+			if (scale >= scale_needed) {
+				if (scale < minscale) {
+					minscale = scale;
+					*psc = i;
+					*sc = j;
+				}
+				break;
 			}
 		}
 
-	pr_warn("Can not find valid baud rate,speed_hz is %d,clkrate is %ld\
-		,we use the max prescaler value.\n", speed_hz, clkrate);
-	*pbr = ARRAY_SIZE(pbr_tbl) - 1;
-	*br =  ARRAY_SIZE(brs) - 1;
+	if (minscale == INT_MAX) {
+		pr_warn("Cannot find correct scale values for %dns delay at clkrate %ld, using max prescaler value",
+			delay_ns, clkrate);
+		*psc = ARRAY_SIZE(pscale_tbl) - 1;
+		*sc = SPI_CTAR_SCALE_BITS;
+	}
 }
 
 static int dspi_transfer_write(struct fsl_dspi *dspi)
@@ -345,7 +391,10 @@ static int dspi_setup(struct spi_device *spi)
 {
 	struct chip_data *chip;
 	struct fsl_dspi *dspi = spi_master_get_devdata(spi->master);
-	unsigned char br = 0, pbr = 0, fmsz = 0;
+	u32 cs_sck_delay = 0, sck_cs_delay = 0;
+	unsigned char br = 0, pbr = 0, pcssck = 0, cssck = 0;
+	unsigned char pasc = 0, asc = 0, fmsz = 0;
+	unsigned long clkrate;
 
 	if ((spi->bits_per_word >= 4) && (spi->bits_per_word <= 16)) {
 		fmsz = spi->bits_per_word - 1;
@@ -362,18 +411,34 @@ static int dspi_setup(struct spi_device *spi)
 			return -ENOMEM;
 	}
 
+	of_property_read_u32(spi->dev.of_node, "fsl,spi-cs-sck-delay",
+			&cs_sck_delay);
+
+	of_property_read_u32(spi->dev.of_node, "fsl,spi-sck-cs-delay",
+			&sck_cs_delay);
+
 	chip->mcr_val = SPI_MCR_MASTER | SPI_MCR_PCSIS |
 		SPI_MCR_CLR_TXF | SPI_MCR_CLR_RXF;
 
 	chip->void_write_data = 0;
 
-	hz_to_spi_baud(&pbr, &br,
-			spi->max_speed_hz, clk_get_rate(dspi->clk));
+	clkrate = clk_get_rate(dspi->clk);
+	hz_to_spi_baud(&pbr, &br, spi->max_speed_hz, clkrate);
+
+	/* Set PCS to SCK delay scale values */
+	ns_delay_scale(&pcssck, &cssck, cs_sck_delay, clkrate);
+
+	/* Set After SCK delay scale values */
+	ns_delay_scale(&pasc, &asc, sck_cs_delay, clkrate);
 
 	chip->ctar_val =  SPI_CTAR_FMSZ(fmsz)
 		| SPI_CTAR_CPOL(spi->mode & SPI_CPOL ? 1 : 0)
 		| SPI_CTAR_CPHA(spi->mode & SPI_CPHA ? 1 : 0)
 		| SPI_CTAR_LSBFE(spi->mode & SPI_LSB_FIRST ? 1 : 0)
+		| SPI_CTAR_PCSSCK(pcssck)
+		| SPI_CTAR_CSSCK(cssck)
+		| SPI_CTAR_PASC(pasc)
+		| SPI_CTAR_ASC(asc)
 		| SPI_CTAR_PBR(pbr)
 		| SPI_CTAR_BR(br);
 
diff --git a/drivers/spi/spi-img-spfi.c b/drivers/spi/spi-img-spfi.c
index c01567d53581..788e2b176a4f 100644
--- a/drivers/spi/spi-img-spfi.c
+++ b/drivers/spi/spi-img-spfi.c
@@ -12,6 +12,7 @@
 #include <linux/clk.h>
 #include <linux/delay.h>
 #include <linux/dmaengine.h>
+#include <linux/gpio.h>
 #include <linux/interrupt.h>
 #include <linux/io.h>
 #include <linux/irq.h>
@@ -122,36 +123,31 @@ static inline void spfi_start(struct img_spfi *spfi)
 	spfi_writel(spfi, val, SPFI_CONTROL);
 }
 
-static inline void spfi_stop(struct img_spfi *spfi)
-{
-	u32 val;
-
-	val = spfi_readl(spfi, SPFI_CONTROL);
-	val &= ~SPFI_CONTROL_SPFI_EN;
-	spfi_writel(spfi, val, SPFI_CONTROL);
-}
-
 static inline void spfi_reset(struct img_spfi *spfi)
 {
 	spfi_writel(spfi, SPFI_CONTROL_SOFT_RESET, SPFI_CONTROL);
-	udelay(1);
 	spfi_writel(spfi, 0, SPFI_CONTROL);
 }
 
-static void spfi_flush_tx_fifo(struct img_spfi *spfi)
+static int spfi_wait_all_done(struct img_spfi *spfi)
 {
-	unsigned long timeout = jiffies + msecs_to_jiffies(10);
+	unsigned long timeout = jiffies + msecs_to_jiffies(50);
 
-	spfi_writel(spfi, SPFI_INTERRUPT_SDE, SPFI_INTERRUPT_CLEAR);
 	while (time_before(jiffies, timeout)) {
-		if (spfi_readl(spfi, SPFI_INTERRUPT_STATUS) &
-		    SPFI_INTERRUPT_SDE)
-			return;
+		u32 status = spfi_readl(spfi, SPFI_INTERRUPT_STATUS);
+
+		if (status & SPFI_INTERRUPT_ALLDONETRIG) {
+			spfi_writel(spfi, SPFI_INTERRUPT_ALLDONETRIG,
+				    SPFI_INTERRUPT_CLEAR);
+			return 0;
+		}
 		cpu_relax();
 	}
 
-	dev_err(spfi->dev, "Timed out waiting for FIFO to drain\n");
+	dev_err(spfi->dev, "Timed out waiting for transaction to complete\n");
 	spfi_reset(spfi);
+
+	return -ETIMEDOUT;
 }
 
 static unsigned int spfi_pio_write32(struct img_spfi *spfi, const u32 *buf,
@@ -237,6 +233,7 @@ static int img_spfi_start_pio(struct spi_master *master,
 	const void *tx_buf = xfer->tx_buf;
 	void *rx_buf = xfer->rx_buf;
 	unsigned long timeout;
+	int ret;
 
 	if (tx_buf)
 		tx_bytes = xfer->len;
@@ -269,16 +266,15 @@ static int img_spfi_start_pio(struct spi_master *master,
 		cpu_relax();
 	}
 
+	ret = spfi_wait_all_done(spfi);
+	if (ret < 0)
+		return ret;
+
 	if (rx_bytes > 0 || tx_bytes > 0) {
 		dev_err(spfi->dev, "PIO transfer timed out\n");
-		spfi_reset(spfi);
 		return -ETIMEDOUT;
 	}
 
-	if (tx_buf)
-		spfi_flush_tx_fifo(spfi);
-	spfi_stop(spfi);
-
 	return 0;
 }
 
@@ -287,14 +283,12 @@ static void img_spfi_dma_rx_cb(void *data)
 	struct img_spfi *spfi = data;
 	unsigned long flags;
 
-	spin_lock_irqsave(&spfi->lock, flags);
+	spfi_wait_all_done(spfi);
 
+	spin_lock_irqsave(&spfi->lock, flags);
 	spfi->rx_dma_busy = false;
-	if (!spfi->tx_dma_busy) {
-		spfi_stop(spfi);
+	if (!spfi->tx_dma_busy)
 		spi_finalize_current_transfer(spfi->master);
-	}
-
 	spin_unlock_irqrestore(&spfi->lock, flags);
 }
 
@@ -303,16 +297,12 @@ static void img_spfi_dma_tx_cb(void *data)
 	struct img_spfi *spfi = data;
 	unsigned long flags;
 
-	spfi_flush_tx_fifo(spfi);
+	spfi_wait_all_done(spfi);
 
 	spin_lock_irqsave(&spfi->lock, flags);
-
 	spfi->tx_dma_busy = false;
-	if (!spfi->rx_dma_busy) {
-		spfi_stop(spfi);
+	if (!spfi->rx_dma_busy)
 		spi_finalize_current_transfer(spfi->master);
-	}
-
 	spin_unlock_irqrestore(&spfi->lock, flags);
 }
 
@@ -397,6 +387,75 @@ stop_dma:
 	return -EIO;
 }
 
+static void img_spfi_handle_err(struct spi_master *master,
+				struct spi_message *msg)
+{
+	struct img_spfi *spfi = spi_master_get_devdata(master);
+	unsigned long flags;
+
+	/*
+	 * Stop all DMA and reset the controller if the previous transaction
+	 * timed-out and never completed it's DMA.
+	 */
+	spin_lock_irqsave(&spfi->lock, flags);
+	if (spfi->tx_dma_busy || spfi->rx_dma_busy) {
+		spfi->tx_dma_busy = false;
+		spfi->rx_dma_busy = false;
+
+		dmaengine_terminate_all(spfi->tx_ch);
+		dmaengine_terminate_all(spfi->rx_ch);
+	}
+	spin_unlock_irqrestore(&spfi->lock, flags);
+}
+
+static int img_spfi_prepare(struct spi_master *master, struct spi_message *msg)
+{
+	struct img_spfi *spfi = spi_master_get_devdata(master);
+	u32 val;
+
+	val = spfi_readl(spfi, SPFI_PORT_STATE);
+	if (msg->spi->mode & SPI_CPHA)
+		val |= SPFI_PORT_STATE_CK_PHASE(msg->spi->chip_select);
+	else
+		val &= ~SPFI_PORT_STATE_CK_PHASE(msg->spi->chip_select);
+	if (msg->spi->mode & SPI_CPOL)
+		val |= SPFI_PORT_STATE_CK_POL(msg->spi->chip_select);
+	else
+		val &= ~SPFI_PORT_STATE_CK_POL(msg->spi->chip_select);
+	spfi_writel(spfi, val, SPFI_PORT_STATE);
+
+	return 0;
+}
+
+static int img_spfi_unprepare(struct spi_master *master,
+			      struct spi_message *msg)
+{
+	struct img_spfi *spfi = spi_master_get_devdata(master);
+
+	spfi_reset(spfi);
+
+	return 0;
+}
+
+static int img_spfi_setup(struct spi_device *spi)
+{
+	int ret;
+
+	ret = gpio_request_one(spi->cs_gpio, (spi->mode & SPI_CS_HIGH) ?
+			       GPIOF_OUT_INIT_LOW : GPIOF_OUT_INIT_HIGH,
+			       dev_name(&spi->dev));
+	if (ret)
+		dev_err(&spi->dev, "can't request chipselect gpio %d\n",
+				spi->cs_gpio);
+
+	return ret;
+}
+
+static void img_spfi_cleanup(struct spi_device *spi)
+{
+	gpio_free(spi->cs_gpio);
+}
+
 static void img_spfi_config(struct spi_master *master, struct spi_device *spi,
 			    struct spi_transfer *xfer)
 {
@@ -405,10 +464,10 @@ static void img_spfi_config(struct spi_master *master, struct spi_device *spi,
 
 	/*
 	 * output = spfi_clk * (BITCLK / 512), where BITCLK must be a
-	 * power of 2 up to 256 (where 255 == 256 since BITCLK is 8 bits)
+	 * power of 2 up to 128
 	 */
-	div = DIV_ROUND_UP(master->max_speed_hz, xfer->speed_hz);
-	div = clamp(512 / (1 << get_count_order(div)), 1, 255);
+	div = DIV_ROUND_UP(clk_get_rate(spfi->spfi_clk), xfer->speed_hz);
+	div = clamp(512 / (1 << get_count_order(div)), 1, 128);
 
 	val = spfi_readl(spfi, SPFI_DEVICE_PARAMETER(spi->chip_select));
 	val &= ~(SPFI_DEVICE_PARAMETER_BITCLK_MASK <<
@@ -416,6 +475,9 @@ static void img_spfi_config(struct spi_master *master, struct spi_device *spi,
 	val |= div << SPFI_DEVICE_PARAMETER_BITCLK_SHIFT;
 	spfi_writel(spfi, val, SPFI_DEVICE_PARAMETER(spi->chip_select));
 
+	spfi_writel(spfi, xfer->len << SPFI_TRANSACTION_TSIZE_SHIFT,
+		    SPFI_TRANSACTION);
+
 	val = spfi_readl(spfi, SPFI_CONTROL);
 	val &= ~(SPFI_CONTROL_SEND_DMA | SPFI_CONTROL_GET_DMA);
 	if (xfer->tx_buf)
@@ -429,25 +491,7 @@ static void img_spfi_config(struct spi_master *master, struct spi_device *spi,
 	else if (xfer->tx_nbits == SPI_NBITS_QUAD &&
 		 xfer->rx_nbits == SPI_NBITS_QUAD)
 		val |= SPFI_CONTROL_TMODE_QUAD << SPFI_CONTROL_TMODE_SHIFT;
-	val &= ~SPFI_CONTROL_CONTINUE;
-	if (!xfer->cs_change && !list_is_last(&xfer->transfer_list,
-					      &master->cur_msg->transfers))
-		val |= SPFI_CONTROL_CONTINUE;
 	spfi_writel(spfi, val, SPFI_CONTROL);
-
-	val = spfi_readl(spfi, SPFI_PORT_STATE);
-	if (spi->mode & SPI_CPHA)
-		val |= SPFI_PORT_STATE_CK_PHASE(spi->chip_select);
-	else
-		val &= ~SPFI_PORT_STATE_CK_PHASE(spi->chip_select);
-	if (spi->mode & SPI_CPOL)
-		val |= SPFI_PORT_STATE_CK_POL(spi->chip_select);
-	else
-		val &= ~SPFI_PORT_STATE_CK_POL(spi->chip_select);
-	spfi_writel(spfi, val, SPFI_PORT_STATE);
-
-	spfi_writel(spfi, xfer->len << SPFI_TRANSACTION_TSIZE_SHIFT,
-		    SPFI_TRANSACTION);
 }
 
 static int img_spfi_transfer_one(struct spi_master *master,
@@ -455,25 +499,13 @@ static int img_spfi_transfer_one(struct spi_master *master,
 				 struct spi_transfer *xfer)
 {
 	struct img_spfi *spfi = spi_master_get_devdata(spi->master);
-	bool dma_reset = false;
-	unsigned long flags;
 	int ret;
 
-	/*
-	 * Stop all DMA and reset the controller if the previous transaction
-	 * timed-out and never completed it's DMA.
-	 */
-	spin_lock_irqsave(&spfi->lock, flags);
-	if (spfi->tx_dma_busy || spfi->rx_dma_busy) {
-		dev_err(spfi->dev, "SPI DMA still busy\n");
-		dma_reset = true;
-	}
-	spin_unlock_irqrestore(&spfi->lock, flags);
-
-	if (dma_reset) {
-		dmaengine_terminate_all(spfi->tx_ch);
-		dmaengine_terminate_all(spfi->rx_ch);
-		spfi_reset(spfi);
+	if (xfer->len > SPFI_TRANSACTION_TSIZE_MASK) {
+		dev_err(spfi->dev,
+			"Transfer length (%d) is greater than the max supported (%d)",
+			xfer->len, SPFI_TRANSACTION_TSIZE_MASK);
+		return -EINVAL;
 	}
 
 	img_spfi_config(master, spi, xfer);
@@ -485,17 +517,6 @@ static int img_spfi_transfer_one(struct spi_master *master,
 	return ret;
 }
 
-static void img_spfi_set_cs(struct spi_device *spi, bool enable)
-{
-	struct img_spfi *spfi = spi_master_get_devdata(spi->master);
-	u32 val;
-
-	val = spfi_readl(spfi, SPFI_PORT_STATE);
-	val &= ~(SPFI_PORT_STATE_DEV_SEL_MASK << SPFI_PORT_STATE_DEV_SEL_SHIFT);
-	val |= spi->chip_select << SPFI_PORT_STATE_DEV_SEL_SHIFT;
-	spfi_writel(spfi, val, SPFI_PORT_STATE);
-}
-
 static bool img_spfi_can_dma(struct spi_master *master, struct spi_device *spi,
 			     struct spi_transfer *xfer)
 {
@@ -584,14 +605,17 @@ static int img_spfi_probe(struct platform_device *pdev)
 	master->mode_bits = SPI_CPOL | SPI_CPHA | SPI_TX_DUAL | SPI_RX_DUAL;
 	if (of_property_read_bool(spfi->dev->of_node, "img,supports-quad-mode"))
 		master->mode_bits |= SPI_TX_QUAD | SPI_RX_QUAD;
-	master->num_chipselect = 5;
 	master->dev.of_node = pdev->dev.of_node;
 	master->bits_per_word_mask = SPI_BPW_MASK(32) | SPI_BPW_MASK(8);
-	master->max_speed_hz = clk_get_rate(spfi->spfi_clk);
-	master->min_speed_hz = master->max_speed_hz / 512;
+	master->max_speed_hz = clk_get_rate(spfi->spfi_clk) / 4;
+	master->min_speed_hz = clk_get_rate(spfi->spfi_clk) / 512;
 
-	master->set_cs = img_spfi_set_cs;
+	master->setup = img_spfi_setup;
+	master->cleanup = img_spfi_cleanup;
 	master->transfer_one = img_spfi_transfer_one;
+	master->prepare_message = img_spfi_prepare;
+	master->unprepare_message = img_spfi_unprepare;
+	master->handle_err = img_spfi_handle_err;
 
 	spfi->tx_ch = dma_request_slave_channel(spfi->dev, "tx");
 	spfi->rx_ch = dma_request_slave_channel(spfi->dev, "rx");
diff --git a/drivers/spi/spi-imx.c b/drivers/spi/spi-imx.c
index 6fea4af51c41..f08e812b2984 100644
--- a/drivers/spi/spi-imx.c
+++ b/drivers/spi/spi-imx.c
@@ -370,8 +370,6 @@ static int __maybe_unused mx51_ecspi_config(struct spi_imx_data *spi_imx,
 	if (spi_imx->dma_is_inited) {
 		dma = readl(spi_imx->base + MX51_ECSPI_DMA);
 
-		spi_imx->tx_wml = spi_imx_get_fifosize(spi_imx) / 2;
-		spi_imx->rx_wml = spi_imx_get_fifosize(spi_imx) / 2;
 		spi_imx->rxt_wml = spi_imx_get_fifosize(spi_imx) / 2;
 		rx_wml_cfg = spi_imx->rx_wml << MX51_ECSPI_DMA_RX_WML_OFFSET;
 		tx_wml_cfg = spi_imx->tx_wml << MX51_ECSPI_DMA_TX_WML_OFFSET;
@@ -868,6 +866,8 @@ static int spi_imx_sdma_init(struct device *dev, struct spi_imx_data *spi_imx,
 	master->max_dma_len = MAX_SDMA_BD_BYTES;
 	spi_imx->bitbang.master->flags = SPI_MASTER_MUST_RX |
 					 SPI_MASTER_MUST_TX;
+	spi_imx->tx_wml = spi_imx_get_fifosize(spi_imx) / 2;
+	spi_imx->rx_wml = spi_imx_get_fifosize(spi_imx) / 2;
 	spi_imx->dma_is_inited = 1;
 
 	return 0;
@@ -903,7 +903,7 @@ static int spi_imx_dma_transfer(struct spi_imx_data *spi_imx,
 
 	if (tx) {
 		desc_tx = dmaengine_prep_slave_sg(master->dma_tx,
-					tx->sgl, tx->nents, DMA_TO_DEVICE,
+					tx->sgl, tx->nents, DMA_MEM_TO_DEV,
 					DMA_PREP_INTERRUPT | DMA_CTRL_ACK);
 		if (!desc_tx)
 			goto no_dma;
@@ -915,7 +915,7 @@ static int spi_imx_dma_transfer(struct spi_imx_data *spi_imx,
 
 	if (rx) {
 		desc_rx = dmaengine_prep_slave_sg(master->dma_rx,
-					rx->sgl, rx->nents, DMA_FROM_DEVICE,
+					rx->sgl, rx->nents, DMA_DEV_TO_MEM,
 					DMA_PREP_INTERRUPT | DMA_CTRL_ACK);
 		if (!desc_rx)
 			goto no_dma;
diff --git a/drivers/spi/spi-mpc512x-psc.c b/drivers/spi/spi-mpc512x-psc.c
index ecae0d4e2945..965d2bdcfdcc 100644
--- a/drivers/spi/spi-mpc512x-psc.c
+++ b/drivers/spi/spi-mpc512x-psc.c
@@ -588,7 +588,7 @@ static int mpc512x_psc_spi_of_remove(struct platform_device *op)
 	return mpc512x_psc_spi_do_remove(&op->dev);
 }
 
-static struct of_device_id mpc512x_psc_spi_of_match[] = {
+static const struct of_device_id mpc512x_psc_spi_of_match[] = {
 	{ .compatible = "fsl,mpc5121-psc-spi", },
 	{},
 };
diff --git a/drivers/spi/spi-octeon.c b/drivers/spi/spi-octeon.c
index b283d537d16a..e99d6a93d394 100644
--- a/drivers/spi/spi-octeon.c
+++ b/drivers/spi/spi-octeon.c
@@ -238,7 +238,7 @@ static int octeon_spi_remove(struct platform_device *pdev)
 	return 0;
 }
 
-static struct of_device_id octeon_spi_match[] = {
+static const struct of_device_id octeon_spi_match[] = {
 	{ .compatible = "cavium,octeon-3010-spi", },
 	{},
 };
diff --git a/drivers/spi/spi-omap-100k.c b/drivers/spi/spi-omap-100k.c
index d890d309dff9..35b332dacb13 100644
--- a/drivers/spi/spi-omap-100k.c
+++ b/drivers/spi/spi-omap-100k.c
@@ -24,6 +24,7 @@
 #include <linux/device.h>
 #include <linux/delay.h>
 #include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
 #include <linux/err.h>
 #include <linux/clk.h>
 #include <linux/io.h>
@@ -294,16 +295,6 @@ static int omap1_spi100k_setup(struct spi_device *spi)
 	return ret;
 }
 
-static int omap1_spi100k_prepare_hardware(struct spi_master *master)
-{
-	struct omap1_spi100k *spi100k = spi_master_get_devdata(master);
-
-	clk_prepare_enable(spi100k->ick);
-	clk_prepare_enable(spi100k->fck);
-
-	return 0;
-}
-
 static int omap1_spi100k_transfer_one_message(struct spi_master *master,
 					      struct spi_message *m)
 {
@@ -372,16 +363,6 @@ static int omap1_spi100k_transfer_one_message(struct spi_master *master,
 	return status;
 }
 
-static int omap1_spi100k_unprepare_hardware(struct spi_master *master)
-{
-	struct omap1_spi100k *spi100k = spi_master_get_devdata(master);
-
-	clk_disable_unprepare(spi100k->ick);
-	clk_disable_unprepare(spi100k->fck);
-
-	return 0;
-}
-
 static int omap1_spi100k_probe(struct platform_device *pdev)
 {
 	struct spi_master       *master;
@@ -402,14 +383,12 @@ static int omap1_spi100k_probe(struct platform_device *pdev)
 
 	master->setup = omap1_spi100k_setup;
 	master->transfer_one_message = omap1_spi100k_transfer_one_message;
-	master->prepare_transfer_hardware = omap1_spi100k_prepare_hardware;
-	master->unprepare_transfer_hardware = omap1_spi100k_unprepare_hardware;
-	master->cleanup = NULL;
 	master->num_chipselect = 2;
 	master->mode_bits = MODEBITS;
 	master->bits_per_word_mask = SPI_BPW_RANGE_MASK(4, 32);
 	master->min_speed_hz = OMAP1_SPI100K_MAX_FREQ/(1<<16);
 	master->max_speed_hz = OMAP1_SPI100K_MAX_FREQ;
+	master->auto_runtime_pm = true;
 
 	spi100k = spi_master_get_devdata(master);
 
@@ -434,22 +413,96 @@ static int omap1_spi100k_probe(struct platform_device *pdev)
 		goto err;
 	}
 
+	status = clk_prepare_enable(spi100k->ick);
+	if (status != 0) {
+		dev_err(&pdev->dev, "failed to enable ick: %d\n", status);
+		goto err;
+	}
+
+	status = clk_prepare_enable(spi100k->fck);
+	if (status != 0) {
+		dev_err(&pdev->dev, "failed to enable fck: %d\n", status);
+		goto err_ick;
+	}
+
+	pm_runtime_enable(&pdev->dev);
+	pm_runtime_set_active(&pdev->dev);
+
 	status = devm_spi_register_master(&pdev->dev, master);
 	if (status < 0)
-		goto err;
+		goto err_fck;
 
 	return status;
 
+err_fck:
+	clk_disable_unprepare(spi100k->fck);
+err_ick:
+	clk_disable_unprepare(spi100k->ick);
 err:
 	spi_master_put(master);
 	return status;
 }
 
+static int omap1_spi100k_remove(struct platform_device *pdev)
+{
+	struct spi_master *master = spi_master_get(platform_get_drvdata(pdev));
+	struct omap1_spi100k *spi100k = spi_master_get_devdata(master);
+
+	pm_runtime_disable(&pdev->dev);
+
+	clk_disable_unprepare(spi100k->fck);
+	clk_disable_unprepare(spi100k->ick);
+
+	return 0;
+}
+
+#ifdef CONFIG_PM
+static int omap1_spi100k_runtime_suspend(struct device *dev)
+{
+	struct spi_master *master = spi_master_get(dev_get_drvdata(dev));
+	struct omap1_spi100k *spi100k = spi_master_get_devdata(master);
+
+	clk_disable_unprepare(spi100k->ick);
+	clk_disable_unprepare(spi100k->fck);
+
+	return 0;
+}
+
+static int omap1_spi100k_runtime_resume(struct device *dev)
+{
+	struct spi_master *master = spi_master_get(dev_get_drvdata(dev));
+	struct omap1_spi100k *spi100k = spi_master_get_devdata(master);
+	int ret;
+
+	ret = clk_prepare_enable(spi100k->ick);
+	if (ret != 0) {
+		dev_err(dev, "Failed to enable ick: %d\n", ret);
+		return ret;
+	}
+
+	ret = clk_prepare_enable(spi100k->fck);
+	if (ret != 0) {
+		dev_err(dev, "Failed to enable fck: %d\n", ret);
+		clk_disable_unprepare(spi100k->ick);
+		return ret;
+	}
+
+	return 0;
+}
+#endif
+
+static const struct dev_pm_ops omap1_spi100k_pm = {
+	SET_RUNTIME_PM_OPS(omap1_spi100k_runtime_suspend,
+			   omap1_spi100k_runtime_resume, NULL)
+};
+
 static struct platform_driver omap1_spi100k_driver = {
 	.driver = {
 		.name		= "omap1_spi100k",
+		.pm		= &omap1_spi100k_pm,
 	},
 	.probe		= omap1_spi100k_probe,
+	.remove		= omap1_spi100k_remove,
 };
 
 module_platform_driver(omap1_spi100k_driver);
diff --git a/drivers/spi/spi-omap-uwire.c b/drivers/spi/spi-omap-uwire.c
index 3c0844457c07..55576db31549 100644
--- a/drivers/spi/spi-omap-uwire.c
+++ b/drivers/spi/spi-omap-uwire.c
@@ -44,7 +44,6 @@
 #include <linux/module.h>
 #include <linux/io.h>
 
-#include <asm/irq.h>
 #include <mach/hardware.h>
 #include <asm/mach-types.h>
 
diff --git a/drivers/spi/spi-pl022.c b/drivers/spi/spi-pl022.c
index 89ca162801da..94af80676684 100644
--- a/drivers/spi/spi-pl022.c
+++ b/drivers/spi/spi-pl022.c
@@ -285,7 +285,12 @@
  */
 #define DEFAULT_SSP_REG_IMSC  0x0UL
 #define DISABLE_ALL_INTERRUPTS DEFAULT_SSP_REG_IMSC
-#define ENABLE_ALL_INTERRUPTS (~DEFAULT_SSP_REG_IMSC)
+#define ENABLE_ALL_INTERRUPTS ( \
+	SSP_IMSC_MASK_RORIM | \
+	SSP_IMSC_MASK_RTIM | \
+	SSP_IMSC_MASK_RXIM | \
+	SSP_IMSC_MASK_TXIM \
+)
 
 #define CLEAR_ALL_INTERRUPTS  0x3
 
@@ -534,12 +539,12 @@ static void giveback(struct pl022 *pl022)
 	pl022->cur_msg = NULL;
 	pl022->cur_transfer = NULL;
 	pl022->cur_chip = NULL;
-	spi_finalize_current_message(pl022->master);
 
 	/* disable the SPI/SSP operation */
 	writew((readw(SSP_CR1(pl022->virtbase)) &
 		(~SSP_CR1_MASK_SSE)), SSP_CR1(pl022->virtbase));
 
+	spi_finalize_current_message(pl022->master);
 }
 
 /**
@@ -1251,7 +1256,6 @@ static irqreturn_t pl022_interrupt_handler(int irq, void *dev_id)
 	struct pl022 *pl022 = dev_id;
 	struct spi_message *msg = pl022->cur_msg;
 	u16 irq_status = 0;
-	u16 flag = 0;
 
 	if (unlikely(!msg)) {
 		dev_err(&pl022->adev->dev,
@@ -1280,9 +1284,6 @@ static irqreturn_t pl022_interrupt_handler(int irq, void *dev_id)
 		if (readw(SSP_SR(pl022->virtbase)) & SSP_SR_MASK_RFF)
 			dev_err(&pl022->adev->dev,
 				"RXFIFO is full\n");
-		if (readw(SSP_SR(pl022->virtbase)) & SSP_SR_MASK_TNF)
-			dev_err(&pl022->adev->dev,
-				"TXFIFO is full\n");
 
 		/*
 		 * Disable and clear interrupts, disable SSP,
@@ -1303,8 +1304,7 @@ static irqreturn_t pl022_interrupt_handler(int irq, void *dev_id)
 
 	readwriter(pl022);
 
-	if ((pl022->tx == pl022->tx_end) && (flag == 0)) {
-		flag = 1;
+	if (pl022->tx == pl022->tx_end) {
 		/* Disable Transmit interrupt, enable receive interrupt */
 		writew((readw(SSP_IMSC(pl022->virtbase)) &
 		       ~SSP_IMSC_MASK_TXIM) | SSP_IMSC_MASK_RXIM,
diff --git a/drivers/spi/spi-pxa2xx.c b/drivers/spi/spi-pxa2xx.c
index 6f72ad01e041..e3223ac75a7c 100644
--- a/drivers/spi/spi-pxa2xx.c
+++ b/drivers/spi/spi-pxa2xx.c
@@ -20,6 +20,7 @@
 #include <linux/errno.h>
 #include <linux/err.h>
 #include <linux/interrupt.h>
+#include <linux/kernel.h>
 #include <linux/platform_device.h>
 #include <linux/spi/pxa2xx_spi.h>
 #include <linux/spi/spi.h>
@@ -30,10 +31,6 @@
 #include <linux/pm_runtime.h>
 #include <linux/acpi.h>
 
-#include <asm/io.h>
-#include <asm/irq.h>
-#include <asm/delay.h>
-
 #include "spi-pxa2xx.h"
 
 MODULE_AUTHOR("Stephen Street");
@@ -67,54 +64,6 @@ MODULE_ALIAS("platform:pxa2xx-spi");
 #define LPSS_TX_LOTHRESH_DFLT	160
 #define LPSS_TX_HITHRESH_DFLT	224
 
-struct quark_spi_rate {
-	u32 bitrate;
-	u32 dds_clk_rate;
-	u32 clk_div;
-};
-
-/*
- * 'rate', 'dds', 'clk_div' lookup table, which is defined in
- * the Quark SPI datasheet.
- */
-static const struct quark_spi_rate quark_spi_rate_table[] = {
-/*	bitrate,	dds_clk_rate,	clk_div */
-	{50000000,	0x800000,	0},
-	{40000000,	0x666666,	0},
-	{25000000,	0x400000,	0},
-	{20000000,	0x666666,	1},
-	{16667000,	0x800000,	2},
-	{13333000,	0x666666,	2},
-	{12500000,	0x200000,	0},
-	{10000000,	0x800000,	4},
-	{8000000,	0x666666,	4},
-	{6250000,	0x400000,	3},
-	{5000000,	0x400000,	4},
-	{4000000,	0x666666,	9},
-	{3125000,	0x80000,	0},
-	{2500000,	0x400000,	9},
-	{2000000,	0x666666,	19},
-	{1563000,	0x40000,	0},
-	{1250000,	0x200000,	9},
-	{1000000,	0x400000,	24},
-	{800000,	0x666666,	49},
-	{781250,	0x20000,	0},
-	{625000,	0x200000,	19},
-	{500000,	0x400000,	49},
-	{400000,	0x666666,	99},
-	{390625,	0x10000,	0},
-	{250000,	0x400000,	99},
-	{200000,	0x666666,	199},
-	{195313,	0x8000,		0},
-	{125000,	0x100000,	49},
-	{100000,	0x200000,	124},
-	{50000,		0x100000,	124},
-	{25000,		0x80000,	124},
-	{10016,		0x20000,	77},
-	{5040,		0x20000,	154},
-	{1002,		0x8000,		194},
-};
-
 /* Offset from drv_data->lpss_base */
 #define GENERAL_REG		0x08
 #define GENERAL_REG_RXTO_HOLDOFF_DISABLE BIT(24)
@@ -701,25 +650,124 @@ static irqreturn_t ssp_int(int irq, void *dev_id)
 }
 
 /*
- * The Quark SPI data sheet gives a table, and for the given 'rate',
- * the 'dds' and 'clk_div' can be found in the table.
+ * The Quark SPI has an additional 24 bit register (DDS_CLK_RATE) to multiply
+ * input frequency by fractions of 2^24. It also has a divider by 5.
+ *
+ * There are formulas to get baud rate value for given input frequency and
+ * divider parameters, such as DDS_CLK_RATE and SCR:
+ *
+ * Fsys = 200MHz
+ *
+ * Fssp = Fsys * DDS_CLK_RATE / 2^24			(1)
+ * Baud rate = Fsclk = Fssp / (2 * (SCR + 1))		(2)
+ *
+ * DDS_CLK_RATE either 2^n or 2^n / 5.
+ * SCR is in range 0 .. 255
+ *
+ * Divisor = 5^i * 2^j * 2 * k
+ *       i = [0, 1]      i = 1 iff j = 0 or j > 3
+ *       j = [0, 23]     j = 0 iff i = 1
+ *       k = [1, 256]
+ * Special case: j = 0, i = 1: Divisor = 2 / 5
+ *
+ * Accordingly to the specification the recommended values for DDS_CLK_RATE
+ * are:
+ *	Case 1:		2^n, n = [0, 23]
+ *	Case 2:		2^24 * 2 / 5 (0x666666)
+ *	Case 3:		less than or equal to 2^24 / 5 / 16 (0x33333)
+ *
+ * In all cases the lowest possible value is better.
+ *
+ * The function calculates parameters for all cases and chooses the one closest
+ * to the asked baud rate.
  */
-static u32 quark_x1000_set_clk_regvals(u32 rate, u32 *dds, u32 *clk_div)
+static unsigned int quark_x1000_get_clk_div(int rate, u32 *dds)
 {
-	unsigned int i;
-
-	for (i = 0; i < ARRAY_SIZE(quark_spi_rate_table); i++) {
-		if (rate >= quark_spi_rate_table[i].bitrate) {
-			*dds = quark_spi_rate_table[i].dds_clk_rate;
-			*clk_div = quark_spi_rate_table[i].clk_div;
-			return quark_spi_rate_table[i].bitrate;
+	unsigned long xtal = 200000000;
+	unsigned long fref = xtal / 2;		/* mandatory division by 2,
+						   see (2) */
+						/* case 3 */
+	unsigned long fref1 = fref / 2;		/* case 1 */
+	unsigned long fref2 = fref * 2 / 5;	/* case 2 */
+	unsigned long scale;
+	unsigned long q, q1, q2;
+	long r, r1, r2;
+	u32 mul;
+
+	/* Case 1 */
+
+	/* Set initial value for DDS_CLK_RATE */
+	mul = (1 << 24) >> 1;
+
+	/* Calculate initial quot */
+	q1 = DIV_ROUND_CLOSEST(fref1, rate);
+
+	/* Scale q1 if it's too big */
+	if (q1 > 256) {
+		/* Scale q1 to range [1, 512] */
+		scale = fls_long(q1 - 1);
+		if (scale > 9) {
+			q1 >>= scale - 9;
+			mul >>= scale - 9;
 		}
+
+		/* Round the result if we have a remainder */
+		q1 += q1 & 1;
 	}
 
-	*dds = quark_spi_rate_table[i-1].dds_clk_rate;
-	*clk_div = quark_spi_rate_table[i-1].clk_div;
+	/* Decrease DDS_CLK_RATE as much as we can without loss in precision */
+	scale = __ffs(q1);
+	q1 >>= scale;
+	mul >>= scale;
+
+	/* Get the remainder */
+	r1 = abs(fref1 / (1 << (24 - fls_long(mul))) / q1 - rate);
+
+	/* Case 2 */
+
+	q2 = DIV_ROUND_CLOSEST(fref2, rate);
+	r2 = abs(fref2 / q2 - rate);
 
-	return quark_spi_rate_table[i-1].bitrate;
+	/*
+	 * Choose the best between two: less remainder we have the better. We
+	 * can't go case 2 if q2 is greater than 256 since SCR register can
+	 * hold only values 0 .. 255.
+	 */
+	if (r2 >= r1 || q2 > 256) {
+		/* case 1 is better */
+		r = r1;
+		q = q1;
+	} else {
+		/* case 2 is better */
+		r = r2;
+		q = q2;
+		mul = (1 << 24) * 2 / 5;
+	}
+
+	/* Check case 3 only If the divisor is big enough */
+	if (fref / rate >= 80) {
+		u64 fssp;
+		u32 m;
+
+		/* Calculate initial quot */
+		q1 = DIV_ROUND_CLOSEST(fref, rate);
+		m = (1 << 24) / q1;
+
+		/* Get the remainder */
+		fssp = (u64)fref * m;
+		do_div(fssp, 1 << 24);
+		r1 = abs(fssp - rate);
+
+		/* Choose this one if it suits better */
+		if (r1 < r) {
+			/* case 3 is better */
+			q = 1;
+			mul = m;
+		}
+	}
+
+	*dds = mul;
+	return q - 1;
 }
 
 static unsigned int ssp_get_clk_div(struct driver_data *drv_data, int rate)
@@ -730,23 +778,25 @@ static unsigned int ssp_get_clk_div(struct driver_data *drv_data, int rate)
 	rate = min_t(int, ssp_clk, rate);
 
 	if (ssp->type == PXA25x_SSP || ssp->type == CE4100_SSP)
-		return ((ssp_clk / (2 * rate) - 1) & 0xff) << 8;
+		return (ssp_clk / (2 * rate) - 1) & 0xff;
 	else
-		return ((ssp_clk / rate - 1) & 0xfff) << 8;
+		return (ssp_clk / rate - 1) & 0xfff;
 }
 
 static unsigned int pxa2xx_ssp_get_clk_div(struct driver_data *drv_data,
 					   struct chip_data *chip, int rate)
 {
-	u32 clk_div;
+	unsigned int clk_div;
 
 	switch (drv_data->ssp_type) {
 	case QUARK_X1000_SSP:
-		quark_x1000_set_clk_regvals(rate, &chip->dds_rate, &clk_div);
-		return clk_div << 8;
+		clk_div = quark_x1000_get_clk_div(rate, &chip->dds_rate);
+		break;
 	default:
-		return ssp_get_clk_div(drv_data, rate);
+		clk_div = ssp_get_clk_div(drv_data, rate);
+		break;
 	}
+	return clk_div << 8;
 }
 
 static void pump_transfers(unsigned long data)
diff --git a/drivers/spi/spi-qup.c b/drivers/spi/spi-qup.c
index ff9cdbdb6672..810a7fae3479 100644
--- a/drivers/spi/spi-qup.c
+++ b/drivers/spi/spi-qup.c
@@ -22,6 +22,8 @@
 #include <linux/platform_device.h>
 #include <linux/pm_runtime.h>
 #include <linux/spi/spi.h>
+#include <linux/dmaengine.h>
+#include <linux/dma-mapping.h>
 
 #define QUP_CONFIG			0x0000
 #define QUP_STATE			0x0004
@@ -116,6 +118,8 @@
 
 #define SPI_NUM_CHIPSELECTS		4
 
+#define SPI_MAX_DMA_XFER		(SZ_64K - 64)
+
 /* high speed mode is when bus rate is greater then 26MHz */
 #define SPI_HS_MIN_RATE			26000000
 #define SPI_MAX_RATE			50000000
@@ -140,9 +144,14 @@ struct spi_qup {
 	struct completion	done;
 	int			error;
 	int			w_size;	/* bytes per SPI word */
+	int			n_words;
 	int			tx_bytes;
 	int			rx_bytes;
 	int			qup_v1;
+
+	int			use_dma;
+	struct dma_slave_config	rx_conf;
+	struct dma_slave_config	tx_conf;
 };
 
 
@@ -198,7 +207,6 @@ static int spi_qup_set_state(struct spi_qup *controller, u32 state)
 	return 0;
 }
 
-
 static void spi_qup_fifo_read(struct spi_qup *controller,
 			    struct spi_transfer *xfer)
 {
@@ -266,6 +274,107 @@ static void spi_qup_fifo_write(struct spi_qup *controller,
 	}
 }
 
+static void spi_qup_dma_done(void *data)
+{
+	struct spi_qup *qup = data;
+
+	complete(&qup->done);
+}
+
+static int spi_qup_prep_sg(struct spi_master *master, struct spi_transfer *xfer,
+			   enum dma_transfer_direction dir,
+			   dma_async_tx_callback callback)
+{
+	struct spi_qup *qup = spi_master_get_devdata(master);
+	unsigned long flags = DMA_PREP_INTERRUPT | DMA_PREP_FENCE;
+	struct dma_async_tx_descriptor *desc;
+	struct scatterlist *sgl;
+	struct dma_chan *chan;
+	dma_cookie_t cookie;
+	unsigned int nents;
+
+	if (dir == DMA_MEM_TO_DEV) {
+		chan = master->dma_tx;
+		nents = xfer->tx_sg.nents;
+		sgl = xfer->tx_sg.sgl;
+	} else {
+		chan = master->dma_rx;
+		nents = xfer->rx_sg.nents;
+		sgl = xfer->rx_sg.sgl;
+	}
+
+	desc = dmaengine_prep_slave_sg(chan, sgl, nents, dir, flags);
+	if (!desc)
+		return -EINVAL;
+
+	desc->callback = callback;
+	desc->callback_param = qup;
+
+	cookie = dmaengine_submit(desc);
+
+	return dma_submit_error(cookie);
+}
+
+static void spi_qup_dma_terminate(struct spi_master *master,
+				  struct spi_transfer *xfer)
+{
+	if (xfer->tx_buf)
+		dmaengine_terminate_all(master->dma_tx);
+	if (xfer->rx_buf)
+		dmaengine_terminate_all(master->dma_rx);
+}
+
+static int spi_qup_do_dma(struct spi_master *master, struct spi_transfer *xfer)
+{
+	dma_async_tx_callback rx_done = NULL, tx_done = NULL;
+	int ret;
+
+	if (xfer->rx_buf)
+		rx_done = spi_qup_dma_done;
+	else if (xfer->tx_buf)
+		tx_done = spi_qup_dma_done;
+
+	if (xfer->rx_buf) {
+		ret = spi_qup_prep_sg(master, xfer, DMA_DEV_TO_MEM, rx_done);
+		if (ret)
+			return ret;
+
+		dma_async_issue_pending(master->dma_rx);
+	}
+
+	if (xfer->tx_buf) {
+		ret = spi_qup_prep_sg(master, xfer, DMA_MEM_TO_DEV, tx_done);
+		if (ret)
+			return ret;
+
+		dma_async_issue_pending(master->dma_tx);
+	}
+
+	return 0;
+}
+
+static int spi_qup_do_pio(struct spi_master *master, struct spi_transfer *xfer)
+{
+	struct spi_qup *qup = spi_master_get_devdata(master);
+	int ret;
+
+	ret = spi_qup_set_state(qup, QUP_STATE_RUN);
+	if (ret) {
+		dev_warn(qup->dev, "cannot set RUN state\n");
+		return ret;
+	}
+
+	ret = spi_qup_set_state(qup, QUP_STATE_PAUSE);
+	if (ret) {
+		dev_warn(qup->dev, "cannot set PAUSE state\n");
+		return ret;
+	}
+
+	spi_qup_fifo_write(qup, xfer);
+
+	return 0;
+}
+
 static irqreturn_t spi_qup_qup_irq(int irq, void *dev_id)
 {
 	struct spi_qup *controller = dev_id;
@@ -315,11 +424,13 @@ static irqreturn_t spi_qup_qup_irq(int irq, void *dev_id)
 		error = -EIO;
 	}
 
-	if (opflags & QUP_OP_IN_SERVICE_FLAG)
-		spi_qup_fifo_read(controller, xfer);
+	if (!controller->use_dma) {
+		if (opflags & QUP_OP_IN_SERVICE_FLAG)
+			spi_qup_fifo_read(controller, xfer);
 
-	if (opflags & QUP_OP_OUT_SERVICE_FLAG)
-		spi_qup_fifo_write(controller, xfer);
+		if (opflags & QUP_OP_OUT_SERVICE_FLAG)
+			spi_qup_fifo_write(controller, xfer);
+	}
 
 	spin_lock_irqsave(&controller->lock, flags);
 	controller->error = error;
@@ -332,13 +443,35 @@ static irqreturn_t spi_qup_qup_irq(int irq, void *dev_id)
 	return IRQ_HANDLED;
 }
 
+static u32
+spi_qup_get_mode(struct spi_master *master, struct spi_transfer *xfer)
+{
+	struct spi_qup *qup = spi_master_get_devdata(master);
+	u32 mode;
+
+	qup->w_size = 4;
+
+	if (xfer->bits_per_word <= 8)
+		qup->w_size = 1;
+	else if (xfer->bits_per_word <= 16)
+		qup->w_size = 2;
+
+	qup->n_words = xfer->len / qup->w_size;
+
+	if (qup->n_words <= (qup->in_fifo_sz / sizeof(u32)))
+		mode = QUP_IO_M_MODE_FIFO;
+	else
+		mode = QUP_IO_M_MODE_BLOCK;
+
+	return mode;
+}
 
 /* set clock freq ... bits per word */
 static int spi_qup_io_config(struct spi_device *spi, struct spi_transfer *xfer)
 {
 	struct spi_qup *controller = spi_master_get_devdata(spi->master);
 	u32 config, iomode, mode, control;
-	int ret, n_words, w_size;
+	int ret, n_words;
 
 	if (spi->mode & SPI_LOOP && xfer->len > controller->in_fifo_sz) {
 		dev_err(controller->dev, "too big size for loopback %d > %d\n",
@@ -358,35 +491,54 @@ static int spi_qup_io_config(struct spi_device *spi, struct spi_transfer *xfer)
 		return -EIO;
 	}
 
-	w_size = 4;
-	if (xfer->bits_per_word <= 8)
-		w_size = 1;
-	else if (xfer->bits_per_word <= 16)
-		w_size = 2;
+	mode = spi_qup_get_mode(spi->master, xfer);
+	n_words = controller->n_words;
 
-	n_words = xfer->len / w_size;
-	controller->w_size = w_size;
-
-	if (n_words <= (controller->in_fifo_sz / sizeof(u32))) {
-		mode = QUP_IO_M_MODE_FIFO;
+	if (mode == QUP_IO_M_MODE_FIFO) {
 		writel_relaxed(n_words, controller->base + QUP_MX_READ_CNT);
 		writel_relaxed(n_words, controller->base + QUP_MX_WRITE_CNT);
 		/* must be zero for FIFO */
 		writel_relaxed(0, controller->base + QUP_MX_INPUT_CNT);
 		writel_relaxed(0, controller->base + QUP_MX_OUTPUT_CNT);
-	} else {
-		mode = QUP_IO_M_MODE_BLOCK;
+	} else if (!controller->use_dma) {
 		writel_relaxed(n_words, controller->base + QUP_MX_INPUT_CNT);
 		writel_relaxed(n_words, controller->base + QUP_MX_OUTPUT_CNT);
 		/* must be zero for BLOCK and BAM */
 		writel_relaxed(0, controller->base + QUP_MX_READ_CNT);
 		writel_relaxed(0, controller->base + QUP_MX_WRITE_CNT);
+	} else {
+		mode = QUP_IO_M_MODE_BAM;
+		writel_relaxed(0, controller->base + QUP_MX_READ_CNT);
+		writel_relaxed(0, controller->base + QUP_MX_WRITE_CNT);
+
+		if (!controller->qup_v1) {
+			void __iomem *input_cnt;
+
+			input_cnt = controller->base + QUP_MX_INPUT_CNT;
+			/*
+			 * for DMA transfers, both QUP_MX_INPUT_CNT and
+			 * QUP_MX_OUTPUT_CNT must be zero to all cases but one.
+			 * That case is a non-balanced transfer when there is
+			 * only a rx_buf.
+			 */
+			if (xfer->tx_buf)
+				writel_relaxed(0, input_cnt);
+			else
+				writel_relaxed(n_words, input_cnt);
+
+			writel_relaxed(0, controller->base + QUP_MX_OUTPUT_CNT);
+		}
 	}
 
 	iomode = readl_relaxed(controller->base + QUP_IO_M_MODES);
 	/* Set input and output transfer mode */
 	iomode &= ~(QUP_IO_M_INPUT_MODE_MASK | QUP_IO_M_OUTPUT_MODE_MASK);
-	iomode &= ~(QUP_IO_M_PACK_EN | QUP_IO_M_UNPACK_EN);
+
+	if (!controller->use_dma)
+		iomode &= ~(QUP_IO_M_PACK_EN | QUP_IO_M_UNPACK_EN);
+	else
+		iomode |= QUP_IO_M_PACK_EN | QUP_IO_M_UNPACK_EN;
+
 	iomode |= (mode << QUP_IO_M_OUTPUT_MODE_MASK_SHIFT);
 	iomode |= (mode << QUP_IO_M_INPUT_MODE_MASK_SHIFT);
 
@@ -428,11 +580,31 @@ static int spi_qup_io_config(struct spi_device *spi, struct spi_transfer *xfer)
 	config &= ~(QUP_CONFIG_NO_INPUT | QUP_CONFIG_NO_OUTPUT | QUP_CONFIG_N);
 	config |= xfer->bits_per_word - 1;
 	config |= QUP_CONFIG_SPI_MODE;
+
+	if (controller->use_dma) {
+		if (!xfer->tx_buf)
+			config |= QUP_CONFIG_NO_OUTPUT;
+		if (!xfer->rx_buf)
+			config |= QUP_CONFIG_NO_INPUT;
+	}
+
 	writel_relaxed(config, controller->base + QUP_CONFIG);
 
 	/* only write to OPERATIONAL_MASK when register is present */
-	if (!controller->qup_v1)
-		writel_relaxed(0, controller->base + QUP_OPERATIONAL_MASK);
+	if (!controller->qup_v1) {
+		u32 mask = 0;
+
+		/*
+		 * mask INPUT and OUTPUT service flags to prevent IRQs on FIFO
+		 * status change in BAM mode
+		 */
+
+		if (mode == QUP_IO_M_MODE_BAM)
+			mask = QUP_OP_IN_SERVICE_FLAG | QUP_OP_OUT_SERVICE_FLAG;
+
+		writel_relaxed(mask, controller->base + QUP_OPERATIONAL_MASK);
+	}
+
 	return 0;
 }
 
@@ -461,17 +633,13 @@ static int spi_qup_transfer_one(struct spi_master *master,
 	controller->tx_bytes = 0;
 	spin_unlock_irqrestore(&controller->lock, flags);
 
-	if (spi_qup_set_state(controller, QUP_STATE_RUN)) {
-		dev_warn(controller->dev, "cannot set RUN state\n");
-		goto exit;
-	}
+	if (controller->use_dma)
+		ret = spi_qup_do_dma(master, xfer);
+	else
+		ret = spi_qup_do_pio(master, xfer);
 
-	if (spi_qup_set_state(controller, QUP_STATE_PAUSE)) {
-		dev_warn(controller->dev, "cannot set PAUSE state\n");
+	if (ret)
 		goto exit;
-	}
-
-	spi_qup_fifo_write(controller, xfer);
 
 	if (spi_qup_set_state(controller, QUP_STATE_RUN)) {
 		dev_warn(controller->dev, "cannot set EXECUTE state\n");
@@ -480,6 +648,7 @@ static int spi_qup_transfer_one(struct spi_master *master,
 
 	if (!wait_for_completion_timeout(&controller->done, timeout))
 		ret = -ETIMEDOUT;
+
 exit:
 	spi_qup_set_state(controller, QUP_STATE_RESET);
 	spin_lock_irqsave(&controller->lock, flags);
@@ -487,6 +656,97 @@ exit:
 	if (!ret)
 		ret = controller->error;
 	spin_unlock_irqrestore(&controller->lock, flags);
+
+	if (ret && controller->use_dma)
+		spi_qup_dma_terminate(master, xfer);
+
+	return ret;
+}
+
+static bool spi_qup_can_dma(struct spi_master *master, struct spi_device *spi,
+			    struct spi_transfer *xfer)
+{
+	struct spi_qup *qup = spi_master_get_devdata(master);
+	size_t dma_align = dma_get_cache_alignment();
+	u32 mode;
+
+	qup->use_dma = 0;
+
+	if (xfer->rx_buf && (xfer->len % qup->in_blk_sz ||
+	    IS_ERR_OR_NULL(master->dma_rx) ||
+	    !IS_ALIGNED((size_t)xfer->rx_buf, dma_align)))
+		return false;
+
+	if (xfer->tx_buf && (xfer->len % qup->out_blk_sz ||
+	    IS_ERR_OR_NULL(master->dma_tx) ||
+	    !IS_ALIGNED((size_t)xfer->tx_buf, dma_align)))
+		return false;
+
+	mode = spi_qup_get_mode(master, xfer);
+	if (mode == QUP_IO_M_MODE_FIFO)
+		return false;
+
+	qup->use_dma = 1;
+
+	return true;
+}
+
+static void spi_qup_release_dma(struct spi_master *master)
+{
+	if (!IS_ERR_OR_NULL(master->dma_rx))
+		dma_release_channel(master->dma_rx);
+	if (!IS_ERR_OR_NULL(master->dma_tx))
+		dma_release_channel(master->dma_tx);
+}
+
+static int spi_qup_init_dma(struct spi_master *master, resource_size_t base)
+{
+	struct spi_qup *spi = spi_master_get_devdata(master);
+	struct dma_slave_config *rx_conf = &spi->rx_conf,
+				*tx_conf = &spi->tx_conf;
+	struct device *dev = spi->dev;
+	int ret;
+
+	/* allocate dma resources, if available */
+	master->dma_rx = dma_request_slave_channel_reason(dev, "rx");
+	if (IS_ERR(master->dma_rx))
+		return PTR_ERR(master->dma_rx);
+
+	master->dma_tx = dma_request_slave_channel_reason(dev, "tx");
+	if (IS_ERR(master->dma_tx)) {
+		ret = PTR_ERR(master->dma_tx);
+		goto err_tx;
+	}
+
+	/* set DMA parameters */
+	rx_conf->direction = DMA_DEV_TO_MEM;
+	rx_conf->device_fc = 1;
+	rx_conf->src_addr = base + QUP_INPUT_FIFO;
+	rx_conf->src_maxburst = spi->in_blk_sz;
+
+	tx_conf->direction = DMA_MEM_TO_DEV;
+	tx_conf->device_fc = 1;
+	tx_conf->dst_addr = base + QUP_OUTPUT_FIFO;
+	tx_conf->dst_maxburst = spi->out_blk_sz;
+
+	ret = dmaengine_slave_config(master->dma_rx, rx_conf);
+	if (ret) {
+		dev_err(dev, "failed to configure RX channel\n");
+		goto err;
+	}
+
+	ret = dmaengine_slave_config(master->dma_tx, tx_conf);
+	if (ret) {
+		dev_err(dev, "failed to configure TX channel\n");
+		goto err;
+	}
+
+	return 0;
+
+err:
+	dma_release_channel(master->dma_tx);
+err_tx:
+	dma_release_channel(master->dma_rx);
 	return ret;
 }
 
@@ -498,7 +758,7 @@ static int spi_qup_probe(struct platform_device *pdev)
 	struct resource *res;
 	struct device *dev;
 	void __iomem *base;
-	u32 max_freq, iomode;
+	u32 max_freq, iomode, num_cs;
 	int ret, irq, size;
 
 	dev = &pdev->dev;
@@ -550,10 +810,11 @@ static int spi_qup_probe(struct platform_device *pdev)
 	}
 
 	/* use num-cs unless not present or out of range */
-	if (of_property_read_u16(dev->of_node, "num-cs",
-			&master->num_chipselect) ||
-			(master->num_chipselect > SPI_NUM_CHIPSELECTS))
+	if (of_property_read_u32(dev->of_node, "num-cs", &num_cs) ||
+	    num_cs > SPI_NUM_CHIPSELECTS)
 		master->num_chipselect = SPI_NUM_CHIPSELECTS;
+	else
+		master->num_chipselect = num_cs;
 
 	master->bus_num = pdev->id;
 	master->mode_bits = SPI_CPOL | SPI_CPHA | SPI_CS_HIGH | SPI_LOOP;
@@ -562,6 +823,8 @@ static int spi_qup_probe(struct platform_device *pdev)
 	master->transfer_one = spi_qup_transfer_one;
 	master->dev.of_node = pdev->dev.of_node;
 	master->auto_runtime_pm = true;
+	master->dma_alignment = dma_get_cache_alignment();
+	master->max_dma_len = SPI_MAX_DMA_XFER;
 
 	platform_set_drvdata(pdev, master);
 
@@ -573,6 +836,12 @@ static int spi_qup_probe(struct platform_device *pdev)
 	controller->cclk = cclk;
 	controller->irq = irq;
 
+	ret = spi_qup_init_dma(master, res->start);
+	if (ret == -EPROBE_DEFER)
+		goto error;
+	else if (!ret)
+		master->can_dma = spi_qup_can_dma;
+
 	/* set v1 flag if device is version 1 */
 	if (of_device_is_compatible(dev->of_node, "qcom,spi-qup-v1.1.1"))
 		controller->qup_v1 = 1;
@@ -609,7 +878,7 @@ static int spi_qup_probe(struct platform_device *pdev)
 	ret = spi_qup_set_state(controller, QUP_STATE_RESET);
 	if (ret) {
 		dev_err(dev, "cannot set RESET state\n");
-		goto error;
+		goto error_dma;
 	}
 
 	writel_relaxed(0, base + QUP_OPERATIONAL);
@@ -633,7 +902,7 @@ static int spi_qup_probe(struct platform_device *pdev)
 	ret = devm_request_irq(dev, irq, spi_qup_qup_irq,
 			       IRQF_TRIGGER_HIGH, pdev->name, controller);
 	if (ret)
-		goto error;
+		goto error_dma;
 
 	pm_runtime_set_autosuspend_delay(dev, MSEC_PER_SEC);
 	pm_runtime_use_autosuspend(dev);
@@ -648,6 +917,8 @@ static int spi_qup_probe(struct platform_device *pdev)
 
 disable_pm:
 	pm_runtime_disable(&pdev->dev);
+error_dma:
+	spi_qup_release_dma(master);
 error:
 	clk_disable_unprepare(cclk);
 	clk_disable_unprepare(iclk);
@@ -739,6 +1010,8 @@ static int spi_qup_remove(struct platform_device *pdev)
 	if (ret)
 		return ret;
 
+	spi_qup_release_dma(master);
+
 	clk_disable_unprepare(controller->cclk);
 	clk_disable_unprepare(controller->iclk);
 
diff --git a/drivers/spi/spi-rockchip.c b/drivers/spi/spi-rockchip.c
index 1a777dc261d6..68e7efeb9a27 100644
--- a/drivers/spi/spi-rockchip.c
+++ b/drivers/spi/spi-rockchip.c
@@ -179,6 +179,7 @@ struct rockchip_spi {
 	u8 tmode;
 	u8 bpw;
 	u8 n_bytes;
+	u8 rsd_nsecs;
 	unsigned len;
 	u32 speed;
 
@@ -302,8 +303,8 @@ static int rockchip_spi_prepare_message(struct spi_master *master,
 	return 0;
 }
 
-static int rockchip_spi_unprepare_message(struct spi_master *master,
-					  struct spi_message *msg)
+static void rockchip_spi_handle_err(struct spi_master *master,
+				    struct spi_message *msg)
 {
 	unsigned long flags;
 	struct rockchip_spi *rs = spi_master_get_devdata(master);
@@ -313,8 +314,8 @@ static int rockchip_spi_unprepare_message(struct spi_master *master,
 	/*
 	 * For DMA mode, we need terminate DMA channel and flush
 	 * fifo for the next transfer if DMA thansfer timeout.
-	 * unprepare_message() was called by core if transfer complete
-	 * or timeout. Maybe it is reasonable for error handling here.
+	 * handle_err() was called by core if transfer failed.
+	 * Maybe it is reasonable for error handling here.
 	 */
 	if (rs->use_dma) {
 		if (rs->state & RXBUSY) {
@@ -327,6 +328,12 @@ static int rockchip_spi_unprepare_message(struct spi_master *master,
 	}
 
 	spin_unlock_irqrestore(&rs->lock, flags);
+}
+
+static int rockchip_spi_unprepare_message(struct spi_master *master,
+					  struct spi_message *msg)
+{
+	struct rockchip_spi *rs = spi_master_get_devdata(master);
 
 	spi_enable_chip(rs, 0);
 
@@ -493,6 +500,7 @@ static void rockchip_spi_config(struct rockchip_spi *rs)
 {
 	u32 div = 0;
 	u32 dmacr = 0;
+	int rsd = 0;
 
 	u32 cr0 = (CR0_BHT_8BIT << CR0_BHT_OFFSET)
 		| (CR0_SSD_ONE << CR0_SSD_OFFSET);
@@ -519,9 +527,23 @@ static void rockchip_spi_config(struct rockchip_spi *rs)
 	}
 
 	/* div doesn't support odd number */
-	div = max_t(u32, rs->max_freq / rs->speed, 1);
+	div = DIV_ROUND_UP(rs->max_freq, rs->speed);
 	div = (div + 1) & 0xfffe;
 
+	/* Rx sample delay is expressed in parent clock cycles (max 3) */
+	rsd = DIV_ROUND_CLOSEST(rs->rsd_nsecs * (rs->max_freq >> 8),
+				1000000000 >> 8);
+	if (!rsd && rs->rsd_nsecs) {
+		pr_warn_once("rockchip-spi: %u Hz are too slow to express %u ns delay\n",
+			     rs->max_freq, rs->rsd_nsecs);
+	} else if (rsd > 3) {
+		rsd = 3;
+		pr_warn_once("rockchip-spi: %u Hz are too fast to express %u ns delay, clamping at %u ns\n",
+			     rs->max_freq, rs->rsd_nsecs,
+			     rsd * 1000000000U / rs->max_freq);
+	}
+	cr0 |= rsd << CR0_RSD_OFFSET;
+
 	writel_relaxed(cr0, rs->regs + ROCKCHIP_SPI_CTRLR0);
 
 	writel_relaxed(rs->len - 1, rs->regs + ROCKCHIP_SPI_CTRLR1);
@@ -614,6 +636,7 @@ static int rockchip_spi_probe(struct platform_device *pdev)
 	struct rockchip_spi *rs;
 	struct spi_master *master;
 	struct resource *mem;
+	u32 rsd_nsecs;
 
 	master = spi_alloc_master(&pdev->dev, sizeof(struct rockchip_spi));
 	if (!master)
@@ -665,6 +688,10 @@ static int rockchip_spi_probe(struct platform_device *pdev)
 	rs->dev = &pdev->dev;
 	rs->max_freq = clk_get_rate(rs->spiclk);
 
+	if (!of_property_read_u32(pdev->dev.of_node, "rx-sample-delay-ns",
+				  &rsd_nsecs))
+		rs->rsd_nsecs = rsd_nsecs;
+
 	rs->fifo_len = get_fifo_len(rs);
 	if (!rs->fifo_len) {
 		dev_err(&pdev->dev, "Failed to get fifo length\n");
@@ -688,6 +715,7 @@ static int rockchip_spi_probe(struct platform_device *pdev)
 	master->prepare_message = rockchip_spi_prepare_message;
 	master->unprepare_message = rockchip_spi_unprepare_message;
 	master->transfer_one = rockchip_spi_transfer_one;
+	master->handle_err = rockchip_spi_handle_err;
 
 	rs->dma_tx.ch = dma_request_slave_channel(rs->dev, "tx");
 	if (!rs->dma_tx.ch)
diff --git a/drivers/spi/spi-rspi.c b/drivers/spi/spi-rspi.c
index 46ce47076e63..186924aa4740 100644
--- a/drivers/spi/spi-rspi.c
+++ b/drivers/spi/spi-rspi.c
@@ -177,6 +177,13 @@
 #define SPBFCR_RXRST		0x40	/* Receive Buffer Data Reset */
 #define SPBFCR_TXTRG_MASK	0x30	/* Transmit Buffer Data Triggering Number */
 #define SPBFCR_RXTRG_MASK	0x07	/* Receive Buffer Data Triggering Number */
+/* QSPI on R-Car Gen2 */
+#define SPBFCR_TXTRG_1B		0x00	/* 31 bytes (1 byte available) */
+#define SPBFCR_TXTRG_32B	0x30	/* 0 byte (32 bytes available) */
+#define SPBFCR_RXTRG_1B		0x00	/* 1 byte (31 bytes available) */
+#define SPBFCR_RXTRG_32B	0x07	/* 32 bytes (0 byte available) */
+
+#define QSPI_BUFFER_SIZE        32u
 
 struct rspi_data {
 	void __iomem *addr;
@@ -366,6 +373,52 @@ static int qspi_set_config_register(struct rspi_data *rspi, int access_size)
 	return 0;
 }
 
+static void qspi_update(const struct rspi_data *rspi, u8 mask, u8 val, u8 reg)
+{
+	u8 data;
+
+	data = rspi_read8(rspi, reg);
+	data &= ~mask;
+	data |= (val & mask);
+	rspi_write8(rspi, data, reg);
+}
+
+static int qspi_set_send_trigger(struct rspi_data *rspi, unsigned int len)
+{
+	unsigned int n;
+
+	n = min(len, QSPI_BUFFER_SIZE);
+
+	if (len >= QSPI_BUFFER_SIZE) {
+		/* sets triggering number to 32 bytes */
+		qspi_update(rspi, SPBFCR_TXTRG_MASK,
+			     SPBFCR_TXTRG_32B, QSPI_SPBFCR);
+	} else {
+		/* sets triggering number to 1 byte */
+		qspi_update(rspi, SPBFCR_TXTRG_MASK,
+			     SPBFCR_TXTRG_1B, QSPI_SPBFCR);
+	}
+
+	return n;
+}
+
+static void qspi_set_receive_trigger(struct rspi_data *rspi, unsigned int len)
+{
+	unsigned int n;
+
+	n = min(len, QSPI_BUFFER_SIZE);
+
+	if (len >= QSPI_BUFFER_SIZE) {
+		/* sets triggering number to 32 bytes */
+		qspi_update(rspi, SPBFCR_RXTRG_MASK,
+			     SPBFCR_RXTRG_32B, QSPI_SPBFCR);
+	} else {
+		/* sets triggering number to 1 byte */
+		qspi_update(rspi, SPBFCR_RXTRG_MASK,
+			     SPBFCR_RXTRG_1B, QSPI_SPBFCR);
+	}
+}
+
 #define set_config_register(spi, n) spi->ops->set_config_register(spi, n)
 
 static void rspi_enable_irq(const struct rspi_data *rspi, u8 enable)
@@ -609,19 +662,29 @@ static bool rspi_can_dma(struct spi_master *master, struct spi_device *spi,
 	return __rspi_can_dma(rspi, xfer);
 }
 
-static int rspi_common_transfer(struct rspi_data *rspi,
-				struct spi_transfer *xfer)
+static int rspi_dma_check_then_transfer(struct rspi_data *rspi,
+					 struct spi_transfer *xfer)
 {
-	int ret;
-
 	if (rspi->master->can_dma && __rspi_can_dma(rspi, xfer)) {
 		/* rx_buf can be NULL on RSPI on SH in TX-only Mode */
-		ret = rspi_dma_transfer(rspi, &xfer->tx_sg,
+		int ret = rspi_dma_transfer(rspi, &xfer->tx_sg,
 					xfer->rx_buf ? &xfer->rx_sg : NULL);
 		if (ret != -EAGAIN)
-			return ret;
+			return 0;
 	}
 
+	return -EAGAIN;
+}
+
+static int rspi_common_transfer(struct rspi_data *rspi,
+				struct spi_transfer *xfer)
+{
+	int ret;
+
+	ret = rspi_dma_check_then_transfer(rspi, xfer);
+	if (ret != -EAGAIN)
+		return ret;
+
 	ret = rspi_pio_transfer(rspi, xfer->tx_buf, xfer->rx_buf, xfer->len);
 	if (ret < 0)
 		return ret;
@@ -661,12 +724,59 @@ static int rspi_rz_transfer_one(struct spi_master *master,
 	return rspi_common_transfer(rspi, xfer);
 }
 
+static int qspi_trigger_transfer_out_int(struct rspi_data *rspi, const u8 *tx,
+					u8 *rx, unsigned int len)
+{
+	int i, n, ret;
+	int error;
+
+	while (len > 0) {
+		n = qspi_set_send_trigger(rspi, len);
+		qspi_set_receive_trigger(rspi, len);
+		if (n == QSPI_BUFFER_SIZE) {
+			error = rspi_wait_for_tx_empty(rspi);
+			if (error < 0) {
+				dev_err(&rspi->master->dev, "transmit timeout\n");
+				return error;
+			}
+			for (i = 0; i < n; i++)
+				rspi_write_data(rspi, *tx++);
+
+			error = rspi_wait_for_rx_full(rspi);
+			if (error < 0) {
+				dev_err(&rspi->master->dev, "receive timeout\n");
+				return error;
+			}
+			for (i = 0; i < n; i++)
+				*rx++ = rspi_read_data(rspi);
+		} else {
+			ret = rspi_pio_transfer(rspi, tx, rx, n);
+			if (ret < 0)
+				return ret;
+		}
+		len -= n;
+	}
+
+	return 0;
+}
+
 static int qspi_transfer_out_in(struct rspi_data *rspi,
 				struct spi_transfer *xfer)
 {
+	int ret;
+
 	qspi_receive_init(rspi);
 
-	return rspi_common_transfer(rspi, xfer);
+	ret = rspi_dma_check_then_transfer(rspi, xfer);
+	if (ret != -EAGAIN)
+		return ret;
+
+	ret = qspi_trigger_transfer_out_int(rspi, xfer->tx_buf,
+					    xfer->rx_buf, xfer->len);
+	if (ret < 0)
+		return ret;
+
+	return 0;
 }
 
 static int qspi_transfer_out(struct rspi_data *rspi, struct spi_transfer *xfer)
diff --git a/drivers/spi/spi-s3c64xx.c b/drivers/spi/spi-s3c64xx.c
index 9231c34b5a5c..b1c6731fbf27 100644
--- a/drivers/spi/spi-s3c64xx.c
+++ b/drivers/spi/spi-s3c64xx.c
@@ -324,7 +324,7 @@ static int s3c64xx_spi_prepare_transfer(struct spi_master *spi)
 
 		/* Acquire DMA channels */
 		sdd->rx_dma.ch = dma_request_slave_channel_compat(mask, filter,
-				   (void *)sdd->rx_dma.dmach, dev, "rx");
+				   (void *)(long)sdd->rx_dma.dmach, dev, "rx");
 		if (!sdd->rx_dma.ch) {
 			dev_err(dev, "Failed to get RX DMA channel\n");
 			ret = -EBUSY;
@@ -333,7 +333,7 @@ static int s3c64xx_spi_prepare_transfer(struct spi_master *spi)
 		spi->dma_rx = sdd->rx_dma.ch;
 
 		sdd->tx_dma.ch = dma_request_slave_channel_compat(mask, filter,
-				   (void *)sdd->tx_dma.dmach, dev, "tx");
+				   (void *)(long)sdd->tx_dma.dmach, dev, "tx");
 		if (!sdd->tx_dma.ch) {
 			dev_err(dev, "Failed to get TX DMA channel\n");
 			ret = -EBUSY;
diff --git a/drivers/spi/spi-sc18is602.c b/drivers/spi/spi-sc18is602.c
index 5a56acf8a43e..36af4d48a700 100644
--- a/drivers/spi/spi-sc18is602.c
+++ b/drivers/spi/spi-sc18is602.c
@@ -286,7 +286,7 @@ static int sc18is602_probe(struct i2c_client *client,
 			hw->freq = SC18IS602_CLOCK;
 		break;
 	}
-	master->bus_num = client->adapter->nr;
+	master->bus_num = np ? -1 : client->adapter->nr;
 	master->mode_bits = SPI_CPHA | SPI_CPOL | SPI_LSB_FIRST;
 	master->bits_per_word_mask = SPI_BPW_MASK(8);
 	master->setup = sc18is602_setup;
diff --git a/drivers/spi/spi-st-ssc4.c b/drivers/spi/spi-st-ssc4.c
index 2faeaa7b57a8..f17c0abe299f 100644
--- a/drivers/spi/spi-st-ssc4.c
+++ b/drivers/spi/spi-st-ssc4.c
@@ -482,7 +482,7 @@ static const struct dev_pm_ops spi_st_pm = {
 	SET_RUNTIME_PM_OPS(spi_st_runtime_suspend, spi_st_runtime_resume, NULL)
 };
 
-static struct of_device_id stm_spi_match[] = {
+static const struct of_device_id stm_spi_match[] = {
 	{ .compatible = "st,comms-ssc4-spi", },
 	{},
 };
diff --git a/drivers/spi/spi-ti-qspi.c b/drivers/spi/spi-ti-qspi.c
index 884a716e50cb..5c0616870358 100644
--- a/drivers/spi/spi-ti-qspi.c
+++ b/drivers/spi/spi-ti-qspi.c
@@ -101,6 +101,7 @@ struct ti_qspi {
 #define QSPI_FLEN(n)			((n - 1) << 0)
 
 /* STATUS REGISTER */
+#define BUSY				0x01
 #define WC				0x02
 
 /* INTERRUPT REGISTER */
@@ -199,6 +200,21 @@ static void ti_qspi_restore_ctx(struct ti_qspi *qspi)
 	ti_qspi_write(qspi, ctx_reg->clkctrl, QSPI_SPI_CLOCK_CNTRL_REG);
 }
 
+static inline u32 qspi_is_busy(struct ti_qspi *qspi)
+{
+	u32 stat;
+	unsigned long timeout = jiffies + QSPI_COMPLETION_TIMEOUT;
+
+	stat = ti_qspi_read(qspi, QSPI_SPI_STATUS_REG);
+	while ((stat & BUSY) && time_after(timeout, jiffies)) {
+		cpu_relax();
+		stat = ti_qspi_read(qspi, QSPI_SPI_STATUS_REG);
+	}
+
+	WARN(stat & BUSY, "qspi busy\n");
+	return stat & BUSY;
+}
+
 static int qspi_write_msg(struct ti_qspi *qspi, struct spi_transfer *t)
 {
 	int wlen, count;
@@ -211,6 +227,9 @@ static int qspi_write_msg(struct ti_qspi *qspi, struct spi_transfer *t)
 	wlen = t->bits_per_word >> 3;	/* in bytes */
 
 	while (count) {
+		if (qspi_is_busy(qspi))
+			return -EBUSY;
+
 		switch (wlen) {
 		case 1:
 			dev_dbg(qspi->dev, "tx cmd %08x dc %08x data %02x\n",
@@ -266,6 +285,9 @@ static int qspi_read_msg(struct ti_qspi *qspi, struct spi_transfer *t)
 
 	while (count) {
 		dev_dbg(qspi->dev, "rx cmd %08x dc %08x\n", cmd, qspi->dc);
+		if (qspi_is_busy(qspi))
+			return -EBUSY;
+
 		ti_qspi_write(qspi, cmd, QSPI_SPI_CMD_REG);
 		if (!wait_for_completion_timeout(&qspi->transfer_complete,
 						 QSPI_COMPLETION_TIMEOUT)) {
diff --git a/drivers/spi/spi.c b/drivers/spi/spi.c
index c64a3e59fce3..d5d7d2235163 100644
--- a/drivers/spi/spi.c
+++ b/drivers/spi/spi.c
@@ -16,7 +16,6 @@
  */
 
 #include <linux/kernel.h>
-#include <linux/kmod.h>
 #include <linux/device.h>
 #include <linux/init.h>
 #include <linux/cache.h>
@@ -129,125 +128,11 @@ static int spi_uevent(struct device *dev, struct kobj_uevent_env *env)
 	return 0;
 }
 
-#ifdef CONFIG_PM_SLEEP
-static int spi_legacy_suspend(struct device *dev, pm_message_t message)
-{
-	int			value = 0;
-	struct spi_driver	*drv = to_spi_driver(dev->driver);
-
-	/* suspend will stop irqs and dma; no more i/o */
-	if (drv) {
-		if (drv->suspend)
-			value = drv->suspend(to_spi_device(dev), message);
-		else
-			dev_dbg(dev, "... can't suspend\n");
-	}
-	return value;
-}
-
-static int spi_legacy_resume(struct device *dev)
-{
-	int			value = 0;
-	struct spi_driver	*drv = to_spi_driver(dev->driver);
-
-	/* resume may restart the i/o queue */
-	if (drv) {
-		if (drv->resume)
-			value = drv->resume(to_spi_device(dev));
-		else
-			dev_dbg(dev, "... can't resume\n");
-	}
-	return value;
-}
-
-static int spi_pm_suspend(struct device *dev)
-{
-	const struct dev_pm_ops *pm = dev->driver ? dev->driver->pm : NULL;
-
-	if (pm)
-		return pm_generic_suspend(dev);
-	else
-		return spi_legacy_suspend(dev, PMSG_SUSPEND);
-}
-
-static int spi_pm_resume(struct device *dev)
-{
-	const struct dev_pm_ops *pm = dev->driver ? dev->driver->pm : NULL;
-
-	if (pm)
-		return pm_generic_resume(dev);
-	else
-		return spi_legacy_resume(dev);
-}
-
-static int spi_pm_freeze(struct device *dev)
-{
-	const struct dev_pm_ops *pm = dev->driver ? dev->driver->pm : NULL;
-
-	if (pm)
-		return pm_generic_freeze(dev);
-	else
-		return spi_legacy_suspend(dev, PMSG_FREEZE);
-}
-
-static int spi_pm_thaw(struct device *dev)
-{
-	const struct dev_pm_ops *pm = dev->driver ? dev->driver->pm : NULL;
-
-	if (pm)
-		return pm_generic_thaw(dev);
-	else
-		return spi_legacy_resume(dev);
-}
-
-static int spi_pm_poweroff(struct device *dev)
-{
-	const struct dev_pm_ops *pm = dev->driver ? dev->driver->pm : NULL;
-
-	if (pm)
-		return pm_generic_poweroff(dev);
-	else
-		return spi_legacy_suspend(dev, PMSG_HIBERNATE);
-}
-
-static int spi_pm_restore(struct device *dev)
-{
-	const struct dev_pm_ops *pm = dev->driver ? dev->driver->pm : NULL;
-
-	if (pm)
-		return pm_generic_restore(dev);
-	else
-		return spi_legacy_resume(dev);
-}
-#else
-#define spi_pm_suspend	NULL
-#define spi_pm_resume	NULL
-#define spi_pm_freeze	NULL
-#define spi_pm_thaw	NULL
-#define spi_pm_poweroff	NULL
-#define spi_pm_restore	NULL
-#endif
-
-static const struct dev_pm_ops spi_pm = {
-	.suspend = spi_pm_suspend,
-	.resume = spi_pm_resume,
-	.freeze = spi_pm_freeze,
-	.thaw = spi_pm_thaw,
-	.poweroff = spi_pm_poweroff,
-	.restore = spi_pm_restore,
-	SET_RUNTIME_PM_OPS(
-		pm_generic_runtime_suspend,
-		pm_generic_runtime_resume,
-		NULL
-	)
-};
-
 struct bus_type spi_bus_type = {
 	.name		= "spi",
 	.dev_groups	= spi_dev_groups,
 	.match		= spi_match_device,
 	.uevent		= spi_uevent,
-	.pm		= &spi_pm,
 };
 EXPORT_SYMBOL_GPL(spi_bus_type);
 
@@ -851,6 +736,9 @@ out:
 	if (msg->status == -EINPROGRESS)
 		msg->status = ret;
 
+	if (msg->status && master->handle_err)
+		master->handle_err(master, msg);
+
 	spi_finalize_current_message(master);
 
 	return ret;
@@ -1105,13 +993,14 @@ void spi_finalize_current_message(struct spi_master *master)
 				"failed to unprepare message: %d\n", ret);
 		}
 	}
+
+	trace_spi_message_done(mesg);
+
 	master->cur_msg_prepared = false;
 
 	mesg->state = NULL;
 	if (mesg->complete)
 		mesg->complete(mesg->context);
-
-	trace_spi_message_done(mesg);
 }
 EXPORT_SYMBOL_GPL(spi_finalize_current_message);
 
@@ -1359,7 +1248,6 @@ of_register_spi_device(struct spi_master *master, struct device_node *nc)
 	spi->dev.of_node = nc;
 
 	/* Register the new device */
-	request_module("%s%s", SPI_MODULE_PREFIX, spi->modalias);
 	rc = spi_add_device(spi);
 	if (rc) {
 		dev_err(&master->dev, "spi_device register error %s\n",
@@ -1893,6 +1781,8 @@ int spi_setup(struct spi_device *spi)
 	if (!spi->max_speed_hz)
 		spi->max_speed_hz = spi->master->max_speed_hz;
 
+	spi_set_cs(spi, false);
+
 	if (spi->master->setup)
 		status = spi->master->setup(spi);
 
diff --git a/drivers/spi/spidev.c b/drivers/spi/spidev.c
index 4eb7a980e670..92c909eed6b5 100644
--- a/drivers/spi/spidev.c
+++ b/drivers/spi/spidev.c
@@ -223,7 +223,7 @@ static int spidev_message(struct spidev_data *spidev,
 	struct spi_transfer	*k_xfers;
 	struct spi_transfer	*k_tmp;
 	struct spi_ioc_transfer *u_tmp;
-	unsigned		n, total;
+	unsigned		n, total, tx_total, rx_total;
 	u8			*tx_buf, *rx_buf;
 	int			status = -EFAULT;
 
@@ -239,33 +239,52 @@ static int spidev_message(struct spidev_data *spidev,
 	tx_buf = spidev->tx_buffer;
 	rx_buf = spidev->rx_buffer;
 	total = 0;
+	tx_total = 0;
+	rx_total = 0;
 	for (n = n_xfers, k_tmp = k_xfers, u_tmp = u_xfers;
 			n;
 			n--, k_tmp++, u_tmp++) {
 		k_tmp->len = u_tmp->len;
 
 		total += k_tmp->len;
-		if (total > bufsiz) {
+		/* Since the function returns the total length of transfers
+		 * on success, restrict the total to positive int values to
+		 * avoid the return value looking like an error.  Also check
+		 * each transfer length to avoid arithmetic overflow.
+		 */
+		if (total > INT_MAX || k_tmp->len > INT_MAX) {
 			status = -EMSGSIZE;
 			goto done;
 		}
 
 		if (u_tmp->rx_buf) {
+			/* this transfer needs space in RX bounce buffer */
+			rx_total += k_tmp->len;
+			if (rx_total > bufsiz) {
+				status = -EMSGSIZE;
+				goto done;
+			}
 			k_tmp->rx_buf = rx_buf;
 			if (!access_ok(VERIFY_WRITE, (u8 __user *)
 						(uintptr_t) u_tmp->rx_buf,
 						u_tmp->len))
 				goto done;
+			rx_buf += k_tmp->len;
 		}
 		if (u_tmp->tx_buf) {
+			/* this transfer needs space in TX bounce buffer */
+			tx_total += k_tmp->len;
+			if (tx_total > bufsiz) {
+				status = -EMSGSIZE;
+				goto done;
+			}
 			k_tmp->tx_buf = tx_buf;
 			if (copy_from_user(tx_buf, (const u8 __user *)
 						(uintptr_t) u_tmp->tx_buf,
 					u_tmp->len))
 				goto done;
+			tx_buf += k_tmp->len;
 		}
-		tx_buf += k_tmp->len;
-		rx_buf += k_tmp->len;
 
 		k_tmp->cs_change = !!u_tmp->cs_change;
 		k_tmp->tx_nbits = u_tmp->tx_nbits;
@@ -303,8 +322,8 @@ static int spidev_message(struct spidev_data *spidev,
 				status = -EFAULT;
 				goto done;
 			}
+			rx_buf += u_tmp->len;
 		}
-		rx_buf += u_tmp->len;
 	}
 	status = total;
 
@@ -684,6 +703,14 @@ static const struct file_operations spidev_fops = {
 
 static struct class *spidev_class;
 
+#ifdef CONFIG_OF
+static const struct of_device_id spidev_dt_ids[] = {
+	{ .compatible = "rohm,dh2228fv" },
+	{},
+};
+MODULE_DEVICE_TABLE(of, spidev_dt_ids);
+#endif
+
 /*-------------------------------------------------------------------------*/
 
 static int spidev_probe(struct spi_device *spi)
@@ -692,6 +719,17 @@ static int spidev_probe(struct spi_device *spi)
 	int			status;
 	unsigned long		minor;
 
+	/*
+	 * spidev should never be referenced in DT without a specific
+	 * compatbile string, it is a Linux implementation thing
+	 * rather than a description of the hardware.
+	 */
+	if (spi->dev.of_node && !of_match_device(spidev_dt_ids, &spi->dev)) {
+		dev_err(&spi->dev, "buggy DT: spidev listed directly in DT\n");
+		WARN_ON(spi->dev.of_node &&
+			!of_match_device(spidev_dt_ids, &spi->dev));
+	}
+
 	/* Allocate driver data */
 	spidev = kzalloc(sizeof(*spidev), GFP_KERNEL);
 	if (!spidev)
@@ -758,13 +796,6 @@ static int spidev_remove(struct spi_device *spi)
 	return 0;
 }
 
-static const struct of_device_id spidev_dt_ids[] = {
-	{ .compatible = "rohm,dh2228fv" },
-	{},
-};
-
-MODULE_DEVICE_TABLE(of, spidev_dt_ids);
-
 static struct spi_driver spidev_spi_driver = {
 	.driver = {
 		.name =		"spidev",
diff --git a/drivers/staging/Kconfig b/drivers/staging/Kconfig
index 45baa83be7ce..bfacf69f68f4 100644
--- a/drivers/staging/Kconfig
+++ b/drivers/staging/Kconfig
@@ -58,6 +58,8 @@ source "drivers/staging/iio/Kconfig"
 
 source "drivers/staging/sm7xxfb/Kconfig"
 
+source "drivers/staging/sm750fb/Kconfig"
+
 source "drivers/staging/xgifb/Kconfig"
 
 source "drivers/staging/emxx_udc/Kconfig"
@@ -108,4 +110,6 @@ source "drivers/staging/fbtft/Kconfig"
 
 source "drivers/staging/i2o/Kconfig"
 
+source "drivers/staging/fsl-mc/Kconfig"
+
 endif # STAGING
diff --git a/drivers/staging/Makefile b/drivers/staging/Makefile
index 29160790841f..2bbd1bf04c55 100644
--- a/drivers/staging/Makefile
+++ b/drivers/staging/Makefile
@@ -23,6 +23,7 @@ obj-$(CONFIG_VT6656)		+= vt6656/
 obj-$(CONFIG_VME_BUS)		+= vme/
 obj-$(CONFIG_IIO)		+= iio/
 obj-$(CONFIG_FB_SM7XX)		+= sm7xxfb/
+obj-$(CONFIG_FB_SM7XX)		+= sm750fb/
 obj-$(CONFIG_FB_XGI)		+= xgifb/
 obj-$(CONFIG_USB_EMXX)		+= emxx_udc/
 obj-$(CONFIG_FT1000)		+= ft1000/
@@ -46,3 +47,4 @@ obj-$(CONFIG_UNISYSSPAR)	+= unisys/
 obj-$(CONFIG_COMMON_CLK_XLNX_CLKWZRD)	+= clocking-wizard/
 obj-$(CONFIG_FB_TFT)		+= fbtft/
 obj-$(CONFIG_I2O)		+= i2o/
+obj-$(CONFIG_FSL_MC_BUS)	+= fsl-mc/
diff --git a/drivers/staging/android/ion/ion.c b/drivers/staging/android/ion/ion.c
index b8f1c491553e..0e3d8c7add24 100644
--- a/drivers/staging/android/ion/ion.c
+++ b/drivers/staging/android/ion/ion.c
@@ -566,8 +566,8 @@ int ion_phys(struct ion_client *client, struct ion_handle *handle,
 	buffer = handle->buffer;
 
 	if (!buffer->heap->ops->phys) {
-		pr_err("%s: ion_phys is not implemented by this heap.\n",
-		       __func__);
+		pr_err("%s: ion_phys is not implemented by this heap (name=%s, type=%d).\n",
+			__func__, buffer->heap->name, buffer->heap->type);
 		mutex_unlock(&client->lock);
 		return -ENODEV;
 	}
@@ -1395,7 +1395,7 @@ static int ion_debug_heap_show(struct seq_file *s, void *unused)
 	size_t total_size = 0;
 	size_t total_orphaned_size = 0;
 
-	seq_printf(s, "%16.s %16.s %16.s\n", "client", "pid", "size");
+	seq_printf(s, "%16s %16s %16s\n", "client", "pid", "size");
 	seq_puts(s, "----------------------------------------------------\n");
 
 	for (n = rb_first(&dev->clients); n; n = rb_next(n)) {
@@ -1409,10 +1409,10 @@ static int ion_debug_heap_show(struct seq_file *s, void *unused)
 			char task_comm[TASK_COMM_LEN];
 
 			get_task_comm(task_comm, client->task);
-			seq_printf(s, "%16.s %16u %16zu\n", task_comm,
+			seq_printf(s, "%16s %16u %16zu\n", task_comm,
 				   client->pid, size);
 		} else {
-			seq_printf(s, "%16.s %16u %16zu\n", client->name,
+			seq_printf(s, "%16s %16u %16zu\n", client->name,
 				   client->pid, size);
 		}
 	}
@@ -1426,7 +1426,7 @@ static int ion_debug_heap_show(struct seq_file *s, void *unused)
 			continue;
 		total_size += buffer->size;
 		if (!buffer->handle_count) {
-			seq_printf(s, "%16.s %16u %16zu %d %d\n",
+			seq_printf(s, "%16s %16u %16zu %d %d\n",
 				   buffer->task_comm, buffer->pid,
 				   buffer->size, buffer->kmap_cnt,
 				   atomic_read(&buffer->ref.refcount));
@@ -1435,11 +1435,11 @@ static int ion_debug_heap_show(struct seq_file *s, void *unused)
 	}
 	mutex_unlock(&dev->buffer_lock);
 	seq_puts(s, "----------------------------------------------------\n");
-	seq_printf(s, "%16.s %16zu\n", "total orphaned",
+	seq_printf(s, "%16s %16zu\n", "total orphaned",
 		   total_orphaned_size);
-	seq_printf(s, "%16.s %16zu\n", "total ", total_size);
+	seq_printf(s, "%16s %16zu\n", "total ", total_size);
 	if (heap->flags & ION_HEAP_FLAG_DEFER_FREE)
-		seq_printf(s, "%16.s %16zu\n", "deferred free",
+		seq_printf(s, "%16s %16zu\n", "deferred free",
 				heap->free_list_size);
 	seq_puts(s, "----------------------------------------------------\n");
 
diff --git a/drivers/staging/android/ion/ion_test.c b/drivers/staging/android/ion/ion_test.c
index 654acb5c8eba..3bc461cbbfa3 100644
--- a/drivers/staging/android/ion/ion_test.c
+++ b/drivers/staging/android/ion/ion_test.c
@@ -280,3 +280,4 @@ static void __exit ion_test_exit(void)
 
 module_init(ion_test_init);
 module_exit(ion_test_exit);
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/staging/android/sync.c b/drivers/staging/android/sync.c
index 7bdb62bf6b40..f83e00c78051 100644
--- a/drivers/staging/android/sync.c
+++ b/drivers/staging/android/sync.c
@@ -114,7 +114,7 @@ void sync_timeline_signal(struct sync_timeline *obj)
 	list_for_each_entry_safe(pt, next, &obj->active_list_head,
 				 active_list) {
 		if (fence_is_signaled_locked(&pt->base))
-			list_del(&pt->active_list);
+			list_del_init(&pt->active_list);
 	}
 
 	spin_unlock_irqrestore(&obj->child_list_lock, flags);
diff --git a/drivers/staging/comedi/Kconfig b/drivers/staging/comedi/Kconfig
index 593fcb1783b4..61c6351f55ac 100644
--- a/drivers/staging/comedi/Kconfig
+++ b/drivers/staging/comedi/Kconfig
@@ -108,6 +108,7 @@ if COMEDI_ISA_DRIVERS
 
 config COMEDI_PCL711
 	tristate "Advantech PCL-711/711b and ADlink ACL-8112 ISA card support"
+	select COMEDI_8254
 	---help---
 	  Enable support for Advantech PCL-711 and 711b, ADlink ACL-8112
 
@@ -169,6 +170,7 @@ config COMEDI_PCL730
 config COMEDI_PCL812
 	tristate "Advantech PCL-812/813 and ADlink ACL-8112/8113/8113/8216"
 	select COMEDI_ISADMA if ISA_DMA_API
+	select COMEDI_8254
 	---help---
 	  Enable support for Advantech PCL-812/PG, PCL-813/B, ADLink
 	  ACL-8112DG/HG/PG, ACL-8113, ACL-8216, ICP DAS A-821PGH/PGL/PGL-NDA,
@@ -180,6 +182,7 @@ config COMEDI_PCL812
 config COMEDI_PCL816
 	tristate "Advantech PCL-814 and PCL-816 ISA card support"
 	select COMEDI_ISADMA if ISA_DMA_API
+	select COMEDI_8254
 	---help---
 	  Enable support for Advantech PCL-814 and PCL-816 ISA cards
 
@@ -189,6 +192,7 @@ config COMEDI_PCL816
 config COMEDI_PCL818
 	tristate "Advantech PCL-718 and PCL-818 ISA card support"
 	select COMEDI_ISADMA if ISA_DMA_API
+	select COMEDI_8254
 	---help---
 	  Enable support for Advantech PCL-818 ISA cards
 	  PCL-818L, PCL-818H, PCL-818HD, PCL-818HG, PCL-818 and PCL-718
@@ -259,6 +263,7 @@ config COMEDI_DAC02
 
 config COMEDI_DAS16M1
 	tristate "MeasurementComputing CIO-DAS16/M1DAS-16 ISA card support"
+	select COMEDI_8254
 	select COMEDI_8255
 	---help---
 	  Enable support for Measurement Computing CIO-DAS16/M1 ISA cards.
@@ -282,6 +287,7 @@ config COMEDI_DAS08_ISA
 config COMEDI_DAS16
 	tristate "DAS-16 compatible ISA and PC/104 card support"
 	select COMEDI_ISADMA if ISA_DMA_API
+	select COMEDI_8254
 	select COMEDI_8255
 	---help---
 	  Enable support for Keithley Metrabyte/ComputerBoards DAS16
@@ -298,6 +304,7 @@ config COMEDI_DAS16
 
 config COMEDI_DAS800
 	tristate "DAS800 and compatible ISA card support"
+	select COMEDI_8254
 	---help---
 	  Enable support for Keithley Metrabyte DAS800 and compatible ISA cards
 	  Keithley Metrabyte DAS-800, DAS-801, DAS-802
@@ -310,6 +317,7 @@ config COMEDI_DAS800
 config COMEDI_DAS1800
 	tristate "DAS1800 and compatible ISA card support"
 	select COMEDI_ISADMA if ISA_DMA_API
+	select COMEDI_8254
 	---help---
 	  Enable support for DAS1800 and compatible ISA cards
 	  Keithley Metrabyte DAS-1701ST, DAS-1701ST-DA, DAS-1701/AO,
@@ -323,6 +331,7 @@ config COMEDI_DAS1800
 
 config COMEDI_DAS6402
 	tristate "DAS6402 and compatible ISA card support"
+	select COMEDI_8254
 	---help---
 	  Enable support for DAS6402 and compatible ISA cards
 	  Computerboards, Keithley Metrabyte DAS6402 and compatibles
@@ -463,6 +472,7 @@ config COMEDI_ADQ12B
 config COMEDI_NI_AT_A2150
 	tristate "NI AT-A2150 ISA card support"
 	select COMEDI_ISADMA if ISA_DMA_API
+	select COMEDI_8254
 	---help---
 	  Enable support for National Instruments AT-A2150 cards
 
@@ -471,6 +481,7 @@ config COMEDI_NI_AT_A2150
 
 config COMEDI_NI_AT_AO
 	tristate "NI AT-AO-6/10 EISA card support"
+	select COMEDI_8254
 	---help---
 	  Enable support for National Instruments AT-AO-6/10 cards
 
@@ -715,6 +726,7 @@ config COMEDI_ADL_PCI8164
 
 config COMEDI_ADL_PCI9111
 	tristate "ADLink PCI-9111HR support"
+	select COMEDI_8254
 	---help---
 	  Enable support for ADlink PCI9111 cards
 
@@ -724,6 +736,7 @@ config COMEDI_ADL_PCI9111
 config COMEDI_ADL_PCI9118
 	tristate "ADLink PCI-9118DG, PCI-9118HG, PCI-9118HR support"
 	depends on HAS_DMA
+	select COMEDI_8254
 	---help---
 	  Enable support for ADlink PCI-9118DG, PCI-9118HG, PCI-9118HR cards
 
@@ -732,6 +745,7 @@ config COMEDI_ADL_PCI9118
 
 config COMEDI_ADV_PCI1710
 	tristate "Advantech PCI-171x, PCI-1720 and PCI-1731 support"
+	select COMEDI_8254
 	---help---
 	  Enable support for Advantech PCI-1710, PCI-1710HG, PCI-1711,
 	  PCI-1713, PCI-1720 and PCI-1731
@@ -759,6 +773,7 @@ config COMEDI_ADV_PCI1724
 
 config COMEDI_ADV_PCI_DIO
 	tristate "Advantech PCI DIO card support"
+	select COMEDI_8254
 	select COMEDI_8255
 	---help---
 	  Enable support for Advantech PCI DIO cards
@@ -799,6 +814,7 @@ config COMEDI_AMPLC_PC263_PCI
 
 config COMEDI_AMPLC_PCI224
 	tristate "Amplicon PCI224 and PCI234 support"
+	select COMEDI_8254
 	---help---
 	  Enable support for Amplicon PCI224 and PCI234 AO boards
 
@@ -807,6 +823,7 @@ config COMEDI_AMPLC_PCI224
 
 config COMEDI_AMPLC_PCI230
 	tristate "Amplicon PCI230 and PCI260 support"
+	select COMEDI_8254
 	select COMEDI_8255
 	---help---
 	  Enable support for Amplicon PCI230 and PCI260 Multifunction I/O
@@ -912,6 +929,7 @@ config COMEDI_CB_PCIDAS64
 
 config COMEDI_CB_PCIDAS
 	tristate "MeasurementComputing PCI-DAS support"
+	select COMEDI_8254
 	select COMEDI_8255
 	---help---
 	  Enable support for ComputerBoards/MeasurementComputing PCI-DAS with
@@ -935,6 +953,7 @@ config COMEDI_CB_PCIDDA
 
 config COMEDI_CB_PCIMDAS
 	tristate "MeasurementComputing PCIM-DAS1602/16, PCIe-DAS1602/16 support"
+	select COMEDI_8254
 	select COMEDI_8255
 	---help---
 	  Enable support for ComputerBoards/MeasurementComputing PCI Migration
@@ -954,6 +973,7 @@ config COMEDI_CB_PCIMDDA
 
 config COMEDI_ME4000
 	tristate "Meilhaus ME-4000 support"
+	select COMEDI_8254
 	---help---
 	  Enable support for Meilhaus PCI data acquisition cards
 	  ME-4650, ME-4670i, ME-4680, ME-4680i and ME-4680is
@@ -1091,6 +1111,7 @@ if COMEDI_PCMCIA_DRIVERS
 
 config COMEDI_CB_DAS16_CS
 	tristate "CB DAS16 series PCMCIA support"
+	select COMEDI_8254
 	---help---
 	  Enable support for the ComputerBoards/MeasurementComputing PCMCIA
 	  cards DAS16/16, PCM-DAS16D/12 and PCM-DAS16s/16
@@ -1222,6 +1243,9 @@ config COMEDI_VMK80XX
 
 endif # COMEDI_USB_DRIVERS
 
+config COMEDI_8254
+	tristate
+
 config COMEDI_8255
 	tristate "Generic 8255 support"
 	---help---
@@ -1252,6 +1276,7 @@ config COMEDI_KCOMEDILIB
 	  called kcomedilib.
 
 config COMEDI_AMPLC_DIO200
+	select COMEDI_8254
 	tristate
 
 config COMEDI_AMPLC_PC236
@@ -1260,6 +1285,7 @@ config COMEDI_AMPLC_PC236
 
 config COMEDI_DAS08
 	tristate
+	select COMEDI_8254
 	select COMEDI_8255
 
 config COMEDI_ISADMA
@@ -1267,6 +1293,7 @@ config COMEDI_ISADMA
 
 config COMEDI_NI_LABPC
 	tristate
+	select COMEDI_8254
 	select COMEDI_8255
 
 config COMEDI_NI_LABPC_ISADMA
diff --git a/drivers/staging/comedi/comedi_fops.c b/drivers/staging/comedi/comedi_fops.c
index 727640e89c73..e78ddbe5a954 100644
--- a/drivers/staging/comedi/comedi_fops.c
+++ b/drivers/staging/comedi/comedi_fops.c
@@ -144,7 +144,7 @@ static void comedi_device_cleanup(struct comedi_device *dev)
 {
 	struct module *driver_module = NULL;
 
-	if (dev == NULL)
+	if (!dev)
 		return;
 	mutex_lock(&dev->mutex);
 	if (dev->attached)
@@ -260,7 +260,7 @@ comedi_read_subdevice(const struct comedi_device *dev, unsigned int minor)
 
 	if (minor >= COMEDI_NUM_BOARD_MINORS) {
 		s = comedi_subdevice_from_minor(dev, minor);
-		if (s == NULL || (s->subdev_flags & SDF_CMD_READ))
+		if (!s || (s->subdev_flags & SDF_CMD_READ))
 			return s;
 	}
 	return dev->read_subdev;
@@ -273,7 +273,7 @@ comedi_write_subdevice(const struct comedi_device *dev, unsigned int minor)
 
 	if (minor >= COMEDI_NUM_BOARD_MINORS) {
 		s = comedi_subdevice_from_minor(dev, minor);
-		if (s == NULL || (s->subdev_flags & SDF_CMD_WRITE))
+		if (!s || (s->subdev_flags & SDF_CMD_WRITE))
 			return s;
 	}
 	return dev->write_subdev;
@@ -290,9 +290,9 @@ static void comedi_file_reset(struct file *file)
 	write_s = dev->write_subdev;
 	if (minor >= COMEDI_NUM_BOARD_MINORS) {
 		s = comedi_subdevice_from_minor(dev, minor);
-		if (s == NULL || s->subdev_flags & SDF_CMD_READ)
+		if (!s || s->subdev_flags & SDF_CMD_READ)
 			read_s = s;
-		if (s == NULL || s->subdev_flags & SDF_CMD_WRITE)
+		if (!s || s->subdev_flags & SDF_CMD_WRITE)
 			write_s = s;
 	}
 	cfp->last_attached = dev->attached;
@@ -601,28 +601,55 @@ static struct attribute *comedi_dev_attrs[] = {
 };
 ATTRIBUTE_GROUPS(comedi_dev);
 
-static void comedi_set_subdevice_runflags(struct comedi_subdevice *s,
-					  unsigned mask, unsigned bits)
+static void __comedi_clear_subdevice_runflags(struct comedi_subdevice *s,
+					      unsigned bits)
+{
+	s->runflags &= ~bits;
+}
+
+static void __comedi_set_subdevice_runflags(struct comedi_subdevice *s,
+					    unsigned bits)
+{
+	s->runflags |= bits;
+}
+
+static void comedi_update_subdevice_runflags(struct comedi_subdevice *s,
+					     unsigned mask, unsigned bits)
 {
 	unsigned long flags;
 
 	spin_lock_irqsave(&s->spin_lock, flags);
-	s->runflags &= ~mask;
-	s->runflags |= (bits & mask);
+	__comedi_clear_subdevice_runflags(s, mask);
+	__comedi_set_subdevice_runflags(s, bits & mask);
 	spin_unlock_irqrestore(&s->spin_lock, flags);
 }
 
+static unsigned __comedi_get_subdevice_runflags(struct comedi_subdevice *s)
+{
+	return s->runflags;
+}
+
 static unsigned comedi_get_subdevice_runflags(struct comedi_subdevice *s)
 {
 	unsigned long flags;
 	unsigned runflags;
 
 	spin_lock_irqsave(&s->spin_lock, flags);
-	runflags = s->runflags;
+	runflags = __comedi_get_subdevice_runflags(s);
 	spin_unlock_irqrestore(&s->spin_lock, flags);
 	return runflags;
 }
 
+static bool comedi_is_runflags_running(unsigned runflags)
+{
+	return runflags & COMEDI_SRF_RUNNING;
+}
+
+static bool comedi_is_runflags_in_error(unsigned runflags)
+{
+	return runflags & COMEDI_SRF_ERROR;
+}
+
 /**
  * comedi_is_subdevice_running - check if async command running on subdevice
  * @s: comedi_subdevice struct
@@ -634,22 +661,22 @@ bool comedi_is_subdevice_running(struct comedi_subdevice *s)
 {
 	unsigned runflags = comedi_get_subdevice_runflags(s);
 
-	return (runflags & COMEDI_SRF_RUNNING) ? true : false;
+	return comedi_is_runflags_running(runflags);
 }
 EXPORT_SYMBOL_GPL(comedi_is_subdevice_running);
 
-static bool comedi_is_subdevice_in_error(struct comedi_subdevice *s)
+static bool __comedi_is_subdevice_running(struct comedi_subdevice *s)
 {
-	unsigned runflags = comedi_get_subdevice_runflags(s);
+	unsigned runflags = __comedi_get_subdevice_runflags(s);
 
-	return (runflags & COMEDI_SRF_ERROR) ? true : false;
+	return comedi_is_runflags_running(runflags);
 }
 
 static bool comedi_is_subdevice_idle(struct comedi_subdevice *s)
 {
 	unsigned runflags = comedi_get_subdevice_runflags(s);
 
-	return (runflags & COMEDI_SRF_BUSY_MASK) ? false : true;
+	return !(runflags & COMEDI_SRF_BUSY_MASK);
 }
 
 /**
@@ -677,14 +704,14 @@ static void do_become_nonbusy(struct comedi_device *dev,
 {
 	struct comedi_async *async = s->async;
 
-	comedi_set_subdevice_runflags(s, COMEDI_SRF_RUNNING, 0);
+	comedi_update_subdevice_runflags(s, COMEDI_SRF_RUNNING, 0);
 	if (async) {
 		comedi_buf_reset(s);
 		async->inttrig = NULL;
 		kfree(async->cmd.chanlist);
 		async->cmd.chanlist = NULL;
 		s->busy = NULL;
-		wake_up_interruptible_all(&s->async->wait_head);
+		wake_up_interruptible_all(&async->wait_head);
 	} else {
 		dev_err(dev->class_dev,
 			"BUG: (?) do_become_nonbusy called with async=NULL\n");
@@ -759,7 +786,7 @@ static int do_devconfig_ioctl(struct comedi_device *dev,
 	if (!capable(CAP_SYS_ADMIN))
 		return -EPERM;
 
-	if (arg == NULL) {
+	if (!arg) {
 		if (is_device_busy(dev))
 			return -EBUSY;
 		if (dev->attached) {
@@ -1678,8 +1705,8 @@ static int do_cmd_ioctl(struct comedi_device *dev,
 	if (async->cmd.flags & CMDF_WAKE_EOS)
 		async->cb_mask |= COMEDI_CB_EOS;
 
-	comedi_set_subdevice_runflags(s, COMEDI_SRF_BUSY_MASK,
-				      COMEDI_SRF_RUNNING);
+	comedi_update_subdevice_runflags(s, COMEDI_SRF_BUSY_MASK,
+					 COMEDI_SRF_RUNNING);
 
 	/*
 	 * Set s->busy _after_ setting COMEDI_SRF_RUNNING flag to avoid
@@ -1840,7 +1867,7 @@ static int do_cancel_ioctl(struct comedi_device *dev, unsigned long arg,
 	if (arg >= dev->n_subdevices)
 		return -EINVAL;
 	s = &dev->subdevices[arg];
-	if (s->async == NULL)
+	if (!s->async)
 		return -EINVAL;
 
 	if (!s->busy)
@@ -2282,13 +2309,16 @@ static ssize_t comedi_write(struct file *file, const char __user *buf,
 	add_wait_queue(&async->wait_head, &wait);
 	on_wait_queue = true;
 	while (nbytes > 0 && !retval) {
+		unsigned runflags;
+
 		set_current_state(TASK_INTERRUPTIBLE);
 
-		if (!comedi_is_subdevice_running(s)) {
+		runflags = comedi_get_subdevice_runflags(s);
+		if (!comedi_is_runflags_running(runflags)) {
 			if (count == 0) {
 				struct comedi_subdevice *new_s;
 
-				if (comedi_is_subdevice_in_error(s))
+				if (comedi_is_runflags_in_error(runflags))
 					retval = -EPIPE;
 				else
 					retval = 0;
@@ -2435,8 +2465,10 @@ static ssize_t comedi_read(struct file *file, char __user *buf, size_t nbytes,
 			n = m;
 
 		if (n == 0) {
-			if (!comedi_is_subdevice_running(s)) {
-				if (comedi_is_subdevice_in_error(s))
+			unsigned runflags = comedi_get_subdevice_runflags(s);
+
+			if (!comedi_is_runflags_running(runflags)) {
+				if (comedi_is_runflags_in_error(runflags))
 					retval = -EPIPE;
 				else
 					retval = 0;
@@ -2638,39 +2670,38 @@ static const struct file_operations comedi_fops = {
 void comedi_event(struct comedi_device *dev, struct comedi_subdevice *s)
 {
 	struct comedi_async *async = s->async;
-	unsigned runflags = 0;
-	unsigned runflags_mask = 0;
+	unsigned int events;
+	int si_code = 0;
+	unsigned long flags;
 
-	if (!comedi_is_subdevice_running(s))
+	spin_lock_irqsave(&s->spin_lock, flags);
+
+	events = async->events;
+	async->events = 0;
+	if (!__comedi_is_subdevice_running(s)) {
+		spin_unlock_irqrestore(&s->spin_lock, flags);
 		return;
+	}
 
-	if (s->async->events & COMEDI_CB_CANCEL_MASK)
-		runflags_mask |= COMEDI_SRF_RUNNING;
+	if (events & COMEDI_CB_CANCEL_MASK)
+		__comedi_clear_subdevice_runflags(s, COMEDI_SRF_RUNNING);
 
 	/*
-	 * Remember if an error event has occurred, so an error
-	 * can be returned the next time the user does a read().
+	 * Remember if an error event has occurred, so an error can be
+	 * returned the next time the user does a read() or write().
 	 */
-	if (s->async->events & COMEDI_CB_ERROR_MASK) {
-		runflags_mask |= COMEDI_SRF_ERROR;
-		runflags |= COMEDI_SRF_ERROR;
-	}
-	if (runflags_mask) {
-		/*
-		 * Sets COMEDI_SRF_ERROR and COMEDI_SRF_RUNNING together
-		 * atomically.
-		 */
-		comedi_set_subdevice_runflags(s, runflags_mask, runflags);
-	}
+	if (events & COMEDI_CB_ERROR_MASK)
+		__comedi_set_subdevice_runflags(s, COMEDI_SRF_ERROR);
 
-	if (async->cb_mask & s->async->events) {
+	if (async->cb_mask & events) {
 		wake_up_interruptible(&async->wait_head);
-		if (s->subdev_flags & SDF_CMD_READ)
-			kill_fasync(&dev->async_queue, SIGIO, POLL_IN);
-		if (s->subdev_flags & SDF_CMD_WRITE)
-			kill_fasync(&dev->async_queue, SIGIO, POLL_OUT);
+		si_code = async->cmd.flags & CMDF_WRITE ? POLL_OUT : POLL_IN;
 	}
-	s->async->events = 0;
+
+	spin_unlock_irqrestore(&s->spin_lock, flags);
+
+	if (si_code)
+		kill_fasync(&dev->async_queue, SIGIO, si_code);
 }
 EXPORT_SYMBOL_GPL(comedi_event);
 
@@ -2682,7 +2713,7 @@ struct comedi_device *comedi_alloc_board_minor(struct device *hardware_device)
 	unsigned i;
 
 	dev = kzalloc(sizeof(*dev), GFP_KERNEL);
-	if (dev == NULL)
+	if (!dev)
 		return ERR_PTR(-ENOMEM);
 	comedi_device_init(dev);
 	comedi_set_hw_dev(dev, hardware_device);
@@ -2690,7 +2721,7 @@ struct comedi_device *comedi_alloc_board_minor(struct device *hardware_device)
 	mutex_lock(&comedi_board_minor_table_lock);
 	for (i = hardware_device ? comedi_num_legacy_minors : 0;
 	     i < COMEDI_NUM_BOARD_MINORS; ++i) {
-		if (comedi_board_minor_table[i] == NULL) {
+		if (!comedi_board_minor_table[i]) {
 			comedi_board_minor_table[i] = dev;
 			break;
 		}
@@ -2700,7 +2731,8 @@ struct comedi_device *comedi_alloc_board_minor(struct device *hardware_device)
 		mutex_unlock(&dev->mutex);
 		comedi_device_cleanup(dev);
 		comedi_dev_put(dev);
-		pr_err("ran out of minor numbers for board device files\n");
+		dev_err(hardware_device,
+			"ran out of minor numbers for board device files\n");
 		return ERR_PTR(-EBUSY);
 	}
 	dev->minor = i;
@@ -2746,14 +2778,15 @@ int comedi_alloc_subdevice_minor(struct comedi_subdevice *s)
 
 	mutex_lock(&comedi_subdevice_minor_table_lock);
 	for (i = 0; i < COMEDI_NUM_SUBDEVICE_MINORS; ++i) {
-		if (comedi_subdevice_minor_table[i] == NULL) {
+		if (!comedi_subdevice_minor_table[i]) {
 			comedi_subdevice_minor_table[i] = s;
 			break;
 		}
 	}
 	mutex_unlock(&comedi_subdevice_minor_table_lock);
 	if (i == COMEDI_NUM_SUBDEVICE_MINORS) {
-		pr_err("ran out of minor numbers for subdevice files\n");
+		dev_err(dev->class_dev,
+			"ran out of minor numbers for subdevice files\n");
 		return -EBUSY;
 	}
 	i += COMEDI_NUM_BOARD_MINORS;
@@ -2771,7 +2804,7 @@ void comedi_free_subdevice_minor(struct comedi_subdevice *s)
 {
 	unsigned int i;
 
-	if (s == NULL)
+	if (!s)
 		return;
 	if (s->minor < 0)
 		return;
diff --git a/drivers/staging/comedi/comedi_internal.h b/drivers/staging/comedi/comedi_internal.h
index d57817c19aea..3b918538847e 100644
--- a/drivers/staging/comedi/comedi_internal.h
+++ b/drivers/staging/comedi/comedi_internal.h
@@ -1,11 +1,21 @@
 #ifndef _COMEDI_INTERNAL_H
 #define _COMEDI_INTERNAL_H
 
+#include <linux/compiler.h>
 #include <linux/types.h>
 
 /*
  * various internal comedi stuff
  */
+
+struct comedi_buf_map;
+struct comedi_devconfig;
+struct comedi_device;
+struct comedi_insn;
+struct comedi_rangeinfo;
+struct comedi_subdevice;
+struct device;
+
 int do_rangeinfo_ioctl(struct comedi_device *dev,
 		       struct comedi_rangeinfo __user *arg);
 struct comedi_device *comedi_alloc_board_minor(struct device *hardware_device);
diff --git a/drivers/staging/comedi/comedi_pci.c b/drivers/staging/comedi/comedi_pci.c
index 6ba59c977006..027f0f4e59c1 100644
--- a/drivers/staging/comedi/comedi_pci.c
+++ b/drivers/staging/comedi/comedi_pci.c
@@ -17,10 +17,9 @@
  */
 
 #include <linux/module.h>
-#include <linux/pci.h>
 #include <linux/interrupt.h>
 
-#include "comedidev.h"
+#include "comedi_pci.h"
 
 /**
  * comedi_to_pci_dev() - comedi_device pointer to pci_dev pointer.
diff --git a/drivers/staging/comedi/comedi_pci.h b/drivers/staging/comedi/comedi_pci.h
new file mode 100644
index 000000000000..4005cc9cf7f1
--- /dev/null
+++ b/drivers/staging/comedi/comedi_pci.h
@@ -0,0 +1,64 @@
+/*
+ * comedi_pci.h
+ * header file for Comedi PCI drivers
+ *
+ * COMEDI - Linux Control and Measurement Device Interface
+ * Copyright (C) 1997-2000 David A. Schleef <ds@schleef.org>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#ifndef _COMEDI_PCI_H
+#define _COMEDI_PCI_H
+
+#include <linux/pci.h>
+
+#include "comedidev.h"
+
+/*
+ * PCI Vendor IDs not in <linux/pci_ids.h>
+ */
+#define PCI_VENDOR_ID_KOLTER		0x1001
+#define PCI_VENDOR_ID_ICP		0x104c
+#define PCI_VENDOR_ID_DT		0x1116
+#define PCI_VENDOR_ID_IOTECH		0x1616
+#define PCI_VENDOR_ID_CONTEC		0x1221
+#define PCI_VENDOR_ID_RTD		0x1435
+#define PCI_VENDOR_ID_HUMUSOFT		0x186c
+
+struct pci_dev *comedi_to_pci_dev(struct comedi_device *);
+
+int comedi_pci_enable(struct comedi_device *);
+void comedi_pci_disable(struct comedi_device *);
+void comedi_pci_detach(struct comedi_device *);
+
+int comedi_pci_auto_config(struct pci_dev *, struct comedi_driver *,
+			   unsigned long context);
+void comedi_pci_auto_unconfig(struct pci_dev *);
+
+int comedi_pci_driver_register(struct comedi_driver *, struct pci_driver *);
+void comedi_pci_driver_unregister(struct comedi_driver *, struct pci_driver *);
+
+/**
+ * module_comedi_pci_driver() - Helper macro for registering a comedi PCI driver
+ * @__comedi_driver: comedi_driver struct
+ * @__pci_driver: pci_driver struct
+ *
+ * Helper macro for comedi PCI drivers which do not do anything special
+ * in module init/exit. This eliminates a lot of boilerplate. Each
+ * module may only use this macro once, and calling it replaces
+ * module_init() and module_exit()
+ */
+#define module_comedi_pci_driver(__comedi_driver, __pci_driver) \
+	module_driver(__comedi_driver, comedi_pci_driver_register, \
+			comedi_pci_driver_unregister, &(__pci_driver))
+
+#endif /* _COMEDI_PCI_H */
diff --git a/drivers/staging/comedi/comedidev.h b/drivers/staging/comedi/comedidev.h
index e138eb0dc374..dfab5a84b011 100644
--- a/drivers/staging/comedi/comedidev.h
+++ b/drivers/staging/comedi/comedidev.h
@@ -256,6 +256,7 @@ struct comedi_driver {
 struct comedi_device {
 	int use_count;
 	struct comedi_driver *driver;
+	struct comedi_8254 *pacer;
 	void *private;
 
 	struct device *class_dev;
@@ -463,6 +464,84 @@ static inline unsigned int comedi_samples_to_bytes(struct comedi_subdevice *s,
 	return nsamples << comedi_sample_shift(s);
 }
 
+/**
+ * comedi_check_trigger_src() - trivially validate a comedi_cmd trigger source
+ * @src: pointer to the trigger source to validate
+ * @flags: bitmask of valid TRIG_* for the trigger
+ *
+ * This is used in "step 1" of the do_cmdtest functions of comedi drivers
+ * to vaildate the comedi_cmd triggers. The mask of the @src against the
+ * @flags allows the userspace comedilib to pass all the comedi_cmd
+ * triggers as TRIG_ANY and get back a bitmask of the valid trigger sources.
+ */
+static inline int comedi_check_trigger_src(unsigned int *src,
+					   unsigned int flags)
+{
+	unsigned int orig_src = *src;
+
+	*src = orig_src & flags;
+	if (*src == TRIG_INVALID || *src != orig_src)
+		return -EINVAL;
+	return 0;
+}
+
+/**
+ * comedi_check_trigger_is_unique() - make sure a trigger source is unique
+ * @src: the trigger source to check
+ */
+static inline int comedi_check_trigger_is_unique(unsigned int src)
+{
+	/* this test is true if more than one _src bit is set */
+	if ((src & (src - 1)) != 0)
+		return -EINVAL;
+	return 0;
+}
+
+/**
+ * comedi_check_trigger_arg_is() - trivially validate a trigger argument
+ * @arg: pointer to the trigger arg to validate
+ * @val: the value the argument should be
+ */
+static inline int comedi_check_trigger_arg_is(unsigned int *arg,
+					      unsigned int val)
+{
+	if (*arg != val) {
+		*arg = val;
+		return -EINVAL;
+	}
+	return 0;
+}
+
+/**
+ * comedi_check_trigger_arg_min() - trivially validate a trigger argument
+ * @arg: pointer to the trigger arg to validate
+ * @val: the minimum value the argument should be
+ */
+static inline int comedi_check_trigger_arg_min(unsigned int *arg,
+					       unsigned int val)
+{
+	if (*arg < val) {
+		*arg = val;
+		return -EINVAL;
+	}
+	return 0;
+}
+
+/**
+ * comedi_check_trigger_arg_max() - trivially validate a trigger argument
+ * @arg: pointer to the trigger arg to validate
+ * @val: the maximum value the argument should be
+ */
+static inline int comedi_check_trigger_arg_max(unsigned int *arg,
+					       unsigned int val)
+{
+	if (*arg > val) {
+		*arg = val;
+		return -EINVAL;
+	}
+	return 0;
+}
+
 /*
  * Must set dev->hw_dev if you wish to dma directly into comedi's buffer.
  * Also useful for retrieving a previously configured hardware device of
@@ -553,47 +632,4 @@ void comedi_driver_unregister(struct comedi_driver *);
 	module_driver(__comedi_driver, comedi_driver_register, \
 			comedi_driver_unregister)
 
-/* comedi_pci.c - comedi PCI driver specific functions */
-
-/*
- * PCI Vendor IDs not in <linux/pci_ids.h>
- */
-#define PCI_VENDOR_ID_KOLTER		0x1001
-#define PCI_VENDOR_ID_ICP		0x104c
-#define PCI_VENDOR_ID_DT		0x1116
-#define PCI_VENDOR_ID_IOTECH		0x1616
-#define PCI_VENDOR_ID_CONTEC		0x1221
-#define PCI_VENDOR_ID_RTD		0x1435
-#define PCI_VENDOR_ID_HUMUSOFT		0x186c
-
-struct pci_dev;
-struct pci_driver;
-
-struct pci_dev *comedi_to_pci_dev(struct comedi_device *);
-
-int comedi_pci_enable(struct comedi_device *);
-void comedi_pci_disable(struct comedi_device *);
-void comedi_pci_detach(struct comedi_device *);
-
-int comedi_pci_auto_config(struct pci_dev *, struct comedi_driver *,
-			   unsigned long context);
-void comedi_pci_auto_unconfig(struct pci_dev *);
-
-int comedi_pci_driver_register(struct comedi_driver *, struct pci_driver *);
-void comedi_pci_driver_unregister(struct comedi_driver *, struct pci_driver *);
-
-/**
- * module_comedi_pci_driver() - Helper macro for registering a comedi PCI driver
- * @__comedi_driver: comedi_driver struct
- * @__pci_driver: pci_driver struct
- *
- * Helper macro for comedi PCI drivers which do not do anything special
- * in module init/exit. This eliminates a lot of boilerplate. Each
- * module may only use this macro once, and calling it replaces
- * module_init() and module_exit()
- */
-#define module_comedi_pci_driver(__comedi_driver, __pci_driver) \
-	module_driver(__comedi_driver, comedi_pci_driver_register, \
-			comedi_pci_driver_unregister, &(__pci_driver))
-
 #endif /* _COMEDIDEV_H */
diff --git a/drivers/staging/comedi/drivers.c b/drivers/staging/comedi/drivers.c
index f32e71438948..57dcffe00204 100644
--- a/drivers/staging/comedi/drivers.c
+++ b/drivers/staging/comedi/drivers.c
@@ -46,7 +46,7 @@ int comedi_set_hw_dev(struct comedi_device *dev, struct device *hw_dev)
 {
 	if (hw_dev == dev->hw_dev)
 		return 0;
-	if (dev->hw_dev != NULL)
+	if (dev->hw_dev)
 		return -EEXIST;
 	dev->hw_dev = get_device(hw_dev);
 	return 0;
@@ -139,7 +139,9 @@ static void comedi_device_detach_cleanup(struct comedi_device *dev)
 		dev->n_subdevices = 0;
 	}
 	kfree(dev->private);
+	kfree(dev->pacer);
 	dev->private = NULL;
+	dev->pacer = NULL;
 	dev->driver = NULL;
 	dev->board_name = NULL;
 	dev->board_ptr = NULL;
@@ -800,7 +802,7 @@ int comedi_device_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 		}
 		module_put(driv->module);
 	}
-	if (driv == NULL) {
+	if (!driv) {
 		/*  recognize has failed if we get here */
 		/*  report valid board names before returning error */
 		for (driv = comedi_drivers; driv; driv = driv->next) {
@@ -812,7 +814,7 @@ int comedi_device_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 		ret = -EIO;
 		goto out;
 	}
-	if (driv->attach == NULL) {
+	if (!driv->attach) {
 		/* driver does not support manual configuration */
 		dev_warn(dev->class_dev,
 			 "driver '%s' does not support attach using comedi_config\n",
@@ -896,7 +898,7 @@ EXPORT_SYMBOL_GPL(comedi_auto_config);
 
 void comedi_auto_unconfig(struct device *hardware_device)
 {
-	if (hardware_device == NULL)
+	if (!hardware_device)
 		return;
 	comedi_release_hardware_device(hardware_device);
 }
diff --git a/drivers/staging/comedi/drivers/8253.h b/drivers/staging/comedi/drivers/8253.h
deleted file mode 100644
index 51b9c8d279c0..000000000000
--- a/drivers/staging/comedi/drivers/8253.h
+++ /dev/null
@@ -1,347 +0,0 @@
-/*
- * comedi/drivers/8253.h
- * Header file for 8253
- *
- * COMEDI - Linux Control and Measurement Device Interface
- * Copyright (C) 2000 David A. Schleef <ds@schleef.org>
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- */
-
-#ifndef _8253_H
-#define _8253_H
-
-#include "../comedi.h"
-
-/*
- * Common oscillator base values in nanoseconds
- */
-#define I8254_OSC_BASE_10MHZ		100
-#define I8254_OSC_BASE_5MHZ		200
-#define I8254_OSC_BASE_4MHZ		250
-#define I8254_OSC_BASE_2MHZ		500
-#define I8254_OSC_BASE_1MHZ		1000
-
-static inline void i8253_cascade_ns_to_timer(int i8253_osc_base,
-					     unsigned int *d1,
-					     unsigned int *d2,
-					     unsigned int *nanosec,
-					     unsigned int flags)
-{
-	unsigned int divider;
-	unsigned int div1, div2;
-	unsigned int div1_glb, div2_glb, ns_glb;
-	unsigned int div1_lub, div2_lub, ns_lub;
-	unsigned int ns;
-	unsigned int start;
-	unsigned int ns_low, ns_high;
-	static const unsigned int max_count = 0x10000;
-	/*
-	 * exit early if everything is already correct (this can save time
-	 * since this function may be called repeatedly during command tests
-	 * and execution)
-	 */
-	div1 = *d1 ? *d1 : max_count;
-	div2 = *d2 ? *d2 : max_count;
-	divider = div1 * div2;
-	if (div1 * div2 * i8253_osc_base == *nanosec &&
-	    div1 > 1 && div1 <= max_count && div2 > 1 && div2 <= max_count &&
-	    /* check for overflow */
-	    divider > div1 && divider > div2 &&
-	    divider * i8253_osc_base > divider &&
-	    divider * i8253_osc_base > i8253_osc_base) {
-		return;
-	}
-
-	divider = *nanosec / i8253_osc_base;
-
-	div1_lub = div2_lub = 0;
-	div1_glb = div2_glb = 0;
-
-	ns_glb = 0;
-	ns_lub = 0xffffffff;
-
-	div2 = max_count;
-	start = divider / div2;
-	if (start < 2)
-		start = 2;
-	for (div1 = start; div1 <= divider / div1 + 1 && div1 <= max_count;
-	     div1++) {
-		for (div2 = divider / div1;
-		     div1 * div2 <= divider + div1 + 1 && div2 <= max_count;
-		     div2++) {
-			ns = i8253_osc_base * div1 * div2;
-			if (ns <= *nanosec && ns > ns_glb) {
-				ns_glb = ns;
-				div1_glb = div1;
-				div2_glb = div2;
-			}
-			if (ns >= *nanosec && ns < ns_lub) {
-				ns_lub = ns;
-				div1_lub = div1;
-				div2_lub = div2;
-			}
-		}
-	}
-
-	switch (flags & CMDF_ROUND_MASK) {
-	case CMDF_ROUND_NEAREST:
-	default:
-		ns_high = div1_lub * div2_lub * i8253_osc_base;
-		ns_low = div1_glb * div2_glb * i8253_osc_base;
-		if (ns_high - *nanosec < *nanosec - ns_low) {
-			div1 = div1_lub;
-			div2 = div2_lub;
-		} else {
-			div1 = div1_glb;
-			div2 = div2_glb;
-		}
-		break;
-	case CMDF_ROUND_UP:
-		div1 = div1_lub;
-		div2 = div2_lub;
-		break;
-	case CMDF_ROUND_DOWN:
-		div1 = div1_glb;
-		div2 = div2_glb;
-		break;
-	}
-
-	*nanosec = div1 * div2 * i8253_osc_base;
-	/* masking is done since counter maps zero to 0x10000 */
-	*d1 = div1 & 0xffff;
-	*d2 = div2 & 0xffff;
-}
-
-#ifndef CMDTEST
-/*
- * i8254_load programs 8254 counter chip.  It should also work for the 8253.
- * base_address is the lowest io address
- * for the chip (the address of counter 0).
- * counter_number is the counter you want to load (0,1 or 2)
- * count is the number to load into the counter.
- *
- * You probably want to use mode 2.
- *
- * Use i8254_mm_load() if you board uses memory-mapped io, it is
- * the same as i8254_load() except it uses writeb() instead of outb().
- *
- * Neither i8254_load() or i8254_read() do their loading/reading
- * atomically.  The 16 bit read/writes are performed with two successive
- * 8 bit read/writes.  So if two parts of your driver do a load/read on
- * the same counter, it may be necessary to protect these functions
- * with a spinlock.
- *
- * FMH
- */
-
-#define i8254_control_reg	3
-
-static inline int i8254_load(unsigned long base_address, unsigned int regshift,
-			     unsigned int counter_number, unsigned int count,
-			     unsigned int mode)
-{
-	unsigned int byte;
-
-	if (counter_number > 2)
-		return -1;
-	if (count > 0xffff)
-		return -1;
-	if (mode > 5)
-		return -1;
-	if ((mode == 2 || mode == 3) && count == 1)
-		return -1;
-
-	byte = counter_number << 6;
-	byte |= 0x30;		/* load low then high byte */
-	byte |= (mode << 1);	/* set counter mode */
-	outb(byte, base_address + (i8254_control_reg << regshift));
-	byte = count & 0xff;	/* lsb of counter value */
-	outb(byte, base_address + (counter_number << regshift));
-	byte = (count >> 8) & 0xff;	/* msb of counter value */
-	outb(byte, base_address + (counter_number << regshift));
-
-	return 0;
-}
-
-static inline int i8254_mm_load(void __iomem *base_address,
-				unsigned int regshift,
-				unsigned int counter_number,
-				unsigned int count,
-				unsigned int mode)
-{
-	unsigned int byte;
-
-	if (counter_number > 2)
-		return -1;
-	if (count > 0xffff)
-		return -1;
-	if (mode > 5)
-		return -1;
-	if ((mode == 2 || mode == 3) && count == 1)
-		return -1;
-
-	byte = counter_number << 6;
-	byte |= 0x30;		/* load low then high byte */
-	byte |= (mode << 1);	/* set counter mode */
-	writeb(byte, base_address + (i8254_control_reg << regshift));
-	byte = count & 0xff;	/* lsb of counter value */
-	writeb(byte, base_address + (counter_number << regshift));
-	byte = (count >> 8) & 0xff;	/* msb of counter value */
-	writeb(byte, base_address + (counter_number << regshift));
-
-	return 0;
-}
-
-/* Returns 16 bit counter value, should work for 8253 also. */
-static inline int i8254_read(unsigned long base_address, unsigned int regshift,
-			     unsigned int counter_number)
-{
-	unsigned int byte;
-	int ret;
-
-	if (counter_number > 2)
-		return -1;
-
-	/* latch counter */
-	byte = counter_number << 6;
-	outb(byte, base_address + (i8254_control_reg << regshift));
-
-	/* read lsb */
-	ret = inb(base_address + (counter_number << regshift));
-	/* read msb */
-	ret += inb(base_address + (counter_number << regshift)) << 8;
-
-	return ret;
-}
-
-static inline int i8254_mm_read(void __iomem *base_address,
-				unsigned int regshift,
-				unsigned int counter_number)
-{
-	unsigned int byte;
-	int ret;
-
-	if (counter_number > 2)
-		return -1;
-
-	/* latch counter */
-	byte = counter_number << 6;
-	writeb(byte, base_address + (i8254_control_reg << regshift));
-
-	/* read lsb */
-	ret = readb(base_address + (counter_number << regshift));
-	/* read msb */
-	ret += readb(base_address + (counter_number << regshift)) << 8;
-
-	return ret;
-}
-
-/* Loads 16 bit initial counter value, should work for 8253 also. */
-static inline void i8254_write(unsigned long base_address,
-			       unsigned int regshift,
-			       unsigned int counter_number, unsigned int count)
-{
-	unsigned int byte;
-
-	if (counter_number > 2)
-		return;
-
-	byte = count & 0xff;	/* lsb of counter value */
-	outb(byte, base_address + (counter_number << regshift));
-	byte = (count >> 8) & 0xff;	/* msb of counter value */
-	outb(byte, base_address + (counter_number << regshift));
-}
-
-static inline void i8254_mm_write(void __iomem *base_address,
-				  unsigned int regshift,
-				  unsigned int counter_number,
-				  unsigned int count)
-{
-	unsigned int byte;
-
-	if (counter_number > 2)
-		return;
-
-	byte = count & 0xff;	/* lsb of counter value */
-	writeb(byte, base_address + (counter_number << regshift));
-	byte = (count >> 8) & 0xff;	/* msb of counter value */
-	writeb(byte, base_address + (counter_number << regshift));
-}
-
-/*
- * Set counter mode, should work for 8253 also.
- * Note: the 'mode' value is different to that for i8254_load() and comes
- * from the INSN_CONFIG_8254_SET_MODE command:
- *   I8254_MODE0, I8254_MODE1, ..., I8254_MODE5
- * OR'ed with:
- *   I8254_BCD, I8254_BINARY
- */
-static inline int i8254_set_mode(unsigned long base_address,
-				 unsigned int regshift,
-				 unsigned int counter_number, unsigned int mode)
-{
-	unsigned int byte;
-
-	if (counter_number > 2)
-		return -1;
-	if (mode > (I8254_MODE5 | I8254_BCD))
-		return -1;
-
-	byte = counter_number << 6;
-	byte |= 0x30;		/* load low then high byte */
-	byte |= mode;		/* set counter mode and BCD|binary */
-	outb(byte, base_address + (i8254_control_reg << regshift));
-
-	return 0;
-}
-
-static inline int i8254_mm_set_mode(void __iomem *base_address,
-				    unsigned int regshift,
-				    unsigned int counter_number,
-				    unsigned int mode)
-{
-	unsigned int byte;
-
-	if (counter_number > 2)
-		return -1;
-	if (mode > (I8254_MODE5 | I8254_BCD))
-		return -1;
-
-	byte = counter_number << 6;
-	byte |= 0x30;		/* load low then high byte */
-	byte |= mode;		/* set counter mode and BCD|binary */
-	writeb(byte, base_address + (i8254_control_reg << regshift));
-
-	return 0;
-}
-
-static inline int i8254_status(unsigned long base_address,
-			       unsigned int regshift,
-			       unsigned int counter_number)
-{
-	outb(0xE0 | (2 << counter_number),
-	     base_address + (i8254_control_reg << regshift));
-	return inb(base_address + (counter_number << regshift));
-}
-
-static inline int i8254_mm_status(void __iomem *base_address,
-				  unsigned int regshift,
-				  unsigned int counter_number)
-{
-	writeb(0xE0 | (2 << counter_number),
-	       base_address + (i8254_control_reg << regshift));
-	return readb(base_address + (counter_number << regshift));
-}
-
-#endif
-
-#endif
diff --git a/drivers/staging/comedi/drivers/8255.c b/drivers/staging/comedi/drivers/8255.c
index c2f15de6a547..ba89321df65d 100644
--- a/drivers/staging/comedi/drivers/8255.c
+++ b/drivers/staging/comedi/drivers/8255.c
@@ -51,7 +51,6 @@
 #include <linux/module.h>
 #include "../comedidev.h"
 
-#include "comedi_fc.h"
 #include "8255.h"
 
 struct subdev_8255_private {
diff --git a/drivers/staging/comedi/drivers/8255_pci.c b/drivers/staging/comedi/drivers/8255_pci.c
index 984764211a2d..bb9854b56807 100644
--- a/drivers/staging/comedi/drivers/8255_pci.c
+++ b/drivers/staging/comedi/drivers/8255_pci.c
@@ -62,9 +62,8 @@
  */
 
 #include <linux/module.h>
-#include <linux/pci.h>
 
-#include "../comedidev.h"
+#include "../comedi_pci.h"
 
 #include "8255.h"
 
@@ -178,7 +177,7 @@ static const struct pci_8255_boardinfo pci_8255_boards[] = {
 	},
 };
 
-/* ripped from mite.h and mite_setup2() to avoid mite dependancy */
+/* ripped from mite.h and mite_setup2() to avoid mite dependency */
 #define MITE_IODWBSR	0xc0	 /* IO Device Window Base Size Register */
 #define WENAB		(1 << 7) /* window enable */
 
diff --git a/drivers/staging/comedi/drivers/Makefile b/drivers/staging/comedi/drivers/Makefile
index 7d1fbd53a8ab..d6d834006015 100644
--- a/drivers/staging/comedi/drivers/Makefile
+++ b/drivers/staging/comedi/drivers/Makefile
@@ -3,6 +3,7 @@
 ccflags-$(CONFIG_COMEDI_DEBUG)		:= -DDEBUG
 
 # Comedi "helper" modules
+obj-$(CONFIG_COMEDI_8254)		+= comedi_8254.o
 obj-$(CONFIG_COMEDI_ISADMA)		+= comedi_isadma.o
 
 # Comedi misc drivers
diff --git a/drivers/staging/comedi/drivers/addi_apci_1032.c b/drivers/staging/comedi/drivers/addi_apci_1032.c
index 4911b627203b..b37166d57b64 100644
--- a/drivers/staging/comedi/drivers/addi_apci_1032.c
+++ b/drivers/staging/comedi/drivers/addi_apci_1032.c
@@ -71,11 +71,9 @@
  */
 
 #include <linux/module.h>
-#include <linux/pci.h>
 #include <linux/interrupt.h>
 
-#include "../comedidev.h"
-#include "comedi_fc.h"
+#include "../comedi_pci.h"
 #include "amcc_s5933.h"
 
 /*
@@ -196,11 +194,11 @@ static int apci1032_cos_cmdtest(struct comedi_device *dev,
 
 	/* Step 1 : check if triggers are trivially valid */
 
-	err |= cfc_check_trigger_src(&cmd->start_src, TRIG_NOW);
-	err |= cfc_check_trigger_src(&cmd->scan_begin_src, TRIG_EXT);
-	err |= cfc_check_trigger_src(&cmd->convert_src, TRIG_FOLLOW);
-	err |= cfc_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
-	err |= cfc_check_trigger_src(&cmd->stop_src, TRIG_NONE);
+	err |= comedi_check_trigger_src(&cmd->start_src, TRIG_NOW);
+	err |= comedi_check_trigger_src(&cmd->scan_begin_src, TRIG_EXT);
+	err |= comedi_check_trigger_src(&cmd->convert_src, TRIG_FOLLOW);
+	err |= comedi_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
+	err |= comedi_check_trigger_src(&cmd->stop_src, TRIG_NONE);
 
 	if (err)
 		return 1;
@@ -210,11 +208,12 @@ static int apci1032_cos_cmdtest(struct comedi_device *dev,
 
 	/* Step 3: check if arguments are trivially valid */
 
-	err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
-	err |= cfc_check_trigger_arg_is(&cmd->scan_begin_arg, 0);
-	err |= cfc_check_trigger_arg_is(&cmd->convert_arg, 0);
-	err |= cfc_check_trigger_arg_is(&cmd->scan_end_arg, cmd->chanlist_len);
-	err |= cfc_check_trigger_arg_is(&cmd->stop_arg, 0);
+	err |= comedi_check_trigger_arg_is(&cmd->start_arg, 0);
+	err |= comedi_check_trigger_arg_is(&cmd->scan_begin_arg, 0);
+	err |= comedi_check_trigger_arg_is(&cmd->convert_arg, 0);
+	err |= comedi_check_trigger_arg_is(&cmd->scan_end_arg,
+					   cmd->chanlist_len);
+	err |= comedi_check_trigger_arg_is(&cmd->stop_arg, 0);
 
 	if (err)
 		return 3;
@@ -238,7 +237,7 @@ static int apci1032_cos_cmd(struct comedi_device *dev,
 
 	if (!devpriv->ctrl) {
 		dev_warn(dev->class_dev,
-			"Interrupts disabled due to mode configuration!\n");
+			 "Interrupts disabled due to mode configuration!\n");
 		return -EINVAL;
 	}
 
@@ -296,7 +295,7 @@ static int apci1032_di_insn_bits(struct comedi_device *dev,
 }
 
 static int apci1032_auto_attach(struct comedi_device *dev,
-					  unsigned long context_unused)
+				unsigned long context_unused)
 {
 	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
 	struct apci1032_private *devpriv;
diff --git a/drivers/staging/comedi/drivers/addi_apci_1500.c b/drivers/staging/comedi/drivers/addi_apci_1500.c
index f15aa1f6b476..63991c49ff23 100644
--- a/drivers/staging/comedi/drivers/addi_apci_1500.c
+++ b/drivers/staging/comedi/drivers/addi_apci_1500.c
@@ -22,11 +22,9 @@
  */
 
 #include <linux/module.h>
-#include <linux/pci.h>
 #include <linux/interrupt.h>
 
-#include "../comedidev.h"
-#include "comedi_fc.h"
+#include "../comedi_pci.h"
 #include "amcc_s5933.h"
 #include "z8536.h"
 
@@ -249,8 +247,8 @@ static irqreturn_t apci1500_interrupt(int irq, void *d)
 	 *
 	 *    Mask     Meaning
 	 * ----------  ------------------------------------------
-	 * 0x00000001  Event 1 has occured
-	 * 0x00000010  Event 2 has occured
+	 * 0x00000001  Event 1 has occurred
+	 * 0x00000010  Event 2 has occurred
 	 * 0x00000100  Counter/timer 1 has run down (not implemented)
 	 * 0x00001000  Counter/timer 2 has run down (not implemented)
 	 * 0x00010000  Counter 3 has run down (not implemented)
@@ -386,11 +384,11 @@ static int apci1500_di_cmdtest(struct comedi_device *dev,
 
 	/* Step 1 : check if triggers are trivially valid */
 
-	err |= cfc_check_trigger_src(&cmd->start_src, TRIG_INT);
-	err |= cfc_check_trigger_src(&cmd->scan_begin_src, TRIG_EXT);
-	err |= cfc_check_trigger_src(&cmd->convert_src, TRIG_FOLLOW);
-	err |= cfc_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
-	err |= cfc_check_trigger_src(&cmd->stop_src, TRIG_NONE);
+	err |= comedi_check_trigger_src(&cmd->start_src, TRIG_INT);
+	err |= comedi_check_trigger_src(&cmd->scan_begin_src, TRIG_EXT);
+	err |= comedi_check_trigger_src(&cmd->convert_src, TRIG_FOLLOW);
+	err |= comedi_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
+	err |= comedi_check_trigger_src(&cmd->stop_src, TRIG_NONE);
 
 	if (err)
 		return 1;
@@ -415,12 +413,13 @@ static int apci1500_di_cmdtest(struct comedi_device *dev,
 	 *   3	OR mode for Port A (digital inputs 0-7)
 	 *	OR mode for Port B (digital inputs 8-13 and internal signals)
 	 */
-	err |= cfc_check_trigger_arg_max(&cmd->start_arg, 3);
+	err |= comedi_check_trigger_arg_max(&cmd->start_arg, 3);
 
-	err |= cfc_check_trigger_arg_is(&cmd->scan_begin_arg, 0);
-	err |= cfc_check_trigger_arg_is(&cmd->convert_arg, 0);
-	err |= cfc_check_trigger_arg_is(&cmd->scan_end_arg, cmd->chanlist_len);
-	err |= cfc_check_trigger_arg_is(&cmd->stop_arg, 0);
+	err |= comedi_check_trigger_arg_is(&cmd->scan_begin_arg, 0);
+	err |= comedi_check_trigger_arg_is(&cmd->convert_arg, 0);
+	err |= comedi_check_trigger_arg_is(&cmd->scan_end_arg,
+					   cmd->chanlist_len);
+	err |= comedi_check_trigger_arg_is(&cmd->stop_arg, 0);
 
 	if (err)
 		return 3;
@@ -514,11 +513,11 @@ static int apci1500_di_cfg_trig(struct comedi_device *dev,
 
 		src = pt & 0xff;
 		if (src)
-			ret |= cfc_check_trigger_is_unique(src);
+			ret |= comedi_check_trigger_is_unique(src);
 
 		src = (pt >> 8) & 0xff;
 		if (src)
-			ret |= cfc_check_trigger_is_unique(src);
+			ret |= comedi_check_trigger_is_unique(src);
 
 		if (ret) {
 			dev_dbg(dev->class_dev,
diff --git a/drivers/staging/comedi/drivers/addi_apci_1516.c b/drivers/staging/comedi/drivers/addi_apci_1516.c
index d8410415cc90..9c516d1fe896 100644
--- a/drivers/staging/comedi/drivers/addi_apci_1516.c
+++ b/drivers/staging/comedi/drivers/addi_apci_1516.c
@@ -23,11 +23,9 @@
  */
 
 #include <linux/module.h>
-#include <linux/pci.h>
 
-#include "../comedidev.h"
+#include "../comedi_pci.h"
 #include "addi_watchdog.h"
-#include "comedi_fc.h"
 
 /*
  * PCI bar 1 I/O Register map - Digital input/output
diff --git a/drivers/staging/comedi/drivers/addi_apci_1564.c b/drivers/staging/comedi/drivers/addi_apci_1564.c
index 6872b69da5db..33e58b9a21b2 100644
--- a/drivers/staging/comedi/drivers/addi_apci_1564.c
+++ b/drivers/staging/comedi/drivers/addi_apci_1564.c
@@ -22,12 +22,10 @@
  */
 
 #include <linux/module.h>
-#include <linux/pci.h>
 #include <linux/interrupt.h>
 #include <linux/sched.h>
 
-#include "../comedidev.h"
-#include "comedi_fc.h"
+#include "../comedi_pci.h"
 #include "addi_tcw.h"
 #include "addi_watchdog.h"
 
@@ -107,12 +105,12 @@
 #define APCI1564_COUNTER(x)			((x) * 0x20)
 
 struct apci1564_private {
-	unsigned long eeprom;		/* base address of EEPROM register */
-	unsigned long timer;		/* base address of 12-bit timer */
-	unsigned long counters;		/* base address of 32-bit counters */
-	unsigned int mode1;		/* riding-edge/high level channels */
-	unsigned int mode2;		/* falling-edge/low level channels */
-	unsigned int ctrl;		/* interrupt mode OR (edge) . AND (level) */
+	unsigned long eeprom;	/* base address of EEPROM register */
+	unsigned long timer;	/* base address of 12-bit timer */
+	unsigned long counters;	/* base address of 32-bit counters */
+	unsigned int mode1;	/* riding-edge/high level channels */
+	unsigned int mode2;	/* falling-edge/low level channels */
+	unsigned int ctrl;	/* interrupt mode OR (edge) . AND (level) */
 	struct task_struct *tsk_current;
 };
 
@@ -365,11 +363,11 @@ static int apci1564_cos_cmdtest(struct comedi_device *dev,
 
 	/* Step 1 : check if triggers are trivially valid */
 
-	err |= cfc_check_trigger_src(&cmd->start_src, TRIG_NOW);
-	err |= cfc_check_trigger_src(&cmd->scan_begin_src, TRIG_EXT);
-	err |= cfc_check_trigger_src(&cmd->convert_src, TRIG_FOLLOW);
-	err |= cfc_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
-	err |= cfc_check_trigger_src(&cmd->stop_src, TRIG_NONE);
+	err |= comedi_check_trigger_src(&cmd->start_src, TRIG_NOW);
+	err |= comedi_check_trigger_src(&cmd->scan_begin_src, TRIG_EXT);
+	err |= comedi_check_trigger_src(&cmd->convert_src, TRIG_FOLLOW);
+	err |= comedi_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
+	err |= comedi_check_trigger_src(&cmd->stop_src, TRIG_NONE);
 
 	if (err)
 		return 1;
@@ -379,11 +377,12 @@ static int apci1564_cos_cmdtest(struct comedi_device *dev,
 
 	/* Step 3: check if arguments are trivially valid */
 
-	err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
-	err |= cfc_check_trigger_arg_is(&cmd->scan_begin_arg, 0);
-	err |= cfc_check_trigger_arg_is(&cmd->convert_arg, 0);
-	err |= cfc_check_trigger_arg_is(&cmd->scan_end_arg, cmd->chanlist_len);
-	err |= cfc_check_trigger_arg_is(&cmd->stop_arg, 0);
+	err |= comedi_check_trigger_arg_is(&cmd->start_arg, 0);
+	err |= comedi_check_trigger_arg_is(&cmd->scan_begin_arg, 0);
+	err |= comedi_check_trigger_arg_is(&cmd->convert_arg, 0);
+	err |= comedi_check_trigger_arg_is(&cmd->scan_end_arg,
+					   cmd->chanlist_len);
+	err |= comedi_check_trigger_arg_is(&cmd->stop_arg, 0);
 
 	if (err)
 		return 3;
@@ -407,7 +406,7 @@ static int apci1564_cos_cmd(struct comedi_device *dev,
 
 	if (!devpriv->ctrl) {
 		dev_warn(dev->class_dev,
-			"Interrupts disabled due to mode configuration!\n");
+			 "Interrupts disabled due to mode configuration!\n");
 		return -EINVAL;
 	}
 
@@ -430,7 +429,7 @@ static int apci1564_cos_cancel(struct comedi_device *dev,
 }
 
 static int apci1564_auto_attach(struct comedi_device *dev,
-				      unsigned long context_unused)
+				unsigned long context_unused)
 {
 	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
 	struct apci1564_private *devpriv;
diff --git a/drivers/staging/comedi/drivers/addi_apci_16xx.c b/drivers/staging/comedi/drivers/addi_apci_16xx.c
index a1248dab369f..c63133a12a4e 100644
--- a/drivers/staging/comedi/drivers/addi_apci_16xx.c
+++ b/drivers/staging/comedi/drivers/addi_apci_16xx.c
@@ -23,9 +23,8 @@
  */
 
 #include <linux/module.h>
-#include <linux/pci.h>
 
-#include "../comedidev.h"
+#include "../comedi_pci.h"
 
 /*
  * Register I/O map
diff --git a/drivers/staging/comedi/drivers/addi_apci_2032.c b/drivers/staging/comedi/drivers/addi_apci_2032.c
index eebf4f151b39..ad715253bdcc 100644
--- a/drivers/staging/comedi/drivers/addi_apci_2032.c
+++ b/drivers/staging/comedi/drivers/addi_apci_2032.c
@@ -23,13 +23,11 @@
  */
 
 #include <linux/module.h>
-#include <linux/pci.h>
 #include <linux/interrupt.h>
 #include <linux/slab.h>
 
-#include "../comedidev.h"
+#include "../comedi_pci.h"
 #include "addi_watchdog.h"
-#include "comedi_fc.h"
 
 /*
  * PCI bar 1 I/O Register map
@@ -93,17 +91,17 @@ static int apci2032_int_cmdtest(struct comedi_device *dev,
 
 	/* Step 1 : check if triggers are trivially valid */
 
-	err |= cfc_check_trigger_src(&cmd->start_src, TRIG_NOW);
-	err |= cfc_check_trigger_src(&cmd->scan_begin_src, TRIG_EXT);
-	err |= cfc_check_trigger_src(&cmd->convert_src, TRIG_NOW);
-	err |= cfc_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
-	err |= cfc_check_trigger_src(&cmd->stop_src, TRIG_COUNT | TRIG_NONE);
+	err |= comedi_check_trigger_src(&cmd->start_src, TRIG_NOW);
+	err |= comedi_check_trigger_src(&cmd->scan_begin_src, TRIG_EXT);
+	err |= comedi_check_trigger_src(&cmd->convert_src, TRIG_NOW);
+	err |= comedi_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
+	err |= comedi_check_trigger_src(&cmd->stop_src, TRIG_COUNT | TRIG_NONE);
 
 	if (err)
 		return 1;
 
 	/* Step 2a : make sure trigger sources are unique */
-	err |= cfc_check_trigger_is_unique(cmd->stop_src);
+	err |= comedi_check_trigger_is_unique(cmd->stop_src);
 
 	/* Step 2b : and mutually compatible */
 
@@ -112,14 +110,15 @@ static int apci2032_int_cmdtest(struct comedi_device *dev,
 
 	/* Step 3: check if arguments are trivially valid */
 
-	err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
-	err |= cfc_check_trigger_arg_is(&cmd->scan_begin_arg, 0);
-	err |= cfc_check_trigger_arg_is(&cmd->convert_arg, 0);
-	err |= cfc_check_trigger_arg_is(&cmd->scan_end_arg, cmd->chanlist_len);
+	err |= comedi_check_trigger_arg_is(&cmd->start_arg, 0);
+	err |= comedi_check_trigger_arg_is(&cmd->scan_begin_arg, 0);
+	err |= comedi_check_trigger_arg_is(&cmd->convert_arg, 0);
+	err |= comedi_check_trigger_arg_is(&cmd->scan_end_arg,
+					   cmd->chanlist_len);
 	if (cmd->stop_src == TRIG_COUNT)
-		err |= cfc_check_trigger_arg_min(&cmd->stop_arg, 1);
+		err |= comedi_check_trigger_arg_min(&cmd->stop_arg, 1);
 	else	/* TRIG_NONE */
-		err |= cfc_check_trigger_arg_is(&cmd->stop_arg, 0);
+		err |= comedi_check_trigger_arg_is(&cmd->stop_arg, 0);
 
 	if (err)
 		return 3;
diff --git a/drivers/staging/comedi/drivers/addi_apci_2200.c b/drivers/staging/comedi/drivers/addi_apci_2200.c
index 1f9d13661ac9..2b382a52d80d 100644
--- a/drivers/staging/comedi/drivers/addi_apci_2200.c
+++ b/drivers/staging/comedi/drivers/addi_apci_2200.c
@@ -23,9 +23,8 @@
  */
 
 #include <linux/module.h>
-#include <linux/pci.h>
 
-#include "../comedidev.h"
+#include "../comedi_pci.h"
 #include "addi_watchdog.h"
 
 /*
diff --git a/drivers/staging/comedi/drivers/addi_apci_3120.c b/drivers/staging/comedi/drivers/addi_apci_3120.c
index c65f9407fd06..95dc64bfec73 100644
--- a/drivers/staging/comedi/drivers/addi_apci_3120.c
+++ b/drivers/staging/comedi/drivers/addi_apci_3120.c
@@ -22,11 +22,9 @@
  */
 
 #include <linux/module.h>
-#include <linux/pci.h>
 #include <linux/interrupt.h>
 
-#include "../comedidev.h"
-#include "comedi_fc.h"
+#include "../comedi_pci.h"
 #include "amcc_s5933.h"
 
 /*
@@ -612,21 +610,21 @@ static int apci3120_ai_cmdtest(struct comedi_device *dev,
 
 	/* Step 1 : check if triggers are trivially valid */
 
-	err |= cfc_check_trigger_src(&cmd->start_src, TRIG_NOW | TRIG_EXT);
-	err |= cfc_check_trigger_src(&cmd->scan_begin_src,
+	err |= comedi_check_trigger_src(&cmd->start_src, TRIG_NOW | TRIG_EXT);
+	err |= comedi_check_trigger_src(&cmd->scan_begin_src,
 					TRIG_TIMER | TRIG_FOLLOW);
-	err |= cfc_check_trigger_src(&cmd->convert_src, TRIG_TIMER);
-	err |= cfc_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
-	err |= cfc_check_trigger_src(&cmd->stop_src, TRIG_COUNT | TRIG_NONE);
+	err |= comedi_check_trigger_src(&cmd->convert_src, TRIG_TIMER);
+	err |= comedi_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
+	err |= comedi_check_trigger_src(&cmd->stop_src, TRIG_COUNT | TRIG_NONE);
 
 	if (err)
 		return 1;
 
 	/* Step 2a : make sure trigger sources are unique */
 
-	err |= cfc_check_trigger_is_unique(cmd->start_src);
-	err |= cfc_check_trigger_is_unique(cmd->scan_begin_src);
-	err |= cfc_check_trigger_is_unique(cmd->stop_src);
+	err |= comedi_check_trigger_is_unique(cmd->start_src);
+	err |= comedi_check_trigger_is_unique(cmd->scan_begin_src);
+	err |= comedi_check_trigger_is_unique(cmd->stop_src);
 
 	/* Step 2b : and mutually compatible */
 
@@ -635,21 +633,24 @@ static int apci3120_ai_cmdtest(struct comedi_device *dev,
 
 	/* Step 3: check if arguments are trivially valid */
 
-	err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
+	err |= comedi_check_trigger_arg_is(&cmd->start_arg, 0);
 
-	if (cmd->scan_begin_src == TRIG_TIMER)	/* Test Delay timing */
-		err |= cfc_check_trigger_arg_min(&cmd->scan_begin_arg, 100000);
+	if (cmd->scan_begin_src == TRIG_TIMER) {	/* Test Delay timing */
+		err |= comedi_check_trigger_arg_min(&cmd->scan_begin_arg,
+						    100000);
+	}
 
 	/* minimum conversion time per sample is 10us */
-	err |= cfc_check_trigger_arg_min(&cmd->convert_arg, 10000);
+	err |= comedi_check_trigger_arg_min(&cmd->convert_arg, 10000);
 
-	err |= cfc_check_trigger_arg_min(&cmd->chanlist_len, 1);
-	err |= cfc_check_trigger_arg_is(&cmd->scan_end_arg, cmd->chanlist_len);
+	err |= comedi_check_trigger_arg_min(&cmd->chanlist_len, 1);
+	err |= comedi_check_trigger_arg_is(&cmd->scan_end_arg,
+					   cmd->chanlist_len);
 
 	if (cmd->stop_src == TRIG_COUNT)
-		err |= cfc_check_trigger_arg_min(&cmd->stop_arg, 1);
+		err |= comedi_check_trigger_arg_min(&cmd->stop_arg, 1);
 	else	/*  TRIG_NONE */
-		err |= cfc_check_trigger_arg_is(&cmd->stop_arg, 0);
+		err |= comedi_check_trigger_arg_is(&cmd->stop_arg, 0);
 
 	if (err)
 		return 3;
@@ -659,7 +660,7 @@ static int apci3120_ai_cmdtest(struct comedi_device *dev,
 	if (cmd->scan_begin_src == TRIG_TIMER) {
 		/* scan begin must be larger than the scan time */
 		arg = cmd->convert_arg * cmd->scan_end_arg;
-		err |= cfc_check_trigger_arg_min(&cmd->scan_begin_arg, arg);
+		err |= comedi_check_trigger_arg_min(&cmd->scan_begin_arg, arg);
 	}
 
 	if (err)
diff --git a/drivers/staging/comedi/drivers/addi_apci_3501.c b/drivers/staging/comedi/drivers/addi_apci_3501.c
index 5961f195ba0b..73786a3f3df9 100644
--- a/drivers/staging/comedi/drivers/addi_apci_3501.c
+++ b/drivers/staging/comedi/drivers/addi_apci_3501.c
@@ -23,12 +23,10 @@
  */
 
 #include <linux/module.h>
-#include <linux/pci.h>
 #include <linux/interrupt.h>
 #include <linux/sched.h>
 
-#include "../comedidev.h"
-#include "comedi_fc.h"
+#include "../comedi_pci.h"
 #include "amcc_s5933.h"
 
 /*
@@ -203,7 +201,7 @@ static unsigned short apci3501_eeprom_readw(unsigned long iobase,
 		outb(NVCMD_LOAD_HIGH, iobase + AMCC_OP_REG_MCSR_NVCMD);
 		apci3501_eeprom_wait(iobase);
 		outb(((addr + i) >> 8) & 0xff,
-			iobase + AMCC_OP_REG_MCSR_NVDATA);
+		     iobase + AMCC_OP_REG_MCSR_NVDATA);
 		apci3501_eeprom_wait(iobase);
 
 		/* Read the eeprom data byte */
@@ -270,7 +268,7 @@ static irqreturn_t apci3501_interrupt(int irq, void *d)
 
 	/*  Disable Interrupt */
 	ul_Command1 = inl(dev->iobase + APCI3501_TIMER_CTRL_REG);
-	ul_Command1 = (ul_Command1 & 0xFFFFF9FDul);
+	ul_Command1 = ul_Command1 & 0xFFFFF9FDul;
 	outl(ul_Command1, dev->iobase + APCI3501_TIMER_CTRL_REG);
 
 	ui_Timer_AOWatchdog = inl(dev->iobase + APCI3501_TIMER_IRQ_REG) & 0x1;
@@ -282,7 +280,7 @@ static irqreturn_t apci3501_interrupt(int irq, void *d)
 	/* Enable Interrupt Send a signal to from kernel to user space */
 	send_sig(SIGIO, devpriv->tsk_Current, 0);
 	ul_Command1 = inl(dev->iobase + APCI3501_TIMER_CTRL_REG);
-	ul_Command1 = ((ul_Command1 & 0xFFFFF9FDul) | 1 << 1);
+	ul_Command1 = (ul_Command1 & 0xFFFFF9FDul) | 1 << 1;
 	outl(ul_Command1, dev->iobase + APCI3501_TIMER_CTRL_REG);
 	inl(dev->iobase + APCI3501_TIMER_STATUS_REG);
 
diff --git a/drivers/staging/comedi/drivers/addi_apci_3xxx.c b/drivers/staging/comedi/drivers/addi_apci_3xxx.c
index c173810a3b5b..bef6efc84efd 100644
--- a/drivers/staging/comedi/drivers/addi_apci_3xxx.c
+++ b/drivers/staging/comedi/drivers/addi_apci_3xxx.c
@@ -23,12 +23,9 @@
  */
 
 #include <linux/module.h>
-#include <linux/pci.h>
 #include <linux/interrupt.h>
 
-#include "../comedidev.h"
-
-#include "comedi_fc.h"
+#include "../comedi_pci.h"
 
 #define CONV_UNIT_NS		(1 << 0)
 #define CONV_UNIT_US		(1 << 1)
@@ -529,18 +526,18 @@ static int apci3xxx_ai_cmdtest(struct comedi_device *dev,
 
 	/* Step 1 : check if triggers are trivially valid */
 
-	err |= cfc_check_trigger_src(&cmd->start_src, TRIG_NOW);
-	err |= cfc_check_trigger_src(&cmd->scan_begin_src, TRIG_FOLLOW);
-	err |= cfc_check_trigger_src(&cmd->convert_src, TRIG_TIMER);
-	err |= cfc_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
-	err |= cfc_check_trigger_src(&cmd->stop_src, TRIG_COUNT | TRIG_NONE);
+	err |= comedi_check_trigger_src(&cmd->start_src, TRIG_NOW);
+	err |= comedi_check_trigger_src(&cmd->scan_begin_src, TRIG_FOLLOW);
+	err |= comedi_check_trigger_src(&cmd->convert_src, TRIG_TIMER);
+	err |= comedi_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
+	err |= comedi_check_trigger_src(&cmd->stop_src, TRIG_COUNT | TRIG_NONE);
 
 	if (err)
 		return 1;
 
 	/* Step 2a : make sure trigger sources are unique */
 
-	err |= cfc_check_trigger_is_unique(cmd->stop_src);
+	err |= comedi_check_trigger_is_unique(cmd->stop_src);
 
 	/* Step 2b : and mutually compatible */
 
@@ -549,16 +546,17 @@ static int apci3xxx_ai_cmdtest(struct comedi_device *dev,
 
 	/* Step 3: check if arguments are trivially valid */
 
-	err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
-	err |= cfc_check_trigger_arg_is(&cmd->scan_begin_arg, 0);
-	err |= cfc_check_trigger_arg_min(&cmd->convert_arg,
-					 board->ai_min_acq_ns);
-	err |= cfc_check_trigger_arg_is(&cmd->scan_end_arg, cmd->chanlist_len);
+	err |= comedi_check_trigger_arg_is(&cmd->start_arg, 0);
+	err |= comedi_check_trigger_arg_is(&cmd->scan_begin_arg, 0);
+	err |= comedi_check_trigger_arg_min(&cmd->convert_arg,
+					    board->ai_min_acq_ns);
+	err |= comedi_check_trigger_arg_is(&cmd->scan_end_arg,
+					   cmd->chanlist_len);
 
 	if (cmd->stop_src == TRIG_COUNT)
-		err |= cfc_check_trigger_arg_min(&cmd->stop_arg, 1);
+		err |= comedi_check_trigger_arg_min(&cmd->stop_arg, 1);
 	else	/* TRIG_NONE */
-		err |= cfc_check_trigger_arg_is(&cmd->stop_arg, 0);
+		err |= comedi_check_trigger_arg_is(&cmd->stop_arg, 0);
 
 	if (err)
 		return 3;
@@ -567,7 +565,7 @@ static int apci3xxx_ai_cmdtest(struct comedi_device *dev,
 
 	arg = cmd->convert_arg;
 	err |= apci3xxx_ai_ns_to_timer(dev, &arg, cmd->flags);
-	err |= cfc_check_trigger_arg_is(&cmd->convert_arg, arg);
+	err |= comedi_check_trigger_arg_is(&cmd->convert_arg, arg);
 
 	if (err)
 		return 4;
diff --git a/drivers/staging/comedi/drivers/addi_watchdog.c b/drivers/staging/comedi/drivers/addi_watchdog.c
index c5b082d4e51e..9d9853fe54a0 100644
--- a/drivers/staging/comedi/drivers/addi_watchdog.c
+++ b/drivers/staging/comedi/drivers/addi_watchdog.c
@@ -54,7 +54,7 @@ static int addi_watchdog_insn_config(struct comedi_device *dev,
 
 		/* Time base is 20ms, let the user know the timeout */
 		dev_info(dev->class_dev, "watchdog enabled, timeout:%dms\n",
-			20 * reload + 20);
+			 20 * reload + 20);
 		break;
 	case INSN_CONFIG_DISARM:
 		spriv->wdog_ctrl = 0;
diff --git a/drivers/staging/comedi/drivers/adl_pci6208.c b/drivers/staging/comedi/drivers/adl_pci6208.c
index a3ea4b7c18dd..7ed3fd6fbd3e 100644
--- a/drivers/staging/comedi/drivers/adl_pci6208.c
+++ b/drivers/staging/comedi/drivers/adl_pci6208.c
@@ -19,19 +19,22 @@
 /*
  * Driver: adl_pci6208
  * Description: ADLink PCI-6208/6216 Series Multi-channel Analog Output Cards
- * Devices: [ADLink] PCI-6208 (adl_pci6208), PCI-6216 (adl_pci6216)
+ * Devices: [ADLink] PCI-6208 (adl_pci6208), PCI-6216
  * Author: nsyeow <nsyeow@pd.jaring.my>
- * Updated: Fri, 30 Jan 2004 14:44:27 +0800
+ * Updated: Wed, 11 Feb 2015 11:37:18 +0000
  * Status: untested
  *
  * Configuration Options: not applicable, uses PCI auto config
+ *
+ * All supported devices share the same PCI device ID and are treated as a
+ * PCI-6216 with 16 analog output channels.  On a PCI-6208, the upper 8
+ * channels exist in registers, but don't go to DAC chips.
  */
 
 #include <linux/module.h>
 #include <linux/delay.h>
-#include <linux/pci.h>
 
-#include "../comedidev.h"
+#include "../comedi_pci.h"
 
 /*
  * PCI-6208/6216-GL register map
@@ -45,27 +48,6 @@
 #define PCI6208_DIO_DI_MASK		(0xf0)
 #define PCI6208_DIO_DI_SHIFT		(4)
 
-enum pci6208_boardid {
-	BOARD_PCI6208,
-	BOARD_PCI6216,
-};
-
-struct pci6208_board {
-	const char *name;
-	int ao_chans;
-};
-
-static const struct pci6208_board pci6208_boards[] = {
-	[BOARD_PCI6208] = {
-		.name		= "adl_pci6208",
-		.ao_chans	= 8,
-	},
-	[BOARD_PCI6216] = {
-		.name		= "adl_pci6216",
-		.ao_chans	= 16,
-	},
-};
-
 static int pci6208_ao_eoc(struct comedi_device *dev,
 			  struct comedi_subdevice *s,
 			  struct comedi_insn *insn,
@@ -136,21 +118,13 @@ static int pci6208_do_insn_bits(struct comedi_device *dev,
 }
 
 static int pci6208_auto_attach(struct comedi_device *dev,
-			       unsigned long context)
+			       unsigned long context_unused)
 {
 	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
-	const struct pci6208_board *boardinfo = NULL;
 	struct comedi_subdevice *s;
 	unsigned int val;
 	int ret;
 
-	if (context < ARRAY_SIZE(pci6208_boards))
-		boardinfo = &pci6208_boards[context];
-	if (!boardinfo)
-		return -ENODEV;
-	dev->board_ptr = boardinfo;
-	dev->board_name = boardinfo->name;
-
 	ret = comedi_pci_enable(dev);
 	if (ret)
 		return ret;
@@ -164,7 +138,7 @@ static int pci6208_auto_attach(struct comedi_device *dev,
 	/* analog output subdevice */
 	s->type		= COMEDI_SUBD_AO;
 	s->subdev_flags	= SDF_WRITABLE;
-	s->n_chan	= boardinfo->ao_chans;
+	s->n_chan	= 16;	/* Only 8 usable on PCI-6208 */
 	s->maxdata	= 0xffff;
 	s->range_table	= &range_bipolar10;
 	s->insn_write	= pci6208_ao_insn_write;
@@ -217,8 +191,9 @@ static int adl_pci6208_pci_probe(struct pci_dev *dev,
 }
 
 static const struct pci_device_id adl_pci6208_pci_table[] = {
-	{ PCI_VDEVICE(ADLINK, 0x6208), BOARD_PCI6208 },
-	{ PCI_VDEVICE(ADLINK, 0x6216), BOARD_PCI6216 },
+	{ PCI_DEVICE(PCI_VENDOR_ID_ADLINK, 0x6208) },
+	{ PCI_DEVICE_SUB(PCI_VENDOR_ID_PLX, PCI_DEVICE_ID_PLX_9050,
+			 0x9999, 0x6208) },
 	{ 0 }
 };
 MODULE_DEVICE_TABLE(pci, adl_pci6208_pci_table);
diff --git a/drivers/staging/comedi/drivers/adl_pci7x3x.c b/drivers/staging/comedi/drivers/adl_pci7x3x.c
index 618e641ffaac..934af3ff7897 100644
--- a/drivers/staging/comedi/drivers/adl_pci7x3x.c
+++ b/drivers/staging/comedi/drivers/adl_pci7x3x.c
@@ -53,9 +53,8 @@
  */
 
 #include <linux/module.h>
-#include <linux/pci.h>
 
-#include "../comedidev.h"
+#include "../comedi_pci.h"
 
 /*
  * Register I/O map (32-bit access only)
diff --git a/drivers/staging/comedi/drivers/adl_pci8164.c b/drivers/staging/comedi/drivers/adl_pci8164.c
index cc6c53b800a7..da901c8dec0e 100644
--- a/drivers/staging/comedi/drivers/adl_pci8164.c
+++ b/drivers/staging/comedi/drivers/adl_pci8164.c
@@ -28,9 +28,8 @@
 
 #include <linux/kernel.h>
 #include <linux/module.h>
-#include <linux/pci.h>
 
-#include "../comedidev.h"
+#include "../comedi_pci.h"
 
 #define PCI8164_AXIS(x)		((x) * 0x08)
 #define PCI8164_CMD_MSTS_REG	0x00
@@ -69,7 +68,7 @@ static int adl_pci8164_insn_write(struct comedi_device *dev,
 }
 
 static int adl_pci8164_auto_attach(struct comedi_device *dev,
-					     unsigned long context_unused)
+				   unsigned long context_unused)
 {
 	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
 	struct comedi_subdevice *s;
diff --git a/drivers/staging/comedi/drivers/adl_pci9111.c b/drivers/staging/comedi/drivers/adl_pci9111.c
index f68dc99f8e27..c9df3afe97f6 100644
--- a/drivers/staging/comedi/drivers/adl_pci9111.c
+++ b/drivers/staging/comedi/drivers/adl_pci9111.c
@@ -65,15 +65,13 @@ TODO:
 */
 
 #include <linux/module.h>
-#include <linux/pci.h>
 #include <linux/delay.h>
 #include <linux/interrupt.h>
 
-#include "../comedidev.h"
+#include "../comedi_pci.h"
 
-#include "8253.h"
 #include "plx9052.h"
-#include "comedi_fc.h"
+#include "comedi_8254.h"
 
 #define PCI9111_FIFO_HALF_SIZE	512
 
@@ -137,9 +135,6 @@ struct pci9111_private_data {
 	unsigned int chunk_counter;
 	unsigned int chunk_num_samples;
 
-	unsigned int div1;
-	unsigned int div2;
-
 	unsigned short ai_bounce_buffer[2 * PCI9111_FIFO_HALF_SIZE];
 };
 
@@ -167,21 +162,6 @@ static void plx9050_interrupt_control(unsigned long io_base,
 	outb(flags, io_base + PLX9052_INTCSR);
 }
 
-static void pci9111_timer_set(struct comedi_device *dev)
-{
-	struct pci9111_private_data *dev_private = dev->private;
-	unsigned long timer_base = dev->iobase + PCI9111_8254_BASE_REG;
-
-	i8254_set_mode(timer_base, 1, 0, I8254_MODE0 | I8254_BINARY);
-	i8254_set_mode(timer_base, 1, 1, I8254_MODE2 | I8254_BINARY);
-	i8254_set_mode(timer_base, 1, 2, I8254_MODE2 | I8254_BINARY);
-
-	udelay(1);
-
-	i8254_write(timer_base, 1, 2, dev_private->div2);
-	i8254_write(timer_base, 1, 1, dev_private->div1);
-}
-
 enum pci9111_ISC0_sources {
 	irq_on_eoc,
 	irq_on_fifo_half_full
@@ -281,19 +261,18 @@ static int pci9111_ai_do_cmd_test(struct comedi_device *dev,
 				  struct comedi_subdevice *s,
 				  struct comedi_cmd *cmd)
 {
-	struct pci9111_private_data *dev_private = dev->private;
 	int err = 0;
 	unsigned int arg;
 
 	/* Step 1 : check if triggers are trivially valid */
 
-	err |= cfc_check_trigger_src(&cmd->start_src, TRIG_NOW);
-	err |= cfc_check_trigger_src(&cmd->scan_begin_src,
+	err |= comedi_check_trigger_src(&cmd->start_src, TRIG_NOW);
+	err |= comedi_check_trigger_src(&cmd->scan_begin_src,
 					TRIG_TIMER | TRIG_FOLLOW | TRIG_EXT);
-	err |= cfc_check_trigger_src(&cmd->convert_src,
+	err |= comedi_check_trigger_src(&cmd->convert_src,
 					TRIG_TIMER | TRIG_EXT);
-	err |= cfc_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
-	err |= cfc_check_trigger_src(&cmd->stop_src,
+	err |= comedi_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
+	err |= comedi_check_trigger_src(&cmd->stop_src,
 					TRIG_COUNT | TRIG_NONE);
 
 	if (err)
@@ -301,9 +280,9 @@ static int pci9111_ai_do_cmd_test(struct comedi_device *dev,
 
 	/* Step 2a : make sure trigger sources are unique */
 
-	err |= cfc_check_trigger_is_unique(cmd->scan_begin_src);
-	err |= cfc_check_trigger_is_unique(cmd->convert_src);
-	err |= cfc_check_trigger_is_unique(cmd->stop_src);
+	err |= comedi_check_trigger_is_unique(cmd->scan_begin_src);
+	err |= comedi_check_trigger_is_unique(cmd->convert_src);
+	err |= comedi_check_trigger_is_unique(cmd->stop_src);
 
 	/* Step 2b : and mutually compatible */
 
@@ -317,26 +296,29 @@ static int pci9111_ai_do_cmd_test(struct comedi_device *dev,
 
 	/* Step 3: check if arguments are trivially valid */
 
-	err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
+	err |= comedi_check_trigger_arg_is(&cmd->start_arg, 0);
 
-	if (cmd->convert_src == TRIG_TIMER)
-		err |= cfc_check_trigger_arg_min(&cmd->convert_arg,
+	if (cmd->convert_src == TRIG_TIMER) {
+		err |= comedi_check_trigger_arg_min(&cmd->convert_arg,
 					PCI9111_AI_ACQUISITION_PERIOD_MIN_NS);
-	else	/* TRIG_EXT */
-		err |= cfc_check_trigger_arg_is(&cmd->convert_arg, 0);
+	} else {	/* TRIG_EXT */
+		err |= comedi_check_trigger_arg_is(&cmd->convert_arg, 0);
+	}
 
-	if (cmd->scan_begin_src == TRIG_TIMER)
-		err |= cfc_check_trigger_arg_min(&cmd->scan_begin_arg,
+	if (cmd->scan_begin_src == TRIG_TIMER) {
+		err |= comedi_check_trigger_arg_min(&cmd->scan_begin_arg,
 					PCI9111_AI_ACQUISITION_PERIOD_MIN_NS);
-	else	/* TRIG_FOLLOW || TRIG_EXT */
-		err |= cfc_check_trigger_arg_is(&cmd->scan_begin_arg, 0);
+	} else {	/* TRIG_FOLLOW || TRIG_EXT */
+		err |= comedi_check_trigger_arg_is(&cmd->scan_begin_arg, 0);
+	}
 
-	err |= cfc_check_trigger_arg_is(&cmd->scan_end_arg, cmd->chanlist_len);
+	err |= comedi_check_trigger_arg_is(&cmd->scan_end_arg,
+					   cmd->chanlist_len);
 
 	if (cmd->stop_src == TRIG_COUNT)
-		err |= cfc_check_trigger_arg_min(&cmd->stop_arg, 1);
+		err |= comedi_check_trigger_arg_min(&cmd->stop_arg, 1);
 	else	/* TRIG_NONE */
-		err |= cfc_check_trigger_arg_is(&cmd->stop_arg, 0);
+		err |= comedi_check_trigger_arg_is(&cmd->stop_arg, 0);
 
 	if (err)
 		return 3;
@@ -345,11 +327,8 @@ static int pci9111_ai_do_cmd_test(struct comedi_device *dev,
 
 	if (cmd->convert_src == TRIG_TIMER) {
 		arg = cmd->convert_arg;
-		i8253_cascade_ns_to_timer(I8254_OSC_BASE_2MHZ,
-					  &dev_private->div1,
-					  &dev_private->div2,
-					  &arg, cmd->flags);
-		err |= cfc_check_trigger_arg_is(&cmd->convert_arg, arg);
+		comedi_8254_cascade_ns_to_timer(dev->pacer, &arg, cmd->flags);
+		err |= comedi_check_trigger_arg_is(&cmd->convert_arg, arg);
 	}
 
 	/*
@@ -362,7 +341,7 @@ static int pci9111_ai_do_cmd_test(struct comedi_device *dev,
 		if (arg < cmd->scan_begin_arg)
 			arg *= (cmd->scan_begin_arg / arg);
 
-		err |= cfc_check_trigger_arg_is(&cmd->scan_begin_arg, arg);
+		err |= comedi_check_trigger_arg_is(&cmd->scan_begin_arg, arg);
 	}
 
 	if (err)
@@ -376,7 +355,6 @@ static int pci9111_ai_do_cmd_test(struct comedi_device *dev,
 		return 5;
 
 	return 0;
-
 }
 
 static int pci9111_ai_do_cmd(struct comedi_device *dev,
@@ -400,13 +378,14 @@ static int pci9111_ai_do_cmd(struct comedi_device *dev,
 	/*  This is the same gain on every channel */
 
 	outb(CR_RANGE(cmd->chanlist[0]) & PCI9111_AI_RANGE_MASK,
-		dev->iobase + PCI9111_AI_RANGE_STAT_REG);
+	     dev->iobase + PCI9111_AI_RANGE_STAT_REG);
 
 	/*  Set timer pacer */
 	dev_private->scan_delay = 0;
 	if (cmd->convert_src == TRIG_TIMER) {
 		trig |= PCI9111_AI_TRIG_CTRL_TPST;
-		pci9111_timer_set(dev);
+		comedi_8254_update_divisors(dev->pacer);
+		comedi_8254_pacer_enable(dev->pacer, 1, 2, true);
 		pci9111_fifo_reset(dev);
 		pci9111_interrupt_source_set(dev, irq_on_fifo_half_full,
 					     irq_on_timer_tick);
@@ -593,7 +572,7 @@ static int pci9111_ai_insn_read(struct comedi_device *dev,
 	status = inb(dev->iobase + PCI9111_AI_RANGE_STAT_REG);
 	if ((status & PCI9111_AI_RANGE_MASK) != range) {
 		outb(range & PCI9111_AI_RANGE_MASK,
-			dev->iobase + PCI9111_AI_RANGE_STAT_REG);
+		     dev->iobase + PCI9111_AI_RANGE_STAT_REG);
 	}
 
 	pci9111_fifo_reset(dev);
@@ -667,16 +646,11 @@ static int pci9111_reset(struct comedi_device *dev)
 	/* disable A/D triggers (software trigger mode) and auto scan off */
 	outb(0, dev->iobase + PCI9111_AI_TRIG_CTRL_REG);
 
-	/* Reset 8254 chip */
-	dev_private->div1 = 0;
-	dev_private->div2 = 0;
-	pci9111_timer_set(dev);
-
 	return 0;
 }
 
 static int pci9111_auto_attach(struct comedi_device *dev,
-					 unsigned long context_unused)
+			       unsigned long context_unused)
 {
 	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
 	struct pci9111_private_data *dev_private;
@@ -702,6 +676,11 @@ static int pci9111_auto_attach(struct comedi_device *dev,
 			dev->irq = pcidev->irq;
 	}
 
+	dev->pacer = comedi_8254_init(dev->iobase + PCI9111_8254_BASE_REG,
+				      I8254_OSC_BASE_2MHZ, I8254_IO16, 0);
+	if (!dev->pacer)
+		return -ENOMEM;
+
 	ret = comedi_alloc_subdevices(dev, 4);
 	if (ret)
 		return ret;
diff --git a/drivers/staging/comedi/drivers/adl_pci9118.c b/drivers/staging/comedi/drivers/adl_pci9118.c
index f61e392c2d3e..fb3043dcfff1 100644
--- a/drivers/staging/comedi/drivers/adl_pci9118.c
+++ b/drivers/staging/comedi/drivers/adl_pci9118.c
@@ -73,17 +73,15 @@
  */
 
 #include <linux/module.h>
-#include <linux/pci.h>
 #include <linux/delay.h>
 #include <linux/gfp.h>
 #include <linux/interrupt.h>
 #include <linux/io.h>
 
-#include "../comedidev.h"
+#include "../comedi_pci.h"
 
 #include "amcc_s5933.h"
-#include "8253.h"
-#include "comedi_fc.h"
+#include "comedi_8254.h"
 
 #define IORANGE_9118	64	/* I hope */
 #define PCI9118_CHANLEN	255	/*
@@ -94,8 +92,7 @@
 /*
  * PCI BAR2 Register map (dev->iobase)
  */
-#define PCI9118_TIMER_REG(x)		(0x00 + ((x) * 4))
-#define PCI9118_TIMER_CTRL_REG		0x0c
+#define PCI9118_TIMER_BASE		0x00
 #define PCI9118_AI_FIFO_REG		0x10
 #define PCI9118_AO_REG(x)		(0x10 + ((x) * 4))
 #define PCI9118_AI_STATUS_REG		0x18
@@ -239,10 +236,6 @@ struct pci9118_private {
 					 * measure can start/stop
 					 * on external trigger
 					 */
-	unsigned int ai_divisor1, ai_divisor2;	/*
-						 * divisors for start of measure
-						 * on external start
-						 */
 	unsigned int dma_actbuf;		/* which buffer is used now */
 	struct pci9118_dmabuf dmabuf[2];
 	int softsshdelay;		/*
@@ -297,24 +290,6 @@ static void pci9118_amcc_int_ena(struct comedi_device *dev, bool enable)
 	outl(intcsr, devpriv->iobase_a + AMCC_OP_REG_INTCSR);
 }
 
-static void pci9118_timer_write(struct comedi_device *dev,
-				unsigned int timer, unsigned int val)
-{
-	outl(val & 0xff, dev->iobase + PCI9118_TIMER_REG(timer));
-	outl((val >> 8) & 0xff, dev->iobase + PCI9118_TIMER_REG(timer));
-}
-
-static void pci9118_timer_set_mode(struct comedi_device *dev,
-				   unsigned int timer, unsigned int mode)
-{
-	unsigned int val;
-
-	val = timer << 6;	/* select timer */
-	val |= 0x30;		/* load low then high byte */
-	val |= mode;		/* set timer mode and BCD|binary */
-	outl(val, dev->iobase + PCI9118_TIMER_CTRL_REG);
-}
-
 static void pci9118_ai_reset_fifo(struct comedi_device *dev)
 {
 	/* writing any value resets the A/D FIFO */
@@ -440,8 +415,8 @@ static void interrupt_pci9118_ai_mode4_switch(struct comedi_device *dev,
 	devpriv->ai_cfg = PCI9118_AI_CFG_PDTRG | PCI9118_AI_CFG_PETRG |
 			  PCI9118_AI_CFG_AM;
 	outl(devpriv->ai_cfg, dev->iobase + PCI9118_AI_CFG_REG);
-	pci9118_timer_set_mode(dev, 0, I8254_MODE0);
-	pci9118_timer_write(dev, 0, dmabuf->hw >> 1);
+	comedi_8254_load(dev->pacer, 0, dmabuf->hw >> 1,
+			 I8254_MODE0 | I8254_BINARY);
 	devpriv->ai_cfg |= PCI9118_AI_CFG_START;
 	outl(devpriv->ai_cfg, dev->iobase + PCI9118_AI_CFG_REG);
 }
@@ -577,15 +552,16 @@ static void pci9118_calc_divisors(struct comedi_device *dev,
 				  unsigned int *div1, unsigned int *div2,
 				  unsigned int chnsshfront)
 {
+	struct comedi_8254 *pacer = dev->pacer;
 	struct comedi_cmd *cmd = &s->async->cmd;
 
-	*div1 = *tim2 / I8254_OSC_BASE_4MHZ;	/* convert timer (burst) */
-	*div2 = *tim1 / I8254_OSC_BASE_4MHZ;	/* scan timer */
+	*div1 = *tim2 / pacer->osc_base;	/* convert timer (burst) */
+	*div2 = *tim1 / pacer->osc_base;	/* scan timer */
 	*div2 = *div2 / *div1;			/* major timer is c1*c2 */
 	if (*div2 < chans)
 		*div2 = chans;
 
-	*tim2 = *div1 * I8254_OSC_BASE_4MHZ;	/* real convert timer */
+	*tim2 = *div1 * pacer->osc_base;	/* real convert timer */
 
 	if (cmd->convert_src == TRIG_NOW && !chnsshfront) {
 		/* use BSSH signal */
@@ -593,21 +569,13 @@ static void pci9118_calc_divisors(struct comedi_device *dev,
 			*div2 = chans + 2;
 	}
 
-	*tim1 = *div1 * *div2 * I8254_OSC_BASE_4MHZ;
+	*tim1 = *div1 * *div2 * pacer->osc_base;
 }
 
 static void pci9118_start_pacer(struct comedi_device *dev, int mode)
 {
-	struct pci9118_private *devpriv = dev->private;
-
-	pci9118_timer_set_mode(dev, 1, I8254_MODE2);
-	pci9118_timer_set_mode(dev, 2, I8254_MODE2);
-	udelay(1);
-
-	if ((mode == 1) || (mode == 2) || (mode == 4)) {
-		pci9118_timer_write(dev, 2, devpriv->ai_divisor2);
-		pci9118_timer_write(dev, 1, devpriv->ai_divisor1);
-	}
+	if (mode == 1 || mode == 2 || mode == 4)
+		comedi_8254_pacer_enable(dev->pacer, 1, 2, true);
 }
 
 static int pci9118_ai_cancel(struct comedi_device *dev,
@@ -618,7 +586,7 @@ static int pci9118_ai_cancel(struct comedi_device *dev,
 	if (devpriv->usedma)
 		pci9118_amcc_dma_ena(dev, false);
 	pci9118_exttrg_enable(dev, false);
-	pci9118_start_pacer(dev, 0);	/* stop 8254 counters */
+	comedi_8254_pacer_enable(dev->pacer, 1, 2, false);
 	/* set default config (disable burst and triggers) */
 	devpriv->ai_cfg = PCI9118_AI_CFG_PDTRG | PCI9118_AI_CFG_PETRG;
 	outl(devpriv->ai_cfg, dev->iobase + PCI9118_AI_CFG_REG);
@@ -663,7 +631,6 @@ static void pci9118_ai_munge(struct comedi_device *dev,
 			array[i] ^= 0x8000;
 		else
 			array[i] = (array[i] >> 4) & 0x0fff;
-
 	}
 }
 
@@ -966,7 +933,7 @@ static int Compute_and_setup_dma(struct comedi_device *dev,
 /* outl(0x02000000|AINT_WRITE_COMPL, devpriv->iobase_a+AMCC_OP_REG_INTCSR); */
 	pci9118_amcc_dma_ena(dev, true);
 	outl(inl(devpriv->iobase_a + AMCC_OP_REG_INTCSR) | EN_A2P_TRANSFERS,
-			devpriv->iobase_a + AMCC_OP_REG_INTCSR);
+	     devpriv->iobase_a + AMCC_OP_REG_INTCSR);
 						/* allow bus mastering */
 
 	return 0;
@@ -975,6 +942,7 @@ static int Compute_and_setup_dma(struct comedi_device *dev,
 static int pci9118_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 {
 	struct pci9118_private *devpriv = dev->private;
+	struct comedi_8254 *pacer = dev->pacer;
 	struct comedi_cmd *cmd = &s->async->cmd;
 	unsigned int addchans = 0;
 
@@ -1093,12 +1061,10 @@ static int pci9118_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 		else
 			devpriv->ai_do = 1;
 
-		i8253_cascade_ns_to_timer(I8254_OSC_BASE_4MHZ,
-					  &devpriv->ai_divisor1,
-					  &devpriv->ai_divisor2,
-					  &cmd->convert_arg,
-					  devpriv->ai_flags &
-					  CMDF_ROUND_NEAREST);
+		comedi_8254_cascade_ns_to_timer(pacer, &cmd->convert_arg,
+						devpriv->ai_flags &
+						CMDF_ROUND_NEAREST);
+		comedi_8254_update_divisors(pacer);
 
 		devpriv->ai_ctrl |= PCI9118_AI_CTRL_TMRTR;
 
@@ -1112,8 +1078,8 @@ static int pci9118_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 
 			devpriv->ai_cfg |= PCI9118_AI_CFG_AM;
 			outl(devpriv->ai_cfg, dev->iobase + PCI9118_AI_CFG_REG);
-			pci9118_timer_set_mode(dev, 0, I8254_MODE0);
-			pci9118_timer_write(dev, 0, dmabuf->hw >> 1);
+			comedi_8254_load(pacer, 0, dmabuf->hw >> 1,
+					 I8254_MODE0 | I8254_BINARY);
 			devpriv->ai_cfg |= PCI9118_AI_CFG_START;
 		}
 	}
@@ -1133,8 +1099,8 @@ static int pci9118_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 				      &cmd->scan_begin_arg, &cmd->convert_arg,
 				      devpriv->ai_flags,
 				      devpriv->ai_n_realscanlen,
-				      &devpriv->ai_divisor1,
-				      &devpriv->ai_divisor2,
+				      &pacer->divisor1,
+				      &pacer->divisor2,
 				      devpriv->ai_add_front);
 
 		devpriv->ai_ctrl |= PCI9118_AI_CTRL_TMRTR;
@@ -1162,8 +1128,6 @@ static int pci9118_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	if (devpriv->usedma)
 		devpriv->ai_ctrl |= PCI9118_AI_CTRL_DMA;
 
-	pci9118_start_pacer(dev, -1);	/* stop pacer */
-
 	/* set default config (disable burst and triggers) */
 	devpriv->ai_cfg = PCI9118_AI_CFG_PDTRG | PCI9118_AI_CFG_PETRG;
 	outl(devpriv->ai_cfg, dev->iobase + PCI9118_AI_CFG_REG);
@@ -1206,25 +1170,24 @@ static int pci9118_ai_cmdtest(struct comedi_device *dev,
 	int err = 0;
 	unsigned int flags;
 	unsigned int arg;
-	unsigned int divisor1 = 0, divisor2 = 0;
 
 	/* Step 1 : check if triggers are trivially valid */
 
-	err |= cfc_check_trigger_src(&cmd->start_src,
+	err |= comedi_check_trigger_src(&cmd->start_src,
 					TRIG_NOW | TRIG_EXT | TRIG_INT);
 
 	flags = TRIG_FOLLOW;
 	if (devpriv->master)
 		flags |= TRIG_TIMER | TRIG_EXT;
-	err |= cfc_check_trigger_src(&cmd->scan_begin_src, flags);
+	err |= comedi_check_trigger_src(&cmd->scan_begin_src, flags);
 
 	flags = TRIG_TIMER | TRIG_EXT;
 	if (devpriv->master)
 		flags |= TRIG_NOW;
-	err |= cfc_check_trigger_src(&cmd->convert_src, flags);
+	err |= comedi_check_trigger_src(&cmd->convert_src, flags);
 
-	err |= cfc_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
-	err |= cfc_check_trigger_src(&cmd->stop_src,
+	err |= comedi_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
+	err |= comedi_check_trigger_src(&cmd->stop_src,
 					TRIG_COUNT | TRIG_NONE | TRIG_EXT);
 
 	if (err)
@@ -1232,10 +1195,10 @@ static int pci9118_ai_cmdtest(struct comedi_device *dev,
 
 	/* Step 2a : make sure trigger sources are unique */
 
-	err |= cfc_check_trigger_is_unique(cmd->start_src);
-	err |= cfc_check_trigger_is_unique(cmd->scan_begin_src);
-	err |= cfc_check_trigger_is_unique(cmd->convert_src);
-	err |= cfc_check_trigger_is_unique(cmd->stop_src);
+	err |= comedi_check_trigger_is_unique(cmd->start_src);
+	err |= comedi_check_trigger_is_unique(cmd->scan_begin_src);
+	err |= comedi_check_trigger_is_unique(cmd->convert_src);
+	err |= comedi_check_trigger_is_unique(cmd->stop_src);
 
 	/* Step 2b : and mutually compatible */
 
@@ -1264,7 +1227,7 @@ static int pci9118_ai_cmdtest(struct comedi_device *dev,
 	switch (cmd->start_src) {
 	case TRIG_NOW:
 	case TRIG_EXT:
-		err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
+		err |= comedi_check_trigger_arg_is(&cmd->start_arg, 0);
 		break;
 	case TRIG_INT:
 		/* start_arg is the internal trigger (any value) */
@@ -1272,7 +1235,7 @@ static int pci9118_ai_cmdtest(struct comedi_device *dev,
 	}
 
 	if (cmd->scan_begin_src & (TRIG_FOLLOW | TRIG_EXT))
-		err |= cfc_check_trigger_arg_is(&cmd->scan_begin_arg, 0);
+		err |= comedi_check_trigger_arg_is(&cmd->scan_begin_arg, 0);
 
 	if ((cmd->scan_begin_src == TRIG_TIMER) &&
 	    (cmd->convert_src == TRIG_TIMER) && (cmd->scan_end_arg == 1)) {
@@ -1281,34 +1244,37 @@ static int pci9118_ai_cmdtest(struct comedi_device *dev,
 		cmd->scan_begin_arg = 0;
 	}
 
-	if (cmd->scan_begin_src == TRIG_TIMER)
-		err |= cfc_check_trigger_arg_min(&cmd->scan_begin_arg,
-						 devpriv->ai_ns_min);
+	if (cmd->scan_begin_src == TRIG_TIMER) {
+		err |= comedi_check_trigger_arg_min(&cmd->scan_begin_arg,
+						    devpriv->ai_ns_min);
+	}
 
-	if (cmd->scan_begin_src == TRIG_EXT)
+	if (cmd->scan_begin_src == TRIG_EXT) {
 		if (cmd->scan_begin_arg) {
 			cmd->scan_begin_arg = 0;
 			err |= -EINVAL;
-			err |= cfc_check_trigger_arg_max(&cmd->scan_end_arg,
-							 65535);
+			err |= comedi_check_trigger_arg_max(&cmd->scan_end_arg,
+							    65535);
 		}
+	}
 
-	if (cmd->convert_src & (TRIG_TIMER | TRIG_NOW))
-		err |= cfc_check_trigger_arg_min(&cmd->convert_arg,
-						 devpriv->ai_ns_min);
+	if (cmd->convert_src & (TRIG_TIMER | TRIG_NOW)) {
+		err |= comedi_check_trigger_arg_min(&cmd->convert_arg,
+						    devpriv->ai_ns_min);
+	}
 
 	if (cmd->convert_src == TRIG_EXT)
-		err |= cfc_check_trigger_arg_is(&cmd->convert_arg, 0);
+		err |= comedi_check_trigger_arg_is(&cmd->convert_arg, 0);
 
 	if (cmd->stop_src == TRIG_COUNT)
-		err |= cfc_check_trigger_arg_min(&cmd->stop_arg, 1);
+		err |= comedi_check_trigger_arg_min(&cmd->stop_arg, 1);
 	else	/* TRIG_NONE */
-		err |= cfc_check_trigger_arg_is(&cmd->stop_arg, 0);
+		err |= comedi_check_trigger_arg_is(&cmd->stop_arg, 0);
 
-	err |= cfc_check_trigger_arg_min(&cmd->chanlist_len, 1);
+	err |= comedi_check_trigger_arg_min(&cmd->chanlist_len, 1);
 
-	err |= cfc_check_trigger_arg_min(&cmd->scan_end_arg,
-					 cmd->chanlist_len);
+	err |= comedi_check_trigger_arg_min(&cmd->scan_end_arg,
+					    cmd->chanlist_len);
 
 	if ((cmd->scan_end_arg % cmd->chanlist_len)) {
 		cmd->scan_end_arg =
@@ -1323,18 +1289,14 @@ static int pci9118_ai_cmdtest(struct comedi_device *dev,
 
 	if (cmd->scan_begin_src == TRIG_TIMER) {
 		arg = cmd->scan_begin_arg;
-		i8253_cascade_ns_to_timer(I8254_OSC_BASE_4MHZ,
-					  &divisor1, &divisor2,
-					  &arg, cmd->flags);
-		err |= cfc_check_trigger_arg_is(&cmd->scan_begin_arg, arg);
+		comedi_8254_cascade_ns_to_timer(dev->pacer, &arg, cmd->flags);
+		err |= comedi_check_trigger_arg_is(&cmd->scan_begin_arg, arg);
 	}
 
 	if (cmd->convert_src & (TRIG_TIMER | TRIG_NOW)) {
 		arg = cmd->convert_arg;
-		i8253_cascade_ns_to_timer(I8254_OSC_BASE_4MHZ,
-					  &divisor1, &divisor2,
-					  &arg, cmd->flags);
-		err |= cfc_check_trigger_arg_is(&cmd->convert_arg, arg);
+		comedi_8254_cascade_ns_to_timer(dev->pacer, &arg, cmd->flags);
+		err |= comedi_check_trigger_arg_is(&cmd->convert_arg, arg);
 
 		if (cmd->scan_begin_src == TRIG_TIMER &&
 		    cmd->convert_src == TRIG_NOW) {
@@ -1344,8 +1306,9 @@ static int pci9118_ai_cmdtest(struct comedi_device *dev,
 			} else {
 				arg = cmd->convert_arg * cmd->chanlist_len;
 			}
-			err |= cfc_check_trigger_arg_min(&cmd->scan_begin_arg,
-							 arg);
+			err |= comedi_check_trigger_arg_min(&cmd->
+							    scan_begin_arg,
+							    arg);
 		}
 	}
 
@@ -1482,10 +1445,6 @@ static void pci9118_reset(struct comedi_device *dev)
 	inl(dev->iobase + PCI9118_INT_CTRL_REG);
 	inl(dev->iobase + PCI9118_AI_STATUS_REG);
 
-	/* reset and stop counters */
-	pci9118_timer_set_mode(dev, 0, I8254_MODE0);
-	pci9118_start_pacer(dev, 0);
-
 	/* reset DMA and scan queue */
 	outl(0, dev->iobase + PCI9118_AI_BURST_NUM_REG);
 	outl(1, dev->iobase + PCI9118_AI_AUTOSCAN_MODE_REG);
@@ -1590,6 +1549,11 @@ static int pci9118_common_attach(struct comedi_device *dev,
 	devpriv->iobase_a = pci_resource_start(pcidev, 0);
 	dev->iobase = pci_resource_start(pcidev, 2);
 
+	dev->pacer = comedi_8254_init(dev->iobase + PCI9118_TIMER_BASE,
+				      I8254_OSC_BASE_4MHZ, I8254_IO32, 0);
+	if (!dev->pacer)
+		return -ENOMEM;
+
 	pci9118_reset(dev);
 
 	if (pcidev->irq) {
diff --git a/drivers/staging/comedi/drivers/adq12b.c b/drivers/staging/comedi/drivers/adq12b.c
index 8b15cbec9891..bc5f97f50f9a 100644
--- a/drivers/staging/comedi/drivers/adq12b.c
+++ b/drivers/staging/comedi/drivers/adq12b.c
@@ -69,8 +69,6 @@ If you do not specify any options, they will default to
 
    13-oct-2007
      + first try
-
-
 */
 
 #include <linux/module.h>
@@ -170,7 +168,6 @@ static int adq12b_di_insn_bits(struct comedi_device *dev,
 			       struct comedi_subdevice *s,
 			       struct comedi_insn *insn, unsigned int *data)
 {
-
 	/* only bits 0-4 have information about digital inputs */
 	data[1] = (inb(dev->iobase + ADQ12B_STINR) & ADQ12B_STINR_IN_MASK);
 
diff --git a/drivers/staging/comedi/drivers/adv_pci1710.c b/drivers/staging/comedi/drivers/adv_pci1710.c
index 9800c01e6fb9..0c6aa964c884 100644
--- a/drivers/staging/comedi/drivers/adv_pci1710.c
+++ b/drivers/staging/comedi/drivers/adv_pci1710.c
@@ -4,7 +4,7 @@
  * Author: Michal Dobes <dobes@tesnet.cz>
  *
  * Thanks to ZhenGang Shang <ZhenGang.Shang@Advantech.com.cn>
- * for testing and informations.
+ * for testing and information.
  *
  *  hardware driver for Advantech cards:
  *   card:   PCI-1710, PCI-1710HG, PCI-1711, PCI-1713, PCI-1720, PCI-1731
@@ -42,13 +42,11 @@ Configuration options:
 */
 
 #include <linux/module.h>
-#include <linux/pci.h>
 #include <linux/interrupt.h>
 
-#include "../comedidev.h"
+#include "../comedi_pci.h"
 
-#include "comedi_fc.h"
-#include "8253.h"
+#include "comedi_8254.h"
 #include "amcc_s5933.h"
 
 #define PCI171x_AD_DATA	 0	/* R:   A/D data */
@@ -67,11 +65,6 @@ Configuration options:
 
 #define PCI171X_TIMER_BASE	0x18
 
-#define PCI171x_CNT0	24	/* R/W: 8254 counter 0 */
-#define PCI171x_CNT1	26	/* R/W: 8254 counter 1 */
-#define PCI171x_CNT2	28	/* R/W: 8254 counter 2 */
-#define PCI171x_CNTCTRL	30	/* W:   8254 counter control */
-
 /* upper bits from status register (PCI171x_STATUS) (lower is same with control
  * reg) */
 #define	Status_FE	0x0100	/* 1=FIFO is empty */
@@ -87,16 +80,6 @@ Configuration options:
 #define Control_EXT	0x0004	/* 1=external trigger source */
 #define Control_PACER	0x0002	/* 1=enable internal 8254 trigger source */
 #define Control_SW	0x0001	/* 1=enable software trigger source */
-/* bits from counter control register (PCI171x_CNTCTRL) */
-#define Counter_BCD     0x0001	/* 0 = binary counter, 1 = BCD counter */
-#define Counter_M0      0x0002	/* M0-M2 select modes 0-5 */
-#define Counter_M1      0x0004	/* 000 = mode 0, 010 = mode 2 ... */
-#define Counter_M2      0x0008
-#define Counter_RW0     0x0010	/* RW0/RW1 select read/write mode */
-#define Counter_RW1     0x0020
-#define Counter_SC0     0x0040	/* Select Counter. Only 00 or 11 may */
-#define Counter_SC1     0x0080	/* be used, 00 for CNT0,
-				 * 11 for read-back command */
 
 #define PCI1720_DA0	 0	/* W:   D/A register 0 */
 #define PCI1720_DA1	 2	/* W:   D/A register 1 */
@@ -265,15 +248,9 @@ struct pci1710_private {
 	unsigned char ai_et;
 	unsigned int ai_et_CntrlReg;
 	unsigned int ai_et_MuxVal;
-	unsigned int next_divisor1;
-	unsigned int next_divisor2;
-	unsigned int divisor1;
-	unsigned int divisor2;
 	unsigned int act_chanlist[32];	/*  list of scanned channel */
 	unsigned char saved_seglen;	/* len of the non-repeating chanlist */
 	unsigned char da_ranges;	/*  copy of D/A outpit range register */
-	unsigned int cnt0_write_wait;	/* after a write, wait for update of the
-					 * internal state */
 };
 
 static int pci171x_ai_check_chanlist(struct comedi_device *dev,
@@ -426,7 +403,6 @@ static int pci171x_ai_insn_read(struct comedi_device *dev,
 				unsigned int *data)
 {
 	struct pci1710_private *devpriv = dev->private;
-	unsigned int chan = CR_CHAN(insn->chanspec);
 	int ret = 0;
 	int i;
 
@@ -447,7 +423,7 @@ static int pci171x_ai_insn_read(struct comedi_device *dev,
 		if (ret)
 			break;
 
-		ret = pci171x_ai_read_sample(dev, s, chan, &val);
+		ret = pci171x_ai_read_sample(dev, s, 0, &val);
 		if (ret)
 			break;
 
@@ -509,105 +485,6 @@ static int pci171x_do_insn_bits(struct comedi_device *dev,
 	return insn->n;
 }
 
-static void pci171x_start_pacer(struct comedi_device *dev,
-				bool load_counters)
-{
-	struct pci1710_private *devpriv = dev->private;
-	unsigned long timer_base = dev->iobase + PCI171X_TIMER_BASE;
-
-	i8254_set_mode(timer_base, 1, 2, I8254_MODE2 | I8254_BINARY);
-	i8254_set_mode(timer_base, 1, 1, I8254_MODE2 | I8254_BINARY);
-
-	if (load_counters) {
-		i8254_write(timer_base, 1, 2, devpriv->divisor2);
-		i8254_write(timer_base, 1, 1, devpriv->divisor1);
-	}
-}
-
-static int pci171x_counter_insn_read(struct comedi_device *dev,
-				     struct comedi_subdevice *s,
-				     struct comedi_insn *insn,
-				     unsigned int *data)
-{
-	unsigned int msb, lsb, ccntrl;
-	int i;
-
-	ccntrl = 0xD2;		/* count only */
-	for (i = 0; i < insn->n; i++) {
-		outw(ccntrl, dev->iobase + PCI171x_CNTCTRL);
-
-		lsb = inw(dev->iobase + PCI171x_CNT0) & 0xFF;
-		msb = inw(dev->iobase + PCI171x_CNT0) & 0xFF;
-
-		data[0] = lsb | (msb << 8);
-	}
-
-	return insn->n;
-}
-
-static int pci171x_counter_insn_write(struct comedi_device *dev,
-				      struct comedi_subdevice *s,
-				      struct comedi_insn *insn,
-				      unsigned int *data)
-{
-	struct pci1710_private *devpriv = dev->private;
-	uint msb, lsb, ccntrl, status;
-
-	lsb = data[0] & 0x00FF;
-	msb = (data[0] & 0xFF00) >> 8;
-
-	/* write lsb, then msb */
-	outw(lsb, dev->iobase + PCI171x_CNT0);
-	outw(msb, dev->iobase + PCI171x_CNT0);
-
-	if (devpriv->cnt0_write_wait) {
-		/* wait for the new count to be loaded */
-		ccntrl = 0xE2;
-		do {
-			outw(ccntrl, dev->iobase + PCI171x_CNTCTRL);
-			status = inw(dev->iobase + PCI171x_CNT0) & 0xFF;
-		} while (status & 0x40);
-	}
-
-	return insn->n;
-}
-
-static int pci171x_counter_insn_config(struct comedi_device *dev,
-				       struct comedi_subdevice *s,
-				       struct comedi_insn *insn,
-				       unsigned int *data)
-{
-#ifdef unused
-	/* This doesn't work like a normal Comedi counter config */
-	struct pci1710_private *devpriv = dev->private;
-	uint ccntrl = 0;
-
-	devpriv->cnt0_write_wait = data[0] & 0x20;
-
-	/* internal or external clock? */
-	if (!(data[0] & 0x10)) {	/* internal */
-		devpriv->CntrlReg &= ~Control_CNT0;
-	} else {
-		devpriv->CntrlReg |= Control_CNT0;
-	}
-	outw(devpriv->CntrlReg, dev->iobase + PCI171x_CONTROL);
-
-	if (data[0] & 0x01)
-		ccntrl |= Counter_M0;
-	if (data[0] & 0x02)
-		ccntrl |= Counter_M1;
-	if (data[0] & 0x04)
-		ccntrl |= Counter_M2;
-	if (data[0] & 0x08)
-		ccntrl |= Counter_BCD;
-	ccntrl |= Counter_RW0;	/* set read/write mode */
-	ccntrl |= Counter_RW1;
-	outw(ccntrl, dev->iobase + PCI171x_CNTCTRL);
-#endif
-
-	return 1;
-}
-
 static int pci1720_ao_insn_write(struct comedi_device *dev,
 				 struct comedi_subdevice *s,
 				 struct comedi_insn *insn,
@@ -647,7 +524,7 @@ static int pci171x_ai_cancel(struct comedi_device *dev,
 	devpriv->CntrlReg |= Control_SW;
 	/* reset any operations */
 	outw(devpriv->CntrlReg, dev->iobase + PCI171x_CONTROL);
-	pci171x_start_pacer(dev, false);
+	comedi_8254_pacer_enable(dev->pacer, 1, 2, false);
 	outb(0, dev->iobase + PCI171x_CLRFIFO);
 	outb(0, dev->iobase + PCI171x_CLRINT);
 
@@ -768,7 +645,7 @@ static irqreturn_t interrupt_service_pci1710(int irq, void *d)
 		outb(0, dev->iobase + PCI171x_CLRINT);
 		outw(devpriv->ai_et_MuxVal, dev->iobase + PCI171x_MUX);
 		outw(devpriv->CntrlReg, dev->iobase + PCI171x_CONTROL);
-		pci171x_start_pacer(dev, true);
+		comedi_8254_pacer_enable(dev->pacer, 1, 2, true);
 		return IRQ_HANDLED;
 	}
 
@@ -787,8 +664,6 @@ static int pci171x_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	struct pci1710_private *devpriv = dev->private;
 	struct comedi_cmd *cmd = &s->async->cmd;
 
-	pci171x_start_pacer(dev, false);
-
 	pci171x_ai_setup_chanlist(dev, s, cmd->chanlist, cmd->chanlist_len,
 				  devpriv->saved_seglen);
 
@@ -799,10 +674,9 @@ static int pci171x_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	if ((cmd->flags & CMDF_WAKE_EOS) == 0)
 		devpriv->CntrlReg |= Control_ONEFH;
 
-	devpriv->divisor1 = devpriv->next_divisor1;
-	devpriv->divisor2 = devpriv->next_divisor2;
-
 	if (cmd->convert_src == TRIG_TIMER) {
+		comedi_8254_update_divisors(dev->pacer);
+
 		devpriv->CntrlReg |= Control_PACER | Control_IRQEN;
 		if (cmd->start_src == TRIG_EXT) {
 			devpriv->ai_et_CntrlReg = devpriv->CntrlReg;
@@ -816,7 +690,7 @@ static int pci171x_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 		outw(devpriv->CntrlReg, dev->iobase + PCI171x_CONTROL);
 
 		if (cmd->start_src == TRIG_NOW)
-			pci171x_start_pacer(dev, true);
+			comedi_8254_pacer_enable(dev->pacer, 1, 2, true);
 	} else {	/* TRIG_EXT */
 		devpriv->CntrlReg |= Control_EXT | Control_IRQEN;
 		outw(devpriv->CntrlReg, dev->iobase + PCI171x_CONTROL);
@@ -829,26 +703,25 @@ static int pci171x_ai_cmdtest(struct comedi_device *dev,
 			      struct comedi_subdevice *s,
 			      struct comedi_cmd *cmd)
 {
-	struct pci1710_private *devpriv = dev->private;
 	int err = 0;
-	unsigned int arg;
 
 	/* Step 1 : check if triggers are trivially valid */
 
-	err |= cfc_check_trigger_src(&cmd->start_src, TRIG_NOW | TRIG_EXT);
-	err |= cfc_check_trigger_src(&cmd->scan_begin_src, TRIG_FOLLOW);
-	err |= cfc_check_trigger_src(&cmd->convert_src, TRIG_TIMER | TRIG_EXT);
-	err |= cfc_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
-	err |= cfc_check_trigger_src(&cmd->stop_src, TRIG_COUNT | TRIG_NONE);
+	err |= comedi_check_trigger_src(&cmd->start_src, TRIG_NOW | TRIG_EXT);
+	err |= comedi_check_trigger_src(&cmd->scan_begin_src, TRIG_FOLLOW);
+	err |= comedi_check_trigger_src(&cmd->convert_src,
+					TRIG_TIMER | TRIG_EXT);
+	err |= comedi_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
+	err |= comedi_check_trigger_src(&cmd->stop_src, TRIG_COUNT | TRIG_NONE);
 
 	if (err)
 		return 1;
 
 	/* step 2a: make sure trigger sources are unique */
 
-	err |= cfc_check_trigger_is_unique(cmd->start_src);
-	err |= cfc_check_trigger_is_unique(cmd->convert_src);
-	err |= cfc_check_trigger_is_unique(cmd->stop_src);
+	err |= comedi_check_trigger_is_unique(cmd->start_src);
+	err |= comedi_check_trigger_is_unique(cmd->convert_src);
+	err |= comedi_check_trigger_is_unique(cmd->stop_src);
 
 	/* step 2b: and mutually compatible */
 
@@ -857,20 +730,21 @@ static int pci171x_ai_cmdtest(struct comedi_device *dev,
 
 	/* Step 3: check if arguments are trivially valid */
 
-	err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
-	err |= cfc_check_trigger_arg_is(&cmd->scan_begin_arg, 0);
+	err |= comedi_check_trigger_arg_is(&cmd->start_arg, 0);
+	err |= comedi_check_trigger_arg_is(&cmd->scan_begin_arg, 0);
 
 	if (cmd->convert_src == TRIG_TIMER)
-		err |= cfc_check_trigger_arg_min(&cmd->convert_arg, 10000);
+		err |= comedi_check_trigger_arg_min(&cmd->convert_arg, 10000);
 	else	/* TRIG_FOLLOW */
-		err |= cfc_check_trigger_arg_is(&cmd->convert_arg, 0);
+		err |= comedi_check_trigger_arg_is(&cmd->convert_arg, 0);
 
-	err |= cfc_check_trigger_arg_is(&cmd->scan_end_arg, cmd->chanlist_len);
+	err |= comedi_check_trigger_arg_is(&cmd->scan_end_arg,
+					   cmd->chanlist_len);
 
 	if (cmd->stop_src == TRIG_COUNT)
-		err |= cfc_check_trigger_arg_min(&cmd->stop_arg, 1);
+		err |= comedi_check_trigger_arg_min(&cmd->stop_arg, 1);
 	else	/* TRIG_NONE */
-		err |= cfc_check_trigger_arg_is(&cmd->stop_arg, 0);
+		err |= comedi_check_trigger_arg_is(&cmd->stop_arg, 0);
 
 	if (err)
 		return 3;
@@ -878,12 +752,10 @@ static int pci171x_ai_cmdtest(struct comedi_device *dev,
 	/* step 4: fix up any arguments */
 
 	if (cmd->convert_src == TRIG_TIMER) {
-		arg = cmd->convert_arg;
-		i8253_cascade_ns_to_timer(I8254_OSC_BASE_10MHZ,
-					  &devpriv->next_divisor1,
-					  &devpriv->next_divisor2,
-					  &arg, cmd->flags);
-		err |= cfc_check_trigger_arg_is(&cmd->convert_arg, arg);
+		unsigned int arg = cmd->convert_arg;
+
+		comedi_8254_cascade_ns_to_timer(dev->pacer, &arg, cmd->flags);
+		err |= comedi_check_trigger_arg_is(&cmd->convert_arg, arg);
 	}
 
 	if (err)
@@ -899,19 +771,54 @@ static int pci171x_ai_cmdtest(struct comedi_device *dev,
 	return 0;
 }
 
+static int pci171x_insn_counter_config(struct comedi_device *dev,
+				       struct comedi_subdevice *s,
+				       struct comedi_insn *insn,
+				       unsigned int *data)
+{
+	struct pci1710_private *devpriv = dev->private;
+
+	switch (data[0]) {
+	case INSN_CONFIG_SET_CLOCK_SRC:
+		switch (data[1]) {
+		case 0:	/* internal */
+			devpriv->ai_et_CntrlReg &= ~Control_CNT0;
+			break;
+		case 1:	/* external */
+			devpriv->ai_et_CntrlReg |= Control_CNT0;
+			break;
+		default:
+			return -EINVAL;
+		}
+		outw(devpriv->ai_et_CntrlReg, dev->iobase + PCI171x_CONTROL);
+		break;
+	case INSN_CONFIG_GET_CLOCK_SRC:
+		if (devpriv->ai_et_CntrlReg & Control_CNT0) {
+			data[1] = 1;
+			data[2] = 0;
+		} else {
+			data[1] = 0;
+			data[2] = I8254_OSC_BASE_10MHZ;
+		}
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return insn->n;
+}
+
 static int pci171x_reset(struct comedi_device *dev)
 {
 	const struct boardtype *board = dev->board_ptr;
 	struct pci1710_private *devpriv = dev->private;
 
-	outw(0x30, dev->iobase + PCI171x_CNTCTRL);
 	/* Software trigger, CNT0=external */
 	devpriv->CntrlReg = Control_SW | Control_CNT0;
 	/* reset any operations */
 	outw(devpriv->CntrlReg, dev->iobase + PCI171x_CONTROL);
 	outb(0, dev->iobase + PCI171x_CLRFIFO);	/*  clear FIFO */
 	outb(0, dev->iobase + PCI171x_CLRINT);	/*  clear INT request */
-	pci171x_start_pacer(dev, false);
 	devpriv->da_ranges = 0;
 	if (board->has_ao) {
 		/* set DACs to 0..5V */
@@ -978,6 +885,11 @@ static int pci1710_auto_attach(struct comedi_device *dev,
 		return ret;
 	dev->iobase = pci_resource_start(pcidev, 2);
 
+	dev->pacer = comedi_8254_init(dev->iobase + PCI171X_TIMER_BASE,
+				      I8254_OSC_BASE_10MHZ, I8254_IO16, 0);
+	if (!dev->pacer)
+		return -ENOMEM;
+
 	n_subdevices = 0;
 	if (board->n_aichan)
 		n_subdevices++;
@@ -1074,16 +986,17 @@ static int pci1710_auto_attach(struct comedi_device *dev,
 		subdev++;
 	}
 
+	/* Counter subdevice (8254) */
 	if (board->has_counter) {
 		s = &dev->subdevices[subdev];
-		s->type		= COMEDI_SUBD_COUNTER;
-		s->subdev_flags	= SDF_READABLE | SDF_WRITABLE;
-		s->n_chan	= 1;
-		s->maxdata	= 0xffff;
-		s->range_table	= &range_unknown;
-		s->insn_read	= pci171x_counter_insn_read;
-		s->insn_write	= pci171x_counter_insn_write;
-		s->insn_config	= pci171x_counter_insn_config;
+		comedi_8254_subdevice_init(s, dev->pacer);
+
+		dev->pacer->insn_config = pci171x_insn_counter_config;
+
+		/* counters 1 and 2 are used internally for the pacer */
+		comedi_8254_set_busy(dev->pacer, 1, true);
+		comedi_8254_set_busy(dev->pacer, 2, true);
+
 		subdev++;
 	}
 
diff --git a/drivers/staging/comedi/drivers/adv_pci1723.c b/drivers/staging/comedi/drivers/adv_pci1723.c
index f1945be89eff..1921a97cc5ca 100644
--- a/drivers/staging/comedi/drivers/adv_pci1723.c
+++ b/drivers/staging/comedi/drivers/adv_pci1723.c
@@ -41,9 +41,8 @@
  */
 
 #include <linux/module.h>
-#include <linux/pci.h>
 
-#include "../comedidev.h"
+#include "../comedi_pci.h"
 
 /*
  * PCI Bar 2 I/O Register map (dev->iobase)
diff --git a/drivers/staging/comedi/drivers/adv_pci1724.c b/drivers/staging/comedi/drivers/adv_pci1724.c
index a3573ea6f9c0..f7a7dab013db 100644
--- a/drivers/staging/comedi/drivers/adv_pci1724.c
+++ b/drivers/staging/comedi/drivers/adv_pci1724.c
@@ -47,9 +47,8 @@
  */
 
 #include <linux/module.h>
-#include <linux/pci.h>
 
-#include "../comedidev.h"
+#include "../comedi_pci.h"
 
 /*
  * PCI bar 2 Register I/O map (dev->iobase)
diff --git a/drivers/staging/comedi/drivers/adv_pci_dio.c b/drivers/staging/comedi/drivers/adv_pci_dio.c
index 09609d6d02da..456e87013063 100644
--- a/drivers/staging/comedi/drivers/adv_pci_dio.c
+++ b/drivers/staging/comedi/drivers/adv_pci_dio.c
@@ -30,13 +30,12 @@ Configuration options:
 */
 
 #include <linux/module.h>
-#include <linux/pci.h>
 #include <linux/delay.h>
 
-#include "../comedidev.h"
+#include "../comedi_pci.h"
 
 #include "8255.h"
-#include "8253.h"
+#include "comedi_8254.h"
 
 /* hardware types of the cards */
 enum hw_cards_id {
@@ -60,12 +59,6 @@ enum hw_io_access {
 #define MAX_DO_SUBDEVS	2	/* max number of DO subdevices per card */
 #define MAX_DIO_SUBDEVG	2	/* max number of DIO subdevices group per
 				 * card */
-#define MAX_8254_SUBDEVS   1	/* max number of 8254 counter subdevs per
-				 * card */
-				/* (could be more than one 8254 per
-				 * subdevice) */
-
-#define SIZE_8254	   4	/* 8254 IO space length */
 
 #define PCIDIO_MAINREG	   2	/* main I/O region for all Advantech cards? */
 
@@ -230,7 +223,7 @@ struct diosubd_data {
 	int chans;		/*  num of chans */
 	int addr;		/*  PCI address ofset */
 	int regs;		/*  number of registers to read or 8255
-				    subdevices or 8254 chips */
+				    subdevices */
 	unsigned int specflags;	/*  addon subdevice flags */
 };
 
@@ -243,7 +236,7 @@ struct dio_boardtype {
 	struct diosubd_data sdo[MAX_DO_SUBDEVS];	/*  DO chans */
 	struct diosubd_data sdio[MAX_DIO_SUBDEVG];	/*  DIO 8255 chans */
 	struct diosubd_data boardid;	/*  card supports board ID switch */
-	struct diosubd_data s8254[MAX_8254_SUBDEVS];	/* 8254 subdevices */
+	unsigned long timer_regbase;
 	enum hw_io_access io_access;
 };
 
@@ -286,7 +279,7 @@ static const struct dio_boardtype boardtypes[] = {
 		.sdi[0]		= { 32, PCI1735_DI, 4, 0, },
 		.sdo[0]		= { 32, PCI1735_DO, 4, 0, },
 		.boardid	= { 4, PCI1735_BOARDID, 1, SDF_INTERNAL, },
-		.s8254[0]	= { 3, PCI1735_C8254, 1, 0, },
+		.timer_regbase	= PCI1735_C8254,
 		.io_access	= IO_8b,
 	},
 	[TYPE_PCI1736] = {
@@ -322,7 +315,7 @@ static const struct dio_boardtype boardtypes[] = {
 		.cardtype	= TYPE_PCI1751,
 		.nsubdevs	= 3,
 		.sdio[0]	= { 48, PCI1751_DIO, 2, 0, },
-		.s8254[0]	= { 3, PCI1751_CNT, 1, 0, },
+		.timer_regbase	= PCI1751_CNT,
 		.io_access	= IO_8b,
 	},
 	[TYPE_PCI1752] = {
@@ -425,7 +418,6 @@ static int pci_dio_insn_bits_di_b(struct comedi_device *dev,
 	for (i = 0; i < d->regs; i++)
 		data[1] |= inb(dev->iobase + d->addr + i) << (8 * i);
 
-
 	return insn->n;
 }
 
@@ -487,83 +479,6 @@ static int pci_dio_insn_bits_do_w(struct comedi_device *dev,
 /*
 ==============================================================================
 */
-static int pci_8254_insn_read(struct comedi_device *dev,
-			      struct comedi_subdevice *s,
-			      struct comedi_insn *insn, unsigned int *data)
-{
-	const struct diosubd_data *d = (const struct diosubd_data *)s->private;
-	unsigned int chan, chip, chipchan;
-	unsigned long flags;
-
-	chan = CR_CHAN(insn->chanspec);	/* channel on subdevice */
-	chip = chan / 3;		/* chip on subdevice */
-	chipchan = chan - (3 * chip);	/* channel on chip on subdevice */
-	spin_lock_irqsave(&s->spin_lock, flags);
-	data[0] = i8254_read(dev->iobase + d->addr + (SIZE_8254 * chip),
-			0, chipchan);
-	spin_unlock_irqrestore(&s->spin_lock, flags);
-	return 1;
-}
-
-/*
-==============================================================================
-*/
-static int pci_8254_insn_write(struct comedi_device *dev,
-			       struct comedi_subdevice *s,
-			       struct comedi_insn *insn, unsigned int *data)
-{
-	const struct diosubd_data *d = (const struct diosubd_data *)s->private;
-	unsigned int chan, chip, chipchan;
-	unsigned long flags;
-
-	chan = CR_CHAN(insn->chanspec);	/* channel on subdevice */
-	chip = chan / 3;		/* chip on subdevice */
-	chipchan = chan - (3 * chip);	/* channel on chip on subdevice */
-	spin_lock_irqsave(&s->spin_lock, flags);
-	i8254_write(dev->iobase + d->addr + (SIZE_8254 * chip),
-			0, chipchan, data[0]);
-	spin_unlock_irqrestore(&s->spin_lock, flags);
-	return 1;
-}
-
-/*
-==============================================================================
-*/
-static int pci_8254_insn_config(struct comedi_device *dev,
-				struct comedi_subdevice *s,
-				struct comedi_insn *insn, unsigned int *data)
-{
-	const struct diosubd_data *d = (const struct diosubd_data *)s->private;
-	unsigned int chan, chip, chipchan;
-	unsigned long iobase;
-	int ret = 0;
-	unsigned long flags;
-
-	chan = CR_CHAN(insn->chanspec);	/* channel on subdevice */
-	chip = chan / 3;		/* chip on subdevice */
-	chipchan = chan - (3 * chip);	/* channel on chip on subdevice */
-	iobase = dev->iobase + d->addr + (SIZE_8254 * chip);
-	spin_lock_irqsave(&s->spin_lock, flags);
-	switch (data[0]) {
-	case INSN_CONFIG_SET_COUNTER_MODE:
-		ret = i8254_set_mode(iobase, 0, chipchan, data[1]);
-		if (ret < 0)
-			ret = -EINVAL;
-		break;
-	case INSN_CONFIG_8254_READ_STATUS:
-		data[1] = i8254_status(iobase, 0, chipchan);
-		break;
-	default:
-		ret = -EINVAL;
-		break;
-	}
-	spin_unlock_irqrestore(&s->spin_lock, flags);
-	return ret < 0 ? ret : insn->n;
-}
-
-/*
-==============================================================================
-*/
 static int pci1760_unchecked_mbxrequest(struct comedi_device *dev,
 					unsigned char *omb, unsigned char *imb,
 					int repeats)
@@ -845,9 +760,6 @@ static int pci_dio_reset(struct comedi_device *dev)
 		outb(0, dev->iobase + PCI1735_DO + 1);
 		outb(0, dev->iobase + PCI1735_DO + 2);
 		outb(0, dev->iobase + PCI1735_DO + 3);
-		i8254_set_mode(dev->iobase + PCI1735_C8254, 0, 0, I8254_MODE0);
-		i8254_set_mode(dev->iobase + PCI1735_C8254, 0, 1, I8254_MODE0);
-		i8254_set_mode(dev->iobase + PCI1735_C8254, 0, 2, I8254_MODE0);
 		break;
 
 	case TYPE_PCI1736:
@@ -1029,26 +941,6 @@ static int pci_dio_add_do(struct comedi_device *dev,
 	return 0;
 }
 
-/*
-==============================================================================
-*/
-static int pci_dio_add_8254(struct comedi_device *dev,
-			    struct comedi_subdevice *s,
-			    const struct diosubd_data *d)
-{
-	s->type = COMEDI_SUBD_COUNTER;
-	s->subdev_flags = SDF_WRITABLE | SDF_READABLE;
-	s->n_chan = d->chans;
-	s->maxdata = 65535;
-	s->len_chanlist = d->chans;
-	s->insn_read = pci_8254_insn_read;
-	s->insn_write = pci_8254_insn_write;
-	s->insn_config = pci_8254_insn_config;
-	s->private = (void *)d;
-
-	return 0;
-}
-
 static unsigned long pci_dio_override_cardtype(struct pci_dev *pcidev,
 					       unsigned long cardtype)
 {
@@ -1144,12 +1036,19 @@ static int pci_dio_auto_attach(struct comedi_device *dev,
 		subdev++;
 	}
 
-	for (i = 0; i < MAX_8254_SUBDEVS; i++)
-		if (this_board->s8254[i].chans) {
-			s = &dev->subdevices[subdev];
-			pci_dio_add_8254(dev, s, &this_board->s8254[i]);
-			subdev++;
-		}
+	if (this_board->timer_regbase) {
+		s = &dev->subdevices[subdev];
+
+		dev->pacer = comedi_8254_init(dev->iobase +
+					      this_board->timer_regbase,
+					      0, I8254_IO8, 0);
+		if (!dev->pacer)
+			return -ENOMEM;
+
+		comedi_8254_subdevice_init(s, dev->pacer);
+
+		subdev++;
+	}
 
 	if (this_board->cardtype == TYPE_PCI1760)
 		pci1760_attach(dev);
diff --git a/drivers/staging/comedi/drivers/aio_iiro_16.c b/drivers/staging/comedi/drivers/aio_iiro_16.c
index 1c7b325a373c..35b2f98f0de9 100644
--- a/drivers/staging/comedi/drivers/aio_iiro_16.c
+++ b/drivers/staging/comedi/drivers/aio_iiro_16.c
@@ -42,8 +42,6 @@
 
 #include "../comedidev.h"
 
-#include "comedi_fc.h"
-
 #define AIO_IIRO_16_RELAY_0_7		0x00
 #define AIO_IIRO_16_INPUT_0_7		0x01
 #define AIO_IIRO_16_IRQ			0x02
@@ -116,11 +114,11 @@ static int aio_iiro_16_cos_cmdtest(struct comedi_device *dev,
 
 	/* Step 1 : check if triggers are trivially valid */
 
-	err |= cfc_check_trigger_src(&cmd->start_src, TRIG_NOW);
-	err |= cfc_check_trigger_src(&cmd->scan_begin_src, TRIG_EXT);
-	err |= cfc_check_trigger_src(&cmd->convert_src, TRIG_FOLLOW);
-	err |= cfc_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
-	err |= cfc_check_trigger_src(&cmd->stop_src, TRIG_NONE);
+	err |= comedi_check_trigger_src(&cmd->start_src, TRIG_NOW);
+	err |= comedi_check_trigger_src(&cmd->scan_begin_src, TRIG_EXT);
+	err |= comedi_check_trigger_src(&cmd->convert_src, TRIG_FOLLOW);
+	err |= comedi_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
+	err |= comedi_check_trigger_src(&cmd->stop_src, TRIG_NONE);
 
 	if (err)
 		return 1;
@@ -130,11 +128,12 @@ static int aio_iiro_16_cos_cmdtest(struct comedi_device *dev,
 
 	/* Step 3: check if arguments are trivially valid */
 
-	err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
-	err |= cfc_check_trigger_arg_is(&cmd->scan_begin_arg, 0);
-	err |= cfc_check_trigger_arg_is(&cmd->convert_arg, 0);
-	err |= cfc_check_trigger_arg_is(&cmd->scan_end_arg, cmd->chanlist_len);
-	err |= cfc_check_trigger_arg_is(&cmd->stop_arg, 0);
+	err |= comedi_check_trigger_arg_is(&cmd->start_arg, 0);
+	err |= comedi_check_trigger_arg_is(&cmd->scan_begin_arg, 0);
+	err |= comedi_check_trigger_arg_is(&cmd->convert_arg, 0);
+	err |= comedi_check_trigger_arg_is(&cmd->scan_end_arg,
+					   cmd->chanlist_len);
+	err |= comedi_check_trigger_arg_is(&cmd->stop_arg, 0);
 
 	if (err)
 		return 3;
diff --git a/drivers/staging/comedi/drivers/amplc_dio200_common.c b/drivers/staging/comedi/drivers/amplc_dio200_common.c
index 26aad705aad3..d15a3dc1216a 100644
--- a/drivers/staging/comedi/drivers/amplc_dio200_common.c
+++ b/drivers/staging/comedi/drivers/amplc_dio200_common.c
@@ -25,19 +25,14 @@
 #include "../comedidev.h"
 
 #include "amplc_dio200.h"
-#include "comedi_fc.h"
-#include "8253.h"
+#include "comedi_8254.h"
 #include "8255.h"		/* only for register defines */
 
 /* 200 series registers */
 #define DIO200_IO_SIZE		0x20
 #define DIO200_PCIE_IO_SIZE	0x4000
-#define DIO200_XCLK_SCE		0x18	/* Group X clock selection register */
-#define DIO200_YCLK_SCE		0x19	/* Group Y clock selection register */
-#define DIO200_ZCLK_SCE		0x1a	/* Group Z clock selection register */
-#define DIO200_XGAT_SCE		0x1b	/* Group X gate selection register */
-#define DIO200_YGAT_SCE		0x1c	/* Group Y gate selection register */
-#define DIO200_ZGAT_SCE		0x1d	/* Group Z gate selection register */
+#define DIO200_CLK_SCE(x)	(0x18 + (x))	/* Group X/Y/Z clock sel reg */
+#define DIO200_GAT_SCE(x)	(0x1b + (x))	/* Group X/Y/Z gate sel reg */
 #define DIO200_INT_SCE		0x1e	/* Interrupt enable/status register */
 /* Extra registers for new PCIe boards */
 #define DIO200_ENHANCE		0x20	/* 1 to enable enhanced features */
@@ -101,16 +96,6 @@ static const unsigned int ts_clock_period[TS_CONFIG_MAX_CLK_SRC + 1] = {
 	1000000,		/* 1 millisecond. */
 };
 
-struct dio200_subdev_8254 {
-	unsigned int ofs;		/* Counter base offset */
-	unsigned int clk_sce_ofs;	/* CLK_SCE base address */
-	unsigned int gat_sce_ofs;	/* GAT_SCE base address */
-	int which;			/* Bit 5 of CLK_SCE or GAT_SCE */
-	unsigned int clock_src[3];	/* Current clock sources */
-	unsigned int gate_src[3];	/* Current gate sources */
-	spinlock_t spinlock;
-};
-
 struct dio200_subdev_8255 {
 	unsigned int ofs;		/* DIO base offset */
 };
@@ -177,6 +162,27 @@ static void dio200_write32(struct comedi_device *dev,
 		outl(val, dev->iobase + offset);
 }
 
+static unsigned int dio200_subdev_8254_offset(struct comedi_device *dev,
+					      struct comedi_subdevice *s)
+{
+	const struct dio200_board *board = dev->board_ptr;
+	struct comedi_8254 *i8254 = s->private;
+	unsigned int offset;
+
+	/* get the offset that was passed to comedi_8254_*_init() */
+	if (dev->mmio)
+		offset = i8254->mmio - dev->mmio;
+	else
+		offset = i8254->iobase - dev->iobase;
+
+	/* remove the shift that was added for PCIe boards */
+	if (board->is_pcie)
+		offset >>= 3;
+
+	/* this offset now works for the dio200_{read,write} helpers */
+	return offset;
+}
+
 static int dio200_subdev_intr_insn_bits(struct comedi_device *dev,
 					struct comedi_subdevice *s,
 					struct comedi_insn *insn,
@@ -366,19 +372,19 @@ static int dio200_subdev_intr_cmdtest(struct comedi_device *dev,
 
 	/* Step 1 : check if triggers are trivially valid */
 
-	err |= cfc_check_trigger_src(&cmd->start_src, TRIG_NOW | TRIG_INT);
-	err |= cfc_check_trigger_src(&cmd->scan_begin_src, TRIG_EXT);
-	err |= cfc_check_trigger_src(&cmd->convert_src, TRIG_NOW);
-	err |= cfc_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
-	err |= cfc_check_trigger_src(&cmd->stop_src, TRIG_COUNT | TRIG_NONE);
+	err |= comedi_check_trigger_src(&cmd->start_src, TRIG_NOW | TRIG_INT);
+	err |= comedi_check_trigger_src(&cmd->scan_begin_src, TRIG_EXT);
+	err |= comedi_check_trigger_src(&cmd->convert_src, TRIG_NOW);
+	err |= comedi_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
+	err |= comedi_check_trigger_src(&cmd->stop_src, TRIG_COUNT | TRIG_NONE);
 
 	if (err)
 		return 1;
 
 	/* Step 2a : make sure trigger sources are unique */
 
-	err |= cfc_check_trigger_is_unique(cmd->start_src);
-	err |= cfc_check_trigger_is_unique(cmd->stop_src);
+	err |= comedi_check_trigger_is_unique(cmd->start_src);
+	err |= comedi_check_trigger_is_unique(cmd->stop_src);
 
 	/* Step 2b : and mutually compatible */
 
@@ -387,15 +393,16 @@ static int dio200_subdev_intr_cmdtest(struct comedi_device *dev,
 
 	/* Step 3: check if arguments are trivially valid */
 
-	err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
-	err |= cfc_check_trigger_arg_is(&cmd->scan_begin_arg, 0);
-	err |= cfc_check_trigger_arg_is(&cmd->convert_arg, 0);
-	err |= cfc_check_trigger_arg_is(&cmd->scan_end_arg, cmd->chanlist_len);
+	err |= comedi_check_trigger_arg_is(&cmd->start_arg, 0);
+	err |= comedi_check_trigger_arg_is(&cmd->scan_begin_arg, 0);
+	err |= comedi_check_trigger_arg_is(&cmd->convert_arg, 0);
+	err |= comedi_check_trigger_arg_is(&cmd->scan_end_arg,
+					   cmd->chanlist_len);
 
 	if (cmd->stop_src == TRIG_COUNT)
-		err |= cfc_check_trigger_arg_min(&cmd->stop_arg, 1);
+		err |= comedi_check_trigger_arg_min(&cmd->stop_arg, 1);
 	else	/* TRIG_NONE */
-		err |= cfc_check_trigger_arg_is(&cmd->stop_arg, 0);
+		err |= comedi_check_trigger_arg_is(&cmd->stop_arg, 0);
 
 	if (err)
 		return 3;
@@ -482,175 +489,26 @@ static irqreturn_t dio200_interrupt(int irq, void *d)
 	return IRQ_RETVAL(handled);
 }
 
-static unsigned int dio200_subdev_8254_read_chan(struct comedi_device *dev,
-						 struct comedi_subdevice *s,
-						 unsigned int chan)
-{
-	struct dio200_subdev_8254 *subpriv = s->private;
-	unsigned int val;
-
-	/* latch counter */
-	val = chan << 6;
-	dio200_write8(dev, subpriv->ofs + i8254_control_reg, val);
-	/* read lsb, msb */
-	val = dio200_read8(dev, subpriv->ofs + chan);
-	val += dio200_read8(dev, subpriv->ofs + chan) << 8;
-	return val;
-}
-
-static void dio200_subdev_8254_write_chan(struct comedi_device *dev,
-					  struct comedi_subdevice *s,
-					  unsigned int chan,
-					  unsigned int count)
-{
-	struct dio200_subdev_8254 *subpriv = s->private;
-
-	/* write lsb, msb */
-	dio200_write8(dev, subpriv->ofs + chan, count & 0xff);
-	dio200_write8(dev, subpriv->ofs + chan, (count >> 8) & 0xff);
-}
-
-static void dio200_subdev_8254_set_mode(struct comedi_device *dev,
-					struct comedi_subdevice *s,
-					unsigned int chan,
-					unsigned int mode)
-{
-	struct dio200_subdev_8254 *subpriv = s->private;
-	unsigned int byte;
-
-	byte = chan << 6;
-	byte |= 0x30;		/* access order: lsb, msb */
-	byte |= (mode & 0xf);	/* counter mode and BCD|binary */
-	dio200_write8(dev, subpriv->ofs + i8254_control_reg, byte);
-}
-
-static unsigned int dio200_subdev_8254_status(struct comedi_device *dev,
-					      struct comedi_subdevice *s,
-					      unsigned int chan)
-{
-	struct dio200_subdev_8254 *subpriv = s->private;
-
-	/* latch status */
-	dio200_write8(dev, subpriv->ofs + i8254_control_reg,
-		      0xe0 | (2 << chan));
-	/* read status */
-	return dio200_read8(dev, subpriv->ofs + chan);
-}
-
-static int dio200_subdev_8254_read(struct comedi_device *dev,
-				   struct comedi_subdevice *s,
-				   struct comedi_insn *insn,
-				   unsigned int *data)
-{
-	struct dio200_subdev_8254 *subpriv = s->private;
-	int chan = CR_CHAN(insn->chanspec);
-	unsigned int n;
-	unsigned long flags;
-
-	for (n = 0; n < insn->n; n++) {
-		spin_lock_irqsave(&subpriv->spinlock, flags);
-		data[n] = dio200_subdev_8254_read_chan(dev, s, chan);
-		spin_unlock_irqrestore(&subpriv->spinlock, flags);
-	}
-	return insn->n;
-}
-
-static int dio200_subdev_8254_write(struct comedi_device *dev,
-				    struct comedi_subdevice *s,
-				    struct comedi_insn *insn,
-				    unsigned int *data)
-{
-	struct dio200_subdev_8254 *subpriv = s->private;
-	int chan = CR_CHAN(insn->chanspec);
-	unsigned int n;
-	unsigned long flags;
-
-	for (n = 0; n < insn->n; n++) {
-		spin_lock_irqsave(&subpriv->spinlock, flags);
-		dio200_subdev_8254_write_chan(dev, s, chan, data[n]);
-		spin_unlock_irqrestore(&subpriv->spinlock, flags);
-	}
-	return insn->n;
-}
-
-static int dio200_subdev_8254_set_gate_src(struct comedi_device *dev,
-					   struct comedi_subdevice *s,
-					   unsigned int counter_number,
-					   unsigned int gate_src)
-{
-	const struct dio200_board *board = dev->board_ptr;
-	struct dio200_subdev_8254 *subpriv = s->private;
-	unsigned char byte;
-
-	if (!board->has_clk_gat_sce)
-		return -1;
-	if (counter_number > 2)
-		return -1;
-	if (gate_src > (board->is_pcie ? 31 : 7))
-		return -1;
-
-	subpriv->gate_src[counter_number] = gate_src;
-	byte = gat_sce(subpriv->which, counter_number, gate_src);
-	dio200_write8(dev, subpriv->gat_sce_ofs, byte);
-
-	return 0;
-}
-
-static int dio200_subdev_8254_get_gate_src(struct comedi_device *dev,
-					   struct comedi_subdevice *s,
-					   unsigned int counter_number)
-{
-	const struct dio200_board *board = dev->board_ptr;
-	struct dio200_subdev_8254 *subpriv = s->private;
-
-	if (!board->has_clk_gat_sce)
-		return -1;
-	if (counter_number > 2)
-		return -1;
-
-	return subpriv->gate_src[counter_number];
-}
-
-static int dio200_subdev_8254_set_clock_src(struct comedi_device *dev,
+static void dio200_subdev_8254_set_gate_src(struct comedi_device *dev,
 					    struct comedi_subdevice *s,
-					    unsigned int counter_number,
-					    unsigned int clock_src)
+					    unsigned int chan,
+					    unsigned int src)
 {
-	const struct dio200_board *board = dev->board_ptr;
-	struct dio200_subdev_8254 *subpriv = s->private;
-	unsigned char byte;
-
-	if (!board->has_clk_gat_sce)
-		return -1;
-	if (counter_number > 2)
-		return -1;
-	if (clock_src > (board->is_pcie ? 31 : 7))
-		return -1;
-
-	subpriv->clock_src[counter_number] = clock_src;
-	byte = clk_sce(subpriv->which, counter_number, clock_src);
-	dio200_write8(dev, subpriv->clk_sce_ofs, byte);
+	unsigned int offset = dio200_subdev_8254_offset(dev, s);
 
-	return 0;
+	dio200_write8(dev, DIO200_GAT_SCE(offset >> 3),
+		      gat_sce((offset >> 2) & 1, chan, src));
 }
 
-static int dio200_subdev_8254_get_clock_src(struct comedi_device *dev,
-					    struct comedi_subdevice *s,
-					    unsigned int counter_number,
-					    unsigned int *period_ns)
+static void dio200_subdev_8254_set_clock_src(struct comedi_device *dev,
+					     struct comedi_subdevice *s,
+					     unsigned int chan,
+					     unsigned int src)
 {
-	const struct dio200_board *board = dev->board_ptr;
-	struct dio200_subdev_8254 *subpriv = s->private;
-	unsigned clock_src;
-
-	if (!board->has_clk_gat_sce)
-		return -1;
-	if (counter_number > 2)
-		return -1;
+	unsigned int offset = dio200_subdev_8254_offset(dev, s);
 
-	clock_src = subpriv->clock_src[counter_number];
-	*period_ns = clock_period[clock_src];
-	return clock_src;
+	dio200_write8(dev, DIO200_CLK_SCE(offset >> 3),
+		      clk_sce((offset >> 2) & 1, chan, src));
 }
 
 static int dio200_subdev_8254_config(struct comedi_device *dev,
@@ -658,54 +516,44 @@ static int dio200_subdev_8254_config(struct comedi_device *dev,
 				     struct comedi_insn *insn,
 				     unsigned int *data)
 {
-	struct dio200_subdev_8254 *subpriv = s->private;
-	int ret = 0;
-	int chan = CR_CHAN(insn->chanspec);
-	unsigned long flags;
+	const struct dio200_board *board = dev->board_ptr;
+	struct comedi_8254 *i8254 = s->private;
+	unsigned int chan = CR_CHAN(insn->chanspec);
+	unsigned int max_src = board->is_pcie ? 31 : 7;
+	unsigned int src;
+
+	if (!board->has_clk_gat_sce)
+		return -EINVAL;
 
-	spin_lock_irqsave(&subpriv->spinlock, flags);
 	switch (data[0]) {
-	case INSN_CONFIG_SET_COUNTER_MODE:
-		if (data[1] > (I8254_MODE5 | I8254_BCD))
-			ret = -EINVAL;
-		else
-			dio200_subdev_8254_set_mode(dev, s, chan, data[1]);
-		break;
-	case INSN_CONFIG_8254_READ_STATUS:
-		data[1] = dio200_subdev_8254_status(dev, s, chan);
-		break;
 	case INSN_CONFIG_SET_GATE_SRC:
-		ret = dio200_subdev_8254_set_gate_src(dev, s, chan, data[2]);
-		if (ret < 0)
-			ret = -EINVAL;
+		src = data[2];
+		if (src > max_src)
+			return -EINVAL;
+
+		dio200_subdev_8254_set_gate_src(dev, s, chan, src);
+		i8254->gate_src[chan] = src;
 		break;
 	case INSN_CONFIG_GET_GATE_SRC:
-		ret = dio200_subdev_8254_get_gate_src(dev, s, chan);
-		if (ret < 0) {
-			ret = -EINVAL;
-			break;
-		}
-		data[2] = ret;
+		data[2] = i8254->gate_src[chan];
 		break;
 	case INSN_CONFIG_SET_CLOCK_SRC:
-		ret = dio200_subdev_8254_set_clock_src(dev, s, chan, data[1]);
-		if (ret < 0)
-			ret = -EINVAL;
+		src = data[1];
+		if (src > max_src)
+			return -EINVAL;
+
+		dio200_subdev_8254_set_clock_src(dev, s, chan, src);
+		i8254->clock_src[chan] = src;
 		break;
 	case INSN_CONFIG_GET_CLOCK_SRC:
-		ret = dio200_subdev_8254_get_clock_src(dev, s, chan, &data[2]);
-		if (ret < 0) {
-			ret = -EINVAL;
-			break;
-		}
-		data[1] = ret;
+		data[1] = i8254->clock_src[chan];
+		data[2] = clock_period[i8254->clock_src[chan]];
 		break;
 	default:
-		ret = -EINVAL;
-		break;
+		return -EINVAL;
 	}
-	spin_unlock_irqrestore(&subpriv->spinlock, flags);
-	return ret < 0 ? ret : insn->n;
+
+	return insn->n;
 }
 
 static int dio200_subdev_8254_init(struct comedi_device *dev,
@@ -713,36 +561,46 @@ static int dio200_subdev_8254_init(struct comedi_device *dev,
 				   unsigned int offset)
 {
 	const struct dio200_board *board = dev->board_ptr;
-	struct dio200_subdev_8254 *subpriv;
-	unsigned int chan;
+	struct comedi_8254 *i8254;
+	unsigned int regshift;
+	int chan;
+
+	/*
+	 * PCIe boards need the offset shifted in order to get the
+	 * correct base address of the timer.
+	 */
+	if (board->is_pcie) {
+		offset <<= 3;
+		regshift = 3;
+	} else {
+		regshift = 0;
+	}
 
-	subpriv = comedi_alloc_spriv(s, sizeof(*subpriv));
-	if (!subpriv)
+	if (dev->mmio)
+		i8254 = comedi_8254_mm_init(dev->mmio + offset,
+					    0, I8254_IO8, regshift);
+	else
+		i8254 = comedi_8254_init(dev->iobase + offset,
+					 0, I8254_IO8, regshift);
+	if (!i8254)
 		return -ENOMEM;
 
-	s->type = COMEDI_SUBD_COUNTER;
-	s->subdev_flags = SDF_WRITABLE | SDF_READABLE;
-	s->n_chan = 3;
-	s->maxdata = 0xFFFF;
-	s->insn_read = dio200_subdev_8254_read;
-	s->insn_write = dio200_subdev_8254_write;
-	s->insn_config = dio200_subdev_8254_config;
+	comedi_8254_subdevice_init(s, i8254);
 
-	spin_lock_init(&subpriv->spinlock);
-	subpriv->ofs = offset;
-	if (board->has_clk_gat_sce) {
-		/* Derive CLK_SCE and GAT_SCE register offsets from
-		 * 8254 offset. */
-		subpriv->clk_sce_ofs = DIO200_XCLK_SCE + (offset >> 3);
-		subpriv->gat_sce_ofs = DIO200_XGAT_SCE + (offset >> 3);
-		subpriv->which = (offset >> 2) & 1;
-	}
+	i8254->insn_config = dio200_subdev_8254_config;
+
+	/*
+	 * There could be multiple timers so this driver does not
+	 * use dev->pacer to save the i8254 pointer. Instead,
+	 * comedi_8254_subdevice_init() saved the i8254 pointer in
+	 * s->private. Set the runflag bit so that the core will
+	 * automatically free it when the driver is detached.
+	 */
+	s->runflags |= COMEDI_SRF_FREE_SPRIV;
 
 	/* Initialize channels. */
-	for (chan = 0; chan < 3; chan++) {
-		dio200_subdev_8254_set_mode(dev, s, chan,
-					    I8254_MODE0 | I8254_BINARY);
-		if (board->has_clk_gat_sce) {
+	if (board->has_clk_gat_sce) {
+		for (chan = 0; chan < 3; chan++) {
 			/* Gate source 0 is VCC (logic 1). */
 			dio200_subdev_8254_set_gate_src(dev, s, chan, 0);
 			/* Clock source 0 is the dedicated clock input. */
diff --git a/drivers/staging/comedi/drivers/amplc_dio200_pci.c b/drivers/staging/comedi/drivers/amplc_dio200_pci.c
index b83d1f5a8fb9..d9850c917163 100644
--- a/drivers/staging/comedi/drivers/amplc_dio200_pci.c
+++ b/drivers/staging/comedi/drivers/amplc_dio200_pci.c
@@ -221,10 +221,9 @@
  */
 
 #include <linux/module.h>
-#include <linux/pci.h>
 #include <linux/interrupt.h>
 
-#include "../comedidev.h"
+#include "../comedi_pci.h"
 
 #include "amplc_dio200.h"
 
diff --git a/drivers/staging/comedi/drivers/amplc_pc236_common.c b/drivers/staging/comedi/drivers/amplc_pc236_common.c
index be87172d1f3f..245f932a75a8 100644
--- a/drivers/staging/comedi/drivers/amplc_pc236_common.c
+++ b/drivers/staging/comedi/drivers/amplc_pc236_common.c
@@ -24,7 +24,6 @@
 #include "../comedidev.h"
 
 #include "amplc_pc236.h"
-#include "comedi_fc.h"
 #include "8255.h"
 
 static void pc236_intr_update(struct comedi_device *dev, bool enable)
@@ -82,11 +81,11 @@ static int pc236_intr_cmdtest(struct comedi_device *dev,
 
 	/* Step 1 : check if triggers are trivially valid */
 
-	err |= cfc_check_trigger_src(&cmd->start_src, TRIG_NOW);
-	err |= cfc_check_trigger_src(&cmd->scan_begin_src, TRIG_EXT);
-	err |= cfc_check_trigger_src(&cmd->convert_src, TRIG_FOLLOW);
-	err |= cfc_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
-	err |= cfc_check_trigger_src(&cmd->stop_src, TRIG_NONE);
+	err |= comedi_check_trigger_src(&cmd->start_src, TRIG_NOW);
+	err |= comedi_check_trigger_src(&cmd->scan_begin_src, TRIG_EXT);
+	err |= comedi_check_trigger_src(&cmd->convert_src, TRIG_FOLLOW);
+	err |= comedi_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
+	err |= comedi_check_trigger_src(&cmd->stop_src, TRIG_NONE);
 
 	if (err)
 		return 1;
@@ -96,11 +95,12 @@ static int pc236_intr_cmdtest(struct comedi_device *dev,
 
 	/* Step 3: check it arguments are trivially valid */
 
-	err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
-	err |= cfc_check_trigger_arg_is(&cmd->scan_begin_arg, 0);
-	err |= cfc_check_trigger_arg_is(&cmd->convert_arg, 0);
-	err |= cfc_check_trigger_arg_is(&cmd->scan_end_arg, cmd->chanlist_len);
-	err |= cfc_check_trigger_arg_is(&cmd->stop_arg, 0);
+	err |= comedi_check_trigger_arg_is(&cmd->start_arg, 0);
+	err |= comedi_check_trigger_arg_is(&cmd->scan_begin_arg, 0);
+	err |= comedi_check_trigger_arg_is(&cmd->convert_arg, 0);
+	err |= comedi_check_trigger_arg_is(&cmd->scan_end_arg,
+					   cmd->chanlist_len);
+	err |= comedi_check_trigger_arg_is(&cmd->stop_arg, 0);
 
 	if (err)
 		return 3;
@@ -195,7 +195,6 @@ static void __exit amplc_pc236_common_exit(void)
 }
 module_exit(amplc_pc236_common_exit);
 
-
 MODULE_AUTHOR("Comedi http://www.comedi.org");
 MODULE_DESCRIPTION("Comedi helper for amplc_pc236 and amplc_pci236");
 MODULE_LICENSE("GPL");
diff --git a/drivers/staging/comedi/drivers/amplc_pci224.c b/drivers/staging/comedi/drivers/amplc_pci224.c
index 924c8298c7a0..08a91854837a 100644
--- a/drivers/staging/comedi/drivers/amplc_pci224.c
+++ b/drivers/staging/comedi/drivers/amplc_pci224.c
@@ -103,22 +103,17 @@
  */
 
 #include <linux/module.h>
-#include <linux/pci.h>
 #include <linux/interrupt.h>
 #include <linux/slab.h>
 
-#include "../comedidev.h"
+#include "../comedi_pci.h"
 
-#include "comedi_fc.h"
-#include "8253.h"
+#include "comedi_8254.h"
 
 /*
  * PCI224/234 i/o space 1 (PCIBAR2) registers.
  */
-#define PCI224_Z2_CT0	0x14	/* 82C54 counter/timer 0 */
-#define PCI224_Z2_CT1	0x15	/* 82C54 counter/timer 1 */
-#define PCI224_Z2_CT2	0x16	/* 82C54 counter/timer 2 */
-#define PCI224_Z2_CTC	0x17	/* 82C54 counter/timer control word */
+#define PCI224_Z2_BASE	0x14	/* 82C54 counter/timer */
 #define PCI224_ZCLK_SCE	0x1A	/* Group Z Clock Configuration Register */
 #define PCI224_ZGAT_SCE	0x1D	/* Group Z Gate Configuration Register */
 #define PCI224_INT_SCE	0x1E	/* ISR Interrupt source mask register */
@@ -379,8 +374,6 @@ struct pci224_private {
 	int intr_cpuid;
 	short intr_running;
 	unsigned short daccon;
-	unsigned int cached_div1;
-	unsigned int cached_div2;
 	unsigned short ao_enab;	/* max 16 channels so 'short' will do */
 	unsigned char intsce;
 };
@@ -451,7 +444,6 @@ static void pci224_ao_stop(struct comedi_device *dev,
 	if (!test_and_clear_bit(AO_CMD_STARTED, &devpriv->state))
 		return;
 
-
 	spin_lock_irqsave(&devpriv->ao_spinlock, flags);
 	/* Kill the interrupts. */
 	devpriv->intsce = 0;
@@ -668,28 +660,27 @@ static int
 pci224_ao_cmdtest(struct comedi_device *dev, struct comedi_subdevice *s,
 		  struct comedi_cmd *cmd)
 {
-	struct pci224_private *devpriv = dev->private;
 	int err = 0;
 	unsigned int arg;
 
 	/* Step 1 : check if triggers are trivially valid */
 
-	err |= cfc_check_trigger_src(&cmd->start_src, TRIG_INT | TRIG_EXT);
-	err |= cfc_check_trigger_src(&cmd->scan_begin_src,
-				     TRIG_EXT | TRIG_TIMER);
-	err |= cfc_check_trigger_src(&cmd->convert_src, TRIG_NOW);
-	err |= cfc_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
-	err |= cfc_check_trigger_src(&cmd->stop_src,
-				     TRIG_COUNT | TRIG_EXT | TRIG_NONE);
+	err |= comedi_check_trigger_src(&cmd->start_src, TRIG_INT | TRIG_EXT);
+	err |= comedi_check_trigger_src(&cmd->scan_begin_src,
+					TRIG_EXT | TRIG_TIMER);
+	err |= comedi_check_trigger_src(&cmd->convert_src, TRIG_NOW);
+	err |= comedi_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
+	err |= comedi_check_trigger_src(&cmd->stop_src,
+					TRIG_COUNT | TRIG_EXT | TRIG_NONE);
 
 	if (err)
 		return 1;
 
 	/* Step 2a : make sure trigger sources are unique */
 
-	err |= cfc_check_trigger_is_unique(cmd->start_src);
-	err |= cfc_check_trigger_is_unique(cmd->scan_begin_src);
-	err |= cfc_check_trigger_is_unique(cmd->stop_src);
+	err |= comedi_check_trigger_is_unique(cmd->start_src);
+	err |= comedi_check_trigger_is_unique(cmd->scan_begin_src);
+	err |= comedi_check_trigger_is_unique(cmd->stop_src);
 
 	/* Step 2b : and mutually compatible */
 
@@ -714,7 +705,7 @@ pci224_ao_cmdtest(struct comedi_device *dev, struct comedi_subdevice *s,
 
 	switch (cmd->start_src) {
 	case TRIG_INT:
-		err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
+		err |= comedi_check_trigger_arg_is(&cmd->start_arg, 0);
 		break;
 	case TRIG_EXT:
 		/* Force to external trigger 0. */
@@ -734,13 +725,13 @@ pci224_ao_cmdtest(struct comedi_device *dev, struct comedi_subdevice *s,
 
 	switch (cmd->scan_begin_src) {
 	case TRIG_TIMER:
-		err |= cfc_check_trigger_arg_max(&cmd->scan_begin_arg,
-						 MAX_SCAN_PERIOD);
+		err |= comedi_check_trigger_arg_max(&cmd->scan_begin_arg,
+						    MAX_SCAN_PERIOD);
 
 		arg = cmd->chanlist_len * CONVERT_PERIOD;
 		if (arg < MIN_SCAN_PERIOD)
 			arg = MIN_SCAN_PERIOD;
-		err |= cfc_check_trigger_arg_min(&cmd->scan_begin_arg, arg);
+		err |= comedi_check_trigger_arg_min(&cmd->scan_begin_arg, arg);
 		break;
 	case TRIG_EXT:
 		/* Force to external trigger 0. */
@@ -760,12 +751,13 @@ pci224_ao_cmdtest(struct comedi_device *dev, struct comedi_subdevice *s,
 		break;
 	}
 
-	err |= cfc_check_trigger_arg_is(&cmd->convert_arg, 0);
-	err |= cfc_check_trigger_arg_is(&cmd->scan_end_arg, cmd->chanlist_len);
+	err |= comedi_check_trigger_arg_is(&cmd->convert_arg, 0);
+	err |= comedi_check_trigger_arg_is(&cmd->scan_end_arg,
+					   cmd->chanlist_len);
 
 	switch (cmd->stop_src) {
 	case TRIG_COUNT:
-		err |= cfc_check_trigger_arg_min(&cmd->stop_arg, 1);
+		err |= comedi_check_trigger_arg_min(&cmd->stop_arg, 1);
 		break;
 	case TRIG_EXT:
 		/* Force to external trigger 0. */
@@ -781,7 +773,7 @@ pci224_ao_cmdtest(struct comedi_device *dev, struct comedi_subdevice *s,
 		}
 		break;
 	case TRIG_NONE:
-		err |= cfc_check_trigger_arg_is(&cmd->stop_arg, 0);
+		err |= comedi_check_trigger_arg_is(&cmd->stop_arg, 0);
 		break;
 	}
 
@@ -793,11 +785,8 @@ pci224_ao_cmdtest(struct comedi_device *dev, struct comedi_subdevice *s,
 	if (cmd->scan_begin_src == TRIG_TIMER) {
 		arg = cmd->scan_begin_arg;
 		/* Use two timers. */
-		i8253_cascade_ns_to_timer(I8254_OSC_BASE_10MHZ,
-					  &devpriv->cached_div1,
-					  &devpriv->cached_div2,
-					  &arg, cmd->flags);
-		err |= cfc_check_trigger_arg_is(&cmd->scan_begin_arg, arg);
+		comedi_8254_cascade_ns_to_timer(dev->pacer, &arg, cmd->flags);
+		err |= comedi_check_trigger_arg_is(&cmd->scan_begin_arg, arg);
 	}
 
 	if (err)
@@ -817,7 +806,6 @@ static void pci224_ao_start_pacer(struct comedi_device *dev,
 				  struct comedi_subdevice *s)
 {
 	struct pci224_private *devpriv = dev->private;
-	unsigned long timer_base = devpriv->iobase1 + PCI224_Z2_CT0;
 
 	/*
 	 * The output of timer Z2-0 will be used as the scan trigger
@@ -830,14 +818,10 @@ static void pci224_ao_start_pacer(struct comedi_device *dev,
 	outb(GAT_CONFIG(2, GAT_VCC), devpriv->iobase1 + PCI224_ZGAT_SCE);
 	/* Z2-2 needs 10 MHz clock. */
 	outb(CLK_CONFIG(2, CLK_10MHZ), devpriv->iobase1 + PCI224_ZCLK_SCE);
-	/* Load Z2-2 mode (2) and counter (div1). */
-	i8254_set_mode(timer_base, 0, 2, I8254_MODE2 | I8254_BINARY);
-	i8254_write(timer_base, 0, 2, devpriv->cached_div1);
 	/* Z2-0 is clocked from Z2-2's output. */
 	outb(CLK_CONFIG(0, CLK_OUTNM1), devpriv->iobase1 + PCI224_ZCLK_SCE);
-	/* Load Z2-0 mode (2) and counter (div2). */
-	i8254_set_mode(timer_base, 0, 0, I8254_MODE2 | I8254_BINARY);
-	i8254_write(timer_base, 0, 0, devpriv->cached_div2);
+
+	comedi_8254_pacer_enable(dev->pacer, 2, 0, false);
 }
 
 static int pci224_ao_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
@@ -852,10 +836,9 @@ static int pci224_ao_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	unsigned long flags;
 
 	/* Cannot handle null/empty chanlist. */
-	if (cmd->chanlist == NULL || cmd->chanlist_len == 0)
+	if (!cmd->chanlist || cmd->chanlist_len == 0)
 		return -EINVAL;
 
-
 	/* Determine which channels are enabled and their load order.  */
 	devpriv->ao_enab = 0;
 
@@ -893,8 +876,10 @@ static int pci224_ao_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	outw(devpriv->daccon | PCI224_DACCON_FIFORESET,
 	     dev->iobase + PCI224_DACCON);
 
-	if (cmd->scan_begin_src == TRIG_TIMER)
+	if (cmd->scan_begin_src == TRIG_TIMER) {
+		comedi_8254_update_divisors(dev->pacer);
 		pci224_ao_start_pacer(dev, s);
+	}
 
 	spin_lock_irqsave(&devpriv->ao_spinlock, flags);
 	if (cmd->start_src == TRIG_INT) {
@@ -1042,14 +1027,12 @@ pci224_auto_attach(struct comedi_device *dev, unsigned long context_model)
 	if (!devpriv->ao_scan_vals)
 		return -ENOMEM;
 
-
 	/* Allocate buffer to hold AO channel scan order. */
 	devpriv->ao_scan_order = kmalloc(sizeof(devpriv->ao_scan_order[0]) *
 					 thisboard->ao_chans, GFP_KERNEL);
 	if (!devpriv->ao_scan_order)
 		return -ENOMEM;
 
-
 	/* Disable interrupt sources. */
 	devpriv->intsce = 0;
 	outb(0, devpriv->iobase1 + PCI224_INT_SCE);
@@ -1063,6 +1046,11 @@ pci224_auto_attach(struct comedi_device *dev, unsigned long context_model)
 	outw(devpriv->daccon | PCI224_DACCON_FIFORESET,
 	     dev->iobase + PCI224_DACCON);
 
+	dev->pacer = comedi_8254_init(devpriv->iobase1 + PCI224_Z2_BASE,
+				      I8254_OSC_BASE_10MHZ, I8254_IO8, 0);
+	if (!dev->pacer)
+		return -ENOMEM;
+
 	ret = comedi_alloc_subdevices(dev, 1);
 	if (ret)
 		return ret;
diff --git a/drivers/staging/comedi/drivers/amplc_pci230.c b/drivers/staging/comedi/drivers/amplc_pci230.c
index 49806a5e514c..20d592002557 100644
--- a/drivers/staging/comedi/drivers/amplc_pci230.c
+++ b/drivers/staging/comedi/drivers/amplc_pci230.c
@@ -181,14 +181,12 @@
  */
 
 #include <linux/module.h>
-#include <linux/pci.h>
 #include <linux/delay.h>
 #include <linux/interrupt.h>
 
-#include "../comedidev.h"
+#include "../comedi_pci.h"
 
-#include "comedi_fc.h"
-#include "8253.h"
+#include "comedi_8254.h"
 #include "8255.h"
 
 /*
@@ -206,10 +204,6 @@
 #define PCI230_PPI_X_C		0x02	/* User PPI (82C55) port C */
 #define PCI230_PPI_X_CMD	0x03	/* User PPI (82C55) control word */
 #define PCI230_Z2_CT_BASE	0x14	/* 82C54 counter/timer base */
-#define PCI230_Z2_CT0		0x14	/* 82C54 counter/timer 0 */
-#define PCI230_Z2_CT1		0x15	/* 82C54 counter/timer 1 */
-#define PCI230_Z2_CT2		0x16	/* 82C54 counter/timer 2 */
-#define PCI230_Z2_CTC		0x17	/* 82C54 counter/timer control word */
 #define PCI230_ZCLK_SCE		0x1A	/* Group Z Clock Configuration */
 #define PCI230_ZGAT_SCE		0x1D	/* Group Z Gate Configuration */
 #define PCI230_INT_SCE		0x1E	/* Interrupt source mask (w) */
@@ -377,12 +371,6 @@
 #define CLK_EXT		7	/* external clock */
 /* Macro to construct clock input configuration register value. */
 #define CLK_CONFIG(chan, src)	((((chan) & 3) << 3) | ((src) & 7))
-/* Timebases in ns. */
-#define TIMEBASE_10MHZ		100
-#define TIMEBASE_1MHZ		1000
-#define TIMEBASE_100KHZ		10000
-#define TIMEBASE_10KHZ		100000
-#define TIMEBASE_1KHZ		1000000
 
 /*
  * Counter/timer gate input configuration sources.
@@ -507,11 +495,11 @@ struct pci230_private {
 
 /* PCI230 clock source periods in ns */
 static const unsigned int pci230_timebase[8] = {
-	[CLK_10MHZ] = TIMEBASE_10MHZ,
-	[CLK_1MHZ] = TIMEBASE_1MHZ,
-	[CLK_100KHZ] = TIMEBASE_100KHZ,
-	[CLK_10KHZ] = TIMEBASE_10KHZ,
-	[CLK_1KHZ] = TIMEBASE_1KHZ,
+	[CLK_10MHZ]	= I8254_OSC_BASE_10MHZ,
+	[CLK_1MHZ]	= I8254_OSC_BASE_1MHZ,
+	[CLK_100KHZ]	= I8254_OSC_BASE_100KHZ,
+	[CLK_10KHZ]	= I8254_OSC_BASE_10KHZ,
+	[CLK_1KHZ]	= I8254_OSC_BASE_1KHZ,
 };
 
 /* PCI230 analogue input range table */
@@ -695,7 +683,7 @@ static void pci230_ct_setup_ns_mode(struct comedi_device *dev, unsigned int ct,
 	unsigned int count;
 
 	/* Set mode. */
-	i8254_set_mode(dev->iobase + PCI230_Z2_CT_BASE, 0, ct, mode);
+	comedi_8254_set_mode(dev->pacer, ct, mode);
 	/* Determine clock source and count. */
 	clk_src = pci230_choose_clk_count(ns, &count, flags);
 	/* Program clock source. */
@@ -704,13 +692,13 @@ static void pci230_ct_setup_ns_mode(struct comedi_device *dev, unsigned int ct,
 	if (count >= 65536)
 		count = 0;
 
-	i8254_write(dev->iobase + PCI230_Z2_CT_BASE, 0, ct, count);
+	comedi_8254_write(dev->pacer, ct, count);
 }
 
 static void pci230_cancel_ct(struct comedi_device *dev, unsigned int ct)
 {
-	i8254_set_mode(dev->iobase + PCI230_Z2_CT_BASE, 0, ct, I8254_MODE1);
 	/* Counter ct, 8254 mode 1, initial count not written. */
+	comedi_8254_set_mode(dev->pacer, ct, I8254_MODE1);
 }
 
 static int pci230_ai_eoc(struct comedi_device *dev,
@@ -760,7 +748,7 @@ static int pci230_ai_insn_read(struct comedi_device *dev,
 	 */
 	adccon = PCI230_ADC_TRIG_Z2CT2 | PCI230_ADC_FIFO_EN;
 	/* Set Z2-CT2 output low to avoid any false triggers. */
-	i8254_set_mode(dev->iobase + PCI230_Z2_CT_BASE, 0, 2, I8254_MODE0);
+	comedi_8254_set_mode(dev->pacer, 2, I8254_MODE0);
 	devpriv->ai_bipolar = comedi_range_is_bipolar(s, range);
 	if (aref == AREF_DIFF) {
 		/* Differential. */
@@ -811,10 +799,8 @@ static int pci230_ai_insn_read(struct comedi_device *dev,
 		 * Trigger conversion by toggling Z2-CT2 output
 		 * (finish with output high).
 		 */
-		i8254_set_mode(dev->iobase + PCI230_Z2_CT_BASE, 0,
-			       2, I8254_MODE0);
-		i8254_set_mode(dev->iobase + PCI230_Z2_CT_BASE, 0,
-			       2, I8254_MODE1);
+		comedi_8254_set_mode(dev->pacer, 2, I8254_MODE0);
+		comedi_8254_set_mode(dev->pacer, 2, I8254_MODE1);
 
 		/* wait for conversion to end */
 		ret = comedi_timeout(dev, s, insn, pci230_ai_eoc, 0);
@@ -898,7 +884,7 @@ static int pci230_ao_cmdtest(struct comedi_device *dev,
 
 	/* Step 1 : check if triggers are trivially valid */
 
-	err |= cfc_check_trigger_src(&cmd->start_src, TRIG_INT);
+	err |= comedi_check_trigger_src(&cmd->start_src, TRIG_INT);
 
 	tmp = TRIG_TIMER | TRIG_INT;
 	if (thisboard->min_hwver > 0 && devpriv->hwver >= 2) {
@@ -918,19 +904,19 @@ static int pci230_ao_cmdtest(struct comedi_device *dev,
 		 */
 		tmp |= TRIG_EXT;
 	}
-	err |= cfc_check_trigger_src(&cmd->scan_begin_src, tmp);
+	err |= comedi_check_trigger_src(&cmd->scan_begin_src, tmp);
 
-	err |= cfc_check_trigger_src(&cmd->convert_src, TRIG_NOW);
-	err |= cfc_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
-	err |= cfc_check_trigger_src(&cmd->stop_src, TRIG_COUNT | TRIG_NONE);
+	err |= comedi_check_trigger_src(&cmd->convert_src, TRIG_NOW);
+	err |= comedi_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
+	err |= comedi_check_trigger_src(&cmd->stop_src, TRIG_COUNT | TRIG_NONE);
 
 	if (err)
 		return 1;
 
 	/* Step 2a : make sure trigger sources are unique */
 
-	err |= cfc_check_trigger_is_unique(cmd->scan_begin_src);
-	err |= cfc_check_trigger_is_unique(cmd->stop_src);
+	err |= comedi_check_trigger_is_unique(cmd->scan_begin_src);
+	err |= comedi_check_trigger_is_unique(cmd->stop_src);
 
 	/* Step 2b : and mutually compatible */
 
@@ -939,7 +925,7 @@ static int pci230_ao_cmdtest(struct comedi_device *dev,
 
 	/* Step 3: check if arguments are trivially valid */
 
-	err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
+	err |= comedi_check_trigger_arg_is(&cmd->start_arg, 0);
 
 #define MAX_SPEED_AO	8000	/* 8000 ns => 125 kHz */
 /*
@@ -950,10 +936,10 @@ static int pci230_ao_cmdtest(struct comedi_device *dev,
 
 	switch (cmd->scan_begin_src) {
 	case TRIG_TIMER:
-		err |= cfc_check_trigger_arg_min(&cmd->scan_begin_arg,
-						 MAX_SPEED_AO);
-		err |= cfc_check_trigger_arg_max(&cmd->scan_begin_arg,
-						 MIN_SPEED_AO);
+		err |= comedi_check_trigger_arg_min(&cmd->scan_begin_arg,
+						    MAX_SPEED_AO);
+		err |= comedi_check_trigger_arg_max(&cmd->scan_begin_arg,
+						    MIN_SPEED_AO);
 		break;
 	case TRIG_EXT:
 		/*
@@ -978,16 +964,17 @@ static int pci230_ao_cmdtest(struct comedi_device *dev,
 		}
 		break;
 	default:
-		err |= cfc_check_trigger_arg_is(&cmd->scan_begin_arg, 0);
+		err |= comedi_check_trigger_arg_is(&cmd->scan_begin_arg, 0);
 		break;
 	}
 
-	err |= cfc_check_trigger_arg_is(&cmd->scan_end_arg, cmd->chanlist_len);
+	err |= comedi_check_trigger_arg_is(&cmd->scan_end_arg,
+					   cmd->chanlist_len);
 
 	if (cmd->stop_src == TRIG_COUNT)
-		err |= cfc_check_trigger_arg_min(&cmd->stop_arg, 1);
+		err |= comedi_check_trigger_arg_min(&cmd->stop_arg, 1);
 	else	/* TRIG_NONE */
-		err |= cfc_check_trigger_arg_is(&cmd->stop_arg, 0);
+		err |= comedi_check_trigger_arg_is(&cmd->stop_arg, 0);
 
 	if (err)
 		return 3;
@@ -1520,7 +1507,7 @@ static int pci230_ai_cmdtest(struct comedi_device *dev,
 
 	/* Step 1 : check if triggers are trivially valid */
 
-	err |= cfc_check_trigger_src(&cmd->start_src, TRIG_NOW | TRIG_INT);
+	err |= comedi_check_trigger_src(&cmd->start_src, TRIG_NOW | TRIG_INT);
 
 	tmp = TRIG_FOLLOW | TRIG_TIMER | TRIG_INT;
 	if (thisboard->have_dio || thisboard->min_hwver > 0) {
@@ -1532,21 +1519,21 @@ static int pci230_ai_cmdtest(struct comedi_device *dev,
 		 */
 		tmp |= TRIG_EXT;
 	}
-	err |= cfc_check_trigger_src(&cmd->scan_begin_src, tmp);
-	err |= cfc_check_trigger_src(&cmd->convert_src,
+	err |= comedi_check_trigger_src(&cmd->scan_begin_src, tmp);
+	err |= comedi_check_trigger_src(&cmd->convert_src,
 					TRIG_TIMER | TRIG_INT | TRIG_EXT);
-	err |= cfc_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
-	err |= cfc_check_trigger_src(&cmd->stop_src, TRIG_COUNT | TRIG_NONE);
+	err |= comedi_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
+	err |= comedi_check_trigger_src(&cmd->stop_src, TRIG_COUNT | TRIG_NONE);
 
 	if (err)
 		return 1;
 
 	/* Step 2a : make sure trigger sources are unique */
 
-	err |= cfc_check_trigger_is_unique(cmd->start_src);
-	err |= cfc_check_trigger_is_unique(cmd->scan_begin_src);
-	err |= cfc_check_trigger_is_unique(cmd->convert_src);
-	err |= cfc_check_trigger_is_unique(cmd->stop_src);
+	err |= comedi_check_trigger_is_unique(cmd->start_src);
+	err |= comedi_check_trigger_is_unique(cmd->scan_begin_src);
+	err |= comedi_check_trigger_is_unique(cmd->convert_src);
+	err |= comedi_check_trigger_is_unique(cmd->stop_src);
 
 	/* Step 2b : and mutually compatible */
 
@@ -1563,7 +1550,7 @@ static int pci230_ai_cmdtest(struct comedi_device *dev,
 
 	/* Step 3: check if arguments are trivially valid */
 
-	err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
+	err |= comedi_check_trigger_arg_is(&cmd->start_arg, 0);
 
 #define MAX_SPEED_AI_SE		3200	/* PCI230 SE:   3200 ns => 312.5 kHz */
 #define MAX_SPEED_AI_DIFF	8000	/* PCI230 DIFF: 8000 ns => 125 kHz */
@@ -1598,10 +1585,10 @@ static int pci230_ai_cmdtest(struct comedi_device *dev,
 			max_speed_ai = MAX_SPEED_AI_PLUS;
 		}
 
-		err |= cfc_check_trigger_arg_min(&cmd->convert_arg,
-						 max_speed_ai);
-		err |= cfc_check_trigger_arg_max(&cmd->convert_arg,
-						 MIN_SPEED_AI);
+		err |= comedi_check_trigger_arg_min(&cmd->convert_arg,
+						    max_speed_ai);
+		err |= comedi_check_trigger_arg_max(&cmd->convert_arg,
+						    MIN_SPEED_AI);
 	} else if (cmd->convert_src == TRIG_EXT) {
 		/*
 		 * external trigger
@@ -1636,18 +1623,20 @@ static int pci230_ai_cmdtest(struct comedi_device *dev,
 			 * convert_arg == 0 => trigger on -ve edge.
 			 * convert_arg == 1 => trigger on +ve edge.
 			 */
-			err |= cfc_check_trigger_arg_max(&cmd->convert_arg, 1);
+			err |= comedi_check_trigger_arg_max(&cmd->convert_arg,
+							    1);
 		}
 	} else {
-		err |= cfc_check_trigger_arg_is(&cmd->convert_arg, 0);
+		err |= comedi_check_trigger_arg_is(&cmd->convert_arg, 0);
 	}
 
-	err |= cfc_check_trigger_arg_is(&cmd->scan_end_arg, cmd->chanlist_len);
+	err |= comedi_check_trigger_arg_is(&cmd->scan_end_arg,
+					   cmd->chanlist_len);
 
 	if (cmd->stop_src == TRIG_COUNT)
-		err |= cfc_check_trigger_arg_min(&cmd->stop_arg, 1);
+		err |= comedi_check_trigger_arg_min(&cmd->stop_arg, 1);
 	else	/* TRIG_NONE */
-		err |= cfc_check_trigger_arg_is(&cmd->stop_arg, 0);
+		err |= comedi_check_trigger_arg_is(&cmd->stop_arg, 0);
 
 	if (cmd->scan_begin_src == TRIG_EXT) {
 		/*
@@ -1672,7 +1661,7 @@ static int pci230_ai_cmdtest(struct comedi_device *dev,
 			err |= -EINVAL;
 
 	} else {
-		err |= cfc_check_trigger_arg_is(&cmd->scan_begin_arg, 0);
+		err |= comedi_check_trigger_arg_is(&cmd->scan_begin_arg, 0);
 	}
 
 	if (err)
@@ -1767,8 +1756,8 @@ static int pci230_ai_inttrig_convert(struct comedi_device *dev,
 	 * Trigger conversion by toggling Z2-CT2 output.
 	 * Finish with output high.
 	 */
-	i8254_set_mode(dev->iobase + PCI230_Z2_CT_BASE, 0, 2, I8254_MODE0);
-	i8254_set_mode(dev->iobase + PCI230_Z2_CT_BASE, 0, 2, I8254_MODE1);
+	comedi_8254_set_mode(dev->pacer, 2, I8254_MODE0);
+	comedi_8254_set_mode(dev->pacer, 2, I8254_MODE1);
 	/*
 	 * Delay.  Should driver be responsible for this?  An
 	 * alternative would be to wait until conversion is complete,
@@ -2189,7 +2178,7 @@ static int pci230_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	 * Set counter/timer 2 output high for use as the initial start
 	 * conversion source.
 	 */
-	i8254_set_mode(dev->iobase + PCI230_Z2_CT_BASE, 0, 2, I8254_MODE1);
+	comedi_8254_set_mode(dev->pacer, 2, I8254_MODE1);
 
 	/*
 	 * Temporarily use CT2 output as conversion trigger source and
@@ -2393,7 +2382,7 @@ static int pci230_auto_attach(struct comedi_device *dev,
 	spin_lock_init(&devpriv->ao_stop_spinlock);
 
 	dev->board_ptr = pci230_find_pci_board(pci_dev);
-	if (dev->board_ptr == NULL) {
+	if (!dev->board_ptr) {
 		dev_err(dev->class_dev,
 			"amplc_pci230: BUG! cannot determine board type!\n");
 		return -EINVAL;
@@ -2481,6 +2470,11 @@ static int pci230_auto_attach(struct comedi_device *dev,
 			dev->irq = pci_dev->irq;
 	}
 
+	dev->pacer = comedi_8254_init(dev->iobase + PCI230_Z2_CT_BASE,
+				      0, I8254_IO8, 0);
+	if (!dev->pacer)
+		return -ENOMEM;
+
 	rc = comedi_alloc_subdevices(dev, 3);
 	if (rc)
 		return rc;
diff --git a/drivers/staging/comedi/drivers/amplc_pci236.c b/drivers/staging/comedi/drivers/amplc_pci236.c
index ad1e93dd13a0..31cc38b4bcad 100644
--- a/drivers/staging/comedi/drivers/amplc_pci236.c
+++ b/drivers/staging/comedi/drivers/amplc_pci236.c
@@ -42,10 +42,9 @@
  */
 
 #include <linux/module.h>
-#include <linux/pci.h>
 #include <linux/interrupt.h>
 
-#include "../comedidev.h"
+#include "../comedi_pci.h"
 
 #include "amplc_pc236.h"
 #include "plx9052.h"
diff --git a/drivers/staging/comedi/drivers/amplc_pci263.c b/drivers/staging/comedi/drivers/amplc_pci263.c
index 0d2224b832ac..b6768aa90547 100644
--- a/drivers/staging/comedi/drivers/amplc_pci263.c
+++ b/drivers/staging/comedi/drivers/amplc_pci263.c
@@ -33,9 +33,8 @@ The state of the outputs can be read.
 */
 
 #include <linux/module.h>
-#include <linux/pci.h>
 
-#include "../comedidev.h"
+#include "../comedi_pci.h"
 
 static int pci263_do_insn_bits(struct comedi_device *dev,
 			       struct comedi_subdevice *s,
diff --git a/drivers/staging/comedi/drivers/cb_das16_cs.c b/drivers/staging/comedi/drivers/cb_das16_cs.c
index 1079b6c72b15..ae84f2c0cc9c 100644
--- a/drivers/staging/comedi/drivers/cb_das16_cs.c
+++ b/drivers/staging/comedi/drivers/cb_das16_cs.c
@@ -30,8 +30,6 @@ Devices: [ComputerBoards] PC-CARD DAS16/16 (cb_das16_cs), PC-CARD DAS16/16-AO
 Author: ds
 Updated: Mon, 04 Nov 2002 20:04:21 -0800
 Status: experimental
-
-
 */
 
 #include <linux/module.h>
@@ -40,17 +38,13 @@ Status: experimental
 
 #include "../comedi_pcmcia.h"
 
-#include "comedi_fc.h"
-#include "8253.h"
+#include "comedi_8254.h"
 
 #define DAS16CS_ADC_DATA		0
 #define DAS16CS_DIO_MUX			2
 #define DAS16CS_MISC1			4
 #define DAS16CS_MISC2			6
-#define DAS16CS_CTR0			8
-#define DAS16CS_CTR1			10
-#define DAS16CS_CTR2			12
-#define DAS16CS_CTR_CONTROL		14
+#define DAS16CS_TIMER_BASE		8
 #define DAS16CS_DIO			16
 
 struct das16cs_board {
@@ -279,6 +273,11 @@ static int das16cs_auto_attach(struct comedi_device *dev,
 	if (!devpriv)
 		return -ENOMEM;
 
+	dev->pacer = comedi_8254_init(dev->iobase + DAS16CS_TIMER_BASE,
+				      I8254_OSC_BASE_10MHZ, I8254_IO16, 0);
+	if (!dev->pacer)
+		return -ENOMEM;
+
 	ret = comedi_alloc_subdevices(dev, 3);
 	if (ret)
 		return ret;
diff --git a/drivers/staging/comedi/drivers/cb_pcidas.c b/drivers/staging/comedi/drivers/cb_pcidas.c
index dd0c65a5b5a0..e3591a5824fe 100644
--- a/drivers/staging/comedi/drivers/cb_pcidas.c
+++ b/drivers/staging/comedi/drivers/cb_pcidas.c
@@ -62,16 +62,14 @@ analog triggering on 1602 series
 */
 
 #include <linux/module.h>
-#include <linux/pci.h>
 #include <linux/delay.h>
 #include <linux/interrupt.h>
 
-#include "../comedidev.h"
+#include "../comedi_pci.h"
 
-#include "8253.h"
+#include "comedi_8254.h"
 #include "8255.h"
 #include "amcc_s5933.h"
-#include "comedi_fc.h"
 
 #define AI_BUFFER_SIZE		1024	/* max ai fifo size */
 #define AO_BUFFER_SIZE		1024	/* max ao fifo size */
@@ -338,14 +336,12 @@ static const struct cb_pcidas_board cb_pcidas_boards[] = {
 };
 
 struct cb_pcidas_private {
+	struct comedi_8254 *ao_pacer;
 	/* base addresses */
 	unsigned long s5933_config;
 	unsigned long control_status;
 	unsigned long adc_fifo;
 	unsigned long ao_registers;
-	/* divisors of master clock for analog input pacing */
-	unsigned int divisor1;
-	unsigned int divisor2;
 	/* bits to write to registers */
 	unsigned int adc_fifo_bits;
 	unsigned int s5933_intcsr_bits;
@@ -353,9 +349,6 @@ struct cb_pcidas_private {
 	/* fifo buffers */
 	unsigned short ai_buffer[AI_BUFFER_SIZE];
 	unsigned short ao_buffer[AO_BUFFER_SIZE];
-	/* divisors of master clock for analog output pacing */
-	unsigned int ao_divisor1;
-	unsigned int ao_divisor2;
 	unsigned int calibration_source;
 };
 
@@ -530,7 +523,7 @@ static int wait_for_nvram_ready(unsigned long s5933_base_addr)
 }
 
 static int nvram_read(struct comedi_device *dev, unsigned int address,
-			uint8_t *data)
+		      uint8_t *data)
 {
 	struct cb_pcidas_private *devpriv = dev->private;
 	unsigned long iobase = devpriv->s5933_config;
@@ -778,29 +771,28 @@ static int cb_pcidas_ai_cmdtest(struct comedi_device *dev,
 				struct comedi_cmd *cmd)
 {
 	const struct cb_pcidas_board *thisboard = dev->board_ptr;
-	struct cb_pcidas_private *devpriv = dev->private;
 	int err = 0;
 	unsigned int arg;
 
 	/* Step 1 : check if triggers are trivially valid */
 
-	err |= cfc_check_trigger_src(&cmd->start_src, TRIG_NOW | TRIG_EXT);
-	err |= cfc_check_trigger_src(&cmd->scan_begin_src,
+	err |= comedi_check_trigger_src(&cmd->start_src, TRIG_NOW | TRIG_EXT);
+	err |= comedi_check_trigger_src(&cmd->scan_begin_src,
 					TRIG_FOLLOW | TRIG_TIMER | TRIG_EXT);
-	err |= cfc_check_trigger_src(&cmd->convert_src,
+	err |= comedi_check_trigger_src(&cmd->convert_src,
 					TRIG_TIMER | TRIG_NOW | TRIG_EXT);
-	err |= cfc_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
-	err |= cfc_check_trigger_src(&cmd->stop_src, TRIG_COUNT | TRIG_NONE);
+	err |= comedi_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
+	err |= comedi_check_trigger_src(&cmd->stop_src, TRIG_COUNT | TRIG_NONE);
 
 	if (err)
 		return 1;
 
 	/* Step 2a : make sure trigger sources are unique */
 
-	err |= cfc_check_trigger_is_unique(cmd->start_src);
-	err |= cfc_check_trigger_is_unique(cmd->scan_begin_src);
-	err |= cfc_check_trigger_is_unique(cmd->convert_src);
-	err |= cfc_check_trigger_is_unique(cmd->stop_src);
+	err |= comedi_check_trigger_is_unique(cmd->start_src);
+	err |= comedi_check_trigger_is_unique(cmd->scan_begin_src);
+	err |= comedi_check_trigger_is_unique(cmd->convert_src);
+	err |= comedi_check_trigger_is_unique(cmd->stop_src);
 
 	/* Step 2b : and mutually compatible */
 
@@ -819,7 +811,7 @@ static int cb_pcidas_ai_cmdtest(struct comedi_device *dev,
 
 	switch (cmd->start_src) {
 	case TRIG_NOW:
-		err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
+		err |= comedi_check_trigger_arg_is(&cmd->start_arg, 0);
 		break;
 	case TRIG_EXT:
 		/* External trigger, only CR_EDGE and CR_INVERT flags allowed */
@@ -836,20 +828,24 @@ static int cb_pcidas_ai_cmdtest(struct comedi_device *dev,
 		break;
 	}
 
-	if (cmd->scan_begin_src == TRIG_TIMER)
-		err |= cfc_check_trigger_arg_min(&cmd->scan_begin_arg,
-				thisboard->ai_speed * cmd->chanlist_len);
+	if (cmd->scan_begin_src == TRIG_TIMER) {
+		err |= comedi_check_trigger_arg_min(&cmd->scan_begin_arg,
+						    thisboard->ai_speed *
+						    cmd->chanlist_len);
+	}
 
-	if (cmd->convert_src == TRIG_TIMER)
-		err |= cfc_check_trigger_arg_min(&cmd->convert_arg,
-						 thisboard->ai_speed);
+	if (cmd->convert_src == TRIG_TIMER) {
+		err |= comedi_check_trigger_arg_min(&cmd->convert_arg,
+						    thisboard->ai_speed);
+	}
 
-	err |= cfc_check_trigger_arg_is(&cmd->scan_end_arg, cmd->chanlist_len);
+	err |= comedi_check_trigger_arg_is(&cmd->scan_end_arg,
+					   cmd->chanlist_len);
 
 	if (cmd->stop_src == TRIG_COUNT)
-		err |= cfc_check_trigger_arg_min(&cmd->stop_arg, 1);
+		err |= comedi_check_trigger_arg_min(&cmd->stop_arg, 1);
 	else	/* TRIG_NONE */
-		err |= cfc_check_trigger_arg_is(&cmd->stop_arg, 0);
+		err |= comedi_check_trigger_arg_is(&cmd->stop_arg, 0);
 
 	if (err)
 		return 3;
@@ -858,19 +854,13 @@ static int cb_pcidas_ai_cmdtest(struct comedi_device *dev,
 
 	if (cmd->scan_begin_src == TRIG_TIMER) {
 		arg = cmd->scan_begin_arg;
-		i8253_cascade_ns_to_timer(I8254_OSC_BASE_10MHZ,
-					  &devpriv->divisor1,
-					  &devpriv->divisor2,
-					  &arg, cmd->flags);
-		err |= cfc_check_trigger_arg_is(&cmd->scan_begin_arg, arg);
+		comedi_8254_cascade_ns_to_timer(dev->pacer, &arg, cmd->flags);
+		err |= comedi_check_trigger_arg_is(&cmd->scan_begin_arg, arg);
 	}
 	if (cmd->convert_src == TRIG_TIMER) {
 		arg = cmd->convert_arg;
-		i8253_cascade_ns_to_timer(I8254_OSC_BASE_10MHZ,
-					  &devpriv->divisor1,
-					  &devpriv->divisor2,
-					  &arg, cmd->flags);
-		err |= cfc_check_trigger_arg_is(&cmd->convert_arg, arg);
+		comedi_8254_cascade_ns_to_timer(dev->pacer, &arg, cmd->flags);
+		err |= comedi_check_trigger_arg_is(&cmd->convert_arg, arg);
 	}
 
 	if (err)
@@ -886,18 +876,6 @@ static int cb_pcidas_ai_cmdtest(struct comedi_device *dev,
 	return 0;
 }
 
-static void cb_pcidas_ai_load_counters(struct comedi_device *dev)
-{
-	struct cb_pcidas_private *devpriv = dev->private;
-	unsigned long timer_base = dev->iobase + ADC8254;
-
-	i8254_set_mode(timer_base, 0, 1, I8254_MODE2 | I8254_BINARY);
-	i8254_set_mode(timer_base, 0, 2, I8254_MODE2 | I8254_BINARY);
-
-	i8254_write(timer_base, 0, 1, devpriv->divisor1);
-	i8254_write(timer_base, 0, 2, devpriv->divisor2);
-}
-
 static int cb_pcidas_ai_cmd(struct comedi_device *dev,
 			    struct comedi_subdevice *s)
 {
@@ -933,8 +911,11 @@ static int cb_pcidas_ai_cmd(struct comedi_device *dev,
 	outw(bits, devpriv->control_status + ADCMUX_CONT);
 
 	/*  load counters */
-	if (cmd->scan_begin_src == TRIG_TIMER || cmd->convert_src == TRIG_TIMER)
-		cb_pcidas_ai_load_counters(dev);
+	if (cmd->scan_begin_src == TRIG_TIMER ||
+	    cmd->convert_src == TRIG_TIMER) {
+		comedi_8254_update_divisors(dev->pacer);
+		comedi_8254_pacer_enable(dev->pacer, 1, 2, true);
+	}
 
 	/*  enable interrupts */
 	spin_lock_irqsave(&dev->spinlock, flags);
@@ -1004,24 +985,23 @@ static int cb_pcidas_ao_cmdtest(struct comedi_device *dev,
 	const struct cb_pcidas_board *thisboard = dev->board_ptr;
 	struct cb_pcidas_private *devpriv = dev->private;
 	int err = 0;
-	unsigned int arg;
 
 	/* Step 1 : check if triggers are trivially valid */
 
-	err |= cfc_check_trigger_src(&cmd->start_src, TRIG_INT);
-	err |= cfc_check_trigger_src(&cmd->scan_begin_src,
+	err |= comedi_check_trigger_src(&cmd->start_src, TRIG_INT);
+	err |= comedi_check_trigger_src(&cmd->scan_begin_src,
 					TRIG_TIMER | TRIG_EXT);
-	err |= cfc_check_trigger_src(&cmd->convert_src, TRIG_NOW);
-	err |= cfc_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
-	err |= cfc_check_trigger_src(&cmd->stop_src, TRIG_COUNT | TRIG_NONE);
+	err |= comedi_check_trigger_src(&cmd->convert_src, TRIG_NOW);
+	err |= comedi_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
+	err |= comedi_check_trigger_src(&cmd->stop_src, TRIG_COUNT | TRIG_NONE);
 
 	if (err)
 		return 1;
 
 	/* Step 2a : make sure trigger sources are unique */
 
-	err |= cfc_check_trigger_is_unique(cmd->scan_begin_src);
-	err |= cfc_check_trigger_is_unique(cmd->stop_src);
+	err |= comedi_check_trigger_is_unique(cmd->scan_begin_src);
+	err |= comedi_check_trigger_is_unique(cmd->stop_src);
 
 	/* Step 2b : and mutually compatible */
 
@@ -1030,18 +1010,20 @@ static int cb_pcidas_ao_cmdtest(struct comedi_device *dev,
 
 	/* Step 3: check if arguments are trivially valid */
 
-	err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
+	err |= comedi_check_trigger_arg_is(&cmd->start_arg, 0);
 
-	if (cmd->scan_begin_src == TRIG_TIMER)
-		err |= cfc_check_trigger_arg_min(&cmd->scan_begin_arg,
-						 thisboard->ao_scan_speed);
+	if (cmd->scan_begin_src == TRIG_TIMER) {
+		err |= comedi_check_trigger_arg_min(&cmd->scan_begin_arg,
+						    thisboard->ao_scan_speed);
+	}
 
-	err |= cfc_check_trigger_arg_is(&cmd->scan_end_arg, cmd->chanlist_len);
+	err |= comedi_check_trigger_arg_is(&cmd->scan_end_arg,
+					   cmd->chanlist_len);
 
 	if (cmd->stop_src == TRIG_COUNT)
-		err |= cfc_check_trigger_arg_min(&cmd->stop_arg, 1);
+		err |= comedi_check_trigger_arg_min(&cmd->stop_arg, 1);
 	else	/* TRIG_NONE */
-		err |= cfc_check_trigger_arg_is(&cmd->stop_arg, 0);
+		err |= comedi_check_trigger_arg_is(&cmd->stop_arg, 0);
 
 	if (err)
 		return 3;
@@ -1049,12 +1031,11 @@ static int cb_pcidas_ao_cmdtest(struct comedi_device *dev,
 	/* step 4: fix up any arguments */
 
 	if (cmd->scan_begin_src == TRIG_TIMER) {
-		arg = cmd->scan_begin_arg;
-		i8253_cascade_ns_to_timer(I8254_OSC_BASE_10MHZ,
-					  &devpriv->ao_divisor1,
-					  &devpriv->ao_divisor2,
-					  &arg, cmd->flags);
-		err |= cfc_check_trigger_arg_is(&cmd->scan_begin_arg, arg);
+		unsigned int arg = cmd->scan_begin_arg;
+
+		comedi_8254_cascade_ns_to_timer(devpriv->ao_pacer,
+						&arg, cmd->flags);
+		err |= comedi_check_trigger_arg_is(&cmd->scan_begin_arg, arg);
 	}
 
 	if (err)
@@ -1139,18 +1120,6 @@ static int cb_pcidas_ao_inttrig(struct comedi_device *dev,
 	return 0;
 }
 
-static void cb_pcidas_ao_load_counters(struct comedi_device *dev)
-{
-	struct cb_pcidas_private *devpriv = dev->private;
-	unsigned long timer_base = dev->iobase + DAC8254;
-
-	i8254_set_mode(timer_base, 0, 1, I8254_MODE2 | I8254_BINARY);
-	i8254_set_mode(timer_base, 0, 2, I8254_MODE2 | I8254_BINARY);
-
-	i8254_write(timer_base, 0, 1, devpriv->ao_divisor1);
-	i8254_write(timer_base, 0, 2, devpriv->ao_divisor2);
-}
-
 static int cb_pcidas_ao_cmd(struct comedi_device *dev,
 			    struct comedi_subdevice *s)
 {
@@ -1180,8 +1149,10 @@ static int cb_pcidas_ao_cmd(struct comedi_device *dev,
 	outw(0, devpriv->ao_registers + DACFIFOCLR);
 
 	/*  load counters */
-	if (cmd->scan_begin_src == TRIG_TIMER)
-		cb_pcidas_ao_load_counters(dev);
+	if (cmd->scan_begin_src == TRIG_TIMER) {
+		comedi_8254_update_divisors(devpriv->ao_pacer);
+		comedi_8254_pacer_enable(devpriv->ao_pacer, 1, 2, true);
+	}
 
 	/*  set pacer source */
 	spin_lock_irqsave(&dev->spinlock, flags);
@@ -1408,6 +1379,17 @@ static int cb_pcidas_auto_attach(struct comedi_device *dev,
 	}
 	dev->irq = pcidev->irq;
 
+	dev->pacer = comedi_8254_init(dev->iobase + ADC8254,
+				      I8254_OSC_BASE_10MHZ, I8254_IO8, 0);
+	if (!dev->pacer)
+		return -ENOMEM;
+
+	devpriv->ao_pacer = comedi_8254_init(dev->iobase + DAC8254,
+					     I8254_OSC_BASE_10MHZ,
+					     I8254_IO8, 0);
+	if (!devpriv->ao_pacer)
+		return -ENOMEM;
+
 	ret = comedi_alloc_subdevices(dev, 7);
 	if (ret)
 		return ret;
@@ -1530,8 +1512,9 @@ static int cb_pcidas_auto_attach(struct comedi_device *dev,
 			dac08_write(dev, s->maxdata / 2);
 			s->readback[i] = s->maxdata / 2;
 		}
-	} else
+	} else {
 		s->type = COMEDI_SUBD_UNUSED;
+	}
 
 	/*  make sure mailbox 4 is empty */
 	inl(devpriv->s5933_config + AMCC_OP_REG_IMB4);
@@ -1550,9 +1533,11 @@ static void cb_pcidas_detach(struct comedi_device *dev)
 {
 	struct cb_pcidas_private *devpriv = dev->private;
 
-	if (devpriv && devpriv->s5933_config) {
-		outl(INTCSR_INBOX_INTR_STATUS,
-		     devpriv->s5933_config + AMCC_OP_REG_INTCSR);
+	if (devpriv) {
+		if (devpriv->s5933_config)
+			outl(INTCSR_INBOX_INTR_STATUS,
+			     devpriv->s5933_config + AMCC_OP_REG_INTCSR);
+		kfree(devpriv->ao_pacer);
 	}
 	comedi_pci_detach(dev);
 }
diff --git a/drivers/staging/comedi/drivers/cb_pcidas64.c b/drivers/staging/comedi/drivers/cb_pcidas64.c
index 5b43e4e6d037..a94c33c3d962 100644
--- a/drivers/staging/comedi/drivers/cb_pcidas64.c
+++ b/drivers/staging/comedi/drivers/cb_pcidas64.c
@@ -81,16 +81,13 @@ TODO:
 */
 
 #include <linux/module.h>
-#include <linux/pci.h>
 #include <linux/delay.h>
 #include <linux/interrupt.h>
 
-#include "../comedidev.h"
+#include "../comedi_pci.h"
 
-#include "8253.h"
 #include "8255.h"
 #include "plx9080.h"
-#include "comedi_fc.h"
 
 #define TIMER_BASE 25		/*  40MHz master clock */
 /* 100kHz 'prescaled' clock for slow acquisition,
@@ -1474,9 +1471,8 @@ static int alloc_and_init_dma_members(struct comedi_device *dev)
 		devpriv->ai_buffer[i] =
 			pci_alloc_consistent(pcidev, DMA_BUFFER_SIZE,
 					     &devpriv->ai_buffer_bus_addr[i]);
-		if (devpriv->ai_buffer[i] == NULL)
+		if (!devpriv->ai_buffer[i])
 			return -ENOMEM;
-
 	}
 	for (i = 0; i < AO_DMA_RING_COUNT; i++) {
 		if (ao_cmd_is_supported(thisboard)) {
@@ -1484,9 +1480,8 @@ static int alloc_and_init_dma_members(struct comedi_device *dev)
 				pci_alloc_consistent(pcidev, DMA_BUFFER_SIZE,
 						     &devpriv->
 						      ao_buffer_bus_addr[i]);
-			if (devpriv->ao_buffer[i] == NULL)
+			if (!devpriv->ao_buffer[i])
 				return -ENOMEM;
-
 		}
 	}
 	/*  allocate dma descriptors */
@@ -1494,7 +1489,7 @@ static int alloc_and_init_dma_members(struct comedi_device *dev)
 		pci_alloc_consistent(pcidev, sizeof(struct plx_dma_desc) *
 				     ai_dma_ring_count(thisboard),
 				     &devpriv->ai_dma_desc_bus_addr);
-	if (devpriv->ai_dma_desc == NULL)
+	if (!devpriv->ai_dma_desc)
 		return -ENOMEM;
 
 	if (ao_cmd_is_supported(thisboard)) {
@@ -1503,7 +1498,7 @@ static int alloc_and_init_dma_members(struct comedi_device *dev)
 					     sizeof(struct plx_dma_desc) *
 					     AO_DMA_RING_COUNT,
 					     &devpriv->ao_dma_desc_bus_addr);
-		if (devpriv->ao_dma_desc == NULL)
+		if (!devpriv->ao_dma_desc)
 			return -ENOMEM;
 	}
 	/*  initialize dma descriptors */
@@ -1704,8 +1699,7 @@ static void i2c_write(struct comedi_device *dev, unsigned int address,
 
 	/*  get acknowledge */
 	if (i2c_read_ack(dev) != 0) {
-		dev_err(dev->class_dev, "%s failed: no acknowledge\n",
-			__func__);
+		dev_err(dev->class_dev, "failed: no acknowledge\n");
 		i2c_stop(dev);
 		return;
 	}
@@ -1713,8 +1707,7 @@ static void i2c_write(struct comedi_device *dev, unsigned int address,
 	for (i = 0; i < length; i++) {
 		i2c_write_byte(dev, data[i]);
 		if (i2c_read_ack(dev) != 0) {
-			dev_err(dev->class_dev, "%s failed: no acknowledge\n",
-				__func__);
+			dev_err(dev->class_dev, "failed: no acknowledge\n");
 			i2c_stop(dev);
 			return;
 		}
@@ -1841,7 +1834,6 @@ static int ai_rinsn(struct comedi_device *dev, struct comedi_subdevice *s,
 	}
 
 	for (n = 0; n < insn->n; n++) {
-
 		/*  clear adc buffer (inside loop for 4020 sake) */
 		writew(0, devpriv->main_iobase + ADC_BUFFER_CLEAR_REG);
 
@@ -1903,7 +1895,6 @@ static int ai_config_block_size(struct comedi_device *dev, unsigned int *data)
 		retval = set_ai_fifo_size(dev, fifo_size);
 		if (retval < 0)
 			return retval;
-
 	}
 
 	block_size = ai_fifo_size(dev) / fifo->num_segments * bytes_in_sample;
@@ -2001,7 +1992,8 @@ static unsigned int get_divisor(unsigned int ns, unsigned int flags)
 static void check_adc_timing(struct comedi_device *dev, struct comedi_cmd *cmd)
 {
 	const struct pcidas64_board *thisboard = dev->board_ptr;
-	unsigned int convert_divisor = 0, scan_divisor;
+	unsigned long long convert_divisor = 0;
+	unsigned int scan_divisor;
 	static const int min_convert_divisor = 3;
 	static const int max_convert_divisor =
 		max_counter_value + min_convert_divisor;
@@ -2027,7 +2019,6 @@ static void check_adc_timing(struct comedi_device *dev, struct comedi_cmd *cmd)
 	if (cmd->scan_begin_src == TRIG_TIMER) {
 		scan_divisor = get_divisor(cmd->scan_begin_arg, cmd->flags);
 		if (cmd->convert_src == TRIG_TIMER) {
-			/*  XXX check for integer overflows */
 			min_scan_divisor = convert_divisor * cmd->chanlist_len;
 			max_scan_divisor =
 				(convert_divisor * cmd->chanlist_len - 1) +
@@ -2094,34 +2085,34 @@ static int ai_cmdtest(struct comedi_device *dev, struct comedi_subdevice *s,
 
 	/* Step 1 : check if triggers are trivially valid */
 
-	err |= cfc_check_trigger_src(&cmd->start_src, TRIG_NOW | TRIG_EXT);
+	err |= comedi_check_trigger_src(&cmd->start_src, TRIG_NOW | TRIG_EXT);
 
 	triggers = TRIG_TIMER;
 	if (thisboard->layout == LAYOUT_4020)
 		triggers |= TRIG_OTHER;
 	else
 		triggers |= TRIG_FOLLOW;
-	err |= cfc_check_trigger_src(&cmd->scan_begin_src, triggers);
+	err |= comedi_check_trigger_src(&cmd->scan_begin_src, triggers);
 
 	triggers = TRIG_TIMER;
 	if (thisboard->layout == LAYOUT_4020)
 		triggers |= TRIG_NOW;
 	else
 		triggers |= TRIG_EXT;
-	err |= cfc_check_trigger_src(&cmd->convert_src, triggers);
-	err |= cfc_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
-	err |= cfc_check_trigger_src(&cmd->stop_src,
-				     TRIG_COUNT | TRIG_EXT | TRIG_NONE);
+	err |= comedi_check_trigger_src(&cmd->convert_src, triggers);
+	err |= comedi_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
+	err |= comedi_check_trigger_src(&cmd->stop_src,
+					TRIG_COUNT | TRIG_EXT | TRIG_NONE);
 
 	if (err)
 		return 1;
 
 	/* Step 2a : make sure trigger sources are unique */
 
-	err |= cfc_check_trigger_is_unique(cmd->start_src);
-	err |= cfc_check_trigger_is_unique(cmd->scan_begin_src);
-	err |= cfc_check_trigger_is_unique(cmd->convert_src);
-	err |= cfc_check_trigger_is_unique(cmd->stop_src);
+	err |= comedi_check_trigger_is_unique(cmd->start_src);
+	err |= comedi_check_trigger_is_unique(cmd->scan_begin_src);
+	err |= comedi_check_trigger_is_unique(cmd->convert_src);
+	err |= comedi_check_trigger_is_unique(cmd->stop_src);
 
 	/* Step 2b : and mutually compatible */
 
@@ -2135,7 +2126,7 @@ static int ai_cmdtest(struct comedi_device *dev, struct comedi_subdevice *s,
 
 	switch (cmd->start_src) {
 	case TRIG_NOW:
-		err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
+		err |= comedi_check_trigger_arg_is(&cmd->start_arg, 0);
 		break;
 	case TRIG_EXT:
 		/*
@@ -2147,30 +2138,37 @@ static int ai_cmdtest(struct comedi_device *dev, struct comedi_subdevice *s,
 
 	if (cmd->convert_src == TRIG_TIMER) {
 		if (thisboard->layout == LAYOUT_4020) {
-			err |= cfc_check_trigger_arg_is(&cmd->convert_arg, 0);
+			err |= comedi_check_trigger_arg_is(&cmd->convert_arg,
+							   0);
 		} else {
-			err |= cfc_check_trigger_arg_min(&cmd->convert_arg,
-							 thisboard->ai_speed);
-			/* if scans are timed faster than conversion rate allows */
-			if (cmd->scan_begin_src == TRIG_TIMER)
-				err |= cfc_check_trigger_arg_min(
+			err |= comedi_check_trigger_arg_min(&cmd->convert_arg,
+							    thisboard->
+							    ai_speed);
+			/*
+			 * if scans are timed faster than conversion rate
+			 * allows
+			 */
+			if (cmd->scan_begin_src == TRIG_TIMER) {
+				err |= comedi_check_trigger_arg_min(
 						&cmd->scan_begin_arg,
 						cmd->convert_arg *
 						cmd->chanlist_len);
+			}
 		}
 	}
 
-	err |= cfc_check_trigger_arg_min(&cmd->chanlist_len, 1);
-	err |= cfc_check_trigger_arg_is(&cmd->scan_end_arg, cmd->chanlist_len);
+	err |= comedi_check_trigger_arg_min(&cmd->chanlist_len, 1);
+	err |= comedi_check_trigger_arg_is(&cmd->scan_end_arg,
+					   cmd->chanlist_len);
 
 	switch (cmd->stop_src) {
 	case TRIG_EXT:
 		break;
 	case TRIG_COUNT:
-		err |= cfc_check_trigger_arg_min(&cmd->stop_arg, 1);
+		err |= comedi_check_trigger_arg_min(&cmd->stop_arg, 1);
 		break;
 	case TRIG_NONE:
-		err |= cfc_check_trigger_arg_is(&cmd->stop_arg, 0);
+		err |= comedi_check_trigger_arg_is(&cmd->stop_arg, 0);
 		break;
 	default:
 		break;
@@ -2637,8 +2635,9 @@ static int ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 		bits |= ADC_START_TRIG_EXT_BITS;
 		if (cmd->start_arg & CR_INVERT)
 			bits |= ADC_START_TRIG_FALLING_BIT;
-	} else if (cmd->start_src == TRIG_NOW)
+	} else if (cmd->start_src == TRIG_NOW) {
 		bits |= ADC_START_TRIG_SOFT_BITS;
+	}
 	if (use_hw_sample_counter(cmd))
 		bits |= ADC_SAMPLE_COUNTER_EN_BIT;
 	writew(bits, devpriv->main_iobase + ADC_CONTROL0_REG);
@@ -2827,8 +2826,9 @@ static void handle_ai_interrupt(struct comedi_device *dev,
 		if (devpriv->ai_cmd_running) {
 			spin_unlock_irqrestore(&dev->spinlock, flags);
 			pio_drain_ai_fifo(dev);
-		} else
+		} else {
 			spin_unlock_irqrestore(&dev->spinlock, flags);
+		}
 	}
 	/*  if we are have all the data, then quit */
 	if ((cmd->stop_src == TRIG_COUNT &&
@@ -2978,7 +2978,7 @@ static void handle_ao_interrupt(struct comedi_device *dev,
 	unsigned long flags;
 
 	/* board might not support ao, in which case write_subdev is NULL */
-	if (s == NULL)
+	if (!s)
 		return;
 	async = s->async;
 	cmd = &async->cmd;
@@ -3312,20 +3312,20 @@ static int ao_cmdtest(struct comedi_device *dev, struct comedi_subdevice *s,
 
 	/* Step 1 : check if triggers are trivially valid */
 
-	err |= cfc_check_trigger_src(&cmd->start_src, TRIG_INT | TRIG_EXT);
-	err |= cfc_check_trigger_src(&cmd->scan_begin_src,
-				     TRIG_TIMER | TRIG_EXT);
-	err |= cfc_check_trigger_src(&cmd->convert_src, TRIG_NOW);
-	err |= cfc_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
-	err |= cfc_check_trigger_src(&cmd->stop_src, TRIG_NONE);
+	err |= comedi_check_trigger_src(&cmd->start_src, TRIG_INT | TRIG_EXT);
+	err |= comedi_check_trigger_src(&cmd->scan_begin_src,
+					TRIG_TIMER | TRIG_EXT);
+	err |= comedi_check_trigger_src(&cmd->convert_src, TRIG_NOW);
+	err |= comedi_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
+	err |= comedi_check_trigger_src(&cmd->stop_src, TRIG_NONE);
 
 	if (err)
 		return 1;
 
 	/* Step 2a : make sure trigger sources are unique */
 
-	err |= cfc_check_trigger_is_unique(cmd->start_src);
-	err |= cfc_check_trigger_is_unique(cmd->scan_begin_src);
+	err |= comedi_check_trigger_is_unique(cmd->start_src);
+	err |= comedi_check_trigger_is_unique(cmd->scan_begin_src);
 
 	/* Step 2b : and mutually compatible */
 
@@ -3340,11 +3340,11 @@ static int ao_cmdtest(struct comedi_device *dev, struct comedi_subdevice *s,
 
 	/* Step 3: check if arguments are trivially valid */
 
-	err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
+	err |= comedi_check_trigger_arg_is(&cmd->start_arg, 0);
 
 	if (cmd->scan_begin_src == TRIG_TIMER) {
-		err |= cfc_check_trigger_arg_min(&cmd->scan_begin_arg,
-						 thisboard->ao_scan_speed);
+		err |= comedi_check_trigger_arg_min(&cmd->scan_begin_arg,
+						    thisboard->ao_scan_speed);
 		if (get_ao_divisor(cmd->scan_begin_arg, cmd->flags) >
 		    max_counter_value) {
 			cmd->scan_begin_arg = (max_counter_value + 2) *
@@ -3353,8 +3353,9 @@ static int ao_cmdtest(struct comedi_device *dev, struct comedi_subdevice *s,
 		}
 	}
 
-	err |= cfc_check_trigger_arg_min(&cmd->chanlist_len, 1);
-	err |= cfc_check_trigger_arg_is(&cmd->scan_end_arg, cmd->chanlist_len);
+	err |= comedi_check_trigger_arg_min(&cmd->chanlist_len, 1);
+	err |= comedi_check_trigger_arg_is(&cmd->scan_end_arg,
+					   cmd->chanlist_len);
 
 	if (err)
 		return 3;
@@ -3817,8 +3818,9 @@ static int setup_subdevices(struct comedi_device *dev)
 		s->maxdata = 1;
 		s->range_table = &range_digital;
 		s->insn_bits = di_rbits;
-	} else
+	} else {
 		s->type = COMEDI_SUBD_UNUSED;
+	}
 
 	/*  digital output */
 	if (thisboard->layout == LAYOUT_64XX) {
@@ -3829,8 +3831,9 @@ static int setup_subdevices(struct comedi_device *dev)
 		s->maxdata = 1;
 		s->range_table = &range_digital;
 		s->insn_bits = do_wbits;
-	} else
+	} else {
 		s->type = COMEDI_SUBD_UNUSED;
+	}
 
 	/* 8255 */
 	s = &dev->subdevices[4];
@@ -3858,8 +3861,9 @@ static int setup_subdevices(struct comedi_device *dev)
 		s->range_table = &range_digital;
 		s->insn_config = dio_60xx_config_insn;
 		s->insn_bits = dio_60xx_wbits;
-	} else
+	} else {
 		s->type = COMEDI_SUBD_UNUSED;
+	}
 
 	/*  caldac */
 	s = &dev->subdevices[6];
@@ -3898,8 +3902,9 @@ static int setup_subdevices(struct comedi_device *dev)
 			ad8402_write(dev, i, s->maxdata / 2);
 			s->readback[i] = s->maxdata / 2;
 		}
-	} else
+	} else {
 		s->type = COMEDI_SUBD_UNUSED;
+	}
 
 	/* serial EEPROM, if present */
 	s = &dev->subdevices[8];
@@ -3909,8 +3914,9 @@ static int setup_subdevices(struct comedi_device *dev)
 		s->n_chan = 128;
 		s->maxdata = 0xffff;
 		s->insn_read = eeprom_read_insn;
-	} else
+	} else {
 		s->type = COMEDI_SUBD_UNUSED;
+	}
 
 	/*  user counter subd XXX */
 	s = &dev->subdevices[9];
diff --git a/drivers/staging/comedi/drivers/cb_pcidda.c b/drivers/staging/comedi/drivers/cb_pcidda.c
index 2b2cfcdda5bd..30c9e27d1ed9 100644
--- a/drivers/staging/comedi/drivers/cb_pcidda.c
+++ b/drivers/staging/comedi/drivers/cb_pcidda.c
@@ -36,11 +36,9 @@
  */
 
 #include <linux/module.h>
-#include <linux/pci.h>
 
-#include "../comedidev.h"
+#include "../comedi_pci.h"
 
-#include "comedi_fc.h"
 #include "8255.h"
 
 #define EEPROM_SIZE	128	/*  number of entries in eeprom */
diff --git a/drivers/staging/comedi/drivers/cb_pcimdas.c b/drivers/staging/comedi/drivers/cb_pcimdas.c
index 70dd2c9eecdb..c458e5010a74 100644
--- a/drivers/staging/comedi/drivers/cb_pcimdas.c
+++ b/drivers/staging/comedi/drivers/cb_pcimdas.c
@@ -42,37 +42,112 @@
  */
 
 #include <linux/module.h>
-#include <linux/pci.h>
 #include <linux/interrupt.h>
 
-#include "../comedidev.h"
+#include "../comedi_pci.h"
 
+#include "comedi_8254.h"
 #include "plx9052.h"
 #include "8255.h"
 
-/* Registers for the PCIM-DAS1602/16 and PCIe-DAS1602/16 */
-
-/* DAC Offsets */
-#define ADC_TRIG 0
-#define DAC0_OFFSET 2
-#define DAC1_OFFSET 4
-
-/* AI and Counter Constants */
-#define MUX_LIMITS 0
-#define MAIN_CONN_DIO 1
-#define ADC_STAT 2
-#define ADC_CONV_STAT 3
-#define ADC_INT 4
-#define ADC_PACER 5
-#define BURST_MODE 6
-#define PROG_GAIN 7
-#define CLK8254_1_DATA 8
-#define CLK8254_2_DATA 9
-#define CLK8254_3_DATA 10
-#define CLK8254_CONTROL 11
-#define USER_COUNTER 12
-#define RESID_COUNT_H 13
-#define RESID_COUNT_L 14
+/*
+ * PCI Bar 1 Register map
+ * see plx9052.h for register and bit defines
+ */
+
+/*
+ * PCI Bar 2 Register map (devpriv->daqio)
+ */
+#define PCIMDAS_AI_REG			0x00
+#define PCIMDAS_AI_SOFTTRIG_REG		0x00
+#define PCIMDAS_AO_REG(x)		(0x02 + ((x) * 2))
+
+/*
+ * PCI Bar 3 Register map (devpriv->BADR3)
+ */
+#define PCIMDAS_MUX_REG			0x00
+#define PCIMDAS_MUX(_lo, _hi)		((_lo) | ((_hi) << 4))
+#define PCIMDAS_DI_DO_REG		0x01
+#define PCIMDAS_STATUS_REG		0x02
+#define PCIMDAS_STATUS_EOC		BIT(7)
+#define PCIMDAS_STATUS_UB		BIT(6)
+#define PCIMDAS_STATUS_MUX		BIT(5)
+#define PCIMDAS_STATUS_CLK		BIT(4)
+#define PCIMDAS_STATUS_TO_CURR_MUX(x)	((x) & 0xf)
+#define PCIMDAS_CONV_STATUS_REG		0x03
+#define PCIMDAS_CONV_STATUS_EOC		BIT(7)
+#define PCIMDAS_CONV_STATUS_EOB		BIT(6)
+#define PCIMDAS_CONV_STATUS_EOA		BIT(5)
+#define PCIMDAS_CONV_STATUS_FNE		BIT(4)
+#define PCIMDAS_CONV_STATUS_FHF		BIT(3)
+#define PCIMDAS_CONV_STATUS_OVERRUN	BIT(2)
+#define PCIMDAS_IRQ_REG			0x04
+#define PCIMDAS_IRQ_INTE		BIT(7)
+#define PCIMDAS_IRQ_INT			BIT(6)
+#define PCIMDAS_IRQ_OVERRUN		BIT(4)
+#define PCIMDAS_IRQ_EOA			BIT(3)
+#define PCIMDAS_IRQ_EOA_INT_SEL		BIT(2)
+#define PCIMDAS_IRQ_INTSEL(x)		((x) << 0)
+#define PCIMDAS_IRQ_INTSEL_EOC		PCIMDAS_IRQ_INTSEL(0)
+#define PCIMDAS_IRQ_INTSEL_FNE		PCIMDAS_IRQ_INTSEL(1)
+#define PCIMDAS_IRQ_INTSEL_EOB		PCIMDAS_IRQ_INTSEL(2)
+#define PCIMDAS_IRQ_INTSEL_FHF_EOA	PCIMDAS_IRQ_INTSEL(3)
+#define PCIMDAS_PACER_REG		0x05
+#define PCIMDAS_PACER_GATE_STATUS	BIT(6)
+#define PCIMDAS_PACER_GATE_POL		BIT(5)
+#define PCIMDAS_PACER_GATE_LATCH	BIT(4)
+#define PCIMDAS_PACER_GATE_EN		BIT(3)
+#define PCIMDAS_PACER_EXT_PACER_POL	BIT(2)
+#define PCIMDAS_PACER_SRC(x)		((x) << 0)
+#define PCIMDAS_PACER_SRC_POLLED	PCIMDAS_PACER_SRC(0)
+#define PCIMDAS_PACER_SRC_EXT		PCIMDAS_PACER_SRC(2)
+#define PCIMDAS_PACER_SRC_INT		PCIMDAS_PACER_SRC(3)
+#define PCIMDAS_PACER_SRC_MASK		(3 << 0)
+#define PCIMDAS_BURST_REG		0x06
+#define PCIMDAS_BURST_BME		BIT(1)
+#define PCIMDAS_BURST_CONV_EN		BIT(0)
+#define PCIMDAS_GAIN_REG		0x07
+#define PCIMDAS_8254_BASE		0x08
+#define PCIMDAS_USER_CNTR_REG		0x0c
+#define PCIMDAS_USER_CNTR_CTR1_CLK_SEL	BIT(0)
+#define PCIMDAS_RESIDUE_MSB_REG		0x0d
+#define PCIMDAS_RESIDUE_LSB_REG		0x0e
+
+/*
+ * PCI Bar 4 Register map (dev->iobase)
+ */
+#define PCIMDAS_8255_BASE		0x00
+
+static const struct comedi_lrange cb_pcimdas_ai_bip_range = {
+	4, {
+		BIP_RANGE(10),
+		BIP_RANGE(5),
+		BIP_RANGE(2.5),
+		BIP_RANGE(1.25)
+	}
+};
+
+static const struct comedi_lrange cb_pcimdas_ai_uni_range = {
+	4, {
+		UNI_RANGE(10),
+		UNI_RANGE(5),
+		UNI_RANGE(2.5),
+		UNI_RANGE(1.25)
+	}
+};
+
+/*
+ * The Analog Output range is not programmable. The DAC ranges are
+ * jumper-settable on the board. The settings are not software-readable.
+ */
+static const struct comedi_lrange cb_pcimdas_ao_range = {
+	4, {
+		BIP_RANGE(10),
+		BIP_RANGE(5),
+		UNI_RANGE(10),
+		UNI_RANGE(5)
+	}
+};
 
 /*
  * this structure is for data unique to this hardware driver.  If
@@ -94,59 +169,47 @@ static int cb_pcimdas_ai_eoc(struct comedi_device *dev,
 	struct cb_pcimdas_private *devpriv = dev->private;
 	unsigned int status;
 
-	status = inb(devpriv->BADR3 + 2);
-	if ((status & 0x80) == 0)
+	status = inb(devpriv->BADR3 + PCIMDAS_STATUS_REG);
+	if ((status & PCIMDAS_STATUS_EOC) == 0)
 		return 0;
 	return -EBUSY;
 }
 
-static int cb_pcimdas_ai_rinsn(struct comedi_device *dev,
-			       struct comedi_subdevice *s,
-			       struct comedi_insn *insn, unsigned int *data)
+static int cb_pcimdas_ai_insn_read(struct comedi_device *dev,
+				   struct comedi_subdevice *s,
+				   struct comedi_insn *insn,
+				   unsigned int *data)
 {
 	struct cb_pcimdas_private *devpriv = dev->private;
+	unsigned int chan = CR_CHAN(insn->chanspec);
+	unsigned int range = CR_RANGE(insn->chanspec);
 	int n;
 	unsigned int d;
-	int chan = CR_CHAN(insn->chanspec);
-	unsigned short chanlims;
-	int maxchans;
 	int ret;
 
 	/*  only support sw initiated reads from a single channel */
 
-	/* check channel number */
-	if ((inb(devpriv->BADR3 + 2) & 0x20) == 0)	/* differential mode */
-		maxchans = s->n_chan / 2;
-	else
-		maxchans = s->n_chan;
-
-	if (chan > (maxchans - 1))
-		return -ETIMEDOUT;	/* *** Wrong error code. Fixme. */
-
 	/* configure for sw initiated read */
-	d = inb(devpriv->BADR3 + 5);
-	if ((d & 0x03) > 0) {	/* only reset if needed. */
-		d = d & 0xfd;
-		outb(d, devpriv->BADR3 + 5);
+	d = inb(devpriv->BADR3 + PCIMDAS_PACER_REG);
+	if ((d & PCIMDAS_PACER_SRC_MASK) != PCIMDAS_PACER_SRC_POLLED) {
+		d &= ~PCIMDAS_PACER_SRC_MASK;
+		d |= PCIMDAS_PACER_SRC_POLLED;
+		outb(d, devpriv->BADR3 + PCIMDAS_PACER_REG);
 	}
 
 	/* set bursting off, conversions on */
-	outb(0x01, devpriv->BADR3 + 6);
+	outb(PCIMDAS_BURST_CONV_EN, devpriv->BADR3 + PCIMDAS_BURST_REG);
 
-	/* set range to 10V. UP/BP is controlled by a switch on the board */
-	outb(0x00, devpriv->BADR3 + 7);
+	/* set range */
+	outb(range, devpriv->BADR3 + PCIMDAS_GAIN_REG);
 
-	/*
-	 * write channel limits to multiplexer, set Low (bits 0-3) and
-	 * High (bits 4-7) channels to chan.
-	 */
-	chanlims = chan | (chan << 4);
-	outb(chanlims, devpriv->BADR3 + 0);
+	/* set mux for single channel scan */
+	outb(PCIMDAS_MUX(chan, chan), devpriv->BADR3 + PCIMDAS_MUX_REG);
 
 	/* convert n samples */
 	for (n = 0; n < insn->n; n++) {
 		/* trigger conversion */
-		outw(0, devpriv->daqio + 0);
+		outw(0, devpriv->daqio + PCIMDAS_AI_SOFTTRIG_REG);
 
 		/* wait for conversion to end */
 		ret = comedi_timeout(dev, s, insn, cb_pcimdas_ai_eoc, 0);
@@ -154,7 +217,7 @@ static int cb_pcimdas_ai_rinsn(struct comedi_device *dev,
 			return ret;
 
 		/* read data */
-		data[n] = inw(devpriv->daqio + 0);
+		data[n] = inw(devpriv->daqio + PCIMDAS_AI_REG);
 	}
 
 	/* return the number of samples read/written */
@@ -169,20 +232,128 @@ static int cb_pcimdas_ao_insn_write(struct comedi_device *dev,
 	struct cb_pcimdas_private *devpriv = dev->private;
 	unsigned int chan = CR_CHAN(insn->chanspec);
 	unsigned int val = s->readback[chan];
-	unsigned int reg = (chan) ? DAC1_OFFSET : DAC0_OFFSET;
 	int i;
 
 	for (i = 0; i < insn->n; i++) {
 		val = data[i];
-		outw(val, devpriv->daqio + reg);
+		outw(val, devpriv->daqio + PCIMDAS_AO_REG(chan));
 	}
 	s->readback[chan] = val;
 
 	return insn->n;
 }
 
+static int cb_pcimdas_di_insn_read(struct comedi_device *dev,
+				   struct comedi_subdevice *s,
+				   struct comedi_insn *insn,
+				   unsigned int *data)
+{
+	struct cb_pcimdas_private *devpriv = dev->private;
+	unsigned int val;
+
+	val = inb(devpriv->BADR3 + PCIMDAS_DI_DO_REG);
+
+	data[1] = val & 0x0f;
+
+	return insn->n;
+}
+
+static int cb_pcimdas_do_insn_write(struct comedi_device *dev,
+				    struct comedi_subdevice *s,
+				    struct comedi_insn *insn,
+				    unsigned int *data)
+{
+	struct cb_pcimdas_private *devpriv = dev->private;
+
+	if (comedi_dio_update_state(s, data))
+		outb(s->state, devpriv->BADR3 + PCIMDAS_DI_DO_REG);
+
+	data[1] = s->state;
+
+	return insn->n;
+}
+
+static int cb_pcimdas_counter_insn_config(struct comedi_device *dev,
+					  struct comedi_subdevice *s,
+					  struct comedi_insn *insn,
+					  unsigned int *data)
+{
+	struct cb_pcimdas_private *devpriv = dev->private;
+	unsigned int ctrl;
+
+	switch (data[0]) {
+	case INSN_CONFIG_SET_CLOCK_SRC:
+		switch (data[1]) {
+		case 0:	/* internal 100 kHz clock */
+			ctrl = PCIMDAS_USER_CNTR_CTR1_CLK_SEL;
+			break;
+		case 1:	/* external clk on pin 21 */
+			ctrl = 0;
+			break;
+		default:
+			return -EINVAL;
+		}
+		outb(ctrl, devpriv->BADR3 + PCIMDAS_USER_CNTR_REG);
+		break;
+	case INSN_CONFIG_GET_CLOCK_SRC:
+		ctrl = inb(devpriv->BADR3 + PCIMDAS_USER_CNTR_REG);
+		if (ctrl & PCIMDAS_USER_CNTR_CTR1_CLK_SEL) {
+			data[1] = 0;
+			data[2] = I8254_OSC_BASE_100KHZ;
+		} else {
+			data[1] = 1;
+			data[2] = 0;
+		}
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return insn->n;
+}
+
+static unsigned int cb_pcimdas_pacer_clk(struct comedi_device *dev)
+{
+	struct cb_pcimdas_private *devpriv = dev->private;
+	unsigned int status;
+
+	/* The Pacer Clock jumper selects a 10 MHz or 1 MHz clock */
+	status = inb(devpriv->BADR3 + PCIMDAS_STATUS_REG);
+	if (status & PCIMDAS_STATUS_CLK)
+		return I8254_OSC_BASE_10MHZ;
+	return I8254_OSC_BASE_1MHZ;
+}
+
+static bool cb_pcimdas_is_ai_se(struct comedi_device *dev)
+{
+	struct cb_pcimdas_private *devpriv = dev->private;
+	unsigned int status;
+
+	/*
+	 * The number of Analog Input channels is set with the
+	 * Analog Input Mode Switch on the board. The board can
+	 * have 16 single-ended or 8 differential channels.
+	 */
+	status = inb(devpriv->BADR3 + PCIMDAS_STATUS_REG);
+	return status & PCIMDAS_STATUS_MUX;
+}
+
+static bool cb_pcimdas_is_ai_uni(struct comedi_device *dev)
+{
+	struct cb_pcimdas_private *devpriv = dev->private;
+	unsigned int status;
+
+	/*
+	 * The Analog Input range polarity is set with the
+	 * Analog Input Polarity Switch on the board. The
+	 * inputs can be set to Unipolar or Bipolar ranges.
+	 */
+	status = inb(devpriv->BADR3 + PCIMDAS_STATUS_REG);
+	return status & PCIMDAS_STATUS_UB;
+}
+
 static int cb_pcimdas_auto_attach(struct comedi_device *dev,
-					    unsigned long context_unused)
+				  unsigned long context_unused)
 {
 	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
 	struct cb_pcimdas_private *devpriv;
@@ -201,42 +372,79 @@ static int cb_pcimdas_auto_attach(struct comedi_device *dev,
 	devpriv->BADR3 = pci_resource_start(pcidev, 3);
 	dev->iobase = pci_resource_start(pcidev, 4);
 
-	ret = comedi_alloc_subdevices(dev, 3);
+	dev->pacer = comedi_8254_init(devpriv->BADR3 + PCIMDAS_8254_BASE,
+				      cb_pcimdas_pacer_clk(dev),
+				      I8254_IO8, 0);
+	if (!dev->pacer)
+		return -ENOMEM;
+
+	ret = comedi_alloc_subdevices(dev, 6);
 	if (ret)
 		return ret;
 
+	/* Analog Input subdevice */
 	s = &dev->subdevices[0];
-	/* dev->read_subdev=s; */
-	/*  analog input subdevice */
-	s->type = COMEDI_SUBD_AI;
-	s->subdev_flags = SDF_READABLE | SDF_GROUND;
-	s->n_chan = 16;
-	s->maxdata = 0xffff;
-	s->range_table = &range_unknown;
-	s->len_chanlist = 1;	/*  This is the maximum chanlist length that */
-	/*  the board can handle */
-	s->insn_read = cb_pcimdas_ai_rinsn;
+	s->type		= COMEDI_SUBD_AI;
+	s->subdev_flags	= SDF_READABLE;
+	if (cb_pcimdas_is_ai_se(dev)) {
+		s->subdev_flags	|= SDF_GROUND;
+		s->n_chan	= 16;
+	} else {
+		s->subdev_flags	|= SDF_DIFF;
+		s->n_chan	= 8;
+	}
+	s->maxdata	= 0xffff;
+	s->range_table	= cb_pcimdas_is_ai_uni(dev) ? &cb_pcimdas_ai_uni_range
+						    : &cb_pcimdas_ai_bip_range;
+	s->insn_read	= cb_pcimdas_ai_insn_read;
 
+	/* Analog Output subdevice */
 	s = &dev->subdevices[1];
-	/*  analog output subdevice */
-	s->type = COMEDI_SUBD_AO;
-	s->subdev_flags = SDF_WRITABLE;
-	s->n_chan = 2;
-	s->maxdata = 0xfff;
-	/* ranges are hardware settable, but not software readable. */
-	s->range_table = &range_unknown;
-	s->insn_write = cb_pcimdas_ao_insn_write;
+	s->type		= COMEDI_SUBD_AO;
+	s->subdev_flags	= SDF_WRITABLE;
+	s->n_chan	= 2;
+	s->maxdata	= 0xfff;
+	s->range_table	= &cb_pcimdas_ao_range;
+	s->insn_write	= cb_pcimdas_ao_insn_write;
 
 	ret = comedi_alloc_subdev_readback(s);
 	if (ret)
 		return ret;
 
+	/* Digital I/O subdevice */
 	s = &dev->subdevices[2];
-	/* digital i/o subdevice */
-	ret = subdev_8255_init(dev, s, NULL, 0x00);
+	ret = subdev_8255_init(dev, s, NULL, PCIMDAS_8255_BASE);
 	if (ret)
 		return ret;
 
+	/* Digital Input subdevice (main connector) */
+	s = &dev->subdevices[3];
+	s->type		= COMEDI_SUBD_DI;
+	s->subdev_flags	= SDF_READABLE;
+	s->n_chan	= 4;
+	s->maxdata	= 1;
+	s->range_table	= &range_digital;
+	s->insn_read	= cb_pcimdas_di_insn_read;
+
+	/* Digital Output subdevice (main connector) */
+	s = &dev->subdevices[4];
+	s->type		= COMEDI_SUBD_DO;
+	s->subdev_flags	= SDF_WRITABLE;
+	s->n_chan	= 4;
+	s->maxdata	= 1;
+	s->range_table	= &range_digital;
+	s->insn_write	= cb_pcimdas_do_insn_write;
+
+	/* Counter subdevice (8254) */
+	s = &dev->subdevices[5];
+	comedi_8254_subdevice_init(s, dev->pacer);
+
+	dev->pacer->insn_config = cb_pcimdas_counter_insn_config;
+
+	/* counters 1 and 2 are used internally for the pacer */
+	comedi_8254_set_busy(dev->pacer, 1, true);
+	comedi_8254_set_busy(dev->pacer, 2, true);
+
 	return 0;
 }
 
diff --git a/drivers/staging/comedi/drivers/cb_pcimdda.c b/drivers/staging/comedi/drivers/cb_pcimdda.c
index 03043e7b9b58..a4781dbbdd82 100644
--- a/drivers/staging/comedi/drivers/cb_pcimdda.c
+++ b/drivers/staging/comedi/drivers/cb_pcimdda.c
@@ -75,9 +75,8 @@ Configuration Options: not applicable, uses PCI auto config
  */
 
 #include <linux/module.h>
-#include <linux/pci.h>
 
-#include "../comedidev.h"
+#include "../comedi_pci.h"
 
 #include "8255.h"
 
@@ -134,7 +133,7 @@ static int cb_pcimdda_ao_insn_read(struct comedi_device *dev,
 }
 
 static int cb_pcimdda_auto_attach(struct comedi_device *dev,
-					    unsigned long context_unused)
+				  unsigned long context_unused)
 {
 	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
 	struct comedi_subdevice *s;
diff --git a/drivers/staging/comedi/drivers/comedi_8254.c b/drivers/staging/comedi/drivers/comedi_8254.c
new file mode 100644
index 000000000000..0d5d56b61f60
--- /dev/null
+++ b/drivers/staging/comedi/drivers/comedi_8254.c
@@ -0,0 +1,664 @@
+/*
+ * comedi_8254.c
+ * Generic 8254 timer/counter support
+ * Copyright (C) 2014 H Hartley Sweeten <hsweeten@visionengravers.com>
+ *
+ * Based on 8253.h and various subdevice implementations in comedi drivers.
+ *
+ * COMEDI - Linux Control and Measurement Device Interface
+ * Copyright (C) 2000 David A. Schleef <ds@schleef.org>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+/*
+ * Module: comedi_8254
+ * Description: Generic 8254 timer/counter support
+ * Author: H Hartley Sweeten <hsweeten@visionengravers.com>
+ * Updated: Thu Jan 8 16:45:45 MST 2015
+ * Status: works
+ *
+ * This module is not used directly by end-users. Rather, it is used by other
+ * drivers to provide support for an 8254 Programmable Interval Timer. These
+ * counters are typically used to generate the pacer clock used for data
+ * acquisition. Some drivers also expose the counters for general purpose use.
+ *
+ * This module provides the following basic functions:
+ *
+ * comedi_8254_init() / comedi_8254_mm_init()
+ *	Initializes this module to access the 8254 registers. The _mm version
+ *	sets up the module for MMIO register access the other for PIO access.
+ *	The pointer returned from these functions is normally stored in the
+ *	comedi_device dev->pacer and will be freed by the comedi core during
+ *	the driver (*detach). If a driver has multiple 8254 devices, they need
+ *	to be stored in the drivers private data and freed when the driver is
+ *	detached.
+ *
+ *	NOTE: The counters are reset by setting them to I8254_MODE0 as part of
+ *	this initialization.
+ *
+ * comedi_8254_set_mode()
+ *	Sets a counters operation mode:
+ *		I8254_MODE0	Interrupt on terminal count
+ *		I8254_MODE1	Hardware retriggerable one-shot
+ *		I8254_MODE2	Rate generator
+ *		I8254_MODE3	Square wave mode
+ *		I8254_MODE4	Software triggered strobe
+ *		I8254_MODE5	Hardware triggered strobe (retriggerable)
+ *
+ *	In addition I8254_BCD and I8254_BINARY specify the counting mode:
+ *		I8254_BCD	BCD counting
+ *		I8254_BINARY	Binary counting
+ *
+ * comedi_8254_write()
+ *	Writes an initial value to a counter.
+ *
+ *	The largest possible initial count is 0; this is equivalent to 2^16
+ *	for binary counting and 10^4 for BCD counting.
+ *
+ *	NOTE: The counter does not stop when it reaches zero. In Mode 0, 1, 4,
+ *	and 5 the counter "wraps around" to the highest count, either 0xffff
+ *	for binary counting or 9999 for BCD counting, and continues counting.
+ *	Modes 2 and 3 are periodic; the counter reloads itself with the initial
+ *	count and continues counting from there.
+ *
+ * comedi_8254_read()
+ *	Reads the current value from a counter.
+ *
+ * comedi_8254_status()
+ *	Reads the status of a counter.
+ *
+ * comedi_8254_load()
+ *	Sets a counters operation mode and writes the initial value.
+ *
+ * Typically the pacer clock is created by cascading two of the 16-bit counters
+ * to create a 32-bit rate generator (I8254_MODE2). These functions are
+ * provided to handle the cascaded counters:
+ *
+ * comedi_8254_ns_to_timer()
+ *	Calculates the divisor value needed for a single counter to generate
+ *	ns timing.
+ *
+ * comedi_8254_cascade_ns_to_timer()
+ *	Calculates the two divisor values needed to the generate the pacer
+ *	clock (in ns).
+ *
+ * comedi_8254_update_divisors()
+ *	Transfers the intermediate divisor values to the current divisors.
+ *
+ * comedi_8254_pacer_enable()
+ *	Programs the mode of the cascaded counters and writes the current
+ *	divisor values.
+ *
+ * To expose the counters as a subdevice for general purpose use the following
+ * functions a provided:
+ *
+ * comedi_8254_subdevice_init()
+ *	Initializes a comedi_subdevice to use the 8254 timer.
+ *
+ * comedi_8254_set_busy()
+ *	Internally flags a counter as "busy". This is done to protect the
+ *	counters that are used for the cascaded 32-bit pacer.
+ *
+ * The subdevice provides (*insn_read) and (*insn_write) operations to read
+ * the current value and write an initial value to a counter. A (*insn_config)
+ * operation is also provided to handle the following comedi instructions:
+ *
+ *	INSN_CONFIG_SET_COUNTER_MODE	calls comedi_8254_set_mode()
+ *	INSN_CONFIG_8254_READ_STATUS	calls comedi_8254_status()
+ *
+ * The (*insn_config) member of comedi_8254 can be initialized by the external
+ * driver to handle any additional instructions.
+ *
+ * NOTE: Gate control, clock routing, and any interrupt handling for the
+ * counters is not handled by this module. These features are driver dependent.
+ */
+
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/io.h>
+
+#include "../comedidev.h"
+
+#include "comedi_8254.h"
+
+static unsigned int __i8254_read(struct comedi_8254 *i8254, unsigned int reg)
+{
+	unsigned int reg_offset = (reg * i8254->iosize) << i8254->regshift;
+	unsigned int val;
+
+	switch (i8254->iosize) {
+	default:
+	case I8254_IO8:
+		if (i8254->mmio)
+			val = readb(i8254->mmio + reg_offset);
+		else
+			val = inb(i8254->iobase + reg_offset);
+		break;
+	case I8254_IO16:
+		if (i8254->mmio)
+			val = readw(i8254->mmio + reg_offset);
+		else
+			val = inw(i8254->iobase + reg_offset);
+		break;
+	case I8254_IO32:
+		if (i8254->mmio)
+			val = readl(i8254->mmio + reg_offset);
+		else
+			val = inl(i8254->iobase + reg_offset);
+		break;
+	}
+	return val & 0xff;
+}
+
+static void __i8254_write(struct comedi_8254 *i8254,
+			  unsigned int val, unsigned int reg)
+{
+	unsigned int reg_offset = (reg * i8254->iosize) << i8254->regshift;
+
+	switch (i8254->iosize) {
+	default:
+	case I8254_IO8:
+		if (i8254->mmio)
+			writeb(val, i8254->mmio + reg_offset);
+		else
+			outb(val, i8254->iobase + reg_offset);
+		break;
+	case I8254_IO16:
+		if (i8254->mmio)
+			writew(val, i8254->mmio + reg_offset);
+		else
+			outw(val, i8254->iobase + reg_offset);
+		break;
+	case I8254_IO32:
+		if (i8254->mmio)
+			writel(val, i8254->mmio + reg_offset);
+		else
+			outl(val, i8254->iobase + reg_offset);
+		break;
+	}
+}
+
+/**
+ * comedi_8254_status - return the status of a counter
+ * @i8254:	comedi_8254 struct for the timer
+ * @counter:	the counter number
+ */
+unsigned int comedi_8254_status(struct comedi_8254 *i8254, unsigned int counter)
+{
+	unsigned int cmd;
+
+	if (counter > 2)
+		return 0;
+
+	cmd = I8254_CTRL_READBACK_STATUS | I8254_CTRL_READBACK_SEL_CTR(counter);
+	__i8254_write(i8254, cmd, I8254_CTRL_REG);
+
+	return __i8254_read(i8254, counter);
+}
+EXPORT_SYMBOL_GPL(comedi_8254_status);
+
+/**
+ * comedi_8254_read - read the current counter value
+ * @i8254:	comedi_8254 struct for the timer
+ * @counter:	the counter number
+ */
+unsigned int comedi_8254_read(struct comedi_8254 *i8254, unsigned int counter)
+{
+	unsigned int val;
+
+	if (counter > 2)
+		return 0;
+
+	/* latch counter */
+	__i8254_write(i8254, I8254_CTRL_SEL_CTR(counter) | I8254_CTRL_LATCH,
+		      I8254_CTRL_REG);
+
+	/* read LSB then MSB */
+	val = __i8254_read(i8254, counter);
+	val |= (__i8254_read(i8254, counter) << 8);
+
+	return val;
+}
+EXPORT_SYMBOL_GPL(comedi_8254_read);
+
+/**
+ * comedi_8254_write - load a 16-bit initial counter value
+ * @i8254:	comedi_8254 struct for the timer
+ * @counter:	the counter number
+ * @val:	the initial value
+ */
+void comedi_8254_write(struct comedi_8254 *i8254,
+		       unsigned int counter, unsigned int val)
+{
+	unsigned int byte;
+
+	if (counter > 2)
+		return;
+	if (val > 0xffff)
+		return;
+
+	/* load LSB then MSB */
+	byte = val & 0xff;
+	__i8254_write(i8254, byte, counter);
+	byte = (val >> 8) & 0xff;
+	__i8254_write(i8254, byte, counter);
+}
+EXPORT_SYMBOL_GPL(comedi_8254_write);
+
+/**
+ * comedi_8254_set_mode - set the mode of a counter
+ * @i8254:	comedi_8254 struct for the timer
+ * @counter:	the counter number
+ * @mode:	the I8254_MODEx and I8254_BCD|I8254_BINARY
+ */
+int comedi_8254_set_mode(struct comedi_8254 *i8254, unsigned int counter,
+			 unsigned int mode)
+{
+	unsigned int byte;
+
+	if (counter > 2)
+		return -EINVAL;
+	if (mode > (I8254_MODE5 | I8254_BCD))
+		return -EINVAL;
+
+	byte = I8254_CTRL_SEL_CTR(counter) |	/* select counter */
+	       I8254_CTRL_LSB_MSB |		/* load LSB then MSB */
+	       mode;				/* mode and BCD|binary */
+	__i8254_write(i8254, byte, I8254_CTRL_REG);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(comedi_8254_set_mode);
+
+/**
+ * comedi_8254_load - program the mode and initial count of a counter
+ * @i8254:	comedi_8254 struct for the timer
+ * @counter:	the counter number
+ * @mode:	the I8254_MODEx and I8254_BCD|I8254_BINARY
+ * @val:	the initial value
+ */
+int comedi_8254_load(struct comedi_8254 *i8254, unsigned int counter,
+		     unsigned int val, unsigned int mode)
+{
+	if (counter > 2)
+		return -EINVAL;
+	if (val > 0xffff)
+		return -EINVAL;
+	if (mode > (I8254_MODE5 | I8254_BCD))
+		return -EINVAL;
+
+	comedi_8254_set_mode(i8254, counter, mode);
+	comedi_8254_write(i8254, counter, val);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(comedi_8254_load);
+
+/**
+ * comedi_8254_pacer_enable - set the mode and load the cascaded counters
+ * @i8254:	comedi_8254 struct for the timer
+ * @counter1:	the counter number for the first divisor
+ * @counter2:	the counter number for the second divisor
+ * @enable:	flag to enable (load) the counters
+ */
+void comedi_8254_pacer_enable(struct comedi_8254 *i8254,
+			      unsigned int counter1,
+			      unsigned int counter2,
+			      bool enable)
+{
+	unsigned int mode;
+
+	if (counter1 > 2 || counter2 > 2 || counter1 == counter2)
+		return;
+
+	if (enable)
+		mode = I8254_MODE2 | I8254_BINARY;
+	else
+		mode = I8254_MODE0 | I8254_BINARY;
+
+	comedi_8254_set_mode(i8254, counter1, mode);
+	comedi_8254_set_mode(i8254, counter2, mode);
+
+	if (enable) {
+		/*
+		 * Divisors are loaded second counter then first counter to
+		 * avoid possible issues with the first counter expiring
+		 * before the second counter is loaded.
+		 */
+		comedi_8254_write(i8254, counter2, i8254->divisor2);
+		comedi_8254_write(i8254, counter1, i8254->divisor1);
+	}
+}
+EXPORT_SYMBOL_GPL(comedi_8254_pacer_enable);
+
+/**
+ * comedi_8254_update_divisors - update the divisors for the cascaded counters
+ * @i8254:	comedi_8254 struct for the timer
+ */
+void comedi_8254_update_divisors(struct comedi_8254 *i8254)
+{
+	/* masking is done since counter maps zero to 0x10000 */
+	i8254->divisor = i8254->next_div & 0xffff;
+	i8254->divisor1 = i8254->next_div1 & 0xffff;
+	i8254->divisor2 = i8254->next_div2 & 0xffff;
+}
+EXPORT_SYMBOL_GPL(comedi_8254_update_divisors);
+
+/**
+ * comedi_8254_cascade_ns_to_timer - calculate the cascaded divisor values
+ * @i8254:	comedi_8254 struct for the timer
+ * @nanosec:	the desired ns time
+ * @flags:	comedi_cmd flags
+ */
+void comedi_8254_cascade_ns_to_timer(struct comedi_8254 *i8254,
+				     unsigned int *nanosec,
+				     unsigned int flags)
+{
+	unsigned int d1 = i8254->next_div1 ? i8254->next_div1 : I8254_MAX_COUNT;
+	unsigned int d2 = i8254->next_div2 ? i8254->next_div2 : I8254_MAX_COUNT;
+	unsigned int div = d1 * d2;
+	unsigned int ns_lub = 0xffffffff;
+	unsigned int ns_glb = 0;
+	unsigned int d1_lub = 0;
+	unsigned int d1_glb = 0;
+	unsigned int d2_lub = 0;
+	unsigned int d2_glb = 0;
+	unsigned int start;
+	unsigned int ns;
+	unsigned int ns_low;
+	unsigned int ns_high;
+
+	/* exit early if everything is already correct */
+	if (div * i8254->osc_base == *nanosec &&
+	    d1 > 1 && d1 <= I8254_MAX_COUNT &&
+	    d2 > 1 && d2 <= I8254_MAX_COUNT &&
+	    /* check for overflow */
+	    div > d1 && div > d2 &&
+	    div * i8254->osc_base > div &&
+	    div * i8254->osc_base > i8254->osc_base)
+		return;
+
+	div = *nanosec / i8254->osc_base;
+	d2 = I8254_MAX_COUNT;
+	start = div / d2;
+	if (start < 2)
+		start = 2;
+	for (d1 = start; d1 <= div / d1 + 1 && d1 <= I8254_MAX_COUNT; d1++) {
+		for (d2 = div / d1;
+		     d1 * d2 <= div + d1 + 1 && d2 <= I8254_MAX_COUNT; d2++) {
+			ns = i8254->osc_base * d1 * d2;
+			if (ns <= *nanosec && ns > ns_glb) {
+				ns_glb = ns;
+				d1_glb = d1;
+				d2_glb = d2;
+			}
+			if (ns >= *nanosec && ns < ns_lub) {
+				ns_lub = ns;
+				d1_lub = d1;
+				d2_lub = d2;
+			}
+		}
+	}
+
+	switch (flags & CMDF_ROUND_MASK) {
+	case CMDF_ROUND_NEAREST:
+	default:
+		ns_high = d1_lub * d2_lub * i8254->osc_base;
+		ns_low = d1_glb * d2_glb * i8254->osc_base;
+		if (ns_high - *nanosec < *nanosec - ns_low) {
+			d1 = d1_lub;
+			d2 = d2_lub;
+		} else {
+			d1 = d1_glb;
+			d2 = d2_glb;
+		}
+		break;
+	case CMDF_ROUND_UP:
+		d1 = d1_lub;
+		d2 = d2_lub;
+		break;
+	case CMDF_ROUND_DOWN:
+		d1 = d1_glb;
+		d2 = d2_glb;
+		break;
+	}
+
+	*nanosec = d1 * d2 * i8254->osc_base;
+	i8254->next_div1 = d1;
+	i8254->next_div2 = d2;
+}
+EXPORT_SYMBOL_GPL(comedi_8254_cascade_ns_to_timer);
+
+/**
+ * comedi_8254_ns_to_timer - calculate the divisor value for nanosec timing
+ * @i8254:	comedi_8254 struct for the timer
+ * @nanosec:	the desired ns time
+ * @flags:	comedi_cmd flags
+ */
+void comedi_8254_ns_to_timer(struct comedi_8254 *i8254,
+			     unsigned int *nanosec, unsigned int flags)
+{
+	unsigned int divisor;
+
+	switch (flags & CMDF_ROUND_MASK) {
+	default:
+	case CMDF_ROUND_NEAREST:
+		divisor = DIV_ROUND_CLOSEST(*nanosec, i8254->osc_base);
+		break;
+	case CMDF_ROUND_UP:
+		divisor = DIV_ROUND_UP(*nanosec, i8254->osc_base);
+		break;
+	case CMDF_ROUND_DOWN:
+		divisor = *nanosec / i8254->osc_base;
+		break;
+	}
+	if (divisor < 2)
+		divisor = 2;
+	if (divisor > I8254_MAX_COUNT)
+		divisor = I8254_MAX_COUNT;
+
+	*nanosec = divisor * i8254->osc_base;
+	i8254->next_div = divisor;
+}
+EXPORT_SYMBOL_GPL(comedi_8254_ns_to_timer);
+
+/**
+ * comedi_8254_set_busy - set/clear the "busy" flag for a given counter
+ * @i8254:	comedi_8254 struct for the timer
+ * @counter:	the counter number
+ * @busy:	set/clear flag
+ */
+void comedi_8254_set_busy(struct comedi_8254 *i8254,
+			  unsigned int counter, bool busy)
+{
+	if (counter < 3)
+		i8254->busy[counter] = busy;
+}
+EXPORT_SYMBOL_GPL(comedi_8254_set_busy);
+
+static int comedi_8254_insn_read(struct comedi_device *dev,
+				 struct comedi_subdevice *s,
+				 struct comedi_insn *insn,
+				 unsigned int *data)
+{
+	struct comedi_8254 *i8254 = s->private;
+	unsigned int chan = CR_CHAN(insn->chanspec);
+	int i;
+
+	if (i8254->busy[chan])
+		return -EBUSY;
+
+	for (i = 0; i < insn->n; i++)
+		data[i] = comedi_8254_read(i8254, chan);
+
+	return insn->n;
+}
+
+static int comedi_8254_insn_write(struct comedi_device *dev,
+				  struct comedi_subdevice *s,
+				  struct comedi_insn *insn,
+				  unsigned int *data)
+{
+	struct comedi_8254 *i8254 = s->private;
+	unsigned int chan = CR_CHAN(insn->chanspec);
+
+	if (i8254->busy[chan])
+		return -EBUSY;
+
+	if (insn->n)
+		comedi_8254_write(i8254, chan, data[insn->n - 1]);
+
+	return insn->n;
+}
+
+static int comedi_8254_insn_config(struct comedi_device *dev,
+				   struct comedi_subdevice *s,
+				   struct comedi_insn *insn,
+				   unsigned int *data)
+{
+	struct comedi_8254 *i8254 = s->private;
+	unsigned int chan = CR_CHAN(insn->chanspec);
+	int ret;
+
+	if (i8254->busy[chan])
+		return -EBUSY;
+
+	switch (data[0]) {
+	case INSN_CONFIG_RESET:
+		ret = comedi_8254_set_mode(i8254, chan,
+					   I8254_MODE0 | I8254_BINARY);
+		if (ret)
+			return ret;
+		break;
+	case INSN_CONFIG_SET_COUNTER_MODE:
+		ret = comedi_8254_set_mode(i8254, chan, data[1]);
+		if (ret)
+			return ret;
+		break;
+	case INSN_CONFIG_8254_READ_STATUS:
+		data[1] = comedi_8254_status(i8254, chan);
+		break;
+	default:
+		/*
+		 * If available, call the driver provided (*insn_config)
+		 * to handle any driver implemented instructions.
+		 */
+		if (i8254->insn_config)
+			return i8254->insn_config(dev, s, insn, data);
+
+		return -EINVAL;
+	}
+
+	return insn->n;
+}
+
+/**
+ * comedi_8254_subdevice_init - initialize a comedi_subdevice for the 8254 timer
+ * @s:		comedi_subdevice struct
+ */
+void comedi_8254_subdevice_init(struct comedi_subdevice *s,
+				struct comedi_8254 *i8254)
+{
+	s->type		= COMEDI_SUBD_COUNTER;
+	s->subdev_flags	= SDF_READABLE | SDF_WRITABLE;
+	s->n_chan	= 3;
+	s->maxdata	= 0xffff;
+	s->range_table	= &range_unknown;
+	s->insn_read	= comedi_8254_insn_read;
+	s->insn_write	= comedi_8254_insn_write;
+	s->insn_config	= comedi_8254_insn_config;
+
+	s->private	= i8254;
+}
+EXPORT_SYMBOL_GPL(comedi_8254_subdevice_init);
+
+static struct comedi_8254 *__i8254_init(unsigned long iobase,
+					void __iomem *mmio,
+					unsigned int osc_base,
+					unsigned int iosize,
+					unsigned int regshift)
+{
+	struct comedi_8254 *i8254;
+	int i;
+
+	/* sanity check that the iosize is valid */
+	if (!(iosize == I8254_IO8 || iosize == I8254_IO16 ||
+	      iosize == I8254_IO32))
+		return NULL;
+
+	i8254 = kzalloc(sizeof(*i8254), GFP_KERNEL);
+	if (!i8254)
+		return NULL;
+
+	i8254->iobase	= iobase;
+	i8254->mmio	= mmio;
+	i8254->iosize	= iosize;
+	i8254->regshift	= regshift;
+
+	/* default osc_base to the max speed of a generic 8254 timer */
+	i8254->osc_base	= osc_base ? osc_base : I8254_OSC_BASE_10MHZ;
+
+	/* reset all the counters by setting them to I8254_MODE0 */
+	for (i = 0; i < 3; i++)
+		comedi_8254_set_mode(i8254, i, I8254_MODE0 | I8254_BINARY);
+
+	return i8254;
+}
+
+/**
+ * comedi_8254_init - allocate and initialize the 8254 device for pio access
+ * @mmio:	port I/O base address
+ * @osc_base:	base time of the counter in ns
+ *		OPTIONAL - only used by comedi_8254_cascade_ns_to_timer()
+ * @iosize:	I/O register size
+ * @regshift:	register gap shift
+ */
+struct comedi_8254 *comedi_8254_init(unsigned long iobase,
+				     unsigned int osc_base,
+				     unsigned int iosize,
+				     unsigned int regshift)
+{
+	return __i8254_init(iobase, NULL, osc_base, iosize, regshift);
+}
+EXPORT_SYMBOL_GPL(comedi_8254_init);
+
+/**
+ * comedi_8254_mm_init - allocate and initialize the 8254 device for mmio access
+ * @mmio:	memory mapped I/O base address
+ * @osc_base:	base time of the counter in ns
+ *		OPTIONAL - only used by comedi_8254_cascade_ns_to_timer()
+ * @iosize:	I/O register size
+ * @regshift:	register gap shift
+ */
+struct comedi_8254 *comedi_8254_mm_init(void __iomem *mmio,
+					unsigned int osc_base,
+					unsigned int iosize,
+					unsigned int regshift)
+{
+	return __i8254_init(0, mmio, osc_base, iosize, regshift);
+}
+EXPORT_SYMBOL_GPL(comedi_8254_mm_init);
+
+static int __init comedi_8254_module_init(void)
+{
+	return 0;
+}
+module_init(comedi_8254_module_init);
+
+static void __exit comedi_8254_module_exit(void)
+{
+}
+module_exit(comedi_8254_module_exit);
+
+MODULE_AUTHOR("H Hartley Sweeten <hsweeten@visionengravers.com>");
+MODULE_DESCRIPTION("Comedi: Generic 8254 timer/counter support");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/comedi/drivers/comedi_8254.h b/drivers/staging/comedi/drivers/comedi_8254.h
new file mode 100644
index 000000000000..d89f6d94f8aa
--- /dev/null
+++ b/drivers/staging/comedi/drivers/comedi_8254.h
@@ -0,0 +1,133 @@
+/*
+ * comedi_8254.h
+ * Generic 8254 timer/counter support
+ * Copyright (C) 2014 H Hartley Sweeten <hsweeten@visionengravers.com>
+ *
+ * COMEDI - Linux Control and Measurement Device Interface
+ * Copyright (C) 2000 David A. Schleef <ds@schleef.org>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#ifndef _COMEDI_8254_H
+#define _COMEDI_8254_H
+
+/*
+ * Common oscillator base values in nanoseconds
+ */
+#define I8254_OSC_BASE_10MHZ	100
+#define I8254_OSC_BASE_5MHZ	200
+#define I8254_OSC_BASE_4MHZ	250
+#define I8254_OSC_BASE_2MHZ	500
+#define I8254_OSC_BASE_1MHZ	1000
+#define I8254_OSC_BASE_100KHZ	10000
+#define I8254_OSC_BASE_10KHZ	100000
+#define I8254_OSC_BASE_1KHZ	1000000
+
+/*
+ * I/O access size used to read/write registers
+ */
+#define I8254_IO8		1
+#define I8254_IO16		2
+#define I8254_IO32		4
+
+/*
+ * Register map for generic 8254 timer (I8254_IO8 with 0 regshift)
+ */
+#define I8254_COUNTER0_REG		0x00
+#define I8254_COUNTER1_REG		0x01
+#define I8254_COUNTER2_REG		0x02
+#define I8254_CTRL_REG			0x03
+#define I8254_CTRL_SEL_CTR(x)		((x) << 6)
+#define I8254_CTRL_READBACK_COUNT	((3 << 6) | (1 << 4))
+#define I8254_CTRL_READBACK_STATUS	((3 << 6) | (1 << 5))
+#define I8254_CTRL_READBACK_SEL_CTR(x)	(2 << (x))
+#define I8254_CTRL_LATCH		(0 << 4)
+#define I8254_CTRL_LSB_ONLY		(1 << 4)
+#define I8254_CTRL_MSB_ONLY		(2 << 4)
+#define I8254_CTRL_LSB_MSB		(3 << 4)
+
+/* counter maps zero to 0x10000 */
+#define I8254_MAX_COUNT			0x10000
+
+/**
+ * struct comedi_8254 - private data used by this module
+ * @iobase:		PIO base address of the registers (in/out)
+ * @mmio:		MMIO base address of the registers (read/write)
+ * @iosize:		I/O size used to access the registers (b/w/l)
+ * @regshift:		register gap shift
+ * @osc_base:		cascaded oscillator speed in ns
+ * @divisor:		divisor for single counter
+ * @divisor1:		divisor loaded into first cascaded counter
+ * @divisor2:		divisor loaded into second cascaded counter
+ * #next_div:		next divisor for single counter
+ * @next_div1:		next divisor to use for first cascaded counter
+ * @next_div2:		next divisor to use for second cascaded counter
+ * @clock_src;		current clock source for each counter (driver specific)
+ * @gate_src;		current gate source  for each counter (driver specific)
+ * @busy:		flags used to indicate that a counter is "busy"
+ * @insn_config:	driver specific (*insn_config) callback
+ */
+struct comedi_8254 {
+	unsigned long iobase;
+	void __iomem *mmio;
+	unsigned int iosize;
+	unsigned int regshift;
+	unsigned int osc_base;
+	unsigned int divisor;
+	unsigned int divisor1;
+	unsigned int divisor2;
+	unsigned int next_div;
+	unsigned int next_div1;
+	unsigned int next_div2;
+	unsigned int clock_src[3];
+	unsigned int gate_src[3];
+	bool busy[3];
+
+	int (*insn_config)(struct comedi_device *, struct comedi_subdevice *s,
+			   struct comedi_insn *, unsigned int *data);
+};
+
+unsigned int comedi_8254_status(struct comedi_8254 *, unsigned int counter);
+unsigned int comedi_8254_read(struct comedi_8254 *, unsigned int counter);
+void comedi_8254_write(struct comedi_8254 *,
+		       unsigned int counter, unsigned int val);
+
+int comedi_8254_set_mode(struct comedi_8254 *,
+			 unsigned int counter, unsigned int mode);
+int comedi_8254_load(struct comedi_8254 *,
+		     unsigned int counter, unsigned int val, unsigned int mode);
+
+void comedi_8254_pacer_enable(struct comedi_8254 *,
+			      unsigned int counter1, unsigned int counter2,
+			      bool enable);
+void comedi_8254_update_divisors(struct comedi_8254 *);
+void comedi_8254_cascade_ns_to_timer(struct comedi_8254 *,
+				     unsigned int *nanosec, unsigned int flags);
+void comedi_8254_ns_to_timer(struct comedi_8254 *,
+			     unsigned int *nanosec, unsigned int flags);
+
+void comedi_8254_set_busy(struct comedi_8254 *,
+			  unsigned int counter, bool busy);
+
+void comedi_8254_subdevice_init(struct comedi_subdevice *,
+				struct comedi_8254 *);
+
+struct comedi_8254 *comedi_8254_init(unsigned long iobase,
+				     unsigned int osc_base,
+				     unsigned int iosize,
+				     unsigned int regshift);
+struct comedi_8254 *comedi_8254_mm_init(void __iomem *mmio,
+					unsigned int osc_base,
+					unsigned int iosize,
+					unsigned int regshift);
+
+#endif	/* _COMEDI_8254_H */
diff --git a/drivers/staging/comedi/drivers/comedi_bond.c b/drivers/staging/comedi/drivers/comedi_bond.c
index 221d3819c967..96db0c2686a1 100644
--- a/drivers/staging/comedi/drivers/comedi_bond.c
+++ b/drivers/staging/comedi/drivers/comedi_bond.c
@@ -267,7 +267,6 @@ static int do_dev_config(struct comedi_device *dev, struct comedi_devconfig *it)
 				strlcat(devpriv->name, buf,
 					sizeof(devpriv->name));
 			}
-
 		}
 	}
 
@@ -313,9 +312,9 @@ static int bonding_attach(struct comedi_device *dev,
 	s->insn_config = bonding_dio_insn_config;
 
 	dev_info(dev->class_dev,
-		"%s: %s attached, %u channels from %u devices\n",
-		dev->driver->driver_name, dev->board_name,
-		devpriv->nchans, devpriv->ndevs);
+		 "%s: %s attached, %u channels from %u devices\n",
+		 dev->driver->driver_name, dev->board_name,
+		 devpriv->nchans, devpriv->ndevs);
 
 	return 0;
 }
diff --git a/drivers/staging/comedi/drivers/comedi_fc.h b/drivers/staging/comedi/drivers/comedi_fc.h
deleted file mode 100644
index 756be931c1a4..000000000000
--- a/drivers/staging/comedi/drivers/comedi_fc.h
+++ /dev/null
@@ -1,102 +0,0 @@
-/*
- * comedi_fc.h
- * This is a place for code driver writers wish to share between
- * two or more drivers. These functions are meant to be used only
- * by drivers, they are NOT part of the kcomedilib API!
- *
- * Author: Frank Mori Hess <fmhess@users.sourceforge.net>
- * Copyright (C) 2002 Frank Mori Hess
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- */
-
-#ifndef _COMEDI_FC_H
-#define _COMEDI_FC_H
-
-#include "../comedidev.h"
-
-/**
- * cfc_check_trigger_src() - trivially validate a comedi_cmd trigger source
- * @src: pointer to the trigger source to validate
- * @flags: bitmask of valid TRIG_* for the trigger
- *
- * This is used in "step 1" of the do_cmdtest functions of comedi drivers
- * to vaildate the comedi_cmd triggers. The mask of the @src against the
- * @flags allows the userspace comedilib to pass all the comedi_cmd
- * triggers as TRIG_ANY and get back a bitmask of the valid trigger sources.
- */
-static inline int cfc_check_trigger_src(unsigned int *src, unsigned int flags)
-{
-	unsigned int orig_src = *src;
-
-	*src = orig_src & flags;
-	if (*src == TRIG_INVALID || *src != orig_src)
-		return -EINVAL;
-	return 0;
-}
-
-/**
- * cfc_check_trigger_is_unique() - make sure a trigger source is unique
- * @src: the trigger source to check
- */
-static inline int cfc_check_trigger_is_unique(unsigned int src)
-{
-	/* this test is true if more than one _src bit is set */
-	if ((src & (src - 1)) != 0)
-		return -EINVAL;
-	return 0;
-}
-
-/**
- * cfc_check_trigger_arg_is() - trivially validate a trigger argument
- * @arg: pointer to the trigger arg to validate
- * @val: the value the argument should be
- */
-static inline int cfc_check_trigger_arg_is(unsigned int *arg, unsigned int val)
-{
-	if (*arg != val) {
-		*arg = val;
-		return -EINVAL;
-	}
-	return 0;
-}
-
-/**
- * cfc_check_trigger_arg_min() - trivially validate a trigger argument
- * @arg: pointer to the trigger arg to validate
- * @val: the minimum value the argument should be
- */
-static inline int cfc_check_trigger_arg_min(unsigned int *arg,
-					    unsigned int val)
-{
-	if (*arg < val) {
-		*arg = val;
-		return -EINVAL;
-	}
-	return 0;
-}
-
-/**
- * cfc_check_trigger_arg_max() - trivially validate a trigger argument
- * @arg: pointer to the trigger arg to validate
- * @val: the maximum value the argument should be
- */
-static inline int cfc_check_trigger_arg_max(unsigned int *arg,
-					    unsigned int val)
-{
-	if (*arg > val) {
-		*arg = val;
-		return -EINVAL;
-	}
-	return 0;
-}
-
-#endif /* _COMEDI_FC_H */
diff --git a/drivers/staging/comedi/drivers/comedi_isadma.c b/drivers/staging/comedi/drivers/comedi_isadma.c
index dbdea71d6b95..6ba71d114a95 100644
--- a/drivers/staging/comedi/drivers/comedi_isadma.c
+++ b/drivers/staging/comedi/drivers/comedi_isadma.c
@@ -91,9 +91,10 @@ unsigned int comedi_isadma_disable_on_sample(unsigned int dma_chan,
 			stalled++;
 			if (stalled > 10)
 				break;
+		} else {
+			residue = new_residue;
+			stalled = 0;
 		}
-		residue = new_residue;
-		stalled = 0;
 	}
 	return residue;
 }
@@ -234,7 +235,8 @@ void comedi_isadma_free(struct comedi_isadma *dma)
 			desc = &dma->desc[i];
 			if (desc->virt_addr)
 				dma_free_coherent(NULL, desc->maxsize,
-						desc->virt_addr, desc->hw_addr);
+						  desc->virt_addr,
+						  desc->hw_addr);
 		}
 		kfree(dma->desc);
 	}
diff --git a/drivers/staging/comedi/drivers/comedi_parport.c b/drivers/staging/comedi/drivers/comedi_parport.c
index ceef6931edbe..15a4093efda1 100644
--- a/drivers/staging/comedi/drivers/comedi_parport.c
+++ b/drivers/staging/comedi/drivers/comedi_parport.c
@@ -69,8 +69,6 @@
 
 #include "../comedidev.h"
 
-#include "comedi_fc.h"
-
 /*
  * Register map
  */
@@ -161,11 +159,11 @@ static int parport_intr_cmdtest(struct comedi_device *dev,
 
 	/* Step 1 : check if triggers are trivially valid */
 
-	err |= cfc_check_trigger_src(&cmd->start_src, TRIG_NOW);
-	err |= cfc_check_trigger_src(&cmd->scan_begin_src, TRIG_EXT);
-	err |= cfc_check_trigger_src(&cmd->convert_src, TRIG_FOLLOW);
-	err |= cfc_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
-	err |= cfc_check_trigger_src(&cmd->stop_src, TRIG_NONE);
+	err |= comedi_check_trigger_src(&cmd->start_src, TRIG_NOW);
+	err |= comedi_check_trigger_src(&cmd->scan_begin_src, TRIG_EXT);
+	err |= comedi_check_trigger_src(&cmd->convert_src, TRIG_FOLLOW);
+	err |= comedi_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
+	err |= comedi_check_trigger_src(&cmd->stop_src, TRIG_NONE);
 
 	if (err)
 		return 1;
@@ -175,11 +173,12 @@ static int parport_intr_cmdtest(struct comedi_device *dev,
 
 	/* Step 3: check if arguments are trivially valid */
 
-	err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
-	err |= cfc_check_trigger_arg_is(&cmd->scan_begin_arg, 0);
-	err |= cfc_check_trigger_arg_is(&cmd->convert_arg, 0);
-	err |= cfc_check_trigger_arg_is(&cmd->scan_end_arg, cmd->chanlist_len);
-	err |= cfc_check_trigger_arg_is(&cmd->stop_arg, 0);
+	err |= comedi_check_trigger_arg_is(&cmd->start_arg, 0);
+	err |= comedi_check_trigger_arg_is(&cmd->scan_begin_arg, 0);
+	err |= comedi_check_trigger_arg_is(&cmd->convert_arg, 0);
+	err |= comedi_check_trigger_arg_is(&cmd->scan_end_arg,
+					   cmd->chanlist_len);
+	err |= comedi_check_trigger_arg_is(&cmd->stop_arg, 0);
 
 	if (err)
 		return 3;
diff --git a/drivers/staging/comedi/drivers/comedi_test.c b/drivers/staging/comedi/drivers/comedi_test.c
index e56525a1c8f3..80d613c0fbc6 100644
--- a/drivers/staging/comedi/drivers/comedi_test.c
+++ b/drivers/staging/comedi/drivers/comedi_test.c
@@ -50,7 +50,6 @@ zero volts).
 
 #include <asm/div64.h>
 
-#include "comedi_fc.h"
 #include <linux/timer.h>
 #include <linux/ktime.h>
 
@@ -225,19 +224,20 @@ static int waveform_ai_cmdtest(struct comedi_device *dev,
 
 	/* Step 1 : check if triggers are trivially valid */
 
-	err |= cfc_check_trigger_src(&cmd->start_src, TRIG_NOW);
-	err |= cfc_check_trigger_src(&cmd->scan_begin_src, TRIG_TIMER);
-	err |= cfc_check_trigger_src(&cmd->convert_src, TRIG_NOW | TRIG_TIMER);
-	err |= cfc_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
-	err |= cfc_check_trigger_src(&cmd->stop_src, TRIG_COUNT | TRIG_NONE);
+	err |= comedi_check_trigger_src(&cmd->start_src, TRIG_NOW);
+	err |= comedi_check_trigger_src(&cmd->scan_begin_src, TRIG_TIMER);
+	err |= comedi_check_trigger_src(&cmd->convert_src,
+					TRIG_NOW | TRIG_TIMER);
+	err |= comedi_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
+	err |= comedi_check_trigger_src(&cmd->stop_src, TRIG_COUNT | TRIG_NONE);
 
 	if (err)
 		return 1;
 
 	/* Step 2a : make sure trigger sources are unique */
 
-	err |= cfc_check_trigger_is_unique(cmd->convert_src);
-	err |= cfc_check_trigger_is_unique(cmd->stop_src);
+	err |= comedi_check_trigger_is_unique(cmd->convert_src);
+	err |= comedi_check_trigger_is_unique(cmd->stop_src);
 
 	/* Step 2b : and mutually compatible */
 
@@ -246,26 +246,30 @@ static int waveform_ai_cmdtest(struct comedi_device *dev,
 
 	/* Step 3: check if arguments are trivially valid */
 
-	err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
+	err |= comedi_check_trigger_arg_is(&cmd->start_arg, 0);
 
 	if (cmd->convert_src == TRIG_NOW)
-		err |= cfc_check_trigger_arg_is(&cmd->convert_arg, 0);
+		err |= comedi_check_trigger_arg_is(&cmd->convert_arg, 0);
 
 	if (cmd->scan_begin_src == TRIG_TIMER) {
-		err |= cfc_check_trigger_arg_min(&cmd->scan_begin_arg,
-						 nano_per_micro);
-		if (cmd->convert_src == TRIG_TIMER)
-			err |= cfc_check_trigger_arg_min(&cmd->scan_begin_arg,
-					cmd->convert_arg * cmd->chanlist_len);
+		err |= comedi_check_trigger_arg_min(&cmd->scan_begin_arg,
+						    nano_per_micro);
+		if (cmd->convert_src == TRIG_TIMER) {
+			err |= comedi_check_trigger_arg_min(&cmd->
+							    scan_begin_arg,
+							    cmd->convert_arg *
+							    cmd->chanlist_len);
+		}
 	}
 
-	err |= cfc_check_trigger_arg_min(&cmd->chanlist_len, 1);
-	err |= cfc_check_trigger_arg_is(&cmd->scan_end_arg, cmd->chanlist_len);
+	err |= comedi_check_trigger_arg_min(&cmd->chanlist_len, 1);
+	err |= comedi_check_trigger_arg_is(&cmd->scan_end_arg,
+					   cmd->chanlist_len);
 
 	if (cmd->stop_src == TRIG_COUNT)
-		err |= cfc_check_trigger_arg_min(&cmd->stop_arg, 1);
+		err |= comedi_check_trigger_arg_min(&cmd->stop_arg, 1);
 	else	/* TRIG_NONE */
-		err |= cfc_check_trigger_arg_is(&cmd->stop_arg, 0);
+		err |= comedi_check_trigger_arg_is(&cmd->stop_arg, 0);
 
 	if (err)
 		return 3;
@@ -277,14 +281,14 @@ static int waveform_ai_cmdtest(struct comedi_device *dev,
 		/* round to nearest microsec */
 		arg = nano_per_micro *
 		      ((arg + (nano_per_micro / 2)) / nano_per_micro);
-		err |= cfc_check_trigger_arg_is(&cmd->scan_begin_arg, arg);
+		err |= comedi_check_trigger_arg_is(&cmd->scan_begin_arg, arg);
 	}
 	if (cmd->convert_src == TRIG_TIMER) {
 		arg = cmd->convert_arg;
 		/* round to nearest microsec */
 		arg = nano_per_micro *
 		      ((arg + (nano_per_micro / 2)) / nano_per_micro);
-		err |= cfc_check_trigger_arg_is(&cmd->convert_arg, arg);
+		err |= comedi_check_trigger_arg_is(&cmd->convert_arg, arg);
 	}
 
 	if (err)
@@ -420,9 +424,8 @@ static int waveform_attach(struct comedi_device *dev,
 	for (i = 0; i < s->n_chan; i++)
 		devpriv->ao_loopbacks[i] = s->maxdata / 2;
 
-	init_timer(&devpriv->timer);
-	devpriv->timer.function = waveform_ai_interrupt;
-	devpriv->timer.data = (unsigned long)dev;
+	setup_timer(&devpriv->timer, waveform_ai_interrupt,
+		    (unsigned long)dev);
 
 	dev_info(dev->class_dev,
 		 "%s: %i microvolt, %li microsecond waveform attached\n",
diff --git a/drivers/staging/comedi/drivers/contec_pci_dio.c b/drivers/staging/comedi/drivers/contec_pci_dio.c
index 205f9df345a2..4956a49a6140 100644
--- a/drivers/staging/comedi/drivers/contec_pci_dio.c
+++ b/drivers/staging/comedi/drivers/contec_pci_dio.c
@@ -26,9 +26,8 @@ Configuration Options: not applicable, uses comedi PCI auto config
 */
 
 #include <linux/module.h>
-#include <linux/pci.h>
 
-#include "../comedidev.h"
+#include "../comedi_pci.h"
 
 /*
  * Register map
@@ -59,7 +58,7 @@ static int contec_di_insn_bits(struct comedi_device *dev,
 }
 
 static int contec_auto_attach(struct comedi_device *dev,
-					unsigned long context_unused)
+			      unsigned long context_unused)
 {
 	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
 	struct comedi_subdevice *s;
diff --git a/drivers/staging/comedi/drivers/daqboard2000.c b/drivers/staging/comedi/drivers/daqboard2000.c
index 96697fbb5239..f97d18d92255 100644
--- a/drivers/staging/comedi/drivers/daqboard2000.c
+++ b/drivers/staging/comedi/drivers/daqboard2000.c
@@ -103,11 +103,10 @@ Configuration options: not applicable, uses PCI auto config
  */
 
 #include <linux/module.h>
-#include <linux/pci.h>
 #include <linux/delay.h>
 #include <linux/interrupt.h>
 
-#include "../comedidev.h"
+#include "../comedi_pci.h"
 
 #include "8255.h"
 
@@ -649,7 +648,7 @@ static const void *daqboard2000_find_boardinfo(struct comedi_device *dev,
 }
 
 static int daqboard2000_auto_attach(struct comedi_device *dev,
-					      unsigned long context_unused)
+				    unsigned long context_unused)
 {
 	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
 	const struct daq200_boardtype *board;
diff --git a/drivers/staging/comedi/drivers/das08.c b/drivers/staging/comedi/drivers/das08.c
index c78c0df9bbe3..73f4c8dbbde3 100644
--- a/drivers/staging/comedi/drivers/das08.c
+++ b/drivers/staging/comedi/drivers/das08.c
@@ -23,7 +23,7 @@
 #include "../comedidev.h"
 
 #include "8255.h"
-#include "8253.h"
+#include "comedi_8254.h"
 #include "das08.h"
 
 /*
@@ -359,62 +359,6 @@ static int das08_ao_insn_write(struct comedi_device *dev,
 	return insn->n;
 }
 
-static void i8254_initialize(struct comedi_device *dev)
-{
-	const struct das08_board_struct *thisboard = dev->board_ptr;
-	unsigned long i8254_iobase = dev->iobase + thisboard->i8254_offset;
-	unsigned int mode = I8254_MODE0 | I8254_BINARY;
-	int i;
-
-	for (i = 0; i < 3; ++i)
-		i8254_set_mode(i8254_iobase, 0, i, mode);
-}
-
-static int das08_counter_read(struct comedi_device *dev,
-			      struct comedi_subdevice *s,
-			      struct comedi_insn *insn, unsigned int *data)
-{
-	const struct das08_board_struct *thisboard = dev->board_ptr;
-	unsigned long i8254_iobase = dev->iobase + thisboard->i8254_offset;
-	int chan = insn->chanspec;
-
-	data[0] = i8254_read(i8254_iobase, 0, chan);
-	return 1;
-}
-
-static int das08_counter_write(struct comedi_device *dev,
-			       struct comedi_subdevice *s,
-			       struct comedi_insn *insn, unsigned int *data)
-{
-	const struct das08_board_struct *thisboard = dev->board_ptr;
-	unsigned long i8254_iobase = dev->iobase + thisboard->i8254_offset;
-	int chan = insn->chanspec;
-
-	i8254_write(i8254_iobase, 0, chan, data[0]);
-	return 1;
-}
-
-static int das08_counter_config(struct comedi_device *dev,
-				struct comedi_subdevice *s,
-				struct comedi_insn *insn, unsigned int *data)
-{
-	const struct das08_board_struct *thisboard = dev->board_ptr;
-	unsigned long i8254_iobase = dev->iobase + thisboard->i8254_offset;
-	int chan = insn->chanspec;
-
-	switch (data[0]) {
-	case INSN_CONFIG_SET_COUNTER_MODE:
-		i8254_set_mode(i8254_iobase, 0, chan, data[1]);
-		break;
-	case INSN_CONFIG_8254_READ_STATUS:
-		data[1] = i8254_status(i8254_iobase, 0, chan);
-		break;
-	default:
-		return -EINVAL;
-	}
-	return 2;
-}
-
 int das08_common_attach(struct comedi_device *dev, unsigned long iobase)
 {
 	const struct das08_board_struct *thisboard = dev->board_ptr;
@@ -464,7 +408,7 @@ int das08_common_attach(struct comedi_device *dev, unsigned long iobase)
 		if (ret)
 			return ret;
 
-		/* intialize all channels to 0V */
+		/* initialize all channels to 0V */
 		for (i = 0; i < s->n_chan; i++) {
 			s->readback[i] = s->maxdata / 2;
 			das08_ao_set_data(dev, i, s->readback[i]);
@@ -511,17 +455,16 @@ int das08_common_attach(struct comedi_device *dev, unsigned long iobase)
 		s->type = COMEDI_SUBD_UNUSED;
 	}
 
+	/* Counter subdevice (8254) */
 	s = &dev->subdevices[5];
-	/* 8254 */
-	if (thisboard->i8254_offset != 0) {
-		s->type = COMEDI_SUBD_COUNTER;
-		s->subdev_flags = SDF_WRITABLE | SDF_READABLE;
-		s->n_chan = 3;
-		s->maxdata = 0xFFFF;
-		s->insn_read = das08_counter_read;
-		s->insn_write = das08_counter_write;
-		s->insn_config = das08_counter_config;
-		i8254_initialize(dev);
+	if (thisboard->i8254_offset) {
+		dev->pacer = comedi_8254_init(dev->iobase +
+					      thisboard->i8254_offset,
+					      0, I8254_IO8, 0);
+		if (!dev->pacer)
+			return -ENOMEM;
+
+		comedi_8254_subdevice_init(s, dev->pacer);
 	} else {
 		s->type = COMEDI_SUBD_UNUSED;
 	}
diff --git a/drivers/staging/comedi/drivers/das08_pci.c b/drivers/staging/comedi/drivers/das08_pci.c
index b2ea10b848c3..d8d27fa44491 100644
--- a/drivers/staging/comedi/drivers/das08_pci.c
+++ b/drivers/staging/comedi/drivers/das08_pci.c
@@ -32,9 +32,8 @@
  */
 
 #include <linux/module.h>
-#include <linux/pci.h>
 
-#include "../comedidev.h"
+#include "../comedi_pci.h"
 
 #include "das08.h"
 
diff --git a/drivers/staging/comedi/drivers/das16.c b/drivers/staging/comedi/drivers/das16.c
index 2c20311120f1..d7cf4b153f7c 100644
--- a/drivers/staging/comedi/drivers/das16.c
+++ b/drivers/staging/comedi/drivers/das16.c
@@ -76,8 +76,7 @@
 #include "../comedidev.h"
 
 #include "comedi_isadma.h"
-#include "comedi_fc.h"
-#include "8253.h"
+#include "comedi_8254.h"
 #include "8255.h"
 
 #define DAS16_DMA_SIZE 0xff00	/*  size in bytes of allocated dma buffer */
@@ -599,29 +598,29 @@ static int das16_cmd_test(struct comedi_device *dev, struct comedi_subdevice *s,
 
 	/* Step 1 : check if triggers are trivially valid */
 
-	err |= cfc_check_trigger_src(&cmd->start_src, TRIG_NOW);
+	err |= comedi_check_trigger_src(&cmd->start_src, TRIG_NOW);
 
 	trig_mask = TRIG_FOLLOW;
 	if (devpriv->can_burst)
 		trig_mask |= TRIG_TIMER | TRIG_EXT;
-	err |= cfc_check_trigger_src(&cmd->scan_begin_src, trig_mask);
+	err |= comedi_check_trigger_src(&cmd->scan_begin_src, trig_mask);
 
 	trig_mask = TRIG_TIMER | TRIG_EXT;
 	if (devpriv->can_burst)
 		trig_mask |= TRIG_NOW;
-	err |= cfc_check_trigger_src(&cmd->convert_src, trig_mask);
+	err |= comedi_check_trigger_src(&cmd->convert_src, trig_mask);
 
-	err |= cfc_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
-	err |= cfc_check_trigger_src(&cmd->stop_src, TRIG_COUNT | TRIG_NONE);
+	err |= comedi_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
+	err |= comedi_check_trigger_src(&cmd->stop_src, TRIG_COUNT | TRIG_NONE);
 
 	if (err)
 		return 1;
 
 	/* Step 2a : make sure trigger sources are unique */
 
-	err |= cfc_check_trigger_is_unique(cmd->scan_begin_src);
-	err |= cfc_check_trigger_is_unique(cmd->convert_src);
-	err |= cfc_check_trigger_is_unique(cmd->stop_src);
+	err |= comedi_check_trigger_is_unique(cmd->scan_begin_src);
+	err |= comedi_check_trigger_is_unique(cmd->convert_src);
+	err |= comedi_check_trigger_is_unique(cmd->stop_src);
 
 	/* Step 2b : and mutually compatible */
 
@@ -636,26 +635,30 @@ static int das16_cmd_test(struct comedi_device *dev, struct comedi_subdevice *s,
 
 	/* Step 3: check if arguments are trivially valid */
 
-	err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
+	err |= comedi_check_trigger_arg_is(&cmd->start_arg, 0);
 
 	if (cmd->scan_begin_src == TRIG_FOLLOW)	/* internal trigger */
-		err |= cfc_check_trigger_arg_is(&cmd->scan_begin_arg, 0);
+		err |= comedi_check_trigger_arg_is(&cmd->scan_begin_arg, 0);
 
-	err |= cfc_check_trigger_arg_is(&cmd->scan_end_arg, cmd->chanlist_len);
+	err |= comedi_check_trigger_arg_is(&cmd->scan_end_arg,
+					   cmd->chanlist_len);
 
 	/* check against maximum frequency */
-	if (cmd->scan_begin_src == TRIG_TIMER)
-		err |= cfc_check_trigger_arg_min(&cmd->scan_begin_arg,
-					board->ai_speed * cmd->chanlist_len);
+	if (cmd->scan_begin_src == TRIG_TIMER) {
+		err |= comedi_check_trigger_arg_min(&cmd->scan_begin_arg,
+						    board->ai_speed *
+						    cmd->chanlist_len);
+	}
 
-	if (cmd->convert_src == TRIG_TIMER)
-		err |= cfc_check_trigger_arg_min(&cmd->convert_arg,
-						 board->ai_speed);
+	if (cmd->convert_src == TRIG_TIMER) {
+		err |= comedi_check_trigger_arg_min(&cmd->convert_arg,
+						    board->ai_speed);
+	}
 
 	if (cmd->stop_src == TRIG_COUNT)
-		err |= cfc_check_trigger_arg_min(&cmd->stop_arg, 1);
+		err |= comedi_check_trigger_arg_min(&cmd->stop_arg, 1);
 	else	/* TRIG_NONE */
-		err |= cfc_check_trigger_arg_is(&cmd->stop_arg, 0);
+		err |= comedi_check_trigger_arg_is(&cmd->stop_arg, 0);
 
 	if (err)
 		return 3;
@@ -663,19 +666,13 @@ static int das16_cmd_test(struct comedi_device *dev, struct comedi_subdevice *s,
 	/*  step 4: fix up arguments */
 	if (cmd->scan_begin_src == TRIG_TIMER) {
 		arg = cmd->scan_begin_arg;
-		i8253_cascade_ns_to_timer(devpriv->clockbase,
-					  &devpriv->divisor1,
-					  &devpriv->divisor2,
-					  &arg, cmd->flags);
-		err |= cfc_check_trigger_arg_is(&cmd->scan_begin_arg, arg);
+		comedi_8254_cascade_ns_to_timer(dev->pacer, &arg, cmd->flags);
+		err |= comedi_check_trigger_arg_is(&cmd->scan_begin_arg, arg);
 	}
 	if (cmd->convert_src == TRIG_TIMER) {
 		arg = cmd->convert_arg;
-		i8253_cascade_ns_to_timer(devpriv->clockbase,
-					  &devpriv->divisor1,
-					  &devpriv->divisor2,
-					  &arg, cmd->flags);
-		err |= cfc_check_trigger_arg_is(&cmd->convert_arg, arg);
+		comedi_8254_cascade_ns_to_timer(dev->pacer, &arg, cmd->flags);
+		err |= comedi_check_trigger_arg_is(&cmd->convert_arg, arg);
 	}
 	if (err)
 		return 4;
@@ -693,17 +690,9 @@ static int das16_cmd_test(struct comedi_device *dev, struct comedi_subdevice *s,
 static unsigned int das16_set_pacer(struct comedi_device *dev, unsigned int ns,
 				    unsigned int flags)
 {
-	struct das16_private_struct *devpriv = dev->private;
-	unsigned long timer_base = dev->iobase + DAS16_TIMER_BASE_REG;
-
-	i8253_cascade_ns_to_timer(devpriv->clockbase,
-				  &devpriv->divisor1, &devpriv->divisor2,
-				  &ns, flags);
-
-	i8254_set_mode(timer_base, 0, 1, I8254_MODE2 | I8254_BINARY);
-	i8254_set_mode(timer_base, 0, 2, I8254_MODE2 | I8254_BINARY);
-	i8254_write(timer_base, 0, 1, devpriv->divisor1);
-	i8254_write(timer_base, 0, 2, devpriv->divisor2);
+	comedi_8254_cascade_ns_to_timer(dev->pacer, &ns, flags);
+	comedi_8254_update_divisors(dev->pacer);
+	comedi_8254_pacer_enable(dev->pacer, 1, 2, true);
 
 	return ns;
 }
@@ -722,7 +711,7 @@ static int das16_cmd_exec(struct comedi_device *dev, struct comedi_subdevice *s)
 
 	if (cmd->flags & CMDF_PRIORITY) {
 		dev_err(dev->class_dev,
-			 "isa dma transfers cannot be performed with CMDF_PRIORITY, aborting\n");
+			"isa dma transfers cannot be performed with CMDF_PRIORITY, aborting\n");
 		return -1;
 	}
 
@@ -935,7 +924,6 @@ static void das16_reset(struct comedi_device *dev)
 	outb(0, dev->iobase + DAS16_STATUS_REG);
 	outb(0, dev->iobase + DAS16_CTRL_REG);
 	outb(0, dev->iobase + DAS16_PACER_REG);
-	outb(0, dev->iobase + DAS16_TIMER_BASE_REG + i8254_control_reg);
 }
 
 static void das16_alloc_dma(struct comedi_device *dev, unsigned int dma_chan)
@@ -950,9 +938,8 @@ static void das16_alloc_dma(struct comedi_device *dev, unsigned int dma_chan)
 	devpriv->dma = comedi_isadma_alloc(dev, 2, dma_chan, dma_chan,
 					   DAS16_DMA_SIZE, COMEDI_ISADMA_READ);
 	if (devpriv->dma) {
-		init_timer(&devpriv->timer);
-		devpriv->timer.function = das16_timer_interrupt;
-		devpriv->timer.data = (unsigned long)dev;
+		setup_timer(&devpriv->timer, das16_timer_interrupt,
+			    (unsigned long)dev);
 	}
 }
 
@@ -1039,6 +1026,7 @@ static int das16_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	const struct das16_board *board = dev->board_ptr;
 	struct das16_private_struct *devpriv;
 	struct comedi_subdevice *s;
+	unsigned int osc_base;
 	unsigned int status;
 	int ret;
 
@@ -1078,21 +1066,21 @@ static int das16_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 		return -EINVAL;
 
 	/*  get master clock speed */
+	osc_base = I8254_OSC_BASE_1MHZ;
 	if (devpriv->can_burst) {
 		status = inb(dev->iobase + DAS1600_STATUS_REG);
-
 		if (status & DAS1600_STATUS_CLK_10MHZ)
-			devpriv->clockbase = I8254_OSC_BASE_10MHZ;
-		else
-			devpriv->clockbase = I8254_OSC_BASE_1MHZ;
+			osc_base = I8254_OSC_BASE_10MHZ;
 	} else {
 		if (it->options[3])
-			devpriv->clockbase = I8254_OSC_BASE_1MHZ /
-					     it->options[3];
-		else
-			devpriv->clockbase = I8254_OSC_BASE_1MHZ;
+			osc_base = I8254_OSC_BASE_1MHZ / it->options[3];
 	}
 
+	dev->pacer = comedi_8254_init(dev->iobase + DAS16_TIMER_BASE_REG,
+				      osc_base, I8254_IO8, 0);
+	if (!dev->pacer)
+		return -ENOMEM;
+
 	das16_alloc_dma(dev, it->options[2]);
 
 	ret = comedi_alloc_subdevices(dev, 4 + board->has_8255);
diff --git a/drivers/staging/comedi/drivers/das16m1.c b/drivers/staging/comedi/drivers/das16m1.c
index 3666a68979fb..1adf6a71a9f3 100644
--- a/drivers/staging/comedi/drivers/das16m1.c
+++ b/drivers/staging/comedi/drivers/das16m1.c
@@ -53,12 +53,12 @@ irq can be omitted, although the cmd interface will not work without it.
 */
 
 #include <linux/module.h>
+#include <linux/slab.h>
 #include <linux/interrupt.h>
 #include "../comedidev.h"
 
 #include "8255.h"
-#include "8253.h"
-#include "comedi_fc.h"
+#include "comedi_8254.h"
 
 #define DAS16M1_SIZE2 8
 
@@ -103,8 +103,6 @@ irq can be omitted, although the cmd interface will not work without it.
 #define   Q_RANGE(x)             (((x) & 0xf) << 4)
 #define   UNIPOLAR               0x40
 #define DAS16M1_8254_FIRST             0x8
-#define DAS16M1_8254_FIRST_CNTRL       0xb
-#define   TOTAL_CLEAR                    0x30
 #define DAS16M1_8254_SECOND            0xc
 #define DAS16M1_82C55                  0x400
 #define DAS16M1_8254_THIRD             0x404
@@ -124,6 +122,7 @@ static const struct comedi_lrange range_das16m1 = {
 };
 
 struct das16m1_private_struct {
+	struct comedi_8254 *counter;
 	unsigned int control_state;
 	unsigned int adc_count;	/*  number of samples completed */
 	/* initial value in lower half of hardware conversion counter,
@@ -131,8 +130,6 @@ struct das16m1_private_struct {
 	 * counter yet (loaded by first sample conversion) */
 	u16 initial_hw_count;
 	unsigned short ai_buffer[FIFO_SIZE];
-	unsigned int divisor1;	/*  divides master clock to obtain conversion speed */
-	unsigned int divisor2;	/*  divides master clock to obtain conversion speed */
 	unsigned long extra_iobase;
 };
 
@@ -169,7 +166,7 @@ static int das16m1_ai_check_chanlist(struct comedi_device *dev,
 
 		if ((i % 2) != (chan % 2)) {
 			dev_dbg(dev->class_dev,
-				 "even/odd channels must go have even/odd chanlist indices\n");
+				"even/odd channels must go have even/odd chanlist indices\n");
 			return -EINVAL;
 		}
 	}
@@ -180,26 +177,25 @@ static int das16m1_ai_check_chanlist(struct comedi_device *dev,
 static int das16m1_cmd_test(struct comedi_device *dev,
 			    struct comedi_subdevice *s, struct comedi_cmd *cmd)
 {
-	struct das16m1_private_struct *devpriv = dev->private;
 	int err = 0;
-	unsigned int arg;
 
 	/* Step 1 : check if triggers are trivially valid */
 
-	err |= cfc_check_trigger_src(&cmd->start_src, TRIG_NOW | TRIG_EXT);
-	err |= cfc_check_trigger_src(&cmd->scan_begin_src, TRIG_FOLLOW);
-	err |= cfc_check_trigger_src(&cmd->convert_src, TRIG_TIMER | TRIG_EXT);
-	err |= cfc_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
-	err |= cfc_check_trigger_src(&cmd->stop_src, TRIG_COUNT | TRIG_NONE);
+	err |= comedi_check_trigger_src(&cmd->start_src, TRIG_NOW | TRIG_EXT);
+	err |= comedi_check_trigger_src(&cmd->scan_begin_src, TRIG_FOLLOW);
+	err |= comedi_check_trigger_src(&cmd->convert_src,
+					TRIG_TIMER | TRIG_EXT);
+	err |= comedi_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
+	err |= comedi_check_trigger_src(&cmd->stop_src, TRIG_COUNT | TRIG_NONE);
 
 	if (err)
 		return 1;
 
 	/* Step 2a : make sure trigger sources are unique */
 
-	err |= cfc_check_trigger_is_unique(cmd->start_src);
-	err |= cfc_check_trigger_is_unique(cmd->convert_src);
-	err |= cfc_check_trigger_is_unique(cmd->stop_src);
+	err |= comedi_check_trigger_is_unique(cmd->start_src);
+	err |= comedi_check_trigger_is_unique(cmd->convert_src);
+	err |= comedi_check_trigger_is_unique(cmd->stop_src);
 
 	/* Step 2b : and mutually compatible */
 
@@ -208,20 +204,21 @@ static int das16m1_cmd_test(struct comedi_device *dev,
 
 	/* Step 3: check if arguments are trivially valid */
 
-	err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
+	err |= comedi_check_trigger_arg_is(&cmd->start_arg, 0);
 
 	if (cmd->scan_begin_src == TRIG_FOLLOW)	/* internal trigger */
-		err |= cfc_check_trigger_arg_is(&cmd->scan_begin_arg, 0);
+		err |= comedi_check_trigger_arg_is(&cmd->scan_begin_arg, 0);
 
 	if (cmd->convert_src == TRIG_TIMER)
-		err |= cfc_check_trigger_arg_min(&cmd->convert_arg, 1000);
+		err |= comedi_check_trigger_arg_min(&cmd->convert_arg, 1000);
 
-	err |= cfc_check_trigger_arg_is(&cmd->scan_end_arg, cmd->chanlist_len);
+	err |= comedi_check_trigger_arg_is(&cmd->scan_end_arg,
+					   cmd->chanlist_len);
 
 	if (cmd->stop_src == TRIG_COUNT)
-		err |= cfc_check_trigger_arg_min(&cmd->stop_arg, 1);
+		err |= comedi_check_trigger_arg_min(&cmd->stop_arg, 1);
 	else	/* TRIG_NONE */
-		err |= cfc_check_trigger_arg_is(&cmd->stop_arg, 0);
+		err |= comedi_check_trigger_arg_is(&cmd->stop_arg, 0);
 
 	if (err)
 		return 3;
@@ -229,12 +226,10 @@ static int das16m1_cmd_test(struct comedi_device *dev,
 	/* step 4: fix up arguments */
 
 	if (cmd->convert_src == TRIG_TIMER) {
-		arg = cmd->convert_arg;
-		i8253_cascade_ns_to_timer(I8254_OSC_BASE_10MHZ,
-					  &devpriv->divisor1,
-					  &devpriv->divisor2,
-					  &arg, cmd->flags);
-		err |= cfc_check_trigger_arg_is(&cmd->convert_arg, arg);
+		unsigned int arg = cmd->convert_arg;
+
+		comedi_8254_cascade_ns_to_timer(dev->pacer, &arg, cmd->flags);
+		err |= comedi_check_trigger_arg_is(&cmd->convert_arg, arg);
 	}
 
 	if (err)
@@ -250,25 +245,12 @@ static int das16m1_cmd_test(struct comedi_device *dev,
 	return 0;
 }
 
-static void das16m1_set_pacer(struct comedi_device *dev)
-{
-	struct das16m1_private_struct *devpriv = dev->private;
-	unsigned long timer_base = dev->iobase + DAS16M1_8254_SECOND;
-
-	i8254_set_mode(timer_base, 0, 1, I8254_MODE2 | I8254_BINARY);
-	i8254_set_mode(timer_base, 0, 2, I8254_MODE2 | I8254_BINARY);
-
-	i8254_write(timer_base, 0, 1, devpriv->divisor1);
-	i8254_write(timer_base, 0, 2, devpriv->divisor2);
-}
-
 static int das16m1_cmd_exec(struct comedi_device *dev,
 			    struct comedi_subdevice *s)
 {
 	struct das16m1_private_struct *devpriv = dev->private;
 	struct comedi_async *async = s->async;
 	struct comedi_cmd *cmd = &async->cmd;
-	unsigned long timer_base = dev->iobase + DAS16M1_8254_FIRST;
 	unsigned int byte, i;
 
 	/* disable interrupts and internal pacer */
@@ -277,14 +259,21 @@ static int das16m1_cmd_exec(struct comedi_device *dev,
 
 	/*  set software count */
 	devpriv->adc_count = 0;
-	/* Initialize lower half of hardware counter, used to determine how
+
+	/*
+	 * Initialize lower half of hardware counter, used to determine how
 	 * many samples are in fifo.  Value doesn't actually load into counter
-	 * until counter's next clock (the next a/d conversion) */
-	i8254_set_mode(timer_base, 0, 1, I8254_MODE2 | I8254_BINARY);
-	i8254_write(timer_base, 0, 1, 0);
-	/* remember current reading of counter so we know when counter has
-	 * actually been loaded */
-	devpriv->initial_hw_count = i8254_read(timer_base, 0, 1);
+	 * until counter's next clock (the next a/d conversion).
+	 */
+	comedi_8254_set_mode(devpriv->counter, 1, I8254_MODE2 | I8254_BINARY);
+	comedi_8254_write(devpriv->counter, 1, 0);
+
+	/*
+	 * Remember current reading of counter so we know when counter has
+	 * actually been loaded.
+	 */
+	devpriv->initial_hw_count = comedi_8254_read(devpriv->counter, 1);
+
 	/* setup channel/gain queue */
 	for (i = 0; i < cmd->chanlist_len; i++) {
 		outb(i, dev->iobase + DAS16M1_QUEUE_ADDR);
@@ -297,7 +286,8 @@ static int das16m1_cmd_exec(struct comedi_device *dev,
 	/* enable interrupts and set internal pacer counter mode and counts */
 	devpriv->control_state &= ~PACER_MASK;
 	if (cmd->convert_src == TRIG_TIMER) {
-		das16m1_set_pacer(dev);
+		comedi_8254_update_divisors(dev->pacer);
+		comedi_8254_pacer_enable(dev->pacer, 1, 2, true);
 		devpriv->control_state |= INT_PACER;
 	} else {	/* TRIG_EXT */
 		devpriv->control_state |= EXT_PACER;
@@ -417,8 +407,8 @@ static void das16m1_handler(struct comedi_device *dev, unsigned int status)
 	async = s->async;
 	cmd = &async->cmd;
 
-	/*  figure out how many samples are in fifo */
-	hw_counter = i8254_read(dev->iobase + DAS16M1_8254_FIRST, 0, 1);
+	/* figure out how many samples are in fifo */
+	hw_counter = comedi_8254_read(devpriv->counter, 1);
 	/* make sure hardware counter reading is not bogus due to initial value
 	 * not having been loaded yet */
 	if (devpriv->adc_count == 0 && hw_counter == devpriv->initial_hw_count) {
@@ -563,6 +553,16 @@ static int das16m1_attach(struct comedi_device *dev,
 			dev->irq = it->options[1];
 	}
 
+	dev->pacer = comedi_8254_init(dev->iobase + DAS16M1_8254_SECOND,
+				      I8254_OSC_BASE_10MHZ, I8254_IO8, 0);
+	if (!dev->pacer)
+		return -ENOMEM;
+
+	devpriv->counter = comedi_8254_init(dev->iobase + DAS16M1_8254_FIRST,
+					    0, I8254_IO8, 0);
+	if (!devpriv->counter)
+		return -ENOMEM;
+
 	ret = comedi_alloc_subdevices(dev, 4);
 	if (ret)
 		return ret;
@@ -609,9 +609,6 @@ static int das16m1_attach(struct comedi_device *dev,
 	if (ret)
 		return ret;
 
-	/*  disable upper half of hardware conversion counter so it doesn't mess with us */
-	outb(TOTAL_CLEAR, dev->iobase + DAS16M1_8254_FIRST_CNTRL);
-
 	/*  initialize digital output lines */
 	outb(0, dev->iobase + DAS16M1_DIO);
 
@@ -626,8 +623,11 @@ static void das16m1_detach(struct comedi_device *dev)
 {
 	struct das16m1_private_struct *devpriv = dev->private;
 
-	if (devpriv && devpriv->extra_iobase)
-		release_region(devpriv->extra_iobase, DAS16M1_SIZE2);
+	if (devpriv) {
+		if (devpriv->extra_iobase)
+			release_region(devpriv->extra_iobase, DAS16M1_SIZE2);
+		kfree(devpriv->counter);
+	}
 	comedi_legacy_detach(dev);
 }
 
diff --git a/drivers/staging/comedi/drivers/das1800.c b/drivers/staging/comedi/drivers/das1800.c
index 0790a28828de..53baf37cd21a 100644
--- a/drivers/staging/comedi/drivers/das1800.c
+++ b/drivers/staging/comedi/drivers/das1800.c
@@ -102,8 +102,7 @@ TODO:
 #include "../comedidev.h"
 
 #include "comedi_isadma.h"
-#include "comedi_fc.h"
-#include "8253.h"
+#include "comedi_8254.h"
 
 /* misc. defines */
 #define DAS1800_SIZE           16	/* uses 16 io addresses */
@@ -422,8 +421,6 @@ static const struct das1800_board das1800_boards[] = {
 
 struct das1800_private {
 	struct comedi_isadma *dma;
-	unsigned int divisor1;	/* value to load into board's counter 1 for timed conversions */
-	unsigned int divisor2;	/* value to load into board's counter 2 for timed conversions */
 	int irq_dma_bits;	/* bits for control register b */
 	/* dma bits for control register b, stored so that dma can be
 	 * turned on and off */
@@ -491,9 +488,7 @@ static void das1800_handle_fifo_not_empty(struct comedi_device *dev,
 
 	while (inb(dev->iobase + DAS1800_STATUS) & FNE) {
 		dpnt = inw(dev->iobase + DAS1800_FIFO);
-		/* convert to unsigned type if we are in a bipolar mode */
-		if (!unipolar)
-			;
+		/* convert to unsigned type */
 		dpnt = munge_bipolar_sample(dev, dpnt);
 		comedi_buf_write_samples(s, &dpnt, 1);
 
@@ -731,18 +726,18 @@ static int das1800_ai_do_cmdtest(struct comedi_device *dev,
 				 struct comedi_cmd *cmd)
 {
 	const struct das1800_board *thisboard = dev->board_ptr;
-	struct das1800_private *devpriv = dev->private;
 	int err = 0;
 	unsigned int arg;
 
 	/* Step 1 : check if triggers are trivially valid */
 
-	err |= cfc_check_trigger_src(&cmd->start_src, TRIG_NOW | TRIG_EXT);
-	err |= cfc_check_trigger_src(&cmd->scan_begin_src,
+	err |= comedi_check_trigger_src(&cmd->start_src, TRIG_NOW | TRIG_EXT);
+	err |= comedi_check_trigger_src(&cmd->scan_begin_src,
 					TRIG_FOLLOW | TRIG_TIMER | TRIG_EXT);
-	err |= cfc_check_trigger_src(&cmd->convert_src, TRIG_TIMER | TRIG_EXT);
-	err |= cfc_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
-	err |= cfc_check_trigger_src(&cmd->stop_src,
+	err |= comedi_check_trigger_src(&cmd->convert_src,
+					TRIG_TIMER | TRIG_EXT);
+	err |= comedi_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
+	err |= comedi_check_trigger_src(&cmd->stop_src,
 					TRIG_COUNT | TRIG_EXT | TRIG_NONE);
 
 	if (err)
@@ -750,10 +745,10 @@ static int das1800_ai_do_cmdtest(struct comedi_device *dev,
 
 	/* Step 2a : make sure trigger sources are unique */
 
-	err |= cfc_check_trigger_is_unique(cmd->start_src);
-	err |= cfc_check_trigger_is_unique(cmd->scan_begin_src);
-	err |= cfc_check_trigger_is_unique(cmd->convert_src);
-	err |= cfc_check_trigger_is_unique(cmd->stop_src);
+	err |= comedi_check_trigger_is_unique(cmd->start_src);
+	err |= comedi_check_trigger_is_unique(cmd->scan_begin_src);
+	err |= comedi_check_trigger_is_unique(cmd->convert_src);
+	err |= comedi_check_trigger_is_unique(cmd->stop_src);
 
 	/* Step 2b : and mutually compatible */
 
@@ -766,21 +761,23 @@ static int das1800_ai_do_cmdtest(struct comedi_device *dev,
 
 	/* Step 3: check if arguments are trivially valid */
 
-	err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
+	err |= comedi_check_trigger_arg_is(&cmd->start_arg, 0);
 
-	if (cmd->convert_src == TRIG_TIMER)
-		err |= cfc_check_trigger_arg_min(&cmd->convert_arg,
-						 thisboard->ai_speed);
+	if (cmd->convert_src == TRIG_TIMER) {
+		err |= comedi_check_trigger_arg_min(&cmd->convert_arg,
+						    thisboard->ai_speed);
+	}
 
-	err |= cfc_check_trigger_arg_min(&cmd->chanlist_len, 1);
-	err |= cfc_check_trigger_arg_is(&cmd->scan_end_arg, cmd->chanlist_len);
+	err |= comedi_check_trigger_arg_min(&cmd->chanlist_len, 1);
+	err |= comedi_check_trigger_arg_is(&cmd->scan_end_arg,
+					   cmd->chanlist_len);
 
 	switch (cmd->stop_src) {
 	case TRIG_COUNT:
-		err |= cfc_check_trigger_arg_min(&cmd->stop_arg, 1);
+		err |= comedi_check_trigger_arg_min(&cmd->stop_arg, 1);
 		break;
 	case TRIG_NONE:
-		err |= cfc_check_trigger_arg_is(&cmd->stop_arg, 0);
+		err |= comedi_check_trigger_arg_is(&cmd->stop_arg, 0);
 		break;
 	default:
 		break;
@@ -795,35 +792,24 @@ static int das1800_ai_do_cmdtest(struct comedi_device *dev,
 	    cmd->convert_src == TRIG_TIMER) {
 		/* we are not in burst mode */
 		arg = cmd->convert_arg;
-		i8253_cascade_ns_to_timer(I8254_OSC_BASE_5MHZ,
-					  &devpriv->divisor1,
-					  &devpriv->divisor2,
-					  &cmd->convert_arg, cmd->flags);
-		if (arg != cmd->convert_arg)
-			err++;
+		comedi_8254_cascade_ns_to_timer(dev->pacer, &arg, cmd->flags);
+		err |= comedi_check_trigger_arg_is(&cmd->convert_arg, arg);
 	} else if (cmd->convert_src == TRIG_TIMER) {
 		/* we are in burst mode */
-		arg = cmd->convert_arg;
-		cmd->convert_arg = burst_convert_arg(cmd->convert_arg,
-						     cmd->flags);
-		if (arg != cmd->convert_arg)
-			err++;
+		arg = burst_convert_arg(cmd->convert_arg, cmd->flags);
+		err |= comedi_check_trigger_arg_is(&cmd->convert_arg, arg);
 
 		if (cmd->scan_begin_src == TRIG_TIMER) {
 			arg = cmd->convert_arg * cmd->chanlist_len;
-			if (arg > cmd->scan_begin_arg) {
-				cmd->scan_begin_arg = arg;
-				err++;
-			}
+			err |= comedi_check_trigger_arg_max(&cmd->
+							    scan_begin_arg,
+							    arg);
 
 			arg = cmd->scan_begin_arg;
-			i8253_cascade_ns_to_timer(I8254_OSC_BASE_5MHZ,
-						  &devpriv->divisor1,
-						  &devpriv->divisor2,
-						  &cmd->scan_begin_arg,
-						  cmd->flags);
-			if (arg != cmd->scan_begin_arg)
-				err++;
+			comedi_8254_cascade_ns_to_timer(dev->pacer, &arg,
+							cmd->flags);
+			err |= comedi_check_trigger_arg_is(&cmd->scan_begin_arg,
+							   arg);
 		}
 	}
 
@@ -910,31 +896,6 @@ static int control_c_bits(const struct comedi_cmd *cmd)
 	return control_c;
 }
 
-static void das1800_setup_counters(struct comedi_device *dev,
-				   const struct comedi_cmd *cmd)
-{
-	struct das1800_private *devpriv = dev->private;
-	unsigned long timer_base = dev->iobase + DAS1800_COUNTER;
-
-	/* setup cascaded counters for conversion/scan frequency */
-	if ((cmd->scan_begin_src == TRIG_FOLLOW ||
-	     cmd->scan_begin_src == TRIG_TIMER) &&
-	    cmd->convert_src == TRIG_TIMER) {
-		i8254_set_mode(timer_base, 0, 1, I8254_MODE2 | I8254_BINARY);
-		i8254_set_mode(timer_base, 0, 2, I8254_MODE2 | I8254_BINARY);
-
-		i8254_write(timer_base, 0, 1, devpriv->divisor1);
-		i8254_write(timer_base, 0, 2, devpriv->divisor2);
-	}
-
-	/* setup counter 0 for 'about triggering' */
-	if (cmd->stop_src == TRIG_EXT) {
-		i8254_set_mode(timer_base, 0, 0, I8254_MODE0 | I8254_BINARY);
-
-		i8254_write(timer_base, 0, 0, 1);
-	}
-}
-
 static unsigned int das1800_ai_transfer_size(struct comedi_device *dev,
 					     struct comedi_subdevice *s,
 					     unsigned int maxbytes,
@@ -1053,7 +1014,19 @@ static int das1800_ai_do_cmd(struct comedi_device *dev,
 
 	/* setup card and start */
 	program_chanlist(dev, cmd);
-	das1800_setup_counters(dev, cmd);
+
+	/* setup cascaded counters for conversion/scan frequency */
+	if ((cmd->scan_begin_src == TRIG_FOLLOW ||
+	     cmd->scan_begin_src == TRIG_TIMER) &&
+	    cmd->convert_src == TRIG_TIMER) {
+		comedi_8254_update_divisors(dev->pacer);
+		comedi_8254_pacer_enable(dev->pacer, 1, 2, true);
+	}
+
+	/* setup counter 0 for 'about triggering' */
+	if (cmd->stop_src == TRIG_EXT)
+		comedi_8254_load(dev->pacer, 0, 1, I8254_MODE0 | I8254_BINARY);
+
 	das1800_ai_setup_dma(dev, s);
 	outb(control_c, dev->iobase + DAS1800_CONTROL_C);
 	/*  set conversion rate and length for burst mode */
@@ -1171,7 +1144,6 @@ static int das1800_di_rbits(struct comedi_device *dev,
 			    struct comedi_subdevice *s,
 			    struct comedi_insn *insn, unsigned int *data)
 {
-
 	data[1] = inb(dev->iobase + DAS1800_DIGITAL) & 0xf;
 	data[0] = 0;
 
@@ -1378,6 +1350,11 @@ static int das1800_attach(struct comedi_device *dev,
 	if (!devpriv->fifo_buf)
 		return -ENOMEM;
 
+	dev->pacer = comedi_8254_init(dev->iobase + DAS1800_COUNTER,
+				      I8254_OSC_BASE_5MHZ, I8254_IO8, 0);
+	if (!dev->pacer)
+		return -ENOMEM;
+
 	ret = comedi_alloc_subdevices(dev, 4);
 	if (ret)
 		return ret;
diff --git a/drivers/staging/comedi/drivers/das6402.c b/drivers/staging/comedi/drivers/das6402.c
index b8755b50a11e..1701294b79cd 100644
--- a/drivers/staging/comedi/drivers/das6402.c
+++ b/drivers/staging/comedi/drivers/das6402.c
@@ -35,8 +35,8 @@
 #include <linux/interrupt.h>
 
 #include "../comedidev.h"
-#include "comedi_fc.h"
-#include "8253.h"
+
+#include "comedi_8254.h"
 
 /*
  * Register I/O map
@@ -138,11 +138,6 @@ static struct das6402_boardinfo das6402_boards[] = {
 
 struct das6402_private {
 	unsigned int irq;
-
-	unsigned int count;
-	unsigned int divider1;
-	unsigned int divider2;
-
 	unsigned int ao_range;
 };
 
@@ -172,27 +167,6 @@ static void das6402_ai_clear_eoc(struct comedi_device *dev)
 	outb(DAS6402_STATUS_W_CLRINT, dev->iobase + DAS6402_STATUS_REG);
 }
 
-static void das6402_enable_counter(struct comedi_device *dev, bool load)
-{
-	struct das6402_private *devpriv = dev->private;
-	unsigned long timer_iobase = dev->iobase + DAS6402_TIMER_BASE;
-
-	if (load) {
-		i8254_set_mode(timer_iobase, 0, 0, I8254_MODE0 | I8254_BINARY);
-		i8254_set_mode(timer_iobase, 0, 1, I8254_MODE2 | I8254_BINARY);
-		i8254_set_mode(timer_iobase, 0, 2, I8254_MODE2 | I8254_BINARY);
-
-		i8254_write(timer_iobase, 0, 0, devpriv->count);
-		i8254_write(timer_iobase, 0, 1, devpriv->divider1);
-		i8254_write(timer_iobase, 0, 2, devpriv->divider2);
-
-	} else {
-		i8254_set_mode(timer_iobase, 0, 0, I8254_MODE0 | I8254_BINARY);
-		i8254_set_mode(timer_iobase, 0, 1, I8254_MODE0 | I8254_BINARY);
-		i8254_set_mode(timer_iobase, 0, 2, I8254_MODE0 | I8254_BINARY);
-	}
-}
-
 static unsigned int das6402_ai_read_sample(struct comedi_device *dev,
 					   struct comedi_subdevice *s)
 {
@@ -267,7 +241,8 @@ static int das6402_ai_cmd(struct comedi_device *dev,
 	outw(DAS6402_AI_MUX_HI(chan_hi) | DAS6402_AI_MUX_LO(chan_lo),
 	     dev->iobase + DAS6402_AI_MUX_REG);
 
-	das6402_enable_counter(dev, true);
+	comedi_8254_update_divisors(dev->pacer);
+	comedi_8254_pacer_enable(dev->pacer, 1, 2, true);
 
 	/* enable interrupt and pacer trigger */
 	outb(DAS6402_CTRL_INTE |
@@ -322,24 +297,23 @@ static int das6402_ai_cmdtest(struct comedi_device *dev,
 			      struct comedi_subdevice *s,
 			      struct comedi_cmd *cmd)
 {
-	struct das6402_private *devpriv = dev->private;
 	int err = 0;
 	unsigned int arg;
 
 	/* Step 1 : check if triggers are trivially valid */
 
-	err |= cfc_check_trigger_src(&cmd->start_src, TRIG_NOW);
-	err |= cfc_check_trigger_src(&cmd->scan_begin_src, TRIG_FOLLOW);
-	err |= cfc_check_trigger_src(&cmd->convert_src, TRIG_TIMER);
-	err |= cfc_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
-	err |= cfc_check_trigger_src(&cmd->stop_src, TRIG_COUNT | TRIG_NONE);
+	err |= comedi_check_trigger_src(&cmd->start_src, TRIG_NOW);
+	err |= comedi_check_trigger_src(&cmd->scan_begin_src, TRIG_FOLLOW);
+	err |= comedi_check_trigger_src(&cmd->convert_src, TRIG_TIMER);
+	err |= comedi_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
+	err |= comedi_check_trigger_src(&cmd->stop_src, TRIG_COUNT | TRIG_NONE);
 
 	if (err)
 		return 1;
 
 	/* Step 2a : make sure trigger sources are unique */
 
-	err |= cfc_check_trigger_is_unique(cmd->stop_src);
+	err |= comedi_check_trigger_is_unique(cmd->stop_src);
 
 	/* Step 2b : and mutually compatible */
 
@@ -348,30 +322,26 @@ static int das6402_ai_cmdtest(struct comedi_device *dev,
 
 	/* Step 3: check if arguments are trivially valid */
 
-	err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
-	err |= cfc_check_trigger_arg_is(&cmd->scan_begin_arg, 0);
-	err |= cfc_check_trigger_arg_min(&cmd->convert_arg, 10000);
-	err |= cfc_check_trigger_arg_min(&cmd->chanlist_len, 1);
-	err |= cfc_check_trigger_arg_is(&cmd->scan_end_arg, cmd->chanlist_len);
+	err |= comedi_check_trigger_arg_is(&cmd->start_arg, 0);
+	err |= comedi_check_trigger_arg_is(&cmd->scan_begin_arg, 0);
+	err |= comedi_check_trigger_arg_min(&cmd->convert_arg, 10000);
+	err |= comedi_check_trigger_arg_min(&cmd->chanlist_len, 1);
+	err |= comedi_check_trigger_arg_is(&cmd->scan_end_arg,
+					   cmd->chanlist_len);
 
 	if (cmd->stop_src == TRIG_COUNT)
-		err |= cfc_check_trigger_arg_min(&cmd->stop_arg, 1);
+		err |= comedi_check_trigger_arg_min(&cmd->stop_arg, 1);
 	else	/* TRIG_NONE */
-		err |= cfc_check_trigger_arg_is(&cmd->stop_arg, 0);
+		err |= comedi_check_trigger_arg_is(&cmd->stop_arg, 0);
 
 	if (err)
 		return 3;
 
 	/* step 4: fix up any arguments */
 
-	if (cmd->convert_src == TRIG_TIMER) {
-		arg = cmd->convert_arg;
-		i8253_cascade_ns_to_timer(I8254_OSC_BASE_10MHZ,
-					  &devpriv->divider1,
-					  &devpriv->divider2,
-					  &arg, cmd->flags);
-		err |= cfc_check_trigger_arg_is(&cmd->convert_arg, arg);
-	}
+	arg = cmd->convert_arg;
+	comedi_8254_cascade_ns_to_timer(dev->pacer, &arg, cmd->flags);
+	err |= comedi_check_trigger_arg_is(&cmd->convert_arg, arg);
 
 	if (err)
 		return 4;
@@ -581,8 +551,6 @@ static void das6402_reset(struct comedi_device *dev)
 	outw(0, dev->iobase + DAS6402_AO_DATA_REG(0));
 	inw(dev->iobase + DAS6402_AO_LSB_REG(0));
 
-	das6402_enable_counter(dev, false);
-
 	/* set all digital outputs low */
 	outb(0, dev->iobase + DAS6402_DI_DO_REG);
 
@@ -631,6 +599,11 @@ static int das6402_attach(struct comedi_device *dev,
 		}
 	}
 
+	dev->pacer = comedi_8254_init(dev->iobase + DAS6402_TIMER_BASE,
+				      I8254_OSC_BASE_10MHZ, I8254_IO8, 0);
+	if (!dev->pacer)
+		return -ENOMEM;
+
 	ret = comedi_alloc_subdevices(dev, 4);
 	if (ret)
 		return ret;
diff --git a/drivers/staging/comedi/drivers/das800.c b/drivers/staging/comedi/drivers/das800.c
index ff7f4be3f314..39d304a1259f 100644
--- a/drivers/staging/comedi/drivers/das800.c
+++ b/drivers/staging/comedi/drivers/das800.c
@@ -43,8 +43,6 @@ Notes:
 
 	The cio-das802/16 does not have a fifo-empty status bit!  Therefore
 	only fifo-half-full transfers are possible with this card.
-*/
-/*
 
 cmd triggers supported:
 	start_src:      TRIG_NOW | TRIG_EXT
@@ -52,18 +50,15 @@ cmd triggers supported:
 	scan_end_src:   TRIG_COUNT
 	convert_src:    TRIG_TIMER | TRIG_EXT
 	stop_src:       TRIG_NONE | TRIG_COUNT
-
-
 */
 
 #include <linux/module.h>
 #include <linux/interrupt.h>
-#include "../comedidev.h"
-
 #include <linux/delay.h>
 
-#include "8253.h"
-#include "comedi_fc.h"
+#include "../comedidev.h"
+
+#include "comedi_8254.h"
 
 #define N_CHAN_AI             8	/*  number of analog input channels */
 
@@ -219,8 +214,6 @@ static const struct das800_board das800_boards[] = {
 };
 
 struct das800_private {
-	unsigned int divisor1;	/* counter 1 value for timed conversions */
-	unsigned int divisor2;	/* counter 2 value for timed conversions */
 	unsigned int do_bits;	/* digital output bits */
 };
 
@@ -272,17 +265,6 @@ static void das800_disable(struct comedi_device *dev)
 	spin_unlock_irqrestore(&dev->spinlock, irq_flags);
 }
 
-static void das800_set_frequency(struct comedi_device *dev)
-{
-	struct das800_private *devpriv = dev->private;
-	unsigned long timer_base = dev->iobase + DAS800_8254;
-
-	i8254_set_mode(timer_base, 0, 1, I8254_MODE2 | I8254_BINARY);
-	i8254_set_mode(timer_base, 0, 2, I8254_MODE2 | I8254_BINARY);
-	i8254_write(timer_base, 0, 1, devpriv->divisor1);
-	i8254_write(timer_base, 0, 2, devpriv->divisor2);
-}
-
 static int das800_cancel(struct comedi_device *dev, struct comedi_subdevice *s)
 {
 	das800_disable(dev);
@@ -322,26 +304,25 @@ static int das800_ai_do_cmdtest(struct comedi_device *dev,
 				struct comedi_cmd *cmd)
 {
 	const struct das800_board *thisboard = dev->board_ptr;
-	struct das800_private *devpriv = dev->private;
 	int err = 0;
-	unsigned int arg;
 
 	/* Step 1 : check if triggers are trivially valid */
 
-	err |= cfc_check_trigger_src(&cmd->start_src, TRIG_NOW | TRIG_EXT);
-	err |= cfc_check_trigger_src(&cmd->scan_begin_src, TRIG_FOLLOW);
-	err |= cfc_check_trigger_src(&cmd->convert_src, TRIG_TIMER | TRIG_EXT);
-	err |= cfc_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
-	err |= cfc_check_trigger_src(&cmd->stop_src, TRIG_COUNT | TRIG_NONE);
+	err |= comedi_check_trigger_src(&cmd->start_src, TRIG_NOW | TRIG_EXT);
+	err |= comedi_check_trigger_src(&cmd->scan_begin_src, TRIG_FOLLOW);
+	err |= comedi_check_trigger_src(&cmd->convert_src,
+					TRIG_TIMER | TRIG_EXT);
+	err |= comedi_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
+	err |= comedi_check_trigger_src(&cmd->stop_src, TRIG_COUNT | TRIG_NONE);
 
 	if (err)
 		return 1;
 
 	/* Step 2a : make sure trigger sources are unique */
 
-	err |= cfc_check_trigger_is_unique(cmd->start_src);
-	err |= cfc_check_trigger_is_unique(cmd->convert_src);
-	err |= cfc_check_trigger_is_unique(cmd->stop_src);
+	err |= comedi_check_trigger_is_unique(cmd->start_src);
+	err |= comedi_check_trigger_is_unique(cmd->convert_src);
+	err |= comedi_check_trigger_is_unique(cmd->stop_src);
 
 	/* Step 2b : and mutually compatible */
 
@@ -350,19 +331,21 @@ static int das800_ai_do_cmdtest(struct comedi_device *dev,
 
 	/* Step 3: check if arguments are trivially valid */
 
-	err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
+	err |= comedi_check_trigger_arg_is(&cmd->start_arg, 0);
 
-	if (cmd->convert_src == TRIG_TIMER)
-		err |= cfc_check_trigger_arg_min(&cmd->convert_arg,
-						 thisboard->ai_speed);
+	if (cmd->convert_src == TRIG_TIMER) {
+		err |= comedi_check_trigger_arg_min(&cmd->convert_arg,
+						    thisboard->ai_speed);
+	}
 
-	err |= cfc_check_trigger_arg_min(&cmd->chanlist_len, 1);
-	err |= cfc_check_trigger_arg_is(&cmd->scan_end_arg, cmd->chanlist_len);
+	err |= comedi_check_trigger_arg_min(&cmd->chanlist_len, 1);
+	err |= comedi_check_trigger_arg_is(&cmd->scan_end_arg,
+					   cmd->chanlist_len);
 
 	if (cmd->stop_src == TRIG_COUNT)
-		err |= cfc_check_trigger_arg_min(&cmd->stop_arg, 1);
+		err |= comedi_check_trigger_arg_min(&cmd->stop_arg, 1);
 	else	/* TRIG_NONE */
-		err |= cfc_check_trigger_arg_is(&cmd->stop_arg, 0);
+		err |= comedi_check_trigger_arg_is(&cmd->stop_arg, 0);
 
 	if (err)
 		return 3;
@@ -370,12 +353,10 @@ static int das800_ai_do_cmdtest(struct comedi_device *dev,
 	/* step 4: fix up any arguments */
 
 	if (cmd->convert_src == TRIG_TIMER) {
-		arg = cmd->convert_arg;
-		i8253_cascade_ns_to_timer(I8254_OSC_BASE_1MHZ,
-					  &devpriv->divisor1,
-					  &devpriv->divisor2,
-					  &arg, cmd->flags);
-		err |= cfc_check_trigger_arg_is(&cmd->convert_arg, arg);
+		unsigned int arg = cmd->convert_arg;
+
+		comedi_8254_cascade_ns_to_timer(dev->pacer, &arg, cmd->flags);
+		err |= comedi_check_trigger_arg_is(&cmd->convert_arg, arg);
 	}
 
 	if (err)
@@ -426,8 +407,8 @@ static int das800_ai_do_cmd(struct comedi_device *dev,
 		conv_bits |= DTEN;
 	if (cmd->convert_src == TRIG_TIMER) {
 		conv_bits |= CASC | ITE;
-		/* set conversion frequency */
-		das800_set_frequency(dev);
+		comedi_8254_update_divisors(dev->pacer);
+		comedi_8254_pacer_enable(dev->pacer, 1, 2, true);
 	}
 
 	spin_lock_irqsave(&dev->spinlock, irq_flags);
@@ -697,6 +678,11 @@ static int das800_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 			dev->irq = irq;
 	}
 
+	dev->pacer = comedi_8254_init(dev->iobase + DAS800_8254,
+				      I8254_OSC_BASE_1MHZ, I8254_IO8, 0);
+	if (!dev->pacer)
+		return -ENOMEM;
+
 	ret = comedi_alloc_subdevices(dev, 3);
 	if (ret)
 		return ret;
diff --git a/drivers/staging/comedi/drivers/dmm32at.c b/drivers/staging/comedi/drivers/dmm32at.c
index 1af006609fc1..bb2883c83afa 100644
--- a/drivers/staging/comedi/drivers/dmm32at.c
+++ b/drivers/staging/comedi/drivers/dmm32at.c
@@ -30,7 +30,7 @@
  * This driver is for the Diamond Systems MM-32-AT board
  *	http://www.diamondsystems.com/products/diamondmm32at
  *
- * It is being used on serveral projects inside NASA, without
+ * It is being used on several projects inside NASA, without
  * problems so far. For analog input commands, TRIG_EXT is not
  * yet supported.
  */
@@ -41,7 +41,6 @@
 #include "../comedidev.h"
 
 #include "8255.h"
-#include "comedi_fc.h"
 
 /* Board register addresses */
 #define DMM32AT_AI_START_CONV_REG	0x00
@@ -274,18 +273,18 @@ static int dmm32at_ai_cmdtest(struct comedi_device *dev,
 
 	/* Step 1 : check if triggers are trivially valid */
 
-	err |= cfc_check_trigger_src(&cmd->start_src, TRIG_NOW);
-	err |= cfc_check_trigger_src(&cmd->scan_begin_src, TRIG_TIMER);
-	err |= cfc_check_trigger_src(&cmd->convert_src, TRIG_TIMER);
-	err |= cfc_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
-	err |= cfc_check_trigger_src(&cmd->stop_src, TRIG_COUNT | TRIG_NONE);
+	err |= comedi_check_trigger_src(&cmd->start_src, TRIG_NOW);
+	err |= comedi_check_trigger_src(&cmd->scan_begin_src, TRIG_TIMER);
+	err |= comedi_check_trigger_src(&cmd->convert_src, TRIG_TIMER);
+	err |= comedi_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
+	err |= comedi_check_trigger_src(&cmd->stop_src, TRIG_COUNT | TRIG_NONE);
 
 	if (err)
 		return 1;
 
 	/* Step 2a : make sure trigger sources are unique */
 
-	err |= cfc_check_trigger_is_unique(cmd->stop_src);
+	err |= comedi_check_trigger_is_unique(cmd->stop_src);
 
 	/* Step 2b : and mutually compatible */
 
@@ -294,10 +293,10 @@ static int dmm32at_ai_cmdtest(struct comedi_device *dev,
 
 	/* Step 3: check if arguments are trivially valid */
 
-	err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
+	err |= comedi_check_trigger_arg_is(&cmd->start_arg, 0);
 
-	err |= cfc_check_trigger_arg_min(&cmd->scan_begin_arg, 1000000);
-	err |= cfc_check_trigger_arg_max(&cmd->scan_begin_arg, 1000000000);
+	err |= comedi_check_trigger_arg_min(&cmd->scan_begin_arg, 1000000);
+	err |= comedi_check_trigger_arg_max(&cmd->scan_begin_arg, 1000000000);
 
 	if (cmd->convert_arg >= 17500)
 		cmd->convert_arg = 20000;
@@ -308,12 +307,13 @@ static int dmm32at_ai_cmdtest(struct comedi_device *dev,
 	else
 		cmd->convert_arg = 5000;
 
-	err |= cfc_check_trigger_arg_is(&cmd->scan_end_arg, cmd->chanlist_len);
+	err |= comedi_check_trigger_arg_is(&cmd->scan_end_arg,
+					   cmd->chanlist_len);
 
 	if (cmd->stop_src == TRIG_COUNT)
-		err |= cfc_check_trigger_arg_min(&cmd->stop_arg, 1);
+		err |= comedi_check_trigger_arg_min(&cmd->stop_arg, 1);
 	else /* TRIG_NONE */
-		err |= cfc_check_trigger_arg_is(&cmd->stop_arg, 0);
+		err |= comedi_check_trigger_arg_is(&cmd->stop_arg, 0);
 
 	if (err)
 		return 3;
@@ -321,7 +321,7 @@ static int dmm32at_ai_cmdtest(struct comedi_device *dev,
 	/* Step 4: fix up any arguments */
 
 	arg = cmd->convert_arg * cmd->scan_end_arg;
-	err |= cfc_check_trigger_arg_min(&cmd->scan_begin_arg, arg);
+	err |= comedi_check_trigger_arg_min(&cmd->scan_begin_arg, arg);
 
 	if (err)
 		return 4;
@@ -391,13 +391,12 @@ static int dmm32at_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 		/* start the clock and enable the interrupts */
 		dmm32at_setaitimer(dev, cmd->scan_begin_arg);
 	} else {
-		/* start the interrups and initiate a single scan */
+		/* start the interrupts and initiate a single scan */
 		outb(DMM32AT_INTCLK_ADINT, dev->iobase + DMM32AT_INTCLK_REG);
 		outb(0xff, dev->iobase + DMM32AT_AI_START_CONV_REG);
 	}
 
 	return 0;
-
 }
 
 static int dmm32at_ai_cancel(struct comedi_device *dev,
diff --git a/drivers/staging/comedi/drivers/dt2801.c b/drivers/staging/comedi/drivers/dt2801.c
index b96e60ffad73..80e38dedd359 100644
--- a/drivers/staging/comedi/drivers/dt2801.c
+++ b/drivers/staging/comedi/drivers/dt2801.c
@@ -126,7 +126,6 @@ static const struct comedi_lrange range_dt2801_ai_pgl_unipolar = {
 };
 
 struct dt2801_board {
-
 	const char *name;
 	int boardcode;
 	int ad_diff;
@@ -280,7 +279,7 @@ static int dt2801_writedata2(struct comedi_device *dev, unsigned int data)
 	ret = dt2801_writedata(dev, data & 0xff);
 	if (ret < 0)
 		return ret;
-	ret = dt2801_writedata(dev, (data >> 8));
+	ret = dt2801_writedata(dev, data >> 8);
 	if (ret < 0)
 		return ret;
 
diff --git a/drivers/staging/comedi/drivers/dt2811.c b/drivers/staging/comedi/drivers/dt2811.c
index d660f277487e..a80773291fdc 100644
--- a/drivers/staging/comedi/drivers/dt2811.c
+++ b/drivers/staging/comedi/drivers/dt2811.c
@@ -194,7 +194,6 @@ static const struct comedi_lrange range_dt2811_pgl_ai_5_bipolar = {
 #define DT2811_ADMODE   0x03
 
 struct dt2811_board {
-
 	const char *name;
 	const struct comedi_lrange *bip_5;
 	const struct comedi_lrange *bip_2_5;
diff --git a/drivers/staging/comedi/drivers/dt2814.c b/drivers/staging/comedi/drivers/dt2814.c
index 9805be13005a..66705f9a0621 100644
--- a/drivers/staging/comedi/drivers/dt2814.c
+++ b/drivers/staging/comedi/drivers/dt2814.c
@@ -40,8 +40,6 @@ addition, the clock does not seem to be very accurate.
 
 #include <linux/delay.h>
 
-#include "comedi_fc.h"
-
 #define DT2814_CSR 0
 #define DT2814_DATA 1
 
@@ -56,7 +54,6 @@ addition, the clock does not seem to be very accurate.
 #define DT2814_CHANMASK 0x0f
 
 struct dt2814_private {
-
 	int ntrig;
 	int curadchan;
 };
@@ -130,18 +127,18 @@ static int dt2814_ai_cmdtest(struct comedi_device *dev,
 
 	/* Step 1 : check if triggers are trivially valid */
 
-	err |= cfc_check_trigger_src(&cmd->start_src, TRIG_NOW);
-	err |= cfc_check_trigger_src(&cmd->scan_begin_src, TRIG_TIMER);
-	err |= cfc_check_trigger_src(&cmd->convert_src, TRIG_NOW);
-	err |= cfc_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
-	err |= cfc_check_trigger_src(&cmd->stop_src, TRIG_COUNT | TRIG_NONE);
+	err |= comedi_check_trigger_src(&cmd->start_src, TRIG_NOW);
+	err |= comedi_check_trigger_src(&cmd->scan_begin_src, TRIG_TIMER);
+	err |= comedi_check_trigger_src(&cmd->convert_src, TRIG_NOW);
+	err |= comedi_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
+	err |= comedi_check_trigger_src(&cmd->stop_src, TRIG_COUNT | TRIG_NONE);
 
 	if (err)
 		return 1;
 
 	/* Step 2a : make sure trigger sources are unique */
 
-	err |= cfc_check_trigger_is_unique(cmd->stop_src);
+	err |= comedi_check_trigger_is_unique(cmd->stop_src);
 
 	/* Step 2b : and mutually compatible */
 
@@ -150,18 +147,19 @@ static int dt2814_ai_cmdtest(struct comedi_device *dev,
 
 	/* Step 3: check if arguments are trivially valid */
 
-	err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
+	err |= comedi_check_trigger_arg_is(&cmd->start_arg, 0);
 
-	err |= cfc_check_trigger_arg_max(&cmd->scan_begin_arg, 1000000000);
-	err |= cfc_check_trigger_arg_min(&cmd->scan_begin_arg,
-					 DT2814_MAX_SPEED);
+	err |= comedi_check_trigger_arg_max(&cmd->scan_begin_arg, 1000000000);
+	err |= comedi_check_trigger_arg_min(&cmd->scan_begin_arg,
+					    DT2814_MAX_SPEED);
 
-	err |= cfc_check_trigger_arg_is(&cmd->scan_end_arg, cmd->chanlist_len);
+	err |= comedi_check_trigger_arg_is(&cmd->scan_end_arg,
+					   cmd->chanlist_len);
 
 	if (cmd->stop_src == TRIG_COUNT)
-		err |= cfc_check_trigger_arg_min(&cmd->stop_arg, 2);
+		err |= comedi_check_trigger_arg_min(&cmd->stop_arg, 2);
 	else	/* TRIG_NONE */
-		err |= cfc_check_trigger_arg_is(&cmd->stop_arg, 0);
+		err |= comedi_check_trigger_arg_is(&cmd->stop_arg, 0);
 
 	if (err)
 		return 3;
@@ -170,7 +168,7 @@ static int dt2814_ai_cmdtest(struct comedi_device *dev,
 
 	arg = cmd->scan_begin_arg;
 	dt2814_ns_to_timer(&arg, cmd->flags);
-	err |= cfc_check_trigger_arg_is(&cmd->scan_begin_arg, arg);
+	err |= comedi_check_trigger_arg_is(&cmd->scan_begin_arg, arg);
 
 	if (err)
 		return 4;
@@ -193,7 +191,6 @@ static int dt2814_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	outb(chan | DT2814_ENB | (trigvar << 5), dev->iobase + DT2814_CSR);
 
 	return 0;
-
 }
 
 static irqreturn_t dt2814_interrupt(int irq, void *d)
diff --git a/drivers/staging/comedi/drivers/dt2815.c b/drivers/staging/comedi/drivers/dt2815.c
index a98fb66fdd53..fb08569c1ac1 100644
--- a/drivers/staging/comedi/drivers/dt2815.c
+++ b/drivers/staging/comedi/drivers/dt2815.c
@@ -60,7 +60,6 @@ Configuration options:
 #define DT2815_STATUS 1
 
 struct dt2815_private {
-
 	const struct comedi_lrange *range_type_list[8];
 	unsigned int ao_readback[8];
 };
diff --git a/drivers/staging/comedi/drivers/dt282x.c b/drivers/staging/comedi/drivers/dt282x.c
index db21d2135856..5a536a00066f 100644
--- a/drivers/staging/comedi/drivers/dt282x.c
+++ b/drivers/staging/comedi/drivers/dt282x.c
@@ -64,7 +64,6 @@
 #include "../comedidev.h"
 
 #include "comedi_isadma.h"
-#include "comedi_fc.h"
 
 /*
  * Register map
@@ -661,20 +660,20 @@ static int dt282x_ai_cmdtest(struct comedi_device *dev,
 
 	/* Step 1 : check if triggers are trivially valid */
 
-	err |= cfc_check_trigger_src(&cmd->start_src, TRIG_NOW);
-	err |= cfc_check_trigger_src(&cmd->scan_begin_src,
+	err |= comedi_check_trigger_src(&cmd->start_src, TRIG_NOW);
+	err |= comedi_check_trigger_src(&cmd->scan_begin_src,
 					TRIG_FOLLOW | TRIG_EXT);
-	err |= cfc_check_trigger_src(&cmd->convert_src, TRIG_TIMER);
-	err |= cfc_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
-	err |= cfc_check_trigger_src(&cmd->stop_src, TRIG_COUNT | TRIG_NONE);
+	err |= comedi_check_trigger_src(&cmd->convert_src, TRIG_TIMER);
+	err |= comedi_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
+	err |= comedi_check_trigger_src(&cmd->stop_src, TRIG_COUNT | TRIG_NONE);
 
 	if (err)
 		return 1;
 
 	/* Step 2a : make sure trigger sources are unique */
 
-	err |= cfc_check_trigger_is_unique(cmd->scan_begin_src);
-	err |= cfc_check_trigger_is_unique(cmd->stop_src);
+	err |= comedi_check_trigger_is_unique(cmd->scan_begin_src);
+	err |= comedi_check_trigger_is_unique(cmd->stop_src);
 
 	/* Step 2b : and mutually compatible */
 
@@ -683,21 +682,22 @@ static int dt282x_ai_cmdtest(struct comedi_device *dev,
 
 	/* Step 3: check if arguments are trivially valid */
 
-	err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
+	err |= comedi_check_trigger_arg_is(&cmd->start_arg, 0);
 
-	err |= cfc_check_trigger_arg_is(&cmd->scan_begin_arg, 0);
+	err |= comedi_check_trigger_arg_is(&cmd->scan_begin_arg, 0);
 
-	err |= cfc_check_trigger_arg_min(&cmd->convert_arg, 4000);
+	err |= comedi_check_trigger_arg_min(&cmd->convert_arg, 4000);
 
 #define SLOWEST_TIMER	(250*(1<<15)*255)
-	err |= cfc_check_trigger_arg_max(&cmd->convert_arg, SLOWEST_TIMER);
-	err |= cfc_check_trigger_arg_min(&cmd->convert_arg, board->ai_speed);
-	err |= cfc_check_trigger_arg_is(&cmd->scan_end_arg, cmd->chanlist_len);
+	err |= comedi_check_trigger_arg_max(&cmd->convert_arg, SLOWEST_TIMER);
+	err |= comedi_check_trigger_arg_min(&cmd->convert_arg, board->ai_speed);
+	err |= comedi_check_trigger_arg_is(&cmd->scan_end_arg,
+					   cmd->chanlist_len);
 
 	if (cmd->stop_src == TRIG_COUNT)
-		err |= cfc_check_trigger_arg_min(&cmd->stop_arg, 1);
+		err |= comedi_check_trigger_arg_min(&cmd->stop_arg, 1);
 	else	/* TRIG_EXT | TRIG_NONE */
-		err |= cfc_check_trigger_arg_is(&cmd->stop_arg, 0);
+		err |= comedi_check_trigger_arg_is(&cmd->stop_arg, 0);
 
 	if (err)
 		return 3;
@@ -706,7 +706,7 @@ static int dt282x_ai_cmdtest(struct comedi_device *dev,
 
 	arg = cmd->convert_arg;
 	devpriv->divisor = dt282x_ns_to_timer(&arg, cmd->flags);
-	err |= cfc_check_trigger_arg_is(&cmd->convert_arg, arg);
+	err |= comedi_check_trigger_arg_is(&cmd->convert_arg, arg);
 
 	if (err)
 		return 4;
@@ -764,7 +764,7 @@ static int dt282x_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 
 	if (cmd->scan_begin_src == TRIG_FOLLOW) {
 		outw(devpriv->supcsr | DT2821_SUPCSR_STRIG,
-			dev->iobase + DT2821_SUPCSR_REG);
+		     dev->iobase + DT2821_SUPCSR_REG);
 	} else {
 		devpriv->supcsr |= DT2821_SUPCSR_XTRIG;
 		outw(devpriv->supcsr, dev->iobase + DT2821_SUPCSR_REG);
@@ -831,18 +831,18 @@ static int dt282x_ao_cmdtest(struct comedi_device *dev,
 
 	/* Step 1 : check if triggers are trivially valid */
 
-	err |= cfc_check_trigger_src(&cmd->start_src, TRIG_INT);
-	err |= cfc_check_trigger_src(&cmd->scan_begin_src, TRIG_TIMER);
-	err |= cfc_check_trigger_src(&cmd->convert_src, TRIG_NOW);
-	err |= cfc_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
-	err |= cfc_check_trigger_src(&cmd->stop_src, TRIG_COUNT | TRIG_NONE);
+	err |= comedi_check_trigger_src(&cmd->start_src, TRIG_INT);
+	err |= comedi_check_trigger_src(&cmd->scan_begin_src, TRIG_TIMER);
+	err |= comedi_check_trigger_src(&cmd->convert_src, TRIG_NOW);
+	err |= comedi_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
+	err |= comedi_check_trigger_src(&cmd->stop_src, TRIG_COUNT | TRIG_NONE);
 
 	if (err)
 		return 1;
 
 	/* Step 2a : make sure trigger sources are unique */
 
-	err |= cfc_check_trigger_is_unique(cmd->stop_src);
+	err |= comedi_check_trigger_is_unique(cmd->stop_src);
 
 	/* Step 2b : and mutually compatible */
 
@@ -851,15 +851,16 @@ static int dt282x_ao_cmdtest(struct comedi_device *dev,
 
 	/* Step 3: check if arguments are trivially valid */
 
-	err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
-	err |= cfc_check_trigger_arg_min(&cmd->scan_begin_arg, 5000);
-	err |= cfc_check_trigger_arg_is(&cmd->convert_arg, 0);
-	err |= cfc_check_trigger_arg_is(&cmd->scan_end_arg, cmd->chanlist_len);
+	err |= comedi_check_trigger_arg_is(&cmd->start_arg, 0);
+	err |= comedi_check_trigger_arg_min(&cmd->scan_begin_arg, 5000);
+	err |= comedi_check_trigger_arg_is(&cmd->convert_arg, 0);
+	err |= comedi_check_trigger_arg_is(&cmd->scan_end_arg,
+					   cmd->chanlist_len);
 
 	if (cmd->stop_src == TRIG_COUNT)
-		err |= cfc_check_trigger_arg_min(&cmd->stop_arg, 1);
+		err |= comedi_check_trigger_arg_min(&cmd->stop_arg, 1);
 	else	/* TRIG_EXT | TRIG_NONE */
-		err |= cfc_check_trigger_arg_is(&cmd->stop_arg, 0);
+		err |= comedi_check_trigger_arg_is(&cmd->stop_arg, 0);
 
 	if (err)
 		return 3;
@@ -868,13 +869,12 @@ static int dt282x_ao_cmdtest(struct comedi_device *dev,
 
 	arg = cmd->scan_begin_arg;
 	devpriv->divisor = dt282x_ns_to_timer(&arg, cmd->flags);
-	err |= cfc_check_trigger_arg_is(&cmd->scan_begin_arg, arg);
+	err |= comedi_check_trigger_arg_is(&cmd->scan_begin_arg, arg);
 
 	if (err)
 		return 4;
 
 	return 0;
-
 }
 
 static int dt282x_ao_inttrig(struct comedi_device *dev,
diff --git a/drivers/staging/comedi/drivers/dt3000.c b/drivers/staging/comedi/drivers/dt3000.c
index 0aa51980e327..031282c82f43 100644
--- a/drivers/staging/comedi/drivers/dt3000.c
+++ b/drivers/staging/comedi/drivers/dt3000.c
@@ -49,13 +49,10 @@ AO commands are not supported.
 */
 
 #include <linux/module.h>
-#include <linux/pci.h>
 #include <linux/delay.h>
 #include <linux/interrupt.h>
 
-#include "../comedidev.h"
-
-#include "comedi_fc.h"
+#include "../comedi_pci.h"
 
 static const struct comedi_lrange range_dt3000_ai = {
 	4, {
@@ -409,11 +406,11 @@ static int dt3k_ai_cmdtest(struct comedi_device *dev,
 
 	/* Step 1 : check if triggers are trivially valid */
 
-	err |= cfc_check_trigger_src(&cmd->start_src, TRIG_NOW);
-	err |= cfc_check_trigger_src(&cmd->scan_begin_src, TRIG_TIMER);
-	err |= cfc_check_trigger_src(&cmd->convert_src, TRIG_TIMER);
-	err |= cfc_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
-	err |= cfc_check_trigger_src(&cmd->stop_src, TRIG_COUNT);
+	err |= comedi_check_trigger_src(&cmd->start_src, TRIG_NOW);
+	err |= comedi_check_trigger_src(&cmd->scan_begin_src, TRIG_TIMER);
+	err |= comedi_check_trigger_src(&cmd->convert_src, TRIG_TIMER);
+	err |= comedi_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
+	err |= comedi_check_trigger_src(&cmd->stop_src, TRIG_COUNT);
 
 	if (err)
 		return 1;
@@ -423,28 +420,29 @@ static int dt3k_ai_cmdtest(struct comedi_device *dev,
 
 	/* Step 3: check if arguments are trivially valid */
 
-	err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
+	err |= comedi_check_trigger_arg_is(&cmd->start_arg, 0);
 
 	if (cmd->scan_begin_src == TRIG_TIMER) {
-		err |= cfc_check_trigger_arg_min(&cmd->scan_begin_arg,
-						 this_board->ai_speed);
-		err |= cfc_check_trigger_arg_max(&cmd->scan_begin_arg,
-						 100 * 16 * 65535);
+		err |= comedi_check_trigger_arg_min(&cmd->scan_begin_arg,
+						    this_board->ai_speed);
+		err |= comedi_check_trigger_arg_max(&cmd->scan_begin_arg,
+						    100 * 16 * 65535);
 	}
 
 	if (cmd->convert_src == TRIG_TIMER) {
-		err |= cfc_check_trigger_arg_min(&cmd->convert_arg,
-						 this_board->ai_speed);
-		err |= cfc_check_trigger_arg_max(&cmd->convert_arg,
-						 50 * 16 * 65535);
+		err |= comedi_check_trigger_arg_min(&cmd->convert_arg,
+						    this_board->ai_speed);
+		err |= comedi_check_trigger_arg_max(&cmd->convert_arg,
+						    50 * 16 * 65535);
 	}
 
-	err |= cfc_check_trigger_arg_is(&cmd->scan_end_arg, cmd->chanlist_len);
+	err |= comedi_check_trigger_arg_is(&cmd->scan_end_arg,
+					   cmd->chanlist_len);
 
 	if (cmd->stop_src == TRIG_COUNT)
-		err |= cfc_check_trigger_arg_max(&cmd->stop_arg, 0x00ffffff);
+		err |= comedi_check_trigger_arg_max(&cmd->stop_arg, 0x00ffffff);
 	else	/* TRIG_NONE */
-		err |= cfc_check_trigger_arg_is(&cmd->stop_arg, 0);
+		err |= comedi_check_trigger_arg_is(&cmd->stop_arg, 0);
 
 	if (err)
 		return 3;
@@ -454,18 +452,19 @@ static int dt3k_ai_cmdtest(struct comedi_device *dev,
 	if (cmd->scan_begin_src == TRIG_TIMER) {
 		arg = cmd->scan_begin_arg;
 		dt3k_ns_to_timer(100, &arg, cmd->flags);
-		err |= cfc_check_trigger_arg_is(&cmd->scan_begin_arg, arg);
+		err |= comedi_check_trigger_arg_is(&cmd->scan_begin_arg, arg);
 	}
 
 	if (cmd->convert_src == TRIG_TIMER) {
 		arg = cmd->convert_arg;
 		dt3k_ns_to_timer(50, &arg, cmd->flags);
-		err |= cfc_check_trigger_arg_is(&cmd->convert_arg, arg);
+		err |= comedi_check_trigger_arg_is(&cmd->convert_arg, arg);
 
 		if (cmd->scan_begin_src == TRIG_TIMER) {
 			arg = cmd->convert_arg * cmd->scan_end_arg;
-			err |= cfc_check_trigger_arg_min(&cmd->scan_begin_arg,
-							 arg);
+			err |= comedi_check_trigger_arg_min(&cmd->
+							    scan_begin_arg,
+							    arg);
 		}
 	}
 
diff --git a/drivers/staging/comedi/drivers/dyna_pci10xx.c b/drivers/staging/comedi/drivers/dyna_pci10xx.c
index 6c1e442f6c81..c9eb26fab44e 100644
--- a/drivers/staging/comedi/drivers/dyna_pci10xx.c
+++ b/drivers/staging/comedi/drivers/dyna_pci10xx.c
@@ -34,10 +34,9 @@
 
 #include <linux/module.h>
 #include <linux/delay.h>
-#include <linux/pci.h>
 #include <linux/mutex.h>
 
-#include "../comedidev.h"
+#include "../comedi_pci.h"
 
 #define READ_TIMEOUT 50
 
@@ -70,8 +69,9 @@ static int dyna_pci10xx_ai_eoc(struct comedi_device *dev,
 }
 
 static int dyna_pci10xx_insn_read_ai(struct comedi_device *dev,
-			struct comedi_subdevice *s,
-			struct comedi_insn *insn, unsigned int *data)
+				     struct comedi_subdevice *s,
+				     struct comedi_insn *insn,
+				     unsigned int *data)
 {
 	struct dyna_pci10xx_private *devpriv = dev->private;
 	int n;
@@ -109,8 +109,9 @@ static int dyna_pci10xx_insn_read_ai(struct comedi_device *dev,
 
 /* analog output callback */
 static int dyna_pci10xx_insn_write_ao(struct comedi_device *dev,
-				 struct comedi_subdevice *s,
-				 struct comedi_insn *insn, unsigned int *data)
+				      struct comedi_subdevice *s,
+				      struct comedi_insn *insn,
+				      unsigned int *data)
 {
 	struct dyna_pci10xx_private *devpriv = dev->private;
 	int n;
@@ -132,8 +133,9 @@ static int dyna_pci10xx_insn_write_ao(struct comedi_device *dev,
 
 /* digital input bit interface */
 static int dyna_pci10xx_di_insn_bits(struct comedi_device *dev,
-			      struct comedi_subdevice *s,
-			      struct comedi_insn *insn, unsigned int *data)
+				     struct comedi_subdevice *s,
+				     struct comedi_insn *insn,
+				     unsigned int *data)
 {
 	struct dyna_pci10xx_private *devpriv = dev->private;
 	u16 d = 0;
@@ -171,7 +173,7 @@ static int dyna_pci10xx_do_insn_bits(struct comedi_device *dev,
 }
 
 static int dyna_pci10xx_auto_attach(struct comedi_device *dev,
-					      unsigned long context_unused)
+				    unsigned long context_unused)
 {
 	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
 	struct dyna_pci10xx_private *devpriv;
diff --git a/drivers/staging/comedi/drivers/gsc_hpdi.c b/drivers/staging/comedi/drivers/gsc_hpdi.c
index deada9784b69..3cb6409c4f01 100644
--- a/drivers/staging/comedi/drivers/gsc_hpdi.c
+++ b/drivers/staging/comedi/drivers/gsc_hpdi.c
@@ -41,14 +41,12 @@
  */
 
 #include <linux/module.h>
-#include <linux/pci.h>
 #include <linux/delay.h>
 #include <linux/interrupt.h>
 
-#include "../comedidev.h"
+#include "../comedi_pci.h"
 
 #include "plx9080.h"
-#include "comedi_fc.h"
 
 /*
  * PCI BAR2 Register map (dev->mmio)
@@ -386,18 +384,18 @@ static int gsc_hpdi_cmd_test(struct comedi_device *dev,
 
 	/* Step 1 : check if triggers are trivially valid */
 
-	err |= cfc_check_trigger_src(&cmd->start_src, TRIG_NOW);
-	err |= cfc_check_trigger_src(&cmd->scan_begin_src, TRIG_EXT);
-	err |= cfc_check_trigger_src(&cmd->convert_src, TRIG_NOW);
-	err |= cfc_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
-	err |= cfc_check_trigger_src(&cmd->stop_src, TRIG_COUNT | TRIG_NONE);
+	err |= comedi_check_trigger_src(&cmd->start_src, TRIG_NOW);
+	err |= comedi_check_trigger_src(&cmd->scan_begin_src, TRIG_EXT);
+	err |= comedi_check_trigger_src(&cmd->convert_src, TRIG_NOW);
+	err |= comedi_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
+	err |= comedi_check_trigger_src(&cmd->stop_src, TRIG_COUNT | TRIG_NONE);
 
 	if (err)
 		return 1;
 
 	/* Step 2a : make sure trigger sources are unique */
 
-	err |= cfc_check_trigger_is_unique(cmd->stop_src);
+	err |= comedi_check_trigger_is_unique(cmd->stop_src);
 
 	/* Step 2b : and mutually compatible */
 
@@ -406,18 +404,19 @@ static int gsc_hpdi_cmd_test(struct comedi_device *dev,
 
 	/* Step 3: check if arguments are trivially valid */
 
-	err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
+	err |= comedi_check_trigger_arg_is(&cmd->start_arg, 0);
 
 	if (!cmd->chanlist_len || !cmd->chanlist) {
 		cmd->chanlist_len = 32;
 		err |= -EINVAL;
 	}
-	err |= cfc_check_trigger_arg_is(&cmd->scan_end_arg, cmd->chanlist_len);
+	err |= comedi_check_trigger_arg_is(&cmd->scan_end_arg,
+					   cmd->chanlist_len);
 
 	if (cmd->stop_src == TRIG_COUNT)
-		err |= cfc_check_trigger_arg_min(&cmd->stop_arg, 1);
+		err |= comedi_check_trigger_arg_min(&cmd->stop_arg, 1);
 	else	/* TRIG_NONE */
-		err |= cfc_check_trigger_arg_is(&cmd->stop_arg, 0);
+		err |= comedi_check_trigger_arg_is(&cmd->stop_arg, 0);
 
 	if (err)
 		return 3;
@@ -433,7 +432,6 @@ static int gsc_hpdi_cmd_test(struct comedi_device *dev,
 		return 5;
 
 	return 0;
-
 }
 
 /* setup dma descriptors so a link completes every 'len' bytes */
diff --git a/drivers/staging/comedi/drivers/icp_multi.c b/drivers/staging/comedi/drivers/icp_multi.c
index 1ea168620103..1e104ebf8057 100644
--- a/drivers/staging/comedi/drivers/icp_multi.c
+++ b/drivers/staging/comedi/drivers/icp_multi.c
@@ -43,11 +43,10 @@ Configuration options: not applicable, uses PCI auto config
 */
 
 #include <linux/module.h>
-#include <linux/pci.h>
 #include <linux/delay.h>
 #include <linux/interrupt.h>
 
-#include "../comedidev.h"
+#include "../comedi_pci.h"
 
 #define ICP_MULTI_ADC_CSR	0	/* R/W: ADC command/status register */
 #define ICP_MULTI_AI		2	/* R:   Analogue input data */
@@ -370,7 +369,6 @@ static irqreturn_t interrupt_service_icp_multi(int irq, void *d)
 		break;
 	default:
 		break;
-
 	}
 
 	return IRQ_HANDLED;
@@ -445,7 +443,7 @@ static int icp_multi_reset(struct comedi_device *dev)
 }
 
 static int icp_multi_auto_attach(struct comedi_device *dev,
-					   unsigned long context_unused)
+				 unsigned long context_unused)
 {
 	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
 	struct icp_multi_private *devpriv;
diff --git a/drivers/staging/comedi/drivers/jr3_pci.c b/drivers/staging/comedi/drivers/jr3_pci.c
index 81fab2dfafa4..b87192e0f9aa 100644
--- a/drivers/staging/comedi/drivers/jr3_pci.c
+++ b/drivers/staging/comedi/drivers/jr3_pci.c
@@ -39,14 +39,13 @@
 
 #include <linux/kernel.h>
 #include <linux/module.h>
-#include <linux/pci.h>
 #include <linux/delay.h>
 #include <linux/ctype.h>
 #include <linux/jiffies.h>
 #include <linux/slab.h>
 #include <linux/timer.h>
 
-#include "../comedidev.h"
+#include "../comedi_pci.h"
 
 #include "jr3_pci.h"
 
@@ -706,8 +705,6 @@ static int jr3_pci_auto_attach(struct comedi_device *dev,
 	if (!devpriv)
 		return -ENOMEM;
 
-	init_timer(&devpriv->timer);
-
 	ret = comedi_pci_enable(dev);
 	if (ret)
 		return ret;
@@ -775,8 +772,7 @@ static int jr3_pci_auto_attach(struct comedi_device *dev,
 		spriv->next_time_max = jiffies + msecs_to_jiffies(2000);
 	}
 
-	devpriv->timer.data = (unsigned long)dev;
-	devpriv->timer.function = jr3_pci_poll_dev;
+	setup_timer(&devpriv->timer, jr3_pci_poll_dev, (unsigned long)dev);
 	devpriv->timer.expires = jiffies + msecs_to_jiffies(1000);
 	add_timer(&devpriv->timer);
 
diff --git a/drivers/staging/comedi/drivers/ke_counter.c b/drivers/staging/comedi/drivers/ke_counter.c
index 3c19e0f178ca..dc642edf4f65 100644
--- a/drivers/staging/comedi/drivers/ke_counter.c
+++ b/drivers/staging/comedi/drivers/ke_counter.c
@@ -28,9 +28,8 @@
  */
 
 #include <linux/module.h>
-#include <linux/pci.h>
 
-#include "../comedidev.h"
+#include "../comedi_pci.h"
 
 /*
  * PCI BAR 0 Register I/O map
diff --git a/drivers/staging/comedi/drivers/me4000.c b/drivers/staging/comedi/drivers/me4000.c
index d120aa244cf9..7a8c27dd7154 100644
--- a/drivers/staging/comedi/drivers/me4000.c
+++ b/drivers/staging/comedi/drivers/me4000.c
@@ -41,14 +41,12 @@ broken.
  */
 
 #include <linux/module.h>
-#include <linux/pci.h>
 #include <linux/delay.h>
 #include <linux/interrupt.h>
 
-#include "../comedidev.h"
+#include "../comedi_pci.h"
 
-#include "comedi_fc.h"
-#include "8253.h"
+#include "comedi_8254.h"
 #include "plx9052.h"
 
 #define ME4000_FIRMWARE		"me4000_firmware.bin"
@@ -170,7 +168,6 @@ broken.
 
 struct me4000_info {
 	unsigned long plx_regbase;
-	unsigned long timer_regbase;
 };
 
 enum me4000_boardid {
@@ -424,7 +421,7 @@ static void me4000_reset(struct comedi_device *dev)
 
 	/* Set both stop bits in the analog input control register */
 	outl(ME4000_AI_CTRL_BIT_IMMEDIATE_STOP | ME4000_AI_CTRL_BIT_STOP,
-		dev->iobase + ME4000_AI_CTRL_REG);
+	     dev->iobase + ME4000_AI_CTRL_REG);
 
 	/* Set both stop bits in the analog output control register */
 	val = ME4000_AO_CTRL_BIT_IMMEDIATE_STOP | ME4000_AO_CTRL_BIT_STOP;
@@ -438,7 +435,7 @@ static void me4000_reset(struct comedi_device *dev)
 
 	/* Set the adustment register for AO demux */
 	outl(ME4000_AO_DEMUX_ADJUST_VALUE,
-		    dev->iobase + ME4000_AO_DEMUX_ADJUST_REG);
+	     dev->iobase + ME4000_AO_DEMUX_ADJUST_REG);
 
 	/*
 	 * Set digital I/O direction for port 0
@@ -609,7 +606,7 @@ static int me4000_ai_check_chanlist(struct comedi_device *dev,
 
 			if (!comedi_range_is_bipolar(s, range)) {
 				dev_dbg(dev->class_dev,
-				       "Bipolar is not selected in differential mode\n");
+					"Bipolar is not selected in differential mode\n");
 				return -EINVAL;
 			}
 		}
@@ -624,7 +621,6 @@ static int ai_round_cmd_args(struct comedi_device *dev,
 			     unsigned int *init_ticks,
 			     unsigned int *scan_ticks, unsigned int *chan_ticks)
 {
-
 	int rest;
 
 	*init_ticks = 0;
@@ -731,7 +727,6 @@ static int ai_prepare(struct comedi_device *dev,
 		      unsigned int init_ticks,
 		      unsigned int scan_ticks, unsigned int chan_ticks)
 {
-
 	unsigned int tmp = 0;
 
 	/* Write timer arguments */
@@ -772,12 +767,12 @@ static int ai_prepare(struct comedi_device *dev,
 	/* Stop triggers */
 	if (cmd->stop_src == TRIG_COUNT) {
 		outl(cmd->chanlist_len * cmd->stop_arg,
-			    dev->iobase + ME4000_AI_SAMPLE_COUNTER_REG);
+		     dev->iobase + ME4000_AI_SAMPLE_COUNTER_REG);
 		tmp |= ME4000_AI_CTRL_BIT_HF_IRQ | ME4000_AI_CTRL_BIT_SC_IRQ;
 	} else if (cmd->stop_src == TRIG_NONE &&
 		   cmd->scan_end_src == TRIG_COUNT) {
 		outl(cmd->scan_end_arg,
-			    dev->iobase + ME4000_AI_SAMPLE_COUNTER_REG);
+		     dev->iobase + ME4000_AI_SAMPLE_COUNTER_REG);
 		tmp |= ME4000_AI_CTRL_BIT_HF_IRQ | ME4000_AI_CTRL_BIT_SC_IRQ;
 	} else {
 		tmp |= ME4000_AI_CTRL_BIT_HF_IRQ;
@@ -827,7 +822,6 @@ static int me4000_ai_do_cmd_test(struct comedi_device *dev,
 				 struct comedi_subdevice *s,
 				 struct comedi_cmd *cmd)
 {
-
 	unsigned int init_ticks;
 	unsigned int chan_ticks;
 	unsigned int scan_ticks;
@@ -838,24 +832,25 @@ static int me4000_ai_do_cmd_test(struct comedi_device *dev,
 
 	/* Step 1 : check if triggers are trivially valid */
 
-	err |= cfc_check_trigger_src(&cmd->start_src, TRIG_NOW | TRIG_EXT);
-	err |= cfc_check_trigger_src(&cmd->scan_begin_src,
+	err |= comedi_check_trigger_src(&cmd->start_src, TRIG_NOW | TRIG_EXT);
+	err |= comedi_check_trigger_src(&cmd->scan_begin_src,
 					TRIG_FOLLOW | TRIG_TIMER | TRIG_EXT);
-	err |= cfc_check_trigger_src(&cmd->convert_src, TRIG_TIMER | TRIG_EXT);
-	err |= cfc_check_trigger_src(&cmd->scan_end_src,
+	err |= comedi_check_trigger_src(&cmd->convert_src,
+					TRIG_TIMER | TRIG_EXT);
+	err |= comedi_check_trigger_src(&cmd->scan_end_src,
 					TRIG_NONE | TRIG_COUNT);
-	err |= cfc_check_trigger_src(&cmd->stop_src, TRIG_NONE | TRIG_COUNT);
+	err |= comedi_check_trigger_src(&cmd->stop_src, TRIG_NONE | TRIG_COUNT);
 
 	if (err)
 		return 1;
 
 	/* Step 2a : make sure trigger sources are unique */
 
-	err |= cfc_check_trigger_is_unique(cmd->start_src);
-	err |= cfc_check_trigger_is_unique(cmd->scan_begin_src);
-	err |= cfc_check_trigger_is_unique(cmd->convert_src);
-	err |= cfc_check_trigger_is_unique(cmd->scan_end_src);
-	err |= cfc_check_trigger_is_unique(cmd->stop_src);
+	err |= comedi_check_trigger_is_unique(cmd->start_src);
+	err |= comedi_check_trigger_is_unique(cmd->scan_begin_src);
+	err |= comedi_check_trigger_is_unique(cmd->convert_src);
+	err |= comedi_check_trigger_is_unique(cmd->scan_end_src);
+	err |= comedi_check_trigger_is_unique(cmd->stop_src);
 
 	/* Step 2b : and mutually compatible */
 
@@ -886,7 +881,7 @@ static int me4000_ai_do_cmd_test(struct comedi_device *dev,
 
 	/* Step 3: check if arguments are trivially valid */
 
-	err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
+	err |= comedi_check_trigger_arg_is(&cmd->start_arg, 0);
 
 	if (cmd->chanlist_len < 1) {
 		cmd->chanlist_len = 1;
@@ -906,9 +901,9 @@ static int me4000_ai_do_cmd_test(struct comedi_device *dev,
 	}
 
 	if (cmd->stop_src == TRIG_COUNT)
-		err |= cfc_check_trigger_arg_min(&cmd->stop_arg, 1);
+		err |= comedi_check_trigger_arg_min(&cmd->stop_arg, 1);
 	else	/* TRIG_NONE */
-		err |= cfc_check_trigger_arg_is(&cmd->stop_arg, 0);
+		err |= comedi_check_trigger_arg_is(&cmd->stop_arg, 0);
 
 	if (err)
 		return 3;
@@ -919,7 +914,6 @@ static int me4000_ai_do_cmd_test(struct comedi_device *dev,
 	if (cmd->start_src == TRIG_NOW &&
 	    cmd->scan_begin_src == TRIG_TIMER &&
 	    cmd->convert_src == TRIG_TIMER) {
-
 		/* Check timer arguments */
 		if (init_ticks < ME4000_AI_MIN_TICKS) {
 			dev_err(dev->class_dev, "Invalid start arg\n");
@@ -941,7 +935,6 @@ static int me4000_ai_do_cmd_test(struct comedi_device *dev,
 	} else if (cmd->start_src == TRIG_NOW &&
 		   cmd->scan_begin_src == TRIG_FOLLOW &&
 		   cmd->convert_src == TRIG_TIMER) {
-
 		/* Check timer arguments */
 		if (init_ticks < ME4000_AI_MIN_TICKS) {
 			dev_err(dev->class_dev, "Invalid start arg\n");
@@ -956,7 +949,6 @@ static int me4000_ai_do_cmd_test(struct comedi_device *dev,
 	} else if (cmd->start_src == TRIG_EXT &&
 		   cmd->scan_begin_src == TRIG_TIMER &&
 		   cmd->convert_src == TRIG_TIMER) {
-
 		/* Check timer arguments */
 		if (init_ticks < ME4000_AI_MIN_TICKS) {
 			dev_err(dev->class_dev, "Invalid start arg\n");
@@ -978,7 +970,6 @@ static int me4000_ai_do_cmd_test(struct comedi_device *dev,
 	} else if (cmd->start_src == TRIG_EXT &&
 		   cmd->scan_begin_src == TRIG_FOLLOW &&
 		   cmd->convert_src == TRIG_TIMER) {
-
 		/* Check timer arguments */
 		if (init_ticks < ME4000_AI_MIN_TICKS) {
 			dev_err(dev->class_dev, "Invalid start arg\n");
@@ -993,7 +984,6 @@ static int me4000_ai_do_cmd_test(struct comedi_device *dev,
 	} else if (cmd->start_src == TRIG_EXT &&
 		   cmd->scan_begin_src == TRIG_EXT &&
 		   cmd->convert_src == TRIG_TIMER) {
-
 		/* Check timer arguments */
 		if (init_ticks < ME4000_AI_MIN_TICKS) {
 			dev_err(dev->class_dev, "Invalid start arg\n");
@@ -1008,7 +998,6 @@ static int me4000_ai_do_cmd_test(struct comedi_device *dev,
 	} else if (cmd->start_src == TRIG_EXT &&
 		   cmd->scan_begin_src == TRIG_EXT &&
 		   cmd->convert_src == TRIG_EXT) {
-
 		/* Check timer arguments */
 		if (init_ticks < ME4000_AI_MIN_TICKS) {
 			dev_err(dev->class_dev, "Invalid start arg\n");
@@ -1187,13 +1176,13 @@ static int me4000_dio_insn_bits(struct comedi_device *dev,
 {
 	if (comedi_dio_update_state(s, data)) {
 		outl((s->state >> 0) & 0xFF,
-			    dev->iobase + ME4000_DIO_PORT_0_REG);
+		     dev->iobase + ME4000_DIO_PORT_0_REG);
 		outl((s->state >> 8) & 0xFF,
-			    dev->iobase + ME4000_DIO_PORT_1_REG);
+		     dev->iobase + ME4000_DIO_PORT_1_REG);
 		outl((s->state >> 16) & 0xFF,
-			    dev->iobase + ME4000_DIO_PORT_2_REG);
+		     dev->iobase + ME4000_DIO_PORT_2_REG);
 		outl((s->state >> 24) & 0xFF,
-			    dev->iobase + ME4000_DIO_PORT_3_REG);
+		     dev->iobase + ME4000_DIO_PORT_3_REG);
 	}
 
 	data[1] = ((inl(dev->iobase + ME4000_DIO_PORT_0_REG) & 0xFF) << 0) |
@@ -1259,85 +1248,6 @@ static int me4000_dio_insn_config(struct comedi_device *dev,
 	return insn->n;
 }
 
-/*=============================================================================
-  Counter section
-  ===========================================================================*/
-
-static int me4000_cnt_insn_config(struct comedi_device *dev,
-				  struct comedi_subdevice *s,
-				  struct comedi_insn *insn,
-				  unsigned int *data)
-{
-	struct me4000_info *info = dev->private;
-	unsigned int chan = CR_CHAN(insn->chanspec);
-	int err;
-
-	switch (data[0]) {
-	case GPCT_RESET:
-		if (insn->n != 1)
-			return -EINVAL;
-
-		err = i8254_set_mode(info->timer_regbase, 0, chan,
-				     I8254_MODE0 | I8254_BINARY);
-		if (err)
-			return err;
-		i8254_write(info->timer_regbase, 0, chan, 0);
-		break;
-	case GPCT_SET_OPERATION:
-		if (insn->n != 2)
-			return -EINVAL;
-
-		err = i8254_set_mode(info->timer_regbase, 0, chan,
-				(data[1] << 1) | I8254_BINARY);
-		if (err)
-			return err;
-		break;
-	default:
-		return -EINVAL;
-	}
-
-	return insn->n;
-}
-
-static int me4000_cnt_insn_read(struct comedi_device *dev,
-				struct comedi_subdevice *s,
-				struct comedi_insn *insn, unsigned int *data)
-{
-	struct me4000_info *info = dev->private;
-
-	if (insn->n == 0)
-		return 0;
-
-	if (insn->n > 1) {
-		dev_err(dev->class_dev, "Invalid instruction length %d\n",
-			insn->n);
-		return -EINVAL;
-	}
-
-	data[0] = i8254_read(info->timer_regbase, 0, insn->chanspec);
-
-	return 1;
-}
-
-static int me4000_cnt_insn_write(struct comedi_device *dev,
-				 struct comedi_subdevice *s,
-				 struct comedi_insn *insn, unsigned int *data)
-{
-	struct me4000_info *info = dev->private;
-
-	if (insn->n == 0) {
-		return 0;
-	} else if (insn->n > 1) {
-		dev_err(dev->class_dev, "Invalid instruction length %d\n",
-			insn->n);
-		return -EINVAL;
-	}
-
-	i8254_write(info->timer_regbase, 0, insn->chanspec, data[0]);
-
-	return 1;
-}
-
 static int me4000_auto_attach(struct comedi_device *dev,
 			      unsigned long context)
 {
@@ -1364,8 +1274,7 @@ static int me4000_auto_attach(struct comedi_device *dev,
 
 	info->plx_regbase = pci_resource_start(pcidev, 1);
 	dev->iobase = pci_resource_start(pcidev, 2);
-	info->timer_regbase = pci_resource_start(pcidev, 3);
-	if (!info->plx_regbase || !dev->iobase || !info->timer_regbase)
+	if (!info->plx_regbase || !dev->iobase)
 		return -ENODEV;
 
 	result = comedi_load_firmware(dev, &pcidev->dev, ME4000_FIRMWARE,
@@ -1377,7 +1286,7 @@ static int me4000_auto_attach(struct comedi_device *dev,
 
 	if (pcidev->irq > 0) {
 		result = request_irq(pcidev->irq, me4000_ai_isr, IRQF_SHARED,
-				  dev->board_name, dev);
+				     dev->board_name, dev);
 		if (result == 0)
 			dev->irq = pcidev->irq;
 	}
@@ -1459,23 +1368,22 @@ static int me4000_auto_attach(struct comedi_device *dev,
 	if (!inl(dev->iobase + ME4000_DIO_DIR_REG)) {
 		s->io_bits |= 0xFF;
 		outl(ME4000_DIO_CTRL_BIT_MODE_0,
-			dev->iobase + ME4000_DIO_DIR_REG);
+		     dev->iobase + ME4000_DIO_DIR_REG);
 	}
 
-    /*=========================================================================
-      Counter subdevice
-      ========================================================================*/
-
+	/* Counter subdevice (8254) */
 	s = &dev->subdevices[3];
-
 	if (thisboard->has_counter) {
-		s->type = COMEDI_SUBD_COUNTER;
-		s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
-		s->n_chan = 3;
-		s->maxdata = 0xFFFF;	/*  16 bit counters */
-		s->insn_read = me4000_cnt_insn_read;
-		s->insn_write = me4000_cnt_insn_write;
-		s->insn_config = me4000_cnt_insn_config;
+		unsigned long timer_base = pci_resource_start(pcidev, 3);
+
+		if (!timer_base)
+			return -ENODEV;
+
+		dev->pacer = comedi_8254_init(timer_base, 0, I8254_IO8, 0);
+		if (!dev->pacer)
+			return -ENOMEM;
+
+		comedi_8254_subdevice_init(s, dev->pacer);
 	} else {
 		s->type = COMEDI_SUBD_UNUSED;
 	}
diff --git a/drivers/staging/comedi/drivers/me_daq.c b/drivers/staging/comedi/drivers/me_daq.c
index 92e23527f2cb..d78e9195fbce 100644
--- a/drivers/staging/comedi/drivers/me_daq.c
+++ b/drivers/staging/comedi/drivers/me_daq.c
@@ -30,11 +30,10 @@
  */
 
 #include <linux/module.h>
-#include <linux/pci.h>
 #include <linux/interrupt.h>
 #include <linux/sched.h>
 
-#include "../comedidev.h"
+#include "../comedi_pci.h"
 
 #include "plx9052.h"
 
diff --git a/drivers/staging/comedi/drivers/mf6x4.c b/drivers/staging/comedi/drivers/mf6x4.c
index db972bce2b5b..a675e2ef9b45 100644
--- a/drivers/staging/comedi/drivers/mf6x4.c
+++ b/drivers/staging/comedi/drivers/mf6x4.c
@@ -26,9 +26,9 @@
  */
 
 #include <linux/module.h>
-#include <linux/pci.h>
 #include <linux/delay.h>
-#include "../comedidev.h"
+
+#include "../comedi_pci.h"
 
 /* Registers present in BAR0 memory region */
 #define MF624_GPIOC_R					0x54
@@ -237,7 +237,6 @@ static int mf6x4_auto_attach(struct comedi_device *dev, unsigned long context)
 	else
 		devpriv->gpioc_R = devpriv->bar0_mem + MF624_GPIOC_R;
 
-
 	ret = comedi_alloc_subdevices(dev, 4);
 	if (ret)
 		return ret;
diff --git a/drivers/staging/comedi/drivers/mite.c b/drivers/staging/comedi/drivers/mite.c
index 1e537a5cf862..e43a0c8323c1 100644
--- a/drivers/staging/comedi/drivers/mite.c
+++ b/drivers/staging/comedi/drivers/mite.c
@@ -47,11 +47,9 @@
 #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
 
 #include <linux/module.h>
-#include <linux/pci.h>
 
-#include "../comedidev.h"
+#include "../comedi_pci.h"
 
-#include "comedi_fc.h"
 #include "mite.h"
 
 #define TOP_OF_PAGE(x) ((x)|(~(PAGE_MASK)))
@@ -186,10 +184,10 @@ struct mite_dma_descriptor_ring *mite_alloc_ring(struct mite_struct *mite)
 	struct mite_dma_descriptor_ring *ring =
 	    kmalloc(sizeof(struct mite_dma_descriptor_ring), GFP_KERNEL);
 
-	if (ring == NULL)
-		return ring;
+	if (!ring)
+		return NULL;
 	ring->hw_dev = get_device(&mite->pcidev->dev);
-	if (ring->hw_dev == NULL) {
+	if (!ring->hw_dev) {
 		kfree(ring);
 		return NULL;
 	}
diff --git a/drivers/staging/comedi/drivers/mite.h b/drivers/staging/comedi/drivers/mite.h
index b2b12045b3a5..b3ca7fc3a31e 100644
--- a/drivers/staging/comedi/drivers/mite.h
+++ b/drivers/staging/comedi/drivers/mite.h
@@ -19,10 +19,9 @@
 #ifndef _MITE_H_
 #define _MITE_H_
 
-#include <linux/pci.h>
 #include <linux/log2.h>
 #include <linux/slab.h>
-#include "../comedidev.h"
+#include "../comedi_pci.h"
 
 #define PCIMIO_COMPAT
 
diff --git a/drivers/staging/comedi/drivers/mpc624.c b/drivers/staging/comedi/drivers/mpc624.c
index 1241f9987cab..0207b8edfcb4 100644
--- a/drivers/staging/comedi/drivers/mpc624.c
+++ b/drivers/staging/comedi/drivers/mpc624.c
@@ -115,7 +115,6 @@ Configuration Options:
 	(MPC624_OSR4 | MPC624_OSR3 | MPC624_OSR2 | MPC624_OSR1 | MPC624_OSR0)
 /* -------------------------------------------------------------------------- */
 struct mpc624_private {
-
 	/*  set by mpc624_attach() from driver's parameters */
 	unsigned long int ulConvertionRate;
 };
diff --git a/drivers/staging/comedi/drivers/ni_6527.c b/drivers/staging/comedi/drivers/ni_6527.c
index 530f716f6586..62a817e4cd64 100644
--- a/drivers/staging/comedi/drivers/ni_6527.c
+++ b/drivers/staging/comedi/drivers/ni_6527.c
@@ -28,12 +28,9 @@
  */
 
 #include <linux/module.h>
-#include <linux/pci.h>
 #include <linux/interrupt.h>
 
-#include "../comedidev.h"
-
-#include "comedi_fc.h"
+#include "../comedi_pci.h"
 
 /*
  * PCI BAR1 - Register memory map
@@ -224,11 +221,11 @@ static int ni6527_intr_cmdtest(struct comedi_device *dev,
 
 	/* Step 1 : check if triggers are trivially valid */
 
-	err |= cfc_check_trigger_src(&cmd->start_src, TRIG_NOW);
-	err |= cfc_check_trigger_src(&cmd->scan_begin_src, TRIG_OTHER);
-	err |= cfc_check_trigger_src(&cmd->convert_src, TRIG_FOLLOW);
-	err |= cfc_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
-	err |= cfc_check_trigger_src(&cmd->stop_src, TRIG_COUNT);
+	err |= comedi_check_trigger_src(&cmd->start_src, TRIG_NOW);
+	err |= comedi_check_trigger_src(&cmd->scan_begin_src, TRIG_OTHER);
+	err |= comedi_check_trigger_src(&cmd->convert_src, TRIG_FOLLOW);
+	err |= comedi_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
+	err |= comedi_check_trigger_src(&cmd->stop_src, TRIG_COUNT);
 
 	if (err)
 		return 1;
@@ -238,11 +235,12 @@ static int ni6527_intr_cmdtest(struct comedi_device *dev,
 
 	/* Step 3: check if arguments are trivially valid */
 
-	err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
-	err |= cfc_check_trigger_arg_is(&cmd->scan_begin_arg, 0);
-	err |= cfc_check_trigger_arg_is(&cmd->convert_arg, 0);
-	err |= cfc_check_trigger_arg_is(&cmd->scan_end_arg, cmd->chanlist_len);
-	err |= cfc_check_trigger_arg_is(&cmd->stop_arg, 0);
+	err |= comedi_check_trigger_arg_is(&cmd->start_arg, 0);
+	err |= comedi_check_trigger_arg_is(&cmd->scan_begin_arg, 0);
+	err |= comedi_check_trigger_arg_is(&cmd->convert_arg, 0);
+	err |= comedi_check_trigger_arg_is(&cmd->scan_end_arg,
+					   cmd->chanlist_len);
+	err |= comedi_check_trigger_arg_is(&cmd->stop_arg, 0);
 
 	if (err)
 		return 3;
diff --git a/drivers/staging/comedi/drivers/ni_65xx.c b/drivers/staging/comedi/drivers/ni_65xx.c
index 67cb758eb0cd..800d57426070 100644
--- a/drivers/staging/comedi/drivers/ni_65xx.c
+++ b/drivers/staging/comedi/drivers/ni_65xx.c
@@ -57,12 +57,9 @@
  */
 
 #include <linux/module.h>
-#include <linux/pci.h>
 #include <linux/interrupt.h>
 
-#include "../comedidev.h"
-
-#include "comedi_fc.h"
+#include "../comedi_pci.h"
 
 /*
  * PCI BAR1 Register Map
@@ -508,11 +505,11 @@ static int ni_65xx_intr_cmdtest(struct comedi_device *dev,
 
 	/* Step 1 : check if triggers are trivially valid */
 
-	err |= cfc_check_trigger_src(&cmd->start_src, TRIG_NOW);
-	err |= cfc_check_trigger_src(&cmd->scan_begin_src, TRIG_OTHER);
-	err |= cfc_check_trigger_src(&cmd->convert_src, TRIG_FOLLOW);
-	err |= cfc_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
-	err |= cfc_check_trigger_src(&cmd->stop_src, TRIG_COUNT);
+	err |= comedi_check_trigger_src(&cmd->start_src, TRIG_NOW);
+	err |= comedi_check_trigger_src(&cmd->scan_begin_src, TRIG_OTHER);
+	err |= comedi_check_trigger_src(&cmd->convert_src, TRIG_FOLLOW);
+	err |= comedi_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
+	err |= comedi_check_trigger_src(&cmd->stop_src, TRIG_COUNT);
 
 	if (err)
 		return 1;
@@ -522,11 +519,12 @@ static int ni_65xx_intr_cmdtest(struct comedi_device *dev,
 
 	/* Step 3: check if arguments are trivially valid */
 
-	err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
-	err |= cfc_check_trigger_arg_is(&cmd->scan_begin_arg, 0);
-	err |= cfc_check_trigger_arg_is(&cmd->convert_arg, 0);
-	err |= cfc_check_trigger_arg_is(&cmd->scan_end_arg, cmd->chanlist_len);
-	err |= cfc_check_trigger_arg_is(&cmd->stop_arg, 0);
+	err |= comedi_check_trigger_arg_is(&cmd->start_arg, 0);
+	err |= comedi_check_trigger_arg_is(&cmd->scan_begin_arg, 0);
+	err |= comedi_check_trigger_arg_is(&cmd->convert_arg, 0);
+	err |= comedi_check_trigger_arg_is(&cmd->scan_end_arg,
+					   cmd->chanlist_len);
+	err |= comedi_check_trigger_arg_is(&cmd->stop_arg, 0);
 
 	if (err)
 		return 3;
@@ -613,7 +611,7 @@ static int ni_65xx_intr_insn_config(struct comedi_device *dev,
 	return insn->n;
 }
 
-/* ripped from mite.h and mite_setup2() to avoid mite dependancy */
+/* ripped from mite.h and mite_setup2() to avoid mite dependency */
 #define MITE_IODWBSR	0xc0	 /* IO Device Window Base Size Register */
 #define WENAB		(1 << 7) /* window enable */
 
@@ -676,7 +674,7 @@ static int ni_65xx_auto_attach(struct comedi_device *dev,
 	}
 
 	dev_info(dev->class_dev, "board: %s, ID=0x%02x", dev->board_name,
-	       readb(dev->mmio + NI_65XX_ID_REG));
+		 readb(dev->mmio + NI_65XX_ID_REG));
 
 	ret = comedi_alloc_subdevices(dev, 4);
 	if (ret)
diff --git a/drivers/staging/comedi/drivers/ni_660x.c b/drivers/staging/comedi/drivers/ni_660x.c
index 1e4dd82b12ea..46647c64f369 100644
--- a/drivers/staging/comedi/drivers/ni_660x.c
+++ b/drivers/staging/comedi/drivers/ni_660x.c
@@ -35,12 +35,10 @@
  */
 
 #include <linux/module.h>
-#include <linux/pci.h>
 #include <linux/interrupt.h>
 
-#include "../comedidev.h"
+#include "../comedi_pci.h"
 
-#include "comedi_fc.h"
 #include "mite.h"
 #include "ni_tio.h"
 
@@ -195,7 +193,6 @@ static inline unsigned NI_660X_GPCT_SUBDEV(unsigned index)
 }
 
 struct NI_660xRegisterData {
-
 	const char *name;	/*  Register Name */
 	int offset;		/*  Offset from base address from GPCT chip */
 	enum ni_660x_register_direction direction;
@@ -702,7 +699,7 @@ static int ni_660x_request_mite_channel(struct comedi_device *dev,
 	BUG_ON(counter->mite_chan);
 	mite_chan = mite_request_channel(devpriv->mite,
 					 mite_ring(devpriv, counter));
-	if (mite_chan == NULL) {
+	if (!mite_chan) {
 		spin_unlock_irqrestore(&devpriv->mite_channel_lock, flags);
 		dev_err(dev->class_dev,
 			"failed to reserve mite dma channel for counter\n");
@@ -861,7 +858,7 @@ static int ni_660x_alloc_mite_rings(struct comedi_device *dev)
 		for (j = 0; j < counters_per_chip; ++j) {
 			devpriv->mite_rings[i][j] =
 			    mite_alloc_ring(devpriv->mite);
-			if (devpriv->mite_rings[i][j] == NULL)
+			if (!devpriv->mite_rings[i][j])
 				return -ENOMEM;
 		}
 	}
@@ -1107,7 +1104,7 @@ static int ni_660x_auto_attach(struct comedi_device *dev,
 						     ni_gpct_variant_660x,
 						     ni_660x_num_counters
 						     (dev));
-	if (devpriv->counter_dev == NULL)
+	if (!devpriv->counter_dev)
 		return -ENOMEM;
 	for (i = 0; i < NI_660X_MAX_NUM_COUNTERS; ++i) {
 		s = &dev->subdevices[NI_660X_GPCT_SUBDEV(i)];
diff --git a/drivers/staging/comedi/drivers/ni_670x.c b/drivers/staging/comedi/drivers/ni_670x.c
index c42a81c0bfa1..13c6ccb1fdea 100644
--- a/drivers/staging/comedi/drivers/ni_670x.c
+++ b/drivers/staging/comedi/drivers/ni_670x.c
@@ -37,11 +37,10 @@ Commands are not supported.
 */
 
 #include <linux/module.h>
-#include <linux/pci.h>
 #include <linux/interrupt.h>
 #include <linux/slab.h>
 
-#include "../comedidev.h"
+#include "../comedi_pci.h"
 
 #define AO_VALUE_OFFSET			0x00
 #define	AO_CHAN_OFFSET			0x0c
@@ -146,7 +145,7 @@ static int ni_670x_dio_insn_config(struct comedi_device *dev,
 	return insn->n;
 }
 
-/* ripped from mite.h and mite_setup2() to avoid mite dependancy */
+/* ripped from mite.h and mite_setup2() to avoid mite dependency */
 #define MITE_IODWBSR	0xc0	 /* IO Device Window Base Size Register */
 #define WENAB		(1 << 7) /* window enable */
 
diff --git a/drivers/staging/comedi/drivers/ni_at_a2150.c b/drivers/staging/comedi/drivers/ni_at_a2150.c
index a1ce0b0b8c41..3a972d1538ab 100644
--- a/drivers/staging/comedi/drivers/ni_at_a2150.c
+++ b/drivers/staging/comedi/drivers/ni_at_a2150.c
@@ -67,8 +67,7 @@ TRIG_WAKE_EOS
 #include "../comedidev.h"
 
 #include "comedi_isadma.h"
-#include "comedi_fc.h"
-#include "8253.h"
+#include "comedi_8254.h"
 
 #define A2150_DMA_BUFFER_SIZE	0xff00	/*  size in bytes of dma buffer */
 
@@ -110,8 +109,6 @@ TRIG_WAKE_EOS
 #define   DMA_INTR_EN_BIT 		0x800	/*  enable interrupt on dma terminal count */
 #define   DMA_DEM_EN_BIT	0x1000	/*  enables demand mode dma */
 #define I8253_BASE_REG		0x14
-#define I8253_MODE_REG		0x17
-#define   HW_COUNT_DISABLE		0x30	/*  disable hardware counting of conversions */
 
 struct a2150_board {
 	const char *name;
@@ -422,19 +419,19 @@ static int a2150_ai_cmdtest(struct comedi_device *dev,
 
 	/* Step 1 : check if triggers are trivially valid */
 
-	err |= cfc_check_trigger_src(&cmd->start_src, TRIG_NOW | TRIG_EXT);
-	err |= cfc_check_trigger_src(&cmd->scan_begin_src, TRIG_TIMER);
-	err |= cfc_check_trigger_src(&cmd->convert_src, TRIG_NOW);
-	err |= cfc_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
-	err |= cfc_check_trigger_src(&cmd->stop_src, TRIG_COUNT | TRIG_NONE);
+	err |= comedi_check_trigger_src(&cmd->start_src, TRIG_NOW | TRIG_EXT);
+	err |= comedi_check_trigger_src(&cmd->scan_begin_src, TRIG_TIMER);
+	err |= comedi_check_trigger_src(&cmd->convert_src, TRIG_NOW);
+	err |= comedi_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
+	err |= comedi_check_trigger_src(&cmd->stop_src, TRIG_COUNT | TRIG_NONE);
 
 	if (err)
 		return 1;
 
 	/* Step 2a : make sure trigger sources are unique */
 
-	err |= cfc_check_trigger_is_unique(cmd->start_src);
-	err |= cfc_check_trigger_is_unique(cmd->stop_src);
+	err |= comedi_check_trigger_is_unique(cmd->start_src);
+	err |= comedi_check_trigger_is_unique(cmd->stop_src);
 
 	/* Step 2b : and mutually compatible */
 
@@ -443,19 +440,21 @@ static int a2150_ai_cmdtest(struct comedi_device *dev,
 
 	/* Step 3: check if arguments are trivially valid */
 
-	err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
+	err |= comedi_check_trigger_arg_is(&cmd->start_arg, 0);
 
-	if (cmd->convert_src == TRIG_TIMER)
-		err |= cfc_check_trigger_arg_min(&cmd->convert_arg,
-						 thisboard->ai_speed);
+	if (cmd->convert_src == TRIG_TIMER) {
+		err |= comedi_check_trigger_arg_min(&cmd->convert_arg,
+						    thisboard->ai_speed);
+	}
 
-	err |= cfc_check_trigger_arg_min(&cmd->chanlist_len, 1);
-	err |= cfc_check_trigger_arg_is(&cmd->scan_end_arg, cmd->chanlist_len);
+	err |= comedi_check_trigger_arg_min(&cmd->chanlist_len, 1);
+	err |= comedi_check_trigger_arg_is(&cmd->scan_end_arg,
+					   cmd->chanlist_len);
 
 	if (cmd->stop_src == TRIG_COUNT)
-		err |= cfc_check_trigger_arg_min(&cmd->stop_arg, 1);
+		err |= comedi_check_trigger_arg_min(&cmd->stop_arg, 1);
 	else	/* TRIG_NONE */
-		err |= cfc_check_trigger_arg_is(&cmd->stop_arg, 0);
+		err |= comedi_check_trigger_arg_is(&cmd->stop_arg, 0);
 
 	if (err)
 		return 3;
@@ -465,7 +464,7 @@ static int a2150_ai_cmdtest(struct comedi_device *dev,
 	if (cmd->scan_begin_src == TRIG_TIMER) {
 		arg = cmd->scan_begin_arg;
 		a2150_get_timing(dev, &arg, cmd->flags);
-		err |= cfc_check_trigger_arg_is(&cmd->scan_begin_arg, arg);
+		err |= comedi_check_trigger_arg_is(&cmd->scan_begin_arg, arg);
 	}
 
 	if (err)
@@ -488,7 +487,6 @@ static int a2150_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	struct comedi_isadma_desc *desc = &dma->desc[0];
 	struct comedi_async *async = s->async;
 	struct comedi_cmd *cmd = &async->cmd;
-	unsigned long timer_base = dev->iobase + I8253_BASE_REG;
 	unsigned int old_config_bits = devpriv->config_bits;
 	unsigned int trigger_bits;
 
@@ -547,8 +545,7 @@ static int a2150_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	outw(devpriv->irq_dma_bits, dev->iobase + IRQ_DMA_CNTRL_REG);
 
 	/*  may need to wait 72 sampling periods if timing was changed */
-	i8254_set_mode(timer_base, 0, 2, I8254_MODE0 | I8254_BINARY);
-	i8254_write(timer_base, 0, 2, 72);
+	comedi_8254_load(dev->pacer, 2, 72, I8254_MODE0 | I8254_BINARY);
 
 	/*  setup start triggering */
 	trigger_bits = 0;
@@ -726,6 +723,11 @@ static int a2150_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	/* an IRQ and DMA are required to support async commands */
 	a2150_alloc_irq_and_dma(dev, it);
 
+	dev->pacer = comedi_8254_init(dev->iobase + I8253_BASE_REG,
+				      0, I8254_IO8, 0);
+	if (!dev->pacer)
+		return -ENOMEM;
+
 	ret = comedi_alloc_subdevices(dev, 1);
 	if (ret)
 		return ret;
@@ -747,10 +749,6 @@ static int a2150_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 		s->cancel = a2150_cancel;
 	}
 
-	/* need to do this for software counting of completed conversions, to
-	 * prevent hardware count from stopping acquisition */
-	outw(HW_COUNT_DISABLE, dev->iobase + I8253_MODE_REG);
-
 	/*  set card's irq and dma levels */
 	outw(devpriv->irq_dma_bits, dev->iobase + IRQ_DMA_CNTRL_REG);
 
diff --git a/drivers/staging/comedi/drivers/ni_at_ao.c b/drivers/staging/comedi/drivers/ni_at_ao.c
index 9eeaf3c5a858..f27aa0e82234 100644
--- a/drivers/staging/comedi/drivers/ni_at_ao.c
+++ b/drivers/staging/comedi/drivers/ni_at_ao.c
@@ -37,7 +37,7 @@
 
 #include "../comedidev.h"
 
-#include "8253.h"
+#include "comedi_8254.h"
 
 /*
  * Register map
@@ -274,7 +274,6 @@ static int atao_calib_insn_write(struct comedi_device *dev,
 static void atao_reset(struct comedi_device *dev)
 {
 	struct atao_private *devpriv = dev->private;
-	unsigned long timer_base = dev->iobase + ATAO_82C53_BASE;
 
 	/* This is the reset sequence described in the manual */
 
@@ -282,9 +281,9 @@ static void atao_reset(struct comedi_device *dev)
 	outw(devpriv->cfg1, dev->iobase + ATAO_CFG1_REG);
 
 	/* Put outputs of counter 1 and counter 2 in a high state */
-	i8254_set_mode(timer_base, 0, 0, I8254_MODE4 | I8254_BINARY);
-	i8254_set_mode(timer_base, 0, 1, I8254_MODE4 | I8254_BINARY);
-	i8254_write(timer_base, 0, 0, 0x0003);
+	comedi_8254_set_mode(dev->pacer, 0, I8254_MODE4 | I8254_BINARY);
+	comedi_8254_set_mode(dev->pacer, 1, I8254_MODE4 | I8254_BINARY);
+	comedi_8254_write(dev->pacer, 0, 0x0003);
 
 	outw(ATAO_CFG2_CALLD_NOP, dev->iobase + ATAO_CFG2_REG);
 
@@ -315,6 +314,11 @@ static int atao_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	if (!devpriv)
 		return -ENOMEM;
 
+	dev->pacer = comedi_8254_init(dev->iobase + ATAO_82C53_BASE,
+				      0, I8254_IO8, 0);
+	if (!dev->pacer)
+		return -ENOMEM;
+
 	ret = comedi_alloc_subdevices(dev, 4);
 	if (ret)
 		return ret;
diff --git a/drivers/staging/comedi/drivers/ni_atmio.c b/drivers/staging/comedi/drivers/ni_atmio.c
index 301f154be813..1304b06980a6 100644
--- a/drivers/staging/comedi/drivers/ni_atmio.c
+++ b/drivers/staging/comedi/drivers/ni_atmio.c
@@ -250,7 +250,7 @@ static int ni_isapnp_find_board(struct pnp_dev **dev)
 					  ISAPNP_FUNCTION(ni_boards[i].
 							  isapnp_id), NULL);
 
-		if (isapnp_dev == NULL || isapnp_dev->card == NULL)
+		if (!isapnp_dev || !isapnp_dev->card)
 			continue;
 
 		if (pnp_device_attach(isapnp_dev) < 0)
@@ -282,7 +282,6 @@ static int ni_getboardtype(struct comedi_device *dev)
 	for (i = 0; i < ARRAY_SIZE(ni_boards); i++) {
 		if (ni_boards[i].device_id == device_id)
 			return i;
-
 	}
 	if (device_id == 255)
 		dev_err(dev->class_dev, "can't find board\n");
@@ -355,7 +354,6 @@ static int ni_atmio_attach(struct comedi_device *dev,
 	if (ret < 0)
 		return ret;
 
-
 	return 0;
 }
 
diff --git a/drivers/staging/comedi/drivers/ni_atmio16d.c b/drivers/staging/comedi/drivers/ni_atmio16d.c
index c484c89c94b5..c3eb54622bc3 100644
--- a/drivers/staging/comedi/drivers/ni_atmio16d.c
+++ b/drivers/staging/comedi/drivers/ni_atmio16d.c
@@ -34,7 +34,6 @@ Devices: [National Instruments] AT-MIO-16 (atmio16), AT-MIO-16D (atmio16d)
 #include <linux/interrupt.h>
 #include "../comedidev.h"
 
-#include "comedi_fc.h"
 #include "8255.h"
 
 /* Configuration and Status Registers */
@@ -96,7 +95,6 @@ Devices: [National Instruments] AT-MIO-16 (atmio16), AT-MIO-16D (atmio16d)
 #define CLOCK_100_HZ	0x8F25
 
 struct atmio16_board_t {
-
 	const char *name;
 	int has_8255;
 };
@@ -234,20 +232,20 @@ static int atmio16d_ai_cmdtest(struct comedi_device *dev,
 
 	/* Step 1 : check if triggers are trivially valid */
 
-	err |= cfc_check_trigger_src(&cmd->start_src, TRIG_NOW);
-	err |= cfc_check_trigger_src(&cmd->scan_begin_src,
+	err |= comedi_check_trigger_src(&cmd->start_src, TRIG_NOW);
+	err |= comedi_check_trigger_src(&cmd->scan_begin_src,
 					TRIG_FOLLOW | TRIG_TIMER);
-	err |= cfc_check_trigger_src(&cmd->convert_src, TRIG_TIMER);
-	err |= cfc_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
-	err |= cfc_check_trigger_src(&cmd->stop_src, TRIG_COUNT | TRIG_NONE);
+	err |= comedi_check_trigger_src(&cmd->convert_src, TRIG_TIMER);
+	err |= comedi_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
+	err |= comedi_check_trigger_src(&cmd->stop_src, TRIG_COUNT | TRIG_NONE);
 
 	if (err)
 		return 1;
 
 	/* Step 2a : make sure trigger sources are unique */
 
-	err |= cfc_check_trigger_is_unique(cmd->scan_begin_src);
-	err |= cfc_check_trigger_is_unique(cmd->stop_src);
+	err |= comedi_check_trigger_is_unique(cmd->scan_begin_src);
+	err |= comedi_check_trigger_is_unique(cmd->stop_src);
 
 	/* Step 2b : and mutually compatible */
 
@@ -256,30 +254,31 @@ static int atmio16d_ai_cmdtest(struct comedi_device *dev,
 
 	/* Step 3: check if arguments are trivially valid */
 
-	err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
+	err |= comedi_check_trigger_arg_is(&cmd->start_arg, 0);
 
 	if (cmd->scan_begin_src == TRIG_FOLLOW) {
 		/* internal trigger */
-		err |= cfc_check_trigger_arg_is(&cmd->scan_begin_arg, 0);
+		err |= comedi_check_trigger_arg_is(&cmd->scan_begin_arg, 0);
 	} else {
 #if 0
 		/* external trigger */
 		/* should be level/edge, hi/lo specification here */
-		err |= cfc_check_trigger_arg_is(&cmd->scan_begin_arg, 0);
+		err |= comedi_check_trigger_arg_is(&cmd->scan_begin_arg, 0);
 #endif
 	}
 
-	err |= cfc_check_trigger_arg_min(&cmd->convert_arg, 10000);
+	err |= comedi_check_trigger_arg_min(&cmd->convert_arg, 10000);
 #if 0
-	err |= cfc_check_trigger_arg_max(&cmd->convert_arg, SLOWEST_TIMER);
+	err |= comedi_check_trigger_arg_max(&cmd->convert_arg, SLOWEST_TIMER);
 #endif
 
-	err |= cfc_check_trigger_arg_is(&cmd->scan_end_arg, cmd->chanlist_len);
+	err |= comedi_check_trigger_arg_is(&cmd->scan_end_arg,
+					   cmd->chanlist_len);
 
 	if (cmd->stop_src == TRIG_COUNT)
-		err |= cfc_check_trigger_arg_min(&cmd->stop_arg, 1);
+		err |= comedi_check_trigger_arg_min(&cmd->stop_arg, 1);
 	else	/* TRIG_NONE */
-		err |= cfc_check_trigger_arg_is(&cmd->stop_arg, 0);
+		err |= comedi_check_trigger_arg_is(&cmd->stop_arg, 0);
 
 	if (err)
 		return 3;
diff --git a/drivers/staging/comedi/drivers/ni_labpc.c b/drivers/staging/comedi/drivers/ni_labpc.c
index a916047791b8..51e5e942b442 100644
--- a/drivers/staging/comedi/drivers/ni_labpc.c
+++ b/drivers/staging/comedi/drivers/ni_labpc.c
@@ -105,6 +105,7 @@ static int labpc_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 static void labpc_detach(struct comedi_device *dev)
 {
 	labpc_free_dma_chan(dev);
+	labpc_common_detach(dev);
 	comedi_legacy_detach(dev);
 }
 
diff --git a/drivers/staging/comedi/drivers/ni_labpc.h b/drivers/staging/comedi/drivers/ni_labpc.h
index be89ae479afc..83f878adbd53 100644
--- a/drivers/staging/comedi/drivers/ni_labpc.h
+++ b/drivers/staging/comedi/drivers/ni_labpc.h
@@ -36,6 +36,7 @@ struct labpc_boardinfo {
 
 struct labpc_private {
 	struct comedi_isadma *dma;
+	struct comedi_8254 *counter;
 
 	/*  number of data points left to be taken */
 	unsigned long long count;
@@ -49,20 +50,6 @@ struct labpc_private {
 	/*  store last read of board status registers */
 	unsigned int stat1;
 	unsigned int stat2;
-	/*
-	 * value to load into board's counter a0 (conversion pacing) for timed
-	 * conversions
-	 */
-	unsigned int divisor_a0;
-	/*
-	 * value to load into board's counter b0 (master) for timed conversions
-	 */
-	unsigned int divisor_b0;
-	/*
-	 * value to load into board's counter b1 (scan pacing) for timed
-	 * conversions
-	 */
-	unsigned int divisor_b1;
 
 	/* we are using dma/fifo-half-full/etc. */
 	enum transfer_type current_transfer;
@@ -77,5 +64,6 @@ struct labpc_private {
 
 int labpc_common_attach(struct comedi_device *dev,
 			unsigned int irq, unsigned long isr_flags);
+void labpc_common_detach(struct comedi_device *dev);
 
 #endif /* _NI_LABPC_H */
diff --git a/drivers/staging/comedi/drivers/ni_labpc_common.c b/drivers/staging/comedi/drivers/ni_labpc_common.c
index b88ee2614bfe..863afb28ee28 100644
--- a/drivers/staging/comedi/drivers/ni_labpc_common.c
+++ b/drivers/staging/comedi/drivers/ni_labpc_common.c
@@ -20,12 +20,12 @@
 #include <linux/interrupt.h>
 #include <linux/io.h>
 #include <linux/delay.h>
+#include <linux/slab.h>
 
 #include "../comedidev.h"
 
-#include "8253.h"
+#include "comedi_8254.h"
 #include "8255.h"
-#include "comedi_fc.h"
 #include "ni_labpc.h"
 #include "ni_labpc_regs.h"
 #include "ni_labpc_isadma.h"
@@ -108,32 +108,6 @@ static void labpc_writeb(struct comedi_device *dev,
 	writeb(byte, dev->mmio + reg);
 }
 
-static void labpc_counter_load(struct comedi_device *dev,
-			       unsigned long reg,
-			       unsigned int counter_number,
-			       unsigned int count,
-			       unsigned int mode)
-{
-	if (dev->mmio) {
-		i8254_mm_set_mode(dev->mmio + reg, 0, counter_number, mode);
-		i8254_mm_write(dev->mmio + reg, 0, counter_number, count);
-	} else {
-		i8254_set_mode(dev->iobase + reg, 0, counter_number, mode);
-		i8254_write(dev->iobase + reg, 0, counter_number, count);
-	}
-}
-
-static void labpc_counter_set_mode(struct comedi_device *dev,
-				   unsigned long reg,
-				   unsigned int counter_number,
-				   unsigned int mode)
-{
-	if (dev->mmio)
-		i8254_mm_set_mode(dev->mmio + reg, 0, counter_number, mode);
-	else
-		i8254_set_mode(dev->iobase + reg, 0, counter_number, mode);
-}
-
 static int labpc_cancel(struct comedi_device *dev, struct comedi_subdevice *s)
 {
 	struct labpc_private *devpriv = dev->private;
@@ -284,7 +258,7 @@ static int labpc_ai_insn_read(struct comedi_device *dev,
 	devpriv->write_byte(dev, devpriv->cmd4, CMD4_REG);
 
 	/* initialize pacer counter to prevent any problems */
-	labpc_counter_set_mode(dev, COUNTER_A_BASE_REG, 0, I8254_MODE2);
+	comedi_8254_set_mode(devpriv->counter, 0, I8254_MODE2 | I8254_BINARY);
 
 	labpc_clear_adc_fifo(dev);
 
@@ -367,83 +341,81 @@ static void labpc_set_ai_scan_period(struct comedi_cmd *cmd,
 static void labpc_adc_timing(struct comedi_device *dev, struct comedi_cmd *cmd,
 			     enum scan_mode mode)
 {
-	struct labpc_private *devpriv = dev->private;
+	struct comedi_8254 *pacer = dev->pacer;
+	unsigned int convert_period = labpc_ai_convert_period(cmd, mode);
+	unsigned int scan_period = labpc_ai_scan_period(cmd, mode);
 	unsigned int base_period;
-	unsigned int scan_period;
-	unsigned int convert_period;
 
 	/*
-	 * if both convert and scan triggers are TRIG_TIMER, then they
-	 * both rely on counter b0
+	 * If both convert and scan triggers are TRIG_TIMER, then they
+	 * both rely on counter b0. If only one TRIG_TIMER is used, we
+	 * can use the generic cascaded timing functions.
 	 */
-	convert_period = labpc_ai_convert_period(cmd, mode);
-	scan_period = labpc_ai_scan_period(cmd, mode);
 	if (convert_period && scan_period) {
 		/*
-		 * pick the lowest b0 divisor value we can (for maximum input
+		 * pick the lowest divisor value we can (for maximum input
 		 * clock speed on convert and scan counters)
 		 */
-		devpriv->divisor_b0 = (scan_period - 1) /
-		    (I8254_OSC_BASE_2MHZ * 0x10000) + 1;
+		pacer->next_div1 = (scan_period - 1) /
+				   (pacer->osc_base * I8254_MAX_COUNT) + 1;
 
-		cfc_check_trigger_arg_min(&devpriv->divisor_b0, 2);
-		cfc_check_trigger_arg_max(&devpriv->divisor_b0, 0x10000);
+		comedi_check_trigger_arg_min(&pacer->next_div1, 2);
+		comedi_check_trigger_arg_max(&pacer->next_div1,
+					     I8254_MAX_COUNT);
 
-		base_period = I8254_OSC_BASE_2MHZ * devpriv->divisor_b0;
+		base_period = pacer->osc_base * pacer->next_div1;
 
 		/*  set a0 for conversion frequency and b1 for scan frequency */
 		switch (cmd->flags & CMDF_ROUND_MASK) {
 		default:
 		case CMDF_ROUND_NEAREST:
-			devpriv->divisor_a0 = DIV_ROUND_CLOSEST(convert_period,
-								base_period);
-			devpriv->divisor_b1 = DIV_ROUND_CLOSEST(scan_period,
-								base_period);
+			pacer->next_div = DIV_ROUND_CLOSEST(convert_period,
+							    base_period);
+			pacer->next_div2 = DIV_ROUND_CLOSEST(scan_period,
+							     base_period);
 			break;
 		case CMDF_ROUND_UP:
-			devpriv->divisor_a0 = DIV_ROUND_UP(convert_period,
-							   base_period);
-			devpriv->divisor_b1 = DIV_ROUND_UP(scan_period,
-							   base_period);
+			pacer->next_div = DIV_ROUND_UP(convert_period,
+						       base_period);
+			pacer->next_div2 = DIV_ROUND_UP(scan_period,
+							base_period);
 			break;
 		case CMDF_ROUND_DOWN:
-			devpriv->divisor_a0 = convert_period / base_period;
-			devpriv->divisor_b1 = scan_period / base_period;
+			pacer->next_div = convert_period / base_period;
+			pacer->next_div2 = scan_period / base_period;
 			break;
 		}
 		/*  make sure a0 and b1 values are acceptable */
-		cfc_check_trigger_arg_min(&devpriv->divisor_a0, 2);
-		cfc_check_trigger_arg_max(&devpriv->divisor_a0, 0x10000);
-		cfc_check_trigger_arg_min(&devpriv->divisor_b1, 2);
-		cfc_check_trigger_arg_max(&devpriv->divisor_b1, 0x10000);
+		comedi_check_trigger_arg_min(&pacer->next_div, 2);
+		comedi_check_trigger_arg_max(&pacer->next_div, I8254_MAX_COUNT);
+		comedi_check_trigger_arg_min(&pacer->next_div2, 2);
+		comedi_check_trigger_arg_max(&pacer->next_div2,
+					     I8254_MAX_COUNT);
+
 		/*  write corrected timings to command */
 		labpc_set_ai_convert_period(cmd, mode,
-					    base_period * devpriv->divisor_a0);
+					    base_period * pacer->next_div);
 		labpc_set_ai_scan_period(cmd, mode,
-					 base_period * devpriv->divisor_b1);
-		/*
-		 * if only one TRIG_TIMER is used, we can employ the generic
-		 * cascaded timing functions
-		 */
+					 base_period * pacer->next_div2);
 	} else if (scan_period) {
 		/*
 		 * calculate cascaded counter values
 		 * that give desired scan timing
+		 * (pacer->next_div2 / pacer->next_div1)
 		 */
-		i8253_cascade_ns_to_timer(I8254_OSC_BASE_2MHZ,
-					  &devpriv->divisor_b1,
-					  &devpriv->divisor_b0,
-					  &scan_period, cmd->flags);
+		comedi_8254_cascade_ns_to_timer(pacer, &scan_period,
+						cmd->flags);
 		labpc_set_ai_scan_period(cmd, mode, scan_period);
 	} else if (convert_period) {
 		/*
 		 * calculate cascaded counter values
 		 * that give desired conversion timing
+		 * (pacer->next_div / pacer->next_div1)
 		 */
-		i8253_cascade_ns_to_timer(I8254_OSC_BASE_2MHZ,
-					  &devpriv->divisor_a0,
-					  &devpriv->divisor_b0,
-					  &convert_period, cmd->flags);
+		comedi_8254_cascade_ns_to_timer(pacer, &convert_period,
+						cmd->flags);
+		/* transfer div2 value so correct timer gets updated */
+		pacer->next_div = pacer->next_div2;
 		labpc_set_ai_convert_period(cmd, mode, convert_period);
 	}
 }
@@ -457,7 +429,7 @@ static enum scan_mode labpc_ai_scan_mode(const struct comedi_cmd *cmd)
 		return MODE_SINGLE_CHAN;
 
 	/* chanlist may be NULL during cmdtest */
-	if (cmd->chanlist == NULL)
+	if (!cmd->chanlist)
 		return MODE_MULT_CHAN_UP;
 
 	chan0 = CR_CHAN(cmd->chanlist[0]);
@@ -482,9 +454,6 @@ static int labpc_ai_check_chanlist(struct comedi_device *dev,
 	unsigned int aref0 = CR_AREF(cmd->chanlist[0]);
 	int i;
 
-	if (mode == MODE_SINGLE_CHAN)
-		return 0;
-
 	for (i = 0; i < cmd->chanlist_len; i++) {
 		unsigned int chan = CR_CHAN(cmd->chanlist[i]);
 		unsigned int range = CR_RANGE(cmd->chanlist[i]);
@@ -543,26 +512,27 @@ static int labpc_ai_cmdtest(struct comedi_device *dev,
 
 	/* Step 1 : check if triggers are trivially valid */
 
-	err |= cfc_check_trigger_src(&cmd->start_src, TRIG_NOW | TRIG_EXT);
-	err |= cfc_check_trigger_src(&cmd->scan_begin_src,
+	err |= comedi_check_trigger_src(&cmd->start_src, TRIG_NOW | TRIG_EXT);
+	err |= comedi_check_trigger_src(&cmd->scan_begin_src,
 					TRIG_TIMER | TRIG_FOLLOW | TRIG_EXT);
-	err |= cfc_check_trigger_src(&cmd->convert_src, TRIG_TIMER | TRIG_EXT);
-	err |= cfc_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
+	err |= comedi_check_trigger_src(&cmd->convert_src,
+					TRIG_TIMER | TRIG_EXT);
+	err |= comedi_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
 
 	stop_mask = TRIG_COUNT | TRIG_NONE;
 	if (board->is_labpc1200)
 		stop_mask |= TRIG_EXT;
-	err |= cfc_check_trigger_src(&cmd->stop_src, stop_mask);
+	err |= comedi_check_trigger_src(&cmd->stop_src, stop_mask);
 
 	if (err)
 		return 1;
 
 	/* Step 2a : make sure trigger sources are unique */
 
-	err |= cfc_check_trigger_is_unique(cmd->start_src);
-	err |= cfc_check_trigger_is_unique(cmd->scan_begin_src);
-	err |= cfc_check_trigger_is_unique(cmd->convert_src);
-	err |= cfc_check_trigger_is_unique(cmd->stop_src);
+	err |= comedi_check_trigger_is_unique(cmd->start_src);
+	err |= comedi_check_trigger_is_unique(cmd->scan_begin_src);
+	err |= comedi_check_trigger_is_unique(cmd->convert_src);
+	err |= comedi_check_trigger_is_unique(cmd->stop_src);
 
 	/* Step 2b : and mutually compatible */
 
@@ -577,7 +547,7 @@ static int labpc_ai_cmdtest(struct comedi_device *dev,
 
 	switch (cmd->start_src) {
 	case TRIG_NOW:
-		err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
+		err |= comedi_check_trigger_arg_is(&cmd->start_arg, 0);
 		break;
 	case TRIG_EXT:
 		/* start_arg value is ignored */
@@ -586,27 +556,33 @@ static int labpc_ai_cmdtest(struct comedi_device *dev,
 
 	if (!cmd->chanlist_len)
 		err |= -EINVAL;
-	err |= cfc_check_trigger_arg_is(&cmd->scan_end_arg, cmd->chanlist_len);
+	err |= comedi_check_trigger_arg_is(&cmd->scan_end_arg,
+					   cmd->chanlist_len);
 
-	if (cmd->convert_src == TRIG_TIMER)
-		err |= cfc_check_trigger_arg_min(&cmd->convert_arg,
-						 board->ai_speed);
+	if (cmd->convert_src == TRIG_TIMER) {
+		err |= comedi_check_trigger_arg_min(&cmd->convert_arg,
+						    board->ai_speed);
+	}
 
 	/* make sure scan timing is not too fast */
 	if (cmd->scan_begin_src == TRIG_TIMER) {
-		if (cmd->convert_src == TRIG_TIMER)
-			err |= cfc_check_trigger_arg_min(&cmd->scan_begin_arg,
-					cmd->convert_arg * cmd->chanlist_len);
-		err |= cfc_check_trigger_arg_min(&cmd->scan_begin_arg,
-				board->ai_speed * cmd->chanlist_len);
+		if (cmd->convert_src == TRIG_TIMER) {
+			err |= comedi_check_trigger_arg_min(&cmd->
+							    scan_begin_arg,
+							    cmd->convert_arg *
+							    cmd->chanlist_len);
+		}
+		err |= comedi_check_trigger_arg_min(&cmd->scan_begin_arg,
+						    board->ai_speed *
+						    cmd->chanlist_len);
 	}
 
 	switch (cmd->stop_src) {
 	case TRIG_COUNT:
-		err |= cfc_check_trigger_arg_min(&cmd->stop_arg, 1);
+		err |= comedi_check_trigger_arg_min(&cmd->stop_arg, 1);
 		break;
 	case TRIG_NONE:
-		err |= cfc_check_trigger_arg_is(&cmd->stop_arg, 0);
+		err |= comedi_check_trigger_arg_is(&cmd->stop_arg, 0);
 		break;
 		/*
 		 * TRIG_EXT doesn't care since it doesn't
@@ -670,11 +646,12 @@ static int labpc_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 		 * load counter a1 with count of 3
 		 * (pc+ manual says this is minimum allowed) using mode 0
 		 */
-		labpc_counter_load(dev, COUNTER_A_BASE_REG,
-				   1, 3, I8254_MODE0);
+		comedi_8254_load(devpriv->counter, 1,
+				 3, I8254_MODE0 | I8254_BINARY);
 	} else	{
 		/* just put counter a1 in mode 0 to set its output low */
-		labpc_counter_set_mode(dev, COUNTER_A_BASE_REG, 1, I8254_MODE0);
+		comedi_8254_set_mode(devpriv->counter, 1,
+				     I8254_MODE0 | I8254_BINARY);
 	}
 
 	/* figure out what method we will use to transfer data */
@@ -715,27 +692,24 @@ static int labpc_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 
 	if (cmd->convert_src == TRIG_TIMER ||
 	    cmd->scan_begin_src == TRIG_TIMER) {
-		/*  set up pacing */
-		labpc_adc_timing(dev, cmd, mode);
-		/*  load counter b0 in mode 3 */
-		labpc_counter_load(dev, COUNTER_B_BASE_REG,
-				   0, devpriv->divisor_b0, I8254_MODE3);
-	}
-	/*  set up conversion pacing */
-	if (labpc_ai_convert_period(cmd, mode)) {
-		/*  load counter a0 in mode 2 */
-		labpc_counter_load(dev, COUNTER_A_BASE_REG,
-				   0, devpriv->divisor_a0, I8254_MODE2);
-	} else {
-		/* initialize pacer counter to prevent any problems */
-		labpc_counter_set_mode(dev, COUNTER_A_BASE_REG, 0, I8254_MODE2);
-	}
+		struct comedi_8254 *pacer = dev->pacer;
+		struct comedi_8254 *counter = devpriv->counter;
+
+		comedi_8254_update_divisors(pacer);
 
-	/*  set up scan pacing */
-	if (labpc_ai_scan_period(cmd, mode)) {
-		/*  load counter b1 in mode 2 */
-		labpc_counter_load(dev, COUNTER_B_BASE_REG,
-				   1, devpriv->divisor_b1, I8254_MODE2);
+		/* set up pacing */
+		comedi_8254_load(pacer, 0, pacer->divisor1,
+				 I8254_MODE3 | I8254_BINARY);
+
+		/* set up conversion pacing */
+		comedi_8254_set_mode(counter, 0, I8254_MODE2 | I8254_BINARY);
+		if (labpc_ai_convert_period(cmd, mode))
+			comedi_8254_write(counter, 0, pacer->divisor);
+
+		/* set up scan pacing */
+		if (labpc_ai_scan_period(cmd, mode))
+			comedi_8254_load(pacer, 1, pacer->divisor2,
+					 I8254_MODE2 | I8254_BINARY);
 	}
 
 	labpc_clear_adc_fifo(dev);
@@ -1240,6 +1214,26 @@ int labpc_common_attach(struct comedi_device *dev,
 			dev->irq = irq;
 	}
 
+	if (dev->mmio) {
+		dev->pacer = comedi_8254_mm_init(dev->mmio + COUNTER_B_BASE_REG,
+						 I8254_OSC_BASE_2MHZ,
+						 I8254_IO8, 0);
+		devpriv->counter = comedi_8254_mm_init(dev->mmio +
+						       COUNTER_A_BASE_REG,
+						       I8254_OSC_BASE_2MHZ,
+						       I8254_IO8, 0);
+	} else {
+		dev->pacer = comedi_8254_init(dev->iobase + COUNTER_B_BASE_REG,
+					      I8254_OSC_BASE_2MHZ,
+					      I8254_IO8, 0);
+		devpriv->counter = comedi_8254_init(dev->iobase +
+						    COUNTER_A_BASE_REG,
+						    I8254_OSC_BASE_2MHZ,
+						    I8254_IO8, 0);
+	}
+	if (!dev->pacer || !devpriv->counter)
+		return -ENOMEM;
+
 	ret = comedi_alloc_subdevices(dev, 5);
 	if (ret)
 		return ret;
@@ -1336,6 +1330,15 @@ int labpc_common_attach(struct comedi_device *dev,
 }
 EXPORT_SYMBOL_GPL(labpc_common_attach);
 
+void labpc_common_detach(struct comedi_device *dev)
+{
+	struct labpc_private *devpriv = dev->private;
+
+	if (devpriv)
+		kfree(devpriv->counter);
+}
+EXPORT_SYMBOL_GPL(labpc_common_detach);
+
 static int __init labpc_common_init(void)
 {
 	return 0;
diff --git a/drivers/staging/comedi/drivers/ni_labpc_cs.c b/drivers/staging/comedi/drivers/ni_labpc_cs.c
index 746c4cd9978d..a1c69ac075d5 100644
--- a/drivers/staging/comedi/drivers/ni_labpc_cs.c
+++ b/drivers/staging/comedi/drivers/ni_labpc_cs.c
@@ -68,8 +68,8 @@ static const struct labpc_boardinfo labpc_cs_boards[] = {
 	},
 };
 
-static int labpc_auto_attach(struct comedi_device *dev,
-			     unsigned long context)
+static int labpc_cs_auto_attach(struct comedi_device *dev,
+				unsigned long context)
 {
 	struct pcmcia_device *link = comedi_to_pcmcia_dev(dev);
 	int ret;
@@ -90,11 +90,17 @@ static int labpc_auto_attach(struct comedi_device *dev,
 	return labpc_common_attach(dev, link->irq, IRQF_SHARED);
 }
 
+static void labpc_cs_detach(struct comedi_device *dev)
+{
+	labpc_common_detach(dev);
+	comedi_pcmcia_disable(dev);
+}
+
 static struct comedi_driver driver_labpc_cs = {
 	.driver_name	= "ni_labpc_cs",
 	.module		= THIS_MODULE,
-	.auto_attach	= labpc_auto_attach,
-	.detach		= comedi_pcmcia_disable,
+	.auto_attach	= labpc_cs_auto_attach,
+	.detach		= labpc_cs_detach,
 };
 
 static int labpc_cs_attach(struct pcmcia_device *link)
diff --git a/drivers/staging/comedi/drivers/ni_labpc_isadma.c b/drivers/staging/comedi/drivers/ni_labpc_isadma.c
index 6b4ccd86b3d0..29dbdf5ec25d 100644
--- a/drivers/staging/comedi/drivers/ni_labpc_isadma.c
+++ b/drivers/staging/comedi/drivers/ni_labpc_isadma.c
@@ -23,7 +23,6 @@
 #include "../comedidev.h"
 
 #include "comedi_isadma.h"
-#include "comedi_fc.h"
 #include "ni_labpc.h"
 #include "ni_labpc_regs.h"
 #include "ni_labpc_isadma.h"
diff --git a/drivers/staging/comedi/drivers/ni_labpc_pci.c b/drivers/staging/comedi/drivers/ni_labpc_pci.c
index 0407ff681dfd..77d403801db5 100644
--- a/drivers/staging/comedi/drivers/ni_labpc_pci.c
+++ b/drivers/staging/comedi/drivers/ni_labpc_pci.c
@@ -31,9 +31,8 @@
 
 #include <linux/module.h>
 #include <linux/interrupt.h>
-#include <linux/pci.h>
 
-#include "../comedidev.h"
+#include "../comedi_pci.h"
 
 #include "ni_labpc.h"
 
@@ -51,7 +50,7 @@ static const struct labpc_boardinfo labpc_pci_boards[] = {
 	},
 };
 
-/* ripped from mite.h and mite_setup2() to avoid mite dependancy */
+/* ripped from mite.h and mite_setup2() to avoid mite dependency */
 #define MITE_IODWBSR	0xc0	 /* IO Device Window Base Size Register */
 #define WENAB		(1 << 7) /* window enable */
 
@@ -103,11 +102,17 @@ static int labpc_pci_auto_attach(struct comedi_device *dev,
 	return labpc_common_attach(dev, pcidev->irq, IRQF_SHARED);
 }
 
+static void labpc_pci_detach(struct comedi_device *dev)
+{
+	labpc_common_detach(dev);
+	comedi_pci_detach(dev);
+}
+
 static struct comedi_driver labpc_pci_comedi_driver = {
 	.driver_name	= "labpc_pci",
 	.module		= THIS_MODULE,
 	.auto_attach	= labpc_pci_auto_attach,
-	.detach		= comedi_pci_detach,
+	.detach		= labpc_pci_detach,
 };
 
 static const struct pci_device_id labpc_pci_table[] = {
diff --git a/drivers/staging/comedi/drivers/ni_mio_common.c b/drivers/staging/comedi/drivers/ni_mio_common.c
index b6ddc015dedf..c66affd993aa 100644
--- a/drivers/staging/comedi/drivers/ni_mio_common.c
+++ b/drivers/staging/comedi/drivers/ni_mio_common.c
@@ -57,7 +57,6 @@
 #include <linux/delay.h>
 #include "8255.h"
 #include "mite.h"
-#include "comedi_fc.h"
 
 /* A timeout count */
 #define NI_TIMEOUT 1000
@@ -670,8 +669,8 @@ static inline void ni_set_bitfield(struct comedi_device *dev, int reg,
 		ni_writeb(dev, devpriv->g0_g1_select_reg, G0_G1_Select);
 		break;
 	default:
-		dev_err(dev->class_dev,
-			"%s called with invalid register %d\n", __func__, reg);
+		dev_err(dev->class_dev, "called with invalid register %d\n",
+			reg);
 		break;
 	}
 	mmiowb();
@@ -755,7 +754,7 @@ static int ni_request_ai_mite_channel(struct comedi_device *dev)
 	BUG_ON(devpriv->ai_mite_chan);
 	devpriv->ai_mite_chan =
 	    mite_request_channel(devpriv->mite, devpriv->ai_mite_ring);
-	if (devpriv->ai_mite_chan == NULL) {
+	if (!devpriv->ai_mite_chan) {
 		spin_unlock_irqrestore(&devpriv->mite_channel_lock, flags);
 		dev_err(dev->class_dev,
 			"failed to reserve mite dma channel for analog input\n");
@@ -776,7 +775,7 @@ static int ni_request_ao_mite_channel(struct comedi_device *dev)
 	BUG_ON(devpriv->ao_mite_chan);
 	devpriv->ao_mite_chan =
 	    mite_request_channel(devpriv->mite, devpriv->ao_mite_ring);
-	if (devpriv->ao_mite_chan == NULL) {
+	if (!devpriv->ao_mite_chan) {
 		spin_unlock_irqrestore(&devpriv->mite_channel_lock, flags);
 		dev_err(dev->class_dev,
 			"failed to reserve mite dma channel for analog outut\n");
@@ -802,7 +801,7 @@ static int ni_request_gpct_mite_channel(struct comedi_device *dev,
 	mite_chan =
 	    mite_request_channel(devpriv->mite,
 				 devpriv->gpct_mite_ring[gpct_index]);
-	if (mite_chan == NULL) {
+	if (!mite_chan) {
 		spin_unlock_irqrestore(&devpriv->mite_channel_lock, flags);
 		dev_err(dev->class_dev,
 			"failed to reserve mite dma channel for counter\n");
@@ -828,7 +827,7 @@ static int ni_request_cdo_mite_channel(struct comedi_device *dev)
 	BUG_ON(devpriv->cdo_mite_chan);
 	devpriv->cdo_mite_chan =
 	    mite_request_channel(devpriv->mite, devpriv->cdo_mite_ring);
-	if (devpriv->cdo_mite_chan == NULL) {
+	if (!devpriv->cdo_mite_chan) {
 		spin_unlock_irqrestore(&devpriv->mite_channel_lock, flags);
 		dev_err(dev->class_dev,
 			"failed to reserve mite dma channel for correlated digital output\n");
@@ -1068,7 +1067,7 @@ static int ni_ai_drain_dma(struct comedi_device *dev)
 			udelay(5);
 		}
 		if (i == timeout) {
-			dev_err(dev->class_dev, "%s timed out\n", __func__);
+			dev_err(dev->class_dev, "timed out\n");
 			dev_err(dev->class_dev,
 				"mite_bytes_in_transit=%i, AI_Status1_Register=0x%x\n",
 				mite_bytes_in_transit(devpriv->ai_mite_chan),
@@ -1360,7 +1359,7 @@ static void get_last_sample_611x(struct comedi_device *dev)
 	/* Check if there's a single sample stuck in the FIFO */
 	if (ni_readb(dev, XXX_Status) & 0x80) {
 		dl = ni_readl(dev, ADC_FIFO_Data_611x);
-		data = (dl & 0xffff);
+		data = dl & 0xffff;
 		comedi_buf_write_samples(s, &data, 1);
 	}
 }
@@ -1657,7 +1656,7 @@ static int ni_ai_setup_MITE_dma(struct comedi_device *dev)
 	comedi_buf_write_alloc(s, s->async->prealloc_bufsz);
 
 	spin_lock_irqsave(&devpriv->mite_channel_lock, flags);
-	if (devpriv->ai_mite_chan == NULL) {
+	if (!devpriv->ai_mite_chan) {
 		spin_unlock_irqrestore(&devpriv->mite_channel_lock, flags);
 		return -EIO;
 	}
@@ -1700,8 +1699,9 @@ static int ni_ao_setup_MITE_dma(struct comedi_device *dev)
 			mite_prep_dma(devpriv->ao_mite_chan, 16, 32);
 		}
 		mite_dma_arm(devpriv->ao_mite_chan);
-	} else
+	} else {
 		retval = -EIO;
+	}
 	spin_unlock_irqrestore(&devpriv->mite_channel_lock, flags);
 
 	return retval;
@@ -1870,7 +1870,7 @@ static void ni_m_series_load_channelgain_list(struct comedi_device *dev,
 		chan = CR_CHAN(list[0]);
 		range = CR_RANGE(list[0]);
 		range_code = ni_gainlkup[board->gainlkup][range];
-		dither = ((list[0] & CR_ALT_FILTER) != 0);
+		dither = (list[0] & CR_ALT_FILTER) != 0;
 		bypass_bits = MSeries_AI_Bypass_Config_FIFO_Bit;
 		bypass_bits |= chan;
 		bypass_bits |=
@@ -1894,7 +1894,7 @@ static void ni_m_series_load_channelgain_list(struct comedi_device *dev,
 		chan = CR_CHAN(list[i]);
 		aref = CR_AREF(list[i]);
 		range = CR_RANGE(list[i]);
-		dither = ((list[i] & CR_ALT_FILTER) != 0);
+		dither = (list[i] & CR_ALT_FILTER) != 0;
 
 		range_code = ni_gainlkup[board->gainlkup][range];
 		devpriv->ai_offset[i] = 0;
@@ -2020,7 +2020,7 @@ static void ni_load_channelgain_list(struct comedi_device *dev,
 			chan = CR_CHAN(list[i]);
 		aref = CR_AREF(list[i]);
 		range = CR_RANGE(list[i]);
-		dither = ((list[i] & CR_ALT_FILTER) != 0);
+		dither = (list[i] & CR_ALT_FILTER) != 0;
 
 		/* fix the external/internal range differences */
 		range = ni_gainlkup[board->gainlkup][range];
@@ -2116,8 +2116,7 @@ static int ni_ai_insn_read(struct comedi_device *dev,
 				}
 			}
 			if (i == NI_TIMEOUT) {
-				dev_err(dev->class_dev, "%s timeout\n",
-					__func__);
+				dev_err(dev->class_dev, "timeout\n");
 				return -ETIME;
 			}
 			d += signbits;
@@ -2140,8 +2139,7 @@ static int ni_ai_insn_read(struct comedi_device *dev,
 				}
 			}
 			if (i == NI_TIMEOUT) {
-				dev_err(dev->class_dev, "%s timeout\n",
-					__func__);
+				dev_err(dev->class_dev, "timeout\n");
 				return -ETIME;
 			}
 			data[n] = (((dl >> 16) & 0xFFFF) + signbits) & 0xFFFF;
@@ -2156,8 +2154,7 @@ static int ni_ai_insn_read(struct comedi_device *dev,
 					break;
 			}
 			if (i == NI_TIMEOUT) {
-				dev_err(dev->class_dev, "%s timeout\n",
-					__func__);
+				dev_err(dev->class_dev, "timeout\n");
 				return -ETIME;
 			}
 			if (devpriv->is_m_series) {
@@ -2227,28 +2224,28 @@ static int ni_ai_cmdtest(struct comedi_device *dev, struct comedi_subdevice *s,
 
 	/* Step 1 : check if triggers are trivially valid */
 
-	err |= cfc_check_trigger_src(&cmd->start_src,
+	err |= comedi_check_trigger_src(&cmd->start_src,
 					TRIG_NOW | TRIG_INT | TRIG_EXT);
-	err |= cfc_check_trigger_src(&cmd->scan_begin_src,
+	err |= comedi_check_trigger_src(&cmd->scan_begin_src,
 					TRIG_TIMER | TRIG_EXT);
 
 	sources = TRIG_TIMER | TRIG_EXT;
 	if (devpriv->is_611x || devpriv->is_6143)
 		sources |= TRIG_NOW;
-	err |= cfc_check_trigger_src(&cmd->convert_src, sources);
+	err |= comedi_check_trigger_src(&cmd->convert_src, sources);
 
-	err |= cfc_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
-	err |= cfc_check_trigger_src(&cmd->stop_src, TRIG_COUNT | TRIG_NONE);
+	err |= comedi_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
+	err |= comedi_check_trigger_src(&cmd->stop_src, TRIG_COUNT | TRIG_NONE);
 
 	if (err)
 		return 1;
 
 	/* Step 2a : make sure trigger sources are unique */
 
-	err |= cfc_check_trigger_is_unique(cmd->start_src);
-	err |= cfc_check_trigger_is_unique(cmd->scan_begin_src);
-	err |= cfc_check_trigger_is_unique(cmd->convert_src);
-	err |= cfc_check_trigger_is_unique(cmd->stop_src);
+	err |= comedi_check_trigger_is_unique(cmd->start_src);
+	err |= comedi_check_trigger_is_unique(cmd->scan_begin_src);
+	err |= comedi_check_trigger_is_unique(cmd->convert_src);
+	err |= comedi_check_trigger_is_unique(cmd->stop_src);
 
 	/* Step 2b : and mutually compatible */
 
@@ -2260,7 +2257,7 @@ static int ni_ai_cmdtest(struct comedi_device *dev, struct comedi_subdevice *s,
 	switch (cmd->start_src) {
 	case TRIG_NOW:
 	case TRIG_INT:
-		err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
+		err |= comedi_check_trigger_arg_is(&cmd->start_arg, 0);
 		break;
 	case TRIG_EXT:
 		tmp = CR_CHAN(cmd->start_arg);
@@ -2268,15 +2265,16 @@ static int ni_ai_cmdtest(struct comedi_device *dev, struct comedi_subdevice *s,
 		if (tmp > 16)
 			tmp = 16;
 		tmp |= (cmd->start_arg & (CR_INVERT | CR_EDGE));
-		err |= cfc_check_trigger_arg_is(&cmd->start_arg, tmp);
+		err |= comedi_check_trigger_arg_is(&cmd->start_arg, tmp);
 		break;
 	}
 
 	if (cmd->scan_begin_src == TRIG_TIMER) {
-		err |= cfc_check_trigger_arg_min(&cmd->scan_begin_arg,
+		err |= comedi_check_trigger_arg_min(&cmd->scan_begin_arg,
 			ni_min_ai_scan_period_ns(dev, cmd->chanlist_len));
-		err |= cfc_check_trigger_arg_max(&cmd->scan_begin_arg,
-						 devpriv->clock_ns * 0xffffff);
+		err |= comedi_check_trigger_arg_max(&cmd->scan_begin_arg,
+						    devpriv->clock_ns *
+						    0xffffff);
 	} else if (cmd->scan_begin_src == TRIG_EXT) {
 		/* external trigger */
 		unsigned int tmp = CR_CHAN(cmd->scan_begin_arg);
@@ -2284,19 +2282,21 @@ static int ni_ai_cmdtest(struct comedi_device *dev, struct comedi_subdevice *s,
 		if (tmp > 16)
 			tmp = 16;
 		tmp |= (cmd->scan_begin_arg & (CR_INVERT | CR_EDGE));
-		err |= cfc_check_trigger_arg_is(&cmd->scan_begin_arg, tmp);
+		err |= comedi_check_trigger_arg_is(&cmd->scan_begin_arg, tmp);
 	} else {		/* TRIG_OTHER */
-		err |= cfc_check_trigger_arg_is(&cmd->scan_begin_arg, 0);
+		err |= comedi_check_trigger_arg_is(&cmd->scan_begin_arg, 0);
 	}
 
 	if (cmd->convert_src == TRIG_TIMER) {
 		if (devpriv->is_611x || devpriv->is_6143) {
-			err |= cfc_check_trigger_arg_is(&cmd->convert_arg, 0);
+			err |= comedi_check_trigger_arg_is(&cmd->convert_arg,
+							   0);
 		} else {
-			err |= cfc_check_trigger_arg_min(&cmd->convert_arg,
-							 board->ai_speed);
-			err |= cfc_check_trigger_arg_max(&cmd->convert_arg,
-						devpriv->clock_ns * 0xffff);
+			err |= comedi_check_trigger_arg_min(&cmd->convert_arg,
+							    board->ai_speed);
+			err |= comedi_check_trigger_arg_max(&cmd->convert_arg,
+							    devpriv->clock_ns *
+							    0xffff);
 		}
 	} else if (cmd->convert_src == TRIG_EXT) {
 		/* external trigger */
@@ -2305,23 +2305,24 @@ static int ni_ai_cmdtest(struct comedi_device *dev, struct comedi_subdevice *s,
 		if (tmp > 16)
 			tmp = 16;
 		tmp |= (cmd->convert_arg & (CR_ALT_FILTER | CR_INVERT));
-		err |= cfc_check_trigger_arg_is(&cmd->convert_arg, tmp);
+		err |= comedi_check_trigger_arg_is(&cmd->convert_arg, tmp);
 	} else if (cmd->convert_src == TRIG_NOW) {
-		err |= cfc_check_trigger_arg_is(&cmd->convert_arg, 0);
+		err |= comedi_check_trigger_arg_is(&cmd->convert_arg, 0);
 	}
 
-	err |= cfc_check_trigger_arg_is(&cmd->scan_end_arg, cmd->chanlist_len);
+	err |= comedi_check_trigger_arg_is(&cmd->scan_end_arg,
+					   cmd->chanlist_len);
 
 	if (cmd->stop_src == TRIG_COUNT) {
 		unsigned int max_count = 0x01000000;
 
 		if (devpriv->is_611x)
 			max_count -= num_adc_stages_611x;
-		err |= cfc_check_trigger_arg_max(&cmd->stop_arg, max_count);
-		err |= cfc_check_trigger_arg_min(&cmd->stop_arg, 1);
+		err |= comedi_check_trigger_arg_max(&cmd->stop_arg, max_count);
+		err |= comedi_check_trigger_arg_min(&cmd->stop_arg, 1);
 	} else {
 		/* TRIG_NONE */
-		err |= cfc_check_trigger_arg_is(&cmd->stop_arg, 0);
+		err |= comedi_check_trigger_arg_is(&cmd->stop_arg, 0);
 	}
 
 	if (err)
@@ -2529,7 +2530,7 @@ static int ni_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 		start_stop_select |=
 		    AI_START_Select(1 + CR_CHAN(cmd->scan_begin_arg));
 		ni_stc_writew(dev, start_stop_select,
-				    AI_START_STOP_Select_Register);
+			      AI_START_STOP_Select_Register);
 		break;
 	}
 
@@ -2572,7 +2573,6 @@ static int ni_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	}
 
 	if (dev->irq) {
-
 		/* interrupt on FIFO, errors, SC_TC */
 		interrupt_a_enable |= AI_Error_Interrupt_Enable |
 		    AI_SC_TC_Interrupt_Enable;
@@ -2808,8 +2808,7 @@ static int ni_m_series_ao_config_chanlist(struct comedi_device *dev,
 			break;
 		default:
 			dev_err(dev->class_dev,
-				"%s: bug! unhandled ao reference voltage\n",
-				__func__);
+				"bug! unhandled ao reference voltage\n");
 			break;
 		}
 		switch (krange->max + krange->min) {
@@ -2821,8 +2820,7 @@ static int ni_m_series_ao_config_chanlist(struct comedi_device *dev,
 			break;
 		default:
 			dev_err(dev->class_dev,
-				"%s: bug! unhandled ao offset voltage\n",
-				__func__);
+				"bug! unhandled ao offset voltage\n");
 			break;
 		}
 		if (timed)
@@ -3262,21 +3260,21 @@ static int ni_ao_cmdtest(struct comedi_device *dev, struct comedi_subdevice *s,
 
 	/* Step 1 : check if triggers are trivially valid */
 
-	err |= cfc_check_trigger_src(&cmd->start_src, TRIG_INT | TRIG_EXT);
-	err |= cfc_check_trigger_src(&cmd->scan_begin_src,
+	err |= comedi_check_trigger_src(&cmd->start_src, TRIG_INT | TRIG_EXT);
+	err |= comedi_check_trigger_src(&cmd->scan_begin_src,
 					TRIG_TIMER | TRIG_EXT);
-	err |= cfc_check_trigger_src(&cmd->convert_src, TRIG_NOW);
-	err |= cfc_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
-	err |= cfc_check_trigger_src(&cmd->stop_src, TRIG_COUNT | TRIG_NONE);
+	err |= comedi_check_trigger_src(&cmd->convert_src, TRIG_NOW);
+	err |= comedi_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
+	err |= comedi_check_trigger_src(&cmd->stop_src, TRIG_COUNT | TRIG_NONE);
 
 	if (err)
 		return 1;
 
 	/* Step 2a : make sure trigger sources are unique */
 
-	err |= cfc_check_trigger_is_unique(cmd->start_src);
-	err |= cfc_check_trigger_is_unique(cmd->scan_begin_src);
-	err |= cfc_check_trigger_is_unique(cmd->stop_src);
+	err |= comedi_check_trigger_is_unique(cmd->start_src);
+	err |= comedi_check_trigger_is_unique(cmd->scan_begin_src);
+	err |= comedi_check_trigger_is_unique(cmd->stop_src);
 
 	/* Step 2b : and mutually compatible */
 
@@ -3287,7 +3285,7 @@ static int ni_ao_cmdtest(struct comedi_device *dev, struct comedi_subdevice *s,
 
 	switch (cmd->start_src) {
 	case TRIG_INT:
-		err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
+		err |= comedi_check_trigger_arg_is(&cmd->start_arg, 0);
 		break;
 	case TRIG_EXT:
 		tmp = CR_CHAN(cmd->start_arg);
@@ -3295,24 +3293,26 @@ static int ni_ao_cmdtest(struct comedi_device *dev, struct comedi_subdevice *s,
 		if (tmp > 18)
 			tmp = 18;
 		tmp |= (cmd->start_arg & (CR_INVERT | CR_EDGE));
-		err |= cfc_check_trigger_arg_is(&cmd->start_arg, tmp);
+		err |= comedi_check_trigger_arg_is(&cmd->start_arg, tmp);
 		break;
 	}
 
 	if (cmd->scan_begin_src == TRIG_TIMER) {
-		err |= cfc_check_trigger_arg_min(&cmd->scan_begin_arg,
-						 board->ao_speed);
-		err |= cfc_check_trigger_arg_max(&cmd->scan_begin_arg,
-						 devpriv->clock_ns * 0xffffff);
+		err |= comedi_check_trigger_arg_min(&cmd->scan_begin_arg,
+						    board->ao_speed);
+		err |= comedi_check_trigger_arg_max(&cmd->scan_begin_arg,
+						    devpriv->clock_ns *
+						    0xffffff);
 	}
 
-	err |= cfc_check_trigger_arg_is(&cmd->convert_arg, 0);
-	err |= cfc_check_trigger_arg_is(&cmd->scan_end_arg, cmd->chanlist_len);
+	err |= comedi_check_trigger_arg_is(&cmd->convert_arg, 0);
+	err |= comedi_check_trigger_arg_is(&cmd->scan_end_arg,
+					   cmd->chanlist_len);
 
 	if (cmd->stop_src == TRIG_COUNT)
-		err |= cfc_check_trigger_arg_max(&cmd->stop_arg, 0x00ffffff);
+		err |= comedi_check_trigger_arg_max(&cmd->stop_arg, 0x00ffffff);
 	else	/* TRIG_NONE */
-		err |= cfc_check_trigger_arg_is(&cmd->stop_arg, 0);
+		err |= comedi_check_trigger_arg_is(&cmd->stop_arg, 0);
 
 	if (err)
 		return 3;
@@ -3474,11 +3474,11 @@ static int ni_cdio_cmdtest(struct comedi_device *dev,
 
 	/* Step 1 : check if triggers are trivially valid */
 
-	err |= cfc_check_trigger_src(&cmd->start_src, TRIG_INT);
-	err |= cfc_check_trigger_src(&cmd->scan_begin_src, TRIG_EXT);
-	err |= cfc_check_trigger_src(&cmd->convert_src, TRIG_NOW);
-	err |= cfc_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
-	err |= cfc_check_trigger_src(&cmd->stop_src, TRIG_NONE);
+	err |= comedi_check_trigger_src(&cmd->start_src, TRIG_INT);
+	err |= comedi_check_trigger_src(&cmd->scan_begin_src, TRIG_EXT);
+	err |= comedi_check_trigger_src(&cmd->convert_src, TRIG_NOW);
+	err |= comedi_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
+	err |= comedi_check_trigger_src(&cmd->stop_src, TRIG_NONE);
 
 	if (err)
 		return 1;
@@ -3488,16 +3488,17 @@ static int ni_cdio_cmdtest(struct comedi_device *dev,
 
 	/* Step 3: check if arguments are trivially valid */
 
-	err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
+	err |= comedi_check_trigger_arg_is(&cmd->start_arg, 0);
 
 	tmp = cmd->scan_begin_arg;
 	tmp &= CR_PACK_FLAGS(CDO_Sample_Source_Select_Mask, 0, 0, CR_INVERT);
 	if (tmp != cmd->scan_begin_arg)
 		err |= -EINVAL;
 
-	err |= cfc_check_trigger_arg_is(&cmd->convert_arg, 0);
-	err |= cfc_check_trigger_arg_is(&cmd->scan_end_arg, cmd->chanlist_len);
-	err |= cfc_check_trigger_arg_is(&cmd->stop_arg, 0);
+	err |= comedi_check_trigger_arg_is(&cmd->convert_arg, 0);
+	err |= comedi_check_trigger_arg_is(&cmd->scan_end_arg,
+					   cmd->chanlist_len);
+	err |= comedi_check_trigger_arg_is(&cmd->stop_arg, 0);
 
 	if (err)
 		return 3;
@@ -3694,8 +3695,7 @@ static int ni_serial_hw_readwrite8(struct comedi_device *dev,
 		udelay((devpriv->serial_interval_ns + 999) / 1000);
 		if (--count < 0) {
 			dev_err(dev->class_dev,
-				"%s: SPI serial I/O didn't finish in time!\n",
-				__func__);
+				"SPI serial I/O didn't finish in time!\n");
 			err = -ETIME;
 			goto Error;
 		}
@@ -3705,7 +3705,7 @@ static int ni_serial_hw_readwrite8(struct comedi_device *dev,
 	   DIO_Serial_IO_In_Progress_St goes high one bit too early. */
 	udelay((devpriv->serial_interval_ns + 999) / 1000);
 
-	if (data_in != NULL)
+	if (data_in)
 		*data_in = ni_stc_readw(dev, DIO_Serial_Input_Register);
 
 Error:
@@ -3817,8 +3817,6 @@ static int ni_serial_insn_config(struct comedi_device *dev,
 			      Clock_and_FOUT_Register);
 		return 1;
 
-		break;
-
 	case INSN_CONFIG_BIDIRECTIONAL_DATA:
 
 		if (devpriv->serial_interval_ns == 0)
@@ -3833,8 +3831,7 @@ static int ni_serial_insn_config(struct comedi_device *dev,
 			err = ni_serial_sw_readwrite8(dev, s, byte_out,
 						      &byte_in);
 		} else {
-			dev_err(dev->class_dev, "%s: serial disabled!\n",
-				__func__);
+			dev_err(dev->class_dev, "serial disabled!\n");
 			return -EINVAL;
 		}
 		if (err < 0)
@@ -3846,7 +3843,6 @@ static int ni_serial_insn_config(struct comedi_device *dev,
 	default:
 		return -EINVAL;
 	}
-
 }
 
 static void init_ao_67xx(struct comedi_device *dev, struct comedi_subdevice *s)
@@ -4520,8 +4516,7 @@ static unsigned ni_old_get_pfi_routing(struct comedi_device *dev,
 	case 9:
 		return NI_PFI_OUTPUT_G_GATE0;
 	default:
-		dev_err(dev->class_dev,
-			"%s: bug, unhandled case in switch.\n", __func__);
+		dev_err(dev->class_dev, "bug, unhandled case in switch.\n");
 		break;
 	}
 	return 0;
@@ -4673,7 +4668,7 @@ static int cs5529_wait_for_idle(struct comedi_device *dev)
 			return -EIO;
 	}
 	if (i == timeout) {
-		dev_err(dev->class_dev, "%s timeout\n", __func__);
+		dev_err(dev->class_dev, "timeout\n");
 		return -ETIME;
 	}
 	return 0;
@@ -4908,7 +4903,7 @@ static int ni_mseries_set_pll_master_clock(struct comedi_device *dev,
 					       &devpriv->clock_ns);
 	if (retval < 0) {
 		dev_err(dev->class_dev,
-			"%s: bug, failed to find pll parameters\n", __func__);
+			"bug, failed to find pll parameters\n");
 		return retval;
 	}
 
@@ -4966,14 +4961,14 @@ static int ni_set_master_clock(struct comedi_device *dev,
 					      RTSI_Trig_Direction_Register);
 				if (period_ns == 0) {
 					dev_err(dev->class_dev,
-						"%s: we don't handle an unspecified clock period correctly yet, returning error\n",
-						__func__);
+						"we don't handle an unspecified clock period correctly yet, returning error\n");
 					return -EINVAL;
 				}
 				devpriv->clock_ns = period_ns;
 				devpriv->clock_source = source;
-			} else
+			} else {
 				return -EINVAL;
+			}
 		}
 	}
 	return 3;
@@ -5057,8 +5052,7 @@ static unsigned ni_get_rtsi_routing(struct comedi_device *dev, unsigned chan)
 	} else {
 		if (chan == old_RTSI_clock_channel)
 			return NI_RTSI_OUTPUT_RTSI_OSC;
-		dev_err(dev->class_dev, "%s: bug! should never get here?\n",
-			__func__);
+		dev_err(dev->class_dev, "bug! should never get here?\n");
 		return 0;
 	}
 }
diff --git a/drivers/staging/comedi/drivers/ni_pcidio.c b/drivers/staging/comedi/drivers/ni_pcidio.c
index db399fe8c301..ac79099bc23e 100644
--- a/drivers/staging/comedi/drivers/ni_pcidio.c
+++ b/drivers/staging/comedi/drivers/ni_pcidio.c
@@ -53,9 +53,8 @@ comedi_nonfree_firmware tarball available from http://www.comedi.org
 #include <linux/interrupt.h>
 #include <linux/sched.h>
 
-#include "../comedidev.h"
+#include "../comedi_pci.h"
 
-#include "comedi_fc.h"
 #include "mite.h"
 
 /* defines for the PCI-DIO-32HS */
@@ -304,7 +303,7 @@ static int ni_pcidio_request_di_mite_channel(struct comedi_device *dev)
 	devpriv->di_mite_chan =
 	    mite_request_channel_in_range(devpriv->mite,
 					  devpriv->di_mite_ring, 1, 2);
-	if (devpriv->di_mite_chan == NULL) {
+	if (!devpriv->di_mite_chan) {
 		spin_unlock_irqrestore(&devpriv->mite_channel_lock, flags);
 		dev_err(dev->class_dev, "failed to reserve mite dma channel\n");
 		return -EBUSY;
@@ -354,8 +353,9 @@ static int setup_mite_dma(struct comedi_device *dev, struct comedi_subdevice *s)
 	if (devpriv->di_mite_chan) {
 		mite_prep_dma(devpriv->di_mite_chan, 32, 32);
 		mite_dma_arm(devpriv->di_mite_chan);
-	} else
+	} else {
 		retval = -EIO;
+	}
 	spin_unlock_irqrestore(&devpriv->mite_channel_lock, flags);
 
 	return retval;
@@ -547,21 +547,21 @@ static int ni_pcidio_cmdtest(struct comedi_device *dev,
 
 	/* Step 1 : check if triggers are trivially valid */
 
-	err |= cfc_check_trigger_src(&cmd->start_src, TRIG_NOW | TRIG_INT);
-	err |= cfc_check_trigger_src(&cmd->scan_begin_src,
+	err |= comedi_check_trigger_src(&cmd->start_src, TRIG_NOW | TRIG_INT);
+	err |= comedi_check_trigger_src(&cmd->scan_begin_src,
 					TRIG_TIMER | TRIG_EXT);
-	err |= cfc_check_trigger_src(&cmd->convert_src, TRIG_NOW);
-	err |= cfc_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
-	err |= cfc_check_trigger_src(&cmd->stop_src, TRIG_COUNT | TRIG_NONE);
+	err |= comedi_check_trigger_src(&cmd->convert_src, TRIG_NOW);
+	err |= comedi_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
+	err |= comedi_check_trigger_src(&cmd->stop_src, TRIG_COUNT | TRIG_NONE);
 
 	if (err)
 		return 1;
 
 	/* Step 2a : make sure trigger sources are unique */
 
-	err |= cfc_check_trigger_is_unique(cmd->start_src);
-	err |= cfc_check_trigger_is_unique(cmd->scan_begin_src);
-	err |= cfc_check_trigger_is_unique(cmd->stop_src);
+	err |= comedi_check_trigger_is_unique(cmd->start_src);
+	err |= comedi_check_trigger_is_unique(cmd->scan_begin_src);
+	err |= comedi_check_trigger_is_unique(cmd->stop_src);
 
 	/* Step 2b : and mutually compatible */
 
@@ -570,13 +570,13 @@ static int ni_pcidio_cmdtest(struct comedi_device *dev,
 
 	/* Step 3: check if arguments are trivially valid */
 
-	err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
+	err |= comedi_check_trigger_arg_is(&cmd->start_arg, 0);
 
 #define MAX_SPEED	(TIMER_BASE)	/* in nanoseconds */
 
 	if (cmd->scan_begin_src == TRIG_TIMER) {
-		err |= cfc_check_trigger_arg_min(&cmd->scan_begin_arg,
-						 MAX_SPEED);
+		err |= comedi_check_trigger_arg_min(&cmd->scan_begin_arg,
+						    MAX_SPEED);
 		/* no minimum speed */
 	} else {
 		/* TRIG_EXT */
@@ -587,13 +587,14 @@ static int ni_pcidio_cmdtest(struct comedi_device *dev,
 		}
 	}
 
-	err |= cfc_check_trigger_arg_is(&cmd->convert_arg, 0);
-	err |= cfc_check_trigger_arg_is(&cmd->scan_end_arg, cmd->chanlist_len);
+	err |= comedi_check_trigger_arg_is(&cmd->convert_arg, 0);
+	err |= comedi_check_trigger_arg_is(&cmd->scan_end_arg,
+					   cmd->chanlist_len);
 
 	if (cmd->stop_src == TRIG_COUNT)
-		err |= cfc_check_trigger_arg_min(&cmd->stop_arg, 1);
+		err |= comedi_check_trigger_arg_min(&cmd->stop_arg, 1);
 	else	/* TRIG_NONE */
-		err |= cfc_check_trigger_arg_is(&cmd->stop_arg, 0);
+		err |= comedi_check_trigger_arg_is(&cmd->stop_arg, 0);
 
 	if (err)
 		return 3;
@@ -603,7 +604,7 @@ static int ni_pcidio_cmdtest(struct comedi_device *dev,
 	if (cmd->scan_begin_src == TRIG_TIMER) {
 		arg = cmd->scan_begin_arg;
 		ni_pcidio_ns_to_timer(&arg, cmd->flags);
-		err |= cfc_check_trigger_arg_is(&cmd->scan_begin_arg, arg);
+		err |= comedi_check_trigger_arg_is(&cmd->scan_begin_arg, arg);
 	}
 
 	if (err)
@@ -924,7 +925,7 @@ static int nidio_auto_attach(struct comedi_device *dev,
 		return ret;
 
 	devpriv->di_mite_ring = mite_alloc_ring(devpriv->mite);
-	if (devpriv->di_mite_ring == NULL)
+	if (!devpriv->di_mite_ring)
 		return -ENOMEM;
 
 	if (board->uses_firmware) {
diff --git a/drivers/staging/comedi/drivers/ni_pcimio.c b/drivers/staging/comedi/drivers/ni_pcimio.c
index 3b2bdebbca59..1481f71a31b1 100644
--- a/drivers/staging/comedi/drivers/ni_pcimio.c
+++ b/drivers/staging/comedi/drivers/ni_pcimio.c
@@ -109,7 +109,7 @@ Bugs:
 #include <linux/module.h>
 #include <linux/delay.h>
 
-#include "../comedidev.h"
+#include "../comedi_pci.h"
 
 #include <asm/byteorder.h>
 
@@ -1041,7 +1041,6 @@ static int pcimio_dio_change(struct comedi_device *dev,
 	return 0;
 }
 
-
 static void m_series_init_eeprom_buffer(struct comedi_device *dev)
 {
 	struct ni_private *devpriv = dev->private;
@@ -1183,19 +1182,19 @@ static int pcimio_auto_attach(struct comedi_device *dev,
 		return ret;
 
 	devpriv->ai_mite_ring = mite_alloc_ring(devpriv->mite);
-	if (devpriv->ai_mite_ring == NULL)
+	if (!devpriv->ai_mite_ring)
 		return -ENOMEM;
 	devpriv->ao_mite_ring = mite_alloc_ring(devpriv->mite);
-	if (devpriv->ao_mite_ring == NULL)
+	if (!devpriv->ao_mite_ring)
 		return -ENOMEM;
 	devpriv->cdo_mite_ring = mite_alloc_ring(devpriv->mite);
-	if (devpriv->cdo_mite_ring == NULL)
+	if (!devpriv->cdo_mite_ring)
 		return -ENOMEM;
 	devpriv->gpct_mite_ring[0] = mite_alloc_ring(devpriv->mite);
-	if (devpriv->gpct_mite_ring[0] == NULL)
+	if (!devpriv->gpct_mite_ring[0])
 		return -ENOMEM;
 	devpriv->gpct_mite_ring[1] = mite_alloc_ring(devpriv->mite);
-	if (devpriv->gpct_mite_ring[1] == NULL)
+	if (!devpriv->gpct_mite_ring[1])
 		return -ENOMEM;
 
 	if (devpriv->is_m_series)
diff --git a/drivers/staging/comedi/drivers/ni_tiocmd.c b/drivers/staging/comedi/drivers/ni_tiocmd.c
index d36c3abd3120..9b124b09e914 100644
--- a/drivers/staging/comedi/drivers/ni_tiocmd.c
+++ b/drivers/staging/comedi/drivers/ni_tiocmd.c
@@ -44,7 +44,6 @@ TODO:
 */
 
 #include <linux/module.h>
-#include "comedi_fc.h"
 #include "ni_tio_internal.h"
 #include "mite.h"
 
@@ -201,7 +200,7 @@ int ni_tio_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	unsigned long flags;
 
 	spin_lock_irqsave(&counter->lock, flags);
-	if (counter->mite_chan == NULL) {
+	if (!counter->mite_chan) {
 		dev_err(counter->counter_dev->dev->class_dev,
 			"commands only supported with DMA.  ");
 		dev_err(counter->counter_dev->dev->class_dev,
@@ -234,23 +233,23 @@ int ni_tio_cmdtest(struct comedi_device *dev,
 	sources = TRIG_NOW | TRIG_INT | TRIG_OTHER;
 	if (ni_tio_counting_mode_registers_present(counter->counter_dev))
 		sources |= TRIG_EXT;
-	err |= cfc_check_trigger_src(&cmd->start_src, sources);
+	err |= comedi_check_trigger_src(&cmd->start_src, sources);
 
-	err |= cfc_check_trigger_src(&cmd->scan_begin_src,
+	err |= comedi_check_trigger_src(&cmd->scan_begin_src,
 					TRIG_FOLLOW | TRIG_EXT | TRIG_OTHER);
-	err |= cfc_check_trigger_src(&cmd->convert_src,
+	err |= comedi_check_trigger_src(&cmd->convert_src,
 					TRIG_NOW | TRIG_EXT | TRIG_OTHER);
-	err |= cfc_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
-	err |= cfc_check_trigger_src(&cmd->stop_src, TRIG_NONE);
+	err |= comedi_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
+	err |= comedi_check_trigger_src(&cmd->stop_src, TRIG_NONE);
 
 	if (err)
 		return 1;
 
 	/* Step 2a : make sure trigger sources are unique */
 
-	err |= cfc_check_trigger_is_unique(cmd->start_src);
-	err |= cfc_check_trigger_is_unique(cmd->scan_begin_src);
-	err |= cfc_check_trigger_is_unique(cmd->convert_src);
+	err |= comedi_check_trigger_is_unique(cmd->start_src);
+	err |= comedi_check_trigger_is_unique(cmd->scan_begin_src);
+	err |= comedi_check_trigger_is_unique(cmd->convert_src);
 
 	/* Step 2b : and mutually compatible */
 
@@ -266,7 +265,7 @@ int ni_tio_cmdtest(struct comedi_device *dev,
 	case TRIG_NOW:
 	case TRIG_INT:
 	case TRIG_OTHER:
-		err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
+		err |= comedi_check_trigger_arg_is(&cmd->start_arg, 0);
 		break;
 	case TRIG_EXT:
 		/* start_arg is the start_trigger passed to ni_tio_arm() */
@@ -274,13 +273,14 @@ int ni_tio_cmdtest(struct comedi_device *dev,
 	}
 
 	if (cmd->scan_begin_src != TRIG_EXT)
-		err |= cfc_check_trigger_arg_is(&cmd->scan_begin_arg, 0);
+		err |= comedi_check_trigger_arg_is(&cmd->scan_begin_arg, 0);
 
 	if (cmd->convert_src != TRIG_EXT)
-		err |= cfc_check_trigger_arg_is(&cmd->convert_arg, 0);
+		err |= comedi_check_trigger_arg_is(&cmd->convert_arg, 0);
 
-	err |= cfc_check_trigger_arg_is(&cmd->scan_end_arg, cmd->chanlist_len);
-	err |= cfc_check_trigger_arg_is(&cmd->stop_arg, 0);
+	err |= comedi_check_trigger_arg_is(&cmd->scan_end_arg,
+					   cmd->chanlist_len);
+	err |= comedi_check_trigger_arg_is(&cmd->stop_arg, 0);
 
 	if (err)
 		return 3;
@@ -329,7 +329,7 @@ static int should_ack_gate(struct ni_gpct *counter)
 	case ni_gpct_variant_e_series:
 		spin_lock_irqsave(&counter->lock, flags);
 		{
-			if (counter->mite_chan == NULL ||
+			if (!counter->mite_chan ||
 			    counter->mite_chan->dir != COMEDI_INPUT ||
 			    (mite_done(counter->mite_chan))) {
 				retval = 1;
@@ -366,7 +366,7 @@ static void ni_tio_acknowledge_and_confirm(struct ni_gpct *counter,
 	if (gxx_status & GI_GATE_ERROR(cidx)) {
 		ack |= GI_GATE_ERROR_CONFIRM(cidx);
 		if (gate_error) {
-			/*660x don't support automatic acknowledgement
+			/*660x don't support automatic acknowledgment
 			  of gate interrupt via dma read/write
 			   and report bogus gate errors */
 			if (counter->counter_dev->variant !=
@@ -443,7 +443,7 @@ void ni_tio_handle_interrupt(struct ni_gpct *counter,
 		break;
 	}
 	spin_lock_irqsave(&counter->lock, flags);
-	if (counter->mite_chan == NULL) {
+	if (!counter->mite_chan) {
 		spin_unlock_irqrestore(&counter->lock, flags);
 		return;
 	}
diff --git a/drivers/staging/comedi/drivers/pcl711.c b/drivers/staging/comedi/drivers/pcl711.c
index cb7e4c37b8b9..cfc3a627d0ca 100644
--- a/drivers/staging/comedi/drivers/pcl711.c
+++ b/drivers/staging/comedi/drivers/pcl711.c
@@ -41,8 +41,7 @@
 
 #include "../comedidev.h"
 
-#include "comedi_fc.h"
-#include "8253.h"
+#include "comedi_8254.h"
 
 /*
  * I/O port register map
@@ -153,11 +152,6 @@ static const struct pcl711_board boardtypes[] = {
 	},
 };
 
-struct pcl711_private {
-	unsigned int divisor1;
-	unsigned int divisor2;
-};
-
 static void pcl711_ai_set_mode(struct comedi_device *dev, unsigned int mode)
 {
 	/*
@@ -287,26 +281,24 @@ static int pcl711_ai_insn_read(struct comedi_device *dev,
 static int pcl711_ai_cmdtest(struct comedi_device *dev,
 			     struct comedi_subdevice *s, struct comedi_cmd *cmd)
 {
-	struct pcl711_private *devpriv = dev->private;
 	int err = 0;
-	unsigned int arg;
 
 	/* Step 1 : check if triggers are trivially valid */
 
-	err |= cfc_check_trigger_src(&cmd->start_src, TRIG_NOW);
-	err |= cfc_check_trigger_src(&cmd->scan_begin_src,
+	err |= comedi_check_trigger_src(&cmd->start_src, TRIG_NOW);
+	err |= comedi_check_trigger_src(&cmd->scan_begin_src,
 					TRIG_TIMER | TRIG_EXT);
-	err |= cfc_check_trigger_src(&cmd->convert_src, TRIG_NOW);
-	err |= cfc_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
-	err |= cfc_check_trigger_src(&cmd->stop_src, TRIG_COUNT | TRIG_NONE);
+	err |= comedi_check_trigger_src(&cmd->convert_src, TRIG_NOW);
+	err |= comedi_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
+	err |= comedi_check_trigger_src(&cmd->stop_src, TRIG_COUNT | TRIG_NONE);
 
 	if (err)
 		return 1;
 
 	/* Step 2a : make sure trigger sources are unique */
 
-	err |= cfc_check_trigger_is_unique(cmd->scan_begin_src);
-	err |= cfc_check_trigger_is_unique(cmd->stop_src);
+	err |= comedi_check_trigger_is_unique(cmd->scan_begin_src);
+	err |= comedi_check_trigger_is_unique(cmd->stop_src);
 
 	/* Step 2b : and mutually compatible */
 
@@ -315,23 +307,24 @@ static int pcl711_ai_cmdtest(struct comedi_device *dev,
 
 	/* Step 3: check if arguments are trivially valid */
 
-	err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
+	err |= comedi_check_trigger_arg_is(&cmd->start_arg, 0);
 
 	if (cmd->scan_begin_src == TRIG_EXT) {
-		err |= cfc_check_trigger_arg_is(&cmd->scan_begin_arg, 0);
+		err |= comedi_check_trigger_arg_is(&cmd->scan_begin_arg, 0);
 	} else {
 #define MAX_SPEED 1000
-		err |= cfc_check_trigger_arg_min(&cmd->scan_begin_arg,
-						 MAX_SPEED);
+		err |= comedi_check_trigger_arg_min(&cmd->scan_begin_arg,
+						    MAX_SPEED);
 	}
 
-	err |= cfc_check_trigger_arg_is(&cmd->convert_arg, 0);
-	err |= cfc_check_trigger_arg_is(&cmd->scan_end_arg, cmd->chanlist_len);
+	err |= comedi_check_trigger_arg_is(&cmd->convert_arg, 0);
+	err |= comedi_check_trigger_arg_is(&cmd->scan_end_arg,
+					   cmd->chanlist_len);
 
 	if (cmd->stop_src == TRIG_COUNT)
-		err |= cfc_check_trigger_arg_min(&cmd->stop_arg, 1);
+		err |= comedi_check_trigger_arg_min(&cmd->stop_arg, 1);
 	else	/* TRIG_NONE */
-		err |= cfc_check_trigger_arg_is(&cmd->stop_arg, 0);
+		err |= comedi_check_trigger_arg_is(&cmd->stop_arg, 0);
 
 	if (err)
 		return 3;
@@ -339,12 +332,10 @@ static int pcl711_ai_cmdtest(struct comedi_device *dev,
 	/* step 4 */
 
 	if (cmd->scan_begin_src == TRIG_TIMER) {
-		arg = cmd->scan_begin_arg;
-		i8253_cascade_ns_to_timer(I8254_OSC_BASE_2MHZ,
-					  &devpriv->divisor1,
-					  &devpriv->divisor2,
-					  &arg, cmd->flags);
-		err |= cfc_check_trigger_arg_is(&cmd->scan_begin_arg, arg);
+		unsigned int arg = cmd->scan_begin_arg;
+
+		comedi_8254_cascade_ns_to_timer(dev->pacer, &arg, cmd->flags);
+		err |= comedi_check_trigger_arg_is(&cmd->scan_begin_arg, arg);
 	}
 
 	if (err)
@@ -353,18 +344,6 @@ static int pcl711_ai_cmdtest(struct comedi_device *dev,
 	return 0;
 }
 
-static void pcl711_ai_load_counters(struct comedi_device *dev)
-{
-	struct pcl711_private *devpriv = dev->private;
-	unsigned long timer_base = dev->iobase + PCL711_TIMER_BASE;
-
-	i8254_set_mode(timer_base, 0, 1, I8254_MODE2 | I8254_BINARY);
-	i8254_set_mode(timer_base, 0, 2, I8254_MODE2 | I8254_BINARY);
-
-	i8254_write(timer_base, 0, 1, devpriv->divisor1);
-	i8254_write(timer_base, 0, 2, devpriv->divisor2);
-}
-
 static int pcl711_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 {
 	struct comedi_cmd *cmd = &s->async->cmd;
@@ -372,7 +351,8 @@ static int pcl711_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	pcl711_set_changain(dev, s, cmd->chanlist[0]);
 
 	if (cmd->scan_begin_src == TRIG_TIMER) {
-		pcl711_ai_load_counters(dev);
+		comedi_8254_update_divisors(dev->pacer);
+		comedi_8254_pacer_enable(dev->pacer, 1, 2, true);
 		outb(PCL711_INT_STAT_CLR, dev->iobase + PCL711_INT_STAT_REG);
 		pcl711_ai_set_mode(dev, PCL711_MODE_PACER_IRQ);
 	} else {
@@ -445,14 +425,9 @@ static int pcl711_do_insn_bits(struct comedi_device *dev,
 static int pcl711_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 {
 	const struct pcl711_board *board = dev->board_ptr;
-	struct pcl711_private *devpriv;
 	struct comedi_subdevice *s;
 	int ret;
 
-	devpriv = comedi_alloc_devpriv(dev, sizeof(*devpriv));
-	if (!devpriv)
-		return -ENOMEM;
-
 	ret = comedi_request_region(dev, it->options[0], 0x10);
 	if (ret)
 		return ret;
@@ -464,6 +439,11 @@ static int pcl711_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 			dev->irq = it->options[1];
 	}
 
+	dev->pacer = comedi_8254_init(dev->iobase + PCL711_TIMER_BASE,
+				      I8254_OSC_BASE_2MHZ, I8254_IO8, 0);
+	if (!dev->pacer)
+		return -ENOMEM;
+
 	ret = comedi_alloc_subdevices(dev, 4);
 	if (ret)
 		return ret;
diff --git a/drivers/staging/comedi/drivers/pcl726.c b/drivers/staging/comedi/drivers/pcl726.c
index 40798150cfd8..256850ccb6fa 100644
--- a/drivers/staging/comedi/drivers/pcl726.c
+++ b/drivers/staging/comedi/drivers/pcl726.c
@@ -62,8 +62,6 @@
 
 #include "../comedidev.h"
 
-#include "comedi_fc.h"
-
 #define PCL726_AO_MSB_REG(x)	(0x00 + ((x) * 2))
 #define PCL726_AO_LSB_REG(x)	(0x01 + ((x) * 2))
 #define PCL726_DO_MSB_REG	0x0c
@@ -173,11 +171,11 @@ static int pcl726_intr_cmdtest(struct comedi_device *dev,
 
 	/* Step 1 : check if triggers are trivially valid */
 
-	err |= cfc_check_trigger_src(&cmd->start_src, TRIG_NOW);
-	err |= cfc_check_trigger_src(&cmd->scan_begin_src, TRIG_EXT);
-	err |= cfc_check_trigger_src(&cmd->convert_src, TRIG_FOLLOW);
-	err |= cfc_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
-	err |= cfc_check_trigger_src(&cmd->stop_src, TRIG_NONE);
+	err |= comedi_check_trigger_src(&cmd->start_src, TRIG_NOW);
+	err |= comedi_check_trigger_src(&cmd->scan_begin_src, TRIG_EXT);
+	err |= comedi_check_trigger_src(&cmd->convert_src, TRIG_FOLLOW);
+	err |= comedi_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
+	err |= comedi_check_trigger_src(&cmd->stop_src, TRIG_NONE);
 
 	if (err)
 		return 1;
@@ -187,11 +185,12 @@ static int pcl726_intr_cmdtest(struct comedi_device *dev,
 
 	/* Step 3: check if arguments are trivially valid */
 
-	err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
-	err |= cfc_check_trigger_arg_is(&cmd->scan_begin_arg, 0);
-	err |= cfc_check_trigger_arg_is(&cmd->convert_arg, 0);
-	err |= cfc_check_trigger_arg_is(&cmd->scan_end_arg, cmd->chanlist_len);
-	err |= cfc_check_trigger_arg_is(&cmd->stop_arg, 0);
+	err |= comedi_check_trigger_arg_is(&cmd->start_arg, 0);
+	err |= comedi_check_trigger_arg_is(&cmd->scan_begin_arg, 0);
+	err |= comedi_check_trigger_arg_is(&cmd->convert_arg, 0);
+	err |= comedi_check_trigger_arg_is(&cmd->scan_end_arg,
+					   cmd->chanlist_len);
+	err |= comedi_check_trigger_arg_is(&cmd->stop_arg, 0);
 
 	if (err)
 		return 3;
diff --git a/drivers/staging/comedi/drivers/pcl812.c b/drivers/staging/comedi/drivers/pcl812.c
index 3ffb1ea2ecc8..03a3fd6cd918 100644
--- a/drivers/staging/comedi/drivers/pcl812.c
+++ b/drivers/staging/comedi/drivers/pcl812.c
@@ -117,8 +117,7 @@
 #include "../comedidev.h"
 
 #include "comedi_isadma.h"
-#include "comedi_fc.h"
-#include "8253.h"
+#include "comedi_8254.h"
 
 /* hardware types of the cards */
 #define boardPCL812PG	      0	/* and ACL-8112PG */
@@ -513,8 +512,6 @@ struct pcl812_private {
 	unsigned char mode_reg_int;	/*  there is stored INT number for some card */
 	unsigned int ai_poll_ptr;	/*  how many sampes transfer poll */
 	unsigned int max_812_ai_mode0_rangewait;	/*  setling time for gain */
-	unsigned int divisor1;
-	unsigned int divisor2;
 	unsigned int use_diff:1;
 	unsigned int use_mpc508:1;
 	unsigned int use_ext_trg:1;
@@ -522,21 +519,6 @@ struct pcl812_private {
 	unsigned int ai_eos:1;
 };
 
-static void pcl812_start_pacer(struct comedi_device *dev, bool load_timers)
-{
-	struct pcl812_private *devpriv = dev->private;
-	unsigned long timer_base = dev->iobase + PCL812_TIMER_BASE;
-
-	i8254_set_mode(timer_base, 0, 2, I8254_MODE2 | I8254_BINARY);
-	i8254_set_mode(timer_base, 0, 1, I8254_MODE2 | I8254_BINARY);
-	udelay(1);
-
-	if (load_timers) {
-		i8254_write(timer_base, 0, 2, devpriv->divisor2);
-		i8254_write(timer_base, 0, 1, devpriv->divisor1);
-	}
-}
-
 static void pcl812_ai_setup_dma(struct comedi_device *dev,
 				struct comedi_subdevice *s,
 				unsigned int unread_samples)
@@ -650,28 +632,27 @@ static int pcl812_ai_cmdtest(struct comedi_device *dev,
 	struct pcl812_private *devpriv = dev->private;
 	int err = 0;
 	unsigned int flags;
-	unsigned int arg;
 
 	/* Step 1 : check if triggers are trivially valid */
 
-	err |= cfc_check_trigger_src(&cmd->start_src, TRIG_NOW);
-	err |= cfc_check_trigger_src(&cmd->scan_begin_src, TRIG_FOLLOW);
+	err |= comedi_check_trigger_src(&cmd->start_src, TRIG_NOW);
+	err |= comedi_check_trigger_src(&cmd->scan_begin_src, TRIG_FOLLOW);
 
 	if (devpriv->use_ext_trg)
 		flags = TRIG_EXT;
 	else
 		flags = TRIG_TIMER;
-	err |= cfc_check_trigger_src(&cmd->convert_src, flags);
+	err |= comedi_check_trigger_src(&cmd->convert_src, flags);
 
-	err |= cfc_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
-	err |= cfc_check_trigger_src(&cmd->stop_src, TRIG_COUNT | TRIG_NONE);
+	err |= comedi_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
+	err |= comedi_check_trigger_src(&cmd->stop_src, TRIG_COUNT | TRIG_NONE);
 
 	if (err)
 		return 1;
 
 	/* Step 2a : make sure trigger sources are unique */
 
-	err |= cfc_check_trigger_is_unique(cmd->stop_src);
+	err |= comedi_check_trigger_is_unique(cmd->stop_src);
 
 	/* Step 2b : and mutually compatible */
 
@@ -680,22 +661,24 @@ static int pcl812_ai_cmdtest(struct comedi_device *dev,
 
 	/* Step 3: check if arguments are trivially valid */
 
-	err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
-	err |= cfc_check_trigger_arg_is(&cmd->scan_begin_arg, 0);
+	err |= comedi_check_trigger_arg_is(&cmd->start_arg, 0);
+	err |= comedi_check_trigger_arg_is(&cmd->scan_begin_arg, 0);
 
-	if (cmd->convert_src == TRIG_TIMER)
-		err |= cfc_check_trigger_arg_min(&cmd->convert_arg,
-						 board->ai_ns_min);
-	else	/* TRIG_EXT */
-		err |= cfc_check_trigger_arg_is(&cmd->convert_arg, 0);
+	if (cmd->convert_src == TRIG_TIMER) {
+		err |= comedi_check_trigger_arg_min(&cmd->convert_arg,
+						    board->ai_ns_min);
+	} else {	/* TRIG_EXT */
+		err |= comedi_check_trigger_arg_is(&cmd->convert_arg, 0);
+	}
 
-	err |= cfc_check_trigger_arg_min(&cmd->chanlist_len, 1);
-	err |= cfc_check_trigger_arg_is(&cmd->scan_end_arg, cmd->chanlist_len);
+	err |= comedi_check_trigger_arg_min(&cmd->chanlist_len, 1);
+	err |= comedi_check_trigger_arg_is(&cmd->scan_end_arg,
+					   cmd->chanlist_len);
 
 	if (cmd->stop_src == TRIG_COUNT)
-		err |= cfc_check_trigger_arg_min(&cmd->stop_arg, 1);
+		err |= comedi_check_trigger_arg_min(&cmd->stop_arg, 1);
 	else	/* TRIG_NONE */
-		err |= cfc_check_trigger_arg_is(&cmd->stop_arg, 0);
+		err |= comedi_check_trigger_arg_is(&cmd->stop_arg, 0);
 
 	if (err)
 		return 3;
@@ -703,12 +686,10 @@ static int pcl812_ai_cmdtest(struct comedi_device *dev,
 	/* step 4: fix up any arguments */
 
 	if (cmd->convert_src == TRIG_TIMER) {
-		arg = cmd->convert_arg;
-		i8253_cascade_ns_to_timer(I8254_OSC_BASE_2MHZ,
-					  &devpriv->divisor1,
-					  &devpriv->divisor2,
-					  &arg, cmd->flags);
-		err |= cfc_check_trigger_arg_is(&cmd->convert_arg, arg);
+		unsigned int arg = cmd->convert_arg;
+
+		comedi_8254_cascade_ns_to_timer(dev->pacer, &arg, cmd->flags);
+		err |= comedi_check_trigger_arg_is(&cmd->convert_arg, arg);
 	}
 
 	if (err)
@@ -725,8 +706,6 @@ static int pcl812_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	unsigned int ctrl = 0;
 	unsigned int i;
 
-	pcl812_start_pacer(dev, false);
-
 	pcl812_ai_set_chan_range(dev, cmd->chanlist[0], 1);
 
 	if (dma) {	/*  check if we can use DMA transfer */
@@ -760,7 +739,8 @@ static int pcl812_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 
 	switch (cmd->convert_src) {
 	case TRIG_TIMER:
-		pcl812_start_pacer(dev, true);
+		comedi_8254_update_divisors(dev->pacer);
+		comedi_8254_pacer_enable(dev->pacer, 1, 2, true);
 		break;
 	}
 
@@ -918,7 +898,7 @@ static int pcl812_ai_cancel(struct comedi_device *dev,
 
 	outb(devpriv->mode_reg_int | PCL812_CTRL_DISABLE_TRIG,
 	     dev->iobase + PCL812_CTRL_REG);
-	pcl812_start_pacer(dev, false);
+	comedi_8254_pacer_enable(dev->pacer, 1, 2, false);
 	pcl812_ai_clear_eoc(dev);
 	return 0;
 }
@@ -1010,10 +990,6 @@ static void pcl812_reset(struct comedi_device *dev)
 	     dev->iobase + PCL812_CTRL_REG);
 	pcl812_ai_clear_eoc(dev);
 
-	/* stop pacer */
-	if (board->IRQbits)
-		pcl812_start_pacer(dev, false);
-
 	/*
 	 * Invalidate last_ai_chanspec then set analog input to
 	 * known channel/range.
@@ -1162,11 +1138,19 @@ static int pcl812_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	if (ret)
 		return ret;
 
-	if ((1 << it->options[1]) & board->IRQbits) {
-		ret = request_irq(it->options[1], pcl812_interrupt, 0,
-				  dev->board_name, dev);
-		if (ret == 0)
-			dev->irq = it->options[1];
+	if (board->IRQbits) {
+		dev->pacer = comedi_8254_init(dev->iobase + PCL812_TIMER_BASE,
+					      I8254_OSC_BASE_2MHZ,
+					      I8254_IO8, 0);
+		if (!dev->pacer)
+			return -ENOMEM;
+
+		if ((1 << it->options[1]) & board->IRQbits) {
+			ret = request_irq(it->options[1], pcl812_interrupt, 0,
+					  dev->board_name, dev);
+			if (ret == 0)
+				dev->irq = it->options[1];
+		}
 	}
 
 	/* we need an IRQ to do DMA on channel 3 or 1 */
diff --git a/drivers/staging/comedi/drivers/pcl816.c b/drivers/staging/comedi/drivers/pcl816.c
index da35edfccbc3..1ccb2f19f4be 100644
--- a/drivers/staging/comedi/drivers/pcl816.c
+++ b/drivers/staging/comedi/drivers/pcl816.c
@@ -41,8 +41,7 @@ Configuration Options:
 #include "../comedidev.h"
 
 #include "comedi_isadma.h"
-#include "comedi_fc.h"
-#include "8253.h"
+#include "comedi_8254.h"
 
 /*
  * Register I/O map
@@ -116,31 +115,10 @@ static const struct pcl816_board boardtypes[] = {
 struct pcl816_private {
 	struct comedi_isadma *dma;
 	unsigned int ai_poll_ptr;	/*  how many sampes transfer poll */
-	unsigned int divisor1;
-	unsigned int divisor2;
 	unsigned int ai_cmd_running:1;
 	unsigned int ai_cmd_canceled:1;
 };
 
-static void pcl816_start_pacer(struct comedi_device *dev, bool load_counters)
-{
-	struct pcl816_private *devpriv = dev->private;
-	unsigned long timer_base = dev->iobase + PCL816_TIMER_BASE;
-
-	i8254_set_mode(timer_base, 0, 0, I8254_MODE1 | I8254_BINARY);
-	i8254_write(timer_base, 0, 0, 0x00ff);
-	udelay(1);
-
-	i8254_set_mode(timer_base, 0, 2, I8254_MODE2 | I8254_BINARY);
-	i8254_set_mode(timer_base, 0, 1, I8254_MODE2 | I8254_BINARY);
-	udelay(1);
-
-	if (load_counters) {
-		i8254_write(timer_base, 0, 2, devpriv->divisor2);
-		i8254_write(timer_base, 0, 1, devpriv->divisor1);
-	}
-}
-
 static void pcl816_ai_setup_dma(struct comedi_device *dev,
 				struct comedi_subdevice *s,
 				unsigned int unread_samples)
@@ -367,67 +345,62 @@ static int check_channel_list(struct comedi_device *dev,
 static int pcl816_ai_cmdtest(struct comedi_device *dev,
 			     struct comedi_subdevice *s, struct comedi_cmd *cmd)
 {
-	struct pcl816_private *devpriv = dev->private;
 	int err = 0;
-	unsigned int arg;
 
 	/* Step 1 : check if triggers are trivially valid */
 
-	err |= cfc_check_trigger_src(&cmd->start_src, TRIG_NOW);
-	err |= cfc_check_trigger_src(&cmd->scan_begin_src, TRIG_FOLLOW);
-	err |= cfc_check_trigger_src(&cmd->convert_src, TRIG_EXT | TRIG_TIMER);
-	err |= cfc_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
-	err |= cfc_check_trigger_src(&cmd->stop_src, TRIG_COUNT | TRIG_NONE);
+	err |= comedi_check_trigger_src(&cmd->start_src, TRIG_NOW);
+	err |= comedi_check_trigger_src(&cmd->scan_begin_src, TRIG_FOLLOW);
+	err |= comedi_check_trigger_src(&cmd->convert_src,
+					TRIG_EXT | TRIG_TIMER);
+	err |= comedi_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
+	err |= comedi_check_trigger_src(&cmd->stop_src, TRIG_COUNT | TRIG_NONE);
 
 	if (err)
 		return 1;
 
 	/* Step 2a : make sure trigger sources are unique */
 
-	err |= cfc_check_trigger_is_unique(cmd->convert_src);
-	err |= cfc_check_trigger_is_unique(cmd->stop_src);
+	err |= comedi_check_trigger_is_unique(cmd->convert_src);
+	err |= comedi_check_trigger_is_unique(cmd->stop_src);
 
 	/* Step 2b : and mutually compatible */
 
 	if (err)
 		return 2;
 
-
 	/* Step 3: check if arguments are trivially valid */
 
-	err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
-	err |= cfc_check_trigger_arg_is(&cmd->scan_begin_arg, 0);
+	err |= comedi_check_trigger_arg_is(&cmd->start_arg, 0);
+	err |= comedi_check_trigger_arg_is(&cmd->scan_begin_arg, 0);
 
 	if (cmd->convert_src == TRIG_TIMER)
-		err |= cfc_check_trigger_arg_min(&cmd->convert_arg, 10000);
+		err |= comedi_check_trigger_arg_min(&cmd->convert_arg, 10000);
 	else	/* TRIG_EXT */
-		err |= cfc_check_trigger_arg_is(&cmd->convert_arg, 0);
+		err |= comedi_check_trigger_arg_is(&cmd->convert_arg, 0);
 
-	err |= cfc_check_trigger_arg_is(&cmd->scan_end_arg, cmd->chanlist_len);
+	err |= comedi_check_trigger_arg_is(&cmd->scan_end_arg,
+					   cmd->chanlist_len);
 
 	if (cmd->stop_src == TRIG_COUNT)
-		err |= cfc_check_trigger_arg_min(&cmd->stop_arg, 1);
+		err |= comedi_check_trigger_arg_min(&cmd->stop_arg, 1);
 	else	/* TRIG_NONE */
-		err |= cfc_check_trigger_arg_is(&cmd->stop_arg, 0);
+		err |= comedi_check_trigger_arg_is(&cmd->stop_arg, 0);
 
 	if (err)
 		return 3;
 
-
 	/* step 4: fix up any arguments */
 	if (cmd->convert_src == TRIG_TIMER) {
-		arg = cmd->convert_arg;
-		i8253_cascade_ns_to_timer(I8254_OSC_BASE_10MHZ,
-					  &devpriv->divisor1,
-					  &devpriv->divisor2,
-					  &arg, cmd->flags);
-		err |= cfc_check_trigger_arg_is(&cmd->convert_arg, arg);
+		unsigned int arg = cmd->convert_arg;
+
+		comedi_8254_cascade_ns_to_timer(dev->pacer, &arg, cmd->flags);
+		err |= comedi_check_trigger_arg_is(&cmd->convert_arg, arg);
 	}
 
 	if (err)
 		return 4;
 
-
 	/* step 5: complain about special chanlist considerations */
 
 	if (cmd->chanlist) {
@@ -450,8 +423,6 @@ static int pcl816_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	if (devpriv->ai_cmd_running)
 		return -EBUSY;
 
-	pcl816_start_pacer(dev, false);
-
 	seglen = check_channel_list(dev, s, cmd->chanlist, cmd->chanlist_len);
 	if (seglen < 1)
 		return -EINVAL;
@@ -466,7 +437,11 @@ static int pcl816_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	dma->cur_dma = 0;
 	pcl816_ai_setup_dma(dev, s, 0);
 
-	pcl816_start_pacer(dev, true);
+	comedi_8254_set_mode(dev->pacer, 0, I8254_MODE1 | I8254_BINARY);
+	comedi_8254_write(dev->pacer, 0, 0x0ff);
+	udelay(1);
+	comedi_8254_update_divisors(dev->pacer);
+	comedi_8254_pacer_enable(dev->pacer, 1, 2, true);
 
 	ctrl = PCL816_CTRL_INTEN | PCL816_CTRL_DMAEN | PCL816_CTRL_DMASRC_SLOT0;
 	if (cmd->convert_src == TRIG_TIMER)
@@ -525,11 +500,7 @@ static int pcl816_ai_cancel(struct comedi_device *dev,
 	outb(PCL816_CTRL_DISABLE_TRIG, dev->iobase + PCL816_CTRL_REG);
 	pcl816_ai_clear_eoc(dev);
 
-	/* Stop pacer */
-	i8254_set_mode(dev->iobase + PCL816_TIMER_BASE, 0,
-			2, I8254_MODE0 | I8254_BINARY);
-	i8254_set_mode(dev->iobase + PCL816_TIMER_BASE, 0,
-			1, I8254_MODE0 | I8254_BINARY);
+	comedi_8254_pacer_enable(dev->pacer, 1, 2, false);
 
 	devpriv->ai_cmd_running = 0;
 	devpriv->ai_cmd_canceled = 1;
@@ -596,17 +567,10 @@ static int pcl816_do_insn_bits(struct comedi_device *dev,
 
 static void pcl816_reset(struct comedi_device *dev)
 {
-	unsigned long timer_base = dev->iobase + PCL816_TIMER_BASE;
-
 	outb(PCL816_CTRL_DISABLE_TRIG, dev->iobase + PCL816_CTRL_REG);
 	pcl816_ai_set_chan_range(dev, 0, 0);
 	pcl816_ai_clear_eoc(dev);
 
-	/* Stop pacer */
-	i8254_set_mode(timer_base, 0, 2, I8254_MODE0 | I8254_BINARY);
-	i8254_set_mode(timer_base, 0, 1, I8254_MODE0 | I8254_BINARY);
-	i8254_set_mode(timer_base, 0, 0, I8254_MODE0 | I8254_BINARY);
-
 	/* set all digital outputs low */
 	outb(0, dev->iobase + PCL816_DO_DI_LSB_REG);
 	outb(0, dev->iobase + PCL816_DO_DI_MSB_REG);
@@ -662,6 +626,11 @@ static int pcl816_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	/* an IRQ and DMA are required to support async commands */
 	pcl816_alloc_irq_and_dma(dev, it);
 
+	dev->pacer = comedi_8254_init(dev->iobase + PCL816_TIMER_BASE,
+				      I8254_OSC_BASE_10MHZ, I8254_IO8, 0);
+	if (!dev->pacer)
+		return -ENOMEM;
+
 	ret = comedi_alloc_subdevices(dev, 4);
 	if (ret)
 		return ret;
diff --git a/drivers/staging/comedi/drivers/pcl818.c b/drivers/staging/comedi/drivers/pcl818.c
index 7e4cdea5fe59..e1bdde977302 100644
--- a/drivers/staging/comedi/drivers/pcl818.c
+++ b/drivers/staging/comedi/drivers/pcl818.c
@@ -100,8 +100,7 @@
 #include "../comedidev.h"
 
 #include "comedi_isadma.h"
-#include "comedi_fc.h"
-#include "8253.h"
+#include "comedi_8254.h"
 
 /* boards constants */
 
@@ -299,33 +298,15 @@ struct pcl818_private {
 	struct comedi_isadma *dma;
 	/*  manimal allowed delay between samples (in us) for actual card */
 	unsigned int ns_min;
-	int i8253_osc_base;	/*  1/frequency of on board oscilator in ns */
 	/*  MUX setting for actual AI operations */
 	unsigned int act_chanlist[16];
 	unsigned int act_chanlist_len;	/*  how long is actual MUX list */
 	unsigned int act_chanlist_pos;	/*  actual position in MUX list */
-	unsigned int divisor1;
-	unsigned int divisor2;
 	unsigned int usefifo:1;
 	unsigned int ai_cmd_running:1;
 	unsigned int ai_cmd_canceled:1;
 };
 
-static void pcl818_start_pacer(struct comedi_device *dev, bool load_counters)
-{
-	struct pcl818_private *devpriv = dev->private;
-	unsigned long timer_base = dev->iobase + PCL818_TIMER_BASE;
-
-	i8254_set_mode(timer_base, 0, 2, I8254_MODE2 | I8254_BINARY);
-	i8254_set_mode(timer_base, 0, 1, I8254_MODE2 | I8254_BINARY);
-	udelay(1);
-
-	if (load_counters) {
-		i8254_write(timer_base, 0, 2, devpriv->divisor2);
-		i8254_write(timer_base, 0, 1, devpriv->divisor1);
-	}
-}
-
 static void pcl818_ai_setup_dma(struct comedi_device *dev,
 				struct comedi_subdevice *s,
 				unsigned int unread_samples)
@@ -663,25 +644,24 @@ static int ai_cmdtest(struct comedi_device *dev, struct comedi_subdevice *s,
 		      struct comedi_cmd *cmd)
 {
 	const struct pcl818_board *board = dev->board_ptr;
-	struct pcl818_private *devpriv = dev->private;
 	int err = 0;
-	unsigned int arg;
 
 	/* Step 1 : check if triggers are trivially valid */
 
-	err |= cfc_check_trigger_src(&cmd->start_src, TRIG_NOW);
-	err |= cfc_check_trigger_src(&cmd->scan_begin_src, TRIG_FOLLOW);
-	err |= cfc_check_trigger_src(&cmd->convert_src, TRIG_TIMER | TRIG_EXT);
-	err |= cfc_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
-	err |= cfc_check_trigger_src(&cmd->stop_src, TRIG_COUNT | TRIG_NONE);
+	err |= comedi_check_trigger_src(&cmd->start_src, TRIG_NOW);
+	err |= comedi_check_trigger_src(&cmd->scan_begin_src, TRIG_FOLLOW);
+	err |= comedi_check_trigger_src(&cmd->convert_src,
+					TRIG_TIMER | TRIG_EXT);
+	err |= comedi_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
+	err |= comedi_check_trigger_src(&cmd->stop_src, TRIG_COUNT | TRIG_NONE);
 
 	if (err)
 		return 1;
 
 	/* Step 2a : make sure trigger sources are unique */
 
-	err |= cfc_check_trigger_is_unique(cmd->convert_src);
-	err |= cfc_check_trigger_is_unique(cmd->stop_src);
+	err |= comedi_check_trigger_is_unique(cmd->convert_src);
+	err |= comedi_check_trigger_is_unique(cmd->stop_src);
 
 	/* Step 2b : and mutually compatible */
 
@@ -690,21 +670,23 @@ static int ai_cmdtest(struct comedi_device *dev, struct comedi_subdevice *s,
 
 	/* Step 3: check if arguments are trivially valid */
 
-	err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
-	err |= cfc_check_trigger_arg_is(&cmd->scan_begin_arg, 0);
+	err |= comedi_check_trigger_arg_is(&cmd->start_arg, 0);
+	err |= comedi_check_trigger_arg_is(&cmd->scan_begin_arg, 0);
 
-	if (cmd->convert_src == TRIG_TIMER)
-		err |= cfc_check_trigger_arg_min(&cmd->convert_arg,
-						 board->ns_min);
-	else	/* TRIG_EXT */
-		err |= cfc_check_trigger_arg_is(&cmd->convert_arg, 0);
+	if (cmd->convert_src == TRIG_TIMER) {
+		err |= comedi_check_trigger_arg_min(&cmd->convert_arg,
+						    board->ns_min);
+	} else {	/* TRIG_EXT */
+		err |= comedi_check_trigger_arg_is(&cmd->convert_arg, 0);
+	}
 
-	err |= cfc_check_trigger_arg_is(&cmd->scan_end_arg, cmd->chanlist_len);
+	err |= comedi_check_trigger_arg_is(&cmd->scan_end_arg,
+					   cmd->chanlist_len);
 
 	if (cmd->stop_src == TRIG_COUNT)
-		err |= cfc_check_trigger_arg_min(&cmd->stop_arg, 1);
+		err |= comedi_check_trigger_arg_min(&cmd->stop_arg, 1);
 	else	/* TRIG_NONE */
-		err |= cfc_check_trigger_arg_is(&cmd->stop_arg, 0);
+		err |= comedi_check_trigger_arg_is(&cmd->stop_arg, 0);
 
 	if (err)
 		return 3;
@@ -712,12 +694,10 @@ static int ai_cmdtest(struct comedi_device *dev, struct comedi_subdevice *s,
 	/* step 4: fix up any arguments */
 
 	if (cmd->convert_src == TRIG_TIMER) {
-		arg = cmd->convert_arg;
-		i8253_cascade_ns_to_timer(devpriv->i8253_osc_base,
-					  &devpriv->divisor1,
-					  &devpriv->divisor2,
-					  &arg, cmd->flags);
-		err |= cfc_check_trigger_arg_is(&cmd->convert_arg, arg);
+		unsigned int arg = cmd->convert_arg;
+
+		comedi_8254_cascade_ns_to_timer(dev->pacer, &arg, cmd->flags);
+		err |= comedi_check_trigger_arg_is(&cmd->convert_arg, arg);
 	}
 
 	if (err)
@@ -746,8 +726,6 @@ static int pcl818_ai_cmd(struct comedi_device *dev,
 	if (devpriv->ai_cmd_running)
 		return -EBUSY;
 
-	pcl818_start_pacer(dev, false);
-
 	seglen = check_channel_list(dev, s, cmd->chanlist, cmd->chanlist_len);
 	if (seglen < 1)
 		return -EINVAL;
@@ -779,8 +757,10 @@ static int pcl818_ai_cmd(struct comedi_device *dev,
 	}
 	outb(ctrl, dev->iobase + PCL818_CTRL_REG);
 
-	if (cmd->convert_src == TRIG_TIMER)
-		pcl818_start_pacer(dev, true);
+	if (cmd->convert_src == TRIG_TIMER) {
+		comedi_8254_update_divisors(dev->pacer);
+		comedi_8254_pacer_enable(dev->pacer, 1, 2, true);
+	}
 
 	return 0;
 }
@@ -812,7 +792,7 @@ static int pcl818_ai_cancel(struct comedi_device *dev,
 	}
 
 	outb(PCL818_CTRL_DISABLE_TRIG, dev->iobase + PCL818_CTRL_REG);
-	pcl818_start_pacer(dev, false);
+	comedi_8254_pacer_enable(dev->pacer, 1, 2, false);
 	pcl818_ai_clear_eoc(dev);
 
 	if (devpriv->usefifo) {	/*  FIFO shutdown */
@@ -906,7 +886,6 @@ static int pcl818_do_insn_bits(struct comedi_device *dev,
 static void pcl818_reset(struct comedi_device *dev)
 {
 	const struct pcl818_board *board = dev->board_ptr;
-	unsigned long timer_base = dev->iobase + PCL818_TIMER_BASE;
 	unsigned int chan;
 
 	/* flush and disable the FIFO */
@@ -924,9 +903,6 @@ static void pcl818_reset(struct comedi_device *dev)
 
 	/* stop pacer */
 	outb(PCL818_CNTENABLE_PACER_ENA, dev->iobase + PCL818_CNTENABLE_REG);
-	i8254_set_mode(timer_base, 0, 2, I8254_MODE0 | I8254_BINARY);
-	i8254_set_mode(timer_base, 0, 1, I8254_MODE0 | I8254_BINARY);
-	i8254_set_mode(timer_base, 0, 0, I8254_MODE0 | I8254_BINARY);
 
 	/* set analog output channels to 0V */
 	for (chan = 0; chan < board->n_aochan; chan++) {
@@ -1016,6 +992,7 @@ static int pcl818_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	const struct pcl818_board *board = dev->board_ptr;
 	struct pcl818_private *devpriv;
 	struct comedi_subdevice *s;
+	unsigned int osc_base;
 	int ret;
 
 	devpriv = comedi_alloc_devpriv(dev, sizeof(*devpriv));
@@ -1043,6 +1020,25 @@ static int pcl818_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	if (dev->irq && board->has_dma)
 		pcl818_alloc_dma(dev, it->options[2]);
 
+	/* use 1MHz or 10MHz oscilator */
+	if ((it->options[3] == 0) || (it->options[3] == 10))
+		osc_base = I8254_OSC_BASE_10MHZ;
+	else
+		osc_base = I8254_OSC_BASE_1MHZ;
+
+	dev->pacer = comedi_8254_init(dev->iobase + PCL818_TIMER_BASE,
+				      osc_base, I8254_IO8, 0);
+	if (!dev->pacer)
+		return -ENOMEM;
+
+	/* max sampling speed */
+	devpriv->ns_min = board->ns_min;
+	if (!board->is_818) {
+		/* extended PCL718 to 100kHz DAC */
+		if ((it->options[6] == 1) || (it->options[6] == 100))
+			devpriv->ns_min = 10000;
+	}
+
 	ret = comedi_alloc_subdevices(dev, 4);
 	if (ret)
 		return ret;
@@ -1117,22 +1113,6 @@ static int pcl818_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	s->range_table	= &range_digital;
 	s->insn_bits	= pcl818_do_insn_bits;
 
-	/* select 1/10MHz oscilator */
-	if ((it->options[3] == 0) || (it->options[3] == 10))
-		devpriv->i8253_osc_base = I8254_OSC_BASE_10MHZ;
-	else
-		devpriv->i8253_osc_base = I8254_OSC_BASE_1MHZ;
-
-	/* max sampling speed */
-	devpriv->ns_min = board->ns_min;
-
-	if (!board->is_818) {
-		if ((it->options[6] == 1) || (it->options[6] == 100)) {
-			/* extended PCL718 to 100kHz DAC */
-			devpriv->ns_min = 10000;
-		}
-	}
-
 	pcl818_reset(dev);
 
 	return 0;
diff --git a/drivers/staging/comedi/drivers/pcmmio.c b/drivers/staging/comedi/drivers/pcmmio.c
index 2c0e7ecbf494..10472e6dd002 100644
--- a/drivers/staging/comedi/drivers/pcmmio.c
+++ b/drivers/staging/comedi/drivers/pcmmio.c
@@ -78,8 +78,6 @@
 
 #include "../comedidev.h"
 
-#include "comedi_fc.h"
-
 /*
  * Register I/O map
  */
@@ -483,19 +481,19 @@ static int pcmmio_cmdtest(struct comedi_device *dev,
 
 	/* Step 1 : check if triggers are trivially valid */
 
-	err |= cfc_check_trigger_src(&cmd->start_src, TRIG_NOW | TRIG_INT);
-	err |= cfc_check_trigger_src(&cmd->scan_begin_src, TRIG_EXT);
-	err |= cfc_check_trigger_src(&cmd->convert_src, TRIG_NOW);
-	err |= cfc_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
-	err |= cfc_check_trigger_src(&cmd->stop_src, TRIG_COUNT | TRIG_NONE);
+	err |= comedi_check_trigger_src(&cmd->start_src, TRIG_NOW | TRIG_INT);
+	err |= comedi_check_trigger_src(&cmd->scan_begin_src, TRIG_EXT);
+	err |= comedi_check_trigger_src(&cmd->convert_src, TRIG_NOW);
+	err |= comedi_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
+	err |= comedi_check_trigger_src(&cmd->stop_src, TRIG_COUNT | TRIG_NONE);
 
 	if (err)
 		return 1;
 
 	/* Step 2a : make sure trigger sources are unique */
 
-	err |= cfc_check_trigger_is_unique(cmd->start_src);
-	err |= cfc_check_trigger_is_unique(cmd->stop_src);
+	err |= comedi_check_trigger_is_unique(cmd->start_src);
+	err |= comedi_check_trigger_is_unique(cmd->stop_src);
 
 	/* Step 2b : and mutually compatible */
 
@@ -504,15 +502,16 @@ static int pcmmio_cmdtest(struct comedi_device *dev,
 
 	/* Step 3: check if arguments are trivially valid */
 
-	err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
-	err |= cfc_check_trigger_arg_is(&cmd->scan_begin_arg, 0);
-	err |= cfc_check_trigger_arg_is(&cmd->convert_arg, 0);
-	err |= cfc_check_trigger_arg_is(&cmd->scan_end_arg, cmd->chanlist_len);
+	err |= comedi_check_trigger_arg_is(&cmd->start_arg, 0);
+	err |= comedi_check_trigger_arg_is(&cmd->scan_begin_arg, 0);
+	err |= comedi_check_trigger_arg_is(&cmd->convert_arg, 0);
+	err |= comedi_check_trigger_arg_is(&cmd->scan_end_arg,
+					   cmd->chanlist_len);
 
 	if (cmd->stop_src == TRIG_COUNT)
-		err |= cfc_check_trigger_arg_min(&cmd->stop_arg, 1);
+		err |= comedi_check_trigger_arg_min(&cmd->stop_arg, 1);
 	else	/* TRIG_NONE */
-		err |= cfc_check_trigger_arg_is(&cmd->stop_arg, 0);
+		err |= comedi_check_trigger_arg_is(&cmd->stop_arg, 0);
 
 	if (err)
 		return 3;
diff --git a/drivers/staging/comedi/drivers/pcmuio.c b/drivers/staging/comedi/drivers/pcmuio.c
index a1641d981812..7ea813022ff6 100644
--- a/drivers/staging/comedi/drivers/pcmuio.c
+++ b/drivers/staging/comedi/drivers/pcmuio.c
@@ -77,8 +77,6 @@
 
 #include "../comedidev.h"
 
-#include "comedi_fc.h"
-
 /*
  * Register I/O map
  *
@@ -486,19 +484,19 @@ static int pcmuio_cmdtest(struct comedi_device *dev,
 
 	/* Step 1 : check if triggers are trivially valid */
 
-	err |= cfc_check_trigger_src(&cmd->start_src, TRIG_NOW | TRIG_INT);
-	err |= cfc_check_trigger_src(&cmd->scan_begin_src, TRIG_EXT);
-	err |= cfc_check_trigger_src(&cmd->convert_src, TRIG_NOW);
-	err |= cfc_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
-	err |= cfc_check_trigger_src(&cmd->stop_src, TRIG_COUNT | TRIG_NONE);
+	err |= comedi_check_trigger_src(&cmd->start_src, TRIG_NOW | TRIG_INT);
+	err |= comedi_check_trigger_src(&cmd->scan_begin_src, TRIG_EXT);
+	err |= comedi_check_trigger_src(&cmd->convert_src, TRIG_NOW);
+	err |= comedi_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
+	err |= comedi_check_trigger_src(&cmd->stop_src, TRIG_COUNT | TRIG_NONE);
 
 	if (err)
 		return 1;
 
 	/* Step 2a : make sure trigger sources are unique */
 
-	err |= cfc_check_trigger_is_unique(cmd->start_src);
-	err |= cfc_check_trigger_is_unique(cmd->stop_src);
+	err |= comedi_check_trigger_is_unique(cmd->start_src);
+	err |= comedi_check_trigger_is_unique(cmd->stop_src);
 
 	/* Step 2b : and mutually compatible */
 
@@ -507,15 +505,16 @@ static int pcmuio_cmdtest(struct comedi_device *dev,
 
 	/* Step 3: check if arguments are trivially valid */
 
-	err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
-	err |= cfc_check_trigger_arg_is(&cmd->scan_begin_arg, 0);
-	err |= cfc_check_trigger_arg_is(&cmd->convert_arg, 0);
-	err |= cfc_check_trigger_arg_is(&cmd->scan_end_arg, cmd->chanlist_len);
+	err |= comedi_check_trigger_arg_is(&cmd->start_arg, 0);
+	err |= comedi_check_trigger_arg_is(&cmd->scan_begin_arg, 0);
+	err |= comedi_check_trigger_arg_is(&cmd->convert_arg, 0);
+	err |= comedi_check_trigger_arg_is(&cmd->scan_end_arg,
+					   cmd->chanlist_len);
 
 	if (cmd->stop_src == TRIG_COUNT)
-		err |= cfc_check_trigger_arg_min(&cmd->stop_arg, 1);
+		err |= comedi_check_trigger_arg_min(&cmd->stop_arg, 1);
 	else	/* TRIG_NONE */
-		err |= cfc_check_trigger_arg_is(&cmd->stop_arg, 0);
+		err |= comedi_check_trigger_arg_is(&cmd->stop_arg, 0);
 
 	if (err)
 		return 3;
@@ -568,7 +567,7 @@ static int pcmuio_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 		} else if (it->options[2]) {
 			/* request the irq for the 2nd asic */
 			ret = request_irq(it->options[2], pcmuio_interrupt, 0,
-					dev->board_name, dev);
+					  dev->board_name, dev);
 			if (ret == 0)
 				devpriv->irq2 = it->options[2];
 		}
@@ -588,7 +587,7 @@ static int pcmuio_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 		s->insn_bits	= pcmuio_dio_insn_bits;
 		s->insn_config	= pcmuio_dio_insn_config;
 
-		/* subdevices 0 and 2 can suppport interrupts */
+		/* subdevices 0 and 2 can support interrupts */
 		if ((i == 0 && dev->irq) || (i == 2 && devpriv->irq2)) {
 			/* setup the interrupt subdevice */
 			dev->read_subdev = s;
diff --git a/drivers/staging/comedi/drivers/quatech_daqp_cs.c b/drivers/staging/comedi/drivers/quatech_daqp_cs.c
index 8387fd0e4b7e..152cb146fc16 100644
--- a/drivers/staging/comedi/drivers/quatech_daqp_cs.c
+++ b/drivers/staging/comedi/drivers/quatech_daqp_cs.c
@@ -52,7 +52,6 @@ Devices: [Quatech] DAQP-208 (daqp), DAQP-308
 #include <linux/completion.h>
 
 #include "../comedi_pcmcia.h"
-#include "comedi_fc.h"
 
 struct daqp_private {
 	int stop;
@@ -315,7 +314,6 @@ static int daqp_ai_insn_read(struct comedi_device *dev,
 	devpriv->interrupt_mode = semaphore;
 
 	for (i = 0; i < insn->n; i++) {
-
 		/* Start conversion */
 		outb(DAQP_COMMAND_ARM | DAQP_COMMAND_FIFO_DATA,
 		     dev->iobase + DAQP_COMMAND);
@@ -366,22 +364,22 @@ static int daqp_ai_cmdtest(struct comedi_device *dev,
 
 	/* Step 1 : check if triggers are trivially valid */
 
-	err |= cfc_check_trigger_src(&cmd->start_src, TRIG_NOW);
-	err |= cfc_check_trigger_src(&cmd->scan_begin_src,
+	err |= comedi_check_trigger_src(&cmd->start_src, TRIG_NOW);
+	err |= comedi_check_trigger_src(&cmd->scan_begin_src,
 					TRIG_TIMER | TRIG_FOLLOW);
-	err |= cfc_check_trigger_src(&cmd->convert_src,
+	err |= comedi_check_trigger_src(&cmd->convert_src,
 					TRIG_TIMER | TRIG_NOW);
-	err |= cfc_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
-	err |= cfc_check_trigger_src(&cmd->stop_src, TRIG_COUNT | TRIG_NONE);
+	err |= comedi_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
+	err |= comedi_check_trigger_src(&cmd->stop_src, TRIG_COUNT | TRIG_NONE);
 
 	if (err)
 		return 1;
 
 	/* Step 2a : make sure trigger sources are unique */
 
-	err |= cfc_check_trigger_is_unique(cmd->scan_begin_src);
-	err |= cfc_check_trigger_is_unique(cmd->convert_src);
-	err |= cfc_check_trigger_is_unique(cmd->stop_src);
+	err |= comedi_check_trigger_is_unique(cmd->scan_begin_src);
+	err |= comedi_check_trigger_is_unique(cmd->convert_src);
+	err |= comedi_check_trigger_is_unique(cmd->stop_src);
 
 	/* Step 2b : and mutually compatible */
 
@@ -390,13 +388,14 @@ static int daqp_ai_cmdtest(struct comedi_device *dev,
 
 	/* Step 3: check if arguments are trivially valid */
 
-	err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
+	err |= comedi_check_trigger_arg_is(&cmd->start_arg, 0);
 
 #define MAX_SPEED	10000	/* 100 kHz - in nanoseconds */
 
-	if (cmd->scan_begin_src == TRIG_TIMER)
-		err |= cfc_check_trigger_arg_min(&cmd->scan_begin_arg,
-						 MAX_SPEED);
+	if (cmd->scan_begin_src == TRIG_TIMER) {
+		err |= comedi_check_trigger_arg_min(&cmd->scan_begin_arg,
+						    MAX_SPEED);
+	}
 
 	/* If both scan_begin and convert are both timer values, the only
 	 * way that can make sense is if the scan time is the number of
@@ -408,15 +407,18 @@ static int daqp_ai_cmdtest(struct comedi_device *dev,
 		err |= -EINVAL;
 	}
 
-	if (cmd->convert_src == TRIG_TIMER)
-		err |= cfc_check_trigger_arg_min(&cmd->convert_arg, MAX_SPEED);
+	if (cmd->convert_src == TRIG_TIMER) {
+		err |= comedi_check_trigger_arg_min(&cmd->convert_arg,
+						    MAX_SPEED);
+	}
 
-	err |= cfc_check_trigger_arg_is(&cmd->scan_end_arg, cmd->chanlist_len);
+	err |= comedi_check_trigger_arg_is(&cmd->scan_end_arg,
+					   cmd->chanlist_len);
 
 	if (cmd->stop_src == TRIG_COUNT)
-		err |= cfc_check_trigger_arg_max(&cmd->stop_arg, 0x00ffffff);
+		err |= comedi_check_trigger_arg_max(&cmd->stop_arg, 0x00ffffff);
 	else	/* TRIG_NONE */
-		err |= cfc_check_trigger_arg_is(&cmd->stop_arg, 0);
+		err |= comedi_check_trigger_arg_is(&cmd->stop_arg, 0);
 
 	if (err)
 		return 3;
@@ -426,13 +428,13 @@ static int daqp_ai_cmdtest(struct comedi_device *dev,
 	if (cmd->scan_begin_src == TRIG_TIMER) {
 		arg = cmd->scan_begin_arg;
 		daqp_ns_to_timer(&arg, cmd->flags);
-		err |= cfc_check_trigger_arg_is(&cmd->scan_begin_arg, arg);
+		err |= comedi_check_trigger_arg_is(&cmd->scan_begin_arg, arg);
 	}
 
 	if (cmd->convert_src == TRIG_TIMER) {
 		arg = cmd->convert_arg;
 		daqp_ns_to_timer(&arg, cmd->flags);
-		err |= cfc_check_trigger_arg_is(&cmd->convert_arg, arg);
+		err |= comedi_check_trigger_arg_is(&cmd->convert_arg, arg);
 	}
 
 	if (err)
diff --git a/drivers/staging/comedi/drivers/rtd520.c b/drivers/staging/comedi/drivers/rtd520.c
index c94ad12ed446..4c13f5eb0c84 100644
--- a/drivers/staging/comedi/drivers/rtd520.c
+++ b/drivers/staging/comedi/drivers/rtd520.c
@@ -94,13 +94,11 @@
  */
 
 #include <linux/module.h>
-#include <linux/pci.h>
 #include <linux/delay.h>
 #include <linux/interrupt.h>
 
-#include "../comedidev.h"
+#include "../comedi_pci.h"
 
-#include "comedi_fc.h"
 #include "plx9080.h"
 
 /*
@@ -579,7 +577,7 @@ static int rtd_ai_rinsn(struct comedi_device *dev,
 
 		/* read data */
 		d = readw(devpriv->las1 + LAS1_ADC_FIFO);
-		d = d >> 3;	/* low 3 bits are marker lines */
+		d >>= 3;	/* low 3 bits are marker lines */
 
 		/* convert bipolar data to comedi unsigned data */
 		if (comedi_range_is_bipolar(s, range))
@@ -616,7 +614,7 @@ static int ai_read_n(struct comedi_device *dev, struct comedi_subdevice *s,
 		}
 
 		d = readw(devpriv->las1 + LAS1_ADC_FIFO);
-		d = d >> 3;	/* low 3 bits are marker lines */
+		d >>= 3;	/* low 3 bits are marker lines */
 
 		/* convert bipolar data to comedi unsigned data */
 		if (comedi_range_is_bipolar(s, range))
@@ -734,21 +732,22 @@ static int rtd_ai_cmdtest(struct comedi_device *dev,
 
 	/* Step 1 : check if triggers are trivially valid */
 
-	err |= cfc_check_trigger_src(&cmd->start_src, TRIG_NOW);
-	err |= cfc_check_trigger_src(&cmd->scan_begin_src,
+	err |= comedi_check_trigger_src(&cmd->start_src, TRIG_NOW);
+	err |= comedi_check_trigger_src(&cmd->scan_begin_src,
 					TRIG_TIMER | TRIG_EXT);
-	err |= cfc_check_trigger_src(&cmd->convert_src, TRIG_TIMER | TRIG_EXT);
-	err |= cfc_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
-	err |= cfc_check_trigger_src(&cmd->stop_src, TRIG_COUNT | TRIG_NONE);
+	err |= comedi_check_trigger_src(&cmd->convert_src,
+					TRIG_TIMER | TRIG_EXT);
+	err |= comedi_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
+	err |= comedi_check_trigger_src(&cmd->stop_src, TRIG_COUNT | TRIG_NONE);
 
 	if (err)
 		return 1;
 
 	/* Step 2a : make sure trigger sources are unique */
 
-	err |= cfc_check_trigger_is_unique(cmd->scan_begin_src);
-	err |= cfc_check_trigger_is_unique(cmd->convert_src);
-	err |= cfc_check_trigger_is_unique(cmd->stop_src);
+	err |= comedi_check_trigger_is_unique(cmd->scan_begin_src);
+	err |= comedi_check_trigger_is_unique(cmd->convert_src);
+	err |= comedi_check_trigger_is_unique(cmd->stop_src);
 
 	/* Step 2b : and mutually compatible */
 
@@ -757,32 +756,32 @@ static int rtd_ai_cmdtest(struct comedi_device *dev,
 
 	/* Step 3: check if arguments are trivially valid */
 
-	err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
+	err |= comedi_check_trigger_arg_is(&cmd->start_arg, 0);
 
 	if (cmd->scan_begin_src == TRIG_TIMER) {
 		/* Note: these are time periods, not actual rates */
 		if (1 == cmd->chanlist_len) {	/* no scanning */
-			if (cfc_check_trigger_arg_min(&cmd->scan_begin_arg,
-						      RTD_MAX_SPEED_1)) {
+			if (comedi_check_trigger_arg_min(&cmd->scan_begin_arg,
+							 RTD_MAX_SPEED_1)) {
 				rtd_ns_to_timer(&cmd->scan_begin_arg,
 						CMDF_ROUND_UP);
 				err |= -EINVAL;
 			}
-			if (cfc_check_trigger_arg_max(&cmd->scan_begin_arg,
-						      RTD_MIN_SPEED_1)) {
+			if (comedi_check_trigger_arg_max(&cmd->scan_begin_arg,
+							 RTD_MIN_SPEED_1)) {
 				rtd_ns_to_timer(&cmd->scan_begin_arg,
 						CMDF_ROUND_DOWN);
 				err |= -EINVAL;
 			}
 		} else {
-			if (cfc_check_trigger_arg_min(&cmd->scan_begin_arg,
-						      RTD_MAX_SPEED)) {
+			if (comedi_check_trigger_arg_min(&cmd->scan_begin_arg,
+							 RTD_MAX_SPEED)) {
 				rtd_ns_to_timer(&cmd->scan_begin_arg,
 						CMDF_ROUND_UP);
 				err |= -EINVAL;
 			}
-			if (cfc_check_trigger_arg_max(&cmd->scan_begin_arg,
-						      RTD_MIN_SPEED)) {
+			if (comedi_check_trigger_arg_max(&cmd->scan_begin_arg,
+							 RTD_MIN_SPEED)) {
 				rtd_ns_to_timer(&cmd->scan_begin_arg,
 						CMDF_ROUND_DOWN);
 				err |= -EINVAL;
@@ -792,32 +791,32 @@ static int rtd_ai_cmdtest(struct comedi_device *dev,
 		/* external trigger */
 		/* should be level/edge, hi/lo specification here */
 		/* should specify multiple external triggers */
-		err |= cfc_check_trigger_arg_max(&cmd->scan_begin_arg, 9);
+		err |= comedi_check_trigger_arg_max(&cmd->scan_begin_arg, 9);
 	}
 
 	if (cmd->convert_src == TRIG_TIMER) {
 		if (1 == cmd->chanlist_len) {	/* no scanning */
-			if (cfc_check_trigger_arg_min(&cmd->convert_arg,
-						      RTD_MAX_SPEED_1)) {
+			if (comedi_check_trigger_arg_min(&cmd->convert_arg,
+							 RTD_MAX_SPEED_1)) {
 				rtd_ns_to_timer(&cmd->convert_arg,
 						CMDF_ROUND_UP);
 				err |= -EINVAL;
 			}
-			if (cfc_check_trigger_arg_max(&cmd->convert_arg,
-						      RTD_MIN_SPEED_1)) {
+			if (comedi_check_trigger_arg_max(&cmd->convert_arg,
+							 RTD_MIN_SPEED_1)) {
 				rtd_ns_to_timer(&cmd->convert_arg,
 						CMDF_ROUND_DOWN);
 				err |= -EINVAL;
 			}
 		} else {
-			if (cfc_check_trigger_arg_min(&cmd->convert_arg,
-						      RTD_MAX_SPEED)) {
+			if (comedi_check_trigger_arg_min(&cmd->convert_arg,
+							 RTD_MAX_SPEED)) {
 				rtd_ns_to_timer(&cmd->convert_arg,
 						CMDF_ROUND_UP);
 				err |= -EINVAL;
 			}
-			if (cfc_check_trigger_arg_max(&cmd->convert_arg,
-						      RTD_MIN_SPEED)) {
+			if (comedi_check_trigger_arg_max(&cmd->convert_arg,
+							 RTD_MIN_SPEED)) {
 				rtd_ns_to_timer(&cmd->convert_arg,
 						CMDF_ROUND_DOWN);
 				err |= -EINVAL;
@@ -826,37 +825,38 @@ static int rtd_ai_cmdtest(struct comedi_device *dev,
 	} else {
 		/* external trigger */
 		/* see above */
-		err |= cfc_check_trigger_arg_max(&cmd->convert_arg, 9);
+		err |= comedi_check_trigger_arg_max(&cmd->convert_arg, 9);
 	}
 
-	err |= cfc_check_trigger_arg_is(&cmd->scan_end_arg, cmd->chanlist_len);
+	err |= comedi_check_trigger_arg_is(&cmd->scan_end_arg,
+					   cmd->chanlist_len);
 
 	if (cmd->stop_src == TRIG_COUNT)
-		err |= cfc_check_trigger_arg_min(&cmd->stop_arg, 1);
+		err |= comedi_check_trigger_arg_min(&cmd->stop_arg, 1);
 	else	/* TRIG_NONE */
-		err |= cfc_check_trigger_arg_is(&cmd->stop_arg, 0);
+		err |= comedi_check_trigger_arg_is(&cmd->stop_arg, 0);
 
 	if (err)
 		return 3;
 
-
 	/* step 4: fix up any arguments */
 
 	if (cmd->scan_begin_src == TRIG_TIMER) {
 		arg = cmd->scan_begin_arg;
 		rtd_ns_to_timer(&arg, cmd->flags);
-		err |= cfc_check_trigger_arg_is(&cmd->scan_begin_arg, arg);
+		err |= comedi_check_trigger_arg_is(&cmd->scan_begin_arg, arg);
 	}
 
 	if (cmd->convert_src == TRIG_TIMER) {
 		arg = cmd->convert_arg;
 		rtd_ns_to_timer(&arg, cmd->flags);
-		err |= cfc_check_trigger_arg_is(&cmd->convert_arg, arg);
+		err |= comedi_check_trigger_arg_is(&cmd->convert_arg, arg);
 
 		if (cmd->scan_begin_src == TRIG_TIMER) {
 			arg = cmd->convert_arg * cmd->scan_end_arg;
-			err |= cfc_check_trigger_arg_min(&cmd->scan_begin_arg,
-							 arg);
+			err |= comedi_check_trigger_arg_min(&cmd->
+							    scan_begin_arg,
+							    arg);
 		}
 	}
 
@@ -1181,8 +1181,8 @@ static void rtd_pci_latency_quirk(struct comedi_device *dev,
 	pci_read_config_byte(pcidev, PCI_LATENCY_TIMER, &pci_latency);
 	if (pci_latency < 32) {
 		dev_info(dev->class_dev,
-			"PCI latency changed from %d to %d\n",
-			pci_latency, 32);
+			 "PCI latency changed from %d to %d\n",
+			 pci_latency, 32);
 		pci_write_config_byte(pcidev, PCI_LATENCY_TIMER, 32);
 	}
 }
@@ -1299,12 +1299,8 @@ static void rtd_detach(struct comedi_device *dev)
 		/* Shut down any board ops by resetting it */
 		if (dev->mmio && devpriv->lcfg)
 			rtd_reset(dev);
-		if (dev->irq) {
-			writel(readl(devpriv->lcfg + PLX_INTRCS_REG) &
-				~(ICS_PLIE | ICS_DMA0_E | ICS_DMA1_E),
-				devpriv->lcfg + PLX_INTRCS_REG);
+		if (dev->irq)
 			free_irq(dev->irq, dev);
-		}
 		if (dev->mmio)
 			iounmap(dev->mmio);
 		if (devpriv->las1)
diff --git a/drivers/staging/comedi/drivers/s626.c b/drivers/staging/comedi/drivers/s626.c
index fc497dd92021..781918d8d85f 100644
--- a/drivers/staging/comedi/drivers/s626.c
+++ b/drivers/staging/comedi/drivers/s626.c
@@ -61,14 +61,12 @@
 
 #include <linux/module.h>
 #include <linux/delay.h>
-#include <linux/pci.h>
 #include <linux/interrupt.h>
 #include <linux/kernel.h>
 #include <linux/types.h>
 
-#include "../comedidev.h"
+#include "../comedi_pci.h"
 
-#include "comedi_fc.h"
 #include "s626.h"
 
 struct s626_buffer_dma {
@@ -231,9 +229,9 @@ static void s626_debi_replace(struct comedi_device *dev, unsigned int addr,
 /* **************  EEPROM ACCESS FUNCTIONS  ************** */
 
 static int s626_i2c_handshake_eoc(struct comedi_device *dev,
-				 struct comedi_subdevice *s,
-				 struct comedi_insn *insn,
-				 unsigned long context)
+				  struct comedi_subdevice *s,
+				  struct comedi_insn *insn,
+				  unsigned long context)
 {
 	bool status;
 
@@ -294,7 +292,7 @@ static uint8_t s626_i2c_read(struct comedi_device *dev, uint8_t addr)
 	 *  Byte0 = Not sent.
 	 */
 	if (s626_i2c_handshake(dev, S626_I2C_B2(S626_I2C_ATTRSTART,
-					   (devpriv->i2c_adrs | 1)) |
+						(devpriv->i2c_adrs | 1)) |
 				    S626_I2C_B1(S626_I2C_ATTRSTOP, 0) |
 				    S626_I2C_B0(S626_I2C_ATTRNOP, 0)))
 		/* Abort function and declare error if handshake failed. */
@@ -517,8 +515,8 @@ static int s626_send_dac(struct comedi_device *dev, uint32_t val)
 /*
  * Private helper function: Write setpoint to an application DAC channel.
  */
-static int s626_set_dac(struct comedi_device *dev, uint16_t chan,
-			 int16_t dacdata)
+static int s626_set_dac(struct comedi_device *dev,
+			uint16_t chan, int16_t dacdata)
 {
 	struct s626_private *devpriv = dev->private;
 	uint16_t signmask;
@@ -583,8 +581,8 @@ static int s626_set_dac(struct comedi_device *dev, uint16_t chan,
 	return s626_send_dac(dev, val);
 }
 
-static int s626_write_trim_dac(struct comedi_device *dev, uint8_t logical_chan,
-				uint8_t dac_data)
+static int s626_write_trim_dac(struct comedi_device *dev,
+			       uint8_t logical_chan, uint8_t dac_data)
 {
 	struct s626_private *devpriv = dev->private;
 	uint32_t chan;
@@ -641,7 +639,7 @@ static int s626_load_trim_dacs(struct comedi_device *dev)
 	/* Copy TrimDac setpoint values from EEPROM to TrimDacs. */
 	for (i = 0; i < ARRAY_SIZE(s626_trimchan); i++) {
 		ret = s626_write_trim_dac(dev, i,
-				    s626_i2c_read(dev, s626_trimadrs[i]));
+					  s626_i2c_read(dev, s626_trimadrs[i]));
 		if (ret)
 			return ret;
 	}
@@ -1576,7 +1574,7 @@ static void s626_reset_adc(struct comedi_device *dev, uint8_t *ppl)
 	       dev->mmio + S626_P_RPSADDR1);
 
 	/* Construct RPS program in rps_buf DMA buffer */
-	if (cmd != NULL && cmd->scan_begin_src != TRIG_FOLLOW) {
+	if (cmd->scan_begin_src != TRIG_FOLLOW) {
 		/* Wait for Start trigger. */
 		*rps++ = S626_RPS_PAUSE | S626_RPS_SIGADC;
 		*rps++ = S626_RPS_CLRSIGNAL | S626_RPS_SIGADC;
@@ -1665,7 +1663,7 @@ static void s626_reset_adc(struct comedi_device *dev, uint8_t *ppl)
 			*rps++ = jmp_adrs;
 		}
 
-		if (cmd != NULL && cmd->convert_src != TRIG_NOW) {
+		if (cmd->convert_src != TRIG_NOW) {
 			/* Wait for Start trigger. */
 			*rps++ = S626_RPS_PAUSE | S626_RPS_SIGADC;
 			*rps++ = S626_RPS_CLRSIGNAL | S626_RPS_SIGADC;
@@ -2034,10 +2032,6 @@ static int s626_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	/* reset ai_cmd_running flag */
 	devpriv->ai_cmd_running = 0;
 
-	/* test if cmd is valid */
-	if (cmd == NULL)
-		return -EINVAL;
-
 	s626_ai_load_polllist(ppl, cmd);
 	devpriv->ai_cmd_running = 1;
 	devpriv->ai_convert_count = 0;
@@ -2120,24 +2114,24 @@ static int s626_ai_cmdtest(struct comedi_device *dev,
 
 	/* Step 1 : check if triggers are trivially valid */
 
-	err |= cfc_check_trigger_src(&cmd->start_src,
-				     TRIG_NOW | TRIG_INT | TRIG_EXT);
-	err |= cfc_check_trigger_src(&cmd->scan_begin_src,
-				     TRIG_TIMER | TRIG_EXT | TRIG_FOLLOW);
-	err |= cfc_check_trigger_src(&cmd->convert_src,
-				     TRIG_TIMER | TRIG_EXT | TRIG_NOW);
-	err |= cfc_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
-	err |= cfc_check_trigger_src(&cmd->stop_src, TRIG_COUNT | TRIG_NONE);
+	err |= comedi_check_trigger_src(&cmd->start_src,
+					TRIG_NOW | TRIG_INT | TRIG_EXT);
+	err |= comedi_check_trigger_src(&cmd->scan_begin_src,
+					TRIG_TIMER | TRIG_EXT | TRIG_FOLLOW);
+	err |= comedi_check_trigger_src(&cmd->convert_src,
+					TRIG_TIMER | TRIG_EXT | TRIG_NOW);
+	err |= comedi_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
+	err |= comedi_check_trigger_src(&cmd->stop_src, TRIG_COUNT | TRIG_NONE);
 
 	if (err)
 		return 1;
 
 	/* Step 2a : make sure trigger sources are unique */
 
-	err |= cfc_check_trigger_is_unique(cmd->start_src);
-	err |= cfc_check_trigger_is_unique(cmd->scan_begin_src);
-	err |= cfc_check_trigger_is_unique(cmd->convert_src);
-	err |= cfc_check_trigger_is_unique(cmd->stop_src);
+	err |= comedi_check_trigger_is_unique(cmd->start_src);
+	err |= comedi_check_trigger_is_unique(cmd->scan_begin_src);
+	err |= comedi_check_trigger_is_unique(cmd->convert_src);
+	err |= comedi_check_trigger_is_unique(cmd->stop_src);
 
 	/* Step 2b : and mutually compatible */
 
@@ -2149,49 +2143,53 @@ static int s626_ai_cmdtest(struct comedi_device *dev,
 	switch (cmd->start_src) {
 	case TRIG_NOW:
 	case TRIG_INT:
-		err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
+		err |= comedi_check_trigger_arg_is(&cmd->start_arg, 0);
 		break;
 	case TRIG_EXT:
-		err |= cfc_check_trigger_arg_max(&cmd->start_arg, 39);
+		err |= comedi_check_trigger_arg_max(&cmd->start_arg, 39);
 		break;
 	}
 
 	if (cmd->scan_begin_src == TRIG_EXT)
-		err |= cfc_check_trigger_arg_max(&cmd->scan_begin_arg, 39);
+		err |= comedi_check_trigger_arg_max(&cmd->scan_begin_arg, 39);
 	if (cmd->convert_src == TRIG_EXT)
-		err |= cfc_check_trigger_arg_max(&cmd->convert_arg, 39);
+		err |= comedi_check_trigger_arg_max(&cmd->convert_arg, 39);
 
 #define S626_MAX_SPEED	200000	/* in nanoseconds */
 #define S626_MIN_SPEED	2000000000	/* in nanoseconds */
 
 	if (cmd->scan_begin_src == TRIG_TIMER) {
-		err |= cfc_check_trigger_arg_min(&cmd->scan_begin_arg,
-						 S626_MAX_SPEED);
-		err |= cfc_check_trigger_arg_max(&cmd->scan_begin_arg,
-						 S626_MIN_SPEED);
+		err |= comedi_check_trigger_arg_min(&cmd->scan_begin_arg,
+						    S626_MAX_SPEED);
+		err |= comedi_check_trigger_arg_max(&cmd->scan_begin_arg,
+						    S626_MIN_SPEED);
 	} else {
-		/* external trigger */
-		/* should be level/edge, hi/lo specification here */
-		/* should specify multiple external triggers */
-		/* err |= cfc_check_trigger_arg_max(&cmd->scan_begin_arg, 9); */
+		/*
+		 * external trigger
+		 * should be level/edge, hi/lo specification here
+		 * should specify multiple external triggers
+		 * err |= comedi_check_trigger_arg_max(&cmd->scan_begin_arg, 9);
+		 */
 	}
 	if (cmd->convert_src == TRIG_TIMER) {
-		err |= cfc_check_trigger_arg_min(&cmd->convert_arg,
-						 S626_MAX_SPEED);
-		err |= cfc_check_trigger_arg_max(&cmd->convert_arg,
-						 S626_MIN_SPEED);
+		err |= comedi_check_trigger_arg_min(&cmd->convert_arg,
+						    S626_MAX_SPEED);
+		err |= comedi_check_trigger_arg_max(&cmd->convert_arg,
+						    S626_MIN_SPEED);
 	} else {
-		/* external trigger */
-		/* see above */
-		/* err |= cfc_check_trigger_arg_max(&cmd->scan_begin_arg, 9); */
+		/*
+		 * external trigger - see above
+		 * err |= comedi_check_trigger_arg_max(&cmd->scan_begin_arg, 9);
+		 */
 	}
 
-	err |= cfc_check_trigger_arg_is(&cmd->scan_end_arg, cmd->chanlist_len);
+	err |= comedi_check_trigger_arg_is(&cmd->scan_end_arg,
+					   cmd->chanlist_len);
 
 	if (cmd->stop_src == TRIG_COUNT)
-		err |= cfc_check_trigger_arg_min(&cmd->stop_arg, 1);
+		err |= comedi_check_trigger_arg_min(&cmd->stop_arg, 1);
 	else	/* TRIG_NONE */
-		err |= cfc_check_trigger_arg_is(&cmd->stop_arg, 0);
+		err |= comedi_check_trigger_arg_is(&cmd->stop_arg, 0);
 
 	if (err)
 		return 3;
@@ -2201,18 +2199,19 @@ static int s626_ai_cmdtest(struct comedi_device *dev,
 	if (cmd->scan_begin_src == TRIG_TIMER) {
 		arg = cmd->scan_begin_arg;
 		s626_ns_to_timer(&arg, cmd->flags);
-		err |= cfc_check_trigger_arg_is(&cmd->scan_begin_arg, arg);
+		err |= comedi_check_trigger_arg_is(&cmd->scan_begin_arg, arg);
 	}
 
 	if (cmd->convert_src == TRIG_TIMER) {
 		arg = cmd->convert_arg;
 		s626_ns_to_timer(&arg, cmd->flags);
-		err |= cfc_check_trigger_arg_is(&cmd->convert_arg, arg);
+		err |= comedi_check_trigger_arg_is(&cmd->convert_arg, arg);
 
 		if (cmd->scan_begin_src == TRIG_TIMER) {
 			arg = cmd->convert_arg * cmd->scan_end_arg;
-			err |= cfc_check_trigger_arg_min(&cmd->scan_begin_arg,
-							 arg);
+			err |= comedi_check_trigger_arg_min(&cmd->
+							    scan_begin_arg,
+							    arg);
 		}
 	}
 
@@ -2733,7 +2732,7 @@ static int s626_initialize(struct comedi_device *dev)
 }
 
 static int s626_auto_attach(struct comedi_device *dev,
-				      unsigned long context_unused)
+			    unsigned long context_unused)
 {
 	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
 	struct s626_private *devpriv;
diff --git a/drivers/staging/comedi/drivers/serial2002.c b/drivers/staging/comedi/drivers/serial2002.c
index 71226ee9064e..ad35ed6e93f0 100644
--- a/drivers/staging/comedi/drivers/serial2002.c
+++ b/drivers/staging/comedi/drivers/serial2002.c
@@ -39,14 +39,12 @@ Status: in development
 #include <linux/poll.h>
 
 struct serial2002_range_table_t {
-
 	/*  HACK... */
 	int length;
 	struct comedi_krange range;
 };
 
 struct serial2002_private {
-
 	int port;		/*  /dev/ttyS<port> */
 	int speed;		/*  baudrate */
 	struct file *tty;
@@ -145,8 +143,8 @@ static void serial2002_tty_read_poll_wait(struct file *f, int timeout)
 			break;
 		}
 		do_gettimeofday(&now);
-		elapsed = (1000000 * (now.tv_sec - start.tv_sec) +
-			  now.tv_usec - start.tv_usec);
+		elapsed = 1000000 * (now.tv_sec - start.tv_sec) +
+			  now.tv_usec - start.tv_usec;
 		if (elapsed > timeout)
 			break;
 		set_current_state(TASK_INTERRUPTIBLE);
@@ -300,7 +298,6 @@ static struct serial_data serial2002_read(struct file *f, int timeout)
 		}
 	}
 	return result;
-
 }
 
 static void serial2002_write(struct file *f, struct serial_data data)
diff --git a/drivers/staging/comedi/drivers/ssv_dnp.c b/drivers/staging/comedi/drivers/ssv_dnp.c
index 848c30801580..acc7f3445c58 100644
--- a/drivers/staging/comedi/drivers/ssv_dnp.c
+++ b/drivers/staging/comedi/drivers/ssv_dnp.c
@@ -127,7 +127,6 @@ static int dnp_dio_insn_config(struct comedi_device *dev,
 	outb(val, CSCDR);
 
 	return insn->n;
-
 }
 
 static int dnp_attach(struct comedi_device *dev, struct comedi_devconfig *it)
diff --git a/drivers/staging/comedi/drivers/unioxx5.c b/drivers/staging/comedi/drivers/unioxx5.c
index 7c2276a086ac..51498b889c6c 100644
--- a/drivers/staging/comedi/drivers/unioxx5.c
+++ b/drivers/staging/comedi/drivers/unioxx5.c
@@ -38,7 +38,6 @@ Devices: [Fastwel] UNIOxx-5 (unioxx5),
 
 */
 
-
 #include <linux/module.h>
 #include <linux/delay.h>
 #include "../comedidev.h"
@@ -81,7 +80,6 @@ struct unioxx5_subd_priv {
 
 static int __unioxx5_define_chan_offset(int chan_num)
 {
-
 	if (chan_num < 0 || chan_num > 23)
 		return -1;
 
@@ -266,7 +264,7 @@ static int __unioxx5_analog_write(struct comedi_subdevice *s,
 	/* sending for bytes to module(one byte per cycle iteration) */
 	for (i = 0; i < 4; i++) {
 		while (!((inb(usp->usp_iobase + 0)) & TxBE))
-			;	/* waits while writting will be allowed */
+			;	/* waits while writing will be allowed */
 		outb(usp->usp_extra_data[module][i], usp->usp_iobase + 6);
 	}
 
@@ -399,7 +397,7 @@ static int __unioxx5_subdev_init(struct comedi_device *dev,
 		outb(i + 1, iobase + 5);
 		outb('H', iobase + 6);	/* requests EEPROM world */
 		while (!(inb(iobase + 0) & TxBE))
-			;	/* waits while writting will be allowed */
+			;	/* waits while writing will be allowed */
 		outb(0, iobase + 6);
 
 		/* waits while reading of two bytes will be allowed */
@@ -413,8 +411,9 @@ static int __unioxx5_subdev_init(struct comedi_device *dev,
 		if (ndef_flag) {
 			usp->usp_module_type[i] = 0;
 			ndef_flag = 0;
-		} else
+		} else {
 			usp->usp_module_type[i] = inb(iobase + 6);
+		}
 
 		udelay(1);
 	}
diff --git a/drivers/staging/comedi/drivers/usbdux.c b/drivers/staging/comedi/drivers/usbdux.c
index 1cd7403a4e9c..ced05e581620 100644
--- a/drivers/staging/comedi/drivers/usbdux.c
+++ b/drivers/staging/comedi/drivers/usbdux.c
@@ -85,8 +85,6 @@
 
 #include "../comedi_usb.h"
 
-#include "comedi_fc.h"
-
 /* constants for firmware upload and download */
 #define USBDUX_FIRMWARE		"usbdux_firmware.bin"
 #define USBDUX_FIRMWARE_MAX_LEN	0x2000
@@ -524,19 +522,19 @@ static int usbdux_ai_cmdtest(struct comedi_device *dev,
 
 	/* Step 1 : check if triggers are trivially valid */
 
-	err |= cfc_check_trigger_src(&cmd->start_src, TRIG_NOW | TRIG_INT);
-	err |= cfc_check_trigger_src(&cmd->scan_begin_src, TRIG_TIMER);
-	err |= cfc_check_trigger_src(&cmd->convert_src, TRIG_NOW);
-	err |= cfc_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
-	err |= cfc_check_trigger_src(&cmd->stop_src, TRIG_COUNT | TRIG_NONE);
+	err |= comedi_check_trigger_src(&cmd->start_src, TRIG_NOW | TRIG_INT);
+	err |= comedi_check_trigger_src(&cmd->scan_begin_src, TRIG_TIMER);
+	err |= comedi_check_trigger_src(&cmd->convert_src, TRIG_NOW);
+	err |= comedi_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
+	err |= comedi_check_trigger_src(&cmd->stop_src, TRIG_COUNT | TRIG_NONE);
 
 	if (err)
 		return 1;
 
 	/* Step 2a : make sure trigger sources are unique */
 
-	err |= cfc_check_trigger_is_unique(cmd->start_src);
-	err |= cfc_check_trigger_is_unique(cmd->stop_src);
+	err |= comedi_check_trigger_is_unique(cmd->start_src);
+	err |= comedi_check_trigger_is_unique(cmd->stop_src);
 
 	/* Step 2b : and mutually compatible */
 
@@ -545,10 +543,10 @@ static int usbdux_ai_cmdtest(struct comedi_device *dev,
 
 	/* Step 3: check if arguments are trivially valid */
 
-	err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
+	err |= comedi_check_trigger_arg_is(&cmd->start_arg, 0);
 
 	if (cmd->scan_begin_src == TRIG_FOLLOW)	/* internal trigger */
-		err |= cfc_check_trigger_arg_is(&cmd->scan_begin_arg, 0);
+		err |= comedi_check_trigger_arg_is(&cmd->scan_begin_arg, 0);
 
 	if (cmd->scan_begin_src == TRIG_TIMER) {
 		if (this_usbduxsub->high_speed) {
@@ -563,8 +561,9 @@ static int usbdux_ai_cmdtest(struct comedi_device *dev,
 			while (i < (cmd->chanlist_len))
 				i = i * 2;
 
-			err |= cfc_check_trigger_arg_min(&cmd->scan_begin_arg,
-							 1000000 / 8 * i);
+			err |= comedi_check_trigger_arg_min(&cmd->
+							    scan_begin_arg,
+							    1000000 / 8 * i);
 			/* now calc the real sampling rate with all the
 			 * rounding errors */
 			tmp_timer =
@@ -573,24 +572,26 @@ static int usbdux_ai_cmdtest(struct comedi_device *dev,
 		} else {
 			/* full speed */
 			/* 1kHz scans every USB frame */
-			err |= cfc_check_trigger_arg_min(&cmd->scan_begin_arg,
-							 1000000);
+			err |= comedi_check_trigger_arg_min(&cmd->
+							    scan_begin_arg,
+							    1000000);
 			/*
 			 * calc the real sampling rate with the rounding errors
 			 */
 			tmp_timer = ((unsigned int)(cmd->scan_begin_arg /
 						   1000000)) * 1000000;
 		}
-		err |= cfc_check_trigger_arg_is(&cmd->scan_begin_arg,
-						tmp_timer);
+		err |= comedi_check_trigger_arg_is(&cmd->scan_begin_arg,
+						   tmp_timer);
 	}
 
-	err |= cfc_check_trigger_arg_is(&cmd->scan_end_arg, cmd->chanlist_len);
+	err |= comedi_check_trigger_arg_is(&cmd->scan_end_arg,
+					   cmd->chanlist_len);
 
 	if (cmd->stop_src == TRIG_COUNT)
-		err |= cfc_check_trigger_arg_min(&cmd->stop_arg, 1);
+		err |= comedi_check_trigger_arg_min(&cmd->stop_arg, 1);
 	else	/* TRIG_NONE */
-		err |= cfc_check_trigger_arg_is(&cmd->stop_arg, 0);
+		err |= comedi_check_trigger_arg_is(&cmd->stop_arg, 0);
 
 	if (err)
 		return 3;
@@ -633,8 +634,8 @@ static int receive_dux_commands(struct comedi_device *dev, unsigned int command)
 
 	for (i = 0; i < RETRIES; i++) {
 		ret = usb_bulk_msg(usb, usb_rcvbulkpipe(usb, 8),
-				      devpriv->insn_buf, SIZEINSNBUF,
-				      &nrec, BULK_TIMEOUT);
+				   devpriv->insn_buf, SIZEINSNBUF,
+				   &nrec, BULK_TIMEOUT);
 		if (ret < 0)
 			return ret;
 		if (le16_to_cpu(devpriv->insn_buf[0]) == command)
@@ -895,7 +896,7 @@ static int usbdux_ao_cmdtest(struct comedi_device *dev,
 
 	/* Step 1 : check if triggers are trivially valid */
 
-	err |= cfc_check_trigger_src(&cmd->start_src, TRIG_NOW | TRIG_INT);
+	err |= comedi_check_trigger_src(&cmd->start_src, TRIG_NOW | TRIG_INT);
 
 	if (0) {		/* (this_usbduxsub->high_speed) */
 		/* the sampling rate is set by the coversion rate */
@@ -904,7 +905,7 @@ static int usbdux_ao_cmdtest(struct comedi_device *dev,
 		/* start a new scan (output at once) with a timer */
 		flags = TRIG_TIMER;
 	}
-	err |= cfc_check_trigger_src(&cmd->scan_begin_src, flags);
+	err |= comedi_check_trigger_src(&cmd->scan_begin_src, flags);
 
 	if (0) {		/* (this_usbduxsub->high_speed) */
 		/*
@@ -919,18 +920,18 @@ static int usbdux_ao_cmdtest(struct comedi_device *dev,
 		 */
 		flags = TRIG_NOW;
 	}
-	err |= cfc_check_trigger_src(&cmd->convert_src, flags);
+	err |= comedi_check_trigger_src(&cmd->convert_src, flags);
 
-	err |= cfc_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
-	err |= cfc_check_trigger_src(&cmd->stop_src, TRIG_COUNT | TRIG_NONE);
+	err |= comedi_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
+	err |= comedi_check_trigger_src(&cmd->stop_src, TRIG_COUNT | TRIG_NONE);
 
 	if (err)
 		return 1;
 
 	/* Step 2a : make sure trigger sources are unique */
 
-	err |= cfc_check_trigger_is_unique(cmd->start_src);
-	err |= cfc_check_trigger_is_unique(cmd->stop_src);
+	err |= comedi_check_trigger_is_unique(cmd->start_src);
+	err |= comedi_check_trigger_is_unique(cmd->stop_src);
 
 	/* Step 2b : and mutually compatible */
 
@@ -939,25 +940,27 @@ static int usbdux_ao_cmdtest(struct comedi_device *dev,
 
 	/* Step 3: check if arguments are trivially valid */
 
-	err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
+	err |= comedi_check_trigger_arg_is(&cmd->start_arg, 0);
 
 	if (cmd->scan_begin_src == TRIG_FOLLOW)	/* internal trigger */
-		err |= cfc_check_trigger_arg_is(&cmd->scan_begin_arg, 0);
+		err |= comedi_check_trigger_arg_is(&cmd->scan_begin_arg, 0);
 
-	if (cmd->scan_begin_src == TRIG_TIMER)
-		err |= cfc_check_trigger_arg_min(&cmd->scan_begin_arg,
-						 1000000);
+	if (cmd->scan_begin_src == TRIG_TIMER) {
+		err |= comedi_check_trigger_arg_min(&cmd->scan_begin_arg,
+						    1000000);
+	}
 
 	/* not used now, is for later use */
 	if (cmd->convert_src == TRIG_TIMER)
-		err |= cfc_check_trigger_arg_min(&cmd->convert_arg, 125000);
+		err |= comedi_check_trigger_arg_min(&cmd->convert_arg, 125000);
 
-	err |= cfc_check_trigger_arg_is(&cmd->scan_end_arg, cmd->chanlist_len);
+	err |= comedi_check_trigger_arg_is(&cmd->scan_end_arg,
+					   cmd->chanlist_len);
 
 	if (cmd->stop_src == TRIG_COUNT)
-		err |= cfc_check_trigger_arg_min(&cmd->stop_arg, 1);
+		err |= comedi_check_trigger_arg_min(&cmd->stop_arg, 1);
 	else	/* TRIG_NONE */
-		err |= cfc_check_trigger_arg_is(&cmd->stop_arg, 0);
+		err |= comedi_check_trigger_arg_is(&cmd->stop_arg, 0);
 
 	if (err)
 		return 3;
@@ -1041,7 +1044,6 @@ static int usbdux_dio_insn_bits(struct comedi_device *dev,
 				struct comedi_insn *insn,
 				unsigned int *data)
 {
-
 	struct usbdux_private *devpriv = dev->private;
 	int ret;
 
diff --git a/drivers/staging/comedi/drivers/usbduxfast.c b/drivers/staging/comedi/drivers/usbduxfast.c
index 7ce27c16c2f9..d90dc59982be 100644
--- a/drivers/staging/comedi/drivers/usbduxfast.c
+++ b/drivers/staging/comedi/drivers/usbduxfast.c
@@ -48,7 +48,6 @@
 #include <linux/input.h>
 #include <linux/fcntl.h>
 #include <linux/compiler.h>
-#include "comedi_fc.h"
 #include "../comedi_usb.h"
 
 /*
@@ -343,23 +342,24 @@ static int usbduxfast_ai_cmdtest(struct comedi_device *dev,
 
 	/* Step 1 : check if triggers are trivially valid */
 
-	err |= cfc_check_trigger_src(&cmd->start_src,
+	err |= comedi_check_trigger_src(&cmd->start_src,
 					TRIG_NOW | TRIG_EXT | TRIG_INT);
-	err |= cfc_check_trigger_src(&cmd->scan_begin_src,
+	err |= comedi_check_trigger_src(&cmd->scan_begin_src,
 					TRIG_FOLLOW | TRIG_EXT);
-	err |= cfc_check_trigger_src(&cmd->convert_src, TRIG_TIMER | TRIG_EXT);
-	err |= cfc_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
-	err |= cfc_check_trigger_src(&cmd->stop_src, TRIG_COUNT | TRIG_NONE);
+	err |= comedi_check_trigger_src(&cmd->convert_src,
+					TRIG_TIMER | TRIG_EXT);
+	err |= comedi_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
+	err |= comedi_check_trigger_src(&cmd->stop_src, TRIG_COUNT | TRIG_NONE);
 
 	if (err)
 		return 1;
 
 	/* Step 2a : make sure trigger sources are unique */
 
-	err |= cfc_check_trigger_is_unique(cmd->start_src);
-	err |= cfc_check_trigger_is_unique(cmd->scan_begin_src);
-	err |= cfc_check_trigger_is_unique(cmd->convert_src);
-	err |= cfc_check_trigger_is_unique(cmd->stop_src);
+	err |= comedi_check_trigger_is_unique(cmd->start_src);
+	err |= comedi_check_trigger_is_unique(cmd->scan_begin_src);
+	err |= comedi_check_trigger_is_unique(cmd->convert_src);
+	err |= comedi_check_trigger_is_unique(cmd->stop_src);
 
 	/* Step 2b : and mutually compatible */
 
@@ -372,12 +372,13 @@ static int usbduxfast_ai_cmdtest(struct comedi_device *dev,
 
 	/* Step 3: check if arguments are trivially valid */
 
-	err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
+	err |= comedi_check_trigger_arg_is(&cmd->start_arg, 0);
 
 	if (!cmd->chanlist_len)
 		err |= -EINVAL;
 
-	err |= cfc_check_trigger_arg_is(&cmd->scan_end_arg, cmd->chanlist_len);
+	err |= comedi_check_trigger_arg_is(&cmd->scan_end_arg,
+					   cmd->chanlist_len);
 
 	if (cmd->chanlist_len == 1)
 		min_sample_period = 1;
@@ -394,16 +395,16 @@ static int usbduxfast_ai_cmdtest(struct comedi_device *dev,
 
 		/* calc arg again */
 		tmp = steps / 30;
-		err |= cfc_check_trigger_arg_is(&cmd->convert_arg, tmp);
+		err |= comedi_check_trigger_arg_is(&cmd->convert_arg, tmp);
 	}
 
 	/* stop source */
 	switch (cmd->stop_src) {
 	case TRIG_COUNT:
-		err |= cfc_check_trigger_arg_min(&cmd->stop_arg, 1);
+		err |= comedi_check_trigger_arg_min(&cmd->stop_arg, 1);
 		break;
 	case TRIG_NONE:
-		err |= cfc_check_trigger_arg_is(&cmd->stop_arg, 0);
+		err |= comedi_check_trigger_arg_is(&cmd->stop_arg, 0);
 		break;
 		/*
 		 * TRIG_EXT doesn't care since it doesn't trigger
@@ -419,7 +420,6 @@ static int usbduxfast_ai_cmdtest(struct comedi_device *dev,
 	/* step 4: fix up any arguments */
 
 	return 0;
-
 }
 
 static int usbduxfast_ai_inttrig(struct comedi_device *dev,
diff --git a/drivers/staging/comedi/drivers/usbduxsigma.c b/drivers/staging/comedi/drivers/usbduxsigma.c
index 394969b7458c..eaa9add491df 100644
--- a/drivers/staging/comedi/drivers/usbduxsigma.c
+++ b/drivers/staging/comedi/drivers/usbduxsigma.c
@@ -49,7 +49,6 @@
 #include <linux/compiler.h>
 #include <asm/unaligned.h>
 
-#include "comedi_fc.h"
 #include "../comedi_usb.h"
 
 /* timeout for the USB-transfer in ms*/
@@ -243,9 +242,8 @@ static void usbduxsigma_ai_handle_urb(struct comedi_device *dev,
 		urb->dev = comedi_to_usb_dev(dev);
 		ret = usb_submit_urb(urb, GFP_ATOMIC);
 		if (ret < 0) {
-			dev_err(dev->class_dev,
-				"%s: urb resubmit failed (%d)\n",
-				__func__, ret);
+			dev_err(dev->class_dev, "urb resubmit failed (%d)\n",
+				ret);
 			if (ret == -EL2NSYNC)
 				dev_err(dev->class_dev,
 					"buggy USB host controller or bug in IRQ handler\n");
@@ -292,8 +290,8 @@ static void usbduxsigma_ai_urb_complete(struct urb *urb)
 
 	default:
 		/* a real error */
-		dev_err(dev->class_dev, "%s: non-zero urb status (%d)\n",
-			__func__, urb->status);
+		dev_err(dev->class_dev, "non-zero urb status (%d)\n",
+			urb->status);
 		async->events |= COMEDI_CB_ERROR;
 		break;
 	}
@@ -386,9 +384,8 @@ static void usbduxsigma_ao_handle_urb(struct comedi_device *dev,
 		urb->iso_frame_desc[0].status = 0;
 		ret = usb_submit_urb(urb, GFP_ATOMIC);
 		if (ret < 0) {
-			dev_err(dev->class_dev,
-				"%s: urb resubmit failed (%d)\n",
-				__func__, ret);
+			dev_err(dev->class_dev, "urb resubmit failed (%d)\n",
+				ret);
 			if (ret == -EL2NSYNC)
 				dev_err(dev->class_dev,
 					"buggy USB host controller or bug in IRQ handler\n");
@@ -423,8 +420,8 @@ static void usbduxsigma_ao_urb_complete(struct urb *urb)
 
 	default:
 		/* a real error */
-		dev_err(dev->class_dev, "%s: non-zero urb status (%d)\n",
-			__func__, urb->status);
+		dev_err(dev->class_dev, "non-zero urb status (%d)\n",
+			urb->status);
 		async->events |= COMEDI_CB_ERROR;
 		break;
 	}
@@ -488,19 +485,19 @@ static int usbduxsigma_ai_cmdtest(struct comedi_device *dev,
 
 	/* Step 1 : check if triggers are trivially valid */
 
-	err |= cfc_check_trigger_src(&cmd->start_src, TRIG_NOW | TRIG_INT);
-	err |= cfc_check_trigger_src(&cmd->scan_begin_src, TRIG_TIMER);
-	err |= cfc_check_trigger_src(&cmd->convert_src, TRIG_NOW);
-	err |= cfc_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
-	err |= cfc_check_trigger_src(&cmd->stop_src, TRIG_COUNT | TRIG_NONE);
+	err |= comedi_check_trigger_src(&cmd->start_src, TRIG_NOW | TRIG_INT);
+	err |= comedi_check_trigger_src(&cmd->scan_begin_src, TRIG_TIMER);
+	err |= comedi_check_trigger_src(&cmd->convert_src, TRIG_NOW);
+	err |= comedi_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
+	err |= comedi_check_trigger_src(&cmd->stop_src, TRIG_COUNT | TRIG_NONE);
 
 	if (err)
 		return 1;
 
 	/* Step 2a : make sure trigger sources are unique */
 
-	err |= cfc_check_trigger_is_unique(cmd->start_src);
-	err |= cfc_check_trigger_is_unique(cmd->stop_src);
+	err |= comedi_check_trigger_is_unique(cmd->start_src);
+	err |= comedi_check_trigger_is_unique(cmd->stop_src);
 
 	/* Step 2b : and mutually compatible */
 
@@ -509,10 +506,10 @@ static int usbduxsigma_ai_cmdtest(struct comedi_device *dev,
 
 	/* Step 3: check if arguments are trivially valid */
 
-	err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
+	err |= comedi_check_trigger_arg_is(&cmd->start_arg, 0);
 
 	if (cmd->scan_begin_src == TRIG_FOLLOW)	/* internal trigger */
-		err |= cfc_check_trigger_arg_is(&cmd->scan_begin_arg, 0);
+		err |= comedi_check_trigger_arg_is(&cmd->scan_begin_arg, 0);
 
 	if (cmd->scan_begin_src == TRIG_TIMER) {
 		unsigned int tmp;
@@ -524,27 +521,31 @@ static int usbduxsigma_ai_cmdtest(struct comedi_device *dev,
 			 * sample two channels. Thus, the more channels
 			 * are in the channel list the more time we need.
 			 */
-			err |= cfc_check_trigger_arg_min(&cmd->scan_begin_arg,
-						(1000000 / 8 * interval));
+			err |= comedi_check_trigger_arg_min(&cmd->
+							    scan_begin_arg,
+							    (1000000 / 8 *
+							     interval));
 
 			tmp = (cmd->scan_begin_arg / 125000) * 125000;
 		} else {
 			/* full speed */
 			/* 1kHz scans every USB frame */
-			err |= cfc_check_trigger_arg_min(&cmd->scan_begin_arg,
-							 1000000);
+			err |= comedi_check_trigger_arg_min(&cmd->
+							    scan_begin_arg,
+							    1000000);
 
 			tmp = (cmd->scan_begin_arg / 1000000) * 1000000;
 		}
-		err |= cfc_check_trigger_arg_is(&cmd->scan_begin_arg, tmp);
+		err |= comedi_check_trigger_arg_is(&cmd->scan_begin_arg, tmp);
 	}
 
-	err |= cfc_check_trigger_arg_is(&cmd->scan_end_arg, cmd->chanlist_len);
+	err |= comedi_check_trigger_arg_is(&cmd->scan_end_arg,
+					   cmd->chanlist_len);
 
 	if (cmd->stop_src == TRIG_COUNT)
-		err |= cfc_check_trigger_arg_min(&cmd->stop_arg, 1);
+		err |= comedi_check_trigger_arg_min(&cmd->stop_arg, 1);
 	else	/* TRIG_NONE */
-		err |= cfc_check_trigger_arg_is(&cmd->stop_arg, 0);
+		err |= comedi_check_trigger_arg_is(&cmd->stop_arg, 0);
 
 	if (err)
 		return 3;
@@ -856,7 +857,7 @@ static int usbduxsigma_ao_cmdtest(struct comedi_device *dev,
 
 	/* Step 1 : check if triggers are trivially valid */
 
-	err |= cfc_check_trigger_src(&cmd->start_src, TRIG_NOW | TRIG_INT);
+	err |= comedi_check_trigger_src(&cmd->start_src, TRIG_NOW | TRIG_INT);
 
 	if (high_speed) {
 		/*
@@ -868,11 +869,11 @@ static int usbduxsigma_ao_cmdtest(struct comedi_device *dev,
 		/* start a new scan (output at once) with a timer */
 		flags = TRIG_TIMER;
 	}
-	err |= cfc_check_trigger_src(&cmd->scan_begin_src, flags);
+	err |= comedi_check_trigger_src(&cmd->scan_begin_src, flags);
 
-	err |= cfc_check_trigger_src(&cmd->convert_src, TRIG_NOW);
-	err |= cfc_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
-	err |= cfc_check_trigger_src(&cmd->stop_src, TRIG_COUNT | TRIG_NONE);
+	err |= comedi_check_trigger_src(&cmd->convert_src, TRIG_NOW);
+	err |= comedi_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
+	err |= comedi_check_trigger_src(&cmd->stop_src, TRIG_COUNT | TRIG_NONE);
 
 	if (err) {
 		up(&devpriv->sem);
@@ -881,8 +882,8 @@ static int usbduxsigma_ao_cmdtest(struct comedi_device *dev,
 
 	/* Step 2a : make sure trigger sources are unique */
 
-	err |= cfc_check_trigger_is_unique(cmd->start_src);
-	err |= cfc_check_trigger_is_unique(cmd->stop_src);
+	err |= comedi_check_trigger_is_unique(cmd->start_src);
+	err |= comedi_check_trigger_is_unique(cmd->stop_src);
 
 	/* Step 2b : and mutually compatible */
 
@@ -891,25 +892,27 @@ static int usbduxsigma_ao_cmdtest(struct comedi_device *dev,
 
 	/* Step 3: check if arguments are trivially valid */
 
-	err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
+	err |= comedi_check_trigger_arg_is(&cmd->start_arg, 0);
 
 	if (cmd->scan_begin_src == TRIG_FOLLOW)	/* internal trigger */
-		err |= cfc_check_trigger_arg_is(&cmd->scan_begin_arg, 0);
+		err |= comedi_check_trigger_arg_is(&cmd->scan_begin_arg, 0);
 
-	if (cmd->scan_begin_src == TRIG_TIMER)
-		err |= cfc_check_trigger_arg_min(&cmd->scan_begin_arg,
-						 1000000);
+	if (cmd->scan_begin_src == TRIG_TIMER) {
+		err |= comedi_check_trigger_arg_min(&cmd->scan_begin_arg,
+						    1000000);
+	}
 
 	/* not used now, is for later use */
 	if (cmd->convert_src == TRIG_TIMER)
-		err |= cfc_check_trigger_arg_min(&cmd->convert_arg, 125000);
+		err |= comedi_check_trigger_arg_min(&cmd->convert_arg, 125000);
 
-	err |= cfc_check_trigger_arg_is(&cmd->scan_end_arg, cmd->chanlist_len);
+	err |= comedi_check_trigger_arg_is(&cmd->scan_end_arg,
+					   cmd->chanlist_len);
 
 	if (cmd->stop_src == TRIG_COUNT)
-		err |= cfc_check_trigger_arg_min(&cmd->stop_arg, 1);
+		err |= comedi_check_trigger_arg_min(&cmd->stop_arg, 1);
 	else	/* TRIG_NONE */
-		err |= cfc_check_trigger_arg_is(&cmd->stop_arg, 0);
+		err |= comedi_check_trigger_arg_is(&cmd->stop_arg, 0);
 
 	if (err)
 		return 3;
@@ -1071,9 +1074,8 @@ static void usbduxsigma_pwm_urb_complete(struct urb *urb)
 	default:
 		/* a real error */
 		if (devpriv->pwm_cmd_running) {
-			dev_err(dev->class_dev,
-				"%s: non-zero urb status (%d)\n",
-				__func__, urb->status);
+			dev_err(dev->class_dev, "non-zero urb status (%d)\n",
+				urb->status);
 			usbduxsigma_pwm_stop(dev, 0);	/* w/o unlink */
 		}
 		return;
@@ -1087,8 +1089,7 @@ static void usbduxsigma_pwm_urb_complete(struct urb *urb)
 	urb->status = 0;
 	ret = usb_submit_urb(urb, GFP_ATOMIC);
 	if (ret < 0) {
-		dev_err(dev->class_dev, "%s: urb resubmit failed (%d)\n",
-			__func__, ret);
+		dev_err(dev->class_dev, "urb resubmit failed (%d)\n", ret);
 		if (ret == -EL2NSYNC)
 			dev_err(dev->class_dev,
 				"buggy USB host controller or bug in IRQ handler\n");
diff --git a/drivers/staging/comedi/drivers/vmk80xx.c b/drivers/staging/comedi/drivers/vmk80xx.c
index e37118321a27..a0906685e27f 100644
--- a/drivers/staging/comedi/drivers/vmk80xx.c
+++ b/drivers/staging/comedi/drivers/vmk80xx.c
@@ -103,11 +103,6 @@ enum vmk80xx_model {
 	VMK8061_MODEL
 };
 
-struct firmware_version {
-	unsigned char ic3_vers[32];	/* USB-Controller */
-	unsigned char ic6_vers[32];	/* CPU */
-};
-
 static const struct comedi_lrange vmk8061_range = {
 	2, {
 		UNI_RANGE(5),
@@ -156,68 +151,12 @@ static const struct vmk80xx_board vmk80xx_boardinfo[] = {
 struct vmk80xx_private {
 	struct usb_endpoint_descriptor *ep_rx;
 	struct usb_endpoint_descriptor *ep_tx;
-	struct firmware_version fw;
 	struct semaphore limit_sem;
 	unsigned char *usb_rx_buf;
 	unsigned char *usb_tx_buf;
 	enum vmk80xx_model model;
 };
 
-static int vmk80xx_check_data_link(struct comedi_device *dev)
-{
-	struct vmk80xx_private *devpriv = dev->private;
-	struct usb_device *usb = comedi_to_usb_dev(dev);
-	unsigned int tx_pipe;
-	unsigned int rx_pipe;
-	unsigned char tx[1];
-	unsigned char rx[2];
-
-	tx_pipe = usb_sndbulkpipe(usb, 0x01);
-	rx_pipe = usb_rcvbulkpipe(usb, 0x81);
-
-	tx[0] = VMK8061_CMD_RD_PWR_STAT;
-
-	/*
-	 * Check that IC6 (PIC16F871) is powered and
-	 * running and the data link between IC3 and
-	 * IC6 is working properly
-	 */
-	usb_bulk_msg(usb, tx_pipe, tx, 1, NULL, devpriv->ep_tx->bInterval);
-	usb_bulk_msg(usb, rx_pipe, rx, 2, NULL, HZ * 10);
-
-	return (int)rx[1];
-}
-
-static void vmk80xx_read_eeprom(struct comedi_device *dev, int flag)
-{
-	struct vmk80xx_private *devpriv = dev->private;
-	struct usb_device *usb = comedi_to_usb_dev(dev);
-	unsigned int tx_pipe;
-	unsigned int rx_pipe;
-	unsigned char tx[1];
-	unsigned char rx[64];
-	int cnt;
-
-	tx_pipe = usb_sndbulkpipe(usb, 0x01);
-	rx_pipe = usb_rcvbulkpipe(usb, 0x81);
-
-	tx[0] = VMK8061_CMD_RD_VERSION;
-
-	/*
-	 * Read the firmware version info of IC3 and
-	 * IC6 from the internal EEPROM of the IC
-	 */
-	usb_bulk_msg(usb, tx_pipe, tx, 1, NULL, devpriv->ep_tx->bInterval);
-	usb_bulk_msg(usb, rx_pipe, rx, 64, &cnt, HZ * 10);
-
-	rx[cnt] = '\0';
-
-	if (flag & IC3_VERSION)
-		strncpy(devpriv->fw.ic3_vers, rx + 1, 24);
-	else			/* IC6_VERSION */
-		strncpy(devpriv->fw.ic6_vers, rx + 25, 24);
-}
-
 static void vmk80xx_do_bulk_msg(struct comedi_device *dev)
 {
 	struct vmk80xx_private *devpriv = dev->private;
@@ -878,16 +817,6 @@ static int vmk80xx_auto_attach(struct comedi_device *dev,
 
 	usb_set_intfdata(intf, devpriv);
 
-	if (devpriv->model == VMK8061_MODEL) {
-		vmk80xx_read_eeprom(dev, IC3_VERSION);
-		dev_info(&intf->dev, "%s\n", devpriv->fw.ic3_vers);
-
-		if (vmk80xx_check_data_link(dev)) {
-			vmk80xx_read_eeprom(dev, IC6_VERSION);
-			dev_info(&intf->dev, "%s\n", devpriv->fw.ic6_vers);
-		}
-	}
-
 	if (devpriv->model == VMK8055_MODEL)
 		vmk80xx_reset_device(dev);
 
diff --git a/drivers/staging/comedi/kcomedilib/kcomedilib_main.c b/drivers/staging/comedi/kcomedilib/kcomedilib_main.c
index 973f544e85e1..76bf5619fdd5 100644
--- a/drivers/staging/comedi/kcomedilib/kcomedilib_main.c
+++ b/drivers/staging/comedi/kcomedilib/kcomedilib_main.c
@@ -58,7 +58,7 @@ struct comedi_device *comedi_open(const char *filename)
 		retval = NULL;
 	up_read(&dev->attach_lock);
 
-	if (retval == NULL)
+	if (!retval)
 		comedi_dev_put(dev);
 
 	return retval;
diff --git a/drivers/staging/dgap/dgap.c b/drivers/staging/dgap/dgap.c
index 7184747e0652..6766d5a91a90 100644
--- a/drivers/staging/dgap/dgap.c
+++ b/drivers/staging/dgap/dgap.c
@@ -1361,7 +1361,6 @@ static uint dgap_get_custom_baud(struct channel_t *ch)
 {
 	u8 __iomem *vaddr;
 	ulong offset;
-	uint value;
 
 	if (!ch || ch->magic != DGAP_CHANNEL_MAGIC)
 		return 0;
@@ -1384,8 +1383,7 @@ static uint dgap_get_custom_baud(struct channel_t *ch)
 	offset = (ioread16(vaddr + ECS_SEG) << 4) + (ch->ch_portnum * 0x28)
 	       + LINE_SPEED;
 
-	value = readw(vaddr + offset);
-	return value;
+	return readw(vaddr + offset);
 }
 
 /*
@@ -2196,7 +2194,7 @@ static struct board_t *dgap_found_board(struct pci_dev *pdev, int id,
 	 * will be mapped into the low 2MB of the 4MB memory space
 	 */
 	brd->port = brd->membase + PCI_IO_OFFSET;
-	brd->port_end = brd->port + PCI_IO_SIZE;
+	brd->port_end = brd->port + PCI_IO_SIZE_DGAP;
 
 	/*
 	 * Special initialization for non-PLX boards
@@ -3979,7 +3977,6 @@ static int dgap_get_modem_info(struct channel_t *ch, unsigned int __user *value)
 	int result;
 	u8 mstat;
 	ulong lock_flags;
-	int rc;
 
 	spin_lock_irqsave(&ch->ch_lock, lock_flags);
 
@@ -4004,9 +4001,7 @@ static int dgap_get_modem_info(struct channel_t *ch, unsigned int __user *value)
 	if (mstat & D_CD(ch))
 		result |= TIOCM_CD;
 
-	rc = put_user(result, value);
-
-	return rc;
+	return put_user(result, value);
 }
 
 /*
@@ -7044,8 +7039,7 @@ static int dgap_start(void)
 
 	/* Start the poller */
 	spin_lock_irqsave(&dgap_poll_lock, flags);
-	init_timer(&dgap_poll_timer);
-	dgap_poll_timer.function = dgap_poll_handler;
+	setup_timer(&dgap_poll_timer, dgap_poll_handler, 0);
 	dgap_poll_timer.data = 0;
 	dgap_poll_time = jiffies + dgap_jiffies_from_ms(dgap_poll_tick);
 	dgap_poll_timer.expires = dgap_poll_time;
diff --git a/drivers/staging/dgap/dgap.h b/drivers/staging/dgap/dgap.h
index 684033156e8c..a2e5b26c673a 100644
--- a/drivers/staging/dgap/dgap.h
+++ b/drivers/staging/dgap/dgap.h
@@ -121,7 +121,7 @@
 #define PCI_IO_OFFSET		0x00200000
 
 /* Size of IO (2MB) */
-#define PCI_IO_SIZE		0x00200000
+#define PCI_IO_SIZE_DGAP	0x00200000
 
 /* Number of boards we support at once. */
 #define	MAXBOARDS	32
diff --git a/drivers/staging/dgnc/Makefile b/drivers/staging/dgnc/Makefile
index b69f7b6b1143..995c874f40eb 100644
--- a/drivers/staging/dgnc/Makefile
+++ b/drivers/staging/dgnc/Makefile
@@ -1,5 +1,3 @@
-EXTRA_CFLAGS += -DDG_NAME=\"dgnc-1.3-16\" -DDG_PART=\"40002369_F\"
-
 obj-$(CONFIG_DGNC) += dgnc.o
 
 dgnc-objs :=   dgnc_cls.o dgnc_driver.o\
diff --git a/drivers/staging/dgnc/TODO b/drivers/staging/dgnc/TODO
index 22adff1078f3..2b2c6ea03c61 100644
--- a/drivers/staging/dgnc/TODO
+++ b/drivers/staging/dgnc/TODO
@@ -1,10 +1,10 @@
 * checkpatch fixes
 * remove unecessary comments
-* remove unecessary error messages. Example kzalloc() has its 
+* remove unecessary error messages. Example kzalloc() has its
   own error message. Adding an extra one is useless.
 * use goto statements for error handling when appropriate
-* there is a lot of unecessary code in the driver. It was 
+* there is a lot of unecessary code in the driver. It was
   originally a standalone driver. Remove uneeded code.
 
-Please send patches to Greg Kroah-Hartman <greg@kroah.com> and 
+Please send patches to Greg Kroah-Hartman <greg@kroah.com> and
 Cc: Lidza Louina <lidza.louina@gmail.com>
diff --git a/drivers/staging/dgnc/dgnc_cls.c b/drivers/staging/dgnc/dgnc_cls.c
index bedc5221b6fc..e3564d278d91 100644
--- a/drivers/staging/dgnc/dgnc_cls.c
+++ b/drivers/staging/dgnc/dgnc_cls.c
@@ -11,22 +11,6 @@
  * but WITHOUT ANY WARRANTY, EXPRESS OR IMPLIED; without even the
  * implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
  * PURPOSE.  See the GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
- *
- *
- *	NOTE TO LINUX KERNEL HACKERS:  DO NOT REFORMAT THIS CODE!
- *
- *	This is shared code between Digi's CVS archive and the
- *	Linux Kernel sources.
- *	Changing the source just for reformatting needlessly breaks
- *	our CVS diff history.
- *
- *	Send any bug fixes/changes to:  Eng.Linux at digi dot com.
- *	Thank you.
- *
  */
 
 #include <linux/kernel.h>
@@ -92,14 +76,12 @@ struct board_ops dgnc_cls_ops = {
 	.send_immediate_char =		cls_send_immediate_char
 };
 
-
 static inline void cls_set_cts_flow_control(struct channel_t *ch)
 {
 	unsigned char lcrb = readb(&ch->ch_cls_uart->lcr);
 	unsigned char ier = readb(&ch->ch_cls_uart->ier);
 	unsigned char isr_fcr = 0;
 
-
 	/*
 	 * The Enhanced Register Set may only be accessed when
 	 * the Line Control Register is set to 0xBFh.
@@ -136,14 +118,12 @@ static inline void cls_set_cts_flow_control(struct channel_t *ch)
 
 }
 
-
 static inline void cls_set_ixon_flow_control(struct channel_t *ch)
 {
 	unsigned char lcrb = readb(&ch->ch_cls_uart->lcr);
 	unsigned char ier = readb(&ch->ch_cls_uart->ier);
 	unsigned char isr_fcr = 0;
 
-
 	/*
 	 * The Enhanced Register Set may only be accessed when
 	 * the Line Control Register is set to 0xBFh.
@@ -184,14 +164,12 @@ static inline void cls_set_ixon_flow_control(struct channel_t *ch)
 
 }
 
-
 static inline void cls_set_no_output_flow_control(struct channel_t *ch)
 {
 	unsigned char lcrb = readb(&ch->ch_cls_uart->lcr);
 	unsigned char ier = readb(&ch->ch_cls_uart->ier);
 	unsigned char isr_fcr = 0;
 
-
 	/*
 	 * The Enhanced Register Set may only be accessed when
 	 * the Line Control Register is set to 0xBFh.
@@ -230,14 +208,12 @@ static inline void cls_set_no_output_flow_control(struct channel_t *ch)
 
 }
 
-
 static inline void cls_set_rts_flow_control(struct channel_t *ch)
 {
 	unsigned char lcrb = readb(&ch->ch_cls_uart->lcr);
 	unsigned char ier = readb(&ch->ch_cls_uart->ier);
 	unsigned char isr_fcr = 0;
 
-
 	/*
 	 * The Enhanced Register Set may only be accessed when
 	 * the Line Control Register is set to 0xBFh.
@@ -266,20 +242,17 @@ static inline void cls_set_rts_flow_control(struct channel_t *ch)
 		UART_16654_FCR_TXTRIGGER_16 | UART_FCR_CLEAR_RCVR),
 		&ch->ch_cls_uart->isr_fcr);
 
-
 	ch->ch_r_watermark = 4;
 	ch->ch_r_tlevel = 8;
 
 }
 
-
 static inline void cls_set_ixoff_flow_control(struct channel_t *ch)
 {
 	unsigned char lcrb = readb(&ch->ch_cls_uart->lcr);
 	unsigned char ier = readb(&ch->ch_cls_uart->ier);
 	unsigned char isr_fcr = 0;
 
-
 	/*
 	 * The Enhanced Register Set may only be accessed when
 	 * the Line Control Register is set to 0xBFh.
@@ -316,14 +289,12 @@ static inline void cls_set_ixoff_flow_control(struct channel_t *ch)
 
 }
 
-
 static inline void cls_set_no_input_flow_control(struct channel_t *ch)
 {
 	unsigned char lcrb = readb(&ch->ch_cls_uart->lcr);
 	unsigned char ier = readb(&ch->ch_cls_uart->ier);
 	unsigned char isr_fcr = 0;
 
-
 	/*
 	 * The Enhanced Register Set may only be accessed when
 	 * the Line Control Register is set to 0xBFh.
@@ -357,7 +328,6 @@ static inline void cls_set_no_input_flow_control(struct channel_t *ch)
 
 }
 
-
 /*
  * cls_clear_break.
  * Determines whether its time to shut off break condition.
@@ -393,7 +363,6 @@ static inline void cls_clear_break(struct channel_t *ch, int force)
 	spin_unlock_irqrestore(&ch->ch_lock, flags);
 }
 
-
 /* Parse the ISR register for the specific port */
 static inline void cls_parse_isr(struct dgnc_board *brd, uint port)
 {
@@ -406,7 +375,7 @@ static inline void cls_parse_isr(struct dgnc_board *brd, uint port)
 	 * verified in the interrupt routine.
 	 */
 
-	if (port > brd->nasync)
+	if (port >= brd->nasync)
 		return;
 
 	ch = brd->channels[port];
@@ -457,7 +426,6 @@ static inline void cls_parse_isr(struct dgnc_board *brd, uint port)
 	}
 }
 
-
 /*
  * cls_param()
  * Send any/all changes to the line to the UART.
@@ -711,7 +679,6 @@ static void cls_param(struct tty_struct *tty)
 	cls_parse_modem(ch, readb(&ch->ch_cls_uart->msr));
 }
 
-
 /*
  * Our board poller function.
  */
@@ -784,7 +751,6 @@ static void cls_tasklet(unsigned long data)
 
 }
 
-
 /*
  * cls_intr()
  *
@@ -834,7 +800,6 @@ static irqreturn_t cls_intr(int irq, void *voidbrd)
 	return IRQ_HANDLED;
 }
 
-
 static void cls_disable_receiver(struct channel_t *ch)
 {
 	unsigned char tmp = readb(&ch->ch_cls_uart->ier);
@@ -843,7 +808,6 @@ static void cls_disable_receiver(struct channel_t *ch)
 	writeb(tmp, &ch->ch_cls_uart->ier);
 }
 
-
 static void cls_enable_receiver(struct channel_t *ch)
 {
 	unsigned char tmp = readb(&ch->ch_cls_uart->ier);
@@ -852,7 +816,6 @@ static void cls_enable_receiver(struct channel_t *ch)
 	writeb(tmp, &ch->ch_cls_uart->ier);
 }
 
-
 static void cls_copy_data_from_uart_to_queue(struct channel_t *ch)
 {
 	int qleft = 0;
@@ -942,7 +905,6 @@ static void cls_copy_data_from_uart_to_queue(struct channel_t *ch)
 	spin_unlock_irqrestore(&ch->ch_lock, flags);
 }
 
-
 /*
  * This function basically goes to sleep for secs, or until
  * it gets signalled that the port has fully drained.
@@ -978,7 +940,6 @@ static int cls_drain(struct tty_struct *tty, uint seconds)
 					 ((un->un_flags & UN_EMPTY) == 0));
 }
 
-
 /* Channel lock MUST be held before calling this function! */
 static void cls_flush_uart_write(struct channel_t *ch)
 {
@@ -992,7 +953,6 @@ static void cls_flush_uart_write(struct channel_t *ch)
 	ch->ch_flags |= (CH_TX_FIFO_EMPTY | CH_TX_FIFO_LWM);
 }
 
-
 /* Channel lock MUST be held before calling this function! */
 static void cls_flush_uart_read(struct channel_t *ch)
 {
@@ -1013,7 +973,6 @@ static void cls_flush_uart_read(struct channel_t *ch)
 	udelay(10);
 }
 
-
 static void cls_copy_data_from_queue_to_uart(struct channel_t *ch)
 {
 	ushort head;
@@ -1029,22 +988,16 @@ static void cls_copy_data_from_queue_to_uart(struct channel_t *ch)
 	spin_lock_irqsave(&ch->ch_lock, flags);
 
 	/* No data to write to the UART */
-	if (ch->ch_w_tail == ch->ch_w_head) {
-		spin_unlock_irqrestore(&ch->ch_lock, flags);
-		return;
-	}
+	if (ch->ch_w_tail == ch->ch_w_head)
+		goto exit_unlock;
 
 	/* If port is "stopped", don't send any data to the UART */
 	if ((ch->ch_flags & CH_FORCED_STOP) ||
-				 (ch->ch_flags & CH_BREAK_SENDING)) {
-		spin_unlock_irqrestore(&ch->ch_lock, flags);
-		return;
-	}
+				 (ch->ch_flags & CH_BREAK_SENDING))
+		goto exit_unlock;
 
-	if (!(ch->ch_flags & (CH_TX_FIFO_EMPTY | CH_TX_FIFO_LWM))) {
-		spin_unlock_irqrestore(&ch->ch_lock, flags);
-		return;
-	}
+	if (!(ch->ch_flags & (CH_TX_FIFO_EMPTY | CH_TX_FIFO_LWM)))
+		goto exit_unlock;
 
 	n = 32;
 
@@ -1094,10 +1047,10 @@ static void cls_copy_data_from_queue_to_uart(struct channel_t *ch)
 	if (len_written > 0)
 		ch->ch_flags &= ~(CH_TX_FIFO_EMPTY | CH_TX_FIFO_LWM);
 
+exit_unlock:
 	spin_unlock_irqrestore(&ch->ch_lock, flags);
 }
 
-
 static void cls_parse_modem(struct channel_t *ch, unsigned char signals)
 {
 	unsigned char msignals = signals;
@@ -1162,7 +1115,6 @@ static void cls_parse_modem(struct channel_t *ch, unsigned char signals)
 	spin_unlock_irqrestore(&ch->ch_lock, flags);
 }
 
-
 /* Make the UART raise any of the output signals we want up */
 static void cls_assert_modem_signals(struct channel_t *ch)
 {
@@ -1182,7 +1134,6 @@ static void cls_assert_modem_signals(struct channel_t *ch)
 	udelay(10);
 }
 
-
 static void cls_send_start_character(struct channel_t *ch)
 {
 	if (!ch || ch->magic != DGNC_CHANNEL_MAGIC)
@@ -1194,7 +1145,6 @@ static void cls_send_start_character(struct channel_t *ch)
 	}
 }
 
-
 static void cls_send_stop_character(struct channel_t *ch)
 {
 	if (!ch || ch->magic != DGNC_CHANNEL_MAGIC)
@@ -1206,7 +1156,6 @@ static void cls_send_stop_character(struct channel_t *ch)
 	}
 }
 
-
 /* Inits UART */
 static void cls_uart_init(struct channel_t *ch)
 {
@@ -1244,7 +1193,6 @@ static void cls_uart_init(struct channel_t *ch)
 	readb(&ch->ch_cls_uart->msr);
 }
 
-
 /*
  * Turns off UART.
  */
@@ -1253,7 +1201,6 @@ static void cls_uart_off(struct channel_t *ch)
 	writeb(0, &ch->ch_cls_uart->ier);
 }
 
-
 /*
  * cls_get_uarts_bytes_left.
  * Returns 0 is nothing left in the FIFO, returns 1 otherwise.
@@ -1283,7 +1230,6 @@ static uint cls_get_uart_bytes_left(struct channel_t *ch)
 	return left;
 }
 
-
 /*
  * cls_send_break.
  * Starts sending a break thru the UART.
@@ -1326,7 +1272,6 @@ static void cls_send_break(struct channel_t *ch, int msecs)
 	}
 }
 
-
 /*
  * cls_send_immediate_char.
  * Sends a specific character as soon as possible to the UART,
@@ -1348,7 +1293,6 @@ static void cls_vpd(struct dgnc_board *brd)
 	u8 __iomem           *re_map_vpdbase;/* Remapped memory of the card */
 	int i = 0;
 
-
 	vpdbase = pci_resource_start(brd->pdev, 3);
 
 	/* No VPD */
@@ -1370,4 +1314,3 @@ static void cls_vpd(struct dgnc_board *brd)
 	if (re_map_vpdbase)
 		iounmap(re_map_vpdbase);
 }
-
diff --git a/drivers/staging/dgnc/dgnc_cls.h b/drivers/staging/dgnc/dgnc_cls.h
index 465d79a6f75f..2597e36d38c4 100644
--- a/drivers/staging/dgnc/dgnc_cls.h
+++ b/drivers/staging/dgnc/dgnc_cls.h
@@ -11,21 +11,11 @@
  * but WITHOUT ANY WARRANTY, EXPRESS OR IMPLIED; without even the
  * implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
  * PURPOSE.  See the GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
- *
- *	NOTE: THIS IS A SHARED HEADER. DO NOT CHANGE CODING STYLE!!!
- *
  */
 
 #ifndef __DGNC_CLS_H
 #define __DGNC_CLS_H
 
-#include "dgnc_types.h"
-
-
 /************************************************************************
  * Per channel/port Classic UART structure				*
  ************************************************************************
@@ -35,15 +25,25 @@
  *			U = Unused.					*
  ************************************************************************/
 
+/*
+ * txrx    : WR RHR/THR - Holding reg
+ * ier     : WR IER - Interrupt Enable Reg
+ * isr_fcr : WR ISR/FCR - Interrupt Status Reg/Fifo Control Reg
+ * lcr     : WR LCR - Line Control Reg
+ * mcr     : WR MCR - Modem Control Reg
+ * lsr     : WR LSR - Line Status Reg
+ * msr     : WR MSG - Modem Status Reg
+ * spr     : WR SPR - Scratch pad Reg
+ */
 struct cls_uart_struct {
-	u8 txrx;		/* WR  RHR/THR - Holding Reg */
-	u8 ier;		/* WR  IER - Interrupt Enable Reg */
-	u8 isr_fcr;		/* WR  ISR/FCR - Interrupt Status Reg/Fifo Control Reg */
-	u8 lcr;		/* WR  LCR - Line Control Reg */
-	u8 mcr;		/* WR  MCR - Modem Control Reg */
-	u8 lsr;		/* WR  LSR - Line Status Reg */
-	u8 msr;		/* WR  MSR - Modem Status Reg */
-	u8 spr;		/* WR  SPR - Scratch Pad Reg */
+	u8 txrx;
+	u8 ier;
+	u8 isr_fcr;
+	u8 lcr;
+	u8 mcr;
+	u8 lsr;
+	u8 msr;
+	u8 spr;
 };
 
 /* Where to read the interrupt register (8bits) */
@@ -51,18 +51,15 @@ struct cls_uart_struct {
 
 #define UART_EXAR654_ENHANCED_REGISTER_SET 0xBF
 
-#define UART_16654_FCR_TXTRIGGER_8	0x0
 #define UART_16654_FCR_TXTRIGGER_16	0x10
-#define UART_16654_FCR_TXTRIGGER_32	0x20
-#define UART_16654_FCR_TXTRIGGER_56	0x30
-
-#define UART_16654_FCR_RXTRIGGER_8	0x0
 #define UART_16654_FCR_RXTRIGGER_16	0x40
 #define UART_16654_FCR_RXTRIGGER_56	0x80
-#define UART_16654_FCR_RXTRIGGER_60     0xC0
 
-#define UART_IIR_CTSRTS			0x20	/* Received CTS/RTS change of state */
-#define UART_IIR_RDI_TIMEOUT		0x0C    /* Receiver data TIMEOUT */
+/* Received CTS/RTS change of state */
+#define UART_IIR_CTSRTS			0x20
+
+/* Receiver data TIMEOUT */
+#define UART_IIR_RDI_TIMEOUT		0x0C
 
 /*
  * These are the EXTENDED definitions for the Exar 654's Interrupt
@@ -73,10 +70,6 @@ struct cls_uart_struct {
 #define UART_EXAR654_EFR_IXOFF    0x8     /* Transmit Xon1/Xoff1 */
 #define UART_EXAR654_EFR_RTSDTR   0x40    /* Auto RTS/DTR Flow Control Enable */
 #define UART_EXAR654_EFR_CTSDSR   0x80    /* Auto CTS/DSR Flow COntrol Enable */
-
-#define UART_EXAR654_XOFF_DETECT  0x1     /* Indicates whether chip saw an incoming XOFF char  */
-#define UART_EXAR654_XON_DETECT   0x2     /* Indicates whether chip saw an incoming XON char */
-
 #define UART_EXAR654_IER_XOFF     0x20    /* Xoff Interrupt Enable */
 #define UART_EXAR654_IER_RTSDTR   0x40    /* Output Interrupt Enable */
 #define UART_EXAR654_IER_CTSDSR   0x80    /* Input Interrupt Enable */
diff --git a/drivers/staging/dgnc/dgnc_driver.c b/drivers/staging/dgnc/dgnc_driver.c
index f177d3a258c2..805dc617e3a7 100644
--- a/drivers/staging/dgnc/dgnc_driver.c
+++ b/drivers/staging/dgnc/dgnc_driver.c
@@ -11,22 +11,6 @@
  * but WITHOUT ANY WARRANTY, EXPRESS OR IMPLIED; without even the
  * implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
  * PURPOSE.  See the GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
- *
- *
- *	NOTE TO LINUX KERNEL HACKERS:  DO NOT REFORMAT THIS CODE!
- *
- *	This is shared code between Digi's CVS archive and the
- *	Linux Kernel sources.
- *	Changing the source just for reformatting needlessly breaks
- *	our CVS diff history.
- *
- *	Send any bug fixes/changes to:  Eng.Linux at digi dot com.
- *	Thank you.
- *
  */
 
 
@@ -37,7 +21,6 @@
 #include <linux/sched.h>
 #include "dgnc_driver.h"
 #include "dgnc_pci.h"
-#include "dpacompat.h"
 #include "dgnc_mgmt.h"
 #include "dgnc_tty.h"
 #include "dgnc_cls.h"
@@ -60,7 +43,8 @@ static void		dgnc_init_globals(void);
 static int		dgnc_found_board(struct pci_dev *pdev, int id);
 static void		dgnc_cleanup_board(struct dgnc_board *brd);
 static void		dgnc_poll_handler(ulong dummy);
-static int		dgnc_init_one(struct pci_dev *pdev, const struct pci_device_id *ent);
+static int		dgnc_init_one(struct pci_dev *pdev,
+				      const struct pci_device_id *ent);
 static void		dgnc_do_remap(struct dgnc_board *brd);
 
 /*
@@ -92,8 +76,8 @@ static struct class *dgnc_class;
  * Poller stuff
  */
 static DEFINE_SPINLOCK(dgnc_poll_lock); /* Poll scheduling lock */
-static ulong		dgnc_poll_time;				/* Time of next poll */
-static uint		dgnc_poll_stop;				/* Used to tell poller to stop */
+static ulong		dgnc_poll_time; /* Time of next poll */
+static uint		dgnc_poll_stop; /* Used to tell poller to stop */
 static struct timer_list dgnc_poll_timer;
 
 
@@ -139,14 +123,6 @@ static struct pci_driver dgnc_driver = {
 	.id_table       = dgnc_pci_tbl,
 };
 
-
-char *dgnc_state_text[] = {
-	"Board Failed",
-	"Board Found",
-	"Board READY",
-};
-
-
 /************************************************************************
  *
  * Driver load/unload functions
@@ -214,7 +190,7 @@ static int __init dgnc_init_module(void)
 	 * If something went wrong in the scan, bail out of driver.
 	 */
 	if (rc < 0) {
-		/* Only unregister the pci driver if it was actually registered. */
+		/* Only unregister if it was actually registered. */
 		if (dgnc_NumBoards)
 			pci_unregister_driver(&dgnc_driver);
 		else
@@ -251,9 +227,9 @@ static int dgnc_start(void)
 	 * Register management/dpa devices
 	 */
 	rc = register_chrdev(0, "dgnc", &dgnc_BoardFops);
-	if (rc <= 0) {
+	if (rc < 0) {
 		pr_err(DRVSTR ": Can't register dgnc driver device (%d)\n", rc);
-		return -ENXIO;
+		return rc;
 	}
 	dgnc_Major = rc;
 
@@ -285,9 +261,7 @@ static int dgnc_start(void)
 
 	/* Start the poller */
 	spin_lock_irqsave(&dgnc_poll_lock, flags);
-	init_timer(&dgnc_poll_timer);
-	dgnc_poll_timer.function = dgnc_poll_handler;
-	dgnc_poll_timer.data = 0;
+	setup_timer(&dgnc_poll_timer, dgnc_poll_handler, 0);
 	dgnc_poll_time = jiffies + dgnc_jiffies_from_ms(dgnc_poll_tick);
 	dgnc_poll_timer.expires = dgnc_poll_time;
 	spin_unlock_irqrestore(&dgnc_poll_lock, flags);
@@ -364,7 +338,7 @@ static void dgnc_cleanup_board(struct dgnc_board *brd)
 
 		spin_lock_irqsave(&dgnc_global_lock, flags);
 		brd->msgbuf = NULL;
-		printk("%s", brd->msgbuf_head);
+		dev_dbg(&brd->pdev->dev, "%s\n", brd->msgbuf_head);
 		kfree(brd->msgbuf_head);
 		brd->msgbuf_head = NULL;
 		spin_unlock_irqrestore(&dgnc_global_lock, flags);
@@ -410,7 +384,7 @@ static int dgnc_found_board(struct pci_dev *pdev, int id)
 		return -ENOMEM;
 
 	/* make a temporary message buffer for the boot messages */
-	brd->msgbuf_head = kzalloc(sizeof(u8) * 8192, GFP_KERNEL);
+	brd->msgbuf_head = kcalloc(8192, sizeof(u8), GFP_KERNEL);
 	brd->msgbuf = brd->msgbuf_head;
 
 	if (!brd->msgbuf) {
@@ -553,7 +527,7 @@ static int dgnc_found_board(struct pci_dev *pdev, int id)
 
 		if (brd->re_map_membase) {
 
-			/* After remap is complete, we need to read and store the dvid */
+			/* Read and store the dvid after remapping */
 			brd->dvid = readb(brd->re_map_membase + 0x8D);
 
 			/* Get and store the board VPD, if it exists */
@@ -574,29 +548,19 @@ static int dgnc_found_board(struct pci_dev *pdev, int id)
 
 	rc = dgnc_tty_register(brd);
 	if (rc < 0) {
-		dgnc_tty_uninit(brd);
 		pr_err(DRVSTR ": Can't register tty devices (%d)\n", rc);
-		brd->state = BOARD_FAILED;
-		brd->dpastatus = BD_NOFEP;
 		goto failed;
 	}
 
 	rc = dgnc_finalize_board_init(brd);
 	if (rc < 0) {
 		pr_err(DRVSTR ": Can't finalize board init (%d)\n", rc);
-		brd->state = BOARD_FAILED;
-		brd->dpastatus = BD_NOFEP;
-
 		goto failed;
 	}
 
 	rc = dgnc_tty_init(brd);
 	if (rc < 0) {
-		dgnc_tty_uninit(brd);
 		pr_err(DRVSTR ": Can't init tty devices (%d)\n", rc);
-		brd->state = BOARD_FAILED;
-		brd->dpastatus = BD_NOFEP;
-
 		goto failed;
 	}
 
@@ -606,11 +570,13 @@ static int dgnc_found_board(struct pci_dev *pdev, int id)
 	dgnc_create_ports_sysfiles(brd);
 
 	/* init our poll helper tasklet */
-	tasklet_init(&brd->helper_tasklet, brd->bd_ops->tasklet, (unsigned long) brd);
+	tasklet_init(&brd->helper_tasklet,
+		     brd->bd_ops->tasklet,
+		     (unsigned long) brd);
 
 	spin_lock_irqsave(&dgnc_global_lock, flags);
 	brd->msgbuf = NULL;
-	printk("%s", brd->msgbuf_head);
+	dev_dbg(&brd->pdev->dev, "%s\n", brd->msgbuf_head);
 	kfree(brd->msgbuf_head);
 	brd->msgbuf_head = NULL;
 	spin_unlock_irqrestore(&dgnc_global_lock, flags);
@@ -628,6 +594,9 @@ static int dgnc_found_board(struct pci_dev *pdev, int id)
 	return 0;
 
 failed:
+	dgnc_tty_uninit(brd);
+	brd->state = BOARD_FAILED;
+	brd->dpastatus = BD_NOFEP;
 
 	return -ENXIO;
 
@@ -708,7 +677,7 @@ static void dgnc_poll_handler(ulong dummy)
 
 		spin_lock_irqsave(&brd->bd_lock, flags);
 
-		/* If board is in a failed state, don't bother scheduling a tasklet */
+		/* If board is in a failed state don't schedule a tasklet */
 		if (brd->state == BOARD_FAILED) {
 			spin_unlock_irqrestore(&brd->bd_lock, flags);
 			continue;
@@ -729,11 +698,9 @@ static void dgnc_poll_handler(ulong dummy)
 	new_time = dgnc_poll_time - jiffies;
 
 	if ((ulong) new_time >= 2 * dgnc_poll_tick)
-		dgnc_poll_time = jiffies +  dgnc_jiffies_from_ms(dgnc_poll_tick);
+		dgnc_poll_time = jiffies + dgnc_jiffies_from_ms(dgnc_poll_tick);
 
-	init_timer(&dgnc_poll_timer);
-	dgnc_poll_timer.function = dgnc_poll_handler;
-	dgnc_poll_timer.data = 0;
+	setup_timer(&dgnc_poll_timer, dgnc_poll_handler, 0);
 	dgnc_poll_timer.expires = dgnc_poll_time;
 	spin_unlock_irqrestore(&dgnc_poll_lock, flags);
 
diff --git a/drivers/staging/dgnc/dgnc_driver.h b/drivers/staging/dgnc/dgnc_driver.h
index a8157eba28da..f77fed57b873 100644
--- a/drivers/staging/dgnc/dgnc_driver.h
+++ b/drivers/staging/dgnc/dgnc_driver.h
@@ -12,12 +12,6 @@
  * implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
  * PURPOSE.  See the GNU General Public License for more details.
  *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
- *
- *	NOTE: THIS IS A SHARED HEADER. DO NOT CHANGE CODING STYLE!!!
- *
  *************************************************************************
  *
  * Driver includes
@@ -31,9 +25,7 @@
 #include <linux/tty.h>	  /* To pick up the various tty structs/defines */
 #include <linux/interrupt.h>	/* For irqreturn_t type */
 
-#include "dgnc_types.h"		/* Additional types needed by the Digi header files */
 #include "digi.h"		/* Digi specific ioctl header */
-#include "dgnc_kcompat.h"	/* Kernel 2.4/2.6 compat includes */
 #include "dgnc_sysfs.h"		/* Support for SYSFS */
 
 /*************************************************************************
@@ -46,6 +38,7 @@
 #define	PROCSTR		"dgnc"			/* /proc entries	 */
 #define	DEVSTR		"/dev/dg/dgnc"		/* /dev entries		 */
 #define	DRVSTR		"dgnc"			/* Driver name string	 */
+#define	DG_PART		"40002369_F"		/* RPM part number	 */
 
 #define TRC_TO_CONSOLE 1
 
@@ -211,8 +204,8 @@ struct dgnc_board {
 	struct tty_driver	PrintDriver;
 	char		PrintName[200];
 
-	uint		dgnc_Major_Serial_Registered;
-	uint		dgnc_Major_TransparentPrint_Registered;
+	bool		dgnc_Major_Serial_Registered;
+	bool		dgnc_Major_TransparentPrint_Registered;
 
 	uint		dgnc_Serial_Major;
 	uint		dgnc_TransparentPrint_Major;
@@ -398,11 +391,10 @@ struct channel_t {
 /*
  * Our Global Variables.
  */
-extern uint		dgnc_Major;		/* Our driver/mgmt major	*/
-extern int		dgnc_poll_tick;		/* Poll interval - 20 ms	*/
-extern spinlock_t	dgnc_global_lock;	/* Driver global spinlock	*/
-extern uint		dgnc_NumBoards;		/* Total number of boards	*/
-extern struct dgnc_board	*dgnc_Board[MAXBOARDS];	/* Array of board structs	*/
-extern char		*dgnc_state_text[];	/* Array of state text		*/
+extern uint		dgnc_Major;		/* Our driver/mgmt major */
+extern int		dgnc_poll_tick;		/* Poll interval - 20 ms */
+extern spinlock_t	dgnc_global_lock;	/* Driver global spinlock */
+extern uint		dgnc_NumBoards;		/* Total number of boards */
+extern struct dgnc_board	*dgnc_Board[MAXBOARDS];	/* Array of board structs */
 
 #endif
diff --git a/drivers/staging/dgnc/dgnc_kcompat.h b/drivers/staging/dgnc/dgnc_kcompat.h
deleted file mode 100644
index 566cad0d33e7..000000000000
--- a/drivers/staging/dgnc/dgnc_kcompat.h
+++ /dev/null
@@ -1,46 +0,0 @@
-/*
- * Copyright 2004 Digi International (www.digi.com)
- *      Scott H Kilau <Scott_Kilau at digi dot com>
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2, or (at your option)
- * any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY, EXPRESS OR IMPLIED; without even the
- * implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
- * PURPOSE.  See the GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
- *
- *	NOTE: THIS IS A SHARED HEADER. DO NOT CHANGE CODING STYLE!!!
- *
- *************************************************************************
- *
- * This file is intended to contain all the kernel "differences" between the
- * various kernels that we support.
- *
- *************************************************************************/
-
-#ifndef __DGNC_KCOMPAT_H
-#define __DGNC_KCOMPAT_H
-
-#if !defined(TTY_FLIPBUF_SIZE)
-# define TTY_FLIPBUF_SIZE 512
-#endif
-
-
-/* Sparse stuff */
-# ifndef __user
-#  define __user
-#  define __kernel
-#  define __safe
-#  define __force
-#  define __chk_user_ptr(x) (void)0
-# endif
-
-
-#endif /* ! __DGNC_KCOMPAT_H */
diff --git a/drivers/staging/dgnc/dgnc_mgmt.c b/drivers/staging/dgnc/dgnc_mgmt.c
index 5544a8e7f4bc..b13318a82fe1 100644
--- a/drivers/staging/dgnc/dgnc_mgmt.c
+++ b/drivers/staging/dgnc/dgnc_mgmt.c
@@ -11,22 +11,6 @@
  * but WITHOUT ANY WARRANTY, EXPRESS OR IMPLIED; without even the
  * implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
  * PURPOSE.  See the GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
- *
- *
- *	NOTE TO LINUX KERNEL HACKERS:  DO NOT REFORMAT THIS CODE!
- *
- *	This is shared code between Digi's CVS archive and the
- *	Linux Kernel sources.
- *	Changing the source just for reformatting needlessly breaks
- *	our CVS diff history.
- *
- *	Send any bug fixes/changes to:  Eng.Linux at digi dot com.
- *	Thank you.
- *
  */
 
 /************************************************************************
@@ -46,9 +30,7 @@
 
 #include "dgnc_driver.h"
 #include "dgnc_pci.h"
-#include "dgnc_kcompat.h"	/* Kernel 2.4/2.6 compat includes */
 #include "dgnc_mgmt.h"
-#include "dpacompat.h"
 
 
 /* Our "in use" variables, to enforce 1 open only */
@@ -153,8 +135,7 @@ long dgnc_mgmt_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 		if (copy_from_user(&brd, uarg, sizeof(int)))
 			return -EFAULT;
 
-		if ((brd < 0) || (brd > dgnc_NumBoards) ||
-		    (dgnc_NumBoards == 0))
+		if (brd < 0 || brd >= dgnc_NumBoards)
 			return -ENODEV;
 
 		memset(&di, 0, sizeof(di));
@@ -196,11 +177,11 @@ long dgnc_mgmt_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 		channel = ni.channel;
 
 		/* Verify boundaries on board */
-		if ((board > dgnc_NumBoards) || (dgnc_NumBoards == 0))
+		if (board >= dgnc_NumBoards)
 			return -ENODEV;
 
 		/* Verify boundaries on channel */
-		if ((channel < 0) || (channel > dgnc_Board[board]->nasync))
+		if (channel >= dgnc_Board[board]->nasync)
 			return -ENODEV;
 
 		ch = dgnc_Board[board]->channels[channel];
diff --git a/drivers/staging/dgnc/dgnc_mgmt.h b/drivers/staging/dgnc/dgnc_mgmt.h
index 567f687b18dd..708abe9594d4 100644
--- a/drivers/staging/dgnc/dgnc_mgmt.h
+++ b/drivers/staging/dgnc/dgnc_mgmt.h
@@ -11,12 +11,6 @@
  * but WITHOUT ANY WARRANTY, EXPRESS OR IMPLIED; without even the
  * implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
  * PURPOSE.  See the GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
- *
- *	NOTE: THIS IS A SHARED HEADER. DO NOT CHANGE CODING STYLE!!!
  */
 
 #ifndef __DGNC_MGMT_H
diff --git a/drivers/staging/dgnc/dgnc_neo.c b/drivers/staging/dgnc/dgnc_neo.c
index c9a8a9825cfb..f5a4d365115f 100644
--- a/drivers/staging/dgnc/dgnc_neo.c
+++ b/drivers/staging/dgnc/dgnc_neo.c
@@ -11,22 +11,6 @@
  * but WITHOUT ANY WARRANTY, EXPRESS OR IMPLIED; without even the
  * implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
  * PURPOSE.  See the GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
- *
- *
- *	NOTE TO LINUX KERNEL HACKERS:  DO NOT REFORMAT THIS CODE!
- *
- *	This is shared code between Digi's CVS archive and the
- *	Linux Kernel sources.
- *	Changing the source just for reformatting needlessly breaks
- *	our CVS diff history.
- *
- *	Send any bug fixes/changes to:  Eng.Linux at digi dot com.
- *	Thank you.
- *
  */
 
 
@@ -119,7 +103,7 @@ static inline void neo_set_cts_flow_control(struct channel_t *ch)
 
 	/* Turn on auto CTS flow control */
 #if 1
-	ier |= (UART_17158_IER_CTSDSR);
+	ier |= UART_17158_IER_CTSDSR;
 #else
 	ier &= ~(UART_17158_IER_CTSDSR);
 #endif
@@ -127,7 +111,7 @@ static inline void neo_set_cts_flow_control(struct channel_t *ch)
 	efr |= (UART_17158_EFR_ECB | UART_17158_EFR_CTSDSR);
 
 	/* Turn off auto Xon flow control */
-	efr &= ~(UART_17158_EFR_IXON);
+	efr &= ~UART_17158_EFR_IXON;
 
 	/* Why? Becuz Exar's spec says we have to zero it out before setting it */
 	writeb(0, &ch->ch_neo_uart->efr);
@@ -155,15 +139,15 @@ static inline void neo_set_rts_flow_control(struct channel_t *ch)
 
 	/* Turn on auto RTS flow control */
 #if 1
-	ier |= (UART_17158_IER_RTSDTR);
+	ier |= UART_17158_IER_RTSDTR;
 #else
 	ier &= ~(UART_17158_IER_RTSDTR);
 #endif
 	efr |= (UART_17158_EFR_ECB | UART_17158_EFR_RTSDTR);
 
 	/* Turn off auto Xoff flow control */
-	ier &= ~(UART_17158_IER_XOFF);
-	efr &= ~(UART_17158_EFR_IXOFF);
+	ier &= ~UART_17158_IER_XOFF;
+	efr &= ~UART_17158_EFR_IXOFF;
 
 	/* Why? Becuz Exar's spec says we have to zero it out before setting it */
 	writeb(0, &ch->ch_neo_uart->efr);
@@ -185,7 +169,7 @@ static inline void neo_set_rts_flow_control(struct channel_t *ch)
 	 * RTS/DTR# output pin (MCR bit-0 or 1 to logic 1 after
 	 * it is enabled.
 	 */
-	ch->ch_mostat |= (UART_MCR_RTS);
+	ch->ch_mostat |= UART_MCR_RTS;
 
 	neo_pci_posting_flush(ch->ch_bd);
 }
@@ -197,8 +181,8 @@ static inline void neo_set_ixon_flow_control(struct channel_t *ch)
 	unsigned char efr = readb(&ch->ch_neo_uart->efr);
 
 	/* Turn off auto CTS flow control */
-	ier &= ~(UART_17158_IER_CTSDSR);
-	efr &= ~(UART_17158_EFR_CTSDSR);
+	ier &= ~UART_17158_IER_CTSDSR;
+	efr &= ~UART_17158_EFR_CTSDSR;
 
 	/* Turn on auto Xon flow control */
 	efr |= (UART_17158_EFR_ECB | UART_17158_EFR_IXON);
@@ -234,11 +218,11 @@ static inline void neo_set_ixoff_flow_control(struct channel_t *ch)
 	unsigned char efr = readb(&ch->ch_neo_uart->efr);
 
 	/* Turn off auto RTS flow control */
-	ier &= ~(UART_17158_IER_RTSDTR);
-	efr &= ~(UART_17158_EFR_RTSDTR);
+	ier &= ~UART_17158_IER_RTSDTR;
+	efr &= ~UART_17158_EFR_RTSDTR;
 
 	/* Turn on auto Xoff flow control */
-	ier |= (UART_17158_IER_XOFF);
+	ier |= UART_17158_IER_XOFF;
 	efr |= (UART_17158_EFR_ECB | UART_17158_EFR_IXOFF);
 
 	/* Why? Becuz Exar's spec says we have to zero it out before setting it */
@@ -272,11 +256,11 @@ static inline void neo_set_no_input_flow_control(struct channel_t *ch)
 	unsigned char efr = readb(&ch->ch_neo_uart->efr);
 
 	/* Turn off auto RTS flow control */
-	ier &= ~(UART_17158_IER_RTSDTR);
-	efr &= ~(UART_17158_EFR_RTSDTR);
+	ier &= ~UART_17158_IER_RTSDTR;
+	efr &= ~UART_17158_EFR_RTSDTR;
 
 	/* Turn off auto Xoff flow control */
-	ier &= ~(UART_17158_IER_XOFF);
+	ier &= ~UART_17158_IER_XOFF;
 	if (ch->ch_c_iflag & IXON)
 		efr &= ~(UART_17158_EFR_IXOFF);
 	else
@@ -312,12 +296,12 @@ static inline void neo_set_no_output_flow_control(struct channel_t *ch)
 	unsigned char efr = readb(&ch->ch_neo_uart->efr);
 
 	/* Turn off auto CTS flow control */
-	ier &= ~(UART_17158_IER_CTSDSR);
-	efr &= ~(UART_17158_EFR_CTSDSR);
+	ier &= ~UART_17158_IER_CTSDSR;
+	efr &= ~UART_17158_EFR_CTSDSR;
 
 	/* Turn off auto Xon flow control */
 	if (ch->ch_c_iflag & IXOFF)
-		efr &= ~(UART_17158_EFR_IXON);
+		efr &= ~UART_17158_EFR_IXON;
 	else
 		efr &= ~(UART_17158_EFR_ECB | UART_17158_EFR_IXON);
 
@@ -407,7 +391,7 @@ static inline void neo_parse_isr(struct dgnc_board *brd, uint port)
 	if (!brd || brd->magic != DGNC_BOARD_MAGIC)
 		return;
 
-	if (port > brd->maxports)
+	if (port >= brd->maxports)
 		return;
 
 	ch = brd->channels[port];
@@ -537,7 +521,7 @@ static inline void neo_parse_lsr(struct dgnc_board *brd, uint port)
 	if (!brd || brd->magic != DGNC_BOARD_MAGIC)
 		return;
 
-	if (port > brd->maxports)
+	if (port >= brd->maxports)
 		return;
 
 	ch = brd->channels[port];
@@ -1019,7 +1003,7 @@ static irqreturn_t neo_intr(int irq, void *voidbrd)
 			 */
 
 			/* Verify the port is in range. */
-			if (port > brd->nasync)
+			if (port >= brd->nasync)
 				continue;
 
 			ch = brd->channels[port];
@@ -1203,7 +1187,7 @@ static void neo_copy_data_from_uart_to_queue(struct channel_t *ch)
 		 * IBM pSeries platform.
 		 * 15 bytes max appears to be the magic number.
 		 */
-		n = min((uint) n, (uint) 12);
+		n = min_t(uint, n, 12);
 
 		/*
 		 * Since we are grabbing the linestatus register, which
@@ -1436,16 +1420,13 @@ static void neo_copy_data_from_queue_to_uart(struct channel_t *ch)
 	spin_lock_irqsave(&ch->ch_lock, flags);
 
 	/* No data to write to the UART */
-	if (ch->ch_w_tail == ch->ch_w_head) {
-		spin_unlock_irqrestore(&ch->ch_lock, flags);
-		return;
-	}
+	if (ch->ch_w_tail == ch->ch_w_head)
+		goto exit_unlock;
 
 	/* If port is "stopped", don't send any data to the UART */
-	if ((ch->ch_flags & CH_FORCED_STOP) || (ch->ch_flags & CH_BREAK_SENDING)) {
-		spin_unlock_irqrestore(&ch->ch_lock, flags);
-		return;
-	}
+	if ((ch->ch_flags & CH_FORCED_STOP) ||
+		 (ch->ch_flags & CH_BREAK_SENDING))
+		goto exit_unlock;
 
 	/*
 	 * If FIFOs are disabled. Send data directly to txrx register
@@ -1486,27 +1467,23 @@ static void neo_copy_data_from_queue_to_uart(struct channel_t *ch)
 			ch->ch_w_tail &= WQUEUEMASK;
 			ch->ch_txcount++;
 		}
-		spin_unlock_irqrestore(&ch->ch_lock, flags);
-		return;
+
+		goto exit_unlock;
 	}
 
 	/*
 	 * We have to do it this way, because of the EXAR TXFIFO count bug.
 	 */
 	if ((ch->ch_bd->dvid & 0xf0) < UART_XR17E158_DVID) {
-		if (!(ch->ch_flags & (CH_TX_FIFO_EMPTY | CH_TX_FIFO_LWM))) {
-			spin_unlock_irqrestore(&ch->ch_lock, flags);
-			return;
-		}
+		if (!(ch->ch_flags & (CH_TX_FIFO_EMPTY | CH_TX_FIFO_LWM)))
+			goto exit_unlock;
 
 		len_written = 0;
 
 		n = readb(&ch->ch_neo_uart->tfifo);
 
-		if ((unsigned int) n > ch->ch_t_tlevel) {
-			spin_unlock_irqrestore(&ch->ch_lock, flags);
-			return;
-		}
+		if ((unsigned int) n > ch->ch_t_tlevel)
+			goto exit_unlock;
 
 		n = UART_17158_TX_FIFOSIZE - ch->ch_t_tlevel;
 	} else {
@@ -1570,6 +1547,7 @@ static void neo_copy_data_from_queue_to_uart(struct channel_t *ch)
 		ch->ch_flags &= ~(CH_TX_FIFO_EMPTY | CH_TX_FIFO_LWM);
 	}
 
+exit_unlock:
 	spin_unlock_irqrestore(&ch->ch_lock, flags);
 }
 
diff --git a/drivers/staging/dgnc/dgnc_neo.h b/drivers/staging/dgnc/dgnc_neo.h
index 1a4abb128693..c528df5a0e5a 100644
--- a/drivers/staging/dgnc/dgnc_neo.h
+++ b/drivers/staging/dgnc/dgnc_neo.h
@@ -11,19 +11,11 @@
  * but WITHOUT ANY WARRANTY, EXPRESS OR IMPLIED; without even the
  * implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
  * PURPOSE.  See the GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
- *
- *	NOTE: THIS IS A SHARED HEADER. DO NOT CHANGE CODING STYLE!!!
- *
  */
 
 #ifndef __DGNC_NEO_H
 #define __DGNC_NEO_H
 
-#include "dgnc_types.h"
 #include "dgnc_driver.h"
 
 /************************************************************************
diff --git a/drivers/staging/dgnc/dgnc_pci.h b/drivers/staging/dgnc/dgnc_pci.h
index 5b6f76d98aa7..617d40d1ec19 100644
--- a/drivers/staging/dgnc/dgnc_pci.h
+++ b/drivers/staging/dgnc/dgnc_pci.h
@@ -11,12 +11,6 @@
  * but WITHOUT ANY WARRANTY, EXPRESS OR IMPLIED; without even the
  * implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
  * PURPOSE.  See the GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
- *
- *	NOTE: THIS IS A SHARED HEADER. DO NOT CHANGE CODING STYLE!!!
  */
 
 #ifndef __DGNC_PCI_H
diff --git a/drivers/staging/dgnc/dgnc_sysfs.c b/drivers/staging/dgnc/dgnc_sysfs.c
index 2fd34ca70c59..65551d190515 100644
--- a/drivers/staging/dgnc/dgnc_sysfs.c
+++ b/drivers/staging/dgnc/dgnc_sysfs.c
@@ -11,22 +11,6 @@
  * but WITHOUT ANY WARRANTY, EXPRESS OR IMPLIED; without even the
  * implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
  * PURPOSE.  See the GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
- *
- *
- *      NOTE TO LINUX KERNEL HACKERS:  DO NOT REFORMAT THIS CODE!
- *
- *      This is shared code between Digi's CVS archive and the
- *      Linux Kernel sources.
- *      Changing the source just for reformatting needlessly breaks
- *      our CVS diff history.
- *
- *      Send any bug fixes/changes to:  Eng.Linux at digi dot com.
- *      Thank you.
- *
  */
 
 
@@ -91,7 +75,7 @@ void dgnc_create_driver_sysfiles(struct pci_driver *dgnc_driver)
 	rc |= driver_create_file(driverfs, &driver_attr_maxboards);
 	rc |= driver_create_file(driverfs, &driver_attr_pollrate);
 	if (rc)
-		printk(KERN_ERR "DGNC: sysfs driver_create_file failed!\n");
+		pr_err("DGNC: sysfs driver_create_file failed!\n");
 }
 
 
@@ -360,7 +344,7 @@ void dgnc_create_ports_sysfiles(struct dgnc_board *bd)
 	rc |= device_create_file(&(bd->pdev->dev), &dev_attr_vpd);
 	rc |= device_create_file(&(bd->pdev->dev), &dev_attr_serial_number);
 	if (rc)
-		printk(KERN_ERR "DGNC: sysfs device_create_file failed!\n");
+		dev_err(&bd->pdev->dev, "dgnc: sysfs device_create_file failed!\n");
 }
 
 
diff --git a/drivers/staging/dgnc/dgnc_sysfs.h b/drivers/staging/dgnc/dgnc_sysfs.h
index 68c0de5898a4..be0f90a6768b 100644
--- a/drivers/staging/dgnc/dgnc_sysfs.h
+++ b/drivers/staging/dgnc/dgnc_sysfs.h
@@ -11,20 +11,13 @@
  * but WITHOUT ANY WARRANTY, EXPRESS OR IMPLIED; without even the
  * implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
  * PURPOSE.  See the GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
- *
- *	NOTE: THIS IS A SHARED HEADER. DO NOT CHANGE CODING STYLE!!!
  */
 
 #ifndef __DGNC_SYSFS_H
 #define __DGNC_SYSFS_H
 
-#include "dgnc_driver.h"
-
 #include <linux/device.h>
+#include "dgnc_driver.h"
 
 struct dgnc_board;
 struct channel_t;
@@ -44,6 +37,4 @@ extern int dgnc_tty_class_destroy(void);
 extern void dgnc_create_tty_sysfs(struct un_t *un, struct device *c);
 extern void dgnc_remove_tty_sysfs(struct device *c);
 
-
-
 #endif
diff --git a/drivers/staging/dgnc/dgnc_tty.c b/drivers/staging/dgnc/dgnc_tty.c
index f81a375f8bc1..ce4187f60cb4 100644
--- a/drivers/staging/dgnc/dgnc_tty.c
+++ b/drivers/staging/dgnc/dgnc_tty.c
@@ -11,21 +11,6 @@
  * but WITHOUT ANY WARRANTY, EXPRESS OR IMPLIED; without even the
  * implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
  * PURPOSE.  See the GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
- *
- *
- *	NOTE TO LINUX KERNEL HACKERS:  DO NOT REFORMAT THIS CODE!
- *
- *	This is shared code between Digi's CVS archive and the
- *	Linux Kernel sources.
- *	Changing the source just for reformatting needlessly breaks
- *	our CVS diff history.
- *
- *	Send any bug fixes/changes to:  Eng.Linux at digi dot com.
- *	Thank you.
  */
 
 /************************************************************************
@@ -44,6 +29,7 @@
 #include <linux/ctype.h>
 #include <linux/tty.h>
 #include <linux/tty_flip.h>
+#include <linux/types.h>
 #include <linux/serial_reg.h>
 #include <linux/slab.h>
 #include <linux/delay.h>	/* For udelay */
@@ -51,10 +37,8 @@
 #include <linux/pci.h>
 #include "dgnc_driver.h"
 #include "dgnc_tty.h"
-#include "dgnc_types.h"
 #include "dgnc_neo.h"
 #include "dgnc_cls.h"
-#include "dpacompat.h"
 #include "dgnc_sysfs.h"
 #include "dgnc_utils.h"
 
@@ -236,7 +220,7 @@ int dgnc_tty_register(struct dgnc_board *brd)
 				"Can't register tty device (%d)\n", rc);
 			return rc;
 		}
-		brd->dgnc_Major_Serial_Registered = TRUE;
+		brd->dgnc_Major_Serial_Registered = true;
 	}
 
 	/*
@@ -286,7 +270,7 @@ int dgnc_tty_register(struct dgnc_board *brd)
 				rc);
 			return rc;
 		}
-		brd->dgnc_Major_TransparentPrint_Registered = TRUE;
+		brd->dgnc_Major_TransparentPrint_Registered = true;
 	}
 
 	dgnc_BoardsByMajor[brd->SerialDriver.major] = brd;
@@ -424,7 +408,7 @@ void dgnc_tty_uninit(struct dgnc_board *brd)
 			tty_unregister_device(&brd->SerialDriver, i);
 		}
 		tty_unregister_driver(&brd->SerialDriver);
-		brd->dgnc_Major_Serial_Registered = FALSE;
+		brd->dgnc_Major_Serial_Registered = false;
 	}
 
 	if (brd->dgnc_Major_TransparentPrint_Registered) {
@@ -435,7 +419,7 @@ void dgnc_tty_uninit(struct dgnc_board *brd)
 			tty_unregister_device(&brd->PrintDriver, i);
 		}
 		tty_unregister_driver(&brd->PrintDriver);
-		brd->dgnc_Major_TransparentPrint_Registered = FALSE;
+		brd->dgnc_Major_TransparentPrint_Registered = false;
 	}
 
 	kfree(brd->SerialDriver.ttys);
@@ -507,7 +491,7 @@ void dgnc_input(struct channel_t *ch)
 {
 	struct dgnc_board *bd;
 	struct tty_struct *tp;
-	struct tty_ldisc *ld;
+	struct tty_ldisc *ld = NULL;
 	uint	rmask;
 	ushort	head;
 	ushort	tail;
@@ -539,10 +523,8 @@ void dgnc_input(struct channel_t *ch)
 	tail = ch->ch_r_tail & rmask;
 	data_len = (head - tail) & rmask;
 
-	if (data_len == 0) {
-		spin_unlock_irqrestore(&ch->ch_lock, flags);
-		return;
-	}
+	if (data_len == 0)
+		goto exit_unlock;
 
 	/*
 	 * If the device is not open, or CREAD is off,
@@ -556,17 +538,14 @@ void dgnc_input(struct channel_t *ch)
 		/* Force queue flow control to be released, if needed */
 		dgnc_check_queue_flow_control(ch);
 
-		spin_unlock_irqrestore(&ch->ch_lock, flags);
-		return;
+		goto exit_unlock;
 	}
 
 	/*
 	 * If we are throttled, simply don't read any data.
 	 */
-	if (ch->ch_flags & CH_FORCED_STOPI) {
-		spin_unlock_irqrestore(&ch->ch_lock, flags);
-		return;
-	}
+	if (ch->ch_flags & CH_FORCED_STOPI)
+		goto exit_unlock;
 
 	flip_len = TTY_FLIPBUF_SIZE;
 
@@ -604,12 +583,8 @@ void dgnc_input(struct channel_t *ch)
 		}
 	}
 
-	if (len <= 0) {
-		spin_unlock_irqrestore(&ch->ch_lock, flags);
-		if (ld)
-			tty_ldisc_deref(ld);
-		return;
-	}
+	if (len <= 0)
+		goto exit_unlock;
 
 	/*
 	 * The tty layer in the kernel has changed in 2.6.16+.
@@ -677,6 +652,12 @@ void dgnc_input(struct channel_t *ch)
 
 	if (ld)
 		tty_ldisc_deref(ld);
+	return;
+
+exit_unlock:
+	spin_unlock_irqrestore(&ch->ch_lock, flags);
+	if (ld)
+		tty_ldisc_deref(ld);
 }
 
 
@@ -886,10 +867,6 @@ void dgnc_check_queue_flow_control(struct channel_t *ch)
 				ch->ch_stops_sent++;
 			}
 		}
-		/* No FLOW */
-		else {
-			/* Empty... Can't do anything about the impending overflow... */
-		}
 	}
 
 	/*
@@ -1064,7 +1041,7 @@ static int dgnc_tty_open(struct tty_struct *tty, struct file *file)
 	spin_lock_irqsave(&brd->bd_lock, flags);
 
 	/* If opened device is greater than our number of ports, bail. */
-	if (PORT_NUM(minor) > brd->nasync) {
+	if (PORT_NUM(minor) >= brd->nasync) {
 		spin_unlock_irqrestore(&brd->bd_lock, flags);
 		return -ENXIO;
 	}
@@ -1777,10 +1754,8 @@ static int dgnc_tty_write(struct tty_struct *tty,
 	/*
 	 * Bail if no space left.
 	 */
-	if (count <= 0) {
-		spin_unlock_irqrestore(&ch->ch_lock, flags);
-		return 0;
-	}
+	if (count <= 0)
+		goto exit_retry;
 
 	/*
 	 * Output the printer ON string, if we are in terminal mode, but
@@ -1807,10 +1782,8 @@ static int dgnc_tty_write(struct tty_struct *tty,
 	/*
 	 * If there is nothing left to copy, or I can't handle any more data, leave.
 	 */
-	if (count <= 0) {
-		spin_unlock_irqrestore(&ch->ch_lock, flags);
-		return 0;
-	}
+	if (count <= 0)
+		goto exit_retry;
 
 	if (from_user) {
 
@@ -1896,6 +1869,11 @@ static int dgnc_tty_write(struct tty_struct *tty,
 	}
 
 	return count;
+
+exit_retry:
+
+	spin_unlock_irqrestore(&ch->ch_lock, flags);
+	return 0;
 }
 
 
diff --git a/drivers/staging/dgnc/dgnc_tty.h b/drivers/staging/dgnc/dgnc_tty.h
index 3975f0407143..21d3369b875c 100644
--- a/drivers/staging/dgnc/dgnc_tty.h
+++ b/drivers/staging/dgnc/dgnc_tty.h
@@ -11,12 +11,6 @@
  * but WITHOUT ANY WARRANTY, EXPRESS OR IMPLIED; without even the
  * implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
  * PURPOSE.  See the GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
- *
- *	NOTE: THIS IS A SHARED HEADER. DO NOT CHANGE CODING STYLE!!!
  */
 
 #ifndef __DGNC_TTY_H
diff --git a/drivers/staging/dgnc/dgnc_types.h b/drivers/staging/dgnc/dgnc_types.h
deleted file mode 100644
index 3aafcedbb0d9..000000000000
--- a/drivers/staging/dgnc/dgnc_types.h
+++ /dev/null
@@ -1,33 +0,0 @@
-/*
- * Copyright 2003 Digi International (www.digi.com)
- *	Scott H Kilau <Scott_Kilau at digi dot com>
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2, or (at your option)
- * any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY, EXPRESS OR IMPLIED; without even the
- * implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
- * PURPOSE.  See the GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
- *
- *	NOTE: THIS IS A SHARED HEADER. DO NOT CHANGE CODING STYLE!!!
- */
-
-#ifndef __DGNC_TYPES_H
-#define __DGNC_TYPES_H
-
-#ifndef TRUE
-# define TRUE 1
-#endif
-
-#ifndef FALSE
-# define FALSE 0
-#endif
-
-#endif
diff --git a/drivers/staging/dgnc/dgnc_utils.c b/drivers/staging/dgnc/dgnc_utils.c
index 80b51332292c..f76de82908d3 100644
--- a/drivers/staging/dgnc/dgnc_utils.c
+++ b/drivers/staging/dgnc/dgnc_utils.c
@@ -16,55 +16,3 @@ int dgnc_ms_sleep(ulong ms)
 	schedule_timeout((ms * HZ) / 1000);
 	return signal_pending(current);
 }
-
-/*
- *      dgnc_ioctl_name() : Returns a text version of each ioctl value.
- */
-char *dgnc_ioctl_name(int cmd)
-{
-	switch (cmd) {
-
-	case TCGETA:		return "TCGETA";
-	case TCGETS:		return "TCGETS";
-	case TCSETA:		return "TCSETA";
-	case TCSETS:		return "TCSETS";
-	case TCSETAW:		return "TCSETAW";
-	case TCSETSW:		return "TCSETSW";
-	case TCSETAF:		return "TCSETAF";
-	case TCSETSF:		return "TCSETSF";
-	case TCSBRK:		return "TCSBRK";
-	case TCXONC:		return "TCXONC";
-	case TCFLSH:		return "TCFLSH";
-	case TIOCGSID:		return "TIOCGSID";
-
-	case TIOCGETD:		return "TIOCGETD";
-	case TIOCSETD:		return "TIOCSETD";
-	case TIOCGWINSZ:	return "TIOCGWINSZ";
-	case TIOCSWINSZ:	return "TIOCSWINSZ";
-
-	case TIOCMGET:		return "TIOCMGET";
-	case TIOCMSET:		return "TIOCMSET";
-	case TIOCMBIS:		return "TIOCMBIS";
-	case TIOCMBIC:		return "TIOCMBIC";
-
-	/* from digi.h */
-	case DIGI_SETA:		return "DIGI_SETA";
-	case DIGI_SETAW:	return "DIGI_SETAW";
-	case DIGI_SETAF:	return "DIGI_SETAF";
-	case DIGI_SETFLOW:	return "DIGI_SETFLOW";
-	case DIGI_SETAFLOW:	return "DIGI_SETAFLOW";
-	case DIGI_GETFLOW:	return "DIGI_GETFLOW";
-	case DIGI_GETAFLOW:	return "DIGI_GETAFLOW";
-	case DIGI_GETA:		return "DIGI_GETA";
-	case DIGI_GEDELAY:	return "DIGI_GEDELAY";
-	case DIGI_SEDELAY:	return "DIGI_SEDELAY";
-	case DIGI_GETCUSTOMBAUD: return "DIGI_GETCUSTOMBAUD";
-	case DIGI_SETCUSTOMBAUD: return "DIGI_SETCUSTOMBAUD";
-	case TIOCMODG:		return "TIOCMODG";
-	case TIOCMODS:		return "TIOCMODS";
-	case TIOCSDTR:		return "TIOCSDTR";
-	case TIOCCDTR:		return "TIOCCDTR";
-
-	default:		return "unknown";
-	}
-}
diff --git a/drivers/staging/dgnc/dgnc_utils.h b/drivers/staging/dgnc/dgnc_utils.h
index cebf60163a26..1164c3a09c6b 100644
--- a/drivers/staging/dgnc/dgnc_utils.h
+++ b/drivers/staging/dgnc/dgnc_utils.h
@@ -2,6 +2,5 @@
 #define __DGNC_UTILS_H
 
 int dgnc_ms_sleep(ulong ms);
-char *dgnc_ioctl_name(int cmd);
 
 #endif
diff --git a/drivers/staging/dgnc/digi.h b/drivers/staging/dgnc/digi.h
index d6e0b9f6b24a..d637a7802168 100644
--- a/drivers/staging/dgnc/digi.h
+++ b/drivers/staging/dgnc/digi.h
@@ -11,36 +11,11 @@
  * but WITHOUT ANY WARRANTY, EXPRESS OR IMPLIED; without even the
  * implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
  * PURPOSE.  See the GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
- *
- *	NOTE: THIS IS A SHARED HEADER. DO NOT CHANGE CODING STYLE!!!
  */
 
 #ifndef __DIGI_H
 #define __DIGI_H
 
-/************************************************************************
- ***	Definitions for Digi ditty(1) command.
- ************************************************************************/
-
-
-/*
- * Copyright (c) 1988-96 Digi International Inc., All Rights Reserved.
- */
-
-/************************************************************************
- * This module provides application access to special Digi
- * serial line enhancements which are not standard UNIX(tm) features.
- ************************************************************************/
-
-#if !defined(TIOCMODG)
-
-#define	TIOCMODG	(('d'<<8) | 250)	/* get modem ctrl state	*/
-#define	TIOCMODS	(('d'<<8) | 251)	/* set modem ctrl state	*/
-
 #ifndef TIOCM_LE
 #define		TIOCM_LE	0x01		/* line enable		*/
 #define		TIOCM_DTR	0x02		/* data terminal ready	*/
@@ -55,8 +30,6 @@
 #define		TIOCM_CD	TIOCM_CAR	/* carrier detect (alt)	*/
 #endif
 
-#endif
-
 #if !defined(TIOCMSET)
 #define	TIOCMSET	(('d'<<8) | 252)	/* set modem ctrl state	*/
 #define	TIOCMGET	(('d'<<8) | 253)	/* set modem ctrl state	*/
@@ -67,83 +40,24 @@
 #define	TIOCMBIS	(('d'<<8) | 255)	/* set modem ctrl state */
 #endif
 
-
-#if !defined(TIOCSDTR)
-#define	TIOCSDTR	(('e'<<8) | 0)		/* set DTR		*/
-#define	TIOCCDTR	(('e'<<8) | 1)		/* clear DTR		*/
-#endif
-
-/************************************************************************
- * Ioctl command arguments for DIGI parameters.
- ************************************************************************/
 #define DIGI_GETA	(('e'<<8) | 94)		/* Read params		*/
-
 #define DIGI_SETA	(('e'<<8) | 95)		/* Set params		*/
 #define DIGI_SETAW	(('e'<<8) | 96)		/* Drain & set params	*/
 #define DIGI_SETAF	(('e'<<8) | 97)		/* Drain, flush & set params */
-
-#define DIGI_KME	(('e'<<8) | 98)		/* Read/Write Host	*/
-						/* Adapter Memory	*/
-
-#define	DIGI_GETFLOW	(('e'<<8) | 99)		/* Get startc/stopc flow */
-						/* control characters	 */
-#define	DIGI_SETFLOW	(('e'<<8) | 100)	/* Set startc/stopc flow */
-						/* control characters	 */
-#define	DIGI_GETAFLOW	(('e'<<8) | 101)	/* Get Aux. startc/stopc */
-						/* flow control chars	 */
-#define	DIGI_SETAFLOW	(('e'<<8) | 102)	/* Set Aux. startc/stopc */
-						/* flow control chars	 */
-
-#define DIGI_GEDELAY	(('d'<<8) | 246)	/* Get edelay */
-#define DIGI_SEDELAY	(('d'<<8) | 247)	/* Set edelay */
-
-struct	digiflow_t {
-	unsigned char	startc;				/* flow cntl start char	*/
-	unsigned char	stopc;				/* flow cntl stop char	*/
-};
-
-
-#ifdef	FLOW_2200
-#define	F2200_GETA	(('e'<<8) | 104)	/* Get 2x36 flow cntl flags */
-#define	F2200_SETAW	(('e'<<8) | 105)	/* Set 2x36 flow cntl flags */
-#define		F2200_MASK	0x03		/* 2200 flow cntl bit mask  */
-#define		FCNTL_2200	0x01		/* 2x36 terminal flow cntl  */
-#define		PCNTL_2200	0x02		/* 2x36 printer flow cntl   */
-#define	F2200_XON	0xf8
-#define	P2200_XON	0xf9
-#define	F2200_XOFF	0xfa
-#define	P2200_XOFF	0xfb
-
-#define	FXOFF_MASK	0x03			/* 2200 flow status mask    */
-#define	RCVD_FXOFF	0x01			/* 2x36 Terminal XOFF rcvd  */
-#define	RCVD_PXOFF	0x02			/* 2x36 Printer XOFF rcvd   */
-#endif
-
-/************************************************************************
- * Values for digi_flags
- ************************************************************************/
-#define DIGI_IXON	0x0001		/* Handle IXON in the FEP	*/
+#define DIGI_GET_NI_INFO (('d'<<8) | 250) /* Non-intelligent state info */
+#define DIGI_LOOPBACK (('d'<<8) | 252) /*
+					* Enable/disable UART
+					* internal loopback
+					*/
 #define DIGI_FAST	0x0002		/* Fast baud rates		*/
 #define RTSPACE		0x0004		/* RTS input flow control	*/
 #define CTSPACE		0x0008		/* CTS output flow control	*/
-#define DSRPACE		0x0010		/* DSR output flow control	*/
-#define DCDPACE		0x0020		/* DCD output flow control	*/
-#define DTRPACE		0x0040		/* DTR input flow control	*/
 #define DIGI_COOK	0x0080		/* Cooked processing done in FEP */
 #define DIGI_FORCEDCD	0x0100		/* Force carrier		*/
 #define	DIGI_ALTPIN	0x0200		/* Alternate RJ-45 pin config	*/
-#define	DIGI_AIXON	0x0400		/* Aux flow control in fep	*/
 #define	DIGI_PRINTER	0x0800		/* Hold port open for flow cntrl*/
-#define DIGI_PP_INPUT	0x1000		/* Change parallel port to input*/
 #define DIGI_DTR_TOGGLE	0x2000		/* Support DTR Toggle           */
-#define DIGI_422	0x4000		/* for 422/232 selectable panel */
 #define DIGI_RTS_TOGGLE	0x8000		/* Support RTS Toggle		*/
-
-/************************************************************************
- * These options are not supported on the comxi.
- ************************************************************************/
-#define	DIGI_COMXI	(DIGI_FAST|DIGI_COOK|DSRPACE|DCDPACE|DTRPACE)
-
 #define DIGI_PLEN	28		/* String length		*/
 #define	DIGI_TSIZ	10		/* Terminal string len		*/
 
@@ -163,76 +77,6 @@ struct digi_t {
 };
 
 /************************************************************************
- * KME definitions and structures.
- ************************************************************************/
-#define	RW_IDLE		0	/* Operation complete			*/
-#define	RW_READ		1	/* Read Concentrator Memory		*/
-#define	RW_WRITE	2	/* Write Concentrator Memory		*/
-
-struct rw_t {
-	unsigned char	rw_req;		/* Request type			*/
-	unsigned char	rw_board;	/* Host Adapter board number	*/
-	unsigned char	rw_conc;	/* Concentrator number		*/
-	unsigned char	rw_reserved;	/* Reserved for expansion	*/
-	unsigned int	rw_addr;	/* Address in concentrator	*/
-	unsigned short	rw_size;	/* Read/write request length	*/
-	unsigned char	rw_data[128];	/* Data to read/write		*/
-};
-
-/***********************************************************************
- * Shrink Buffer and Board Information definitions and structures.
-
- ************************************************************************/
-			/* Board type return codes */
-#define	PCXI_TYPE 1     /* Board type at the designated port is a PC/Xi */
-#define PCXM_TYPE 2     /* Board type at the designated port is a PC/Xm */
-#define	PCXE_TYPE 3     /* Board type at the designated port is a PC/Xe */
-#define	MCXI_TYPE 4     /* Board type at the designated port is a MC/Xi */
-#define COMXI_TYPE 5     /* Board type at the designated port is a COM/Xi */
-
-			 /* Non-Zero Result codes. */
-#define RESULT_NOBDFND 1 /* A Digi product at that port is not config installed */
-#define RESULT_NODESCT 2 /* A memory descriptor was not obtainable */
-#define RESULT_NOOSSIG 3 /* FEP/OS signature was not detected on the board */
-#define RESULT_TOOSML  4 /* Too small an area to shrink.  */
-#define RESULT_NOCHAN  5 /* Channel structure for the board was not found */
-
-struct shrink_buf_struct {
-	unsigned int	shrink_buf_vaddr;	/* Virtual address of board */
-	unsigned int	shrink_buf_phys;	/* Physical address of board */
-	unsigned int	shrink_buf_bseg;	/* Amount of board memory */
-	unsigned int	shrink_buf_hseg;	/* '186 Beginning of Dual-Port */
-
-	unsigned int	shrink_buf_lseg;	/* '186 Beginning of freed memory */
-	unsigned int	shrink_buf_mseg;	/* Linear address from start of
-						   dual-port were freed memory
-						   begins, host viewpoint. */
-
-	unsigned int	shrink_buf_bdparam;	/* Parameter for xxmemon and
-						   xxmemoff */
-
-	unsigned int	shrink_buf_reserva;	/* Reserved */
-	unsigned int	shrink_buf_reservb;	/* Reserved */
-	unsigned int	shrink_buf_reservc;	/* Reserved */
-	unsigned int	shrink_buf_reservd;	/* Reserved */
-
-	unsigned char	shrink_buf_result;	/* Reason for call failing
-						   Zero is Good return */
-	unsigned char	shrink_buf_init;	/* Non-Zero if it caused an
-						   xxinit call. */
-
-	unsigned char	shrink_buf_anports;	/* Number of async ports  */
-	unsigned char	shrink_buf_snports;	/* Number of sync  ports */
-	unsigned char	shrink_buf_type;	/* Board type 1 = PC/Xi,
-							      2 = PC/Xm,
-							      3 = PC/Xe
-							      4 = MC/Xi
-							      5 = COMX/i */
-	unsigned char	shrink_buf_card;	/* Card number */
-
-};
-
-/************************************************************************
  * Structure to get driver status information
  ************************************************************************/
 struct digi_dinfo {
@@ -263,52 +107,6 @@ struct digi_info {
 
 #define	DIGI_GETBD	(('d'<<8) | 249)	/* get board info          */
 
-struct digi_stat {
-	unsigned int	info_chan;		/* Channel number (0 based)  */
-	unsigned int	info_brd;		/* Board number (0 based)  */
-	unsigned int	info_cflag;		/* cflag for channel       */
-	unsigned int	info_iflag;		/* iflag for channel       */
-	unsigned int	info_oflag;		/* oflag for channel       */
-	unsigned int	info_mstat;		/* mstat for channel       */
-	unsigned int	info_tx_data;		/* tx_data for channel       */
-	unsigned int	info_rx_data;		/* rx_data for channel       */
-	unsigned int	info_hflow;		/* hflow for channel       */
-	unsigned int	info_reserved[8];	/* for future expansion    */
-};
-
-#define	DIGI_GETSTAT	(('d'<<8) | 244)	/* get board info          */
-/************************************************************************
- *
- * Structure used with ioctl commands for per-channel information
- *
- ************************************************************************/
-struct digi_ch {
-	unsigned int	info_bdnum;		/* Board number (0 based)  */
-	unsigned int	info_channel;		/* Channel index number    */
-	unsigned int	info_ch_cflag;		/* Channel cflag	   */
-	unsigned int	info_ch_iflag;		/* Channel iflag	   */
-	unsigned int	info_ch_oflag;		/* Channel oflag	   */
-	unsigned int	info_chsize;		/* Channel structure size  */
-	unsigned int	info_sleep_stat;	/* sleep status		   */
-	dev_t		info_dev;		/* device number	   */
-	unsigned char	info_initstate;		/* Channel init state	   */
-	unsigned char	info_running;		/* Channel running state   */
-	int		reserved[8];		/* reserved for future use */
-};
-
-/*
-* This structure is used with the DIGI_FEPCMD ioctl to
-* tell the driver which port to send the command for.
-*/
-struct digi_cmd {
-	int	cmd;
-	int	word;
-	int	ncmds;
-	int	chan; /* channel index (zero based) */
-	int	bdid; /* board index (zero based) */
-};
-
-
 struct digi_getbuffer /* Struct for holding buffer use counts */
 {
 	unsigned long tIn;
@@ -328,63 +126,9 @@ struct digi_getcounter {
 	unsigned long tbytes;		/* number of bytes transmitted fully */
 };
 
-/*
-*  info_sleep_stat defines
-*/
-#define INFO_RUNWAIT	0x0001
-#define INFO_WOPEN	0x0002
-#define INFO_TTIOW	0x0004
-#define INFO_CH_RWAIT	0x0008
-#define INFO_CH_WEMPTY	0x0010
-#define INFO_CH_WLOW	0x0020
-#define INFO_XXBUF_BUSY 0x0040
-
-#define	DIGI_GETCH	(('d'<<8) | 245)	/* get board info          */
-
-/* Board type definitions */
-
-#define	SUBTYPE		0007
-#define	T_PCXI		0000
-#define T_PCXM		0001
-#define T_PCXE		0002
-#define T_PCXR		0003
-#define T_SP		0004
-#define T_SP_PLUS	0005
-#	define T_HERC	0000
-#	define T_HOU	0001
-#	define T_LON	0002
-#	define T_CHA	0003
-#define FAMILY		0070
-#define T_COMXI		0000
-#define T_PCXX		0010
-#define T_CX		0020
-#define T_EPC		0030
-#define	T_PCLITE	0040
-#define	T_SPXX		0050
-#define	T_AVXX		0060
-#define T_DXB		0070
-#define T_A2K_4_8	0070
-#define BUSTYPE		0700
-#define T_ISABUS	0000
-#define T_MCBUS		0100
-#define	T_EISABUS	0200
-#define	T_PCIBUS	0400
-
 /* Board State Definitions */
-
 #define	BD_RUNNING	0x0
-#define	BD_REASON	0x7f
-#define	BD_NOTFOUND	0x1
-#define	BD_NOIOPORT	0x2
-#define	BD_NOMEM	0x3
-#define	BD_NOBIOS	0x4
 #define	BD_NOFEP	0x5
-#define	BD_FAILED	0x6
-#define BD_ALLOCATED	0x7
-#define BD_TRIBOOT	0x8
-#define	BD_BADKME	0x80
-
-#define DIGI_SPOLL            (('d'<<8) | 254)  /* change poller rate   */
 
 #define DIGI_SETCUSTOMBAUD	_IOW('e', 106, int)	/* Set integer baud rate */
 #define DIGI_GETCUSTOMBAUD	_IOR('e', 107, int)	/* Get integer baud rate */
@@ -396,20 +140,40 @@ struct digi_getcounter {
 
 #define EV_OPU		0x0001		/* !<Output paused by client */
 #define EV_OPS		0x0002		/* !<Output paused by reqular sw flowctrl */
-#define EV_OPX		0x0004		/* !<Output paused by extra sw flowctrl */
-#define EV_OPH		0x0008		/* !<Output paused by hw flowctrl */
-#define EV_OPT		0x0800		/* !<Output paused for RTS Toggle predelay */
-
 #define EV_IPU		0x0010		/* !<Input paused unconditionally by user */
 #define EV_IPS		0x0020		/* !<Input paused by high/low water marks */
-#define EV_IPA		0x0400		/* !<Input paused by pattern alarm module */
-
 #define EV_TXB		0x0040		/* !<Transmit break pending */
-#define EV_TXI		0x0080		/* !<Transmit immediate pending */
-#define EV_TXF		0x0100		/* !<Transmit flowctrl char pending */
-#define EV_RXB		0x0200		/* !<Break received */
 
-#define EV_OPALL	0x080f		/* !<Output pause flags */
-#define EV_IPALL	0x0430		/* !<Input pause flags */
+/*
+ * This structure holds data needed for the intelligent <--> nonintelligent
+ * DPA translation
+ */
+struct ni_info {
+	int board;
+	int channel;
+	int dtr;
+	int rts;
+	int cts;
+	int dsr;
+	int ri;
+	int dcd;
+	int curtx;
+	int currx;
+	unsigned short iflag;
+	unsigned short oflag;
+	unsigned short cflag;
+	unsigned short lflag;
+	unsigned int mstat;
+	unsigned char hflow;
+	unsigned char xmit_stopped;
+	unsigned char recv_stopped;
+	unsigned int baud;
+};
+
+#define T_CLASSIC 0002
+#define T_PCIBUS 0400
+#define T_NEO_EXPRESS 0001
+#define T_NEO 0000
 
+#define TTY_FLIPBUF_SIZE 512
 #endif /* DIGI_H */
diff --git a/drivers/staging/dgnc/dpacompat.h b/drivers/staging/dgnc/dpacompat.h
deleted file mode 100644
index 33cb394524b8..000000000000
--- a/drivers/staging/dgnc/dpacompat.h
+++ /dev/null
@@ -1,115 +0,0 @@
-/*
- * Copyright 2003 Digi International (www.digi.com)
- *      Scott H Kilau <Scott_Kilau at digi dot com>
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2, or (at your option)
- * any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY, EXPRESS OR IMPLIED; without even the
- * implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
- * PURPOSE.  See the GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
- *
- *      NOTE: THIS IS A SHARED HEADER. DO NOT CHANGE CODING STYLE!!!
- */
-
-
-/*
- * This structure holds data needed for the intelligent <--> nonintelligent
- * DPA translation
- */
-struct ni_info {
-	int board;
-	int channel;
-	int dtr;
-	int rts;
-	int cts;
-	int dsr;
-	int ri;
-	int dcd;
-	int curtx;
-	int currx;
-	unsigned short iflag;
-	unsigned short oflag;
-	unsigned short cflag;
-	unsigned short lflag;
-
-	unsigned int mstat;
-	unsigned char hflow;
-
-	unsigned char xmit_stopped;
-	unsigned char recv_stopped;
-
-	unsigned int baud;
-};
-
-#define RW_READ		1
-#define RW_WRITE        2
-#define DIGI_KME        (('e'<<8) | 98)         /* Read/Write Host */
-
-#define SUBTYPE         0007
-#define T_PCXI          0000
-#define T_PCXEM         0001
-#define T_PCXE          0002
-#define T_PCXR          0003
-#define T_SP            0004
-#define T_SP_PLUS       0005
-
-#define T_HERC   0000
-#define T_HOU    0001
-#define T_LON    0002
-#define T_CHA    0003
-
-#define T_NEO	 0000
-#define T_NEO_EXPRESS  0001
-#define T_CLASSIC 0002
-
-#define FAMILY          0070
-#define T_COMXI         0000
-#define	T_NI		0000
-#define T_PCXX          0010
-#define T_CX            0020
-#define T_EPC           0030
-#define T_PCLITE        0040
-#define T_SPXX          0050
-#define T_AVXX          0060
-#define T_DXB           0070
-#define T_A2K_4_8       0070
-
-#define BUSTYPE         0700
-#define T_ISABUS        0000
-#define T_MCBUS         0100
-#define T_EISABUS       0200
-#define T_PCIBUS        0400
-
-/* Board State Definitions */
-
-#define BD_RUNNING      0x0
-#define BD_REASON       0x7f
-#define BD_NOTFOUND     0x1
-#define BD_NOIOPORT     0x2
-#define BD_NOMEM        0x3
-#define BD_NOBIOS       0x4
-#define BD_NOFEP        0x5
-#define BD_FAILED       0x6
-#define BD_ALLOCATED    0x7
-#define BD_TRIBOOT      0x8
-#define BD_BADKME       0x80
-
-#define DIGI_AIXON      0x0400          /* Aux flow control in fep */
-
-/* Ioctls needed for dpa operation */
-
-#define DIGI_GETDD      (('d'<<8) | 248)       /* get driver info      */
-#define DIGI_GETBD      (('d'<<8) | 249)       /* get board info       */
-#define DIGI_GET_NI_INFO (('d'<<8) | 250)	/* nonintelligent state snfo */
-
-/* Other special ioctls */
-#define DIGI_TIMERIRQ (('d'<<8) | 251)		/* Enable/disable RS_TIMER use */
-#define DIGI_LOOPBACK (('d'<<8) | 252)		/* Enable/disable UART internal loopback */
diff --git a/drivers/staging/emxx_udc/emxx_udc.c b/drivers/staging/emxx_udc/emxx_udc.c
index 4be646ce8a12..fbf82bc735cf 100644
--- a/drivers/staging/emxx_udc/emxx_udc.c
+++ b/drivers/staging/emxx_udc/emxx_udc.c
@@ -115,7 +115,7 @@ static void _nbu2ss_dump_register(struct nbu2ss_udc *udc)
 	pr_info("=== %s()\n", __func__);
 
 	if (udc == NULL) {
-		ERR("%s udc == NULL\n", __func__);
+		pr_err("%s udc == NULL\n", __func__);
 		return;
 	}
 
@@ -201,7 +201,7 @@ static u32 _nbu2ss_get_begin_ram_address(struct nbu2ss_udc *udc)
 	u32		num, buf_type;
 	u32		data, last_ram_adr, use_ram_size;
 
-	PT_EP_REGS	p_ep_regs;
+	struct ep_regs *p_ep_regs;
 
 	last_ram_adr = (D_RAM_SIZE_CTRL / sizeof(u32)) * 2;
 	use_ram_size = 0;
@@ -394,7 +394,7 @@ static void _nbu2ss_ep_dma_exit(struct nbu2ss_udc *udc, struct nbu2ss_ep *ep)
 {
 	u32		num;
 	u32		data;
-	PT_FC_REGS	preg = udc->p_regs;
+	struct fc_regs	*preg = udc->p_regs;
 
 	if (udc->vbus_active == 0)
 		return;		/* VBUS OFF */
@@ -425,7 +425,7 @@ static void _nbu2ss_ep_dma_exit(struct nbu2ss_udc *udc, struct nbu2ss_ep *ep)
 /* Abort DMA */
 static void _nbu2ss_ep_dma_abort(struct nbu2ss_udc *udc, struct nbu2ss_ep *ep)
 {
-	PT_FC_REGS	preg = udc->p_regs;
+	struct fc_regs	*preg = udc->p_regs;
 
 	_nbu2ss_bitclr(&preg->EP_DCR[ep->epnum-1].EP_DCR1, DCR1_EPn_REQEN);
 	mdelay(DMA_DISABLE_TIME);	/* DCR1_EPn_REQEN Clear */
@@ -443,7 +443,7 @@ static void _nbu2ss_ep_in_end(
 {
 	u32		data;
 	u32		num;
-	PT_FC_REGS	preg = udc->p_regs;
+	struct fc_regs	*preg = udc->p_regs;
 
 	if (length >= sizeof(u32))
 		return;
@@ -567,7 +567,7 @@ static int EP0_out_PIO(struct nbu2ss_udc *udc, u8 *pBuf, u32 length)
 	u32		i;
 	int		nret   = 0;
 	u32		iWordLength = 0;
-	USB_REG_ACCESS *pBuf32 = (USB_REG_ACCESS *)pBuf;
+	union usb_reg_access *pBuf32 = (union usb_reg_access *)pBuf;
 
 	/*------------------------------------------------------------*/
 	/* Read Length */
@@ -592,8 +592,8 @@ static int EP0_out_OverBytes(struct nbu2ss_udc *udc, u8 *pBuf, u32 length)
 {
 	u32		i;
 	u32		iReadSize = 0;
-	USB_REG_ACCESS  Temp32;
-	USB_REG_ACCESS *pBuf32 = (USB_REG_ACCESS *)pBuf;
+	union usb_reg_access  Temp32;
+	union usb_reg_access  *pBuf32 = (union usb_reg_access *)pBuf;
 
 	if ((0 < length) && (length < sizeof(u32))) {
 		Temp32.dw = _nbu2ss_readl(&udc->p_regs->EP0_READ);
@@ -613,7 +613,7 @@ static int EP0_in_PIO(struct nbu2ss_udc *udc, u8 *pBuf, u32 length)
 	u32		iMaxLength   = EP0_PACKETSIZE;
 	u32		iWordLength  = 0;
 	u32		iWriteLength = 0;
-	USB_REG_ACCESS *pBuf32 = (USB_REG_ACCESS *)pBuf;
+	union usb_reg_access  *pBuf32 = (union usb_reg_access *)pBuf;
 
 	/*------------------------------------------------------------*/
 	/* Transfer Length */
@@ -638,8 +638,8 @@ static int EP0_in_PIO(struct nbu2ss_udc *udc, u8 *pBuf, u32 length)
 static int EP0_in_OverBytes(struct nbu2ss_udc *udc, u8 *pBuf, u32 iRemainSize)
 {
 	u32		i;
-	USB_REG_ACCESS Temp32;
-	USB_REG_ACCESS *pBuf32 = (USB_REG_ACCESS *)pBuf;
+	union usb_reg_access  Temp32;
+	union usb_reg_access  *pBuf32 = (union usb_reg_access *)pBuf;
 
 	if ((0 < iRemainSize) && (iRemainSize < sizeof(u32))) {
 		for (i = 0 ; i < iRemainSize ; i++)
@@ -808,7 +808,7 @@ static int _nbu2ss_ep0_out_transfer(
 		return 0;		/* Short Packet Transfer End */
 
 	if (req->req.actual > req->req.length) {
-		ERR(" *** Overrun Error\n");
+		dev_err(udc->dev, " *** Overrun Error\n");
 		return -EOVERFLOW;
 	}
 
@@ -840,7 +840,7 @@ static int _nbu2ss_out_dma(
 	u32		burst = 1;
 	u32		data;
 	int		result = -EINVAL;
-	PT_FC_REGS	preg = udc->p_regs;
+	struct fc_regs	*preg = udc->p_regs;
 
 	if (req->dma_flag)
 		return 1;		/* DMA is forwarded */
@@ -900,10 +900,10 @@ static int _nbu2ss_epn_out_pio(
 	u32		i;
 	u32		data;
 	u32		iWordLength;
-	USB_REG_ACCESS	Temp32;
-	USB_REG_ACCESS	*pBuf32;
+	union usb_reg_access	Temp32;
+	union usb_reg_access	*pBuf32;
 	int		result = 0;
-	PT_FC_REGS	preg = udc->p_regs;
+	struct fc_regs	*preg = udc->p_regs;
 
 	if (req->dma_flag)
 		return 1;		/* DMA is forwarded */
@@ -912,7 +912,7 @@ static int _nbu2ss_epn_out_pio(
 		return 0;
 
 	pBuffer = (u8 *)req->req.buf;
-	pBuf32 = (USB_REG_ACCESS *)(pBuffer + req->req.actual);
+	pBuf32 = (union usb_reg_access *)(pBuffer + req->req.actual);
 
 	iWordLength = length / sizeof(u32);
 	if (iWordLength > 0) {
@@ -988,7 +988,7 @@ static int _nbu2ss_epn_out_transfer(
 	u32		num;
 	u32		iRecvLength;
 	int		result = 1;
-	PT_FC_REGS	preg = udc->p_regs;
+	struct fc_regs	*preg = udc->p_regs;
 
 	if (ep->epnum == 0)
 		return -EINVAL;
@@ -1026,8 +1026,8 @@ static int _nbu2ss_epn_out_transfer(
 	}
 
 	if (req->req.actual > req->req.length) {
-		ERR(" *** Overrun Error\n");
-		ERR(" *** actual = %d, length = %d\n",
+		dev_err(udc->dev, " Overrun Error\n");
+		dev_err(udc->dev, " actual = %d, length = %d\n",
 			req->req.actual, req->req.length);
 		result = -EOVERFLOW;
 	}
@@ -1051,7 +1051,7 @@ static int _nbu2ss_in_dma(
 	u32		iWriteLength;
 	u32		data;
 	int		result = -EINVAL;
-	PT_FC_REGS	preg = udc->p_regs;
+	struct fc_regs	*preg = udc->p_regs;
 
 	if (req->dma_flag)
 		return 1;		/* DMA is forwarded */
@@ -1123,17 +1123,17 @@ static int _nbu2ss_epn_in_pio(
 	u32		i;
 	u32		data;
 	u32		iWordLength;
-	USB_REG_ACCESS	Temp32;
-	USB_REG_ACCESS	*pBuf32 = NULL;
+	union usb_reg_access	Temp32;
+	union usb_reg_access	*pBuf32 = NULL;
 	int		result = 0;
-	PT_FC_REGS	preg = udc->p_regs;
+	struct fc_regs	*preg = udc->p_regs;
 
 	if (req->dma_flag)
 		return 1;		/* DMA is forwarded */
 
 	if (length > 0) {
 		pBuffer = (u8 *)req->req.buf;
-		pBuf32 = (USB_REG_ACCESS *)(pBuffer + req->req.actual);
+		pBuf32 = (union usb_reg_access *)(pBuffer + req->req.actual);
 
 		iWordLength = length / sizeof(u32);
 		if (iWordLength > 0) {
@@ -1347,7 +1347,7 @@ static void _nbu2ss_set_endpoint_stall(
 	u8		num, epnum;
 	u32		data;
 	struct nbu2ss_ep *ep;
-	PT_FC_REGS	preg = udc->p_regs;
+	struct fc_regs	*preg = udc->p_regs;
 
 	if ((ep_adrs == 0) || (ep_adrs == 0x80)) {
 		if (bstall) {
@@ -1426,7 +1426,7 @@ static void _nbu2ss_set_test_mode(struct nbu2ss_udc *udc, u32 mode)
 	if (mode > MAX_TEST_MODE_NUM)
 		return;
 
-	pr_info("SET FEATURE : test mode = %d\n", mode);
+	dev_info(udc->dev, "SET FEATURE : test mode = %d\n", mode);
 
 	data = _nbu2ss_readl(&udc->p_regs->USB_CONTROL);
 	data &= ~TEST_FORCE_ENABLE;
@@ -1454,7 +1454,7 @@ static int _nbu2ss_set_feature_device(
 		break;
 
 	case USB_DEVICE_TEST_MODE:
-		wIndex = wIndex >> 8;
+		wIndex >>= 8;
 		if (wIndex <= MAX_TEST_MODE_NUM)
 			result = 0;
 		break;
@@ -1471,7 +1471,7 @@ static int _nbu2ss_get_ep_stall(struct nbu2ss_udc *udc, u8 ep_adrs)
 {
 	u8		epnum;
 	u32		data = 0, bit_data;
-	PT_FC_REGS	preg = udc->p_regs;
+	struct fc_regs	*preg = udc->p_regs;
 
 	epnum = ep_adrs & ~USB_ENDPOINT_DIR_MASK;
 	if (epnum == 0) {
@@ -1566,7 +1566,7 @@ static void _nbu2ss_epn_set_stall(
 	u32	regdata;
 	int	limit_cnt = 0;
 
-	PT_FC_REGS	preg = udc->p_regs;
+	struct fc_regs	*preg = udc->p_regs;
 
 	if (ep->direct == USB_DIR_IN) {
 		for (limit_cnt = 0
@@ -1638,7 +1638,7 @@ static int std_req_get_status(struct nbu2ss_udc *udc)
 		_nbu2ss_ep0_in_transfer(udc, &udc->ep[0], &udc->ep0_req);
 
 	} else {
-		ERR("*** Error GET_STATUS\n");
+		dev_err(udc->dev, " Error GET_STATUS\n");
 	}
 
 	return result;
@@ -1671,7 +1671,7 @@ static int std_req_set_address(struct nbu2ss_udc *udc)
 	if (wValue != (wValue & 0x007F))
 		return -EINVAL;
 
-	wValue = wValue << USB_ADRS_SHIFT;
+	wValue <<= USB_ADRS_SHIFT;
 
 	_nbu2ss_writel(&udc->p_regs->USB_ADDRESS, wValue);
 	_nbu2ss_create_ep0_packet(udc, udc->ep0_buf, 0);
@@ -1885,8 +1885,8 @@ static inline void _nbu2ss_ep0_int(struct nbu2ss_udc *udc)
 			| STG_END_INT | EP0_OUT_NULL_INT);
 
 	if (status == 0) {
-		pr_info("--- %s Not Decode Interrupt\n", __func__);
-		pr_info("--- EP0_STATUS = 0x%08x\n", intr);
+		dev_info(udc->dev, "%s Not Decode Interrupt\n", __func__);
+		dev_info(udc->dev, "EP0_STATUS = 0x%08x\n", intr);
 		return;
 	}
 
@@ -1994,7 +1994,7 @@ static inline void _nbu2ss_epn_in_int(
 	int	result = 0;
 	u32	status;
 
-	PT_FC_REGS	preg = udc->p_regs;
+	struct fc_regs	*preg = udc->p_regs;
 
 	if (req->dma_flag)
 		return;		/* DMA is forwarded */
@@ -2090,7 +2090,7 @@ static inline void _nbu2ss_epn_out_dma_int(
 	u32		num;
 	u32		dmacnt, ep_dmacnt;
 	u32		mpkt;
-	PT_FC_REGS	preg = udc->p_regs;
+	struct fc_regs	*preg = udc->p_regs;
 
 	num = ep->epnum - 1;
 
@@ -2229,8 +2229,7 @@ static int _nbu2ss_nuke(struct nbu2ss_udc *udc,
 		return 0;
 
 	/* called with irqs blocked */
-	while (!list_empty(&ep->queue)) {
-		req = list_entry(ep->queue.next, struct nbu2ss_req, queue);
+	list_for_each_entry(req, &ep->queue, queue) {
 		_nbu2ss_ep_done(ep, req, status);
 	}
 
@@ -2257,11 +2256,6 @@ static int _nbu2ss_pullup(struct nbu2ss_udc *udc, int is_on)
 {
 	u32	reg_dt;
 
-	if (!udc) {
-		ERR("%s, bad param\n", __func__);
-		return -EINVAL;
-	}
-
 	if (udc->vbus_active == 0)
 		return -ESHUTDOWN;
 
@@ -2293,7 +2287,7 @@ static int _nbu2ss_pullup(struct nbu2ss_udc *udc, int is_on)
 /*-------------------------------------------------------------------------*/
 static void _nbu2ss_fifo_flush(struct nbu2ss_udc *udc, struct nbu2ss_ep *ep)
 {
-	PT_FC_REGS	p = udc->p_regs;
+	struct fc_regs	*p = udc->p_regs;
 
 	if (udc->vbus_active == 0)
 		return;
@@ -2350,7 +2344,7 @@ static int _nbu2ss_enable_controller(struct nbu2ss_udc *udc)
 		waitcnt++;
 		udelay(1);	/* 1us wait */
 		if (waitcnt == EPC_PLL_LOCK_COUNT) {
-			ERR("*** Reset Cancel failed\n");
+			dev_err(udc->dev, "*** Reset Cancel failed\n");
 			return -EINVAL;
 		}
 	};
@@ -2412,7 +2406,7 @@ static inline void _nbu2ss_check_vbus(struct nbu2ss_udc *udc)
 		udc->linux_suspended = 0;
 
 		_nbu2ss_reset_controller(udc);
-		pr_info(" ----- VBUS OFF\n");
+		dev_info(udc->dev, " ----- VBUS OFF\n");
 
 		if (udc->vbus_active == 1) {
 			/* VBUS OFF */
@@ -2438,7 +2432,7 @@ static inline void _nbu2ss_check_vbus(struct nbu2ss_udc *udc)
 		if (reg_dt == 0)
 			return;
 
-		pr_info(" ----- VBUS ON\n");
+		dev_info(udc->dev, " ----- VBUS ON\n");
 
 		if (udc->linux_suspended)
 			return;
@@ -2536,7 +2530,7 @@ static irqreturn_t _nbu2ss_udc_irq(int irq, void *_udc)
 	u32	epnum, int_bit;
 
 	struct nbu2ss_udc	*udc = (struct nbu2ss_udc *)_udc;
-	PT_FC_REGS		preg = udc->p_regs;
+	struct fc_regs	*preg = udc->p_regs;
 
 	if (gpio_get_value(VBUS_VALUE) == 0) {
 		_nbu2ss_writel(&preg->USB_INT_STA, ~USB_INT_STA_RW);
@@ -2612,13 +2606,13 @@ static int nbu2ss_ep_enable(
 	struct nbu2ss_udc	*udc;
 
 	if ((_ep == NULL) || (desc == NULL)) {
-		ERR(" *** %s, bad param\n", __func__);
+		pr_err(" *** %s, bad param\n", __func__);
 		return -EINVAL;
 	}
 
 	ep = container_of(_ep, struct nbu2ss_ep, ep);
 	if ((ep == NULL) || (ep->udc == NULL)) {
-		ERR(" *** %s, ep == NULL !!\n", __func__);
+		pr_err(" *** %s, ep == NULL !!\n", __func__);
 		return -EINVAL;
 	}
 
@@ -2626,7 +2620,7 @@ static int nbu2ss_ep_enable(
 	if ((ep_type == USB_ENDPOINT_XFER_CONTROL)
 		|| (ep_type == USB_ENDPOINT_XFER_ISOC)) {
 
-		ERR(" *** %s, bat bmAttributes\n", __func__);
+		pr_err(" *** %s, bat bmAttributes\n", __func__);
 		return -EINVAL;
 	}
 
@@ -2637,7 +2631,7 @@ static int nbu2ss_ep_enable(
 	if ((udc->driver == NULL)
 		|| (udc->gadget.speed == USB_SPEED_UNKNOWN)) {
 
-		ERR(" *** %s, udc !!\n", __func__);
+		dev_err(ep->udc->dev, " *** %s, udc !!\n", __func__);
 		return -ESHUTDOWN;
 	}
 
@@ -2672,13 +2666,13 @@ static int nbu2ss_ep_disable(struct usb_ep *_ep)
 	unsigned long		flags;
 
 	if (_ep == NULL) {
-		ERR(" *** %s, bad param\n", __func__);
+		pr_err(" *** %s, bad param\n", __func__);
 		return -EINVAL;
 	}
 
 	ep = container_of(_ep, struct nbu2ss_ep, ep);
 	if ((ep == NULL) || (ep->udc == NULL)) {
-		ERR(" *** %s, ep == NULL !!\n", __func__);
+		pr_err("udc: *** %s, ep == NULL !!\n", __func__);
 		return -EINVAL;
 	}
 
@@ -2742,10 +2736,10 @@ static int nbu2ss_ep_queue(
 	/* catch various bogus parameters */
 	if ((_ep == NULL) || (_req == NULL)) {
 		if (_ep == NULL)
-			ERR("*** %s --- _ep == NULL\n", __func__);
+			pr_err("udc: %s --- _ep == NULL\n", __func__);
 
 		if (_req == NULL)
-			ERR("*** %s --- _req == NULL\n", __func__);
+			pr_err("udc: %s --- _req == NULL\n", __func__);
 
 		return -EINVAL;
 	}
@@ -2756,13 +2750,13 @@ static int nbu2ss_ep_queue(
 	     || !list_empty(&req->queue))) {
 
 		if (!_req->complete)
-			ERR("*** %s --- !_req->complete\n", __func__);
+			pr_err("udc: %s --- !_req->complete\n", __func__);
 
 		if (!_req->buf)
-			ERR("*** %s --- !_req->buf\n", __func__);
+			pr_err("udc:%s --- !_req->buf\n", __func__);
 
 		if (!list_empty(&req->queue))
-			ERR("*** %s --- !list_empty(&req->queue)\n", __func__);
+			pr_err("%s --- !list_empty(&req->queue)\n", __func__);
 
 		return -EINVAL;
 	}
@@ -2773,12 +2767,13 @@ static int nbu2ss_ep_queue(
 /*	INFO("=== %s(ep%d), zero=%d\n", __func__, ep->epnum, _req->zero); */
 
 	if (udc->vbus_active == 0) {
-		pr_info("Can't ep_queue (VBUS OFF)\n");
+		dev_info(udc->dev, "Can't ep_queue (VBUS OFF)\n");
 		return -ESHUTDOWN;
 	}
 
 	if (unlikely(!udc->driver)) {
-		ERR("%s, bogus device state %p\n", __func__, udc->driver);
+		dev_err(udc->dev, "%s, bogus device state %p\n", __func__,
+				udc->driver);
 		return -ESHUTDOWN;
 	}
 
@@ -2794,7 +2789,7 @@ static int nbu2ss_ep_queue(
 		if (ep->virt_buf == NULL)
 			ep->virt_buf = (u8 *)dma_alloc_coherent(
 				NULL, PAGE_SIZE,
-				&ep->phys_buf, GFP_KERNEL | GFP_DMA);
+				&ep->phys_buf, GFP_ATOMIC | GFP_DMA);
 		if (ep->epnum > 0)  {
 			if (ep->direct == USB_DIR_IN)
 				memcpy(ep->virt_buf, req->req.buf,
@@ -2817,7 +2812,8 @@ static int nbu2ss_ep_queue(
 
 		result = _nbu2ss_start_transfer(udc, ep, req, FALSE);
 		if (result < 0) {
-			ERR(" *** %s, result = %d\n", __func__, result);
+			dev_err(udc->dev, " *** %s, result = %d\n", __func__,
+					result);
 			list_del(&req->queue);
 		} else if ((ep->epnum > 0) && (ep->direct == USB_DIR_OUT)) {
 #ifdef USE_DMA
@@ -2849,13 +2845,13 @@ static int nbu2ss_ep_dequeue(
 
 	/* catch various bogus parameters */
 	if ((_ep == NULL) || (_req == NULL)) {
-		/* ERR("%s, bad param(1)\n", __func__); */
+		/* pr_err("%s, bad param(1)\n", __func__); */
 		return -EINVAL;
 	}
 
 	ep = container_of(_ep, struct nbu2ss_ep, ep);
 	if (!ep) {
-		ERR("%s, ep == NULL !!\n", __func__);
+		pr_err("%s, ep == NULL !!\n", __func__);
 		return -EINVAL;
 	}
 
@@ -2895,19 +2891,19 @@ static int nbu2ss_ep_set_halt(struct usb_ep *_ep, int value)
 /*	INFO("=== %s()\n", __func__); */
 
 	if (!_ep) {
-		ERR("%s, bad param\n", __func__);
+		pr_err("%s, bad param\n", __func__);
 		return -EINVAL;
 	}
 
 	ep = container_of(_ep, struct nbu2ss_ep, ep);
 	if (!ep) {
-		ERR("%s, bad ep\n", __func__);
+		pr_err("%s, bad ep\n", __func__);
 		return -EINVAL;
 	}
 
 	udc = ep->udc;
 	if (!udc) {
-		ERR(" *** %s, bad udc\n", __func__);
+		dev_err(ep->udc->dev, " *** %s, bad udc\n", __func__);
 		return -EINVAL;
 	}
 
@@ -2944,24 +2940,24 @@ static int nbu2ss_ep_fifo_status(struct usb_ep *_ep)
 	struct nbu2ss_ep	*ep;
 	struct nbu2ss_udc	*udc;
 	unsigned long		flags;
-	PT_FC_REGS		preg;
+	struct fc_regs		*preg;
 
 /*	INFO("=== %s()\n", __func__); */
 
 	if (!_ep) {
-		ERR("%s, bad param\n", __func__);
+		pr_err("%s, bad param\n", __func__);
 		return -EINVAL;
 	}
 
 	ep = container_of(_ep, struct nbu2ss_ep, ep);
 	if (!ep) {
-		ERR("%s, bad ep\n", __func__);
+		pr_err("%s, bad ep\n", __func__);
 		return -EINVAL;
 	}
 
 	udc = ep->udc;
 	if (!udc) {
-		ERR("%s, bad udc\n", __func__);
+		dev_err(ep->udc->dev, "%s, bad udc\n", __func__);
 		return -EINVAL;
 	}
 
@@ -2997,19 +2993,19 @@ static void  nbu2ss_ep_fifo_flush(struct usb_ep *_ep)
 /*	INFO("=== %s()\n", __func__); */
 
 	if (!_ep) {
-		ERR("%s, bad param\n", __func__);
+		pr_err("udc: %s, bad param\n", __func__);
 		return;
 	}
 
 	ep = container_of(_ep, struct nbu2ss_ep, ep);
 	if (!_ep) {
-		ERR("%s, bad ep\n", __func__);
+		pr_err("udc: %s, bad ep\n", __func__);
 		return;
 	}
 
 	udc = ep->udc;
 	if (!udc) {
-		ERR("%s, bad udc\n", __func__);
+		dev_err(ep->udc->dev, "%s, bad udc\n", __func__);
 		return;
 	}
 
@@ -3053,13 +3049,13 @@ static int nbu2ss_gad_get_frame(struct usb_gadget *pgadget)
 /*	INFO("=== %s()\n", __func__); */
 
 	if (pgadget == NULL) {
-		ERR("%s, bad param\n", __func__);
+		pr_err("udc: %s, bad param\n", __func__);
 		return -EINVAL;
 	}
 
 	udc = container_of(pgadget, struct nbu2ss_udc, gadget);
 	if (udc == NULL) {
-		ERR("%s, udc == NULL\n", __func__);
+		dev_err(&pgadget->dev, "%s, udc == NULL\n", __func__);
 		return -EINVAL;
 	}
 
@@ -3083,19 +3079,19 @@ static int nbu2ss_gad_wakeup(struct usb_gadget *pgadget)
 /*	INFO("=== %s()\n", __func__); */
 
 	if (pgadget == NULL) {
-		ERR("%s, bad param\n", __func__);
+		pr_err("%s, bad param\n", __func__);
 		return -EINVAL;
 	}
 
 	udc = container_of(pgadget, struct nbu2ss_udc, gadget);
 	if (udc == NULL) {
-		ERR("%s, udc == NULL\n", __func__);
+		dev_err(&pgadget->dev, "%s, udc == NULL\n", __func__);
 		return -EINVAL;
 	}
 
 	data = gpio_get_value(VBUS_VALUE);
 	if (data == 0) {
-		pr_warn("VBUS LEVEL = %d\n", data);
+		dev_warn(&pgadget->dev, "VBUS LEVEL = %d\n", data);
 		return -EINVAL;
 	}
 
@@ -3123,7 +3119,7 @@ static int nbu2ss_gad_set_selfpowered(struct usb_gadget *pgadget,
 /*	INFO("=== %s()\n", __func__); */
 
 	if (pgadget == NULL) {
-		ERR("%s, bad param\n", __func__);
+		pr_err("%s, bad param\n", __func__);
 		return -EINVAL;
 	}
 
@@ -3152,7 +3148,7 @@ static int nbu2ss_gad_vbus_draw(struct usb_gadget *pgadget, unsigned mA)
 /*	INFO("=== %s()\n", __func__); */
 
 	if (pgadget == NULL) {
-		ERR("%s, bad param\n", __func__);
+		pr_err("%s, bad param\n", __func__);
 		return -EINVAL;
 	}
 
@@ -3174,7 +3170,7 @@ static int nbu2ss_gad_pullup(struct usb_gadget *pgadget, int is_on)
 /*	INFO("=== %s()\n", __func__); */
 
 	if (pgadget == NULL) {
-		ERR("%s, bad param\n", __func__);
+		pr_err("%s, bad param\n", __func__);
 		return -EINVAL;
 	}
 
@@ -3341,11 +3337,11 @@ static int nbu2ss_drv_probe(struct platform_device *pdev)
 				  0, driver_name, udc);
 
 	/* IO Memory */
-	udc->p_regs = (PT_FC_REGS)mmio_base;
+	udc->p_regs = (struct fc_regs *)mmio_base;
 
 	/* USB Function Controller Interrupt */
 	if (status != 0) {
-		ERR("request_irq(USB_UDC_IRQ_1) failed\n");
+		dev_err(udc->dev, "request_irq(USB_UDC_IRQ_1) failed\n");
 		goto cleanup1;
 	}
 
@@ -3365,7 +3361,7 @@ static int nbu2ss_drv_probe(struct platform_device *pdev)
 				udc);
 
 	if (status != 0) {
-		ERR("request_irq(INT_VBUS) failed\n");
+		dev_err(udc->dev, "request_irq(INT_VBUS) failed\n");
 		goto cleanup1;
 	}
 
diff --git a/drivers/staging/emxx_udc/emxx_udc.h b/drivers/staging/emxx_udc/emxx_udc.h
index 202e2dc72bba..c19168f78354 100644
--- a/drivers/staging/emxx_udc/emxx_udc.h
+++ b/drivers/staging/emxx_udc/emxx_udc.h
@@ -474,8 +474,8 @@
 
 /*===========================================================================*/
 /* Struct */
-/*------- T_EP_REGS */
-typedef struct _T_EP_REGS {
+/*------- ep_regs */
+struct ep_regs {
 	u32 EP_CONTROL;			/* EP Control */
 	u32 EP_STATUS;			/* EP Status */
 	u32 EP_INT_ENA;			/* EP Interrupt Enable */
@@ -484,18 +484,18 @@ typedef struct _T_EP_REGS {
 	u32 EP_LEN_DCNT;		/* EP Length & DMA count */
 	u32 EP_READ;			/* EP Read */
 	u32 EP_WRITE;			/* EP Write */
-} T_EP_REGS, *PT_EP_REGS;
+};
 
-/*------- T_EP_DCR */
-typedef struct _T_EP_DCR {
+/*------- ep_dcr */
+struct ep_dcr {
 	u32 EP_DCR1;			/* EP_DCR1 */
 	u32 EP_DCR2;			/* EP_DCR2 */
 	u32 EP_TADR;			/* EP_TADR */
 	u32 Reserved;			/* Reserved */
-} T_EP_DCR, *PT_EP_DCR;
+};
 
 /*------- Function Registers */
-typedef struct _T_FC_REGS {
+struct fc_regs {
 	u32 USB_CONTROL;		/* (0x0000) USB Control */
 	u32 USB_STATUS;			/* (0x0004) USB Status */
 	u32 USB_ADDRESS;		/* (0x0008) USB Address */
@@ -513,7 +513,7 @@ typedef struct _T_FC_REGS {
 	u32 EP0_READ;			/* (0x0038) EP0 Read */
 	u32 EP0_WRITE;			/* (0x003C) EP0 Write */
 
-	T_EP_REGS EP_REGS[REG_EP_NUM];	/* Endpoint Register */
+	struct ep_regs EP_REGS[REG_EP_NUM];	/* Endpoint Register */
 
 	u8 Reserved220[0x1000-0x220];	/* (0x0220:0x0FFF) Reserved */
 
@@ -531,11 +531,10 @@ typedef struct _T_FC_REGS {
 
 	u8 Reserved1028[0x110-0x28];	/* (0x1028:0x110F) Reserved */
 
-	T_EP_DCR EP_DCR[REG_EP_NUM];	/* */
+	struct ep_dcr EP_DCR[REG_EP_NUM];	/* */
 
 	u8 Reserved1200[0x1000-0x200];	/* Reserved */
-
-} __attribute__ ((aligned(32))) T_FC_REGS, *PT_FC_REGS;
+} __aligned(32);
 
 
 
@@ -631,18 +630,17 @@ struct nbu2ss_udc {
 
 	u32		curr_config;	/* Current Configuration Number */
 
-	PT_FC_REGS		p_regs;
+	struct fc_regs		*p_regs;
 };
 
 /* USB register access structure */
-typedef volatile union {
+union usb_reg_access {
 	struct {
 		unsigned char	DATA[4];
 	} byte;
 	unsigned int		dw;
-} USB_REG_ACCESS;
+};
 
 /*-------------------------------------------------------------------------*/
-#define ERR(stuff...)		printk(KERN_ERR "udc: " stuff)
 
 #endif  /* _LINUX_EMXX_H */
diff --git a/drivers/staging/fbtft/Kconfig b/drivers/staging/fbtft/Kconfig
index 995a9101a080..6cf0c58f538b 100644
--- a/drivers/staging/fbtft/Kconfig
+++ b/drivers/staging/fbtft/Kconfig
@@ -38,6 +38,12 @@ config FB_TFT_HX8353D
 	help
 	  Generic Framebuffer support for HX8353D
 
+config FB_TFT_ILI9163
+	tristate "FB driver for the ILI9163 LCD Controller"
+	depends on FB_TFT
+	help
+	  Generic Framebuffer support for ILI9163
+
 config FB_TFT_ILI9320
 	tristate "FB driver for the ILI9320 LCD Controller"
 	depends on FB_TFT
diff --git a/drivers/staging/fbtft/Makefile b/drivers/staging/fbtft/Makefile
index e773f0fdcfe8..9e73beee23f4 100644
--- a/drivers/staging/fbtft/Makefile
+++ b/drivers/staging/fbtft/Makefile
@@ -8,6 +8,7 @@ obj-$(CONFIG_FB_TFT_BD663474)    += fb_bd663474.o
 obj-$(CONFIG_FB_TFT_HX8340BN)    += fb_hx8340bn.o
 obj-$(CONFIG_FB_TFT_HX8347D)     += fb_hx8347d.o
 obj-$(CONFIG_FB_TFT_HX8353D)     += fb_hx8353d.o
+obj-$(CONFIG_FB_TFT_ILI9163)     += fb_ili9163.o
 obj-$(CONFIG_FB_TFT_ILI9320)     += fb_ili9320.o
 obj-$(CONFIG_FB_TFT_ILI9325)     += fb_ili9325.o
 obj-$(CONFIG_FB_TFT_ILI9340)     += fb_ili9340.o
diff --git a/drivers/staging/fbtft/README b/drivers/staging/fbtft/README
index bc89b5805f7b..ba4c74c92e4c 100644
--- a/drivers/staging/fbtft/README
+++ b/drivers/staging/fbtft/README
@@ -9,20 +9,20 @@ Development is done on a Raspberry Pi running the Raspbian "wheezy" distribution
 INSTALLATION
   Download kernel sources
 
-  From Linux 3.15  
+  From Linux 3.15
     cd drivers/video/fbdev/fbtft
     git clone https://github.com/notro/fbtft.git
-    
+
     Add to drivers/video/fbdev/Kconfig:   source "drivers/video/fbdev/fbtft/Kconfig"
     Add to drivers/video/fbdev/Makefile:  obj-y += fbtft/
-  
-  Before Linux 3.15  
+
+  Before Linux 3.15
     cd drivers/video
     git clone https://github.com/notro/fbtft.git
-    
+
     Add to drivers/video/Kconfig:   source "drivers/video/fbtft/Kconfig"
     Add to drivers/video/Makefile:  obj-y += fbtft/
-  
+
   Enable driver(s) in menuconfig and build the kernel
 
 
diff --git a/drivers/staging/fbtft/fb_agm1264k-fl.c b/drivers/staging/fbtft/fb_agm1264k-fl.c
index 9cc7d25cf0e5..8f5af1db852c 100644
--- a/drivers/staging/fbtft/fb_agm1264k-fl.c
+++ b/drivers/staging/fbtft/fb_agm1264k-fl.c
@@ -174,7 +174,7 @@ request_gpios_match(struct fbtft_par *par, const struct fbtft_gpio *gpio)
 
 /* This function oses to enter commands
  * first byte - destination controller 0 or 1
- * folowing - commands
+ * following - commands
  */
 static void write_reg8_bus8(struct fbtft_par *par, int len, ...)
 {
@@ -198,8 +198,8 @@ static void write_reg8_bus8(struct fbtft_par *par, int len, ...)
 
 	if (*buf > 1) {
 		va_end(args);
-		dev_err(par->info->device, "%s: Incorrect chip sellect request (%d)\n",
-			__func__, *buf);
+		dev_err(par->info->device,
+			"Incorrect chip select request (%d)\n", *buf);
 		return;
 	}
 
@@ -224,8 +224,8 @@ static void write_reg8_bus8(struct fbtft_par *par, int len, ...)
 		ret = par->fbtftops.write(par, par->buf, len * (sizeof(u8)));
 		if (ret < 0) {
 			va_end(args);
-			dev_err(par->info->device, "%s: write() failed and returned %d\n",
-				__func__, ret);
+			dev_err(par->info->device,
+				"write() failed and returned %d\n", ret);
 			return;
 		}
 	}
@@ -278,10 +278,13 @@ static int write_vmem(struct fbtft_par *par, size_t offset, size_t len)
 	int x, y;
 	int ret = 0;
 
-	/* buffer to convert RGB565 -> grayscale16 -> Ditherd image 1bpp */
+	/* buffer to convert RGB565 -> grayscale16 -> Dithered image 1bpp */
 	signed short *convert_buf = kmalloc(par->info->var.xres *
 		par->info->var.yres * sizeof(signed short), GFP_NOIO);
 
+	if (!convert_buf)
+		return -ENOMEM;
+
 	fbtft_par_dbg(DEBUG_WRITE_VMEM, par, "%s()\n", __func__);
 
 	/* converting to grayscale16 */
@@ -376,8 +379,8 @@ static int write_vmem(struct fbtft_par *par, size_t offset, size_t len)
 			ret = par->fbtftops.write(par, buf, len);
 			if (ret < 0)
 				dev_err(par->info->device,
-					"%s: write failed and returned: %d\n",
-					__func__, ret);
+					"write failed and returned: %d\n",
+					ret);
 		}
 		/* right half of display */
 		if (addr_win.xe >= par->info->var.xres / 2) {
@@ -390,7 +393,7 @@ static int write_vmem(struct fbtft_par *par, size_t offset, size_t len)
 			/* select right side (sc1)
 			 * set addr
 			 */
-			write_reg(par, 0x01, (1 << 6));
+			write_reg(par, 0x01, 1 << 6);
 			write_reg(par, 0x01, (0x17 << 3) | (u8)y);
 
 			/* write bitmap */
@@ -398,8 +401,8 @@ static int write_vmem(struct fbtft_par *par, size_t offset, size_t len)
 			par->fbtftops.write(par, buf, len);
 			if (ret < 0)
 				dev_err(par->info->device,
-					"%s: write failed and returned: %d\n",
-					__func__, ret);
+					"write failed and returned: %d\n",
+					ret);
 		}
 	}
 	kfree(convert_buf);
diff --git a/drivers/staging/fbtft/fb_bd663474.c b/drivers/staging/fbtft/fb_bd663474.c
index 7e00c609c7fe..17a2162a7e5b 100644
--- a/drivers/staging/fbtft/fb_bd663474.c
+++ b/drivers/staging/fbtft/fb_bd663474.c
@@ -47,49 +47,49 @@ static int init_display(struct fbtft_par *par)
 	/* Initialization sequence from Lib_UTFT */
 
 	/* oscillator start */
-	write_reg(par, 0x000,0x0001);	/*oscillator 0: stop, 1: operation */
+	write_reg(par, 0x000, 0x0001);	/*oscillator 0: stop, 1: operation */
 	mdelay(10);
 
 	/* Power settings */
-	write_reg(par, 0x100, 0x0000 ); /* power supply setup */
-	write_reg(par, 0x101, 0x0000 );
-	write_reg(par, 0x102, 0x3110 );
-	write_reg(par, 0x103, 0xe200 );
-	write_reg(par, 0x110, 0x009d );
-	write_reg(par, 0x111, 0x0022 );
-	write_reg(par, 0x100, 0x0120 );
-	mdelay( 20 );
-
-	write_reg(par, 0x100, 0x3120 );
-	mdelay( 80 );
+	write_reg(par, 0x100, 0x0000); /* power supply setup */
+	write_reg(par, 0x101, 0x0000);
+	write_reg(par, 0x102, 0x3110);
+	write_reg(par, 0x103, 0xe200);
+	write_reg(par, 0x110, 0x009d);
+	write_reg(par, 0x111, 0x0022);
+	write_reg(par, 0x100, 0x0120);
+	mdelay(20);
+
+	write_reg(par, 0x100, 0x3120);
+	mdelay(80);
 	/* Display control */
-	write_reg(par, 0x001, 0x0100 );
-	write_reg(par, 0x002, 0x0000 );
-	write_reg(par, 0x003, 0x1230 );
-	write_reg(par, 0x006, 0x0000 );
-	write_reg(par, 0x007, 0x0101 );
-	write_reg(par, 0x008, 0x0808 );
-	write_reg(par, 0x009, 0x0000 );
-	write_reg(par, 0x00b, 0x0000 );
-	write_reg(par, 0x00c, 0x0000 );
-	write_reg(par, 0x00d, 0x0018 );
+	write_reg(par, 0x001, 0x0100);
+	write_reg(par, 0x002, 0x0000);
+	write_reg(par, 0x003, 0x1230);
+	write_reg(par, 0x006, 0x0000);
+	write_reg(par, 0x007, 0x0101);
+	write_reg(par, 0x008, 0x0808);
+	write_reg(par, 0x009, 0x0000);
+	write_reg(par, 0x00b, 0x0000);
+	write_reg(par, 0x00c, 0x0000);
+	write_reg(par, 0x00d, 0x0018);
 	/* LTPS control settings */
-	write_reg(par, 0x012, 0x0000 );
-	write_reg(par, 0x013, 0x0000 );
-	write_reg(par, 0x018, 0x0000 );
-	write_reg(par, 0x019, 0x0000 );
-
-	write_reg(par, 0x203, 0x0000 );
-	write_reg(par, 0x204, 0x0000 );
-
-	write_reg(par, 0x210, 0x0000 );
-	write_reg(par, 0x211, 0x00ef );
-	write_reg(par, 0x212, 0x0000 );
-	write_reg(par, 0x213, 0x013f );
-	write_reg(par, 0x214, 0x0000 );
-	write_reg(par, 0x215, 0x0000 );
-	write_reg(par, 0x216, 0x0000 );
-	write_reg(par, 0x217, 0x0000 );
+	write_reg(par, 0x012, 0x0000);
+	write_reg(par, 0x013, 0x0000);
+	write_reg(par, 0x018, 0x0000);
+	write_reg(par, 0x019, 0x0000);
+
+	write_reg(par, 0x203, 0x0000);
+	write_reg(par, 0x204, 0x0000);
+
+	write_reg(par, 0x210, 0x0000);
+	write_reg(par, 0x211, 0x00ef);
+	write_reg(par, 0x212, 0x0000);
+	write_reg(par, 0x213, 0x013f);
+	write_reg(par, 0x214, 0x0000);
+	write_reg(par, 0x215, 0x0000);
+	write_reg(par, 0x216, 0x0000);
+	write_reg(par, 0x217, 0x0000);
 
 	/* Gray scale settings */
 	write_reg(par, 0x300, 0x5343);
@@ -104,18 +104,18 @@ static int init_display(struct fbtft_par *par)
 	write_reg(par, 0x309, 0x050a);
 
 	/* RAM access settings */
-	write_reg(par, 0x400, 0x4027 );
-	write_reg(par, 0x401, 0x0000 );
-	write_reg(par, 0x402, 0x0000 );  /* First screen drive position (1) */
-	write_reg(par, 0x403, 0x013f );  /* First screen drive position (2) */
-	write_reg(par, 0x404, 0x0000 );
-
-	write_reg(par, 0x200, 0x0000 );
-	write_reg(par, 0x201, 0x0000 );
-	write_reg(par, 0x100, 0x7120 );
-	write_reg(par, 0x007, 0x0103 );
-	mdelay( 10 );
-	write_reg(par, 0x007, 0x0113 );
+	write_reg(par, 0x400, 0x4027);
+	write_reg(par, 0x401, 0x0000);
+	write_reg(par, 0x402, 0x0000);  /* First screen drive position (1) */
+	write_reg(par, 0x403, 0x013f);  /* First screen drive position (2) */
+	write_reg(par, 0x404, 0x0000);
+
+	write_reg(par, 0x200, 0x0000);
+	write_reg(par, 0x201, 0x0000);
+	write_reg(par, 0x100, 0x7120);
+	write_reg(par, 0x007, 0x0103);
+	mdelay(10);
+	write_reg(par, 0x007, 0x0113);
 
 	return 0;
 }
diff --git a/drivers/staging/fbtft/fb_hx8340bn.c b/drivers/staging/fbtft/fb_hx8340bn.c
index 3939502f2c81..54528aa0c0ef 100644
--- a/drivers/staging/fbtft/fb_hx8340bn.c
+++ b/drivers/staging/fbtft/fb_hx8340bn.c
@@ -4,7 +4,7 @@
  * This display uses 9-bit SPI: Data/Command bit + 8 data bits
  * For platforms that doesn't support 9-bit, the driver is capable
  * of emulating this using 8-bit transfer.
- * This is done by transfering eight 9-bit words in 9 bytes.
+ * This is done by transferring eight 9-bit words in 9 bytes.
  *
  * Copyright (C) 2013 Noralf Tronnes
  *
@@ -47,8 +47,6 @@ MODULE_PARM_DESC(emulate, "Force emulation in 9-bit mode");
 
 static int init_display(struct fbtft_par *par)
 {
-	fbtft_par_dbg(DEBUG_INIT_DISPLAY, par, "%s()\n", __func__);
-
 	par->fbtftops.reset(par);
 
 	/* BTL221722-276L startup sequence, from datasheet */
@@ -108,11 +106,8 @@ static int init_display(struct fbtft_par *par)
 	return 0;
 }
 
-void set_addr_win(struct fbtft_par *par, int xs, int ys, int xe, int ye)
+static void set_addr_win(struct fbtft_par *par, int xs, int ys, int xe, int ye)
 {
-	fbtft_par_dbg(DEBUG_SET_ADDR_WIN, par,
-		"%s(xs=%d, ys=%d, xe=%d, ye=%d)\n", __func__, xs, ys, xe, ye);
-
 	write_reg(par, FBTFT_CASET, 0x00, xs, 0x00, xe);
 	write_reg(par, FBTFT_RASET, 0x00, ys, 0x00, ye);
 	write_reg(par, FBTFT_RAMWR);
@@ -120,8 +115,6 @@ void set_addr_win(struct fbtft_par *par, int xs, int ys, int xe, int ye)
 
 static int set_var(struct fbtft_par *par)
 {
-	fbtft_par_dbg(DEBUG_INIT_DISPLAY, par, "%s()\n", __func__);
-
 	/* MADCTL - Memory data access control */
 	/* RGB/BGR can be set with H/W pin SRGB and MADCTL BGR bit */
 #define MY (1 << 7)
@@ -129,7 +122,7 @@ static int set_var(struct fbtft_par *par)
 #define MV (1 << 5)
 	switch (par->info->var.rotate) {
 	case 0:
-		write_reg(par, 0x36, (par->bgr << 3));
+		write_reg(par, 0x36, par->bgr << 3);
 		break;
 	case 270:
 		write_reg(par, 0x36, MX | MV | (par->bgr << 3));
@@ -156,14 +149,12 @@ static int set_var(struct fbtft_par *par)
 static int set_gamma(struct fbtft_par *par, unsigned long *curves)
 {
 	unsigned long mask[] = {
-		0b1111, 0b1111, 0b11111, 0b1111, 0b1111, 0b1111, 0b11111,
-		0b111, 0b111, 0b111, 0b111, 0b111, 0b111, 0b11, 0b11,
-		0b1111, 0b1111, 0b11111, 0b1111, 0b1111, 0b1111, 0b11111,
-		0b111, 0b111, 0b111, 0b111, 0b111, 0b111, 0b0, 0b0 };
+		0x0f, 0x0f, 0x1f, 0x0f, 0x0f, 0x0f, 0x1f, 0x07, 0x07, 0x07,
+		0x07, 0x07, 0x07, 0x03, 0x03, 0x0f, 0x0f, 0x1f, 0x0f, 0x0f,
+		0x0f, 0x1f, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x00, 0x00,
+	};
 	int i, j;
 
-	fbtft_par_dbg(DEBUG_INIT_DISPLAY, par, "%s()\n", __func__);
-
 	/* apply mask */
 	for (i = 0; i < par->gamma.num_curves; i++)
 		for (j = 0; j < par->gamma.num_values; j++)
diff --git a/drivers/staging/fbtft/fb_hx8347d.c b/drivers/staging/fbtft/fb_hx8347d.c
index 8139a8f587b7..03ae95b4f79e 100644
--- a/drivers/staging/fbtft/fb_hx8347d.c
+++ b/drivers/staging/fbtft/fb_hx8347d.c
@@ -115,10 +115,9 @@ static void set_addr_win(struct fbtft_par *par, int xs, int ys, int xe, int ye)
 static int set_gamma(struct fbtft_par *par, unsigned long *curves)
 {
 	unsigned long mask[] = {
-		0b111111, 0b111111, 0b111111, 0b111111, 0b111111, 0b111111,
-		0b1111111, 0b1111111,
-		0b11111, 0b11111, 0b11111, 0b11111, 0b11111,
-		0b1111};
+		0x3f, 0x3f, 0x3f, 0x3f, 0x3f, 0x3f, 0x7f, 0x7f, 0x1f, 0x1f,
+		0x1f, 0x1f, 0x1f, 0x0f,
+	};
 	int i, j;
 	int acc = 0;
 
diff --git a/drivers/staging/fbtft/fb_hx8353d.c b/drivers/staging/fbtft/fb_hx8353d.c
index c9512dc5f4d3..d7f4308e1249 100644
--- a/drivers/staging/fbtft/fb_hx8353d.c
+++ b/drivers/staging/fbtft/fb_hx8353d.c
@@ -84,7 +84,7 @@ static void set_addr_win(struct fbtft_par *par, int xs, int ys, int xe, int ye)
 	/* column address */
 	write_reg(par, 0x2a, xs >> 8, xs & 0xff, xe >> 8, xe & 0xff);
 
-	/* row adress */
+	/* Row address */
 	write_reg(par, 0x2b, ys >> 8, ys & 0xff, ye >> 8, ye & 0xff);
 
 	/* memory write */
@@ -112,7 +112,7 @@ static int set_var(struct fbtft_par *par)
 		write_reg(par, 0x36, my | mv | (par->bgr << 3));
 		break;
 	case 180:
-		write_reg(par, 0x36, (par->bgr << 3));
+		write_reg(par, 0x36, par->bgr << 3);
 		break;
 	case 90:
 		write_reg(par, 0x36, mx | mv | (par->bgr << 3));
diff --git a/drivers/staging/fbtft/fb_ili9163.c b/drivers/staging/fbtft/fb_ili9163.c
new file mode 100644
index 000000000000..ed92a64306ff
--- /dev/null
+++ b/drivers/staging/fbtft/fb_ili9163.c
@@ -0,0 +1,303 @@
+/*
+ * FB driver for the ILI9163 LCD Controller
+ *
+ * Copyright (C) 2015 Kozhevnikov Anatoly
+ *
+ * Based on ili9325.c by Noralf Tronnes and
+ * .S.U.M.O.T.O.Y. by Max MC Costa (https://github.com/sumotoy/TFT_ILI9163C).
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/gpio.h>
+#include <linux/delay.h>
+
+#include "fbtft.h"
+
+#define DRVNAME		"fb_ili9163"
+#define WIDTH		128
+#define HEIGHT		128
+#define BPP		16
+#define FPS		30
+
+#ifdef GAMMA_ADJ
+#define GAMMA_LEN	15
+#define GAMMA_NUM	1
+#define DEFAULT_GAMMA	"36 29 12 22 1C 15 42 B7 2F 13 12 0A 11 0B 06\n"
+#endif
+
+/* ILI9163C commands */
+#define CMD_NOP		0x00 /* Non operation*/
+#define CMD_SWRESET	0x01 /* Soft Reset */
+#define CMD_SLPIN	0x10 /* Sleep ON */
+#define CMD_SLPOUT	0x11 /* Sleep OFF */
+#define CMD_PTLON	0x12 /* Partial Mode ON */
+#define CMD_NORML	0x13 /* Normal Display ON */
+#define CMD_DINVOF	0x20 /* Display Inversion OFF */
+#define CMD_DINVON	0x21 /* Display Inversion ON */
+#define CMD_GAMMASET	0x26 /* Gamma Set (0x01[1],0x02[2],0x04[3],0x08[4]) */
+#define CMD_DISPOFF	0x28 /* Display OFF */
+#define CMD_DISPON	0x29 /* Display ON */
+#define CMD_IDLEON	0x39 /* Idle Mode ON */
+#define CMD_IDLEOF	0x38 /* Idle Mode OFF */
+#define CMD_CLMADRS	0x2A /* Column Address Set */
+#define CMD_PGEADRS	0x2B /* Page Address Set */
+
+#define CMD_RAMWR	0x2C /* Memory Write */
+#define CMD_RAMRD	0x2E /* Memory Read */
+#define CMD_CLRSPACE	0x2D /* Color Space : 4K/65K/262K */
+#define CMD_PARTAREA	0x30 /* Partial Area */
+#define CMD_VSCLLDEF	0x33 /* Vertical Scroll Definition */
+#define CMD_TEFXLON	0x34 /* Tearing Effect Line ON */
+#define CMD_TEFXLOF	0x35 /* Tearing Effect Line OFF */
+#define CMD_MADCTL	0x36 /* Memory Access Control */
+
+#define CMD_PIXFMT	0x3A /* Interface Pixel Format */
+#define CMD_FRMCTR1	0xB1 /* Frame Rate Control
+				(In normal mode/Full colors) */
+#define CMD_FRMCTR2	0xB2 /* Frame Rate Control (In Idle mode/8-colors) */
+#define CMD_FRMCTR3	0xB3 /* Frame Rate Control
+				(In Partial mode/full colors) */
+#define CMD_DINVCTR	0xB4 /* Display Inversion Control */
+#define CMD_RGBBLK	0xB5 /* RGB Interface Blanking Porch setting */
+#define CMD_DFUNCTR	0xB6 /* Display Function set 5 */
+#define CMD_SDRVDIR	0xB7 /* Source Driver Direction Control */
+#define CMD_GDRVDIR	0xB8 /* Gate Driver Direction Control  */
+
+#define CMD_PWCTR1	0xC0 /* Power_Control1 */
+#define CMD_PWCTR2	0xC1 /* Power_Control2 */
+#define CMD_PWCTR3	0xC2 /* Power_Control3 */
+#define CMD_PWCTR4	0xC3 /* Power_Control4 */
+#define CMD_PWCTR5	0xC4 /* Power_Control5 */
+#define CMD_VCOMCTR1	0xC5 /* VCOM_Control 1 */
+#define CMD_VCOMCTR2	0xC6 /* VCOM_Control 2 */
+#define CMD_VCOMOFFS	0xC7 /* VCOM Offset Control */
+#define CMD_PGAMMAC	0xE0 /* Positive Gamma Correction Setting */
+#define CMD_NGAMMAC	0xE1 /* Negative Gamma Correction Setting */
+#define CMD_GAMRSEL	0xF2 /* GAM_R_SEL */
+
+/*
+This display:
+http://www.ebay.com/itm/Replace-Nokia-5110-LCD-1-44-Red-Serial-128X128-SPI-Color-TFT-LCD-Display-Module-/271422122271
+This particular display has a design error! The controller has 3 pins to
+configure to constrain the memory and resolution to a fixed dimension (in
+that case 128x128) but they leaved those pins configured for 128x160 so
+there was several pixel memory addressing problems.
+I solved by setup several parameters that dinamically fix the resolution as
+needit so below the parameters for this display. If you have a strain or a
+correct display (can happen with chinese) you can copy those parameters and
+create setup for different displays.
+*/
+
+#ifdef RED
+#define __OFFSET		32 /*see note 2 - this is the red version */
+#else
+#define __OFFSET		0  /*see note 2 - this is the black version */
+#endif
+
+static int init_display(struct fbtft_par *par)
+{
+	fbtft_par_dbg(DEBUG_INIT_DISPLAY, par, "%s()\n", __func__);
+
+	par->fbtftops.reset(par);
+
+	if (par->gpio.cs != -1)
+		gpio_set_value(par->gpio.cs, 0);  /* Activate chip */
+
+	write_reg(par, CMD_SWRESET); /* software reset */
+	mdelay(500);
+	write_reg(par, CMD_SLPOUT); /* exit sleep */
+	mdelay(5);
+	write_reg(par, CMD_PIXFMT, 0x05); /* Set Color Format 16bit */
+	write_reg(par, CMD_GAMMASET, 0x02); /* default gamma curve 3 */
+#ifdef GAMMA_ADJ
+	write_reg(par, CMD_GAMRSEL, 0x01); /* Enable Gamma adj */
+#endif
+	write_reg(par, CMD_NORML);
+	write_reg(par, CMD_DFUNCTR, 0xff, 0x06);
+	/* Frame Rate Control (In normal mode/Full colors) */
+	write_reg(par, CMD_FRMCTR1, 0x08, 0x02);
+	write_reg(par, CMD_DINVCTR, 0x07); /* display inversion  */
+	/* Set VRH1[4:0] & VC[2:0] for VCI1 & GVDD */
+	write_reg(par, CMD_PWCTR1, 0x0A, 0x02);
+	/* Set BT[2:0] for AVDD & VCL & VGH & VGL  */
+	write_reg(par, CMD_PWCTR2, 0x02);
+	/* Set VMH[6:0] & VML[6:0] for VOMH & VCOML */
+	write_reg(par, CMD_VCOMCTR1, 0x50, 0x63);
+	write_reg(par, CMD_VCOMOFFS, 0);
+
+	write_reg(par, CMD_CLMADRS, 0, 0, 0, WIDTH); /* Set Column Address */
+	write_reg(par, CMD_PGEADRS, 0, 0, 0, HEIGHT); /* Set Page Address */
+
+	write_reg(par, CMD_DISPON); /* display ON */
+	write_reg(par, CMD_RAMWR); /* Memory Write */
+
+	return 0;
+}
+
+static void set_addr_win(struct fbtft_par *par, int xs, int ys,
+				int xe, int ye)
+{
+	fbtft_par_dbg(DEBUG_SET_ADDR_WIN, par,
+		"%s(xs=%d, ys=%d, xe=%d, ye=%d)\n", __func__, xs, ys, xe, ye);
+
+	switch (par->info->var.rotate) {
+	case 0:
+		write_reg(par, CMD_CLMADRS, xs >> 8, xs & 0xff, xe >> 8,
+				xe & 0xff);
+		write_reg(par, CMD_PGEADRS,
+				(ys + __OFFSET) >> 8, (ys + __OFFSET) & 0xff,
+				(ye + __OFFSET) >> 8, (ye + __OFFSET) & 0xff);
+		break;
+	case 90:
+		write_reg(par, CMD_CLMADRS,
+				(xs + __OFFSET) >> 8, (xs + __OFFSET) & 0xff,
+				(xe + __OFFSET) >> 8, (xe + __OFFSET) & 0xff);
+		write_reg(par, CMD_PGEADRS, ys >> 8, ys & 0xff, ye >> 8,
+				ye & 0xff);
+		break;
+	case 180:
+	case 270:
+		write_reg(par, CMD_CLMADRS, xs >> 8, xs & 0xff, xe >> 8,
+				xe & 0xff);
+		write_reg(par, CMD_PGEADRS, ys >> 8, ys & 0xff, ye >> 8,
+				ye & 0xff);
+		break;
+	default:
+		par->info->var.rotate = 0; /* Fix incorrect setting */
+	}
+	write_reg(par, CMD_RAMWR); /* Write Data to GRAM mode */
+}
+
+/*
+7) MY:  1(bottom to top),	0(top to bottom)    Row Address Order
+6) MX:  1(R to L),		0(L to R)	    Column Address Order
+5) MV:  1(Exchanged),		0(normal)	    Row/Column exchange
+4) ML:  1(bottom to top),	0(top to bottom)    Vertical Refresh Order
+3) RGB: 1(BGR),			0(RGB)		    Color Space
+2) MH:  1(R to L),		0(L to R)	    Horizontal Refresh Order
+1)
+0)
+
+	MY, MX, MV, ML,RGB, MH, D1, D0
+	0 | 0 | 0 | 0 | 1 | 0 | 0 | 0	//normal
+	1 | 0 | 0 | 0 | 1 | 0 | 0 | 0	//Y-Mirror
+	0 | 1 | 0 | 0 | 1 | 0 | 0 | 0	//X-Mirror
+	1 | 1 | 0 | 0 | 1 | 0 | 0 | 0	//X-Y-Mirror
+	0 | 0 | 1 | 0 | 1 | 0 | 0 | 0	//X-Y Exchange
+	1 | 0 | 1 | 0 | 1 | 0 | 0 | 0	//X-Y Exchange, Y-Mirror
+	0 | 1 | 1 | 0 | 1 | 0 | 0 | 0	//XY exchange
+	1 | 1 | 1 | 0 | 1 | 0 | 0 | 0
+*/
+static int set_var(struct fbtft_par *par)
+{
+	u8 mactrl_data = 0; /* Avoid compiler warning */
+
+	fbtft_par_dbg(DEBUG_INIT_DISPLAY, par, "%s()\n", __func__);
+
+	switch (par->info->var.rotate) {
+	case 0:
+		mactrl_data = 0x08;
+		break;
+	case 180:
+		mactrl_data = 0xC8;
+		break;
+	case 270:
+		mactrl_data = 0xA8;
+		break;
+	case 90:
+		mactrl_data = 0x68;
+		break;
+	}
+
+	/* Colorspcae */
+	if (par->bgr)
+		mactrl_data |= (1 << 2);
+	write_reg(par, CMD_MADCTL, mactrl_data);
+	write_reg(par, CMD_RAMWR); /* Write Data to GRAM mode */
+	return 0;
+}
+
+#ifdef GAMMA_ADJ
+#define CURVE(num, idx)  curves[num*par->gamma.num_values + idx]
+static int gamma_adj(struct fbtft_par *par, unsigned long *curves)
+{
+	unsigned long mask[] = {
+		0x3F, 0x3F, 0x3F, 0x3F, 0x3F,
+		0x1f, 0x3f, 0x0f, 0x0f, 0x7f, 0x1f,
+		0x3F, 0x3F, 0x3F, 0x3F, 0x3F};
+	int i, j;
+
+	fbtft_par_dbg(DEBUG_INIT_DISPLAY, par, "%s()\n", __func__);
+
+	for (i = 0; i < GAMMA_NUM; i++)
+		for (j = 0; j < GAMMA_LEN; j++)
+			CURVE(i, j) &= mask[i*par->gamma.num_values + j];
+
+	write_reg(par, CMD_PGAMMAC,
+				CURVE(0, 0),
+				CURVE(0, 1),
+				CURVE(0, 2),
+				CURVE(0, 3),
+				CURVE(0, 4),
+				CURVE(0, 5),
+				CURVE(0, 6),
+				(CURVE(0, 7) << 4) | CURVE(0, 8),
+				CURVE(0, 9),
+				CURVE(0, 10),
+				CURVE(0, 11),
+				CURVE(0, 12),
+				CURVE(0, 13),
+				CURVE(0, 14),
+				CURVE(0, 15)
+				);
+
+	write_reg(par, CMD_RAMWR); /* Write Data to GRAM mode */
+
+	return 0;
+}
+#undef CURVE
+#endif
+
+static struct fbtft_display display = {
+	.regwidth = 8,
+	.width = WIDTH,
+	.height = HEIGHT,
+	.bpp = BPP,
+	.fps = FPS,
+#ifdef GAMMA_ADJ
+	.gamma_num = GAMMA_NUM,
+	.gamma_len = GAMMA_LEN,
+	.gamma = DEFAULT_GAMMA,
+#endif
+	.fbtftops = {
+		.init_display = init_display,
+		.set_addr_win = set_addr_win,
+		.set_var = set_var,
+#ifdef GAMMA_ADJ
+		.set_gamma = gamma_adj,
+#endif
+	},
+};
+
+FBTFT_REGISTER_DRIVER(DRVNAME, "ilitek,ili9163", &display);
+
+MODULE_ALIAS("spi:" DRVNAME);
+MODULE_ALIAS("platform:" DRVNAME);
+MODULE_ALIAS("spi:ili9163");
+MODULE_ALIAS("platform:ili9163");
+
+MODULE_DESCRIPTION("FB driver for the ILI9163 LCD Controller");
+MODULE_AUTHOR("Kozhevnikov Anatoly");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/fbtft/fb_ili9320.c b/drivers/staging/fbtft/fb_ili9320.c
index b26d89368da7..3a02edd447d4 100644
--- a/drivers/staging/fbtft/fb_ili9320.c
+++ b/drivers/staging/fbtft/fb_ili9320.c
@@ -47,6 +47,7 @@ static unsigned read_devicecode(struct fbtft_par *par)
 static int init_display(struct fbtft_par *par)
 {
 	unsigned devcode;
+
 	fbtft_par_dbg(DEBUG_INIT_DISPLAY, par, "%s()\n", __func__);
 
 	par->fbtftops.reset(par);
@@ -178,10 +179,9 @@ static int set_var(struct fbtft_par *par)
 static int set_gamma(struct fbtft_par *par, unsigned long *curves)
 {
 	unsigned long mask[] = {
-		0b11111, 0b11111, 0b111, 0b111, 0b111,
-		0b111, 0b111, 0b111, 0b111, 0b111,
-		0b11111, 0b11111, 0b111, 0b111, 0b111,
-		0b111, 0b111, 0b111, 0b111, 0b111 };
+		0x1f, 0x1f, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07,
+		0x1f, 0x1f, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07,
+	};
 	int i, j;
 
 	fbtft_par_dbg(DEBUG_INIT_DISPLAY, par, "%s()\n", __func__);
diff --git a/drivers/staging/fbtft/fb_ili9325.c b/drivers/staging/fbtft/fb_ili9325.c
index 5f88145fac9b..19d254e9a420 100644
--- a/drivers/staging/fbtft/fb_ili9325.c
+++ b/drivers/staging/fbtft/fb_ili9325.c
@@ -41,22 +41,22 @@ static unsigned bt = 6; /* VGL=Vci*4 , VGH=Vci*4 */
 module_param(bt, uint, 0);
 MODULE_PARM_DESC(bt, "Sets the factor used in the step-up circuits");
 
-static unsigned vc = 0b011; /* Vci1=Vci*0.80 */
+static unsigned vc = 0x03; /* Vci1=Vci*0.80 */
 module_param(vc, uint, 0);
 MODULE_PARM_DESC(vc,
 "Sets the ratio factor of Vci to generate the reference voltages Vci1");
 
-static unsigned vrh = 0b1101; /* VREG1OUT=Vci*1.85 */
+static unsigned vrh = 0x0d; /* VREG1OUT=Vci*1.85 */
 module_param(vrh, uint, 0);
 MODULE_PARM_DESC(vrh,
 "Set the amplifying rate (1.6 ~ 1.9) of Vci applied to output the VREG1OUT");
 
-static unsigned vdv = 0b10010; /* VCOMH amplitude=VREG1OUT*0.98 */
+static unsigned vdv = 0x12; /* VCOMH amplitude=VREG1OUT*0.98 */
 module_param(vdv, uint, 0);
 MODULE_PARM_DESC(vdv,
 "Select the factor of VREG1OUT to set the amplitude of Vcom");
 
-static unsigned vcm = 0b001010; /* VCOMH=VREG1OUT*0.735 */
+static unsigned vcm = 0x0a; /* VCOMH=VREG1OUT*0.735 */
 module_param(vcm, uint, 0);
 MODULE_PARM_DESC(vcm, "Set the internal VcomH voltage");
 
@@ -108,11 +108,11 @@ static int init_display(struct fbtft_par *par)
 	if (par->gpio.cs != -1)
 		gpio_set_value(par->gpio.cs, 0);  /* Activate chip */
 
-	bt &= 0b111;
-	vc &= 0b111;
-	vrh &= 0b1111;
-	vdv &= 0b11111;
-	vcm &= 0b111111;
+	bt &= 0x07;
+	vc &= 0x07;
+	vrh &= 0x0f;
+	vdv &= 0x1f;
+	vcm &= 0x3f;
 
 	/* Initialization sequence from ILI9325 Application Notes */
 
@@ -137,7 +137,7 @@ static int init_display(struct fbtft_par *par)
 	write_reg(par, 0x0013, 0x0000); /* VDV[4:0] for VCOM amplitude */
 	mdelay(200); /* Dis-charge capacitor power voltage */
 	write_reg(par, 0x0010, /* SAP, BT[3:0], AP, DSTB, SLP, STB */
-		(1 << 12) | (bt << 8) | (1 << 7) | (0b001 << 4));
+		(1 << 12) | (bt << 8) | (1 << 7) | (0x01 << 4));
 	write_reg(par, 0x0011, 0x220 | vc); /* DC1[2:0], DC0[2:0], VC[2:0] */
 	mdelay(50); /* Delay 50ms */
 	write_reg(par, 0x0012, vrh); /* Internal reference voltage= Vci; */
@@ -233,10 +233,9 @@ static int set_var(struct fbtft_par *par)
 static int set_gamma(struct fbtft_par *par, unsigned long *curves)
 {
 	unsigned long mask[] = {
-		0b11111, 0b11111, 0b111, 0b111, 0b111,
-		0b111, 0b111, 0b111, 0b111, 0b111,
-		0b11111, 0b11111, 0b111, 0b111, 0b111,
-		0b111, 0b111, 0b111, 0b111, 0b111 };
+		0x1f, 0x1f, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07,
+		0x1f, 0x1f, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07,
+	};
 	int i, j;
 
 	fbtft_par_dbg(DEBUG_INIT_DISPLAY, par, "%s()\n", __func__);
diff --git a/drivers/staging/fbtft/fb_ili9340.c b/drivers/staging/fbtft/fb_ili9340.c
index 985687d94ec2..0f4a42f89e5e 100644
--- a/drivers/staging/fbtft/fb_ili9340.c
+++ b/drivers/staging/fbtft/fb_ili9340.c
@@ -39,12 +39,12 @@ static int init_display(struct fbtft_par *par)
 	par->fbtftops.reset(par);
 
 	write_reg(par, 0xEF, 0x03, 0x80, 0x02);
-	write_reg(par, 0xCF, 0x00 , 0XC1 , 0X30);
-	write_reg(par, 0xED, 0x64 , 0x03 , 0X12 , 0X81);
-	write_reg(par, 0xE8, 0x85 , 0x00 , 0x78);
-	write_reg(par, 0xCB, 0x39 , 0x2C , 0x00 , 0x34 , 0x02);
+	write_reg(par, 0xCF, 0x00, 0XC1, 0X30);
+	write_reg(par, 0xED, 0x64, 0x03, 0X12, 0X81);
+	write_reg(par, 0xE8, 0x85, 0x00, 0x78);
+	write_reg(par, 0xCB, 0x39, 0x2C, 0x00, 0x34, 0x02);
 	write_reg(par, 0xF7, 0x20);
-	write_reg(par, 0xEA, 0x00 , 0x00);
+	write_reg(par, 0xEA, 0x00, 0x00);
 
 	/* Power Control 1 */
 	write_reg(par, 0xC0, 0x23);
@@ -104,7 +104,7 @@ static void set_addr_win(struct fbtft_par *par, int xs, int ys, int xe, int ye)
 	/* Column address */
 	write_reg(par, 0x2A, xs >> 8, xs & 0xFF, xe >> 8, xe & 0xFF);
 
-	/* Row adress */
+	/* Row address */
 	write_reg(par, 0x2B, ys >> 8, ys & 0xFF, ye >> 8, ye & 0xFF);
 
 	/* Memory write */
diff --git a/drivers/staging/fbtft/fb_ili9341.c b/drivers/staging/fbtft/fb_ili9341.c
index 225b2d84371f..709492e560b6 100644
--- a/drivers/staging/fbtft/fb_ili9341.c
+++ b/drivers/staging/fbtft/fb_ili9341.c
@@ -4,7 +4,7 @@
  * This display uses 9-bit SPI: Data/Command bit + 8 data bits
  * For platforms that doesn't support 9-bit, the driver is capable
  * of emulating this using 8-bit transfer.
- * This is done by transfering eight 9-bit words in 9 bytes.
+ * This is done by transferring eight 9-bit words in 9 bytes.
  *
  * Copyright (C) 2013 Christian Vogelgsang
  * Based on adafruit22fb.c by Noralf Tronnes
@@ -89,7 +89,7 @@ static void set_addr_win(struct fbtft_par *par, int xs, int ys, int xe, int ye)
 	write_reg(par, 0x2A,
 		(xs >> 8) & 0xFF, xs & 0xFF, (xe >> 8) & 0xFF, xe & 0xFF);
 
-	/* Row adress set */
+	/* Row address set */
 	write_reg(par, 0x2B,
 		(ys >> 8) & 0xFF, ys & 0xFF, (ye >> 8) & 0xFF, ye & 0xFF);
 
diff --git a/drivers/staging/fbtft/fb_ili9481.c b/drivers/staging/fbtft/fb_ili9481.c
index 725157a1ac41..8bae09c2d5ce 100644
--- a/drivers/staging/fbtft/fb_ili9481.c
+++ b/drivers/staging/fbtft/fb_ili9481.c
@@ -63,7 +63,7 @@ static void set_addr_win(struct fbtft_par *par, int xs, int ys, int xe, int ye)
 	/* column address */
 	write_reg(par, 0x2a, xs >> 8, xs & 0xff, xe >> 8, xe & 0xff);
 
-	/* row adress */
+	/* Row address */
 	write_reg(par, 0x2b, ys >> 8, ys & 0xff, ye >> 8, ye & 0xff);
 
 	/* memory write */
diff --git a/drivers/staging/fbtft/fb_ili9486.c b/drivers/staging/fbtft/fb_ili9486.c
index 95b89999d32a..dd4ddca384ad 100644
--- a/drivers/staging/fbtft/fb_ili9486.c
+++ b/drivers/staging/fbtft/fb_ili9486.c
@@ -44,13 +44,13 @@ static int default_init_sequence[] = {
 	-1, 0xC5, 0x00, 0x00, 0x00, 0x00,
 	/* PGAMCTRL(Positive Gamma Control) */
 	-1, 0xE0, 0x0F, 0x1F, 0x1C, 0x0C, 0x0F, 0x08, 0x48, 0x98,
-	          0x37, 0x0A, 0x13, 0x04, 0x11, 0x0D, 0x00,
+		  0x37, 0x0A, 0x13, 0x04, 0x11, 0x0D, 0x00,
 	/* NGAMCTRL(Negative Gamma Control) */
 	-1, 0xE1, 0x0F, 0x32, 0x2E, 0x0B, 0x0D, 0x05, 0x47, 0x75,
-	          0x37, 0x06, 0x10, 0x03, 0x24, 0x20, 0x00,
+		  0x37, 0x06, 0x10, 0x03, 0x24, 0x20, 0x00,
 	/* Digital Gamma Control 1 */
 	-1, 0xE2, 0x0F, 0x32, 0x2E, 0x0B, 0x0D, 0x05, 0x47, 0x75,
-	          0x37, 0x06, 0x10, 0x03, 0x24, 0x20, 0x00,
+		  0x37, 0x06, 0x10, 0x03, 0x24, 0x20, 0x00,
 	/* Sleep OUT */
 	-1, 0x11,
 	/* Display ON */
@@ -67,7 +67,7 @@ static void set_addr_win(struct fbtft_par *par, int xs, int ys, int xe, int ye)
 	/* Column address */
 	write_reg(par, 0x2A, xs >> 8, xs & 0xFF, xe >> 8, xe & 0xFF);
 
-	/* Row adress */
+	/* Row address */
 	write_reg(par, 0x2B, ys >> 8, ys & 0xFF, ye >> 8, ye & 0xFF);
 
 	/* Memory write */
diff --git a/drivers/staging/fbtft/fb_pcd8544.c b/drivers/staging/fbtft/fb_pcd8544.c
index 8b9ebfb49ef8..15da0ec77513 100644
--- a/drivers/staging/fbtft/fb_pcd8544.c
+++ b/drivers/staging/fbtft/fb_pcd8544.c
@@ -34,7 +34,7 @@
 #define WIDTH          84
 #define HEIGHT         48
 #define TXBUFLEN       (84*6)
-#define DEFAULT_GAMMA  "40" /* gamma is used to control contrast in this driver */
+#define DEFAULT_GAMMA  "40" /* gamma controls the contrast in this driver */
 
 static unsigned tc;
 module_param(tc, uint, 0);
@@ -51,61 +51,73 @@ static int init_display(struct fbtft_par *par)
 
 	par->fbtftops.reset(par);
 
-	/* Function set */
-	write_reg(par, 0x21); /* 5:1  1
-	                         2:0  PD - Powerdown control: chip is active
-							 1:0  V  - Entry mode: horizontal addressing
-							 0:1  H  - Extended instruction set control: extended
-						  */
-
-	/* H=1 Temperature control */
-	write_reg(par, 0x04 | (tc & 0x3)); /*
-	                         2:1  1
-	                         1:x  TC1 - Temperature Coefficient: 0x10
-							 0:x  TC0
-						  */
-
-	/* H=1 Bias system */
-	write_reg(par, 0x10 | (bs & 0x7)); /*
-	                         4:1  1
-	                         3:0  0
-							 2:x  BS2 - Bias System
-							 1:x  BS1
-							 0:x  BS0
-	                      */
-
-	/* Function set */
-	write_reg(par, 0x22); /* 5:1  1
-	                         2:0  PD - Powerdown control: chip is active
-							 1:1  V  - Entry mode: vertical addressing
-							 0:0  H  - Extended instruction set control: basic
-						  */
-
-	/* H=0 Display control */
-	write_reg(par, 0x08 | 4); /*
-	                         3:1  1
-	                         2:1  D  - DE: 10=normal mode
-							 1:0  0
-							 0:0  E
-						  */
+	/* Function set
+	 *
+	 * 5:1  1
+	 * 2:0  PD - Powerdown control: chip is active
+	 * 1:0  V  - Entry mode: horizontal addressing
+	 * 0:1  H  - Extended instruction set control: extended
+	 */
+	write_reg(par, 0x21);
+
+	/* H=1 Temperature control
+	 *
+	 * 2:1  1
+	 * 1:x  TC1 - Temperature Coefficient: 0x10
+	 * 0:x  TC0
+	 */
+	write_reg(par, 0x04 | (tc & 0x3));
+
+	/* H=1 Bias system
+	 *
+	 * 4:1  1
+	 * 3:0  0
+	 * 2:x  BS2 - Bias System
+	 * 1:x  BS1
+	 * 0:x  BS0
+	 */
+	write_reg(par, 0x10 | (bs & 0x7));
+
+	/* Function set
+	 *
+	 * 5:1  1
+	 * 2:0  PD - Powerdown control: chip is active
+	 * 1:1  V  - Entry mode: vertical addressing
+	 * 0:0  H  - Extended instruction set control: basic
+	 */
+	write_reg(par, 0x22);
+
+	/* H=0 Display control
+	 *
+	 * 3:1  1
+	 * 2:1  D  - DE: 10=normal mode
+	 * 1:0  0
+	 * 0:0  E
+	 */
+	write_reg(par, 0x08 | 4);
 
 	return 0;
 }
 
 static void set_addr_win(struct fbtft_par *par, int xs, int ys, int xe, int ye)
 {
-	fbtft_par_dbg(DEBUG_SET_ADDR_WIN, par, "%s(xs=%d, ys=%d, xe=%d, ye=%d)\n", __func__, xs, ys, xe, ye);
-
-	/* H=0 Set X address of RAM */
-	write_reg(par, 0x80); /* 7:1  1
-	                         6-0: X[6:0] - 0x00
-	                      */
-
-	/* H=0 Set Y address of RAM */
-	write_reg(par, 0x40); /* 7:0  0
-	                         6:1  1
-	                         2-0: Y[2:0] - 0x0
-	                      */
+	fbtft_par_dbg(DEBUG_SET_ADDR_WIN, par, "%s(xs=%d, ys=%d, xe=%d, ye=%d)\n",
+		      __func__, xs, ys, xe, ye);
+
+	/* H=0 Set X address of RAM
+	 *
+	 * 7:1  1
+	 * 6-0: X[6:0] - 0x00
+	 */
+	write_reg(par, 0x80);
+
+	/* H=0 Set Y address of RAM
+	 *
+	 * 7:0  0
+	 * 6:1  1
+	 * 2-0: Y[2:0] - 0x0
+	 */
+	write_reg(par, 0x40);
 }
 
 static int write_vmem(struct fbtft_par *par, size_t offset, size_t len)
@@ -120,9 +132,8 @@ static int write_vmem(struct fbtft_par *par, size_t offset, size_t len)
 	for (x = 0; x < 84; x++) {
 		for (y = 0; y < 6; y++) {
 			*buf = 0x00;
-			for (i = 0; i < 8; i++) {
+			for (i = 0; i < 8; i++)
 				*buf |= (vmem16[(y*8+i)*84+x] ? 1 : 0) << i;
-			}
 			buf++;
 		}
 	}
@@ -131,7 +142,8 @@ static int write_vmem(struct fbtft_par *par, size_t offset, size_t len)
 	gpio_set_value(par->gpio.dc, 1);
 	ret = par->fbtftops.write(par, par->txbuf.buf, 6*84);
 	if (ret < 0)
-		dev_err(par->info->device, "%s: write failed and returned: %d\n", __func__, ret);
+		dev_err(par->info->device, "write failed and returned: %d\n",
+			ret);
 
 	return ret;
 }
diff --git a/drivers/staging/fbtft/fb_ra8875.c b/drivers/staging/fbtft/fb_ra8875.c
index c323c06344fd..2c4d4dc70c51 100644
--- a/drivers/staging/fbtft/fb_ra8875.c
+++ b/drivers/staging/fbtft/fb_ra8875.c
@@ -11,7 +11,7 @@
 						  *****      *   *
   Date    : 10.06.2014                                      *   *
   Version : V1.13                                          *****
-  Revison : 5
+  Revision : 5
 
 *******************************************************************************
  * This program is free software; you can redistribute it and/or modify
@@ -79,112 +79,112 @@ static int init_display(struct fbtft_par *par)
 
 	if ((par->info->var.xres == 320) && (par->info->var.yres == 240)) {
 		/* PLL clock frequency */
-		write_reg(par, 0x88 , 0x0A);
-		write_reg(par, 0x89 , 0x02);
+		write_reg(par, 0x88, 0x0A);
+		write_reg(par, 0x89, 0x02);
 		mdelay(10);
 		/* color deep / MCU Interface */
-		write_reg(par, 0x10 , 0x0C);
+		write_reg(par, 0x10, 0x0C);
 		/* pixel clock period  */
-		write_reg(par, 0x04 , 0x03);
+		write_reg(par, 0x04, 0x03);
 		mdelay(1);
 		/* horizontal settings */
-		write_reg(par, 0x14 , 0x27);
-		write_reg(par, 0x15 , 0x00);
-		write_reg(par, 0x16 , 0x05);
-		write_reg(par, 0x17 , 0x04);
-		write_reg(par, 0x18 , 0x03);
+		write_reg(par, 0x14, 0x27);
+		write_reg(par, 0x15, 0x00);
+		write_reg(par, 0x16, 0x05);
+		write_reg(par, 0x17, 0x04);
+		write_reg(par, 0x18, 0x03);
 		/* vertical settings */
-		write_reg(par, 0x19 , 0xEF);
-		write_reg(par, 0x1A , 0x00);
-		write_reg(par, 0x1B , 0x05);
-		write_reg(par, 0x1C , 0x00);
-		write_reg(par, 0x1D , 0x0E);
-		write_reg(par, 0x1E , 0x00);
-		write_reg(par, 0x1F , 0x02);
+		write_reg(par, 0x19, 0xEF);
+		write_reg(par, 0x1A, 0x00);
+		write_reg(par, 0x1B, 0x05);
+		write_reg(par, 0x1C, 0x00);
+		write_reg(par, 0x1D, 0x0E);
+		write_reg(par, 0x1E, 0x00);
+		write_reg(par, 0x1F, 0x02);
 	} else if ((par->info->var.xres == 480) && (par->info->var.yres == 272)) {
 		/* PLL clock frequency  */
-		write_reg(par, 0x88 , 0x0A);
-		write_reg(par, 0x89 , 0x02);
+		write_reg(par, 0x88, 0x0A);
+		write_reg(par, 0x89, 0x02);
 		mdelay(10);
 		/* color deep / MCU Interface */
-		write_reg(par, 0x10 , 0x0C);
+		write_reg(par, 0x10, 0x0C);
 		/* pixel clock period  */
-		write_reg(par, 0x04 , 0x82);
+		write_reg(par, 0x04, 0x82);
 		mdelay(1);
 		/* horizontal settings */
-		write_reg(par, 0x14 , 0x3B);
-		write_reg(par, 0x15 , 0x00);
-		write_reg(par, 0x16 , 0x01);
-		write_reg(par, 0x17 , 0x00);
-		write_reg(par, 0x18 , 0x05);
+		write_reg(par, 0x14, 0x3B);
+		write_reg(par, 0x15, 0x00);
+		write_reg(par, 0x16, 0x01);
+		write_reg(par, 0x17, 0x00);
+		write_reg(par, 0x18, 0x05);
 		/* vertical settings */
-		write_reg(par, 0x19 , 0x0F);
-		write_reg(par, 0x1A , 0x01);
-		write_reg(par, 0x1B , 0x02);
-		write_reg(par, 0x1C , 0x00);
-		write_reg(par, 0x1D , 0x07);
-		write_reg(par, 0x1E , 0x00);
-		write_reg(par, 0x1F , 0x09);
+		write_reg(par, 0x19, 0x0F);
+		write_reg(par, 0x1A, 0x01);
+		write_reg(par, 0x1B, 0x02);
+		write_reg(par, 0x1C, 0x00);
+		write_reg(par, 0x1D, 0x07);
+		write_reg(par, 0x1E, 0x00);
+		write_reg(par, 0x1F, 0x09);
 	} else if ((par->info->var.xres == 640) && (par->info->var.yres == 480)) {
 		/* PLL clock frequency */
-		write_reg(par, 0x88 , 0x0B);
-		write_reg(par, 0x89 , 0x02);
+		write_reg(par, 0x88, 0x0B);
+		write_reg(par, 0x89, 0x02);
 		mdelay(10);
 		/* color deep / MCU Interface */
-		write_reg(par, 0x10 , 0x0C);
+		write_reg(par, 0x10, 0x0C);
 		/* pixel clock period */
-		write_reg(par, 0x04 , 0x01);
+		write_reg(par, 0x04, 0x01);
 		mdelay(1);
 		/* horizontal settings */
-		write_reg(par, 0x14 , 0x4F);
-		write_reg(par, 0x15 , 0x05);
-		write_reg(par, 0x16 , 0x0F);
-		write_reg(par, 0x17 , 0x01);
-		write_reg(par, 0x18 , 0x00);
+		write_reg(par, 0x14, 0x4F);
+		write_reg(par, 0x15, 0x05);
+		write_reg(par, 0x16, 0x0F);
+		write_reg(par, 0x17, 0x01);
+		write_reg(par, 0x18, 0x00);
 		/* vertical settings */
-		write_reg(par, 0x19 , 0xDF);
-		write_reg(par, 0x1A , 0x01);
-		write_reg(par, 0x1B , 0x0A);
-		write_reg(par, 0x1C , 0x00);
-		write_reg(par, 0x1D , 0x0E);
-		write_reg(par, 0x1E , 0x00);
-		write_reg(par, 0x1F , 0x01);
+		write_reg(par, 0x19, 0xDF);
+		write_reg(par, 0x1A, 0x01);
+		write_reg(par, 0x1B, 0x0A);
+		write_reg(par, 0x1C, 0x00);
+		write_reg(par, 0x1D, 0x0E);
+		write_reg(par, 0x1E, 0x00);
+		write_reg(par, 0x1F, 0x01);
 	} else if ((par->info->var.xres == 800) && (par->info->var.yres == 480)) {
 		/* PLL clock frequency */
-		write_reg(par, 0x88 , 0x0B);
-		write_reg(par, 0x89 , 0x02);
+		write_reg(par, 0x88, 0x0B);
+		write_reg(par, 0x89, 0x02);
 		mdelay(10);
 		/* color deep / MCU Interface */
-		write_reg(par, 0x10 , 0x0C);
+		write_reg(par, 0x10, 0x0C);
 		/* pixel clock period */
-		write_reg(par, 0x04 , 0x81);
+		write_reg(par, 0x04, 0x81);
 		mdelay(1);
 		/* horizontal settings */
-		write_reg(par, 0x14 , 0x63);
-		write_reg(par, 0x15 , 0x03);
-		write_reg(par, 0x16 , 0x03);
-		write_reg(par, 0x17 , 0x02);
-		write_reg(par, 0x18 , 0x00);
+		write_reg(par, 0x14, 0x63);
+		write_reg(par, 0x15, 0x03);
+		write_reg(par, 0x16, 0x03);
+		write_reg(par, 0x17, 0x02);
+		write_reg(par, 0x18, 0x00);
 		/* vertical settings */
-		write_reg(par, 0x19 , 0xDF);
-		write_reg(par, 0x1A , 0x01);
-		write_reg(par, 0x1B , 0x14);
-		write_reg(par, 0x1C , 0x00);
-		write_reg(par, 0x1D , 0x06);
-		write_reg(par, 0x1E , 0x00);
-		write_reg(par, 0x1F , 0x01);
+		write_reg(par, 0x19, 0xDF);
+		write_reg(par, 0x1A, 0x01);
+		write_reg(par, 0x1B, 0x14);
+		write_reg(par, 0x1C, 0x00);
+		write_reg(par, 0x1D, 0x06);
+		write_reg(par, 0x1E, 0x00);
+		write_reg(par, 0x1F, 0x01);
 	} else {
 		dev_err(par->info->device, "display size is not supported!!");
 		return -1;
 	}
 
 	/* PWM clock */
-	write_reg(par, 0x8a , 0x81);
-	write_reg(par, 0x8b , 0xFF);
+	write_reg(par, 0x8a, 0x81);
+	write_reg(par, 0x8b, 0xFF);
 	mdelay(10);
 
 	/* Display ON */
-	write_reg(par, 0x01 , 0x80);
+	write_reg(par, 0x01, 0x80);
 	mdelay(10);
 
 	return 0;
@@ -196,14 +196,14 @@ static void set_addr_win(struct fbtft_par *par, int xs, int ys, int xe, int ye)
 		"%s(xs=%d, ys=%d, xe=%d, ye=%d)\n", __func__, xs, ys, xe, ye);
 
 	/* Set_Active_Window */
-	write_reg(par, 0x30 , xs & 0x00FF);
-	write_reg(par, 0x31 , (xs & 0xFF00) >> 8);
-	write_reg(par, 0x32 , ys & 0x00FF);
-	write_reg(par, 0x33 , (ys & 0xFF00) >> 8);
-	write_reg(par, 0x34 , (xs+xe) & 0x00FF);
-	write_reg(par, 0x35 , ((xs+xe) & 0xFF00) >> 8);
-	write_reg(par, 0x36 , (ys+ye) & 0x00FF);
-	write_reg(par, 0x37 , ((ys+ye) & 0xFF00) >> 8);
+	write_reg(par, 0x30, xs & 0x00FF);
+	write_reg(par, 0x31, (xs & 0xFF00) >> 8);
+	write_reg(par, 0x32, ys & 0x00FF);
+	write_reg(par, 0x33, (ys & 0xFF00) >> 8);
+	write_reg(par, 0x34, (xs+xe) & 0x00FF);
+	write_reg(par, 0x35, ((xs+xe) & 0xFF00) >> 8);
+	write_reg(par, 0x36, (ys+ye) & 0x00FF);
+	write_reg(par, 0x37, ((ys+ye) & 0xFF00) >> 8);
 
 	/* Set_Memory_Write_Cursor */
 	write_reg(par, 0x46,  xs & 0xff);
@@ -238,8 +238,8 @@ static void write_reg8_bus8(struct fbtft_par *par, int len, ...)
 	ret = par->fbtftops.write(par, par->buf, 2);
 	if (ret < 0) {
 		va_end(args);
-		dev_err(par->info->device, "%s: write() failed and returned %dn",
-			__func__, ret);
+		dev_err(par->info->device, "write() failed and returned %dn",
+			ret);
 		return;
 	}
 	len--;
@@ -256,8 +256,8 @@ static void write_reg8_bus8(struct fbtft_par *par, int len, ...)
 		ret = par->fbtftops.write(par, par->buf, len + 1);
 		if (ret < 0) {
 			va_end(args);
-			dev_err(par->info->device, "%s: write() failed and returned %dn",
-				__func__, ret);
+			dev_err(par->info->device,
+				"write() failed and returned %dn", ret);
 			return;
 		}
 	}
diff --git a/drivers/staging/fbtft/fb_s6d02a1.c b/drivers/staging/fbtft/fb_s6d02a1.c
index e412a42443e5..f3302525ec00 100644
--- a/drivers/staging/fbtft/fb_s6d02a1.c
+++ b/drivers/staging/fbtft/fb_s6d02a1.c
@@ -108,7 +108,7 @@ static void set_addr_win(struct fbtft_par *par, int xs, int ys, int xe, int ye)
 	/* Column address */
 	write_reg(par, 0x2A, xs >> 8, xs & 0xFF, xe >> 8, xe & 0xFF);
 
-	/* Row adress */
+	/* Row address */
 	write_reg(par, 0x2B, ys >> 8, ys & 0xFF, ye >> 8, ye & 0xFF);
 
 	/* Memory write */
@@ -136,7 +136,7 @@ static int set_var(struct fbtft_par *par)
 		write_reg(par, 0x36, MY | MV | (par->bgr << 3));
 		break;
 	case 180:
-		write_reg(par, 0x36, (par->bgr << 3));
+		write_reg(par, 0x36, par->bgr << 3);
 		break;
 	case 90:
 		write_reg(par, 0x36, MX | MV | (par->bgr << 3));
diff --git a/drivers/staging/fbtft/fb_s6d1121.c b/drivers/staging/fbtft/fb_s6d1121.c
index 1ef8c1ad827e..2e1b72ad54aa 100644
--- a/drivers/staging/fbtft/fb_s6d1121.c
+++ b/drivers/staging/fbtft/fb_s6d1121.c
@@ -143,12 +143,10 @@ static int set_var(struct fbtft_par *par)
 static int set_gamma(struct fbtft_par *par, unsigned long *curves)
 {
 	unsigned long mask[] = {
-		0b111111, 0b111111, 0b111111, 0b111111, 0b111111, 0b111111, 
-		0b111111, 0b111111, 0b111111, 0b111111, 0b111111, 0b111111,
-		0b11111, 0b11111,
-		0b111111, 0b111111, 0b111111, 0b111111, 0b111111, 0b111111,
-		0b111111, 0b111111, 0b111111, 0b111111, 0b111111, 0b111111,
-		0b11111, 0b11111 };
+		0x3f, 0x3f, 0x3f, 0x3f, 0x3f, 0x3f, 0x3f, 0x3f, 0x3f, 0x3f,
+		0x3f, 0x3f, 0x1f, 0x1f, 0x3f, 0x3f, 0x3f, 0x3f, 0x3f, 0x3f,
+		0x3f, 0x3f, 0x3f, 0x3f, 0x3f, 0x3f, 0x1f, 0x1f,
+	};
 	int i, j;
 
 	fbtft_par_dbg(DEBUG_INIT_DISPLAY, par, "%s()\n", __func__);
diff --git a/drivers/staging/fbtft/fb_ssd1289.c b/drivers/staging/fbtft/fb_ssd1289.c
index ef46fbca2700..17a77e061498 100644
--- a/drivers/staging/fbtft/fb_ssd1289.c
+++ b/drivers/staging/fbtft/fb_ssd1289.c
@@ -126,16 +126,16 @@ static int set_var(struct fbtft_par *par)
 
 	switch (par->info->var.rotate) {
 	case 0:
-		write_reg(par, 0x11, reg11 | 0b110000);
+		write_reg(par, 0x11, reg11 | 0x30);
 		break;
 	case 270:
-		write_reg(par, 0x11, reg11 | 0b101000);
+		write_reg(par, 0x11, reg11 | 0x28);
 		break;
 	case 180:
-		write_reg(par, 0x11, reg11 | 0b000000);
+		write_reg(par, 0x11, reg11 | 0x00);
 		break;
 	case 90:
-		write_reg(par, 0x11, reg11 | 0b011000);
+		write_reg(par, 0x11, reg11 | 0x18);
 		break;
 	}
 
@@ -151,10 +151,9 @@ static int set_var(struct fbtft_par *par)
 static int set_gamma(struct fbtft_par *par, unsigned long *curves)
 {
 	unsigned long mask[] = {
-		0b11111, 0b11111, 0b111, 0b111, 0b111,
-		0b111, 0b111, 0b111, 0b111, 0b111,
-		0b11111, 0b11111, 0b111, 0b111, 0b111,
-		0b111, 0b111, 0b111, 0b111, 0b111 };
+		0x1f, 0x1f, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07,
+		0x1f, 0x1f, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07,
+	};
 	int i, j;
 
 	fbtft_par_dbg(DEBUG_INIT_DISPLAY, par, "%s()\n", __func__);
diff --git a/drivers/staging/fbtft/fb_ssd1306.c b/drivers/staging/fbtft/fb_ssd1306.c
index 5ea195b0de1b..15ee44dd130b 100644
--- a/drivers/staging/fbtft/fb_ssd1306.c
+++ b/drivers/staging/fbtft/fb_ssd1306.c
@@ -193,8 +193,8 @@ static int write_vmem(struct fbtft_par *par, size_t offset, size_t len)
 	ret = par->fbtftops.write(par, par->txbuf.buf,
 				par->info->var.xres*par->info->var.yres/8);
 	if (ret < 0)
-		dev_err(par->info->device,
-			"%s: write failed and returned: %d\n", __func__, ret);
+		dev_err(par->info->device, "write failed and returned: %d\n",
+			ret);
 
 	return ret;
 }
diff --git a/drivers/staging/fbtft/fb_ssd1331.c b/drivers/staging/fbtft/fb_ssd1331.c
index da7464f90e37..5bb741046c85 100644
--- a/drivers/staging/fbtft/fb_ssd1331.c
+++ b/drivers/staging/fbtft/fb_ssd1331.c
@@ -29,24 +29,24 @@ static int init_display(struct fbtft_par *par)
 
 	write_reg(par, 0xae); /* Display Off */
 	write_reg(par, 0xa0, 0x70 | (par->bgr << 2)); /* Set Colour Depth */
-	write_reg(par, 0x72); // RGB colour
+	write_reg(par, 0x72); /* RGB colour */
 	write_reg(par, 0xa1, 0x00); /* Set Display Start Line */
 	write_reg(par, 0xa2, 0x00); /* Set Display Offset */
 	write_reg(par, 0xa4); /* NORMALDISPLAY */
-	write_reg(par, 0xa8, 0x3f); // Set multiplex
-	write_reg(par, 0xad, 0x8e); // Set master
-	// write_reg(par, 0xb0, 0x0b); // Set power mode
-	write_reg(par, 0xb1, 0x31); // Precharge
-	write_reg(par, 0xb3, 0xf0); // Clock div
-	write_reg(par, 0x8a, 0x64); // Precharge A
-	write_reg(par, 0x8b, 0x78); // Precharge B
-	write_reg(par, 0x8c, 0x64); // Precharge C
-	write_reg(par, 0xbb, 0x3a); // Precharge level
-	write_reg(par, 0xbe, 0x3e); // vcomh
-	write_reg(par, 0x87, 0x06); // Master current
-	write_reg(par, 0x81, 0x91); // Contrast A
-	write_reg(par, 0x82, 0x50); // Contrast B
-	write_reg(par, 0x83, 0x7d); // Contrast C
+	write_reg(par, 0xa8, 0x3f); /* Set multiplex */
+	write_reg(par, 0xad, 0x8e); /* Set master */
+	/* write_reg(par, 0xb0, 0x0b);  Set power mode */
+	write_reg(par, 0xb1, 0x31); /* Precharge */
+	write_reg(par, 0xb3, 0xf0); /* Clock div */
+	write_reg(par, 0x8a, 0x64); /* Precharge A */
+	write_reg(par, 0x8b, 0x78); /* Precharge B */
+	write_reg(par, 0x8c, 0x64); /* Precharge C */
+	write_reg(par, 0xbb, 0x3a); /* Precharge level */
+	write_reg(par, 0xbe, 0x3e); /* vcomh */
+	write_reg(par, 0x87, 0x06); /* Master current */
+	write_reg(par, 0x81, 0x91); /* Contrast A */
+	write_reg(par, 0x82, 0x50); /* Contrast B */
+	write_reg(par, 0x83, 0x7d); /* Contrast C */
 	write_reg(par, 0xaf); /* Set Sleep Mode Display On */
 
 	return 0;
@@ -69,9 +69,8 @@ static void write_reg8_bus8(struct fbtft_par *par, int len, ...)
 
 	if (unlikely(par->debug & DEBUG_WRITE_REGISTER)) {
 		va_start(args, len);
-		for (i = 0; i < len; i++) {
+		for (i = 0; i < len; i++)
 			buf[i] = (u8)va_arg(args, unsigned int);
-		}
 		va_end(args);
 		fbtft_par_dbg_hex(DEBUG_WRITE_REGISTER, par, par->info->device, u8, buf, len, "%s: ", __func__);
 	}
@@ -84,20 +83,21 @@ static void write_reg8_bus8(struct fbtft_par *par, int len, ...)
 	ret = par->fbtftops.write(par, par->buf, sizeof(u8));
 	if (ret < 0) {
 		va_end(args);
-		dev_err(par->info->device, "%s: write() failed and returned %d\n", __func__, ret);
+		dev_err(par->info->device,
+			"write() failed and returned %d\n", ret);
 		return;
 	}
 	len--;
 
 	if (len) {
 		i = len;
-		while (i--) {
+		while (i--)
 			*buf++ = (u8)va_arg(args, unsigned int);
-		}
 		ret = par->fbtftops.write(par, par->buf, len * (sizeof(u8)));
 		if (ret < 0) {
 			va_end(args);
-			dev_err(par->info->device, "%s: write() failed and returned %d\n", __func__, ret);
+			dev_err(par->info->device,
+				"write() failed and returned %d\n", ret);
 			return;
 		}
 	}
diff --git a/drivers/staging/fbtft/fb_ssd1351.c b/drivers/staging/fbtft/fb_ssd1351.c
index 062d98660f63..9bcd7a0aeed4 100644
--- a/drivers/staging/fbtft/fb_ssd1351.c
+++ b/drivers/staging/fbtft/fb_ssd1351.c
@@ -72,6 +72,7 @@ static void set_addr_win(struct fbtft_par *par, int xs, int ys, int xe, int ye)
 static int set_var(struct fbtft_par *par)
 {
 	unsigned remap;
+
 	fbtft_par_dbg(DEBUG_INIT_DISPLAY, par, "%s()\n", __func__);
 
 	if (par->fbtftops.init_display != init_display) {
@@ -86,16 +87,16 @@ static int set_var(struct fbtft_par *par)
 
 	switch (par->info->var.rotate) {
 	case 0:
-		write_reg(par, 0xA0, remap | 0b00 | 1<<4);
+		write_reg(par, 0xA0, remap | 0x00 | 1<<4);
 		break;
 	case 270:
-		write_reg(par, 0xA0, remap | 0b11 | 1<<4);
+		write_reg(par, 0xA0, remap | 0x03 | 1<<4);
 		break;
 	case 180:
-		write_reg(par, 0xA0, remap | 0b10);
+		write_reg(par, 0xA0, remap | 0x02);
 		break;
 	case 90:
-		write_reg(par, 0xA0, remap | 0b01);
+		write_reg(par, 0xA0, remap | 0x01);
 		break;
 	}
 
@@ -217,12 +218,8 @@ static void register_onboard_backlight(struct fbtft_par *par)
 
 	bl_ops = devm_kzalloc(par->info->device, sizeof(struct backlight_ops),
 				GFP_KERNEL);
-	if (!bl_ops) {
-		dev_err(par->info->device,
-			"%s: could not allocate memory for backlight operations.\n",
-			__func__);
+	if (!bl_ops)
 		return;
-	}
 
 	bl_ops->update_status = update_onboard_backlight;
 	bl_props.type = BACKLIGHT_RAW;
diff --git a/drivers/staging/fbtft/fb_st7735r.c b/drivers/staging/fbtft/fb_st7735r.c
index b63aa38e51cf..9d874308447e 100644
--- a/drivers/staging/fbtft/fb_st7735r.c
+++ b/drivers/staging/fbtft/fb_st7735r.c
@@ -31,20 +31,20 @@
 
 static int default_init_sequence[] = {
 	/* SWRESET - Software reset */
-	-1, 0x01,                                
+	-1, 0x01,
 	-2, 150,                               /* delay */
 
 	/* SLPOUT - Sleep out & booster on */
-	-1, 0x11,                          
+	-1, 0x11,
 	-2, 500,                               /* delay */
 
 	/* FRMCTR1 - frame rate control: normal mode
 	     frame rate = fosc / (1 x 2 + 40) * (LINE + 2C + 2D) */
-	-1, 0xB1, 0x01, 0x2C, 0x2D, 
+	-1, 0xB1, 0x01, 0x2C, 0x2D,
 
 	/* FRMCTR2 - frame rate control: idle mode
 	     frame rate = fosc / (1 x 2 + 40) * (LINE + 2C + 2D) */
-	-1, 0xB2, 0x01, 0x2C, 0x2D, 
+	-1, 0xB2, 0x01, 0x2C, 0x2D,
 
 	/* FRMCTR3 - frame rate control - partial mode
 	     dot inversion mode, line inversion mode */
@@ -68,7 +68,7 @@ static int default_init_sequence[] = {
 
 	/* PWCTR4 - Power Control
 	     BCLK/2, Opamp current small & Medium low */
-	-1, 0xC3,0x8A,0x2A,
+	-1, 0xC3, 0x8A, 0x2A,
 
 	/* PWCTR5 - Power Control */
 	-1, 0xC4, 0x8A, 0xEE,
@@ -91,7 +91,7 @@ static int default_init_sequence[] = {
 	-2, 10,                               /* delay */
 
 	/* end marker */
-	-3                                  
+	-3
 };
 
 static void set_addr_win(struct fbtft_par *par, int xs, int ys, int xe, int ye)
@@ -102,7 +102,7 @@ static void set_addr_win(struct fbtft_par *par, int xs, int ys, int xe, int ye)
 	/* Column address */
 	write_reg(par, 0x2A, xs >> 8, xs & 0xFF, xe >> 8, xe & 0xFF);
 
-	/* Row adress */
+	/* Row address */
 	write_reg(par, 0x2B, ys >> 8, ys & 0xFF, ye >> 8, ye & 0xFF);
 
 	/* Memory write */
@@ -130,7 +130,7 @@ static int set_var(struct fbtft_par *par)
 		write_reg(par, 0x36, MY | MV | (par->bgr << 3));
 		break;
 	case 180:
-		write_reg(par, 0x36, (par->bgr << 3));
+		write_reg(par, 0x36, par->bgr << 3);
 		break;
 	case 90:
 		write_reg(par, 0x36, MX | MV | (par->bgr << 3));
@@ -148,21 +148,21 @@ static int set_var(struct fbtft_par *par)
 #define CURVE(num, idx)  curves[num*par->gamma.num_values + idx]
 static int set_gamma(struct fbtft_par *par, unsigned long *curves)
 {
-	int i,j;
+	int i, j;
 
 	fbtft_par_dbg(DEBUG_INIT_DISPLAY, par, "%s()\n", __func__);
 
 	/* apply mask */
 	for (i = 0; i < par->gamma.num_curves; i++)
 		for (j = 0; j < par->gamma.num_values; j++)
-			CURVE(i,j) &= 0b111111;
+			CURVE(i, j) &= 0x3f;
 
 	for (i = 0; i < par->gamma.num_curves; i++)
 		write_reg(par, 0xE0 + i,
 			CURVE(i, 0), CURVE(i, 1), CURVE(i, 2), CURVE(i, 3),
 			CURVE(i, 4), CURVE(i, 5), CURVE(i, 6), CURVE(i, 7),
 			CURVE(i, 8), CURVE(i, 9), CURVE(i, 10), CURVE(i, 11),
-			CURVE(i, 12), CURVE(i, 13), CURVE(i, 14), CURVE(i,15));
+			CURVE(i, 12), CURVE(i, 13), CURVE(i, 14), CURVE(i, 15));
 
 	return 0;
 }
diff --git a/drivers/staging/fbtft/fb_tinylcd.c b/drivers/staging/fbtft/fb_tinylcd.c
index ca98bfb5c7df..c0b1a337fafd 100644
--- a/drivers/staging/fbtft/fb_tinylcd.c
+++ b/drivers/staging/fbtft/fb_tinylcd.c
@@ -70,7 +70,7 @@ static void set_addr_win(struct fbtft_par *par, int xs, int ys, int xe, int ye)
 	/* Column address */
 	write_reg(par, 0x2A, xs >> 8, xs & 0xFF, xe >> 8, xe & 0xFF);
 
-	/* Row adress */
+	/* Row address */
 	write_reg(par, 0x2B, ys >> 8, ys & 0xFF, ye >> 8, ye & 0xFF);
 
 	/* Memory write */
diff --git a/drivers/staging/fbtft/fb_tls8204.c b/drivers/staging/fbtft/fb_tls8204.c
index 8738c7a7bfda..fcd38bf2ed79 100644
--- a/drivers/staging/fbtft/fb_tls8204.c
+++ b/drivers/staging/fbtft/fb_tls8204.c
@@ -127,7 +127,7 @@ static int write_vmem(struct fbtft_par *par, size_t offset, size_t len)
 		ret = par->fbtftops.write(par, par->txbuf.buf, WIDTH);
 		if (ret < 0) {
 			dev_err(par->info->device,
-				"%s: write failed and returned: %d\n", __func__, ret);
+				"write failed and returned: %d\n", ret);
 			break;
 		}
 	}
diff --git a/drivers/staging/fbtft/fb_uc1701.c b/drivers/staging/fbtft/fb_uc1701.c
index d70ac524278c..26d669b57916 100644
--- a/drivers/staging/fbtft/fb_uc1701.c
+++ b/drivers/staging/fbtft/fb_uc1701.c
@@ -183,7 +183,8 @@ static int write_vmem(struct fbtft_par *par, size_t offset, size_t len)
 	}
 
 	if (ret < 0)
-		dev_err(par->info->device, "%s: write failed and returned: %d\n", __func__, ret);
+		dev_err(par->info->device, "write failed and returned: %d\n",
+			ret);
 
 	return ret;
 }
diff --git a/drivers/staging/fbtft/fb_upd161704.c b/drivers/staging/fbtft/fb_upd161704.c
index fff57b330ba2..176c2106d724 100644
--- a/drivers/staging/fbtft/fb_upd161704.c
+++ b/drivers/staging/fbtft/fb_upd161704.c
@@ -47,84 +47,84 @@ static int init_display(struct fbtft_par *par)
 	/* Initialization sequence from Lib_UTFT */
 
 	/* register reset */
-	write_reg(par, 0x0003,0x0001);	/* Soft reset */
+	write_reg(par, 0x0003, 0x0001);	/* Soft reset */
 
 	/* oscillator start */
-	write_reg(par, 0x003A,0x0001);	/*Oscillator 0: stop, 1: operation */
+	write_reg(par, 0x003A, 0x0001);	/*Oscillator 0: stop, 1: operation */
 	udelay(100);
 
 	/* y-setting */
-	write_reg(par, 0x0024,0x007B);	/* amplitude setting */
+	write_reg(par, 0x0024, 0x007B);	/* amplitude setting */
 	udelay(10);
-	write_reg(par, 0x0025,0x003B);	/* amplitude setting */
-	write_reg(par, 0x0026,0x0034);	/* amplitude setting */
+	write_reg(par, 0x0025, 0x003B);	/* amplitude setting */
+	write_reg(par, 0x0026, 0x0034);	/* amplitude setting */
 	udelay(10);
-	write_reg(par, 0x0027,0x0004);	/* amplitude setting */
-	write_reg(par, 0x0052,0x0025);	/* circuit setting 1 */
+	write_reg(par, 0x0027, 0x0004);	/* amplitude setting */
+	write_reg(par, 0x0052, 0x0025);	/* circuit setting 1 */
 	udelay(10);
-	write_reg(par, 0x0053,0x0033);	/* circuit setting 2 */
-	write_reg(par, 0x0061,0x001C);	/* adjustment V10 positive polarity */
+	write_reg(par, 0x0053, 0x0033);	/* circuit setting 2 */
+	write_reg(par, 0x0061, 0x001C);	/* adjustment V10 positive polarity */
 	udelay(10);
-	write_reg(par, 0x0062,0x002C);	/* adjustment V9 negative polarity */
-	write_reg(par, 0x0063,0x0022);	/* adjustment V34 positive polarity */
+	write_reg(par, 0x0062, 0x002C);	/* adjustment V9 negative polarity */
+	write_reg(par, 0x0063, 0x0022);	/* adjustment V34 positive polarity */
 	udelay(10);
-	write_reg(par, 0x0064,0x0027);	/* adjustment V31 negative polarity */
+	write_reg(par, 0x0064, 0x0027);	/* adjustment V31 negative polarity */
 	udelay(10);
-	write_reg(par, 0x0065,0x0014);	/* adjustment V61 negative polarity */
+	write_reg(par, 0x0065, 0x0014);	/* adjustment V61 negative polarity */
 	udelay(10);
-	write_reg(par, 0x0066,0x0010);	/* adjustment V61 negative polarity */
-	
+	write_reg(par, 0x0066, 0x0010);	/* adjustment V61 negative polarity */
+
 	/* Basical clock for 1 line (BASECOUNT[7:0]) number specified */
-	write_reg(par, 0x002E,0x002D);
-	
+	write_reg(par, 0x002E, 0x002D);
+
 	/* Power supply setting */
-	write_reg(par, 0x0019,0x0000);	/* DC/DC output setting */
+	write_reg(par, 0x0019, 0x0000);	/* DC/DC output setting */
 	udelay(200);
-	write_reg(par, 0x001A,0x1000);	/* DC/DC frequency setting */
-	write_reg(par, 0x001B,0x0023);	/* DC/DC rising setting */
-	write_reg(par, 0x001C,0x0C01);	/* Regulator voltage setting */
-	write_reg(par, 0x001D,0x0000);	/* Regulator current setting */
-	write_reg(par, 0x001E,0x0009);	/* VCOM output setting */
-	write_reg(par, 0x001F,0x0035);	/* VCOM amplitude setting */
-	write_reg(par, 0x0020,0x0015);	/* VCOMM cencter setting */
-	write_reg(par, 0x0018,0x1E7B);	/* DC/DC operation setting */
+	write_reg(par, 0x001A, 0x1000);	/* DC/DC frequency setting */
+	write_reg(par, 0x001B, 0x0023);	/* DC/DC rising setting */
+	write_reg(par, 0x001C, 0x0C01);	/* Regulator voltage setting */
+	write_reg(par, 0x001D, 0x0000);	/* Regulator current setting */
+	write_reg(par, 0x001E, 0x0009);	/* VCOM output setting */
+	write_reg(par, 0x001F, 0x0035);	/* VCOM amplitude setting */
+	write_reg(par, 0x0020, 0x0015);	/* VCOMM cencter setting */
+	write_reg(par, 0x0018, 0x1E7B);	/* DC/DC operation setting */
 
 	/* windows setting */
-	write_reg(par, 0x0008,0x0000);	/* Minimum X address */
-	write_reg(par, 0x0009,0x00EF);	/* Maximum X address */
-	write_reg(par, 0x000a,0x0000);	/* Minimum Y address */
-	write_reg(par, 0x000b,0x013F);	/* Maximum Y address */
+	write_reg(par, 0x0008, 0x0000);	/* Minimum X address */
+	write_reg(par, 0x0009, 0x00EF);	/* Maximum X address */
+	write_reg(par, 0x000a, 0x0000);	/* Minimum Y address */
+	write_reg(par, 0x000b, 0x013F);	/* Maximum Y address */
 
 	/* LCD display area setting */
-	write_reg(par, 0x0029,0x0000);	/* [LCDSIZE]  X MIN. size set */
-	write_reg(par, 0x002A,0x0000);	/* [LCDSIZE]  Y MIN. size set */
-	write_reg(par, 0x002B,0x00EF);	/* [LCDSIZE]  X MAX. size set */
-	write_reg(par, 0x002C,0x013F);	/* [LCDSIZE]  Y MAX. size set */
+	write_reg(par, 0x0029, 0x0000);	/* [LCDSIZE]  X MIN. size set */
+	write_reg(par, 0x002A, 0x0000);	/* [LCDSIZE]  Y MIN. size set */
+	write_reg(par, 0x002B, 0x00EF);	/* [LCDSIZE]  X MAX. size set */
+	write_reg(par, 0x002C, 0x013F);	/* [LCDSIZE]  Y MAX. size set */
 
 	/* Gate scan setting */
-	write_reg(par, 0x0032,0x0002);
-	
+	write_reg(par, 0x0032, 0x0002);
+
 	/* n line inversion line number */
-	write_reg(par, 0x0033,0x0000);
+	write_reg(par, 0x0033, 0x0000);
 
 	/* Line inversion/frame inversion/interlace setting */
-	write_reg(par, 0x0037,0x0000);
-	
+	write_reg(par, 0x0037, 0x0000);
+
 	/* Gate scan operation setting register */
-	write_reg(par, 0x003B,0x0001);
-	
+	write_reg(par, 0x003B, 0x0001);
+
 	/* Color mode */
 	/*GS = 0: 260-k color (64 gray scale), GS = 1: 8 color (2 gray scale) */
-	write_reg(par, 0x0004,0x0000);
+	write_reg(par, 0x0004, 0x0000);
 
 	/* RAM control register */
-	write_reg(par, 0x0005,0x0000);	/*Window access 00:Normal, 10:Window */
+	write_reg(par, 0x0005, 0x0000);	/*Window access 00:Normal, 10:Window */
 
 	/* Display setting register 2 */
-	write_reg(par, 0x0001,0x0000);
+	write_reg(par, 0x0001, 0x0000);
 
 	/* display setting */
-	write_reg(par, 0x0000,0x0000);	/* display on */
+	write_reg(par, 0x0000, 0x0000);	/* display on */
 
 	return 0;
 }
diff --git a/drivers/staging/fbtft/fb_watterott.c b/drivers/staging/fbtft/fb_watterott.c
index 975b579359f3..88fb2c0132d5 100644
--- a/drivers/staging/fbtft/fb_watterott.c
+++ b/drivers/staging/fbtft/fb_watterott.c
@@ -65,7 +65,7 @@ static void write_reg8_bus8(struct fbtft_par *par, int len, ...)
 	ret = par->fbtftops.write(par, par->buf, len);
 	if (ret < 0) {
 		dev_err(par->info->device,
-			"%s: write() failed and returned %d\n", __func__, ret);
+			"write() failed and returned %d\n", ret);
 		return;
 	}
 }
diff --git a/drivers/staging/fbtft/fbtft-bus.c b/drivers/staging/fbtft/fbtft-bus.c
index b3cddb0b3d69..52af9cbbc2a6 100644
--- a/drivers/staging/fbtft/fbtft-bus.c
+++ b/drivers/staging/fbtft/fbtft-bus.c
@@ -111,7 +111,7 @@ void fbtft_write_reg8_bus9(struct fbtft_par *par, int len, ...)
 	ret = par->fbtftops.write(par, par->buf, (len + pad) * sizeof(u16));
 	if (ret < 0) {
 		dev_err(par->info->device,
-			"%s: write() failed and returned %d\n", __func__, ret);
+			"write() failed and returned %d\n", ret);
 		return;
 	}
 }
diff --git a/drivers/staging/fbtft/fbtft-core.c b/drivers/staging/fbtft/fbtft-core.c
index 37dcf7eb191a..53b748be2712 100644
--- a/drivers/staging/fbtft/fbtft-core.c
+++ b/drivers/staging/fbtft/fbtft-core.c
@@ -41,15 +41,10 @@
 #include <linux/of_gpio.h>
 
 #include "fbtft.h"
-
-extern void fbtft_sysfs_init(struct fbtft_par *par);
-extern void fbtft_sysfs_exit(struct fbtft_par *par);
-extern void fbtft_expand_debug_value(unsigned long *debug);
-extern int fbtft_gamma_parse_str(struct fbtft_par *par, unsigned long *curves,
-						const char *str, int size);
+#include "internal.h"
 
 static unsigned long debug;
-module_param(debug, ulong , 0);
+module_param(debug, ulong, 0);
 MODULE_PARM_DESC(debug, "override device debug level");
 
 static bool dma = true;
@@ -302,12 +297,8 @@ void fbtft_register_backlight(struct fbtft_par *par)
 
 	bl_ops = devm_kzalloc(par->info->device, sizeof(struct backlight_ops),
 				GFP_KERNEL);
-	if (!bl_ops) {
-		dev_err(par->info->device,
-			"%s: could not allocate memeory for backlight operations.\n",
-			__func__);
+	if (!bl_ops)
 		return;
-	}
 
 	bl_ops->get_brightness = fbtft_backlight_get_brightness;
 	bl_ops->update_status = fbtft_backlight_update_status;
@@ -347,7 +338,7 @@ static void fbtft_set_addr_win(struct fbtft_par *par, int xs, int ys, int xe,
 	write_reg(par, 0x2A,
 		(xs >> 8) & 0xFF, xs & 0xFF, (xe >> 8) & 0xFF, xe & 0xFF);
 
-	/* Row adress set */
+	/* Row address set */
 	write_reg(par, 0x2B,
 		(ys >> 8) & 0xFF, ys & 0xFF, (ye >> 8) & 0xFF, ye & 0xFF);
 
@@ -379,7 +370,7 @@ static void fbtft_update_display(struct fbtft_par *par, unsigned start_line,
 	int ret = 0;
 
 	if (unlikely(par->debug & (DEBUG_TIME_FIRST_UPDATE | DEBUG_TIME_EACH_UPDATE))) {
-		if ((par->debug & DEBUG_TIME_EACH_UPDATE) || \
+		if ((par->debug & DEBUG_TIME_EACH_UPDATE) ||
 				((par->debug & DEBUG_TIME_FIRST_UPDATE) && !par->first_update_done)) {
 			getnstimeofday(&ts_start);
 			timeit = true;
@@ -707,9 +698,8 @@ struct fb_info *fbtft_framebuffer_alloc(struct fbtft_display *display,
 
 	/* sanity check */
 	if (display->gamma_num * display->gamma_len > FBTFT_GAMMA_MAX_VALUES_TOTAL) {
-		dev_err(dev,
-			"%s: FBTFT_GAMMA_MAX_VALUES_TOTAL=%d is exceeded\n",
-			__func__, FBTFT_GAMMA_MAX_VALUES_TOTAL);
+		dev_err(dev, "FBTFT_GAMMA_MAX_VALUES_TOTAL=%d is exceeded\n",
+			FBTFT_GAMMA_MAX_VALUES_TOTAL);
 		return NULL;
 	}
 
@@ -1000,7 +990,7 @@ int fbtft_register_framebuffer(struct fb_info *fb_info)
 	fbtft_sysfs_init(par);
 
 	if (par->txbuf.buf)
-		sprintf(text1, ", %d KiB %sbuffer memory",
+		sprintf(text1, ", %zu KiB %sbuffer memory",
 			par->txbuf.len >> 10, par->txbuf.dma ? "DMA " : "");
 	if (spi)
 		sprintf(text2, ", spi%d.%d at %d MHz", spi->master->bus_num,
@@ -1046,7 +1036,6 @@ int fbtft_unregister_framebuffer(struct fb_info *fb_info)
 {
 	struct fbtft_par *par = fb_info->par;
 	struct spi_device *spi = par->spi;
-	int ret;
 
 	if (spi)
 		spi_set_drvdata(spi, NULL);
@@ -1055,8 +1044,7 @@ int fbtft_unregister_framebuffer(struct fb_info *fb_info)
 	if (par->fbtftops.unregister_backlight)
 		par->fbtftops.unregister_backlight(par);
 	fbtft_sysfs_exit(par);
-	ret = unregister_framebuffer(fb_info);
-	return ret;
+	return unregister_framebuffer(fb_info);
 }
 EXPORT_SYMBOL(fbtft_unregister_framebuffer);
 
@@ -1278,7 +1266,7 @@ static int fbtft_verify_gpios(struct fbtft_par *par)
 	fbtft_par_dbg(DEBUG_VERIFY_GPIOS, par, "%s()\n", __func__);
 
 	pdata = par->info->device->platform_data;
-	if (pdata->display.buswidth != 9 && par->startbyte == 0 && \
+	if (pdata->display.buswidth != 9 && par->startbyte == 0 &&
 							par->gpio.dc < 0) {
 		dev_err(par->info->device,
 			"Missing info about 'dc' gpio. Aborting.\n");
diff --git a/drivers/staging/fbtft/fbtft-io.c b/drivers/staging/fbtft/fbtft-io.c
index 32155a7b2a62..a6f091fb975c 100644
--- a/drivers/staging/fbtft/fbtft-io.c
+++ b/drivers/staging/fbtft/fbtft-io.c
@@ -59,8 +59,7 @@ int fbtft_write_spi_emulate_9(struct fbtft_par *par, void *buf, size_t len)
 	}
 	if ((len % 8) != 0) {
 		dev_err(par->info->device,
-			"%s: error: len=%d must be divisible by 8\n",
-			__func__, len);
+			"error: len=%zu must be divisible by 8\n", len);
 		return -EINVAL;
 	}
 
@@ -106,8 +105,8 @@ int fbtft_read_spi(struct fbtft_par *par, void *buf, size_t len)
 	if (par->startbyte) {
 		if (len > 32) {
 			dev_err(par->info->device,
-				"%s: len=%d can't be larger than 32 when using 'startbyte'\n",
-				__func__, len);
+				"len=%zu can't be larger than 32 when using 'startbyte'\n",
+				len);
 			return -EINVAL;
 		}
 		txbuf[0] = par->startbyte | 0x3;
@@ -155,14 +154,14 @@ int fbtft_write_gpio8_wr(struct fbtft_par *par, void *buf, size_t len)
 			for (i = 0; i < 8; i++) {
 				if ((data & 1) != (prev_data & 1))
 					gpio_set_value(par->gpio.db[i],
-								(data & 1));
+								data & 1);
 				data >>= 1;
 				prev_data >>= 1;
 			}
 		}
 #else
 		for (i = 0; i < 8; i++) {
-			gpio_set_value(par->gpio.db[i], (data & 1));
+			gpio_set_value(par->gpio.db[i], data & 1);
 			data >>= 1;
 		}
 #endif
@@ -205,14 +204,14 @@ int fbtft_write_gpio16_wr(struct fbtft_par *par, void *buf, size_t len)
 			for (i = 0; i < 16; i++) {
 				if ((data & 1) != (prev_data & 1))
 					gpio_set_value(par->gpio.db[i],
-								(data & 1));
+								data & 1);
 				data >>= 1;
 				prev_data >>= 1;
 			}
 		}
 #else
 		for (i = 0; i < 16; i++) {
-			gpio_set_value(par->gpio.db[i], (data & 1));
+			gpio_set_value(par->gpio.db[i], data & 1);
 			data >>= 1;
 		}
 #endif
diff --git a/drivers/staging/fbtft/fbtft-sysfs.c b/drivers/staging/fbtft/fbtft-sysfs.c
index 45f8de3d11ad..c4cc452f9f2b 100644
--- a/drivers/staging/fbtft/fbtft-sysfs.c
+++ b/drivers/staging/fbtft/fbtft-sysfs.c
@@ -1,5 +1,5 @@
 #include "fbtft.h"
-
+#include "internal.h"
 
 static int get_next_ulong(char **str_p, unsigned long *val, char *sep, int base)
 {
@@ -37,10 +37,9 @@ int fbtft_gamma_parse_str(struct fbtft_par *par, unsigned long *curves,
 
 	fbtft_par_dbg(DEBUG_SYSFS, par, "%s\n", str);
 
-	tmp = kmalloc(size+1, GFP_KERNEL);
+	tmp = kmemdup(str, size + 1, GFP_KERNEL);
 	if (!tmp)
 		return -ENOMEM;
-	memcpy(tmp, str, size+1);
 
 	/* replace optional separators */
 	str_p = tmp;
@@ -153,7 +152,7 @@ static struct device_attribute gamma_device_attrs[] = {
 
 void fbtft_expand_debug_value(unsigned long *debug)
 {
-	switch (*debug & 0b111) {
+	switch (*debug & 0x7) {
 	case 1:
 		*debug |= DEBUG_LEVEL_1;
 		break;
diff --git a/drivers/staging/fbtft/fbtft.h b/drivers/staging/fbtft/fbtft.h
index 0dbf3f95fe78..9fd98cb53418 100644
--- a/drivers/staging/fbtft/fbtft.h
+++ b/drivers/staging/fbtft/fbtft.h
@@ -147,7 +147,7 @@ struct fbtft_display {
 /**
  * struct fbtft_platform_data - Passes display specific data to the driver
  * @display: Display properties
- * @gpios: Pointer to an array of piname to gpio mappings
+ * @gpios: Pointer to an array of pinname to gpio mappings
  * @rotate: Display rotation angle
  * @bgr: LCD Controller BGR bit
  * @fps: Frames per second (this will go away, use @fps in @fbtft_display)
@@ -200,7 +200,7 @@ struct fbtft_platform_data {
  * @gpio.cs: LCD Chip Select with parallel interface bus
  * @gpio.db[16]: Parallel databus
  * @gpio.led[16]: Led control signals
- * @gpio.aux[16]: Auxillary signals, not used by core
+ * @gpio.aux[16]: Auxiliary signals, not used by core
  * @init_sequence: Pointer to LCD initialization array
  * @gamma.lock: Mutex for Gamma curve locking
  * @gamma.curves: Pointer to Gamma curve array
@@ -259,9 +259,7 @@ struct fbtft_par {
 #define NUMARGS(...)  (sizeof((int[]){__VA_ARGS__})/sizeof(int))
 
 #define write_reg(par, ...)                                              \
-do {                                                                     \
-	par->fbtftops.write_register(par, NUMARGS(__VA_ARGS__), __VA_ARGS__); \
-} while (0)
+	par->fbtftops.write_register(par, NUMARGS(__VA_ARGS__), __VA_ARGS__)
 
 /* fbtft-core.c */
 extern void fbtft_dbg_hex(const struct device *dev,
@@ -326,8 +324,8 @@ static int fbtft_driver_remove_pdev(struct platform_device *pdev)          \
 }                                                                          \
 									   \
 static const struct of_device_id dt_ids[] = {                              \
-        { .compatible = _compatible },                                     \
-        {},                                                                \
+	{ .compatible = _compatible },                                     \
+	{},                                                                \
 };                                                                         \
 									   \
 MODULE_DEVICE_TABLE(of, dt_ids);                                           \
@@ -337,7 +335,7 @@ static struct spi_driver fbtft_driver_spi_driver = {                       \
 	.driver = {                                                        \
 		.name   = _name,                                           \
 		.owner  = THIS_MODULE,                                     \
-                .of_match_table = of_match_ptr(dt_ids),                    \
+		.of_match_table = of_match_ptr(dt_ids),                    \
 	},                                                                 \
 	.probe  = fbtft_driver_probe_spi,                                  \
 	.remove = fbtft_driver_remove_spi,                                 \
@@ -347,7 +345,7 @@ static struct platform_driver fbtft_driver_platform_driver = {             \
 	.driver = {                                                        \
 		.name   = _name,                                           \
 		.owner  = THIS_MODULE,                                     \
-                .of_match_table = of_match_ptr(dt_ids),                    \
+		.of_match_table = of_match_ptr(dt_ids),                    \
 	},                                                                 \
 	.probe  = fbtft_driver_probe_pdev,                                 \
 	.remove = fbtft_driver_remove_pdev,                                \
diff --git a/drivers/staging/fbtft/fbtft_device.c b/drivers/staging/fbtft/fbtft_device.c
index b9f4c30e39c6..df6cd775ac1e 100644
--- a/drivers/staging/fbtft/fbtft_device.c
+++ b/drivers/staging/fbtft/fbtft_device.c
@@ -29,8 +29,8 @@
 
 #define MAX_GPIOS 32
 
-struct spi_device *spi_device;
-struct platform_device *p_device;
+static struct spi_device *spi_device;
+static struct platform_device *p_device;
 
 static char *name;
 module_param(name, charp, 0);
@@ -109,7 +109,7 @@ module_param_array(init, int, &init_num, 0);
 MODULE_PARM_DESC(init, "Init sequence, used with the custom argument");
 
 static unsigned long debug;
-module_param(debug, ulong , 0);
+module_param(debug, ulong, 0);
 MODULE_PARM_DESC(debug,
 "level: 0-7 (the remaining 29 bits is for advanced usage)");
 
@@ -136,43 +136,69 @@ static void adafruit18_green_tab_set_addr_win(struct fbtft_par *par,
 		"03 1d 07 06 2E 2C 29 2D 2E 2E 37 3F 00 00 02 10"
 
 static int hy28b_init_sequence[] = {
-	-1,0x00e7,0x0010,-1,0x0000,0x0001,-1,0x0001,0x0100,-1,0x0002,0x0700,
-	-1,0x0003,0x1030,-1,0x0004,0x0000,-1,0x0008,0x0207,-1,0x0009,0x0000,
-	-1,0x000a,0x0000,-1,0x000c,0x0001,-1,0x000d,0x0000,-1,0x000f,0x0000,
-	-1,0x0010,0x0000,-1,0x0011,0x0007,-1,0x0012,0x0000,-1,0x0013,0x0000,
-	-2,50,-1,0x0010,0x1590,-1,0x0011,0x0227,-2,50,-1,0x0012,0x009c,-2,50,
-	-1,0x0013,0x1900,-1,0x0029,0x0023,-1,0x002b,0x000e,-2,50,
-	-1,0x0020,0x0000,-1,0x0021,0x0000,-2,50,-1,0x0050,0x0000,
-	-1,0x0051,0x00ef,-1,0x0052,0x0000,-1,0x0053,0x013f,-1,0x0060,0xa700,
-	-1,0x0061,0x0001,-1,0x006a,0x0000,-1,0x0080,0x0000,-1,0x0081,0x0000,
-	-1,0x0082,0x0000,-1,0x0083,0x0000,-1,0x0084,0x0000,-1,0x0085,0x0000,
-	-1,0x0090,0x0010,-1,0x0092,0x0000,-1,0x0093,0x0003,-1,0x0095,0x0110,
-	-1,0x0097,0x0000,-1,0x0098,0x0000,-1,0x0007,0x0133,-1,0x0020,0x0000,
-	-1,0x0021,0x0000,-2,100,-3 };
+	-1, 0x00e7, 0x0010, -1, 0x0000, 0x0001,
+	-1, 0x0001, 0x0100, -1, 0x0002, 0x0700,
+	-1, 0x0003, 0x1030, -1, 0x0004, 0x0000,
+	-1, 0x0008, 0x0207, -1, 0x0009, 0x0000,
+	-1, 0x000a, 0x0000, -1, 0x000c, 0x0001,
+	-1, 0x000d, 0x0000, -1, 0x000f, 0x0000,
+	-1, 0x0010, 0x0000, -1, 0x0011, 0x0007,
+	-1, 0x0012, 0x0000, -1, 0x0013, 0x0000,
+	-2, 50, -1, 0x0010, 0x1590, -1, 0x0011,
+	0x0227, -2, 50, -1, 0x0012, 0x009c, -2, 50,
+	-1, 0x0013, 0x1900, -1, 0x0029, 0x0023,
+	-1, 0x002b, 0x000e, -2, 50,
+	-1, 0x0020, 0x0000, -1, 0x0021, 0x0000,
+	-2, 50, -1, 0x0050, 0x0000,
+	-1, 0x0051, 0x00ef, -1, 0x0052, 0x0000,
+	-1, 0x0053, 0x013f, -1, 0x0060, 0xa700,
+	-1, 0x0061, 0x0001, -1, 0x006a, 0x0000,
+	-1, 0x0080, 0x0000, -1, 0x0081, 0x0000,
+	-1, 0x0082, 0x0000, -1, 0x0083, 0x0000,
+	-1, 0x0084, 0x0000, -1, 0x0085, 0x0000,
+	-1, 0x0090, 0x0010, -1, 0x0092, 0x0000,
+	-1, 0x0093, 0x0003, -1, 0x0095, 0x0110,
+	-1, 0x0097, 0x0000, -1, 0x0098, 0x0000,
+	-1, 0x0007, 0x0133, -1, 0x0020, 0x0000,
+	-1, 0x0021, 0x0000, -2, 100, -3 };
 
 #define HY28B_GAMMA \
 	"04 1F 4 7 7 0 7 7 6 0\n" \
 	"0F 00 1 7 4 0 0 0 6 7"
 
 static int pitft_init_sequence[] = {
-	-1,0x01,-2,5,-1,0x28,-1,0xEF,0x03,0x80,0x02,-1,0xCF,0x00,0xC1,0x30,
-	-1,0xED,0x64,0x03,0x12,0x81,-1,0xE8,0x85,0x00,0x78,
-	-1,0xCB,0x39,0x2C,0x00,0x34,0x02,-1,0xF7,0x20,-1,0xEA,0x00,0x00,
-	-1,0xC0,0x23,-1,0xC1,0x10,-1,0xC5,0x3e,0x28,-1,0xC7,0x86,-1,0x3A,0x55,
-	-1,0xB1,0x00,0x18,-1,0xB6,0x08,0x82,0x27,-1,0xF2,0x00,-1,0x26,0x01,
-	-1,0xE0,0x0F,0x31,0x2B,0x0C,0x0E,0x08,0x4E,0xF1,0x37,0x07,0x10,0x03,
-	0x0E,0x09,0x00,-1,0xE1,0x00,0x0E,0x14,0x03,0x11,0x07,0x31,0xC1,0x48,
-	0x08,0x0F,0x0C,0x31,0x36,0x0F,-1,0x11,-2,100,-1,0x29,-2,20,-3 };
+	-1, 0x01, -2, 5, -1, 0x28, -1, 0xEF,
+	0x03, 0x80, 0x02, -1, 0xCF, 0x00, 0xC1, 0x30,
+	-1, 0xED, 0x64, 0x03, 0x12, 0x81,
+	-1, 0xE8, 0x85, 0x00, 0x78,
+	-1, 0xCB, 0x39, 0x2C, 0x00, 0x34, 0x02,
+	-1, 0xF7, 0x20, -1, 0xEA, 0x00, 0x00,
+	-1, 0xC0, 0x23, -1, 0xC1, 0x10, -1, 0xC5,
+	0x3e, 0x28, -1, 0xC7, 0x86, -1, 0x3A, 0x55,
+	-1, 0xB1, 0x00, 0x18, -1, 0xB6, 0x08, 0x82,
+	0x27, -1, 0xF2, 0x00, -1, 0x26, 0x01,
+	-1, 0xE0, 0x0F, 0x31, 0x2B, 0x0C, 0x0E, 0x08,
+	0x4E, 0xF1, 0x37, 0x07, 0x10, 0x03,
+	0x0E, 0x09, 0x00, -1, 0xE1, 0x00, 0x0E, 0x14,
+	0x03, 0x11, 0x07, 0x31, 0xC1, 0x48,
+	0x08, 0x0F, 0x0C, 0x31, 0x36, 0x0F, -1,
+	0x11, -2, 100, -1, 0x29, -2, 20, -3 };
 
 static int waveshare32b_init_sequence[] = {
-	-1,0xCB,0x39,0x2C,0x00,0x34,0x02,-1,0xCF,0x00,0xC1,0x30,
-	-1,0xE8,0x85,0x00,0x78,-1,0xEA,0x00,0x00,-1,0xED,0x64,0x03,0x12,0x81,
-	-1,0xF7,0x20,-1,0xC0,0x23,-1,0xC1,0x10,-1,0xC5,0x3e,0x28,-1,0xC7,0x86,
-	-1,0x36,0x28,-1,0x3A,0x55,-1,0xB1,0x00,0x18,-1,0xB6,0x08,0x82,0x27,
-	-1,0xF2,0x00,-1,0x26,0x01,
-	-1,0xE0,0x0F,0x31,0x2B,0x0C,0x0E,0x08,0x4E,0xF1,0x37,0x07,0x10,0x03,0x0E,0x09,0x00,
-	-1,0xE1,0x00,0x0E,0x14,0x03,0x11,0x07,0x31,0xC1,0x48,0x08,0x0F,0x0C,0x31,0x36,0x0F,
-	-1,0x11,-2,120,-1,0x29,-1,0x2c,-3 };
+	-1, 0xCB, 0x39, 0x2C, 0x00, 0x34, 0x02,
+	-1, 0xCF, 0x00, 0xC1, 0x30,
+	-1, 0xE8, 0x85, 0x00, 0x78, -1, 0xEA, 0x00,
+	0x00, -1, 0xED, 0x64, 0x03, 0x12, 0x81,
+	-1, 0xF7, 0x20, -1, 0xC0, 0x23, -1, 0xC1,
+	0x10, -1, 0xC5, 0x3e, 0x28, -1, 0xC7, 0x86,
+	-1, 0x36, 0x28, -1, 0x3A, 0x55, -1, 0xB1, 0x00,
+	0x18, -1, 0xB6, 0x08, 0x82, 0x27,
+	-1, 0xF2, 0x00, -1, 0x26, 0x01,
+	-1, 0xE0, 0x0F, 0x31, 0x2B, 0x0C, 0x0E, 0x08, 0x4E,
+	0xF1, 0x37, 0x07, 0x10, 0x03, 0x0E, 0x09, 0x00,
+	-1, 0xE1, 0x00, 0x0E, 0x14, 0x03, 0x11, 0x07, 0x31,
+	0xC1, 0x48, 0x08, 0x0F, 0x0C, 0x31, 0x36, 0x0F,
+	-1, 0x11, -2, 120, -1, 0x29, -1, 0x2c, -3 };
 
 /* Supported displays in alphabetical order */
 static struct fbtft_device_display displays[] = {
@@ -463,7 +489,7 @@ static struct fbtft_device_display displays[] = {
 					.buswidth = 8,
 					.backlight = 1,
 				},
-				.startbyte = 0b01110000,
+				.startbyte = 0x70,
 				.bgr = true,
 				.gpios = (const struct fbtft_gpio []) {
 					{ "reset", 25 },
@@ -484,9 +510,9 @@ static struct fbtft_device_display displays[] = {
 					.backlight = 1,
 					.init_sequence = hy28b_init_sequence,
 				},
-				.startbyte = 0b01110000,
+				.startbyte = 0x70,
 				.bgr = true,
-				.fps= 50,
+				.fps = 50,
 				.gpios = (const struct fbtft_gpio []) {
 					{ "reset", 25 },
 					{ "led", 18 },
@@ -597,7 +623,7 @@ static struct fbtft_device_display displays[] = {
 					.buswidth = 8,
 					.backlight = 1,
 				},
-				.startbyte = 0b01110000,
+				.startbyte = 0x70,
 				.bgr = true,
 				.gpios = (const struct fbtft_gpio []) {
 					{ "reset", 25 },
@@ -676,6 +702,24 @@ static struct fbtft_device_display displays[] = {
 			}
 		}
 	}, {
+		.name = "nokia5110",
+		.spi = &(struct spi_board_info) {
+			.modalias = "fb_ili9163",
+			.max_speed_hz = 12000000,
+			.mode = SPI_MODE_0,
+			.platform_data = &(struct fbtft_platform_data) {
+				.display = {
+					.buswidth = 8,
+					.backlight = 1,
+				},
+				.bgr = true,
+				.gpios = (const struct fbtft_gpio []) {
+					{},
+				},
+			}
+		}
+	}, {
+
 		.name = "piscreen",
 		.spi = &(struct spi_board_info) {
 			.modalias = "fb_ili9486",
@@ -1100,14 +1144,14 @@ static int write_gpio16_wr_slow(struct fbtft_par *par, void *buf, size_t len)
 			for (i = 0; i < 16; i++) {
 				if ((data & 1) != (prev_data & 1))
 					gpio_set_value(par->gpio.db[i],
-								(data & 1));
+								data & 1);
 				data >>= 1;
 				prev_data >>= 1;
 			}
 		}
 #else
 		for (i = 0; i < 16; i++) {
-			gpio_set_value(par->gpio.db[i], (data & 1));
+			gpio_set_value(par->gpio.db[i], data & 1);
 			data >>= 1;
 		}
 #endif
diff --git a/drivers/staging/fbtft/flexfb.c b/drivers/staging/fbtft/flexfb.c
index 90832c36e557..ca39fe90d1b8 100644
--- a/drivers/staging/fbtft/flexfb.c
+++ b/drivers/staging/fbtft/flexfb.c
@@ -423,7 +423,7 @@ static int flexfb_probe_common(struct spi_device *sdev, struct platform_device *
 			}
 			par->fbtftops.write_register = fbtft_write_reg8_bus9;
 			par->fbtftops.write_vmem = fbtft_write_vmem16_bus9;
-			sdev->bits_per_word=9;
+			sdev->bits_per_word = 9;
 			ret = sdev->master->setup(sdev);
 			if (ret) {
 				dev_warn(dev,
diff --git a/drivers/staging/fbtft/internal.h b/drivers/staging/fbtft/internal.h
new file mode 100644
index 000000000000..f69db8289151
--- /dev/null
+++ b/drivers/staging/fbtft/internal.h
@@ -0,0 +1,25 @@
+/*
+ * Copyright (C) 2013 Noralf Tronnes
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the 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 __LINUX_FBTFT__INTERNAL_H
+#define __LINUX_FBTFT_INTERNAL_H
+
+void fbtft_sysfs_init(struct fbtft_par *par);
+void fbtft_sysfs_exit(struct fbtft_par *par);
+void fbtft_expand_debug_value(unsigned long *debug);
+int fbtft_gamma_parse_str(struct fbtft_par *par, unsigned long *curves,
+			  const char *str, int size);
+
+#endif /* __LINUX_FBTFT_INTERNAL_H */
diff --git a/drivers/staging/fsl-mc/Kconfig b/drivers/staging/fsl-mc/Kconfig
new file mode 100644
index 000000000000..32df07b15e09
--- /dev/null
+++ b/drivers/staging/fsl-mc/Kconfig
@@ -0,0 +1 @@
+source "drivers/staging/fsl-mc/bus/Kconfig"
diff --git a/drivers/staging/fsl-mc/Makefile b/drivers/staging/fsl-mc/Makefile
new file mode 100644
index 000000000000..9c6a00128c65
--- /dev/null
+++ b/drivers/staging/fsl-mc/Makefile
@@ -0,0 +1,2 @@
+# Freescale Management Complex (MC) bus drivers
+obj-$(CONFIG_FSL_MC_BUS)	+= bus/
diff --git a/drivers/staging/fsl-mc/TODO b/drivers/staging/fsl-mc/TODO
new file mode 100644
index 000000000000..d78288b4e721
--- /dev/null
+++ b/drivers/staging/fsl-mc/TODO
@@ -0,0 +1,13 @@
+* Add README file (with ASCII art) describing relationships between
+  DPAA2 objects and how combine them to make a NIC, an LS2 switch, etc.
+  Also, define all acronyms used.
+
+* Decide if multiple root fsl-mc buses will be supported per Linux instance,
+  and if so add support for this.
+
+* Add at least one device driver for a DPAA2 object (child device of the
+  fsl-mc bus).
+
+Please send any patches to Greg Kroah-Hartman <gregkh@linuxfoundation.org>,
+german.rivera@freescale.com, devel@driverdev.osuosl.org,
+linux-kernel@vger.kernel.org
diff --git a/drivers/staging/fsl-mc/bus/Kconfig b/drivers/staging/fsl-mc/bus/Kconfig
new file mode 100644
index 000000000000..0d779d9ccbd8
--- /dev/null
+++ b/drivers/staging/fsl-mc/bus/Kconfig
@@ -0,0 +1,24 @@
+#
+# Freescale Management Complex (MC) bus drivers
+#
+# Copyright (C) 2014 Freescale Semiconductor, Inc.
+#
+# This file is released under the GPLv2
+#
+
+config FSL_MC_BUS
+	tristate "Freescale Management Complex (MC) bus driver"
+	depends on OF && ARM64
+	help
+	  Driver to enable the bus infrastructure for the Freescale
+          QorIQ Management Complex (fsl-mc). The fsl-mc is a hardware
+	  module of the QorIQ LS2 SoCs, that does resource management
+	  for hardware building-blocks in the SoC that can be used
+	  to dynamically create networking hardware objects such as
+	  network interfaces (NICs), crypto accelerator instances,
+	  or L2 switches.
+
+	  Only enable this option when building the kernel for
+	  Freescale QorQIQ LS2xxxx SoCs.
+
+
diff --git a/drivers/staging/fsl-mc/bus/Makefile b/drivers/staging/fsl-mc/bus/Makefile
new file mode 100644
index 000000000000..25433a998478
--- /dev/null
+++ b/drivers/staging/fsl-mc/bus/Makefile
@@ -0,0 +1,17 @@
+#
+# Freescale Management Complex (MC) bus drivers
+#
+# Copyright (C) 2014 Freescale Semiconductor, Inc.
+#
+# This file is released under the GPLv2
+#
+obj-$(CONFIG_FSL_MC_BUS) += mc-bus-driver.o
+
+mc-bus-driver-objs := mc-bus.o \
+		      mc-sys.o \
+		      dprc.o \
+		      dpmng.o \
+		      dprc-driver.o \
+		      mc-allocator.o \
+		      dpmcp.o \
+		      dpbp.o
diff --git a/drivers/staging/fsl-mc/bus/dpbp.c b/drivers/staging/fsl-mc/bus/dpbp.c
new file mode 100644
index 000000000000..d99ab6d0bbb1
--- /dev/null
+++ b/drivers/staging/fsl-mc/bus/dpbp.c
@@ -0,0 +1,358 @@
+/* Copyright 2013-2014 Freescale Semiconductor Inc.
+*
+* 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 of the above-listed copyright holders nor the
+* names of any contributors may be used to endorse or promote products
+* derived from this software without specific prior written permission.
+*
+*
+* ALTERNATIVELY, this software may be distributed under the terms of the
+* GNU General Public License ("GPL") as published by the Free Software
+* Foundation, either version 2 of that License or (at your option) any
+* later version.
+*
+* 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 HOLDERS 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 "../include/mc-sys.h"
+#include "../include/mc-cmd.h"
+#include "../include/dpbp.h"
+#include "../include/dpbp-cmd.h"
+
+int dpbp_open(struct fsl_mc_io *mc_io, int dpbp_id, uint16_t *token)
+{
+	struct mc_command cmd = { 0 };
+	int err;
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPBP_CMDID_OPEN,
+					  MC_CMD_PRI_LOW, 0);
+	cmd.params[0] |= mc_enc(0, 32, dpbp_id);
+
+	/* send command to mc*/
+	err = mc_send_command(mc_io, &cmd);
+	if (err)
+		return err;
+
+	/* retrieve response parameters */
+	*token = MC_CMD_HDR_READ_TOKEN(cmd.header);
+
+	return err;
+}
+EXPORT_SYMBOL(dpbp_open);
+
+int dpbp_close(struct fsl_mc_io *mc_io, uint16_t token)
+{
+	struct mc_command cmd = { 0 };
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPBP_CMDID_CLOSE, MC_CMD_PRI_HIGH,
+					  token);
+
+	/* send command to mc*/
+	return mc_send_command(mc_io, &cmd);
+}
+EXPORT_SYMBOL(dpbp_close);
+
+int dpbp_create(struct fsl_mc_io *mc_io,
+		const struct dpbp_cfg *cfg,
+		uint16_t *token)
+{
+	struct mc_command cmd = { 0 };
+	int err;
+
+	(void)(cfg); /* unused */
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPBP_CMDID_CREATE,
+					  MC_CMD_PRI_LOW, 0);
+
+	/* send command to mc*/
+	err = mc_send_command(mc_io, &cmd);
+	if (err)
+		return err;
+
+	/* retrieve response parameters */
+	*token = MC_CMD_HDR_READ_TOKEN(cmd.header);
+
+	return 0;
+}
+
+int dpbp_destroy(struct fsl_mc_io *mc_io, uint16_t token)
+{
+	struct mc_command cmd = { 0 };
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPBP_CMDID_DESTROY,
+					  MC_CMD_PRI_LOW, token);
+
+	/* send command to mc*/
+	return mc_send_command(mc_io, &cmd);
+}
+
+int dpbp_enable(struct fsl_mc_io *mc_io, uint16_t token)
+{
+	struct mc_command cmd = { 0 };
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPBP_CMDID_ENABLE, MC_CMD_PRI_LOW,
+					  token);
+
+	/* send command to mc*/
+	return mc_send_command(mc_io, &cmd);
+}
+EXPORT_SYMBOL(dpbp_enable);
+
+int dpbp_disable(struct fsl_mc_io *mc_io, uint16_t token)
+{
+	struct mc_command cmd = { 0 };
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPBP_CMDID_DISABLE,
+					  MC_CMD_PRI_LOW, token);
+
+	/* send command to mc*/
+	return mc_send_command(mc_io, &cmd);
+}
+EXPORT_SYMBOL(dpbp_disable);
+
+int dpbp_is_enabled(struct fsl_mc_io *mc_io, uint16_t token, int *en)
+{
+	struct mc_command cmd = { 0 };
+	int err;
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPBP_CMDID_IS_ENABLED, MC_CMD_PRI_LOW,
+					  token);
+
+	/* send command to mc*/
+	err = mc_send_command(mc_io, &cmd);
+	if (err)
+		return err;
+
+	/* retrieve response parameters */
+	*en = (int)mc_dec(cmd.params[0], 0, 1);
+
+	return 0;
+}
+
+int dpbp_reset(struct fsl_mc_io *mc_io, uint16_t token)
+{
+	struct mc_command cmd = { 0 };
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPBP_CMDID_RESET,
+					  MC_CMD_PRI_LOW, token);
+
+	/* send command to mc*/
+	return mc_send_command(mc_io, &cmd);
+}
+
+int dpbp_set_irq(struct fsl_mc_io *mc_io,
+		 uint16_t token,
+		 uint8_t irq_index,
+		 uint64_t irq_paddr,
+		 uint32_t irq_val,
+		 int user_irq_id)
+{
+	struct mc_command cmd = { 0 };
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPBP_CMDID_SET_IRQ,
+					  MC_CMD_PRI_LOW, token);
+	cmd.params[0] |= mc_enc(0, 8, irq_index);
+	cmd.params[0] |= mc_enc(32, 32, irq_val);
+	cmd.params[1] |= mc_enc(0, 64, irq_paddr);
+	cmd.params[2] |= mc_enc(0, 32, user_irq_id);
+
+	/* send command to mc*/
+	return mc_send_command(mc_io, &cmd);
+}
+
+int dpbp_get_irq(struct fsl_mc_io *mc_io,
+		 uint16_t token,
+		 uint8_t irq_index,
+		 int *type,
+		 uint64_t *irq_paddr,
+		 uint32_t *irq_val,
+		 int *user_irq_id)
+{
+	struct mc_command cmd = { 0 };
+	int err;
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPBP_CMDID_GET_IRQ,
+					  MC_CMD_PRI_LOW, token);
+	cmd.params[0] |= mc_enc(32, 8, irq_index);
+
+	/* send command to mc*/
+	err = mc_send_command(mc_io, &cmd);
+	if (err)
+		return err;
+
+	/* retrieve response parameters */
+	*irq_val = (uint32_t)mc_dec(cmd.params[0], 0, 32);
+	*irq_paddr = (uint64_t)mc_dec(cmd.params[1], 0, 64);
+	*user_irq_id = (int)mc_dec(cmd.params[2], 0, 32);
+	*type = (int)mc_dec(cmd.params[2], 32, 32);
+	return 0;
+}
+
+int dpbp_set_irq_enable(struct fsl_mc_io *mc_io,
+			uint16_t token,
+			uint8_t irq_index,
+			uint8_t en)
+{
+	struct mc_command cmd = { 0 };
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPBP_CMDID_SET_IRQ_ENABLE,
+					  MC_CMD_PRI_LOW, token);
+	cmd.params[0] |= mc_enc(0, 8, en);
+	cmd.params[0] |= mc_enc(32, 8, irq_index);
+
+	/* send command to mc*/
+	return mc_send_command(mc_io, &cmd);
+}
+
+int dpbp_get_irq_enable(struct fsl_mc_io *mc_io,
+			uint16_t token,
+			uint8_t irq_index,
+			uint8_t *en)
+{
+	struct mc_command cmd = { 0 };
+	int err;
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPBP_CMDID_GET_IRQ_ENABLE,
+					  MC_CMD_PRI_LOW, token);
+	cmd.params[0] |= mc_enc(32, 8, irq_index);
+
+	/* send command to mc*/
+	err = mc_send_command(mc_io, &cmd);
+	if (err)
+		return err;
+
+	/* retrieve response parameters */
+	*en = (uint8_t)mc_dec(cmd.params[0], 0, 8);
+	return 0;
+}
+
+int dpbp_set_irq_mask(struct fsl_mc_io *mc_io,
+		      uint16_t token,
+		      uint8_t irq_index,
+		      uint32_t mask)
+{
+	struct mc_command cmd = { 0 };
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPBP_CMDID_SET_IRQ_MASK,
+					  MC_CMD_PRI_LOW, token);
+	cmd.params[0] |= mc_enc(0, 32, mask);
+	cmd.params[0] |= mc_enc(32, 8, irq_index);
+
+	/* send command to mc*/
+	return mc_send_command(mc_io, &cmd);
+}
+
+int dpbp_get_irq_mask(struct fsl_mc_io *mc_io,
+		      uint16_t token,
+		      uint8_t irq_index,
+		      uint32_t *mask)
+{
+	struct mc_command cmd = { 0 };
+	int err;
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPBP_CMDID_GET_IRQ_MASK,
+					  MC_CMD_PRI_LOW, token);
+	cmd.params[0] |= mc_enc(32, 8, irq_index);
+
+	/* send command to mc*/
+	err = mc_send_command(mc_io, &cmd);
+	if (err)
+		return err;
+
+	/* retrieve response parameters */
+	*mask = (uint32_t)mc_dec(cmd.params[0], 0, 32);
+	return 0;
+}
+
+int dpbp_get_irq_status(struct fsl_mc_io *mc_io,
+			uint16_t token,
+			uint8_t irq_index,
+			uint32_t *status)
+{
+	struct mc_command cmd = { 0 };
+	int err;
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPBP_CMDID_GET_IRQ_STATUS,
+					  MC_CMD_PRI_LOW, token);
+	cmd.params[0] |= mc_enc(32, 8, irq_index);
+
+	/* send command to mc*/
+	err = mc_send_command(mc_io, &cmd);
+	if (err)
+		return err;
+
+	/* retrieve response parameters */
+	*status = (uint32_t)mc_dec(cmd.params[0], 0, 32);
+	return 0;
+}
+
+int dpbp_clear_irq_status(struct fsl_mc_io *mc_io,
+			  uint16_t token,
+			  uint8_t irq_index,
+			  uint32_t status)
+{
+	struct mc_command cmd = { 0 };
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPBP_CMDID_CLEAR_IRQ_STATUS,
+					  MC_CMD_PRI_LOW, token);
+	cmd.params[0] |= mc_enc(0, 32, status);
+	cmd.params[0] |= mc_enc(32, 8, irq_index);
+
+	/* send command to mc*/
+	return mc_send_command(mc_io, &cmd);
+}
+
+int dpbp_get_attributes(struct fsl_mc_io *mc_io,
+			uint16_t token,
+			struct dpbp_attr *attr)
+{
+	struct mc_command cmd = { 0 };
+	int err;
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPBP_CMDID_GET_ATTR,
+					  MC_CMD_PRI_LOW, token);
+
+	/* send command to mc*/
+	err = mc_send_command(mc_io, &cmd);
+	if (err)
+		return err;
+
+	/* retrieve response parameters */
+	attr->bpid = (uint16_t)mc_dec(cmd.params[0], 16, 16);
+	attr->id = (int)mc_dec(cmd.params[0], 32, 32);
+	attr->version.major = (uint16_t)mc_dec(cmd.params[1], 0, 16);
+	attr->version.minor = (uint16_t)mc_dec(cmd.params[1], 16, 16);
+	return 0;
+}
+EXPORT_SYMBOL(dpbp_get_attributes);
diff --git a/drivers/staging/fsl-mc/bus/dpmcp-cmd.h b/drivers/staging/fsl-mc/bus/dpmcp-cmd.h
new file mode 100644
index 000000000000..57f326b60b76
--- /dev/null
+++ b/drivers/staging/fsl-mc/bus/dpmcp-cmd.h
@@ -0,0 +1,136 @@
+/* Copyright 2013-2015 Freescale Semiconductor Inc.
+ *
+ * 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 of the above-listed copyright holders nor the
+ * names of any contributors may be used to endorse or promote products
+ * derived from this software without specific prior written permission.
+ *
+ *
+ * ALTERNATIVELY, this software may be distributed under the terms of the
+ * GNU General Public License ("GPL") as published by the Free Software
+ * Foundation, either version 2 of that License or (at your option) any
+ * later version.
+ *
+ * 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 HOLDERS OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+#ifndef _FSL_DPMCP_CMD_H
+#define _FSL_DPMCP_CMD_H
+
+/* DPMCP Version */
+#define DPMCP_VER_MAJOR				2
+#define DPMCP_VER_MINOR				0
+
+/* Command IDs */
+#define DPMCP_CMDID_CLOSE				0x800
+#define DPMCP_CMDID_OPEN				0x80b
+#define DPMCP_CMDID_CREATE				0x90b
+#define DPMCP_CMDID_DESTROY				0x900
+
+#define DPMCP_CMDID_GET_ATTR				0x004
+#define DPMCP_CMDID_RESET				0x005
+
+#define DPMCP_CMDID_SET_IRQ				0x010
+#define DPMCP_CMDID_GET_IRQ				0x011
+#define DPMCP_CMDID_SET_IRQ_ENABLE			0x012
+#define DPMCP_CMDID_GET_IRQ_ENABLE			0x013
+#define DPMCP_CMDID_SET_IRQ_MASK			0x014
+#define DPMCP_CMDID_GET_IRQ_MASK			0x015
+#define DPMCP_CMDID_GET_IRQ_STATUS			0x016
+#define DPMCP_CMDID_CLEAR_IRQ_STATUS			0x017
+
+/*                cmd, param, offset, width, type, arg_name */
+#define DPMCP_CMD_CREATE(cmd, cfg) \
+	MC_CMD_OP(cmd, 0, 0,  32, int,      cfg->portal_id)
+
+/*                cmd, param, offset, width, type, arg_name */
+#define DPMCP_CMD_SET_IRQ(cmd, irq_index, irq_addr, irq_val, user_irq_id) \
+do { \
+	MC_CMD_OP(cmd, 0, 0,  8,  uint8_t,  irq_index);\
+	MC_CMD_OP(cmd, 0, 32, 32, uint32_t, irq_val);\
+	MC_CMD_OP(cmd, 1, 0,  64, uint64_t, irq_addr); \
+	MC_CMD_OP(cmd, 2, 0,  32, int,	    user_irq_id); \
+} while (0)
+
+/*                cmd, param, offset, width, type, arg_name */
+#define DPMCP_CMD_GET_IRQ(cmd, irq_index) \
+	MC_CMD_OP(cmd, 0, 32, 8,  uint8_t,  irq_index)
+
+/*                cmd, param, offset, width, type, arg_name */
+#define DPMCP_RSP_GET_IRQ(cmd, type, irq_addr, irq_val, user_irq_id) \
+do { \
+	MC_RSP_OP(cmd, 0, 0,  32, uint32_t, irq_val); \
+	MC_RSP_OP(cmd, 1, 0,  64, uint64_t, irq_addr); \
+	MC_RSP_OP(cmd, 2, 0,  32, int,	    user_irq_id); \
+	MC_RSP_OP(cmd, 2, 32, 32, int,	    type); \
+} while (0)
+
+/*                cmd, param, offset, width, type, arg_name */
+#define DPMCP_CMD_SET_IRQ_ENABLE(cmd, irq_index, en) \
+do { \
+	MC_CMD_OP(cmd, 0, 0,  8,  uint8_t,  en); \
+	MC_CMD_OP(cmd, 0, 32, 8,  uint8_t,  irq_index);\
+} while (0)
+
+/*                cmd, param, offset, width, type, arg_name */
+#define DPMCP_CMD_GET_IRQ_ENABLE(cmd, irq_index) \
+	MC_CMD_OP(cmd, 0, 32, 8,  uint8_t,  irq_index)
+
+/*                cmd, param, offset, width, type, arg_name */
+#define DPMCP_RSP_GET_IRQ_ENABLE(cmd, en) \
+	MC_RSP_OP(cmd, 0, 0,  8,  uint8_t,  en)
+
+/*                cmd, param, offset, width, type, arg_name */
+#define DPMCP_CMD_SET_IRQ_MASK(cmd, irq_index, mask) \
+do { \
+	MC_CMD_OP(cmd, 0, 0,  32, uint32_t, mask);\
+	MC_CMD_OP(cmd, 0, 32, 8,  uint8_t,  irq_index);\
+} while (0)
+
+/*                cmd, param, offset, width, type, arg_name */
+#define DPMCP_CMD_GET_IRQ_MASK(cmd, irq_index) \
+	MC_CMD_OP(cmd, 0, 32, 8,  uint8_t,  irq_index)
+
+/*                cmd, param, offset, width, type, arg_name */
+#define DPMCP_RSP_GET_IRQ_MASK(cmd, mask) \
+	MC_RSP_OP(cmd, 0, 0,  32, uint32_t, mask)
+
+/*                cmd, param, offset, width, type, arg_name */
+#define DPMCP_CMD_GET_IRQ_STATUS(cmd, irq_index) \
+	MC_CMD_OP(cmd, 0, 32, 8,  uint8_t,  irq_index)
+
+/*                cmd, param, offset, width, type, arg_name */
+#define DPMCP_RSP_GET_IRQ_STATUS(cmd, status) \
+	MC_RSP_OP(cmd, 0, 0,  32, uint32_t, status)
+
+/*                cmd, param, offset, width, type, arg_name */
+#define DPMCP_CMD_CLEAR_IRQ_STATUS(cmd, irq_index, status) \
+do { \
+	MC_CMD_OP(cmd, 0, 0,  32, uint32_t, status); \
+	MC_CMD_OP(cmd, 0, 32, 8,  uint8_t,  irq_index);\
+} while (0)
+
+/*                cmd, param, offset, width, type,	arg_name */
+#define DPMCP_RSP_GET_ATTRIBUTES(cmd, attr) \
+do { \
+	MC_RSP_OP(cmd, 0, 32, 32, int,	    attr->id);\
+	MC_RSP_OP(cmd, 1, 0,  16, uint16_t, attr->version.major);\
+	MC_RSP_OP(cmd, 1, 16, 16, uint16_t, attr->version.minor);\
+} while (0)
+
+#endif /* _FSL_DPMCP_CMD_H */
diff --git a/drivers/staging/fsl-mc/bus/dpmcp.c b/drivers/staging/fsl-mc/bus/dpmcp.c
new file mode 100644
index 000000000000..6b9da5b7fd00
--- /dev/null
+++ b/drivers/staging/fsl-mc/bus/dpmcp.c
@@ -0,0 +1,308 @@
+/* Copyright 2013-2015 Freescale Semiconductor Inc.
+ *
+ * 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 of the above-listed copyright holders nor the
+ * names of any contributors may be used to endorse or promote products
+ * derived from this software without specific prior written permission.
+ *
+ *
+ * ALTERNATIVELY, this software may be distributed under the terms of the
+ * GNU General Public License ("GPL") as published by the Free Software
+ * Foundation, either version 2 of that License or (at your option) any
+ * later version.
+ *
+ * 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 HOLDERS 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 "../include/mc-sys.h"
+#include "../include/mc-cmd.h"
+#include "dpmcp.h"
+#include "dpmcp-cmd.h"
+
+int dpmcp_open(struct fsl_mc_io *mc_io, int dpmcp_id, uint16_t *token)
+{
+	struct mc_command cmd = { 0 };
+	int err;
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPMCP_CMDID_OPEN,
+					  MC_CMD_PRI_LOW, 0);
+	cmd.params[0] |= mc_enc(0, 32, dpmcp_id);
+
+	/* send command to mc*/
+	err = mc_send_command(mc_io, &cmd);
+	if (err)
+		return err;
+
+	/* retrieve response parameters */
+	*token = MC_CMD_HDR_READ_TOKEN(cmd.header);
+
+	return err;
+}
+
+int dpmcp_close(struct fsl_mc_io *mc_io, uint16_t token)
+{
+	struct mc_command cmd = { 0 };
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPMCP_CMDID_CLOSE, MC_CMD_PRI_HIGH,
+					  token);
+
+	/* send command to mc*/
+	return mc_send_command(mc_io, &cmd);
+}
+
+int dpmcp_create(struct fsl_mc_io *mc_io,
+		 const struct dpmcp_cfg *cfg,
+		uint16_t *token)
+{
+	struct mc_command cmd = { 0 };
+	int err;
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPMCP_CMDID_CREATE,
+					  MC_CMD_PRI_LOW, 0);
+	cmd.params[0] |= mc_enc(0, 32, cfg->portal_id);
+
+	/* send command to mc*/
+	err = mc_send_command(mc_io, &cmd);
+	if (err)
+		return err;
+
+	/* retrieve response parameters */
+	*token = MC_CMD_HDR_READ_TOKEN(cmd.header);
+
+	return 0;
+}
+
+int dpmcp_destroy(struct fsl_mc_io *mc_io, uint16_t token)
+{
+	struct mc_command cmd = { 0 };
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPMCP_CMDID_DESTROY,
+					  MC_CMD_PRI_LOW, token);
+
+	/* send command to mc*/
+	return mc_send_command(mc_io, &cmd);
+}
+
+int dpmcp_reset(struct fsl_mc_io *mc_io, uint16_t token)
+{
+	struct mc_command cmd = { 0 };
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPMCP_CMDID_RESET,
+					  MC_CMD_PRI_LOW, token);
+
+	/* send command to mc*/
+	return mc_send_command(mc_io, &cmd);
+}
+
+int dpmcp_set_irq(struct fsl_mc_io *mc_io,
+		  uint16_t token,
+		 uint8_t irq_index,
+		 uint64_t irq_addr,
+		 uint32_t irq_val,
+		 int user_irq_id)
+{
+	struct mc_command cmd = { 0 };
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPMCP_CMDID_SET_IRQ,
+					  MC_CMD_PRI_LOW, token);
+	cmd.params[0] |= mc_enc(0, 8, irq_index);
+	cmd.params[0] |= mc_enc(32, 32, irq_val);
+	cmd.params[1] |= mc_enc(0, 64, irq_addr);
+	cmd.params[2] |= mc_enc(0, 32, user_irq_id);
+
+	/* send command to mc*/
+	return mc_send_command(mc_io, &cmd);
+}
+
+int dpmcp_get_irq(struct fsl_mc_io *mc_io,
+		  uint16_t token,
+		 uint8_t irq_index,
+		 int *type,
+		 uint64_t *irq_addr,
+		 uint32_t *irq_val,
+		 int *user_irq_id)
+{
+	struct mc_command cmd = { 0 };
+	int err;
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPMCP_CMDID_GET_IRQ,
+					  MC_CMD_PRI_LOW, token);
+	cmd.params[0] |= mc_enc(32, 8, irq_index);
+
+	/* send command to mc*/
+	err = mc_send_command(mc_io, &cmd);
+	if (err)
+		return err;
+
+	/* retrieve response parameters */
+	*irq_val = (uint32_t)mc_dec(cmd.params[0], 0, 32);
+	*irq_addr = (uint64_t)mc_dec(cmd.params[1], 0, 64);
+	*user_irq_id = (int)mc_dec(cmd.params[2], 0, 32);
+	*type = (int)mc_dec(cmd.params[2], 32, 32);
+	return 0;
+}
+
+int dpmcp_set_irq_enable(struct fsl_mc_io *mc_io,
+			 uint16_t token,
+			uint8_t irq_index,
+			uint8_t en)
+{
+	struct mc_command cmd = { 0 };
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPMCP_CMDID_SET_IRQ_ENABLE,
+					  MC_CMD_PRI_LOW, token);
+	cmd.params[0] |= mc_enc(0, 8, en);
+	cmd.params[0] |= mc_enc(32, 8, irq_index);
+
+	/* send command to mc*/
+	return mc_send_command(mc_io, &cmd);
+}
+
+int dpmcp_get_irq_enable(struct fsl_mc_io *mc_io,
+			 uint16_t token,
+			uint8_t irq_index,
+			uint8_t *en)
+{
+	struct mc_command cmd = { 0 };
+	int err;
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPMCP_CMDID_GET_IRQ_ENABLE,
+					  MC_CMD_PRI_LOW, token);
+	cmd.params[0] |= mc_enc(32, 8, irq_index);
+
+	/* send command to mc*/
+	err = mc_send_command(mc_io, &cmd);
+	if (err)
+		return err;
+
+	/* retrieve response parameters */
+	*en = (uint8_t)mc_dec(cmd.params[0], 0, 8);
+	return 0;
+}
+
+int dpmcp_set_irq_mask(struct fsl_mc_io *mc_io,
+		       uint16_t token,
+		      uint8_t irq_index,
+		      uint32_t mask)
+{
+	struct mc_command cmd = { 0 };
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPMCP_CMDID_SET_IRQ_MASK,
+					  MC_CMD_PRI_LOW, token);
+	cmd.params[0] |= mc_enc(0, 32, mask);
+	cmd.params[0] |= mc_enc(32, 8, irq_index);
+
+	/* send command to mc*/
+	return mc_send_command(mc_io, &cmd);
+}
+
+int dpmcp_get_irq_mask(struct fsl_mc_io *mc_io,
+		       uint16_t token,
+		      uint8_t irq_index,
+		      uint32_t *mask)
+{
+	struct mc_command cmd = { 0 };
+	int err;
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPMCP_CMDID_GET_IRQ_MASK,
+					  MC_CMD_PRI_LOW, token);
+	cmd.params[0] |= mc_enc(32, 8, irq_index);
+
+	/* send command to mc*/
+	err = mc_send_command(mc_io, &cmd);
+	if (err)
+		return err;
+
+	/* retrieve response parameters */
+	*mask = (uint32_t)mc_dec(cmd.params[0], 0, 32);
+	return 0;
+}
+
+int dpmcp_get_irq_status(struct fsl_mc_io *mc_io,
+			 uint16_t token,
+			uint8_t irq_index,
+			uint32_t *status)
+{
+	struct mc_command cmd = { 0 };
+	int err;
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPMCP_CMDID_GET_IRQ_STATUS,
+					  MC_CMD_PRI_LOW, token);
+	cmd.params[0] |= mc_enc(32, 8, irq_index);
+
+	/* send command to mc*/
+	err = mc_send_command(mc_io, &cmd);
+	if (err)
+		return err;
+
+	/* retrieve response parameters */
+	*status = (uint32_t)mc_dec(cmd.params[0], 0, 32);
+	return 0;
+}
+
+int dpmcp_clear_irq_status(struct fsl_mc_io *mc_io,
+			   uint16_t token,
+			  uint8_t irq_index,
+			  uint32_t status)
+{
+	struct mc_command cmd = { 0 };
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPMCP_CMDID_CLEAR_IRQ_STATUS,
+					  MC_CMD_PRI_LOW, token);
+	cmd.params[0] |= mc_enc(0, 32, status);
+	cmd.params[0] |= mc_enc(32, 8, irq_index);
+
+	/* send command to mc*/
+	return mc_send_command(mc_io, &cmd);
+}
+
+int dpmcp_get_attributes(struct fsl_mc_io *mc_io,
+			 uint16_t token,
+			struct dpmcp_attr *attr)
+{
+	struct mc_command cmd = { 0 };
+	int err;
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPMCP_CMDID_GET_ATTR,
+					  MC_CMD_PRI_LOW, token);
+
+	/* send command to mc*/
+	err = mc_send_command(mc_io, &cmd);
+	if (err)
+		return err;
+
+	/* retrieve response parameters */
+	attr->id = (int)mc_dec(cmd.params[0], 32, 32);
+	attr->version.major = (uint16_t)mc_dec(cmd.params[1], 0, 16);
+	attr->version.minor = (uint16_t)mc_dec(cmd.params[1], 16, 16);
+	return 0;
+}
diff --git a/drivers/staging/fsl-mc/bus/dpmcp.h b/drivers/staging/fsl-mc/bus/dpmcp.h
new file mode 100644
index 000000000000..5e7c21952ce5
--- /dev/null
+++ b/drivers/staging/fsl-mc/bus/dpmcp.h
@@ -0,0 +1,311 @@
+/* Copyright 2013-2015 Freescale Semiconductor Inc.
+ *
+ * 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 of the above-listed copyright holders nor the
+ * names of any contributors may be used to endorse or promote products
+ * derived from this software without specific prior written permission.
+ *
+ *
+ * ALTERNATIVELY, this software may be distributed under the terms of the
+ * GNU General Public License ("GPL") as published by the Free Software
+ * Foundation, either version 2 of that License or (at your option) any
+ * later version.
+ *
+ * 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 HOLDERS OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+#ifndef __FSL_DPMCP_H
+#define __FSL_DPMCP_H
+
+/* Data Path Management Command Portal API
+ * Contains initialization APIs and runtime control APIs for DPMCP
+ */
+
+struct fsl_mc_io;
+
+/**
+ * dpmcp_open() - Open a control session for the specified object.
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @dpmcp_id:	DPMCP unique ID
+ * @token:	Returned token; use in subsequent API calls
+ *
+ * This function can be used to open a control session for an
+ * already created object; an object may have been declared in
+ * the DPL or by calling the dpmcp_create function.
+ * This function returns a unique authentication token,
+ * associated with the specific object ID and the specific MC
+ * portal; this token must be used in all subsequent commands for
+ * this specific object
+ *
+ * Return:	'0' on Success; Error code otherwise.
+ */
+int dpmcp_open(struct fsl_mc_io *mc_io, int dpmcp_id, uint16_t *token);
+
+/* Get portal ID from pool */
+#define DPMCP_GET_PORTAL_ID_FROM_POOL (-1)
+
+/**
+ * dpmcp_close() - Close the control session of the object
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @token:	Token of DPMCP object
+ *
+ * After this function is called, no further operations are
+ * allowed on the object without opening a new control session.
+ *
+ * Return:	'0' on Success; Error code otherwise.
+ */
+int dpmcp_close(struct fsl_mc_io *mc_io, uint16_t token);
+
+/**
+ * struct dpmcp_cfg() - Structure representing DPMCP configuration
+ * @portal_id:	Portal ID; 'DPMCP_GET_PORTAL_ID_FROM_POOL' to get the portal ID
+ *		from pool
+ */
+struct dpmcp_cfg {
+	int portal_id;
+};
+
+/**
+ * dpmcp_create() - Create the DPMCP object.
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @cfg:	Configuration structure
+ * @token:	Returned token; use in subsequent API calls
+ *
+ * Create the DPMCP object, allocate required resources and
+ * perform required initialization.
+ *
+ * The object can be created either by declaring it in the
+ * DPL file, or by calling this function.
+ * This function returns a unique authentication token,
+ * associated with the specific object ID and the specific MC
+ * portal; this token must be used in all subsequent calls to
+ * this specific object. For objects that are created using the
+ * DPL file, call dpmcp_open function to get an authentication
+ * token first.
+ *
+ * Return:	'0' on Success; Error code otherwise.
+ */
+int dpmcp_create(struct fsl_mc_io	*mc_io,
+		 const struct dpmcp_cfg	*cfg,
+		uint16_t		*token);
+
+/**
+ * dpmcp_destroy() - Destroy the DPMCP object and release all its resources.
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @token:	Token of DPMCP object
+ *
+ * Return:	'0' on Success; error code otherwise.
+ */
+int dpmcp_destroy(struct fsl_mc_io *mc_io, uint16_t token);
+
+/**
+ * dpmcp_reset() - Reset the DPMCP, returns the object to initial state.
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @token:	Token of DPMCP object
+ *
+ * Return:	'0' on Success; Error code otherwise.
+ */
+int dpmcp_reset(struct fsl_mc_io *mc_io, uint16_t token);
+
+/* IRQ */
+/*!
+ * @name dpmcp IRQ Index and Events
+ */
+#define DPMCP_IRQ_INDEX                             0
+/*!< Irq index */
+#define DPMCP_IRQ_EVENT_CMD_DONE                    0x00000001
+/*!< irq event - Indicates that the link state changed */
+/* @} */
+
+/**
+ * dpmcp_set_irq() - Set IRQ information for the DPMCP to trigger an interrupt.
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @token:	Token of DPMCP object
+ * @irq_index:	Identifies the interrupt index to configure
+ * @irq_addr:	Address that must be written to
+ *				signal a message-based interrupt
+ * @irq_val:	Value to write into irq_addr address
+ * @user_irq_id: A user defined number associated with this IRQ
+ *
+ * Return:	'0' on Success; Error code otherwise.
+ */
+int dpmcp_set_irq(struct fsl_mc_io	*mc_io,
+		  uint16_t		token,
+		 uint8_t		irq_index,
+		 uint64_t		irq_addr,
+		 uint32_t		irq_val,
+		 int			user_irq_id);
+
+/**
+ * dpmcp_get_irq() - Get IRQ information from the DPMCP.
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @token:	Token of DPMCP object
+ * @irq_index:	The interrupt index to configure
+ * @type:	Interrupt type: 0 represents message interrupt
+ *				type (both irq_addr and irq_val are valid)
+ * @irq_addr:	Returned address that must be written to
+ *				signal the message-based interrupt
+ * @irq_val:	Value to write into irq_addr address
+ * @user_irq_id: A user defined number associated with this IRQ
+ *
+ * Return:	'0' on Success; Error code otherwise.
+ */
+int dpmcp_get_irq(struct fsl_mc_io	*mc_io,
+		  uint16_t		token,
+		 uint8_t		irq_index,
+		 int			*type,
+		 uint64_t		*irq_addr,
+		 uint32_t		*irq_val,
+		 int			*user_irq_id);
+
+/**
+ * dpmcp_set_irq_enable() - Set overall interrupt state.
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @token:	Token of DPMCP object
+ * @irq_index:	The interrupt index to configure
+ * @en:	Interrupt state - enable = 1, disable = 0
+ *
+ * Allows GPP software to control when interrupts are generated.
+ * Each interrupt can have up to 32 causes.  The enable/disable control's the
+ * overall interrupt state. if the interrupt is disabled no causes will cause
+ * an interrupt.
+ *
+ * Return:	'0' on Success; Error code otherwise.
+ */
+int dpmcp_set_irq_enable(struct fsl_mc_io	*mc_io,
+			 uint16_t		token,
+			uint8_t			irq_index,
+			uint8_t			en);
+
+/**
+ * dpmcp_get_irq_enable() - Get overall interrupt state
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @token:	Token of DPMCP object
+ * @irq_index:	The interrupt index to configure
+ * @en:		Returned interrupt state - enable = 1, disable = 0
+ *
+ * Return:	'0' on Success; Error code otherwise.
+ */
+int dpmcp_get_irq_enable(struct fsl_mc_io	*mc_io,
+			 uint16_t		token,
+			uint8_t			irq_index,
+			uint8_t			*en);
+
+/**
+ * dpmcp_set_irq_mask() - Set interrupt mask.
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @token:	Token of DPMCP object
+ * @irq_index:	The interrupt index to configure
+ * @mask:	Event mask to trigger interrupt;
+ *			each bit:
+ *				0 = ignore event
+ *				1 = consider event for asserting IRQ
+ *
+ * Every interrupt can have up to 32 causes and the interrupt model supports
+ * masking/unmasking each cause independently
+ *
+ * Return:	'0' on Success; Error code otherwise.
+ */
+int dpmcp_set_irq_mask(struct fsl_mc_io	*mc_io,
+		       uint16_t		token,
+		      uint8_t		irq_index,
+		      uint32_t		mask);
+
+/**
+ * dpmcp_get_irq_mask() - Get interrupt mask.
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @token:	Token of DPMCP object
+ * @irq_index:	The interrupt index to configure
+ * @mask:	Returned event mask to trigger interrupt
+ *
+ * Every interrupt can have up to 32 causes and the interrupt model supports
+ * masking/unmasking each cause independently
+ *
+ * Return:	'0' on Success; Error code otherwise.
+ */
+int dpmcp_get_irq_mask(struct fsl_mc_io	*mc_io,
+		       uint16_t		token,
+		      uint8_t		irq_index,
+		      uint32_t		*mask);
+
+/**
+ * dpmcp_get_irq_status() - Get the current status of any pending interrupts.
+ *
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @token:	Token of DPMCP object
+ * @irq_index:	The interrupt index to configure
+ * @status:	Returned interrupts status - one bit per cause:
+ *			0 = no interrupt pending
+ *			1 = interrupt pending
+ *
+ * Return:	'0' on Success; Error code otherwise.
+ */
+int dpmcp_get_irq_status(struct fsl_mc_io	*mc_io,
+			 uint16_t		token,
+			uint8_t			irq_index,
+			uint32_t		*status);
+
+/**
+ * dpmcp_clear_irq_status() - Clear a pending interrupt's status
+ *
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @token:	Token of DPMCP object
+ * @irq_index:	The interrupt index to configure
+ * @status:	Bits to clear (W1C) - one bit per cause:
+ *					0 = don't change
+ *					1 = clear status bit
+ *
+ * Return:	'0' on Success; Error code otherwise.
+ */
+int dpmcp_clear_irq_status(struct fsl_mc_io	*mc_io,
+			   uint16_t		token,
+			  uint8_t		irq_index,
+			  uint32_t		status);
+
+/**
+ * struct dpmcp_attr - Structure representing DPMCP attributes
+ * @id:		DPMCP object ID
+ * @version:	DPMCP version
+ */
+struct dpmcp_attr {
+	int id;
+	/**
+	 * struct version - Structure representing DPMCP version
+	 * @major:	DPMCP major version
+	 * @minor:	DPMCP minor version
+	 */
+	struct {
+		uint16_t major;
+		uint16_t minor;
+	} version;
+};
+
+/**
+ * dpmcp_get_attributes - Retrieve DPMCP attributes.
+ *
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @token:	Token of DPMCP object
+ * @attr:	Returned object's attributes
+ *
+ * Return:	'0' on Success; Error code otherwise.
+ */
+int dpmcp_get_attributes(struct fsl_mc_io	*mc_io,
+			 uint16_t		token,
+			struct dpmcp_attr	*attr);
+
+#endif /* __FSL_DPMCP_H */
diff --git a/drivers/staging/fsl-mc/bus/dpmng-cmd.h b/drivers/staging/fsl-mc/bus/dpmng-cmd.h
new file mode 100644
index 000000000000..ba8cfa9635dd
--- /dev/null
+++ b/drivers/staging/fsl-mc/bus/dpmng-cmd.h
@@ -0,0 +1,47 @@
+/* Copyright 2013-2014 Freescale Semiconductor Inc.
+ *
+ * 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 of the above-listed copyright holders nor the
+ *       names of any contributors may be used to endorse or promote products
+ *       derived from this software without specific prior written permission.
+ *
+ *
+ * ALTERNATIVELY, this software may be distributed under the terms of the
+ * GNU General Public License ("GPL") as published by the Free Software
+ * Foundation, either version 2 of that License or (at your option) any
+ * later version.
+ *
+ * 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 HOLDERS 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.
+ */
+
+/*************************************************************************//*
+ dpmng-cmd.h
+
+ defines portal commands
+
+ *//**************************************************************************/
+
+#ifndef __FSL_DPMNG_CMD_H
+#define __FSL_DPMNG_CMD_H
+
+/* Command IDs */
+#define DPMNG_CMDID_GET_CONT_ID			0x830
+#define DPMNG_CMDID_GET_VERSION			0x831
+
+#endif /* __FSL_DPMNG_CMD_H */
diff --git a/drivers/staging/fsl-mc/bus/dpmng.c b/drivers/staging/fsl-mc/bus/dpmng.c
new file mode 100644
index 000000000000..58328e8118e9
--- /dev/null
+++ b/drivers/staging/fsl-mc/bus/dpmng.c
@@ -0,0 +1,78 @@
+/* Copyright 2013-2014 Freescale Semiconductor Inc.
+*
+* 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 of the above-listed copyright holders nor the
+* names of any contributors may be used to endorse or promote products
+* derived from this software without specific prior written permission.
+*
+*
+* ALTERNATIVELY, this software may be distributed under the terms of the
+* GNU General Public License ("GPL") as published by the Free Software
+* Foundation, either version 2 of that License or (at your option) any
+* later version.
+*
+* 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 HOLDERS 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 "../include/mc-sys.h"
+#include "../include/mc-cmd.h"
+#include "../include/dpmng.h"
+#include "dpmng-cmd.h"
+
+int mc_get_version(struct fsl_mc_io *mc_io, struct mc_version *mc_ver_info)
+{
+	struct mc_command cmd = { 0 };
+	int err;
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPMNG_CMDID_GET_VERSION,
+					  MC_CMD_PRI_LOW, 0);
+
+	/* send command to mc*/
+	err = mc_send_command(mc_io, &cmd);
+	if (err)
+		return err;
+
+	/* retrieve response parameters */
+	mc_ver_info->revision = mc_dec(cmd.params[0], 0, 32);
+	mc_ver_info->major = mc_dec(cmd.params[0], 32, 32);
+	mc_ver_info->minor = mc_dec(cmd.params[1], 0, 32);
+
+	return 0;
+}
+
+int dpmng_get_container_id(struct fsl_mc_io *mc_io, int *container_id)
+{
+	struct mc_command cmd = { 0 };
+	int err;
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPMNG_CMDID_GET_CONT_ID,
+					  MC_CMD_PRI_LOW, 0);
+
+	/* send command to mc*/
+	err = mc_send_command(mc_io, &cmd);
+	if (err)
+		return err;
+
+	/* retrieve response parameters */
+	*container_id = mc_dec(cmd.params[0], 0, 32);
+
+	return 0;
+}
+
diff --git a/drivers/staging/fsl-mc/bus/dprc-cmd.h b/drivers/staging/fsl-mc/bus/dprc-cmd.h
new file mode 100644
index 000000000000..09202489c2b2
--- /dev/null
+++ b/drivers/staging/fsl-mc/bus/dprc-cmd.h
@@ -0,0 +1,84 @@
+/* Copyright 2013-2014 Freescale Semiconductor Inc.
+ *
+ * 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 of the above-listed copyright holders nor the
+ *       names of any contributors may be used to endorse or promote products
+ *       derived from this software without specific prior written permission.
+ *
+ *
+ * ALTERNATIVELY, this software may be distributed under the terms of the
+ * GNU General Public License ("GPL") as published by the Free Software
+ * Foundation, either version 2 of that License or (at your option) any
+ * later version.
+ *
+ * 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 HOLDERS 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.
+ */
+
+/*************************************************************************//*
+ dprc-cmd.h
+
+ defines dprc portal commands
+
+ *//**************************************************************************/
+
+#ifndef _FSL_DPRC_CMD_H
+#define _FSL_DPRC_CMD_H
+
+/* DPRC Version */
+#define DPRC_VER_MAJOR				3
+#define DPRC_VER_MINOR				0
+
+/* Command IDs */
+#define DPRC_CMDID_CLOSE			0x800
+#define DPRC_CMDID_OPEN				0x805
+#define DPRC_CMDID_CREATE			0x905
+
+#define DPRC_CMDID_GET_ATTR			0x004
+#define DPRC_CMDID_RESET_CONT			0x005
+
+#define DPRC_CMDID_SET_IRQ			0x010
+#define DPRC_CMDID_GET_IRQ			0x011
+#define DPRC_CMDID_SET_IRQ_ENABLE		0x012
+#define DPRC_CMDID_GET_IRQ_ENABLE		0x013
+#define DPRC_CMDID_SET_IRQ_MASK			0x014
+#define DPRC_CMDID_GET_IRQ_MASK			0x015
+#define DPRC_CMDID_GET_IRQ_STATUS		0x016
+#define DPRC_CMDID_CLEAR_IRQ_STATUS		0x017
+
+#define DPRC_CMDID_CREATE_CONT			0x151
+#define DPRC_CMDID_DESTROY_CONT			0x152
+#define DPRC_CMDID_SET_RES_QUOTA		0x155
+#define DPRC_CMDID_GET_RES_QUOTA		0x156
+#define DPRC_CMDID_ASSIGN			0x157
+#define DPRC_CMDID_UNASSIGN			0x158
+#define DPRC_CMDID_GET_OBJ_COUNT		0x159
+#define DPRC_CMDID_GET_OBJ			0x15A
+#define DPRC_CMDID_GET_RES_COUNT		0x15B
+#define DPRC_CMDID_GET_RES_IDS			0x15C
+#define DPRC_CMDID_GET_OBJ_REG			0x15E
+
+#define DPRC_CMDID_CONNECT			0x167
+#define DPRC_CMDID_DISCONNECT			0x168
+#define DPRC_CMDID_GET_POOL			0x169
+#define DPRC_CMDID_GET_POOL_COUNT		0x16A
+#define DPRC_CMDID_GET_PORTAL_PADDR		0x16B
+
+#define DPRC_CMDID_GET_CONNECTION		0x16C
+
+#endif /* _FSL_DPRC_CMD_H */
diff --git a/drivers/staging/fsl-mc/bus/dprc-driver.c b/drivers/staging/fsl-mc/bus/dprc-driver.c
new file mode 100644
index 000000000000..35c06cff1a06
--- /dev/null
+++ b/drivers/staging/fsl-mc/bus/dprc-driver.c
@@ -0,0 +1,486 @@
+/*
+ * Freescale data path resource container (DPRC) driver
+ *
+ * Copyright (C) 2014 Freescale Semiconductor, Inc.
+ * Author: German Rivera <German.Rivera@freescale.com>
+ *
+ * This file is licensed under the terms of the GNU General Public
+ * License version 2. This program is licensed "as is" without any
+ * warranty of any kind, whether express or implied.
+ */
+
+#include "../include/mc-private.h"
+#include "../include/mc-sys.h"
+#include <linux/module.h>
+#include <linux/slab.h>
+#include "dprc-cmd.h"
+
+struct dprc_child_objs {
+	int child_count;
+	struct dprc_obj_desc *child_array;
+};
+
+static int __fsl_mc_device_remove_if_not_in_mc(struct device *dev, void *data)
+{
+	int i;
+	struct dprc_child_objs *objs;
+	struct fsl_mc_device *mc_dev;
+
+	WARN_ON(!dev);
+	WARN_ON(!data);
+	mc_dev = to_fsl_mc_device(dev);
+	objs = data;
+
+	for (i = 0; i < objs->child_count; i++) {
+		struct dprc_obj_desc *obj_desc = &objs->child_array[i];
+
+		if (strlen(obj_desc->type) != 0 &&
+		    FSL_MC_DEVICE_MATCH(mc_dev, obj_desc))
+			break;
+	}
+
+	if (i == objs->child_count)
+		fsl_mc_device_remove(mc_dev);
+
+	return 0;
+}
+
+static int __fsl_mc_device_remove(struct device *dev, void *data)
+{
+	WARN_ON(!dev);
+	WARN_ON(data);
+	fsl_mc_device_remove(to_fsl_mc_device(dev));
+	return 0;
+}
+
+/**
+ * dprc_remove_devices - Removes devices for objects removed from a DPRC
+ *
+ * @mc_bus_dev: pointer to the fsl-mc device that represents a DPRC object
+ * @obj_desc_array: array of object descriptors for child objects currently
+ * present in the DPRC in the MC.
+ * @num_child_objects_in_mc: number of entries in obj_desc_array
+ *
+ * Synchronizes the state of the Linux bus driver with the actual state of
+ * the MC by removing devices that represent MC objects that have
+ * been dynamically removed in the physical DPRC.
+ */
+static void dprc_remove_devices(struct fsl_mc_device *mc_bus_dev,
+				struct dprc_obj_desc *obj_desc_array,
+				int num_child_objects_in_mc)
+{
+	if (num_child_objects_in_mc != 0) {
+		/*
+		 * Remove child objects that are in the DPRC in Linux,
+		 * but not in the MC:
+		 */
+		struct dprc_child_objs objs;
+
+		objs.child_count = num_child_objects_in_mc;
+		objs.child_array = obj_desc_array;
+		device_for_each_child(&mc_bus_dev->dev, &objs,
+				      __fsl_mc_device_remove_if_not_in_mc);
+	} else {
+		/*
+		 * There are no child objects for this DPRC in the MC.
+		 * So, remove all the child devices from Linux:
+		 */
+		device_for_each_child(&mc_bus_dev->dev, NULL,
+				      __fsl_mc_device_remove);
+	}
+}
+
+static int __fsl_mc_device_match(struct device *dev, void *data)
+{
+	struct dprc_obj_desc *obj_desc = data;
+	struct fsl_mc_device *mc_dev = to_fsl_mc_device(dev);
+
+	return FSL_MC_DEVICE_MATCH(mc_dev, obj_desc);
+}
+
+static struct fsl_mc_device *fsl_mc_device_lookup(struct dprc_obj_desc
+								*obj_desc,
+						  struct fsl_mc_device
+								*mc_bus_dev)
+{
+	struct device *dev;
+
+	dev = device_find_child(&mc_bus_dev->dev, obj_desc,
+				__fsl_mc_device_match);
+
+	return dev ? to_fsl_mc_device(dev) : NULL;
+}
+
+/**
+ * check_plugged_state_change - Check change in an MC object's plugged state
+ *
+ * @mc_dev: pointer to the fsl-mc device for a given MC object
+ * @obj_desc: pointer to the MC object's descriptor in the MC
+ *
+ * If the plugged state has changed from unplugged to plugged, the fsl-mc
+ * device is bound to the corresponding device driver.
+ * If the plugged state has changed from plugged to unplugged, the fsl-mc
+ * device is unbound from the corresponding device driver.
+ */
+static void check_plugged_state_change(struct fsl_mc_device *mc_dev,
+				       struct dprc_obj_desc *obj_desc)
+{
+	int error;
+	uint32_t plugged_flag_at_mc =
+			(obj_desc->state & DPRC_OBJ_STATE_PLUGGED);
+
+	if (plugged_flag_at_mc !=
+	    (mc_dev->obj_desc.state & DPRC_OBJ_STATE_PLUGGED)) {
+		if (plugged_flag_at_mc) {
+			mc_dev->obj_desc.state |= DPRC_OBJ_STATE_PLUGGED;
+			error = device_attach(&mc_dev->dev);
+			if (error < 0) {
+				dev_err(&mc_dev->dev,
+					"device_attach() failed: %d\n",
+					error);
+			}
+		} else {
+			mc_dev->obj_desc.state &= ~DPRC_OBJ_STATE_PLUGGED;
+			device_release_driver(&mc_dev->dev);
+		}
+	}
+}
+
+/**
+ * dprc_add_new_devices - Adds devices to the logical bus for a DPRC
+ *
+ * @mc_bus_dev: pointer to the fsl-mc device that represents a DPRC object
+ * @obj_desc_array: array of device descriptors for child devices currently
+ * present in the physical DPRC.
+ * @num_child_objects_in_mc: number of entries in obj_desc_array
+ *
+ * Synchronizes the state of the Linux bus driver with the actual
+ * state of the MC by adding objects that have been newly discovered
+ * in the physical DPRC.
+ */
+static void dprc_add_new_devices(struct fsl_mc_device *mc_bus_dev,
+				 struct dprc_obj_desc *obj_desc_array,
+				 int num_child_objects_in_mc)
+{
+	int error;
+	int i;
+
+	for (i = 0; i < num_child_objects_in_mc; i++) {
+		struct fsl_mc_device *child_dev;
+		struct dprc_obj_desc *obj_desc = &obj_desc_array[i];
+
+		if (strlen(obj_desc->type) == 0)
+			continue;
+
+		/*
+		 * Check if device is already known to Linux:
+		 */
+		child_dev = fsl_mc_device_lookup(obj_desc, mc_bus_dev);
+		if (child_dev) {
+			check_plugged_state_change(child_dev, obj_desc);
+			continue;
+		}
+
+		error = fsl_mc_device_add(obj_desc, NULL, &mc_bus_dev->dev,
+					  &child_dev);
+		if (error < 0)
+			continue;
+	}
+}
+
+static void dprc_init_all_resource_pools(struct fsl_mc_device *mc_bus_dev)
+{
+	int pool_type;
+	struct fsl_mc_bus *mc_bus = to_fsl_mc_bus(mc_bus_dev);
+
+	for (pool_type = 0; pool_type < FSL_MC_NUM_POOL_TYPES; pool_type++) {
+		struct fsl_mc_resource_pool *res_pool =
+		    &mc_bus->resource_pools[pool_type];
+
+		res_pool->type = pool_type;
+		res_pool->max_count = 0;
+		res_pool->free_count = 0;
+		res_pool->mc_bus = mc_bus;
+		INIT_LIST_HEAD(&res_pool->free_list);
+		mutex_init(&res_pool->mutex);
+	}
+}
+
+static void dprc_cleanup_resource_pool(struct fsl_mc_device *mc_bus_dev,
+				       enum fsl_mc_pool_type pool_type)
+{
+	struct fsl_mc_resource *resource;
+	struct fsl_mc_resource *next;
+	struct fsl_mc_bus *mc_bus = to_fsl_mc_bus(mc_bus_dev);
+	struct fsl_mc_resource_pool *res_pool =
+					&mc_bus->resource_pools[pool_type];
+	int free_count = 0;
+
+	WARN_ON(res_pool->type != pool_type);
+	WARN_ON(res_pool->free_count != res_pool->max_count);
+
+	list_for_each_entry_safe(resource, next, &res_pool->free_list, node) {
+		free_count++;
+		WARN_ON(resource->type != res_pool->type);
+		WARN_ON(resource->parent_pool != res_pool);
+		devm_kfree(&mc_bus_dev->dev, resource);
+	}
+
+	WARN_ON(free_count != res_pool->free_count);
+}
+
+static void dprc_cleanup_all_resource_pools(struct fsl_mc_device *mc_bus_dev)
+{
+	int pool_type;
+
+	for (pool_type = 0; pool_type < FSL_MC_NUM_POOL_TYPES; pool_type++)
+		dprc_cleanup_resource_pool(mc_bus_dev, pool_type);
+}
+
+/**
+ * dprc_scan_objects - Discover objects in a DPRC
+ *
+ * @mc_bus_dev: pointer to the fsl-mc device that represents a DPRC object
+ *
+ * Detects objects added and removed from a DPRC and synchronizes the
+ * state of the Linux bus driver, MC by adding and removing
+ * devices accordingly.
+ * Two types of devices can be found in a DPRC: allocatable objects (e.g.,
+ * dpbp, dpmcp) and non-allocatable devices (e.g., dprc, dpni).
+ * All allocatable devices needed to be probed before all non-allocatable
+ * devices, to ensure that device drivers for non-allocatable
+ * devices can allocate any type of allocatable devices.
+ * That is, we need to ensure that the corresponding resource pools are
+ * populated before they can get allocation requests from probe callbacks
+ * of the device drivers for the non-allocatable devices.
+ */
+int dprc_scan_objects(struct fsl_mc_device *mc_bus_dev)
+{
+	int num_child_objects;
+	int dprc_get_obj_failures;
+	int error;
+	struct dprc_obj_desc *child_obj_desc_array = NULL;
+
+	error = dprc_get_obj_count(mc_bus_dev->mc_io,
+				   mc_bus_dev->mc_handle,
+				   &num_child_objects);
+	if (error < 0) {
+		dev_err(&mc_bus_dev->dev, "dprc_get_obj_count() failed: %d\n",
+			error);
+		return error;
+	}
+
+	if (num_child_objects != 0) {
+		int i;
+
+		child_obj_desc_array =
+		    devm_kmalloc_array(&mc_bus_dev->dev, num_child_objects,
+				       sizeof(*child_obj_desc_array),
+				       GFP_KERNEL);
+		if (!child_obj_desc_array)
+			return -ENOMEM;
+
+		/*
+		 * Discover objects currently present in the physical DPRC:
+		 */
+		dprc_get_obj_failures = 0;
+		for (i = 0; i < num_child_objects; i++) {
+			struct dprc_obj_desc *obj_desc =
+			    &child_obj_desc_array[i];
+
+			error = dprc_get_obj(mc_bus_dev->mc_io,
+					     mc_bus_dev->mc_handle,
+					     i, obj_desc);
+			if (error < 0) {
+				dev_err(&mc_bus_dev->dev,
+					"dprc_get_obj(i=%d) failed: %d\n",
+					i, error);
+				/*
+				 * Mark the obj entry as "invalid", by using the
+				 * empty string as obj type:
+				 */
+				obj_desc->type[0] = '\0';
+				obj_desc->id = error;
+				dprc_get_obj_failures++;
+				continue;
+			}
+
+			dev_dbg(&mc_bus_dev->dev,
+				"Discovered object: type %s, id %d\n",
+				obj_desc->type, obj_desc->id);
+		}
+
+		if (dprc_get_obj_failures != 0) {
+			dev_err(&mc_bus_dev->dev,
+				"%d out of %d devices could not be retrieved\n",
+				dprc_get_obj_failures, num_child_objects);
+		}
+	}
+
+	dprc_remove_devices(mc_bus_dev, child_obj_desc_array,
+			    num_child_objects);
+
+	dprc_add_new_devices(mc_bus_dev, child_obj_desc_array,
+			     num_child_objects);
+
+	if (child_obj_desc_array)
+		devm_kfree(&mc_bus_dev->dev, child_obj_desc_array);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(dprc_scan_objects);
+
+/**
+ * dprc_scan_container - Scans a physical DPRC and synchronizes Linux bus state
+ *
+ * @mc_bus_dev: pointer to the fsl-mc device that represents a DPRC object
+ *
+ * Scans the physical DPRC and synchronizes the state of the Linux
+ * bus driver with the actual state of the MC by adding and removing
+ * devices as appropriate.
+ */
+int dprc_scan_container(struct fsl_mc_device *mc_bus_dev)
+{
+	int error;
+	struct fsl_mc_bus *mc_bus = to_fsl_mc_bus(mc_bus_dev);
+
+	dprc_init_all_resource_pools(mc_bus_dev);
+
+	/*
+	 * Discover objects in the DPRC:
+	 */
+	mutex_lock(&mc_bus->scan_mutex);
+	error = dprc_scan_objects(mc_bus_dev);
+	mutex_unlock(&mc_bus->scan_mutex);
+	if (error < 0)
+		goto error;
+
+	return 0;
+error:
+	dprc_cleanup_all_resource_pools(mc_bus_dev);
+	return error;
+}
+EXPORT_SYMBOL_GPL(dprc_scan_container);
+
+/**
+ * dprc_probe - callback invoked when a DPRC is being bound to this driver
+ *
+ * @mc_dev: Pointer to fsl-mc device representing a DPRC
+ *
+ * It opens the physical DPRC in the MC.
+ * It scans the DPRC to discover the MC objects contained in it.
+ * It creates the interrupt pool for the MC bus associated with the DPRC.
+ * It configures the interrupts for the DPRC device itself.
+ */
+static int dprc_probe(struct fsl_mc_device *mc_dev)
+{
+	int error;
+	size_t region_size;
+	struct fsl_mc_bus *mc_bus = to_fsl_mc_bus(mc_dev);
+
+	if (WARN_ON(strcmp(mc_dev->obj_desc.type, "dprc") != 0))
+		return -EINVAL;
+
+	if (!mc_dev->mc_io) {
+		/*
+		 * This is a child DPRC:
+		 */
+		if (WARN_ON(mc_dev->obj_desc.region_count == 0))
+			return -EINVAL;
+
+		region_size = mc_dev->regions[0].end -
+			      mc_dev->regions[0].start + 1;
+
+		error = fsl_create_mc_io(&mc_dev->dev,
+					 mc_dev->regions[0].start,
+					 region_size,
+					 NULL, 0, &mc_dev->mc_io);
+		if (error < 0)
+			return error;
+	}
+
+	error = dprc_open(mc_dev->mc_io, mc_dev->obj_desc.id,
+			  &mc_dev->mc_handle);
+	if (error < 0) {
+		dev_err(&mc_dev->dev, "dprc_open() failed: %d\n", error);
+		goto error_cleanup_mc_io;
+	}
+
+	mutex_init(&mc_bus->scan_mutex);
+
+	/*
+	 * Discover MC objects in DPRC object:
+	 */
+	error = dprc_scan_container(mc_dev);
+	if (error < 0)
+		goto error_cleanup_open;
+
+	dev_info(&mc_dev->dev, "DPRC device bound to driver");
+	return 0;
+
+error_cleanup_open:
+	(void)dprc_close(mc_dev->mc_io, mc_dev->mc_handle);
+
+error_cleanup_mc_io:
+	fsl_destroy_mc_io(mc_dev->mc_io);
+	return error;
+}
+
+/**
+ * dprc_remove - callback invoked when a DPRC is being unbound from this driver
+ *
+ * @mc_dev: Pointer to fsl-mc device representing the DPRC
+ *
+ * It removes the DPRC's child objects from Linux (not from the MC) and
+ * closes the DPRC device in the MC.
+ * It tears down the interrupts that were configured for the DPRC device.
+ * It destroys the interrupt pool associated with this MC bus.
+ */
+static int dprc_remove(struct fsl_mc_device *mc_dev)
+{
+	int error;
+
+	if (WARN_ON(strcmp(mc_dev->obj_desc.type, "dprc") != 0))
+		return -EINVAL;
+	if (WARN_ON(!mc_dev->mc_io))
+		return -EINVAL;
+
+	device_for_each_child(&mc_dev->dev, NULL, __fsl_mc_device_remove);
+	dprc_cleanup_all_resource_pools(mc_dev);
+	error = dprc_close(mc_dev->mc_io, mc_dev->mc_handle);
+	if (error < 0)
+		dev_err(&mc_dev->dev, "dprc_close() failed: %d\n", error);
+
+	dev_info(&mc_dev->dev, "DPRC device unbound from driver");
+	return 0;
+}
+
+static const struct fsl_mc_device_match_id match_id_table[] = {
+	{
+	 .vendor = FSL_MC_VENDOR_FREESCALE,
+	 .obj_type = "dprc",
+	 .ver_major = DPRC_VER_MAJOR,
+	 .ver_minor = DPRC_VER_MINOR},
+	{.vendor = 0x0},
+};
+
+static struct fsl_mc_driver dprc_driver = {
+	.driver = {
+		   .name = FSL_MC_DPRC_DRIVER_NAME,
+		   .owner = THIS_MODULE,
+		   .pm = NULL,
+		   },
+	.match_id_table = match_id_table,
+	.probe = dprc_probe,
+	.remove = dprc_remove,
+};
+
+int __init dprc_driver_init(void)
+{
+	return fsl_mc_driver_register(&dprc_driver);
+}
+
+void __exit dprc_driver_exit(void)
+{
+	fsl_mc_driver_unregister(&dprc_driver);
+}
diff --git a/drivers/staging/fsl-mc/bus/dprc.c b/drivers/staging/fsl-mc/bus/dprc.c
new file mode 100644
index 000000000000..19b26e630b62
--- /dev/null
+++ b/drivers/staging/fsl-mc/bus/dprc.c
@@ -0,0 +1,913 @@
+/* Copyright 2013-2014 Freescale Semiconductor Inc.
+*
+* 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 of the above-listed copyright holders nor the
+* names of any contributors may be used to endorse or promote products
+* derived from this software without specific prior written permission.
+*
+*
+* ALTERNATIVELY, this software may be distributed under the terms of the
+* GNU General Public License ("GPL") as published by the Free Software
+* Foundation, either version 2 of that License or (at your option) any
+* later version.
+*
+* 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 HOLDERS 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 "../include/mc-sys.h"
+#include "../include/mc-cmd.h"
+#include "../include/dprc.h"
+#include "dprc-cmd.h"
+
+int dprc_open(struct fsl_mc_io *mc_io, int container_id, uint16_t *token)
+{
+	struct mc_command cmd = { 0 };
+	int err;
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPRC_CMDID_OPEN, MC_CMD_PRI_LOW,
+					  0);
+	cmd.params[0] |= mc_enc(0, 32, container_id);
+
+	/* send command to mc*/
+	err = mc_send_command(mc_io, &cmd);
+	if (err)
+		return err;
+
+	/* retrieve response parameters */
+	*token = MC_CMD_HDR_READ_TOKEN(cmd.header);
+
+	return 0;
+}
+EXPORT_SYMBOL(dprc_open);
+
+int dprc_close(struct fsl_mc_io *mc_io, uint16_t token)
+{
+	struct mc_command cmd = { 0 };
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPRC_CMDID_CLOSE, MC_CMD_PRI_HIGH,
+					  token);
+
+	/* send command to mc*/
+	return mc_send_command(mc_io, &cmd);
+}
+EXPORT_SYMBOL(dprc_close);
+
+int dprc_create_container(struct fsl_mc_io *mc_io,
+			  uint16_t token,
+			  struct dprc_cfg *cfg,
+			  int *child_container_id,
+			  uint64_t *child_portal_paddr)
+{
+	struct mc_command cmd = { 0 };
+	int err;
+
+	/* prepare command */
+	cmd.params[0] |= mc_enc(32, 16, cfg->icid);
+	cmd.params[0] |= mc_enc(0, 32, cfg->options);
+	cmd.params[1] |= mc_enc(32, 32, cfg->portal_id);
+
+	cmd.header = mc_encode_cmd_header(DPRC_CMDID_CREATE_CONT,
+					  MC_CMD_PRI_LOW, token);
+
+	/* send command to mc*/
+	err = mc_send_command(mc_io, &cmd);
+	if (err)
+		return err;
+
+	/* retrieve response parameters */
+	*child_container_id = mc_dec(cmd.params[1], 0, 32);
+	*child_portal_paddr = mc_dec(cmd.params[2], 0, 64);
+
+	return 0;
+}
+
+int dprc_destroy_container(struct fsl_mc_io *mc_io,
+			   uint16_t token,
+			   int child_container_id)
+{
+	struct mc_command cmd = { 0 };
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPRC_CMDID_DESTROY_CONT,
+					  MC_CMD_PRI_LOW, token);
+	cmd.params[0] |= mc_enc(0, 32, child_container_id);
+
+	/* send command to mc*/
+	return mc_send_command(mc_io, &cmd);
+}
+
+int dprc_reset_container(struct fsl_mc_io *mc_io,
+			 uint16_t token,
+			 int child_container_id)
+{
+	struct mc_command cmd = { 0 };
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPRC_CMDID_RESET_CONT,
+					  MC_CMD_PRI_LOW, token);
+	cmd.params[0] |= mc_enc(0, 32, child_container_id);
+
+	/* send command to mc*/
+	return mc_send_command(mc_io, &cmd);
+}
+
+int dprc_get_irq(struct fsl_mc_io *mc_io,
+		 uint16_t token,
+		 uint8_t irq_index,
+		 int *type,
+		 uint64_t *irq_paddr,
+		 uint32_t *irq_val,
+		 int *user_irq_id)
+{
+	struct mc_command cmd = { 0 };
+	int err;
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPRC_CMDID_GET_IRQ,
+					  MC_CMD_PRI_LOW,
+					  token);
+	cmd.params[0] |= mc_enc(32, 8, irq_index);
+
+	/* send command to mc*/
+	err = mc_send_command(mc_io, &cmd);
+	if (err)
+		return err;
+
+	/* retrieve response parameters */
+	*irq_val = mc_dec(cmd.params[0], 0, 32);
+	*irq_paddr = mc_dec(cmd.params[1], 0, 64);
+	*user_irq_id = mc_dec(cmd.params[2], 0, 32);
+	*type = mc_dec(cmd.params[2], 32, 32);
+
+	return 0;
+}
+
+int dprc_set_irq(struct fsl_mc_io *mc_io,
+		 uint16_t token,
+		 uint8_t irq_index,
+		 uint64_t irq_paddr,
+		 uint32_t irq_val,
+		 int user_irq_id)
+{
+	struct mc_command cmd = { 0 };
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPRC_CMDID_SET_IRQ,
+					  MC_CMD_PRI_LOW,
+					  token);
+	cmd.params[0] |= mc_enc(32, 8, irq_index);
+	cmd.params[0] |= mc_enc(0, 32, irq_val);
+	cmd.params[1] |= mc_enc(0, 64, irq_paddr);
+	cmd.params[2] |= mc_enc(0, 32, user_irq_id);
+
+	/* send command to mc*/
+	return mc_send_command(mc_io, &cmd);
+}
+
+int dprc_get_irq_enable(struct fsl_mc_io *mc_io,
+			uint16_t token,
+			uint8_t irq_index,
+			uint8_t *en)
+{
+	struct mc_command cmd = { 0 };
+	int err;
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPRC_CMDID_GET_IRQ_ENABLE,
+					  MC_CMD_PRI_LOW, token);
+	cmd.params[0] |= mc_enc(32, 8, irq_index);
+
+	/* send command to mc*/
+	err = mc_send_command(mc_io, &cmd);
+	if (err)
+		return err;
+
+	/* retrieve response parameters */
+	*en = mc_dec(cmd.params[0], 0, 8);
+
+	return 0;
+}
+
+int dprc_set_irq_enable(struct fsl_mc_io *mc_io,
+			uint16_t token,
+			uint8_t irq_index,
+			uint8_t en)
+{
+	struct mc_command cmd = { 0 };
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPRC_CMDID_SET_IRQ_ENABLE,
+					  MC_CMD_PRI_LOW, token);
+	cmd.params[0] |= mc_enc(0, 8, en);
+	cmd.params[0] |= mc_enc(32, 8, irq_index);
+
+	/* send command to mc*/
+	return mc_send_command(mc_io, &cmd);
+}
+
+int dprc_get_irq_mask(struct fsl_mc_io *mc_io,
+		      uint16_t token,
+		      uint8_t irq_index,
+		      uint32_t *mask)
+{
+	struct mc_command cmd = { 0 };
+	int err;
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPRC_CMDID_GET_IRQ_MASK,
+					  MC_CMD_PRI_LOW, token);
+	cmd.params[0] |= mc_enc(32, 8, irq_index);
+
+	/* send command to mc*/
+	err = mc_send_command(mc_io, &cmd);
+	if (err)
+		return err;
+
+	/* retrieve response parameters */
+	*mask = mc_dec(cmd.params[0], 0, 32);
+
+	return 0;
+}
+
+int dprc_set_irq_mask(struct fsl_mc_io *mc_io,
+		      uint16_t token,
+		      uint8_t irq_index,
+		      uint32_t mask)
+{
+	struct mc_command cmd = { 0 };
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPRC_CMDID_SET_IRQ_MASK,
+					  MC_CMD_PRI_LOW, token);
+	cmd.params[0] |= mc_enc(0, 32, mask);
+	cmd.params[0] |= mc_enc(32, 8, irq_index);
+
+	/* send command to mc*/
+	return mc_send_command(mc_io, &cmd);
+}
+
+int dprc_get_irq_status(struct fsl_mc_io *mc_io,
+			uint16_t token,
+			uint8_t irq_index,
+			uint32_t *status)
+{
+	struct mc_command cmd = { 0 };
+	int err;
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPRC_CMDID_GET_IRQ_STATUS,
+					  MC_CMD_PRI_LOW, token);
+	cmd.params[0] |= mc_enc(32, 8, irq_index);
+
+	/* send command to mc*/
+	err = mc_send_command(mc_io, &cmd);
+	if (err)
+		return err;
+
+	/* retrieve response parameters */
+	*status = mc_dec(cmd.params[0], 0, 32);
+
+	return 0;
+}
+
+int dprc_clear_irq_status(struct fsl_mc_io *mc_io,
+			  uint16_t token,
+			  uint8_t irq_index,
+			  uint32_t status)
+{
+	struct mc_command cmd = { 0 };
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPRC_CMDID_CLEAR_IRQ_STATUS,
+					  MC_CMD_PRI_LOW, token);
+	cmd.params[0] |= mc_enc(0, 32, status);
+	cmd.params[0] |= mc_enc(32, 8, irq_index);
+
+	/* send command to mc*/
+	return mc_send_command(mc_io, &cmd);
+}
+
+int dprc_get_attributes(struct fsl_mc_io *mc_io,
+			uint16_t token,
+			struct dprc_attributes *attr)
+{
+	struct mc_command cmd = { 0 };
+	int err;
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPRC_CMDID_GET_ATTR,
+					  MC_CMD_PRI_LOW,
+					  token);
+
+	/* send command to mc*/
+	err = mc_send_command(mc_io, &cmd);
+	if (err)
+		return err;
+
+	/* retrieve response parameters */
+	attr->container_id = mc_dec(cmd.params[0], 0, 32);
+	attr->icid = mc_dec(cmd.params[0], 32, 16);
+	attr->options = mc_dec(cmd.params[1], 0, 32);
+	attr->portal_id = mc_dec(cmd.params[1], 32, 32);
+	attr->version.major = mc_dec(cmd.params[2], 0, 16);
+	attr->version.minor = mc_dec(cmd.params[2], 16, 16);
+
+	return 0;
+}
+
+int dprc_set_res_quota(struct fsl_mc_io *mc_io,
+		       uint16_t token,
+		       int child_container_id,
+		       char *type,
+		       uint16_t quota)
+{
+	struct mc_command cmd = { 0 };
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPRC_CMDID_SET_RES_QUOTA,
+					  MC_CMD_PRI_LOW, token);
+	cmd.params[0] |= mc_enc(0, 32, child_container_id);
+	cmd.params[0] |= mc_enc(32, 16, quota);
+	cmd.params[1] |= mc_enc(0, 8, type[0]);
+	cmd.params[1] |= mc_enc(8, 8, type[1]);
+	cmd.params[1] |= mc_enc(16, 8, type[2]);
+	cmd.params[1] |= mc_enc(24, 8, type[3]);
+	cmd.params[1] |= mc_enc(32, 8, type[4]);
+	cmd.params[1] |= mc_enc(40, 8, type[5]);
+	cmd.params[1] |= mc_enc(48, 8, type[6]);
+	cmd.params[1] |= mc_enc(56, 8, type[7]);
+	cmd.params[2] |= mc_enc(0, 8, type[8]);
+	cmd.params[2] |= mc_enc(8, 8, type[9]);
+	cmd.params[2] |= mc_enc(16, 8, type[10]);
+	cmd.params[2] |= mc_enc(24, 8, type[11]);
+	cmd.params[2] |= mc_enc(32, 8, type[12]);
+	cmd.params[2] |= mc_enc(40, 8, type[13]);
+	cmd.params[2] |= mc_enc(48, 8, type[14]);
+	cmd.params[2] |= mc_enc(56, 8, '\0');
+
+	/* send command to mc*/
+	return mc_send_command(mc_io, &cmd);
+}
+
+int dprc_get_res_quota(struct fsl_mc_io *mc_io,
+		       uint16_t token,
+		       int child_container_id,
+		       char *type,
+		       uint16_t *quota)
+{
+	struct mc_command cmd = { 0 };
+	int err;
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPRC_CMDID_GET_RES_QUOTA,
+					  MC_CMD_PRI_LOW, token);
+	cmd.params[0] |= mc_enc(0, 32, child_container_id);
+	cmd.params[1] |= mc_enc(0, 8, type[0]);
+	cmd.params[1] |= mc_enc(8, 8, type[1]);
+	cmd.params[1] |= mc_enc(16, 8, type[2]);
+	cmd.params[1] |= mc_enc(24, 8, type[3]);
+	cmd.params[1] |= mc_enc(32, 8, type[4]);
+	cmd.params[1] |= mc_enc(40, 8, type[5]);
+	cmd.params[1] |= mc_enc(48, 8, type[6]);
+	cmd.params[1] |= mc_enc(56, 8, type[7]);
+	cmd.params[2] |= mc_enc(0, 8, type[8]);
+	cmd.params[2] |= mc_enc(8, 8, type[9]);
+	cmd.params[2] |= mc_enc(16, 8, type[10]);
+	cmd.params[2] |= mc_enc(24, 8, type[11]);
+	cmd.params[2] |= mc_enc(32, 8, type[12]);
+	cmd.params[2] |= mc_enc(40, 8, type[13]);
+	cmd.params[2] |= mc_enc(48, 8, type[14]);
+	cmd.params[2] |= mc_enc(56, 8, '\0');
+
+	/* send command to mc*/
+	err = mc_send_command(mc_io, &cmd);
+	if (err)
+		return err;
+
+	/* retrieve response parameters */
+	*quota = mc_dec(cmd.params[0], 32, 16);
+
+	return 0;
+}
+
+int dprc_assign(struct fsl_mc_io *mc_io,
+		uint16_t token,
+		int container_id,
+		struct dprc_res_req *res_req)
+{
+	struct mc_command cmd = { 0 };
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPRC_CMDID_ASSIGN,
+					  MC_CMD_PRI_LOW, token);
+	cmd.params[0] |= mc_enc(0, 32, container_id);
+	cmd.params[0] |= mc_enc(32, 32, res_req->options);
+	cmd.params[1] |= mc_enc(0, 32, res_req->num);
+	cmd.params[1] |= mc_enc(32, 32, res_req->id_base_align);
+	cmd.params[2] |= mc_enc(0, 8, res_req->type[0]);
+	cmd.params[2] |= mc_enc(8, 8, res_req->type[1]);
+	cmd.params[2] |= mc_enc(16, 8, res_req->type[2]);
+	cmd.params[2] |= mc_enc(24, 8, res_req->type[3]);
+	cmd.params[2] |= mc_enc(32, 8, res_req->type[4]);
+	cmd.params[2] |= mc_enc(40, 8, res_req->type[5]);
+	cmd.params[2] |= mc_enc(48, 8, res_req->type[6]);
+	cmd.params[2] |= mc_enc(56, 8, res_req->type[7]);
+	cmd.params[3] |= mc_enc(0, 8, res_req->type[8]);
+	cmd.params[3] |= mc_enc(8, 8, res_req->type[9]);
+	cmd.params[3] |= mc_enc(16, 8, res_req->type[10]);
+	cmd.params[3] |= mc_enc(24, 8, res_req->type[11]);
+	cmd.params[3] |= mc_enc(32, 8, res_req->type[12]);
+	cmd.params[3] |= mc_enc(40, 8, res_req->type[13]);
+	cmd.params[3] |= mc_enc(48, 8, res_req->type[14]);
+	cmd.params[3] |= mc_enc(56, 8, res_req->type[15]);
+
+	/* send command to mc*/
+	return mc_send_command(mc_io, &cmd);
+}
+
+int dprc_unassign(struct fsl_mc_io *mc_io,
+		  uint16_t token,
+		  int child_container_id,
+		  struct dprc_res_req *res_req)
+{
+	struct mc_command cmd = { 0 };
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPRC_CMDID_UNASSIGN,
+					  MC_CMD_PRI_LOW,
+					  token);
+	cmd.params[0] |= mc_enc(0, 32, child_container_id);
+	cmd.params[0] |= mc_enc(32, 32, res_req->options);
+	cmd.params[1] |= mc_enc(0, 32, res_req->num);
+	cmd.params[1] |= mc_enc(32, 32, res_req->id_base_align);
+	cmd.params[2] |= mc_enc(0, 8, res_req->type[0]);
+	cmd.params[2] |= mc_enc(8, 8, res_req->type[1]);
+	cmd.params[2] |= mc_enc(16, 8, res_req->type[2]);
+	cmd.params[2] |= mc_enc(24, 8, res_req->type[3]);
+	cmd.params[2] |= mc_enc(32, 8, res_req->type[4]);
+	cmd.params[2] |= mc_enc(40, 8, res_req->type[5]);
+	cmd.params[2] |= mc_enc(48, 8, res_req->type[6]);
+	cmd.params[2] |= mc_enc(56, 8, res_req->type[7]);
+	cmd.params[3] |= mc_enc(0, 8, res_req->type[8]);
+	cmd.params[3] |= mc_enc(8, 8, res_req->type[9]);
+	cmd.params[3] |= mc_enc(16, 8, res_req->type[10]);
+	cmd.params[3] |= mc_enc(24, 8, res_req->type[11]);
+	cmd.params[3] |= mc_enc(32, 8, res_req->type[12]);
+	cmd.params[3] |= mc_enc(40, 8, res_req->type[13]);
+	cmd.params[3] |= mc_enc(48, 8, res_req->type[14]);
+	cmd.params[3] |= mc_enc(56, 8, res_req->type[15]);
+
+	/* send command to mc*/
+	return mc_send_command(mc_io, &cmd);
+}
+
+int dprc_get_pool_count(struct fsl_mc_io *mc_io,
+			uint16_t token,
+			int *pool_count)
+{
+	struct mc_command cmd = { 0 };
+	int err;
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPRC_CMDID_GET_POOL_COUNT,
+					  MC_CMD_PRI_LOW, token);
+
+	/* send command to mc*/
+	err = mc_send_command(mc_io, &cmd);
+	if (err)
+		return err;
+
+	/* retrieve response parameters */
+	*pool_count = mc_dec(cmd.params[0], 0, 32);
+
+	return 0;
+}
+
+int dprc_get_pool(struct fsl_mc_io *mc_io,
+		  uint16_t token,
+		  int pool_index,
+		  char *type)
+{
+	struct mc_command cmd = { 0 };
+	int err;
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPRC_CMDID_GET_POOL,
+					  MC_CMD_PRI_LOW,
+					  token);
+	cmd.params[0] |= mc_enc(0, 32, pool_index);
+
+	/* send command to mc*/
+	err = mc_send_command(mc_io, &cmd);
+	if (err)
+		return err;
+
+	/* retrieve response parameters */
+	type[0] = mc_dec(cmd.params[1], 0, 8);
+	type[1] = mc_dec(cmd.params[1], 8, 8);
+	type[2] = mc_dec(cmd.params[1], 16, 8);
+	type[3] = mc_dec(cmd.params[1], 24, 8);
+	type[4] = mc_dec(cmd.params[1], 32, 8);
+	type[5] = mc_dec(cmd.params[1], 40, 8);
+	type[6] = mc_dec(cmd.params[1], 48, 8);
+	type[7] = mc_dec(cmd.params[1], 56, 8);
+	type[8] = mc_dec(cmd.params[2], 0, 8);
+	type[9] = mc_dec(cmd.params[2], 8, 8);
+	type[10] = mc_dec(cmd.params[2], 16, 8);
+	type[11] = mc_dec(cmd.params[2], 24, 8);
+	type[12] = mc_dec(cmd.params[2], 32, 8);
+	type[13] = mc_dec(cmd.params[2], 40, 8);
+	type[14] = mc_dec(cmd.params[2], 48, 8);
+	type[15] = '\0';
+
+	return 0;
+}
+
+int dprc_get_obj_count(struct fsl_mc_io *mc_io, uint16_t token, int *obj_count)
+{
+	struct mc_command cmd = { 0 };
+	int err;
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPRC_CMDID_GET_OBJ_COUNT,
+					  MC_CMD_PRI_LOW, token);
+
+	/* send command to mc*/
+	err = mc_send_command(mc_io, &cmd);
+	if (err)
+		return err;
+
+	/* retrieve response parameters */
+	*obj_count = mc_dec(cmd.params[0], 32, 32);
+
+	return 0;
+}
+EXPORT_SYMBOL(dprc_get_obj_count);
+
+int dprc_get_obj(struct fsl_mc_io *mc_io,
+		 uint16_t token,
+		 int obj_index,
+		 struct dprc_obj_desc *obj_desc)
+{
+	struct mc_command cmd = { 0 };
+	int err;
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPRC_CMDID_GET_OBJ,
+					  MC_CMD_PRI_LOW,
+					  token);
+	cmd.params[0] |= mc_enc(0, 32, obj_index);
+
+	/* send command to mc*/
+	err = mc_send_command(mc_io, &cmd);
+	if (err)
+		return err;
+
+	/* retrieve response parameters */
+	obj_desc->id = mc_dec(cmd.params[0], 32, 32);
+	obj_desc->vendor = mc_dec(cmd.params[1], 0, 16);
+	obj_desc->irq_count = mc_dec(cmd.params[1], 16, 8);
+	obj_desc->region_count = mc_dec(cmd.params[1], 24, 8);
+	obj_desc->state = mc_dec(cmd.params[1], 32, 32);
+	obj_desc->ver_major = mc_dec(cmd.params[2], 0, 16);
+	obj_desc->ver_minor = mc_dec(cmd.params[2], 16, 16);
+	obj_desc->type[0] = mc_dec(cmd.params[3], 0, 8);
+	obj_desc->type[1] = mc_dec(cmd.params[3], 8, 8);
+	obj_desc->type[2] = mc_dec(cmd.params[3], 16, 8);
+	obj_desc->type[3] = mc_dec(cmd.params[3], 24, 8);
+	obj_desc->type[4] = mc_dec(cmd.params[3], 32, 8);
+	obj_desc->type[5] = mc_dec(cmd.params[3], 40, 8);
+	obj_desc->type[6] = mc_dec(cmd.params[3], 48, 8);
+	obj_desc->type[7] = mc_dec(cmd.params[3], 56, 8);
+	obj_desc->type[8] = mc_dec(cmd.params[4], 0, 8);
+	obj_desc->type[9] = mc_dec(cmd.params[4], 8, 8);
+	obj_desc->type[10] = mc_dec(cmd.params[4], 16, 8);
+	obj_desc->type[11] = mc_dec(cmd.params[4], 24, 8);
+	obj_desc->type[12] = mc_dec(cmd.params[4], 32, 8);
+	obj_desc->type[13] = mc_dec(cmd.params[4], 40, 8);
+	obj_desc->type[14] = mc_dec(cmd.params[4], 48, 8);
+	obj_desc->type[15] = '\0';
+
+	return 0;
+}
+EXPORT_SYMBOL(dprc_get_obj);
+
+int dprc_get_res_count(struct fsl_mc_io *mc_io,
+		       uint16_t token,
+		       char *type,
+		       int *res_count)
+{
+	struct mc_command cmd = { 0 };
+	int err;
+
+	*res_count = 0;
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPRC_CMDID_GET_RES_COUNT,
+					  MC_CMD_PRI_LOW, token);
+	cmd.params[1] |= mc_enc(0, 8, type[0]);
+	cmd.params[1] |= mc_enc(8, 8, type[1]);
+	cmd.params[1] |= mc_enc(16, 8, type[2]);
+	cmd.params[1] |= mc_enc(24, 8, type[3]);
+	cmd.params[1] |= mc_enc(32, 8, type[4]);
+	cmd.params[1] |= mc_enc(40, 8, type[5]);
+	cmd.params[1] |= mc_enc(48, 8, type[6]);
+	cmd.params[1] |= mc_enc(56, 8, type[7]);
+	cmd.params[2] |= mc_enc(0, 8, type[8]);
+	cmd.params[2] |= mc_enc(8, 8, type[9]);
+	cmd.params[2] |= mc_enc(16, 8, type[10]);
+	cmd.params[2] |= mc_enc(24, 8, type[11]);
+	cmd.params[2] |= mc_enc(32, 8, type[12]);
+	cmd.params[2] |= mc_enc(40, 8, type[13]);
+	cmd.params[2] |= mc_enc(48, 8, type[14]);
+	cmd.params[2] |= mc_enc(56, 8, '\0');
+
+	/* send command to mc*/
+	err = mc_send_command(mc_io, &cmd);
+	if (err)
+		return err;
+
+	/* retrieve response parameters */
+	*res_count = mc_dec(cmd.params[0], 0, 32);
+
+	return 0;
+}
+EXPORT_SYMBOL(dprc_get_res_count);
+
+int dprc_get_res_ids(struct fsl_mc_io *mc_io,
+		     uint16_t token,
+		     char *type,
+		     struct dprc_res_ids_range_desc *range_desc)
+{
+	struct mc_command cmd = { 0 };
+	int err;
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPRC_CMDID_GET_RES_IDS,
+					  MC_CMD_PRI_LOW, token);
+	cmd.params[0] |= mc_enc(42, 7, range_desc->iter_status);
+	cmd.params[1] |= mc_enc(0, 32, range_desc->base_id);
+	cmd.params[1] |= mc_enc(32, 32, range_desc->last_id);
+	cmd.params[2] |= mc_enc(0, 8, type[0]);
+	cmd.params[2] |= mc_enc(8, 8, type[1]);
+	cmd.params[2] |= mc_enc(16, 8, type[2]);
+	cmd.params[2] |= mc_enc(24, 8, type[3]);
+	cmd.params[2] |= mc_enc(32, 8, type[4]);
+	cmd.params[2] |= mc_enc(40, 8, type[5]);
+	cmd.params[2] |= mc_enc(48, 8, type[6]);
+	cmd.params[2] |= mc_enc(56, 8, type[7]);
+	cmd.params[3] |= mc_enc(0, 8, type[8]);
+	cmd.params[3] |= mc_enc(8, 8, type[9]);
+	cmd.params[3] |= mc_enc(16, 8, type[10]);
+	cmd.params[3] |= mc_enc(24, 8, type[11]);
+	cmd.params[3] |= mc_enc(32, 8, type[12]);
+	cmd.params[3] |= mc_enc(40, 8, type[13]);
+	cmd.params[3] |= mc_enc(48, 8, type[14]);
+	cmd.params[3] |= mc_enc(56, 8, '\0');
+
+	/* send command to mc*/
+	err = mc_send_command(mc_io, &cmd);
+	if (err)
+		return err;
+
+	/* retrieve response parameters */
+	range_desc->iter_status = mc_dec(cmd.params[0], 42, 7);
+	range_desc->base_id = mc_dec(cmd.params[1], 0, 32);
+	range_desc->last_id = mc_dec(cmd.params[1], 32, 32);
+
+	return 0;
+}
+EXPORT_SYMBOL(dprc_get_res_ids);
+
+int dprc_get_portal_paddr(struct fsl_mc_io *mc_io,
+			  uint16_t token,
+			  int portal_id,
+			  uint64_t *portal_addr)
+{
+	struct mc_command cmd = { 0 };
+	int err;
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPRC_CMDID_GET_PORTAL_PADDR,
+					  MC_CMD_PRI_LOW, token);
+	cmd.params[0] |= mc_enc(0, 32, portal_id);
+
+	/* send command to mc*/
+	err = mc_send_command(mc_io, &cmd);
+	if (err)
+		return err;
+
+	/* retrieve response parameters */
+	*portal_addr = mc_dec(cmd.params[1], 0, 64);
+
+	return 0;
+}
+EXPORT_SYMBOL(dprc_get_portal_paddr);
+
+int dprc_get_obj_region(struct fsl_mc_io *mc_io,
+			uint16_t token,
+			char *obj_type,
+			int obj_id,
+			uint8_t region_index,
+			struct dprc_region_desc *region_desc)
+{
+	struct mc_command cmd = { 0 };
+	int err;
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPRC_CMDID_GET_OBJ_REG,
+					  MC_CMD_PRI_LOW, token);
+	cmd.params[0] |= mc_enc(0, 32, obj_id);
+	cmd.params[0] |= mc_enc(48, 8, region_index);
+	cmd.params[3] |= mc_enc(0, 8, obj_type[0]);
+	cmd.params[3] |= mc_enc(8, 8, obj_type[1]);
+	cmd.params[3] |= mc_enc(16, 8, obj_type[2]);
+	cmd.params[3] |= mc_enc(24, 8, obj_type[3]);
+	cmd.params[3] |= mc_enc(32, 8, obj_type[4]);
+	cmd.params[3] |= mc_enc(40, 8, obj_type[5]);
+	cmd.params[3] |= mc_enc(48, 8, obj_type[6]);
+	cmd.params[3] |= mc_enc(56, 8, obj_type[7]);
+	cmd.params[4] |= mc_enc(0, 8, obj_type[8]);
+	cmd.params[4] |= mc_enc(8, 8, obj_type[9]);
+	cmd.params[4] |= mc_enc(16, 8, obj_type[10]);
+	cmd.params[4] |= mc_enc(24, 8, obj_type[11]);
+	cmd.params[4] |= mc_enc(32, 8, obj_type[12]);
+	cmd.params[4] |= mc_enc(40, 8, obj_type[13]);
+	cmd.params[4] |= mc_enc(48, 8, obj_type[14]);
+	cmd.params[4] |= mc_enc(56, 8, '\0');
+
+	/* send command to mc*/
+	err = mc_send_command(mc_io, &cmd);
+	if (err)
+		return err;
+
+	/* retrieve response parameters */
+	region_desc->base_paddr = mc_dec(cmd.params[1], 0, 64);
+	region_desc->size = mc_dec(cmd.params[2], 0, 32);
+
+	return 0;
+}
+EXPORT_SYMBOL(dprc_get_obj_region);
+
+int dprc_connect(struct fsl_mc_io *mc_io,
+		 uint16_t token,
+		 const struct dprc_endpoint *endpoint1,
+		 const struct dprc_endpoint *endpoint2)
+{
+	struct mc_command cmd = { 0 };
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPRC_CMDID_CONNECT,
+					  MC_CMD_PRI_LOW,
+					  token);
+	cmd.params[0] |= mc_enc(0, 32, endpoint1->id);
+	cmd.params[0] |= mc_enc(32, 32, endpoint1->interface_id);
+	cmd.params[1] |= mc_enc(0, 32, endpoint2->id);
+	cmd.params[1] |= mc_enc(32, 32, endpoint2->interface_id);
+	cmd.params[2] |= mc_enc(0, 8, endpoint1->type[0]);
+	cmd.params[2] |= mc_enc(8, 8, endpoint1->type[1]);
+	cmd.params[2] |= mc_enc(16, 8, endpoint1->type[2]);
+	cmd.params[2] |= mc_enc(24, 8, endpoint1->type[3]);
+	cmd.params[2] |= mc_enc(32, 8, endpoint1->type[4]);
+	cmd.params[2] |= mc_enc(40, 8, endpoint1->type[5]);
+	cmd.params[2] |= mc_enc(48, 8, endpoint1->type[6]);
+	cmd.params[2] |= mc_enc(56, 8, endpoint1->type[7]);
+	cmd.params[3] |= mc_enc(0, 8, endpoint1->type[8]);
+	cmd.params[3] |= mc_enc(8, 8, endpoint1->type[9]);
+	cmd.params[3] |= mc_enc(16, 8, endpoint1->type[10]);
+	cmd.params[3] |= mc_enc(24, 8, endpoint1->type[11]);
+	cmd.params[3] |= mc_enc(32, 8, endpoint1->type[12]);
+	cmd.params[3] |= mc_enc(40, 8, endpoint1->type[13]);
+	cmd.params[3] |= mc_enc(48, 8, endpoint1->type[14]);
+	cmd.params[3] |= mc_enc(56, 8, endpoint1->type[15]);
+	cmd.params[5] |= mc_enc(0, 8, endpoint2->type[0]);
+	cmd.params[5] |= mc_enc(8, 8, endpoint2->type[1]);
+	cmd.params[5] |= mc_enc(16, 8, endpoint2->type[2]);
+	cmd.params[5] |= mc_enc(24, 8, endpoint2->type[3]);
+	cmd.params[5] |= mc_enc(32, 8, endpoint2->type[4]);
+	cmd.params[5] |= mc_enc(40, 8, endpoint2->type[5]);
+	cmd.params[5] |= mc_enc(48, 8, endpoint2->type[6]);
+	cmd.params[5] |= mc_enc(56, 8, endpoint2->type[7]);
+	cmd.params[6] |= mc_enc(0, 8, endpoint2->type[8]);
+	cmd.params[6] |= mc_enc(8, 8, endpoint2->type[9]);
+	cmd.params[6] |= mc_enc(16, 8, endpoint2->type[10]);
+	cmd.params[6] |= mc_enc(24, 8, endpoint2->type[11]);
+	cmd.params[6] |= mc_enc(32, 8, endpoint2->type[12]);
+	cmd.params[6] |= mc_enc(40, 8, endpoint2->type[13]);
+	cmd.params[6] |= mc_enc(48, 8, endpoint2->type[14]);
+	cmd.params[6] |= mc_enc(56, 8, endpoint2->type[15]);
+
+	/* send command to mc*/
+	return mc_send_command(mc_io, &cmd);
+}
+
+int dprc_disconnect(struct fsl_mc_io *mc_io,
+		    uint16_t token,
+		    const struct dprc_endpoint *endpoint)
+{
+	struct mc_command cmd = { 0 };
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPRC_CMDID_DISCONNECT,
+					  MC_CMD_PRI_LOW,
+					  token);
+	cmd.params[0] |= mc_enc(0, 32, endpoint->id);
+	cmd.params[0] |= mc_enc(32, 32, endpoint->interface_id);
+	cmd.params[1] |= mc_enc(0, 8, endpoint->type[0]);
+	cmd.params[1] |= mc_enc(8, 8, endpoint->type[1]);
+	cmd.params[1] |= mc_enc(16, 8, endpoint->type[2]);
+	cmd.params[1] |= mc_enc(24, 8, endpoint->type[3]);
+	cmd.params[1] |= mc_enc(32, 8, endpoint->type[4]);
+	cmd.params[1] |= mc_enc(40, 8, endpoint->type[5]);
+	cmd.params[1] |= mc_enc(48, 8, endpoint->type[6]);
+	cmd.params[1] |= mc_enc(56, 8, endpoint->type[7]);
+	cmd.params[2] |= mc_enc(0, 8, endpoint->type[8]);
+	cmd.params[2] |= mc_enc(8, 8, endpoint->type[9]);
+	cmd.params[2] |= mc_enc(16, 8, endpoint->type[10]);
+	cmd.params[2] |= mc_enc(24, 8, endpoint->type[11]);
+	cmd.params[2] |= mc_enc(32, 8, endpoint->type[12]);
+	cmd.params[2] |= mc_enc(40, 8, endpoint->type[13]);
+	cmd.params[2] |= mc_enc(48, 8, endpoint->type[14]);
+	cmd.params[2] |= mc_enc(56, 8, endpoint->type[15]);
+
+	/* send command to mc*/
+	return mc_send_command(mc_io, &cmd);
+}
+
+int dprc_get_connection(struct fsl_mc_io *mc_io,
+			uint16_t token,
+					const struct dprc_endpoint *endpoint1,
+					struct dprc_endpoint *endpoint2,
+					int *state)
+{
+	struct mc_command cmd = { 0 };
+	int err;
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPRC_CMDID_GET_CONNECTION,
+					  MC_CMD_PRI_LOW,
+					  token);
+	cmd.params[0] |= mc_enc(0, 32, endpoint1->id);
+	cmd.params[0] |= mc_enc(32, 32, endpoint1->interface_id);
+	cmd.params[1] |= mc_enc(0, 8, endpoint1->type[0]);
+	cmd.params[1] |= mc_enc(8, 8, endpoint1->type[1]);
+	cmd.params[1] |= mc_enc(16, 8, endpoint1->type[2]);
+	cmd.params[1] |= mc_enc(24, 8, endpoint1->type[3]);
+	cmd.params[1] |= mc_enc(32, 8, endpoint1->type[4]);
+	cmd.params[1] |= mc_enc(40, 8, endpoint1->type[5]);
+	cmd.params[1] |= mc_enc(48, 8, endpoint1->type[6]);
+	cmd.params[1] |= mc_enc(56, 8, endpoint1->type[7]);
+	cmd.params[2] |= mc_enc(0, 8, endpoint1->type[8]);
+	cmd.params[2] |= mc_enc(8, 8, endpoint1->type[9]);
+	cmd.params[2] |= mc_enc(16, 8, endpoint1->type[10]);
+	cmd.params[2] |= mc_enc(24, 8, endpoint1->type[11]);
+	cmd.params[2] |= mc_enc(32, 8, endpoint1->type[12]);
+	cmd.params[2] |= mc_enc(40, 8, endpoint1->type[13]);
+	cmd.params[2] |= mc_enc(48, 8, endpoint1->type[14]);
+	cmd.params[2] |= mc_enc(56, 8, endpoint1->type[15]);
+
+	/* send command to mc*/
+	err = mc_send_command(mc_io, &cmd);
+	if (err)
+		return err;
+
+	/* retrieve response parameters */
+	endpoint2->id = mc_dec(cmd.params[3], 0, 32);
+	endpoint2->interface_id = mc_dec(cmd.params[3], 32, 32);
+	endpoint2->type[0] = mc_dec(cmd.params[4], 0, 8);
+	endpoint2->type[1] = mc_dec(cmd.params[4], 8, 8);
+	endpoint2->type[2] = mc_dec(cmd.params[4], 16, 8);
+	endpoint2->type[3] = mc_dec(cmd.params[4], 24, 8);
+	endpoint2->type[4] = mc_dec(cmd.params[4], 32, 8);
+	endpoint2->type[5] = mc_dec(cmd.params[4], 40, 8);
+	endpoint2->type[6] = mc_dec(cmd.params[4], 48, 8);
+	endpoint2->type[7] = mc_dec(cmd.params[4], 56, 8);
+	endpoint2->type[8] = mc_dec(cmd.params[5], 0, 8);
+	endpoint2->type[9] = mc_dec(cmd.params[5], 8, 8);
+	endpoint2->type[10] = mc_dec(cmd.params[5], 16, 8);
+	endpoint2->type[11] = mc_dec(cmd.params[5], 24, 8);
+	endpoint2->type[12] = mc_dec(cmd.params[5], 32, 8);
+	endpoint2->type[13] = mc_dec(cmd.params[5], 40, 8);
+	endpoint2->type[14] = mc_dec(cmd.params[5], 48, 8);
+	endpoint2->type[15] = mc_dec(cmd.params[5], 56, 8);
+	*state = mc_dec(cmd.params[6], 0, 32);
+
+	return 0;
+}
diff --git a/drivers/staging/fsl-mc/bus/mc-allocator.c b/drivers/staging/fsl-mc/bus/mc-allocator.c
new file mode 100644
index 000000000000..e36235ddbe3e
--- /dev/null
+++ b/drivers/staging/fsl-mc/bus/mc-allocator.c
@@ -0,0 +1,573 @@
+/*
+ * Freescale MC object device allocator driver
+ *
+ * Copyright (C) 2013 Freescale Semiconductor, Inc.
+ *
+ * This file is licensed under the terms of the GNU General Public
+ * License version 2. This program is licensed "as is" without any
+ * warranty of any kind, whether express or implied.
+ */
+
+#include "../include/mc-private.h"
+#include "../include/mc-sys.h"
+#include <linux/module.h>
+#include "../include/dpbp-cmd.h"
+#include "../include/dpcon-cmd.h"
+#include "dpmcp-cmd.h"
+#include "dpmcp.h"
+
+/**
+ * fsl_mc_resource_pool_add_device - add allocatable device to a resource
+ * pool of a given MC bus
+ *
+ * @mc_bus: pointer to the MC bus
+ * @pool_type: MC bus pool type
+ * @mc_dev: Pointer to allocatable MC object device
+ *
+ * It adds an allocatable MC object device to a container's resource pool of
+ * the given resource type
+ */
+static int __must_check fsl_mc_resource_pool_add_device(struct fsl_mc_bus
+								*mc_bus,
+							enum fsl_mc_pool_type
+								pool_type,
+							struct fsl_mc_device
+								*mc_dev)
+{
+	struct fsl_mc_resource_pool *res_pool;
+	struct fsl_mc_resource *resource;
+	struct fsl_mc_device *mc_bus_dev = &mc_bus->mc_dev;
+	int error = -EINVAL;
+	bool mutex_locked = false;
+
+	if (WARN_ON(pool_type < 0 || pool_type >= FSL_MC_NUM_POOL_TYPES))
+		goto out;
+	if (WARN_ON(!FSL_MC_IS_ALLOCATABLE(mc_dev->obj_desc.type)))
+		goto out;
+	if (WARN_ON(mc_dev->resource))
+		goto out;
+
+	res_pool = &mc_bus->resource_pools[pool_type];
+	if (WARN_ON(res_pool->type != pool_type))
+		goto out;
+	if (WARN_ON(res_pool->mc_bus != mc_bus))
+		goto out;
+
+	mutex_lock(&res_pool->mutex);
+	mutex_locked = true;
+
+	if (WARN_ON(res_pool->max_count < 0))
+		goto out;
+	if (WARN_ON(res_pool->free_count < 0 ||
+		    res_pool->free_count > res_pool->max_count))
+		goto out;
+
+	resource = devm_kzalloc(&mc_bus_dev->dev, sizeof(*resource),
+				GFP_KERNEL);
+	if (!resource) {
+		error = -ENOMEM;
+		dev_err(&mc_bus_dev->dev,
+			"Failed to allocate memory for fsl_mc_resource\n");
+		goto out;
+	}
+
+	resource->type = pool_type;
+	resource->id = mc_dev->obj_desc.id;
+	resource->data = mc_dev;
+	resource->parent_pool = res_pool;
+	INIT_LIST_HEAD(&resource->node);
+	list_add_tail(&resource->node, &res_pool->free_list);
+	mc_dev->resource = resource;
+	res_pool->free_count++;
+	res_pool->max_count++;
+	error = 0;
+out:
+	if (mutex_locked)
+		mutex_unlock(&res_pool->mutex);
+
+	return error;
+}
+
+/**
+ * fsl_mc_resource_pool_remove_device - remove an allocatable device from a
+ * resource pool
+ *
+ * @mc_dev: Pointer to allocatable MC object device
+ *
+ * It permanently removes an allocatable MC object device from the resource
+ * pool, the device is currently in, as long as it is in the pool's free list.
+ */
+static int __must_check fsl_mc_resource_pool_remove_device(struct fsl_mc_device
+								   *mc_dev)
+{
+	struct fsl_mc_device *mc_bus_dev;
+	struct fsl_mc_bus *mc_bus;
+	struct fsl_mc_resource_pool *res_pool;
+	struct fsl_mc_resource *resource;
+	int error = -EINVAL;
+	bool mutex_locked = false;
+
+	if (WARN_ON(!FSL_MC_IS_ALLOCATABLE(mc_dev->obj_desc.type)))
+		goto out;
+
+	resource = mc_dev->resource;
+	if (WARN_ON(resource->data != mc_dev))
+		goto out;
+
+	mc_bus_dev = to_fsl_mc_device(mc_dev->dev.parent);
+	mc_bus = to_fsl_mc_bus(mc_bus_dev);
+	res_pool = resource->parent_pool;
+	if (WARN_ON(res_pool != &mc_bus->resource_pools[resource->type]))
+		goto out;
+
+	mutex_lock(&res_pool->mutex);
+	mutex_locked = true;
+
+	if (WARN_ON(res_pool->max_count <= 0))
+		goto out;
+	if (WARN_ON(res_pool->free_count <= 0 ||
+		    res_pool->free_count > res_pool->max_count))
+		goto out;
+
+	/*
+	 * If the device is currently allocated, its resource is not
+	 * in the free list and thus, the device cannot be removed.
+	 */
+	if (list_empty(&resource->node)) {
+		error = -EBUSY;
+		dev_err(&mc_bus_dev->dev,
+			"Device %s cannot be removed from resource pool\n",
+			dev_name(&mc_dev->dev));
+		goto out;
+	}
+
+	list_del(&resource->node);
+	INIT_LIST_HEAD(&resource->node);
+	res_pool->free_count--;
+	res_pool->max_count--;
+
+	devm_kfree(&mc_bus_dev->dev, resource);
+	mc_dev->resource = NULL;
+	error = 0;
+out:
+	if (mutex_locked)
+		mutex_unlock(&res_pool->mutex);
+
+	return error;
+}
+
+static const char *const fsl_mc_pool_type_strings[] = {
+	[FSL_MC_POOL_DPMCP] = "dpmcp",
+	[FSL_MC_POOL_DPBP] = "dpbp",
+	[FSL_MC_POOL_DPCON] = "dpcon",
+};
+
+static int __must_check object_type_to_pool_type(const char *object_type,
+						 enum fsl_mc_pool_type
+								*pool_type)
+{
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(fsl_mc_pool_type_strings); i++) {
+		if (strcmp(object_type, fsl_mc_pool_type_strings[i]) == 0) {
+			*pool_type = i;
+			return 0;
+		}
+	}
+
+	return -EINVAL;
+}
+
+int __must_check fsl_mc_resource_allocate(struct fsl_mc_bus *mc_bus,
+					  enum fsl_mc_pool_type pool_type,
+					  struct fsl_mc_resource **new_resource)
+{
+	struct fsl_mc_resource_pool *res_pool;
+	struct fsl_mc_resource *resource;
+	struct fsl_mc_device *mc_bus_dev = &mc_bus->mc_dev;
+	int error = -EINVAL;
+	bool mutex_locked = false;
+
+	BUILD_BUG_ON(ARRAY_SIZE(fsl_mc_pool_type_strings) !=
+		     FSL_MC_NUM_POOL_TYPES);
+
+	*new_resource = NULL;
+	if (WARN_ON(pool_type < 0 || pool_type >= FSL_MC_NUM_POOL_TYPES))
+		goto error;
+
+	res_pool = &mc_bus->resource_pools[pool_type];
+	if (WARN_ON(res_pool->mc_bus != mc_bus))
+		goto error;
+
+	mutex_lock(&res_pool->mutex);
+	mutex_locked = true;
+	resource = list_first_entry_or_null(&res_pool->free_list,
+					    struct fsl_mc_resource, node);
+
+	if (!resource) {
+		WARN_ON(res_pool->free_count != 0);
+		error = -ENXIO;
+		dev_err(&mc_bus_dev->dev,
+			"No more resources of type %s left\n",
+			fsl_mc_pool_type_strings[pool_type]);
+		goto error;
+	}
+
+	if (WARN_ON(resource->type != pool_type))
+		goto error;
+	if (WARN_ON(resource->parent_pool != res_pool))
+		goto error;
+	if (WARN_ON(res_pool->free_count <= 0 ||
+		    res_pool->free_count > res_pool->max_count))
+		goto error;
+
+	list_del(&resource->node);
+	INIT_LIST_HEAD(&resource->node);
+
+	res_pool->free_count--;
+	mutex_unlock(&res_pool->mutex);
+	*new_resource = resource;
+	return 0;
+error:
+	if (mutex_locked)
+		mutex_unlock(&res_pool->mutex);
+
+	return error;
+}
+EXPORT_SYMBOL_GPL(fsl_mc_resource_allocate);
+
+void fsl_mc_resource_free(struct fsl_mc_resource *resource)
+{
+	struct fsl_mc_resource_pool *res_pool;
+	bool mutex_locked = false;
+
+	res_pool = resource->parent_pool;
+	if (WARN_ON(resource->type != res_pool->type))
+		goto out;
+
+	mutex_lock(&res_pool->mutex);
+	mutex_locked = true;
+	if (WARN_ON(res_pool->free_count < 0 ||
+		    res_pool->free_count >= res_pool->max_count))
+		goto out;
+
+	if (WARN_ON(!list_empty(&resource->node)))
+		goto out;
+
+	list_add_tail(&resource->node, &res_pool->free_list);
+	res_pool->free_count++;
+out:
+	if (mutex_locked)
+		mutex_unlock(&res_pool->mutex);
+}
+EXPORT_SYMBOL_GPL(fsl_mc_resource_free);
+
+/**
+ * fsl_mc_portal_allocate - Allocates an MC portal
+ *
+ * @mc_dev: MC device for which the MC portal is to be allocated
+ * @mc_io_flags: Flags for the fsl_mc_io object that wraps the allocated
+ * MC portal.
+ * @new_mc_io: Pointer to area where the pointer to the fsl_mc_io object
+ * that wraps the allocated MC portal is to be returned
+ *
+ * This function allocates an MC portal from the device's parent DPRC,
+ * from the corresponding MC bus' pool of MC portals and wraps
+ * it in a new fsl_mc_io object. If 'mc_dev' is a DPRC itself, the
+ * portal is allocated from its own MC bus.
+ */
+int __must_check fsl_mc_portal_allocate(struct fsl_mc_device *mc_dev,
+					uint16_t mc_io_flags,
+					struct fsl_mc_io **new_mc_io)
+{
+	struct fsl_mc_device *mc_bus_dev;
+	struct fsl_mc_bus *mc_bus;
+	phys_addr_t mc_portal_phys_addr;
+	size_t mc_portal_size;
+	struct fsl_mc_device *mc_adev;
+	int error = -EINVAL;
+	struct fsl_mc_resource *resource = NULL;
+	struct fsl_mc_io *mc_io = NULL;
+
+	if (mc_dev->flags & FSL_MC_IS_DPRC) {
+		mc_bus_dev = mc_dev;
+	} else {
+		if (WARN_ON(mc_dev->dev.parent->bus != &fsl_mc_bus_type))
+			return error;
+
+		mc_bus_dev = to_fsl_mc_device(mc_dev->dev.parent);
+	}
+
+	mc_bus = to_fsl_mc_bus(mc_bus_dev);
+	*new_mc_io = NULL;
+	error = fsl_mc_resource_allocate(mc_bus, FSL_MC_POOL_DPMCP, &resource);
+	if (error < 0)
+		return error;
+
+	mc_adev = resource->data;
+	if (WARN_ON(!mc_adev))
+		goto error_cleanup_resource;
+
+	if (WARN_ON(mc_adev->obj_desc.region_count == 0))
+		goto error_cleanup_resource;
+
+	mc_portal_phys_addr = mc_adev->regions[0].start;
+	mc_portal_size = mc_adev->regions[0].end -
+			 mc_adev->regions[0].start + 1;
+
+	if (WARN_ON(mc_portal_size != mc_bus_dev->mc_io->portal_size))
+		goto error_cleanup_resource;
+
+	error = fsl_create_mc_io(&mc_bus_dev->dev,
+				 mc_portal_phys_addr,
+				 mc_portal_size, resource,
+				 mc_io_flags, &mc_io);
+	if (error < 0)
+		goto error_cleanup_resource;
+
+	*new_mc_io = mc_io;
+	return 0;
+
+error_cleanup_resource:
+	fsl_mc_resource_free(resource);
+	return error;
+}
+EXPORT_SYMBOL_GPL(fsl_mc_portal_allocate);
+
+/**
+ * fsl_mc_portal_free - Returns an MC portal to the pool of free MC portals
+ * of a given MC bus
+ *
+ * @mc_io: Pointer to the fsl_mc_io object that wraps the MC portal to free
+ */
+void fsl_mc_portal_free(struct fsl_mc_io *mc_io)
+{
+	struct fsl_mc_resource *resource;
+
+	resource = mc_io->resource;
+	if (WARN_ON(resource->type != FSL_MC_POOL_DPMCP))
+		return;
+	if (WARN_ON(!resource->data))
+		return;
+
+	fsl_destroy_mc_io(mc_io);
+	fsl_mc_resource_free(resource);
+}
+EXPORT_SYMBOL_GPL(fsl_mc_portal_free);
+
+/**
+ * fsl_mc_portal_reset - Resets the dpmcp object for a given fsl_mc_io object
+ *
+ * @mc_io: Pointer to the fsl_mc_io object that wraps the MC portal to free
+ */
+int fsl_mc_portal_reset(struct fsl_mc_io *mc_io)
+{
+	int error;
+	uint16_t token;
+	struct fsl_mc_resource *resource = mc_io->resource;
+	struct fsl_mc_device *mc_dev = resource->data;
+
+	if (WARN_ON(resource->type != FSL_MC_POOL_DPMCP))
+		return -EINVAL;
+
+	if (WARN_ON(!mc_dev))
+		return -EINVAL;
+
+	error = dpmcp_open(mc_io, mc_dev->obj_desc.id, &token);
+	if (error < 0) {
+		dev_err(&mc_dev->dev, "dpmcp_open() failed: %d\n", error);
+		return error;
+	}
+
+	error = dpmcp_reset(mc_io, token);
+	if (error < 0) {
+		dev_err(&mc_dev->dev, "dpmcp_reset() failed: %d\n", error);
+		return error;
+	}
+
+	error = dpmcp_close(mc_io, token);
+	if (error < 0) {
+		dev_err(&mc_dev->dev, "dpmcp_close() failed: %d\n", error);
+		return error;
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(fsl_mc_portal_reset);
+
+/**
+ * fsl_mc_object_allocate - Allocates a MC object device of the given
+ * pool type from a given MC bus
+ *
+ * @mc_dev: MC device for which the MC object device is to be allocated
+ * @pool_type: MC bus resource pool type
+ * @new_mc_dev: Pointer to area where the pointer to the allocated
+ * MC object device is to be returned
+ *
+ * This function allocates a MC object device from the device's parent DPRC,
+ * from the corresponding MC bus' pool of allocatable MC object devices of
+ * the given resource type. mc_dev cannot be a DPRC itself.
+ *
+ * NOTE: pool_type must be different from FSL_MC_POOL_MCP, since MC
+ * portals are allocated using fsl_mc_portal_allocate(), instead of
+ * this function.
+ */
+int __must_check fsl_mc_object_allocate(struct fsl_mc_device *mc_dev,
+					enum fsl_mc_pool_type pool_type,
+					struct fsl_mc_device **new_mc_adev)
+{
+	struct fsl_mc_device *mc_bus_dev;
+	struct fsl_mc_bus *mc_bus;
+	struct fsl_mc_device *mc_adev;
+	int error = -EINVAL;
+	struct fsl_mc_resource *resource = NULL;
+
+	*new_mc_adev = NULL;
+	if (WARN_ON(mc_dev->flags & FSL_MC_IS_DPRC))
+		goto error;
+
+	if (WARN_ON(mc_dev->dev.parent->bus != &fsl_mc_bus_type))
+		goto error;
+
+	if (WARN_ON(pool_type == FSL_MC_POOL_DPMCP))
+		goto error;
+
+	mc_bus_dev = to_fsl_mc_device(mc_dev->dev.parent);
+	mc_bus = to_fsl_mc_bus(mc_bus_dev);
+	error = fsl_mc_resource_allocate(mc_bus, pool_type, &resource);
+	if (error < 0)
+		goto error;
+
+	mc_adev = resource->data;
+	if (WARN_ON(!mc_adev))
+		goto error;
+
+	*new_mc_adev = mc_adev;
+	return 0;
+error:
+	if (resource)
+		fsl_mc_resource_free(resource);
+
+	return error;
+}
+EXPORT_SYMBOL_GPL(fsl_mc_object_allocate);
+
+/**
+ * fsl_mc_object_free - Returns an allocatable MC object device to the
+ * corresponding resource pool of a given MC bus.
+ *
+ * @mc_adev: Pointer to the MC object device
+ */
+void fsl_mc_object_free(struct fsl_mc_device *mc_adev)
+{
+	struct fsl_mc_resource *resource;
+
+	resource = mc_adev->resource;
+	if (WARN_ON(resource->type == FSL_MC_POOL_DPMCP))
+		return;
+	if (WARN_ON(resource->data != mc_adev))
+		return;
+
+	fsl_mc_resource_free(resource);
+}
+EXPORT_SYMBOL_GPL(fsl_mc_object_free);
+
+/**
+ * fsl_mc_allocator_probe - callback invoked when an allocatable device is
+ * being added to the system
+ */
+static int fsl_mc_allocator_probe(struct fsl_mc_device *mc_dev)
+{
+	enum fsl_mc_pool_type pool_type;
+	struct fsl_mc_device *mc_bus_dev;
+	struct fsl_mc_bus *mc_bus;
+	int error = -EINVAL;
+
+	if (WARN_ON(!FSL_MC_IS_ALLOCATABLE(mc_dev->obj_desc.type)))
+		goto error;
+
+	mc_bus_dev = to_fsl_mc_device(mc_dev->dev.parent);
+	if (WARN_ON(mc_bus_dev->dev.bus != &fsl_mc_bus_type))
+		goto error;
+
+	mc_bus = to_fsl_mc_bus(mc_bus_dev);
+	error = object_type_to_pool_type(mc_dev->obj_desc.type, &pool_type);
+	if (error < 0)
+		goto error;
+
+	error = fsl_mc_resource_pool_add_device(mc_bus, pool_type, mc_dev);
+	if (error < 0)
+		goto error;
+
+	dev_info(&mc_dev->dev,
+		 "Allocatable MC object device bound to fsl_mc_allocator driver");
+	return 0;
+error:
+
+	return error;
+}
+
+/**
+ * fsl_mc_allocator_remove - callback invoked when an allocatable device is
+ * being removed from the system
+ */
+static int fsl_mc_allocator_remove(struct fsl_mc_device *mc_dev)
+{
+	int error = -EINVAL;
+
+	if (WARN_ON(!FSL_MC_IS_ALLOCATABLE(mc_dev->obj_desc.type)))
+		goto out;
+
+	error = fsl_mc_resource_pool_remove_device(mc_dev);
+	if (error < 0)
+		goto out;
+
+	dev_info(&mc_dev->dev,
+		 "Allocatable MC object device unbound from fsl_mc_allocator driver");
+	error = 0;
+out:
+	return error;
+}
+
+static const struct fsl_mc_device_match_id match_id_table[] = {
+	{
+	 .vendor = FSL_MC_VENDOR_FREESCALE,
+	 .obj_type = "dpbp",
+	 .ver_major = DPBP_VER_MAJOR,
+	 .ver_minor = DPBP_VER_MINOR
+	},
+	{
+	 .vendor = FSL_MC_VENDOR_FREESCALE,
+	 .obj_type = "dpmcp",
+	 .ver_major = DPMCP_VER_MAJOR,
+	 .ver_minor = DPMCP_VER_MINOR
+	},
+	{
+	 .vendor = FSL_MC_VENDOR_FREESCALE,
+	 .obj_type = "dpcon",
+	 .ver_major = DPCON_VER_MAJOR,
+	 .ver_minor = DPCON_VER_MINOR
+	},
+	{.vendor = 0x0},
+};
+
+static struct fsl_mc_driver fsl_mc_allocator_driver = {
+	.driver = {
+		   .name = "fsl_mc_allocator",
+		   .owner = THIS_MODULE,
+		   .pm = NULL,
+		   },
+	.match_id_table = match_id_table,
+	.probe = fsl_mc_allocator_probe,
+	.remove = fsl_mc_allocator_remove,
+};
+
+int __init fsl_mc_allocator_driver_init(void)
+{
+	return fsl_mc_driver_register(&fsl_mc_allocator_driver);
+}
+
+void __exit fsl_mc_allocator_driver_exit(void)
+{
+	fsl_mc_driver_unregister(&fsl_mc_allocator_driver);
+}
diff --git a/drivers/staging/fsl-mc/bus/mc-bus.c b/drivers/staging/fsl-mc/bus/mc-bus.c
new file mode 100644
index 000000000000..23512d096427
--- /dev/null
+++ b/drivers/staging/fsl-mc/bus/mc-bus.c
@@ -0,0 +1,793 @@
+/*
+ * Freescale Management Complex (MC) bus driver
+ *
+ * Copyright (C) 2014 Freescale Semiconductor, Inc.
+ * Author: German Rivera <German.Rivera@freescale.com>
+ *
+ * This file is licensed under the terms of the GNU General Public
+ * License version 2. This program is licensed "as is" without any
+ * warranty of any kind, whether express or implied.
+ */
+
+#include "../include/mc-private.h"
+#include <linux/module.h>
+#include <linux/of_device.h>
+#include <linux/of_address.h>
+#include <linux/ioport.h>
+#include <linux/slab.h>
+#include <linux/limits.h>
+#include "../include/dpmng.h"
+#include "../include/mc-sys.h"
+#include "dprc-cmd.h"
+
+static struct kmem_cache *mc_dev_cache;
+
+/**
+ * fsl_mc_bus_match - device to driver matching callback
+ * @dev: the MC object device structure to match against
+ * @drv: the device driver to search for matching MC object device id
+ * structures
+ *
+ * Returns 1 on success, 0 otherwise.
+ */
+static int fsl_mc_bus_match(struct device *dev, struct device_driver *drv)
+{
+	const struct fsl_mc_device_match_id *id;
+	struct fsl_mc_device *mc_dev = to_fsl_mc_device(dev);
+	struct fsl_mc_driver *mc_drv = to_fsl_mc_driver(drv);
+	bool found = false;
+	bool major_version_mismatch = false;
+	bool minor_version_mismatch = false;
+
+	if (WARN_ON(!fsl_mc_bus_type.dev_root))
+		goto out;
+
+	if (!mc_drv->match_id_table)
+		goto out;
+
+	/*
+	 * If the object is not 'plugged' don't match.
+	 * Only exception is the root DPRC, which is a special case.
+	 */
+	if ((mc_dev->obj_desc.state & DPRC_OBJ_STATE_PLUGGED) == 0 &&
+	    &mc_dev->dev != fsl_mc_bus_type.dev_root)
+		goto out;
+
+	/*
+	 * Traverse the match_id table of the given driver, trying to find
+	 * a matching for the given MC object device.
+	 */
+	for (id = mc_drv->match_id_table; id->vendor != 0x0; id++) {
+		if (id->vendor == mc_dev->obj_desc.vendor &&
+		    strcmp(id->obj_type, mc_dev->obj_desc.type) == 0) {
+			if (id->ver_major == mc_dev->obj_desc.ver_major) {
+				found = true;
+				if (id->ver_minor != mc_dev->obj_desc.ver_minor)
+					minor_version_mismatch = true;
+			} else {
+				major_version_mismatch = true;
+			}
+
+			break;
+		}
+	}
+
+	if (major_version_mismatch) {
+		dev_warn(dev,
+			 "Major version mismatch: driver version %u.%u, MC object version %u.%u\n",
+			 id->ver_major, id->ver_minor,
+			 mc_dev->obj_desc.ver_major,
+			 mc_dev->obj_desc.ver_minor);
+	} else if (minor_version_mismatch) {
+		dev_warn(dev,
+			 "Minor version mismatch: driver version %u.%u, MC object version %u.%u\n",
+			 id->ver_major, id->ver_minor,
+			 mc_dev->obj_desc.ver_major,
+			 mc_dev->obj_desc.ver_minor);
+	}
+
+out:
+	dev_dbg(dev, "%smatched\n", found ? "" : "not ");
+	return found;
+}
+
+/**
+ * fsl_mc_bus_uevent - callback invoked when a device is added
+ */
+static int fsl_mc_bus_uevent(struct device *dev, struct kobj_uevent_env *env)
+{
+	pr_debug("%s invoked\n", __func__);
+	return 0;
+}
+
+struct bus_type fsl_mc_bus_type = {
+	.name = "fsl-mc",
+	.match = fsl_mc_bus_match,
+	.uevent = fsl_mc_bus_uevent,
+};
+EXPORT_SYMBOL_GPL(fsl_mc_bus_type);
+
+static int fsl_mc_driver_probe(struct device *dev)
+{
+	struct fsl_mc_driver *mc_drv;
+	struct fsl_mc_device *mc_dev = to_fsl_mc_device(dev);
+	int error;
+
+	if (WARN_ON(!dev->driver))
+		return -EINVAL;
+
+	mc_drv = to_fsl_mc_driver(dev->driver);
+	if (WARN_ON(!mc_drv->probe))
+		return -EINVAL;
+
+	error = mc_drv->probe(mc_dev);
+	if (error < 0) {
+		dev_err(dev, "MC object device probe callback failed: %d\n",
+			error);
+		return error;
+	}
+
+	return 0;
+}
+
+static int fsl_mc_driver_remove(struct device *dev)
+{
+	struct fsl_mc_driver *mc_drv = to_fsl_mc_driver(dev->driver);
+	struct fsl_mc_device *mc_dev = to_fsl_mc_device(dev);
+	int error;
+
+	if (WARN_ON(!dev->driver))
+		return -EINVAL;
+
+	error = mc_drv->remove(mc_dev);
+	if (error < 0) {
+		dev_err(dev,
+			"MC object device remove callback failed: %d\n",
+			error);
+		return error;
+	}
+
+	return 0;
+}
+
+static void fsl_mc_driver_shutdown(struct device *dev)
+{
+	struct fsl_mc_driver *mc_drv = to_fsl_mc_driver(dev->driver);
+	struct fsl_mc_device *mc_dev = to_fsl_mc_device(dev);
+
+	mc_drv->shutdown(mc_dev);
+}
+
+/**
+ * __fsl_mc_driver_register - registers a child device driver with the
+ * MC bus
+ *
+ * This function is implicitly invoked from the registration function of
+ * fsl_mc device drivers, which is generated by the
+ * module_fsl_mc_driver() macro.
+ */
+int __fsl_mc_driver_register(struct fsl_mc_driver *mc_driver,
+			     struct module *owner)
+{
+	int error;
+
+	mc_driver->driver.owner = owner;
+	mc_driver->driver.bus = &fsl_mc_bus_type;
+
+	if (mc_driver->probe)
+		mc_driver->driver.probe = fsl_mc_driver_probe;
+
+	if (mc_driver->remove)
+		mc_driver->driver.remove = fsl_mc_driver_remove;
+
+	if (mc_driver->shutdown)
+		mc_driver->driver.shutdown = fsl_mc_driver_shutdown;
+
+	error = driver_register(&mc_driver->driver);
+	if (error < 0) {
+		pr_err("driver_register() failed for %s: %d\n",
+		       mc_driver->driver.name, error);
+		return error;
+	}
+
+	pr_info("MC object device driver %s registered\n",
+		mc_driver->driver.name);
+	return 0;
+}
+EXPORT_SYMBOL_GPL(__fsl_mc_driver_register);
+
+/**
+ * fsl_mc_driver_unregister - unregisters a device driver from the
+ * MC bus
+ */
+void fsl_mc_driver_unregister(struct fsl_mc_driver *mc_driver)
+{
+	driver_unregister(&mc_driver->driver);
+}
+EXPORT_SYMBOL_GPL(fsl_mc_driver_unregister);
+
+static int get_dprc_icid(struct fsl_mc_io *mc_io,
+			 int container_id, uint16_t *icid)
+{
+	uint16_t dprc_handle;
+	struct dprc_attributes attr;
+	int error;
+
+	error = dprc_open(mc_io, container_id, &dprc_handle);
+	if (error < 0) {
+		pr_err("dprc_open() failed: %d\n", error);
+		return error;
+	}
+
+	memset(&attr, 0, sizeof(attr));
+	error = dprc_get_attributes(mc_io, dprc_handle, &attr);
+	if (error < 0) {
+		pr_err("dprc_get_attributes() failed: %d\n", error);
+		goto common_cleanup;
+	}
+
+	*icid = attr.icid;
+	error = 0;
+
+common_cleanup:
+	(void)dprc_close(mc_io, dprc_handle);
+	return error;
+}
+
+static int translate_mc_addr(uint64_t mc_addr, phys_addr_t *phys_addr)
+{
+	int i;
+	struct fsl_mc *mc = dev_get_drvdata(fsl_mc_bus_type.dev_root->parent);
+
+	if (mc->num_translation_ranges == 0) {
+		/*
+		 * Do identity mapping:
+		 */
+		*phys_addr = mc_addr;
+		return 0;
+	}
+
+	for (i = 0; i < mc->num_translation_ranges; i++) {
+		struct fsl_mc_addr_translation_range *range =
+			&mc->translation_ranges[i];
+
+		if (mc_addr >= range->start_mc_addr &&
+		    mc_addr < range->end_mc_addr) {
+			*phys_addr = range->start_phys_addr +
+				     (mc_addr - range->start_mc_addr);
+			return 0;
+		}
+	}
+
+	return -EFAULT;
+}
+
+static int fsl_mc_device_get_mmio_regions(struct fsl_mc_device *mc_dev,
+					  struct fsl_mc_device *mc_bus_dev)
+{
+	int i;
+	int error;
+	struct resource *regions;
+	struct dprc_obj_desc *obj_desc = &mc_dev->obj_desc;
+	struct device *parent_dev = mc_dev->dev.parent;
+
+	regions = kmalloc_array(obj_desc->region_count,
+				sizeof(regions[0]), GFP_KERNEL);
+	if (!regions)
+		return -ENOMEM;
+
+	for (i = 0; i < obj_desc->region_count; i++) {
+		struct dprc_region_desc region_desc;
+
+		error = dprc_get_obj_region(mc_bus_dev->mc_io,
+					    mc_bus_dev->mc_handle,
+					    obj_desc->type,
+					    obj_desc->id, i, &region_desc);
+		if (error < 0) {
+			dev_err(parent_dev,
+				"dprc_get_obj_region() failed: %d\n", error);
+			goto error_cleanup_regions;
+		}
+
+		WARN_ON(region_desc.base_paddr == 0x0);
+		WARN_ON(region_desc.size == 0);
+		error = translate_mc_addr(region_desc.base_paddr,
+					  &regions[i].start);
+		if (error < 0) {
+			dev_err(parent_dev,
+				"Invalid MC address: %#llx\n",
+				region_desc.base_paddr);
+			goto error_cleanup_regions;
+		}
+
+		regions[i].end = regions[i].start + region_desc.size - 1;
+		regions[i].name = "fsl-mc object MMIO region";
+		regions[i].flags = IORESOURCE_IO;
+	}
+
+	mc_dev->regions = regions;
+	return 0;
+
+error_cleanup_regions:
+	kfree(regions);
+	return error;
+}
+
+/**
+ * Add a newly discovered MC object device to be visible in Linux
+ */
+int fsl_mc_device_add(struct dprc_obj_desc *obj_desc,
+		      struct fsl_mc_io *mc_io,
+		      struct device *parent_dev,
+		      struct fsl_mc_device **new_mc_dev)
+{
+	int error;
+	struct fsl_mc_device *mc_dev = NULL;
+	struct fsl_mc_bus *mc_bus = NULL;
+	struct fsl_mc_device *parent_mc_dev;
+
+	if (parent_dev->bus == &fsl_mc_bus_type)
+		parent_mc_dev = to_fsl_mc_device(parent_dev);
+	else
+		parent_mc_dev = NULL;
+
+	if (strcmp(obj_desc->type, "dprc") == 0) {
+		/*
+		 * Allocate an MC bus device object:
+		 */
+		mc_bus = devm_kzalloc(parent_dev, sizeof(*mc_bus), GFP_KERNEL);
+		if (!mc_bus)
+			return -ENOMEM;
+
+		mc_dev = &mc_bus->mc_dev;
+	} else {
+		/*
+		 * Allocate a regular fsl_mc_device object:
+		 */
+		mc_dev = kmem_cache_zalloc(mc_dev_cache, GFP_KERNEL);
+		if (!mc_dev)
+			return -ENOMEM;
+	}
+
+	mc_dev->obj_desc = *obj_desc;
+	mc_dev->mc_io = mc_io;
+	device_initialize(&mc_dev->dev);
+	mc_dev->dev.parent = parent_dev;
+	mc_dev->dev.bus = &fsl_mc_bus_type;
+	dev_set_name(&mc_dev->dev, "%s.%d", obj_desc->type, obj_desc->id);
+
+	if (strcmp(obj_desc->type, "dprc") == 0) {
+		struct fsl_mc_io *mc_io2;
+
+		mc_dev->flags |= FSL_MC_IS_DPRC;
+
+		/*
+		 * To get the DPRC's ICID, we need to open the DPRC
+		 * in get_dprc_icid(). For child DPRCs, we do so using the
+		 * parent DPRC's MC portal instead of the child DPRC's MC
+		 * portal, in case the child DPRC is already opened with
+		 * its own portal (e.g., the DPRC used by AIOP).
+		 *
+		 * NOTE: There cannot be more than one active open for a
+		 * given MC object, using the same MC portal.
+		 */
+		if (parent_mc_dev) {
+			/*
+			 * device being added is a child DPRC device
+			 */
+			mc_io2 = parent_mc_dev->mc_io;
+		} else {
+			/*
+			 * device being added is the root DPRC device
+			 */
+			if (WARN_ON(!mc_io)) {
+				error = -EINVAL;
+				goto error_cleanup_dev;
+			}
+
+			mc_io2 = mc_io;
+
+			if (!fsl_mc_bus_type.dev_root)
+				fsl_mc_bus_type.dev_root = &mc_dev->dev;
+		}
+
+		error = get_dprc_icid(mc_io2, obj_desc->id, &mc_dev->icid);
+		if (error < 0)
+			goto error_cleanup_dev;
+	} else {
+		/*
+		 * A non-DPRC MC object device has to be a child of another
+		 * MC object (specifically a DPRC object)
+		 */
+		mc_dev->icid = parent_mc_dev->icid;
+		mc_dev->dma_mask = FSL_MC_DEFAULT_DMA_MASK;
+		mc_dev->dev.dma_mask = &mc_dev->dma_mask;
+	}
+
+	/*
+	 * Get MMIO regions for the device from the MC:
+	 *
+	 * NOTE: the root DPRC is a special case as its MMIO region is
+	 * obtained from the device tree
+	 */
+	if (parent_mc_dev && obj_desc->region_count != 0) {
+		error = fsl_mc_device_get_mmio_regions(mc_dev,
+						       parent_mc_dev);
+		if (error < 0)
+			goto error_cleanup_dev;
+	}
+
+	/*
+	 * The device-specific probe callback will get invoked by device_add()
+	 */
+	error = device_add(&mc_dev->dev);
+	if (error < 0) {
+		dev_err(parent_dev,
+			"device_add() failed for device %s: %d\n",
+			dev_name(&mc_dev->dev), error);
+		goto error_cleanup_dev;
+	}
+
+	(void)get_device(&mc_dev->dev);
+	dev_dbg(parent_dev, "Added MC object device %s\n",
+		dev_name(&mc_dev->dev));
+
+	*new_mc_dev = mc_dev;
+	return 0;
+
+error_cleanup_dev:
+	kfree(mc_dev->regions);
+	if (mc_bus)
+		devm_kfree(parent_dev, mc_bus);
+	else
+		kmem_cache_free(mc_dev_cache, mc_dev);
+
+	return error;
+}
+EXPORT_SYMBOL_GPL(fsl_mc_device_add);
+
+/**
+ * fsl_mc_device_remove - Remove a MC object device from being visible to
+ * Linux
+ *
+ * @mc_dev: Pointer to a MC object device object
+ */
+void fsl_mc_device_remove(struct fsl_mc_device *mc_dev)
+{
+	struct fsl_mc_bus *mc_bus = NULL;
+
+	kfree(mc_dev->regions);
+
+	/*
+	 * The device-specific remove callback will get invoked by device_del()
+	 */
+	device_del(&mc_dev->dev);
+	put_device(&mc_dev->dev);
+
+	if (strcmp(mc_dev->obj_desc.type, "dprc") == 0) {
+		mc_bus = to_fsl_mc_bus(mc_dev);
+		if (mc_dev->mc_io) {
+			fsl_destroy_mc_io(mc_dev->mc_io);
+			mc_dev->mc_io = NULL;
+		}
+
+		if (&mc_dev->dev == fsl_mc_bus_type.dev_root)
+			fsl_mc_bus_type.dev_root = NULL;
+	}
+
+	if (mc_bus)
+		devm_kfree(mc_dev->dev.parent, mc_bus);
+	else
+		kmem_cache_free(mc_dev_cache, mc_dev);
+}
+EXPORT_SYMBOL_GPL(fsl_mc_device_remove);
+
+static int parse_mc_ranges(struct device *dev,
+			   int *paddr_cells,
+			   int *mc_addr_cells,
+			   int *mc_size_cells,
+			   const __be32 **ranges_start,
+			   uint8_t *num_ranges)
+{
+	const __be32 *prop;
+	int range_tuple_cell_count;
+	int ranges_len;
+	int tuple_len;
+	struct device_node *mc_node = dev->of_node;
+
+	*ranges_start = of_get_property(mc_node, "ranges", &ranges_len);
+	if (!(*ranges_start) || !ranges_len) {
+		dev_warn(dev,
+			 "missing or empty ranges property for device tree node '%s'\n",
+			 mc_node->name);
+
+		*num_ranges = 0;
+		return 0;
+	}
+
+	*paddr_cells = of_n_addr_cells(mc_node);
+
+	prop = of_get_property(mc_node, "#address-cells", NULL);
+	if (prop)
+		*mc_addr_cells = be32_to_cpup(prop);
+	else
+		*mc_addr_cells = *paddr_cells;
+
+	prop = of_get_property(mc_node, "#size-cells", NULL);
+	if (prop)
+		*mc_size_cells = be32_to_cpup(prop);
+	else
+		*mc_size_cells = of_n_size_cells(mc_node);
+
+	range_tuple_cell_count = *paddr_cells + *mc_addr_cells +
+				 *mc_size_cells;
+
+	tuple_len = range_tuple_cell_count * sizeof(__be32);
+	if (ranges_len % tuple_len != 0) {
+		dev_err(dev, "malformed ranges property '%s'\n", mc_node->name);
+		return -EINVAL;
+	}
+
+	*num_ranges = ranges_len / tuple_len;
+	return 0;
+}
+
+static int get_mc_addr_translation_ranges(struct device *dev,
+					  struct fsl_mc_addr_translation_range
+						**ranges,
+					  uint8_t *num_ranges)
+{
+	int error;
+	int paddr_cells;
+	int mc_addr_cells;
+	int mc_size_cells;
+	int i;
+	const __be32 *ranges_start;
+	const __be32 *cell;
+
+	error = parse_mc_ranges(dev,
+				&paddr_cells,
+				&mc_addr_cells,
+				&mc_size_cells,
+				&ranges_start,
+				num_ranges);
+	if (error < 0)
+		return error;
+
+	if (!(*num_ranges)) {
+		/*
+		 * Missing or empty ranges property ("ranges;") for the
+		 * 'fsl,qoriq-mc' node. In this case, identity mapping
+		 * will be used.
+		 */
+		*ranges = NULL;
+		return 0;
+	}
+
+	*ranges = devm_kcalloc(dev, *num_ranges,
+			       sizeof(struct fsl_mc_addr_translation_range),
+			       GFP_KERNEL);
+	if (!(*ranges))
+		return -ENOMEM;
+
+	cell = ranges_start;
+	for (i = 0; i < *num_ranges; ++i) {
+		struct fsl_mc_addr_translation_range *range = &(*ranges)[i];
+
+		range->start_mc_addr = of_read_number(cell, mc_addr_cells);
+		cell += mc_addr_cells;
+		range->start_phys_addr = of_read_number(cell, paddr_cells);
+		cell += paddr_cells;
+		range->end_mc_addr = range->start_mc_addr +
+				     of_read_number(cell, mc_size_cells);
+
+		cell += mc_size_cells;
+	}
+
+	return 0;
+}
+
+/**
+ * fsl_mc_bus_probe - callback invoked when the root MC bus is being
+ * added
+ */
+static int fsl_mc_bus_probe(struct platform_device *pdev)
+{
+	struct dprc_obj_desc obj_desc;
+	int error;
+	struct fsl_mc *mc;
+	struct fsl_mc_device *mc_bus_dev = NULL;
+	struct fsl_mc_io *mc_io = NULL;
+	int container_id;
+	phys_addr_t mc_portal_phys_addr;
+	uint32_t mc_portal_size;
+	struct mc_version mc_version;
+	struct resource res;
+
+	dev_info(&pdev->dev, "Root MC bus device probed");
+
+	mc = devm_kzalloc(&pdev->dev, sizeof(*mc), GFP_KERNEL);
+	if (!mc)
+		return -ENOMEM;
+
+	platform_set_drvdata(pdev, mc);
+
+	/*
+	 * Get physical address of MC portal for the root DPRC:
+	 */
+	error = of_address_to_resource(pdev->dev.of_node, 0, &res);
+	if (error < 0) {
+		dev_err(&pdev->dev,
+			"of_address_to_resource() failed for %s\n",
+			pdev->dev.of_node->full_name);
+		return error;
+	}
+
+	mc_portal_phys_addr = res.start;
+	mc_portal_size = resource_size(&res);
+	error = fsl_create_mc_io(&pdev->dev, mc_portal_phys_addr,
+				 mc_portal_size, NULL, 0, &mc_io);
+	if (error < 0)
+		return error;
+
+	error = mc_get_version(mc_io, &mc_version);
+	if (error != 0) {
+		dev_err(&pdev->dev,
+			"mc_get_version() failed with error %d\n", error);
+		goto error_cleanup_mc_io;
+	}
+
+	dev_info(&pdev->dev,
+		 "Freescale Management Complex Firmware version: %u.%u.%u\n",
+		 mc_version.major, mc_version.minor, mc_version.revision);
+
+	if (mc_version.major < MC_VER_MAJOR) {
+		dev_err(&pdev->dev,
+			"ERROR: MC firmware version not supported by driver (driver version: %u.%u)\n",
+			MC_VER_MAJOR, MC_VER_MINOR);
+		error = -ENOTSUPP;
+		goto error_cleanup_mc_io;
+	}
+
+	if (mc_version.major > MC_VER_MAJOR) {
+		dev_warn(&pdev->dev,
+			 "WARNING: driver may not support newer MC firmware features (driver version: %u.%u)\n",
+			 MC_VER_MAJOR, MC_VER_MINOR);
+	}
+
+	error = get_mc_addr_translation_ranges(&pdev->dev,
+					       &mc->translation_ranges,
+					       &mc->num_translation_ranges);
+	if (error < 0)
+		goto error_cleanup_mc_io;
+
+	error = dpmng_get_container_id(mc_io, &container_id);
+	if (error < 0) {
+		dev_err(&pdev->dev,
+			"dpmng_get_container_id() failed: %d\n", error);
+		goto error_cleanup_mc_io;
+	}
+
+	obj_desc.vendor = FSL_MC_VENDOR_FREESCALE;
+	strcpy(obj_desc.type, "dprc");
+	obj_desc.id = container_id;
+	obj_desc.ver_major = DPRC_VER_MAJOR;
+	obj_desc.ver_minor = DPRC_VER_MINOR;
+	obj_desc.region_count = 0;
+
+	error = fsl_mc_device_add(&obj_desc, mc_io, &pdev->dev, &mc_bus_dev);
+	if (error < 0)
+		goto error_cleanup_mc_io;
+
+	mc->root_mc_bus_dev = mc_bus_dev;
+	return 0;
+
+error_cleanup_mc_io:
+	fsl_destroy_mc_io(mc_io);
+	return error;
+}
+
+/**
+ * fsl_mc_bus_remove - callback invoked when the root MC bus is being
+ * removed
+ */
+static int fsl_mc_bus_remove(struct platform_device *pdev)
+{
+	struct fsl_mc *mc = platform_get_drvdata(pdev);
+
+	if (WARN_ON(&mc->root_mc_bus_dev->dev != fsl_mc_bus_type.dev_root))
+		return -EINVAL;
+
+	fsl_mc_device_remove(mc->root_mc_bus_dev);
+	dev_info(&pdev->dev, "Root MC bus device removed");
+	return 0;
+}
+
+static const struct of_device_id fsl_mc_bus_match_table[] = {
+	{.compatible = "fsl,qoriq-mc",},
+	{},
+};
+
+MODULE_DEVICE_TABLE(of, fsl_mc_bus_match_table);
+
+static struct platform_driver fsl_mc_bus_driver = {
+	.driver = {
+		   .name = "fsl_mc_bus",
+		   .owner = THIS_MODULE,
+		   .pm = NULL,
+		   .of_match_table = fsl_mc_bus_match_table,
+		   },
+	.probe = fsl_mc_bus_probe,
+	.remove = fsl_mc_bus_remove,
+};
+
+static int __init fsl_mc_bus_driver_init(void)
+{
+	int error;
+
+	mc_dev_cache = kmem_cache_create("fsl_mc_device",
+					 sizeof(struct fsl_mc_device), 0, 0,
+					 NULL);
+	if (!mc_dev_cache) {
+		pr_err("Could not create fsl_mc_device cache\n");
+		return -ENOMEM;
+	}
+
+	error = bus_register(&fsl_mc_bus_type);
+	if (error < 0) {
+		pr_err("fsl-mc bus type registration failed: %d\n", error);
+		goto error_cleanup_cache;
+	}
+
+	pr_info("fsl-mc bus type registered\n");
+
+	error = platform_driver_register(&fsl_mc_bus_driver);
+	if (error < 0) {
+		pr_err("platform_driver_register() failed: %d\n", error);
+		goto error_cleanup_bus;
+	}
+
+	error = dprc_driver_init();
+	if (error < 0)
+		goto error_cleanup_driver;
+
+	error = fsl_mc_allocator_driver_init();
+	if (error < 0)
+		goto error_cleanup_dprc_driver;
+
+	return 0;
+
+error_cleanup_dprc_driver:
+	dprc_driver_exit();
+
+error_cleanup_driver:
+	platform_driver_unregister(&fsl_mc_bus_driver);
+
+error_cleanup_bus:
+	bus_unregister(&fsl_mc_bus_type);
+
+error_cleanup_cache:
+	kmem_cache_destroy(mc_dev_cache);
+	return error;
+}
+
+postcore_initcall(fsl_mc_bus_driver_init);
+
+static void __exit fsl_mc_bus_driver_exit(void)
+{
+	if (WARN_ON(!mc_dev_cache))
+		return;
+
+	fsl_mc_allocator_driver_exit();
+	dprc_driver_exit();
+	platform_driver_unregister(&fsl_mc_bus_driver);
+	bus_unregister(&fsl_mc_bus_type);
+	kmem_cache_destroy(mc_dev_cache);
+	pr_info("MC bus unregistered\n");
+}
+
+module_exit(fsl_mc_bus_driver_exit);
+
+MODULE_AUTHOR("Freescale Semiconductor Inc.");
+MODULE_DESCRIPTION("Freescale Management Complex (MC) bus driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/fsl-mc/bus/mc-sys.c b/drivers/staging/fsl-mc/bus/mc-sys.c
new file mode 100644
index 000000000000..5737f599f0ef
--- /dev/null
+++ b/drivers/staging/fsl-mc/bus/mc-sys.c
@@ -0,0 +1,287 @@
+/* Copyright 2013-2014 Freescale Semiconductor Inc.
+ *
+ * I/O services to send MC commands to the MC hardware
+ *
+ * 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 of the above-listed copyright holders nor the
+ *       names of any contributors may be used to endorse or promote products
+ *       derived from this software without specific prior written permission.
+ *
+ *
+ * ALTERNATIVELY, this software may be distributed under the terms of the
+ * GNU General Public License ("GPL") as published by the Free Software
+ * Foundation, either version 2 of that License or (at your option) any
+ * later version.
+ *
+ * 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 HOLDERS 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 "../include/mc-sys.h"
+#include "../include/mc-cmd.h"
+#include <linux/delay.h>
+#include <linux/slab.h>
+#include <linux/ioport.h>
+#include <linux/device.h>
+
+/**
+ * Timeout in jiffies to wait for the completion of an MC command
+ */
+#define MC_CMD_COMPLETION_TIMEOUT_JIFFIES   (HZ / 2)	/* 500 ms */
+
+/*
+ * usleep_range() min and max values used to throttle down polling
+ * iterations while waiting for MC command completion
+ */
+#define MC_CMD_COMPLETION_POLLING_MIN_SLEEP_USECS    10
+#define MC_CMD_COMPLETION_POLLING_MAX_SLEEP_USECS    500
+
+#define MC_CMD_HDR_READ_CMDID(_hdr) \
+	((uint16_t)mc_dec((_hdr), MC_CMD_HDR_CMDID_O, MC_CMD_HDR_CMDID_S))
+
+/**
+ * Creates an MC I/O object
+ *
+ * @dev: device to be associated with the MC I/O object
+ * @mc_portal_phys_addr: physical address of the MC portal to use
+ * @mc_portal_size: size in bytes of the MC portal
+ * @resource: Pointer to MC bus object allocator resource associated
+ * with this MC I/O object or NULL if none.
+ * @flags: flags for the new MC I/O object
+ * @new_mc_io: Area to return pointer to newly created MC I/O object
+ *
+ * Returns '0' on Success; Error code otherwise.
+ */
+int __must_check fsl_create_mc_io(struct device *dev,
+				  phys_addr_t mc_portal_phys_addr,
+				  uint32_t mc_portal_size,
+				  struct fsl_mc_resource *resource,
+				  uint32_t flags, struct fsl_mc_io **new_mc_io)
+{
+	struct fsl_mc_io *mc_io;
+	void __iomem *mc_portal_virt_addr;
+	struct resource *res;
+
+	mc_io = devm_kzalloc(dev, sizeof(*mc_io), GFP_KERNEL);
+	if (!mc_io)
+		return -ENOMEM;
+
+	mc_io->dev = dev;
+	mc_io->flags = flags;
+	mc_io->portal_phys_addr = mc_portal_phys_addr;
+	mc_io->portal_size = mc_portal_size;
+	mc_io->resource = resource;
+	res = devm_request_mem_region(dev,
+				      mc_portal_phys_addr,
+				      mc_portal_size,
+				      "mc_portal");
+	if (!res) {
+		dev_err(dev,
+			"devm_request_mem_region failed for MC portal %#llx\n",
+			mc_portal_phys_addr);
+		return -EBUSY;
+	}
+
+	mc_portal_virt_addr = devm_ioremap_nocache(dev,
+						   mc_portal_phys_addr,
+						   mc_portal_size);
+	if (!mc_portal_virt_addr) {
+		dev_err(dev,
+			"devm_ioremap_nocache failed for MC portal %#llx\n",
+			mc_portal_phys_addr);
+		return -ENXIO;
+	}
+
+	mc_io->portal_virt_addr = mc_portal_virt_addr;
+	*new_mc_io = mc_io;
+	return 0;
+}
+EXPORT_SYMBOL_GPL(fsl_create_mc_io);
+
+/**
+ * Destroys an MC I/O object
+ *
+ * @mc_io: MC I/O object to destroy
+ */
+void fsl_destroy_mc_io(struct fsl_mc_io *mc_io)
+{
+	devm_iounmap(mc_io->dev, mc_io->portal_virt_addr);
+	devm_release_mem_region(mc_io->dev,
+				mc_io->portal_phys_addr,
+				mc_io->portal_size);
+
+	mc_io->portal_virt_addr = NULL;
+	devm_kfree(mc_io->dev, mc_io);
+}
+EXPORT_SYMBOL_GPL(fsl_destroy_mc_io);
+
+static int mc_status_to_error(enum mc_cmd_status status)
+{
+	static const int mc_status_to_error_map[] = {
+		[MC_CMD_STATUS_OK] = 0,
+		[MC_CMD_STATUS_AUTH_ERR] = -EACCES,
+		[MC_CMD_STATUS_NO_PRIVILEGE] = -EPERM,
+		[MC_CMD_STATUS_DMA_ERR] = -EIO,
+		[MC_CMD_STATUS_CONFIG_ERR] = -ENXIO,
+		[MC_CMD_STATUS_TIMEOUT] = -ETIMEDOUT,
+		[MC_CMD_STATUS_NO_RESOURCE] = -ENAVAIL,
+		[MC_CMD_STATUS_NO_MEMORY] = -ENOMEM,
+		[MC_CMD_STATUS_BUSY] = -EBUSY,
+		[MC_CMD_STATUS_UNSUPPORTED_OP] = -ENOTSUPP,
+		[MC_CMD_STATUS_INVALID_STATE] = -ENODEV,
+	};
+
+	if (WARN_ON((u32)status >= ARRAY_SIZE(mc_status_to_error_map)))
+		return -EINVAL;
+
+	return mc_status_to_error_map[status];
+}
+
+static const char *mc_status_to_string(enum mc_cmd_status status)
+{
+	static const char *const status_strings[] = {
+		[MC_CMD_STATUS_OK] = "Command completed successfully",
+		[MC_CMD_STATUS_READY] = "Command ready to be processed",
+		[MC_CMD_STATUS_AUTH_ERR] = "Authentication error",
+		[MC_CMD_STATUS_NO_PRIVILEGE] = "No privilege",
+		[MC_CMD_STATUS_DMA_ERR] = "DMA or I/O error",
+		[MC_CMD_STATUS_CONFIG_ERR] = "Configuration error",
+		[MC_CMD_STATUS_TIMEOUT] = "Operation timed out",
+		[MC_CMD_STATUS_NO_RESOURCE] = "No resources",
+		[MC_CMD_STATUS_NO_MEMORY] = "No memory available",
+		[MC_CMD_STATUS_BUSY] = "Device is busy",
+		[MC_CMD_STATUS_UNSUPPORTED_OP] = "Unsupported operation",
+		[MC_CMD_STATUS_INVALID_STATE] = "Invalid state"
+	};
+
+	if ((unsigned int)status >= ARRAY_SIZE(status_strings))
+		return "Unknown MC error";
+
+	return status_strings[status];
+}
+
+/**
+ * mc_write_command - writes a command to a Management Complex (MC) portal
+ *
+ * @portal: pointer to an MC portal
+ * @cmd: pointer to a filled command
+ */
+static inline void mc_write_command(struct mc_command __iomem *portal,
+				    struct mc_command *cmd)
+{
+	int i;
+
+	/* copy command parameters into the portal */
+	for (i = 0; i < MC_CMD_NUM_OF_PARAMS; i++)
+		writeq(cmd->params[i], &portal->params[i]);
+
+	/* submit the command by writing the header */
+	writeq(cmd->header, &portal->header);
+}
+
+/**
+ * mc_read_response - reads the response for the last MC command from a
+ * Management Complex (MC) portal
+ *
+ * @portal: pointer to an MC portal
+ * @resp: pointer to command response buffer
+ *
+ * Returns MC_CMD_STATUS_OK on Success; Error code otherwise.
+ */
+static inline enum mc_cmd_status mc_read_response(struct mc_command __iomem *
+						  portal,
+						  struct mc_command *resp)
+{
+	int i;
+	enum mc_cmd_status status;
+
+	/* Copy command response header from MC portal: */
+	resp->header = readq(&portal->header);
+	status = MC_CMD_HDR_READ_STATUS(resp->header);
+	if (status != MC_CMD_STATUS_OK)
+		return status;
+
+	/* Copy command response data from MC portal: */
+	for (i = 0; i < MC_CMD_NUM_OF_PARAMS; i++)
+		resp->params[i] = readq(&portal->params[i]);
+
+	return status;
+}
+
+/**
+ * Sends an command to the MC device using the given MC I/O object
+ *
+ * @mc_io: MC I/O object to be used
+ * @cmd: command to be sent
+ *
+ * Returns '0' on Success; Error code otherwise.
+ *
+ * NOTE: This function cannot be invoked from from atomic contexts.
+ */
+int mc_send_command(struct fsl_mc_io *mc_io, struct mc_command *cmd)
+{
+	enum mc_cmd_status status;
+	unsigned long jiffies_until_timeout =
+	    jiffies + MC_CMD_COMPLETION_TIMEOUT_JIFFIES;
+
+	/*
+	 * Send command to the MC hardware:
+	 */
+	mc_write_command(mc_io->portal_virt_addr, cmd);
+
+	/*
+	 * Wait for response from the MC hardware:
+	 */
+	for (;;) {
+		status = mc_read_response(mc_io->portal_virt_addr, cmd);
+		if (status != MC_CMD_STATUS_READY)
+			break;
+
+		/*
+		 * TODO: When MC command completion interrupts are supported
+		 * call wait function here instead of usleep_range()
+		 */
+		usleep_range(MC_CMD_COMPLETION_POLLING_MIN_SLEEP_USECS,
+			     MC_CMD_COMPLETION_POLLING_MAX_SLEEP_USECS);
+
+		if (time_after_eq(jiffies, jiffies_until_timeout)) {
+			pr_debug("MC command timed out (portal: %#llx, obj handle: %#x, command: %#x)\n",
+				 mc_io->portal_phys_addr,
+				 (unsigned int)
+					MC_CMD_HDR_READ_TOKEN(cmd->header),
+				 (unsigned int)
+					MC_CMD_HDR_READ_CMDID(cmd->header));
+
+			return -ETIMEDOUT;
+		}
+	}
+
+	if (status != MC_CMD_STATUS_OK) {
+		pr_debug("MC command failed: portal: %#llx, obj handle: %#x, command: %#x, status: %s (%#x)\n",
+			 mc_io->portal_phys_addr,
+			 (unsigned int)MC_CMD_HDR_READ_TOKEN(cmd->header),
+			 (unsigned int)MC_CMD_HDR_READ_CMDID(cmd->header),
+			 mc_status_to_string(status),
+			 (unsigned int)status);
+
+		return mc_status_to_error(status);
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL(mc_send_command);
diff --git a/drivers/staging/fsl-mc/include/dpbp-cmd.h b/drivers/staging/fsl-mc/include/dpbp-cmd.h
new file mode 100644
index 000000000000..1fd70a215194
--- /dev/null
+++ b/drivers/staging/fsl-mc/include/dpbp-cmd.h
@@ -0,0 +1,60 @@
+/* Copyright 2013-2014 Freescale Semiconductor Inc.
+*
+* 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 of the above-listed copyright holders nor the
+* names of any contributors may be used to endorse or promote products
+* derived from this software without specific prior written permission.
+*
+*
+* ALTERNATIVELY, this software may be distributed under the terms of the
+* GNU General Public License ("GPL") as published by the Free Software
+* Foundation, either version 2 of that License or (at your option) any
+* later version.
+*
+* 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 HOLDERS OR CONTRIBUTORS BE
+* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*/
+#ifndef _FSL_DPBP_CMD_H
+#define _FSL_DPBP_CMD_H
+
+/* DPBP Version */
+#define DPBP_VER_MAJOR				2
+#define DPBP_VER_MINOR				0
+
+/* Command IDs */
+#define DPBP_CMDID_CLOSE				0x800
+#define DPBP_CMDID_OPEN					0x804
+#define DPBP_CMDID_CREATE				0x904
+#define DPBP_CMDID_DESTROY				0x900
+
+#define DPBP_CMDID_ENABLE				0x002
+#define DPBP_CMDID_DISABLE				0x003
+#define DPBP_CMDID_GET_ATTR				0x004
+#define DPBP_CMDID_RESET				0x005
+#define DPBP_CMDID_IS_ENABLED				0x006
+
+#define DPBP_CMDID_SET_IRQ				0x010
+#define DPBP_CMDID_GET_IRQ				0x011
+#define DPBP_CMDID_SET_IRQ_ENABLE			0x012
+#define DPBP_CMDID_GET_IRQ_ENABLE			0x013
+#define DPBP_CMDID_SET_IRQ_MASK				0x014
+#define DPBP_CMDID_GET_IRQ_MASK				0x015
+#define DPBP_CMDID_GET_IRQ_STATUS			0x016
+#define DPBP_CMDID_CLEAR_IRQ_STATUS			0x017
+
+#endif /* _FSL_DPBP_CMD_H */
diff --git a/drivers/staging/fsl-mc/include/dpbp.h b/drivers/staging/fsl-mc/include/dpbp.h
new file mode 100644
index 000000000000..5f3c8e74d2ad
--- /dev/null
+++ b/drivers/staging/fsl-mc/include/dpbp.h
@@ -0,0 +1,330 @@
+/* Copyright 2013-2015 Freescale Semiconductor Inc.
+ *
+ * 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 of the above-listed copyright holders nor the
+ * names of any contributors may be used to endorse or promote products
+ * derived from this software without specific prior written permission.
+ *
+ *
+ * ALTERNATIVELY, this software may be distributed under the terms of the
+ * GNU General Public License ("GPL") as published by the Free Software
+ * Foundation, either version 2 of that License or (at your option) any
+ * later version.
+ *
+ * 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 HOLDERS OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+#ifndef __FSL_DPBP_H
+#define __FSL_DPBP_H
+
+/* Data Path Buffer Pool API
+ * Contains initialization APIs and runtime control APIs for DPBP
+ */
+
+struct fsl_mc_io;
+
+/**
+ * dpbp_open() - Open a control session for the specified object.
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @dpbp_id:	DPBP unique ID
+ * @token:	Returned token; use in subsequent API calls
+ *
+ * This function can be used to open a control session for an
+ * already created object; an object may have been declared in
+ * the DPL or by calling the dpbp_create function.
+ * This function returns a unique authentication token,
+ * associated with the specific object ID and the specific MC
+ * portal; this token must be used in all subsequent commands for
+ * this specific object
+ *
+ * Return:	'0' on Success; Error code otherwise.
+ */
+int dpbp_open(struct fsl_mc_io *mc_io, int dpbp_id, uint16_t *token);
+
+/**
+ * dpbp_close() - Close the control session of the object
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @token:	Token of DPBP object
+ *
+ * After this function is called, no further operations are
+ * allowed on the object without opening a new control session.
+ *
+ * Return:	'0' on Success; Error code otherwise.
+ */
+int dpbp_close(struct fsl_mc_io *mc_io, uint16_t token);
+
+/**
+ * struct dpbp_cfg() - Structure representing DPBP configuration
+ * @options:	place holder
+ */
+struct dpbp_cfg {
+	uint32_t options;
+};
+
+/**
+ * dpbp_create() - Create the DPBP object.
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @cfg:	Configuration structure
+ * @token:	Returned token; use in subsequent API calls
+ *
+ * Create the DPBP object, allocate required resources and
+ * perform required initialization.
+ *
+ * The object can be created either by declaring it in the
+ * DPL file, or by calling this function.
+ * This function returns a unique authentication token,
+ * associated with the specific object ID and the specific MC
+ * portal; this token must be used in all subsequent calls to
+ * this specific object. For objects that are created using the
+ * DPL file, call dpbp_open function to get an authentication
+ * token first.
+ *
+ * Return:	'0' on Success; Error code otherwise.
+ */
+int dpbp_create(struct fsl_mc_io	*mc_io,
+		const struct dpbp_cfg	*cfg,
+		uint16_t		*token);
+
+/**
+ * dpbp_destroy() - Destroy the DPBP object and release all its resources.
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @token:	Token of DPBP object
+ *
+ * Return:	'0' on Success; error code otherwise.
+ */
+int dpbp_destroy(struct fsl_mc_io *mc_io, uint16_t token);
+
+/**
+ * dpbp_enable() - Enable the DPBP.
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @token:	Token of DPBP object
+ *
+ * Return:	'0' on Success; Error code otherwise.
+ */
+int dpbp_enable(struct fsl_mc_io *mc_io, uint16_t token);
+
+/**
+ * dpbp_disable() - Disable the DPBP.
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @token:	Token of DPBP object
+ *
+ * Return:	'0' on Success; Error code otherwise.
+ */
+int dpbp_disable(struct fsl_mc_io *mc_io, uint16_t token);
+
+/**
+ * dpbp_is_enabled() - Check if the DPBP is enabled.
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @token:	Token of DPBP object
+ * @en:		Returns '1' if object is enabled; '0' otherwise
+ *
+ * Return:	'0' on Success; Error code otherwise.
+ */
+int dpbp_is_enabled(struct fsl_mc_io *mc_io, uint16_t token, int *en);
+
+/**
+ * dpbp_reset() - Reset the DPBP, returns the object to initial state.
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @token:	Token of DPBP object
+ *
+ * Return:	'0' on Success; Error code otherwise.
+ */
+int dpbp_reset(struct fsl_mc_io *mc_io, uint16_t token);
+
+/**
+ * dpbp_set_irq() - Set IRQ information for the DPBP to trigger an interrupt.
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @token:	Token of DPBP object
+ * @irq_index:	Identifies the interrupt index to configure
+ * @irq_addr:	Address that must be written to
+ *				signal a message-based interrupt
+ * @irq_val:	Value to write into irq_addr address
+ * @user_irq_id: A user defined number associated with this IRQ
+ *
+ * Return:	'0' on Success; Error code otherwise.
+ */
+int dpbp_set_irq(struct fsl_mc_io	*mc_io,
+		 uint16_t		token,
+		 uint8_t		irq_index,
+		 uint64_t		irq_addr,
+		 uint32_t		irq_val,
+		 int			user_irq_id);
+
+/**
+ * dpbp_get_irq() - Get IRQ information from the DPBP.
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @token:	Token of DPBP object
+ * @irq_index:	The interrupt index to configure
+ * @type:	Interrupt type: 0 represents message interrupt
+ *				type (both irq_addr and irq_val are valid)
+ * @irq_addr:	Returned address that must be written to
+ *				signal the message-based interrupt
+ * @irq_val:	Value to write into irq_addr address
+ * @user_irq_id: A user defined number associated with this IRQ
+ *
+ * Return:	'0' on Success; Error code otherwise.
+ */
+int dpbp_get_irq(struct fsl_mc_io	*mc_io,
+		 uint16_t		token,
+		 uint8_t		irq_index,
+		 int			*type,
+		 uint64_t		*irq_addr,
+		 uint32_t		*irq_val,
+		 int			*user_irq_id);
+
+/**
+ * dpbp_set_irq_enable() - Set overall interrupt state.
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @token:	Token of DPBP object
+ * @irq_index:	The interrupt index to configure
+ * @en:	Interrupt state - enable = 1, disable = 0
+ *
+ * Allows GPP software to control when interrupts are generated.
+ * Each interrupt can have up to 32 causes.  The enable/disable control's the
+ * overall interrupt state. if the interrupt is disabled no causes will cause
+ * an interrupt.
+ *
+ * Return:	'0' on Success; Error code otherwise.
+ */
+int dpbp_set_irq_enable(struct fsl_mc_io	*mc_io,
+			uint16_t		token,
+			uint8_t			irq_index,
+			uint8_t			en);
+
+/**
+ * dpbp_get_irq_enable() - Get overall interrupt state
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @token:	Token of DPBP object
+ * @irq_index:	The interrupt index to configure
+ * @en:		Returned interrupt state - enable = 1, disable = 0
+ *
+ * Return:	'0' on Success; Error code otherwise.
+ */
+int dpbp_get_irq_enable(struct fsl_mc_io	*mc_io,
+			uint16_t		token,
+			uint8_t			irq_index,
+			uint8_t			*en);
+
+/**
+ * dpbp_set_irq_mask() - Set interrupt mask.
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @token:	Token of DPBP object
+ * @irq_index:	The interrupt index to configure
+ * @mask:	Event mask to trigger interrupt;
+ *			each bit:
+ *				0 = ignore event
+ *				1 = consider event for asserting IRQ
+ *
+ * Every interrupt can have up to 32 causes and the interrupt model supports
+ * masking/unmasking each cause independently
+ *
+ * Return:	'0' on Success; Error code otherwise.
+ */
+int dpbp_set_irq_mask(struct fsl_mc_io	*mc_io,
+		      uint16_t		token,
+		      uint8_t		irq_index,
+		      uint32_t		mask);
+
+/**
+ * dpbp_get_irq_mask() - Get interrupt mask.
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @token:	Token of DPBP object
+ * @irq_index:	The interrupt index to configure
+ * @mask:	Returned event mask to trigger interrupt
+ *
+ * Every interrupt can have up to 32 causes and the interrupt model supports
+ * masking/unmasking each cause independently
+ *
+ * Return:	'0' on Success; Error code otherwise.
+ */
+int dpbp_get_irq_mask(struct fsl_mc_io	*mc_io,
+		      uint16_t		token,
+		      uint8_t		irq_index,
+		      uint32_t		*mask);
+
+/**
+ * dpbp_get_irq_status() - Get the current status of any pending interrupts.
+ *
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @token:	Token of DPBP object
+ * @irq_index:	The interrupt index to configure
+ * @status:	Returned interrupts status - one bit per cause:
+ *			0 = no interrupt pending
+ *			1 = interrupt pending
+ *
+ * Return:	'0' on Success; Error code otherwise.
+ */
+int dpbp_get_irq_status(struct fsl_mc_io	*mc_io,
+			uint16_t		token,
+			uint8_t			irq_index,
+			uint32_t		*status);
+
+/**
+ * dpbp_clear_irq_status() - Clear a pending interrupt's status
+ *
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @token:	Token of DPBP object
+ * @irq_index:	The interrupt index to configure
+ * @status:	Bits to clear (W1C) - one bit per cause:
+ *					0 = don't change
+ *					1 = clear status bit
+ *
+ * Return:	'0' on Success; Error code otherwise.
+ */
+int dpbp_clear_irq_status(struct fsl_mc_io	*mc_io,
+			  uint16_t		token,
+			  uint8_t		irq_index,
+			  uint32_t		status);
+
+/**
+ * struct dpbp_attr - Structure representing DPBP attributes
+ * @id:		DPBP object ID
+ * @version:	DPBP version
+ * @bpid:	Hardware buffer pool ID; should be used as an argument in
+ *		acquire/release operations on buffers
+ */
+struct dpbp_attr {
+	int id;
+	/**
+	 * struct version - Structure representing DPBP version
+	 * @major:	DPBP major version
+	 * @minor:	DPBP minor version
+	 */
+	struct {
+		uint16_t major;
+		uint16_t minor;
+	} version;
+	uint16_t bpid;
+};
+
+/**
+ * dpbp_get_attributes - Retrieve DPBP attributes.
+ *
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @token:	Token of DPBP object
+ * @attr:	Returned object's attributes
+ *
+ * Return:	'0' on Success; Error code otherwise.
+ */
+int dpbp_get_attributes(struct fsl_mc_io	*mc_io,
+			uint16_t		token,
+			struct dpbp_attr	*attr);
+
+/** @} */
+
+#endif /* __FSL_DPBP_H */
diff --git a/drivers/staging/fsl-mc/include/dpcon-cmd.h b/drivers/staging/fsl-mc/include/dpcon-cmd.h
new file mode 100644
index 000000000000..c878d33bfd86
--- /dev/null
+++ b/drivers/staging/fsl-mc/include/dpcon-cmd.h
@@ -0,0 +1,62 @@
+/* Copyright 2013-2015 Freescale Semiconductor Inc.
+*
+* 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 of the above-listed copyright holders nor the
+* names of any contributors may be used to endorse or promote products
+* derived from this software without specific prior written permission.
+*
+*
+* ALTERNATIVELY, this software may be distributed under the terms of the
+* GNU General Public License ("GPL") as published by the Free Software
+* Foundation, either version 2 of that License or (at your option) any
+* later version.
+*
+* 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 HOLDERS OR CONTRIBUTORS BE
+* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*/
+#ifndef _FSL_DPCON_CMD_H
+#define _FSL_DPCON_CMD_H
+
+/* DPCON Version */
+#define DPCON_VER_MAJOR				2
+#define DPCON_VER_MINOR				0
+
+/* Command IDs */
+#define DPCON_CMDID_CLOSE				0x800
+#define DPCON_CMDID_OPEN				0x808
+#define DPCON_CMDID_CREATE				0x908
+#define DPCON_CMDID_DESTROY				0x900
+
+#define DPCON_CMDID_ENABLE				0x002
+#define DPCON_CMDID_DISABLE				0x003
+#define DPCON_CMDID_GET_ATTR				0x004
+#define DPCON_CMDID_RESET				0x005
+#define DPCON_CMDID_IS_ENABLED				0x006
+
+#define DPCON_CMDID_SET_IRQ				0x010
+#define DPCON_CMDID_GET_IRQ				0x011
+#define DPCON_CMDID_SET_IRQ_ENABLE			0x012
+#define DPCON_CMDID_GET_IRQ_ENABLE			0x013
+#define DPCON_CMDID_SET_IRQ_MASK			0x014
+#define DPCON_CMDID_GET_IRQ_MASK			0x015
+#define DPCON_CMDID_GET_IRQ_STATUS			0x016
+#define DPCON_CMDID_CLEAR_IRQ_STATUS			0x017
+
+#define DPCON_CMDID_SET_NOTIFICATION			0x100
+
+#endif /* _FSL_DPCON_CMD_H */
diff --git a/drivers/staging/fsl-mc/include/dpmng.h b/drivers/staging/fsl-mc/include/dpmng.h
new file mode 100644
index 000000000000..1b052b830993
--- /dev/null
+++ b/drivers/staging/fsl-mc/include/dpmng.h
@@ -0,0 +1,80 @@
+/* Copyright 2013-2015 Freescale Semiconductor Inc.
+ *
+ * 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 of the above-listed copyright holders nor the
+ * names of any contributors may be used to endorse or promote products
+ * derived from this software without specific prior written permission.
+ *
+ *
+ * ALTERNATIVELY, this software may be distributed under the terms of the
+ * GNU General Public License ("GPL") as published by the Free Software
+ * Foundation, either version 2 of that License or (at your option) any
+ * later version.
+ *
+ * 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 HOLDERS OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+#ifndef __FSL_DPMNG_H
+#define __FSL_DPMNG_H
+
+/* Management Complex General API
+ * Contains general API for the Management Complex firmware
+ */
+
+struct fsl_mc_io;
+
+/**
+ * Management Complex firmware version information
+ */
+#define MC_VER_MAJOR 6
+#define MC_VER_MINOR 0
+
+/**
+ * struct mc_versoin
+ * @major: Major version number: incremented on API compatibility changes
+ * @minor: Minor version number: incremented on API additions (that are
+ *		backward compatible); reset when major version is incremented
+ * @revision: Internal revision number: incremented on implementation changes
+ *		and/or bug fixes that have no impact on API
+ */
+struct mc_version {
+	uint32_t major;
+	uint32_t minor;
+	uint32_t revision;
+};
+
+/**
+ * mc_get_version() - Retrieves the Management Complex firmware
+ *			version information
+ * @mc_io:		Pointer to opaque I/O object
+ * @mc_ver_info:	Returned version information structure
+ *
+ * Return:	'0' on Success; Error code otherwise.
+ */
+int mc_get_version(struct fsl_mc_io *mc_io, struct mc_version *mc_ver_info);
+
+/**
+ * dpmng_get_container_id() - Get container ID associated with a given portal.
+ * @mc_io:		Pointer to MC portal's I/O object
+ * @container_id:	Requested container ID
+ *
+ * Return:	'0' on Success; Error code otherwise.
+ */
+int dpmng_get_container_id(struct fsl_mc_io *mc_io, int *container_id);
+
+#endif /* __FSL_DPMNG_H */
diff --git a/drivers/staging/fsl-mc/include/dprc.h b/drivers/staging/fsl-mc/include/dprc.h
new file mode 100644
index 000000000000..f1862a78a409
--- /dev/null
+++ b/drivers/staging/fsl-mc/include/dprc.h
@@ -0,0 +1,801 @@
+/* Copyright 2013-2015 Freescale Semiconductor Inc.
+ *
+ * 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 of the above-listed copyright holders nor the
+ * names of any contributors may be used to endorse or promote products
+ * derived from this software without specific prior written permission.
+ *
+ *
+ * ALTERNATIVELY, this software may be distributed under the terms of the
+ * GNU General Public License ("GPL") as published by the Free Software
+ * Foundation, either version 2 of that License or (at your option) any
+ * later version.
+ *
+ * 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 HOLDERS OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+#ifndef _FSL_DPRC_H
+#define _FSL_DPRC_H
+
+/* Data Path Resource Container API
+ * Contains DPRC API for managing and querying DPAA resources
+ */
+
+struct fsl_mc_io;
+
+/**
+ * Set this value as the icid value in dprc_cfg structure when creating a
+ * container, in case the ICID is not selected by the user and should be
+ * allocated by the DPRC from the pool of ICIDs.
+ */
+#define DPRC_GET_ICID_FROM_POOL			(uint16_t)(~(0))
+
+/**
+ * Set this value as the portal_id value in dprc_cfg structure when creating a
+ * container, in case the portal ID is not specifically selected by the
+ * user and should be allocated by the DPRC from the pool of portal ids.
+ */
+#define DPRC_GET_PORTAL_ID_FROM_POOL	(int)(~(0))
+
+/**
+ * dprc_open() - Open DPRC object for use
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @container_id: Container ID to open
+ * @token:	Returned token of DPRC object
+ *
+ * Return:	'0' on Success; Error code otherwise.
+ *
+ * @warning	Required before any operation on the object.
+ */
+int dprc_open(struct fsl_mc_io *mc_io, int container_id, uint16_t *token);
+
+/**
+ * dprc_close() - Close the control session of the object
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @token:	Token of DPRC object
+ *
+ * After this function is called, no further operations are
+ * allowed on the object without opening a new control session.
+ *
+ * Return:	'0' on Success; Error code otherwise.
+ */
+int dprc_close(struct fsl_mc_io *mc_io, uint16_t token);
+
+/**
+ * Container general options
+ *
+ * These options may be selected at container creation by the container creator
+ * and can be retrieved using dprc_get_attributes()
+ */
+
+/* Spawn Policy Option allowed - Indicates that the new container is allowed
+ * to spawn and have its own child containers.
+ */
+#define DPRC_CFG_OPT_SPAWN_ALLOWED		0x00000001
+
+/* General Container allocation policy - Indicates that the new container is
+ * allowed to allocate requested resources from its parent container; if not
+ * set, the container is only allowed to use resources in its own pools; Note
+ * that this is a container's global policy, but the parent container may
+ * override it and set specific quota per resource type.
+ */
+#define DPRC_CFG_OPT_ALLOC_ALLOWED		0x00000002
+
+/* Object initialization allowed - software context associated with this
+ * container is allowed to invoke object initialization operations.
+ */
+#define DPRC_CFG_OPT_OBJ_CREATE_ALLOWED	0x00000004
+
+/* Topology change allowed - software context associated with this
+ * container is allowed to invoke topology operations, such as attach/detach
+ * of network objects.
+ */
+#define DPRC_CFG_OPT_TOPOLOGY_CHANGES_ALLOWED	0x00000008
+
+/* IOMMU bypass - indicates whether objects of this container are permitted
+ * to bypass the IOMMU.
+ */
+#define DPRC_CFG_OPT_IOMMU_BYPASS		0x00000010
+
+/* AIOP - Indicates that container belongs to AIOP.  */
+#define DPRC_CFG_OPT_AIOP			0x00000020
+
+/**
+ * struct dprc_cfg - Container configuration options
+ * @icid: Container's ICID; if set to 'DPRC_GET_ICID_FROM_POOL', a free
+ *		ICID value is allocated by the DPRC
+ * @portal_id: Portal ID; if set to 'DPRC_GET_PORTAL_ID_FROM_POOL', a free
+ *		portal ID is allocated by the DPRC
+ * @options: Combination of 'DPRC_CFG_OPT_<X>' options
+ */
+struct dprc_cfg {
+	uint16_t icid;
+	int portal_id;
+	uint64_t options;
+};
+
+/**
+ * dprc_create_container() - Create child container
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @token:	Token of DPRC object
+ * @cfg:	Child container configuration
+ * @child_container_id:	Returned child container ID
+ * @child_portal_paddr:	Returned base physical address of the
+ *					child portal
+ *
+ * Return:	'0' on Success; Error code otherwise.
+ */
+int dprc_create_container(struct fsl_mc_io	*mc_io,
+			  uint16_t		token,
+			  struct dprc_cfg	*cfg,
+			  int			*child_container_id,
+			  uint64_t		*child_portal_paddr);
+
+/**
+ * dprc_destroy_container() - Destroy child container.
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @token:	Token of DPRC object
+ * @child_container_id:	ID of the container to destroy
+ *
+ * This function terminates the child container, so following this call the
+ * child container ID becomes invalid.
+ *
+ * Notes:
+ * - All resources and objects of the destroyed container are returned to the
+ * parent container or destroyed if were created be the destroyed container.
+ * - This function destroy all the child containers of the specified
+ *   container prior to destroying the container itself.
+ *
+ * warning: Only the parent container is allowed to destroy a child policy
+ *		Container 0 can't be destroyed
+ *
+ * Return:	'0' on Success; Error code otherwise.
+ *
+ */
+int dprc_destroy_container(struct fsl_mc_io	*mc_io,
+			   uint16_t		token,
+			   int			child_container_id);
+
+/**
+ * dprc_reset_container - Reset child container.
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @token:	Token of DPRC object
+ * @child_container_id:	ID of the container to reset
+ *
+ * In case a software context crashes or becomes non-responsive, the parent
+ * may wish to reset its resources container before the software context is
+ * restarted.
+ *
+ * This routine informs all objects assigned to the child container that the
+ * container is being reset, so they may perform any cleanup operations that are
+ * needed. All objects handles that were owned by the child container shall be
+ * closed.
+ *
+ * Note that such request may be submitted even if the child software context
+ * has not crashed, but the resulting object cleanup operations will not be
+ * aware of that.
+ *
+ * Return:	'0' on Success; Error code otherwise.
+ */
+int dprc_reset_container(struct fsl_mc_io *mc_io,
+			 uint16_t token,
+			 int child_container_id);
+
+/* IRQ */
+
+/* Number of dprc's IRQs */
+#define DPRC_NUM_OF_IRQS		1
+
+/* Object irq events */
+
+/* IRQ event - Indicates that a new object assigned to the container */
+#define DPRC_IRQ_EVENT_OBJ_ADDED		0x00000001
+/* IRQ event - Indicates that an object was unassigned from the container */
+#define DPRC_IRQ_EVENT_OBJ_REMOVED		0x00000002
+/* IRQ event - Indicates that resources assigned to the container */
+#define DPRC_IRQ_EVENT_RES_ADDED		0x00000004
+/* IRQ event - Indicates that resources unassigned from the container */
+#define DPRC_IRQ_EVENT_RES_REMOVED		0x00000008
+/* IRQ event - Indicates that one of the descendant containers that opened by
+ * this container is destroyed
+ */
+#define DPRC_IRQ_EVENT_CONTAINER_DESTROYED	0x00000010
+
+/* IRQ event - Indicates that on one of the container's opened object is
+ * destroyed
+ */
+#define DPRC_IRQ_EVENT_OBJ_DESTROYED		0x00000020
+
+/* Irq event - Indicates that object is created at the container */
+#define DPRC_IRQ_EVENT_OBJ_CREATED		0x00000040
+
+/**
+ * dprc_set_irq() - Set IRQ information for the DPRC to trigger an interrupt.
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @token:	Token of DPRC object
+ * @irq_index:	Identifies the interrupt index to configure
+ * @irq_addr:	Address that must be written to
+ *			signal a message-based interrupt
+ * @irq_val:	Value to write into irq_addr address
+ * @user_irq_id: Returned a user defined number associated with this IRQ
+ *
+ * Return:	'0' on Success; Error code otherwise.
+ */
+int dprc_set_irq(struct fsl_mc_io	*mc_io,
+		 uint16_t		token,
+		 uint8_t		irq_index,
+		 uint64_t		irq_addr,
+		 uint32_t		irq_val,
+		 int			user_irq_id);
+
+/**
+ * dprc_get_irq() - Get IRQ information from the DPRC.
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @token:	Token of DPRC object
+ * @irq_index:	The interrupt index to configure
+ * @type:	Returned interrupt type: 0 represents message interrupt
+ *			type (both irq_addr and irq_val are valid)
+ * @irq_addr:	Returned address that must be written to
+ *			signal the message-based interrupt
+ * @irq_val:	Value to write into irq_addr address
+ * @user_irq_id: A user defined number associated with this IRQ
+ *
+ * Return:	'0' on Success; Error code otherwise.
+ */
+int dprc_get_irq(struct fsl_mc_io	*mc_io,
+		 uint16_t		token,
+		 uint8_t		irq_index,
+		 int			*type,
+		 uint64_t		*irq_addr,
+		 uint32_t		*irq_val,
+		 int			*user_irq_id);
+
+/**
+ * dprc_set_irq_enable() - Set overall interrupt state.
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @token:	Token of DPRC object
+ * @irq_index:	The interrupt index to configure
+ * @en:		Interrupt state - enable = 1, disable = 0
+ *
+ * Allows GPP software to control when interrupts are generated.
+ * Each interrupt can have up to 32 causes.  The enable/disable control's the
+ * overall interrupt state. if the interrupt is disabled no causes will cause
+ * an interrupt.
+ *
+ * Return:	'0' on Success; Error code otherwise.
+ */
+int dprc_set_irq_enable(struct fsl_mc_io	*mc_io,
+			uint16_t		token,
+			uint8_t			irq_index,
+			uint8_t			en);
+
+/**
+ * dprc_get_irq_enable() - Get overall interrupt state.
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @token:	Token of DPRC object
+ * @irq_index:  The interrupt index to configure
+ * @en:		Returned interrupt state - enable = 1, disable = 0
+ *
+ * Return:	'0' on Success; Error code otherwise.
+ */
+int dprc_get_irq_enable(struct fsl_mc_io	*mc_io,
+			uint16_t		token,
+			uint8_t			irq_index,
+			uint8_t			*en);
+
+/**
+ * dprc_set_irq_mask() - Set interrupt mask.
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @token:	Token of DPRC object
+ * @irq_index:	The interrupt index to configure
+ * @mask:	event mask to trigger interrupt;
+ *			each bit:
+ *				0 = ignore event
+ *				1 = consider event for asserting irq
+ *
+ * Every interrupt can have up to 32 causes and the interrupt model supports
+ * masking/unmasking each cause independently
+ *
+ * Return:	'0' on Success; Error code otherwise.
+ */
+int dprc_set_irq_mask(struct fsl_mc_io	*mc_io,
+		      uint16_t		token,
+		      uint8_t		irq_index,
+		      uint32_t		mask);
+
+/**
+ * dprc_get_irq_mask() - Get interrupt mask.
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @token:	Token of DPRC object
+ * @irq_index:	The interrupt index to configure
+ * @mask:	Returned event mask to trigger interrupt
+ *
+ * Every interrupt can have up to 32 causes and the interrupt model supports
+ * masking/unmasking each cause independently
+ *
+ * Return:	'0' on Success; Error code otherwise.
+ */
+int dprc_get_irq_mask(struct fsl_mc_io	*mc_io,
+		      uint16_t		token,
+		      uint8_t		irq_index,
+		      uint32_t		*mask);
+
+/**
+ * dprc_get_irq_status() - Get the current status of any pending interrupts.
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @token:	Token of DPRC object
+ * @irq_index:	The interrupt index to configure
+ * @status:	Returned interrupts status - one bit per cause:
+ *			0 = no interrupt pending
+ *			1 = interrupt pending
+ *
+ * Return:	'0' on Success; Error code otherwise.
+ */
+int dprc_get_irq_status(struct fsl_mc_io	*mc_io,
+			uint16_t		token,
+			uint8_t			irq_index,
+			uint32_t		*status);
+
+/**
+ * dprc_clear_irq_status() - Clear a pending interrupt's status
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @token:	Token of DPRC object
+ * @irq_index:	The interrupt index to configure
+ * @status:	bits to clear (W1C) - one bit per cause:
+ *					0 = don't change
+ *					1 = clear status bit
+ *
+ * Return:	'0' on Success; Error code otherwise.
+ */
+int dprc_clear_irq_status(struct fsl_mc_io	*mc_io,
+			  uint16_t		token,
+			  uint8_t		irq_index,
+			  uint32_t		status);
+
+/**
+ * struct dprc_attributes - Container attributes
+ * @container_id: Container's ID
+ * @icid: Container's ICID
+ * @portal_id: Container's portal ID
+ * @options: Container's options as set at container's creation
+ * @version: DPRC version
+ */
+struct dprc_attributes {
+	int container_id;
+	uint16_t icid;
+	int portal_id;
+	uint64_t options;
+	/**
+	 * struct version - DPRC version
+	 * @major: DPRC major version
+	 * @minor: DPRC minor version
+	 */
+	struct {
+		uint16_t major;
+		uint16_t minor;
+	} version;
+};
+
+/**
+ * dprc_get_attributes() - Obtains container attributes
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @token:	Token of DPRC object
+ * @attributes	Returned container attributes
+ *
+ * Return:     '0' on Success; Error code otherwise.
+ */
+int dprc_get_attributes(struct fsl_mc_io	*mc_io,
+			uint16_t		token,
+			struct dprc_attributes	*attributes);
+
+/**
+ * dprc_set_res_quota() - Set allocation policy for a specific resource/object
+ *		type in a child container
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @token:	Token of DPRC object
+ * @child_container_id:	ID of the child container
+ * @type:	Resource/object type
+ * @quota:	Sets the maximum number of resources of	the selected type
+ *		that the child container is allowed to allocate from its parent;
+ *		when quota is set to -1, the policy is the same as container's
+ *		general policy.
+ *
+ * Allocation policy determines whether or not a container may allocate
+ * resources from its parent. Each container has a 'global' allocation policy
+ * that is set when the container is created.
+ *
+ * This function sets allocation policy for a specific resource type.
+ * The default policy for all resource types matches the container's 'global'
+ * allocation policy.
+ *
+ * Return:	'0' on Success; Error code otherwise.
+ *
+ * @warning	Only the parent container is allowed to change a child policy.
+ */
+int dprc_set_res_quota(struct fsl_mc_io	*mc_io,
+		       uint16_t		token,
+		       int		child_container_id,
+		       char		*type,
+		       uint16_t		quota);
+
+/**
+ * dprc_get_res_quota() - Gets the allocation policy of a specific
+ *		resource/object type in a child container
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @token:	Token of DPRC object
+ * @child_container_id;	ID of the child container
+ * @type:	resource/object type
+ * @quota:	Returnes the maximum number of resources of the selected type
+ *		that the child container is allowed to allocate from the parent;
+ *		when quota is set to -1, the policy is the same as container's
+ *		general policy.
+ *
+ * Return:	'0' on Success; Error code otherwise.
+ */
+int dprc_get_res_quota(struct fsl_mc_io	*mc_io,
+		       uint16_t		token,
+		       int		child_container_id,
+		       char		*type,
+		       uint16_t		*quota);
+
+/* Resource request options */
+
+/* Explicit resource ID request - The requested objects/resources
+ * are explicit and sequential (in case of resources).
+ * The base ID is given at res_req at base_align field
+ */
+#define DPRC_RES_REQ_OPT_EXPLICIT		0x00000001
+
+/* Aligned resources request - Relevant only for resources
+ * request (and not objects). Indicates that resources base ID should be
+ * sequential and aligned to the value given at dprc_res_req base_align field
+ */
+#define DPRC_RES_REQ_OPT_ALIGNED		0x00000002
+
+/* Plugged Flag - Relevant only for object assignment request.
+ * Indicates that after all objects assigned. An interrupt will be invoked at
+ * the relevant GPP. The assigned object will be marked as plugged.
+ * plugged objects can't be assigned from their container
+ */
+#define DPRC_RES_REQ_OPT_PLUGGED		0x00000004
+
+/**
+ * struct dprc_res_req - Resource request descriptor, to be used in assignment
+ *			or un-assignment of resources and objects.
+ * @type: Resource/object type: Represent as a NULL terminated string.
+ *	This string may received by using dprc_get_pool() to get resource
+ *	type and dprc_get_obj() to get object type;
+ *	Note: it is not possible to assign/un-assign DPRC objects
+ * @num: Number of resources
+ * @options: Request options: combination of DPRC_RES_REQ_OPT_ options
+ * @id_base_align: In case of explicit assignment (DPRC_RES_REQ_OPT_EXPLICIT
+ *		is set at option), this field represents the required base ID
+ *		for resource allocation; In case of aligned assignment
+ *		(DPRC_RES_REQ_OPT_ALIGNED is set at option), this field
+ *		indicates the required alignment for the resource ID(s) -
+ *		use 0 if there is no alignment or explicit ID requirements
+ */
+struct dprc_res_req {
+	char type[16];
+	uint32_t num;
+	uint32_t options;
+	int id_base_align;
+};
+
+/**
+ * dprc_assign() - Assigns objects or resource to a child container.
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @token:	Token of DPRC object
+ * @container_id: ID of the child container
+ * @res_req:	Describes the type and amount of resources to
+ *			assign to the given container
+ *
+ * Assignment is usually done by a parent (this DPRC) to one of its child
+ * containers.
+ *
+ * According to the DPRC allocation policy, the assigned resources may be taken
+ * (allocated) from the container's ancestors, if not enough resources are
+ * available in the container itself.
+ *
+ * The type of assignment depends on the dprc_res_req options, as follows:
+ * - DPRC_RES_REQ_OPT_EXPLICIT: indicates that assigned resources should have
+ *   the explicit base ID specified at the id_base_align field of res_req.
+ * - DPRC_RES_REQ_OPT_ALIGNED: indicates that the assigned resources should be
+ *   aligned to the value given at id_base_align field of res_req.
+ * - DPRC_RES_REQ_OPT_PLUGGED: Relevant only for object assignment,
+ *   and indicates that the object must be set to the plugged state.
+ *
+ * A container may use this function with its own ID in order to change a
+ * object state to plugged or unplugged.
+ *
+ * If IRQ information has been set in the child DPRC, it will signal an
+ * interrupt following every change in its object assignment.
+ *
+ * Return:	'0' on Success; Error code otherwise.
+ */
+int dprc_assign(struct fsl_mc_io	*mc_io,
+		uint16_t		token,
+		int			container_id,
+		struct dprc_res_req	*res_req);
+
+/**
+ * dprc_unassign() - Un-assigns objects or resources from a child container
+ *		and moves them into this (parent) DPRC.
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @token:	Token of DPRC object
+ * @child_container_id:	ID of the child container
+ * @res_req:	Describes the type and amount of resources to un-assign from
+ *		the child container
+ *
+ * Un-assignment of objects can succeed only if the object is not in the
+ * plugged or opened state.
+ *
+ * Return:	'0' on Success; Error code otherwise.
+ */
+int dprc_unassign(struct fsl_mc_io	*mc_io,
+		  uint16_t		token,
+		  int			child_container_id,
+		  struct dprc_res_req	*res_req);
+
+/**
+ * dprc_get_pool_count() - Get the number of dprc's pools
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @token:	Token of DPRC object
+ * @pool_count:	Returned number of resource pools in the dprc
+ *
+ * Return:	'0' on Success; Error code otherwise.
+ */
+int dprc_get_pool_count(struct fsl_mc_io	*mc_io,
+			uint16_t		token,
+			int			*pool_count);
+
+/**
+ * dprc_get_pool() - Get the type (string) of a certain dprc's pool
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @token:	Token of DPRC object
+ * @pool_index;	Index of the pool to be queried (< pool_count)
+ * @type:	The type of the pool
+ *
+ * The pool types retrieved one by one by incrementing
+ * pool_index up to (not including) the value of pool_count returned
+ * from dprc_get_pool_count(). dprc_get_pool_count() must
+ * be called prior to dprc_get_pool().
+ *
+ * Return:	'0' on Success; Error code otherwise.
+ */
+int dprc_get_pool(struct fsl_mc_io	*mc_io,
+		  uint16_t		token,
+		  int			pool_index,
+		  char			*type);
+
+/**
+ * dprc_get_obj_count() - Obtains the number of objects in the DPRC
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @token:	Token of DPRC object
+ * @obj_count:	Number of objects assigned to the DPRC
+ *
+ * Return:	'0' on Success; Error code otherwise.
+ */
+int dprc_get_obj_count(struct fsl_mc_io *mc_io, uint16_t token, int *obj_count);
+
+/* Objects Attributes Flags */
+
+/* Opened state - Indicates that an object is open by at least one owner */
+#define DPRC_OBJ_STATE_OPEN		0x00000001
+/* Plugged state - Indicates that the object is plugged */
+#define DPRC_OBJ_STATE_PLUGGED		0x00000002
+
+/**
+ * struct dprc_obj_desc - Object descriptor, returned from dprc_get_obj()
+ * @type: Type of object: NULL terminated string
+ * @id: ID of logical object resource
+ * @vendor: Object vendor identifier
+ * @ver_major: Major version number
+ * @ver_minor:  Minor version number
+ * @irq_count: Number of interrupts supported by the object
+ * @region_count: Number of mappable regions supported by the object
+ * @state: Object state: combination of DPRC_OBJ_STATE_ states
+ */
+struct dprc_obj_desc {
+	char type[16];
+	int id;
+	uint16_t vendor;
+	uint16_t ver_major;
+	uint16_t ver_minor;
+	uint8_t irq_count;
+	uint8_t region_count;
+	uint32_t state;
+};
+
+/**
+ * dprc_get_obj() - Get general information on an object
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @token:	Token of DPRC object
+ * @obj_index:	Index of the object to be queried (< obj_count)
+ * @obj_desc:	Returns the requested object descriptor
+ *
+ * The object descriptors are retrieved one by one by incrementing
+ * obj_index up to (not including) the value of obj_count returned
+ * from dprc_get_obj_count(). dprc_get_obj_count() must
+ * be called prior to dprc_get_obj().
+ *
+ * Return:	'0' on Success; Error code otherwise.
+ */
+int dprc_get_obj(struct fsl_mc_io	*mc_io,
+		 uint16_t		token,
+		 int			obj_index,
+		 struct dprc_obj_desc	*obj_desc);
+
+/**
+ * dprc_get_res_count() - Obtains the number of free resources that are assigned
+ *		to this container, by pool type
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @token:	Token of DPRC object
+ * @type:	pool type
+ * @res_count:	Returned number of free resources of the given
+ *			resource type that are assigned to this DPRC
+ *
+ * Return:	'0' on Success; Error code otherwise.
+ */
+int dprc_get_res_count(struct fsl_mc_io	*mc_io,
+		       uint16_t		token,
+		       char		*type,
+		       int		*res_count);
+
+/**
+ * enum dprc_iter_status - Iteration status
+ * @DPRC_ITER_STATUS_FIRST: Perform first iteration
+ * @DPRC_ITER_STATUS_MORE: Indicates more/next iteration is needed
+ * @DPRC_ITER_STATUS_LAST: Indicates last iteration
+ */
+enum dprc_iter_status {
+	DPRC_ITER_STATUS_FIRST = 0,
+	DPRC_ITER_STATUS_MORE = 1,
+	DPRC_ITER_STATUS_LAST = 2
+};
+
+/**
+ * struct dprc_res_ids_range_desc - Resource ID range descriptor
+ * @base_id: Base resource ID of this range
+ * @last_id: Last resource ID of this range
+ * @iter_status: Iteration status - should be set to DPRC_ITER_STATUS_FIRST at
+ *	first iteration; while the returned marker is DPRC_ITER_STATUS_MORE,
+ *	additional iterations are needed, until the returned marker is
+ *	DPRC_ITER_STATUS_LAST
+ */
+struct dprc_res_ids_range_desc {
+	int base_id;
+	int last_id;
+	enum dprc_iter_status iter_status;
+};
+
+/**
+ * dprc_get_res_ids() - Obtains IDs of free resources in the container
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @token:	Token of DPRC object
+ * @type:	pool type
+ * @range_desc:	range descriptor
+ *
+ * Return:	'0' on Success; Error code otherwise.
+ */
+int dprc_get_res_ids(struct fsl_mc_io			*mc_io,
+		     uint16_t				token,
+		     char				*type,
+		     struct dprc_res_ids_range_desc	*range_desc);
+
+/**
+ * dprc_get_portal_paddr() - Get the physical address of MC portals
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @token:	Token of DPRC object
+ * @portal_id:	MC portal ID
+ * @portal_addr: The physical address of the MC portal ID
+ *
+ * Return:	'0' on Success; Error code otherwise.
+ */
+int dprc_get_portal_paddr(struct fsl_mc_io	*mc_io,
+			  uint16_t		token,
+			  int			portal_id,
+			  uint64_t		*portal_addr);
+
+/**
+ * struct dprc_region_desc - Mappable region descriptor
+ * @base_paddr: Region base physical address
+ * @size: Region size (in bytes)
+ */
+struct dprc_region_desc {
+	uint64_t base_paddr;
+	uint32_t size;
+};
+
+/**
+ * dprc_get_obj_region() - Get region information for a specified object.
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @token:	Token of DPRC object
+ * @obj_type;	Object type as returned in dprc_get_obj()
+ * @obj_id:	Unique object instance as returned in dprc_get_obj()
+ * @region_index: The specific region to query
+ * @region_desc:  Returns the requested region descriptor
+ *
+ * Return:	'0' on Success; Error code otherwise.
+ */
+int dprc_get_obj_region(struct fsl_mc_io	*mc_io,
+			uint16_t		token,
+			char			*obj_type,
+			int			obj_id,
+			uint8_t			region_index,
+			struct dprc_region_desc	*region_desc);
+
+/**
+ * struct dprc_endpoint - Endpoint description for link connect/disconnect
+ *			operations
+ * @type: Endpoint object type: NULL terminated string
+ * @id: Endpoint object ID
+ * @interface_id: Interface ID; should be set for endpoints with multiple
+ *		interfaces ("dpsw", "dpdmux"); for others, always set to 0
+ */
+struct dprc_endpoint {
+	char type[16];
+	int id;
+	int interface_id;
+};
+
+/**
+ * dprc_connect() - Connect two endpoints to create a network link between them
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @token:	Token of DPRC object
+ * @endpoint1:	Endpoint 1 configuration parameters
+ * @endpoint2:	Endpoint 2 configuration parameters
+ *
+ * Return:	'0' on Success; Error code otherwise.
+ */
+int dprc_connect(struct fsl_mc_io		*mc_io,
+		 uint16_t			token,
+		 const struct dprc_endpoint	*endpoint1,
+		 const struct dprc_endpoint	*endpoint2);
+
+/**
+ * dprc_disconnect() - Disconnect one endpoint to remove its network connection
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @token:	Token of DPRC object
+ * @endpoint:	Endpoint configuration parameters
+ *
+ * Return:	'0' on Success; Error code otherwise.
+ */
+int dprc_disconnect(struct fsl_mc_io		*mc_io,
+		    uint16_t			token,
+		    const struct dprc_endpoint	*endpoint);
+
+/**
+* dprc_get_connection() - Get connected endpoint and link status if connection
+*			exists.
+* @mc_io		Pointer to MC portal's I/O object
+* @token		Token of DPRC object
+* @endpoint1	Endpoint 1 configuration parameters
+* @endpoint2	Returned endpoint 2 configuration parameters
+* @state:	Returned link state: 1 - link is up, 0 - link is down
+*
+* Return:     '0' on Success; -ENAVAIL if connection does not exist.
+*/
+int dprc_get_connection(struct fsl_mc_io		*mc_io,
+			uint16_t			token,
+			const struct dprc_endpoint	*endpoint1,
+			struct dprc_endpoint		*endpoint2,
+			int				*state);
+
+#endif /* _FSL_DPRC_H */
+
diff --git a/drivers/staging/fsl-mc/include/mc-cmd.h b/drivers/staging/fsl-mc/include/mc-cmd.h
new file mode 100644
index 000000000000..32501e020054
--- /dev/null
+++ b/drivers/staging/fsl-mc/include/mc-cmd.h
@@ -0,0 +1,113 @@
+/* Copyright 2013-2015 Freescale Semiconductor Inc.
+ *
+ * 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 of the above-listed copyright holders nor the
+ * names of any contributors may be used to endorse or promote products
+ * derived from this software without specific prior written permission.
+ *
+ *
+ * ALTERNATIVELY, this software may be distributed under the terms of the
+ * GNU General Public License ("GPL") as published by the Free Software
+ * Foundation, either version 2 of that License or (at your option) any
+ * later version.
+ *
+ * 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 HOLDERS OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+#ifndef __FSL_MC_CMD_H
+#define __FSL_MC_CMD_H
+
+#define MC_CMD_NUM_OF_PARAMS	7
+
+#define MAKE_UMASK64(_width) \
+	((uint64_t)((_width) < 64 ? ((uint64_t)1 << (_width)) - 1 : -1))
+
+static inline uint64_t mc_enc(int lsoffset, int width, uint64_t val)
+{
+	return (uint64_t)(((uint64_t)val & MAKE_UMASK64(width)) << lsoffset);
+}
+
+static inline uint64_t mc_dec(uint64_t val, int lsoffset, int width)
+{
+	return (uint64_t)((val >> lsoffset) & MAKE_UMASK64(width));
+}
+
+struct mc_command {
+	uint64_t header;
+	uint64_t params[MC_CMD_NUM_OF_PARAMS];
+};
+
+enum mc_cmd_status {
+	MC_CMD_STATUS_OK = 0x0, /* Completed successfully */
+	MC_CMD_STATUS_READY = 0x1, /* Ready to be processed */
+	MC_CMD_STATUS_AUTH_ERR = 0x3, /* Authentication error */
+	MC_CMD_STATUS_NO_PRIVILEGE = 0x4, /* No privilege */
+	MC_CMD_STATUS_DMA_ERR = 0x5, /* DMA or I/O error */
+	MC_CMD_STATUS_CONFIG_ERR = 0x6, /* Configuration error */
+	MC_CMD_STATUS_TIMEOUT = 0x7, /* Operation timed out */
+	MC_CMD_STATUS_NO_RESOURCE = 0x8, /* No resources */
+	MC_CMD_STATUS_NO_MEMORY = 0x9, /* No memory available */
+	MC_CMD_STATUS_BUSY = 0xA, /* Device is busy */
+	MC_CMD_STATUS_UNSUPPORTED_OP = 0xB, /* Unsupported operation */
+	MC_CMD_STATUS_INVALID_STATE = 0xC /* Invalid state */
+};
+
+#define MC_CMD_HDR_CMDID_O	52	/* Command ID field offset */
+#define MC_CMD_HDR_CMDID_S	12	/* Command ID field size */
+#define MC_CMD_HDR_TOKEN_O	38	/* Token field offset */
+#define MC_CMD_HDR_TOKEN_S	10	/* Token field size */
+#define MC_CMD_HDR_STATUS_O	16	/* Status field offset */
+#define MC_CMD_HDR_STATUS_S	8	/* Status field size*/
+#define MC_CMD_HDR_PRI_O	15	/* Priority field offset */
+#define MC_CMD_HDR_PRI_S	1	/* Priority field size */
+
+#define MC_CMD_HDR_READ_STATUS(_hdr) \
+	((enum mc_cmd_status)mc_dec((_hdr), \
+		MC_CMD_HDR_STATUS_O, MC_CMD_HDR_STATUS_S))
+
+#define MC_CMD_HDR_READ_TOKEN(_hdr) \
+	((uint16_t)mc_dec((_hdr), MC_CMD_HDR_TOKEN_O, MC_CMD_HDR_TOKEN_S))
+
+#define MC_CMD_PRI_LOW		0 /* Low Priority command indication */
+#define MC_CMD_PRI_HIGH		1 /* High Priority command indication */
+
+#define MC_EXT_OP(_ext, _param, _offset, _width, _type, _arg) \
+	((_ext)[_param] |= mc_enc((_offset), (_width), _arg))
+
+#define MC_CMD_OP(_cmd, _param, _offset, _width, _type, _arg) \
+	((_cmd).params[_param] |= mc_enc((_offset), (_width), _arg))
+
+#define MC_RSP_OP(_cmd, _param, _offset, _width, _type, _arg) \
+	(_arg = (_type)mc_dec(_cmd.params[_param], (_offset), (_width)))
+
+static inline uint64_t mc_encode_cmd_header(uint16_t cmd_id,
+					    uint8_t priority,
+					    uint16_t token)
+{
+	uint64_t hdr;
+
+	hdr = mc_enc(MC_CMD_HDR_CMDID_O, MC_CMD_HDR_CMDID_S, cmd_id);
+	hdr |= mc_enc(MC_CMD_HDR_TOKEN_O, MC_CMD_HDR_TOKEN_S, token);
+	hdr |= mc_enc(MC_CMD_HDR_PRI_O, MC_CMD_HDR_PRI_S, priority);
+	hdr |= mc_enc(MC_CMD_HDR_STATUS_O, MC_CMD_HDR_STATUS_S,
+		       MC_CMD_STATUS_READY);
+
+	return hdr;
+}
+
+#endif /* __FSL_MC_CMD_H */
diff --git a/drivers/staging/fsl-mc/include/mc-private.h b/drivers/staging/fsl-mc/include/mc-private.h
new file mode 100644
index 000000000000..c045f49f227e
--- /dev/null
+++ b/drivers/staging/fsl-mc/include/mc-private.h
@@ -0,0 +1,116 @@
+/*
+ * Freescale Management Complex (MC) bus private declarations
+ *
+ * Copyright (C) 2014 Freescale Semiconductor, Inc.
+ * Author: German Rivera <German.Rivera@freescale.com>
+ *
+ * This file is licensed under the terms of the GNU General Public
+ * License version 2. This program is licensed "as is" without any
+ * warranty of any kind, whether express or implied.
+ */
+#ifndef _FSL_MC_PRIVATE_H_
+#define _FSL_MC_PRIVATE_H_
+
+#include "../include/mc.h"
+#include <linux/mutex.h>
+#include <linux/stringify.h>
+
+#define FSL_MC_DPRC_DRIVER_NAME    "fsl_mc_dprc"
+
+#define FSL_MC_DEVICE_MATCH(_mc_dev, _obj_desc) \
+	(strcmp((_mc_dev)->obj_desc.type, (_obj_desc)->type) == 0 && \
+	 (_mc_dev)->obj_desc.id == (_obj_desc)->id)
+
+#define FSL_MC_IS_ALLOCATABLE(_obj_type) \
+	(strcmp(_obj_type, "dpbp") == 0 || \
+	 strcmp(_obj_type, "dpmcp") == 0 || \
+	 strcmp(_obj_type, "dpcon") == 0)
+
+/**
+ * struct fsl_mc - Private data of a "fsl,qoriq-mc" platform device
+ * @root_mc_bus_dev: MC object device representing the root DPRC
+ * @addr_translation_ranges: array of bus to system address translation ranges
+ */
+struct fsl_mc {
+	struct fsl_mc_device *root_mc_bus_dev;
+	uint8_t num_translation_ranges;
+	struct fsl_mc_addr_translation_range *translation_ranges;
+};
+
+/**
+ * struct fsl_mc_addr_translation_range - bus to system address translation
+ * range
+ * @start_mc_addr: Start MC address of the range being translated
+ * @end_mc_addr: MC address of the first byte after the range (last MC
+ * address of the range is end_mc_addr - 1)
+ * @start_phys_addr: system physical address corresponding to start_mc_addr
+ */
+struct fsl_mc_addr_translation_range {
+	uint64_t start_mc_addr;
+	uint64_t end_mc_addr;
+	phys_addr_t start_phys_addr;
+};
+
+/**
+ * struct fsl_mc_resource_pool - Pool of MC resources of a given
+ * type
+ * @type: type of resources in the pool
+ * @max_count: maximum number of resources in the pool
+ * @free_count: number of free resources in the pool
+ * @mutex: mutex to serialize access to the pool's free list
+ * @free_list: anchor node of list of free resources in the pool
+ * @mc_bus: pointer to the MC bus that owns this resource pool
+ */
+struct fsl_mc_resource_pool {
+	enum fsl_mc_pool_type type;
+	int16_t max_count;
+	int16_t free_count;
+	struct mutex mutex;	/* serializes access to free_list */
+	struct list_head free_list;
+	struct fsl_mc_bus *mc_bus;
+};
+
+/**
+ * struct fsl_mc_bus - logical bus that corresponds to a physical DPRC
+ * @mc_dev: fsl-mc device for the bus device itself.
+ * @resource_pools: array of resource pools (one pool per resource type)
+ * for this MC bus. These resources represent allocatable entities
+ * from the physical DPRC.
+ * @scan_mutex: Serializes bus scanning
+ */
+struct fsl_mc_bus {
+	struct fsl_mc_device mc_dev;
+	struct fsl_mc_resource_pool resource_pools[FSL_MC_NUM_POOL_TYPES];
+	struct mutex scan_mutex;    /* serializes bus scanning */
+};
+
+#define to_fsl_mc_bus(_mc_dev) \
+	container_of(_mc_dev, struct fsl_mc_bus, mc_dev)
+
+int __must_check fsl_mc_device_add(struct dprc_obj_desc *obj_desc,
+				   struct fsl_mc_io *mc_io,
+				   struct device *parent_dev,
+				   struct fsl_mc_device **new_mc_dev);
+
+void fsl_mc_device_remove(struct fsl_mc_device *mc_dev);
+
+int dprc_scan_container(struct fsl_mc_device *mc_bus_dev);
+
+int dprc_scan_objects(struct fsl_mc_device *mc_bus_dev);
+
+int __init dprc_driver_init(void);
+
+void __exit dprc_driver_exit(void);
+
+int __init fsl_mc_allocator_driver_init(void);
+
+void __exit fsl_mc_allocator_driver_exit(void);
+
+int __must_check fsl_mc_resource_allocate(struct fsl_mc_bus *mc_bus,
+					  enum fsl_mc_pool_type pool_type,
+					  struct fsl_mc_resource
+							  **new_resource);
+
+void fsl_mc_resource_free(struct fsl_mc_resource *resource);
+
+#endif /* _FSL_MC_PRIVATE_H_ */
diff --git a/drivers/staging/fsl-mc/include/mc-sys.h b/drivers/staging/fsl-mc/include/mc-sys.h
new file mode 100644
index 000000000000..cb3b5a296615
--- /dev/null
+++ b/drivers/staging/fsl-mc/include/mc-sys.h
@@ -0,0 +1,76 @@
+/* Copyright 2013-2014 Freescale Semiconductor Inc.
+ *
+ * Interface of the I/O services to send MC commands to the MC hardware
+ *
+ * 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 of the above-listed copyright holders nor the
+ *       names of any contributors may be used to endorse or promote products
+ *       derived from this software without specific prior written permission.
+ *
+ *
+ * ALTERNATIVELY, this software may be distributed under the terms of the
+ * GNU General Public License ("GPL") as published by the Free Software
+ * Foundation, either version 2 of that License or (at your option) any
+ * later version.
+ *
+ * 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 HOLDERS OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef _FSL_MC_SYS_H
+#define _FSL_MC_SYS_H
+
+#include <linux/types.h>
+#include <linux/errno.h>
+#include <linux/io.h>
+#include <linux/dma-mapping.h>
+
+struct fsl_mc_resource;
+struct mc_command;
+
+/**
+ * struct fsl_mc_io - MC I/O object to be passed-in to mc_send_command()
+ * @dev: device associated with this Mc I/O object
+ * @flags: flags for mc_send_command()
+ * @portal_size: MC command portal size in bytes
+ * @portal_phys_addr: MC command portal physical address
+ * @portal_virt_addr: MC command portal virtual address
+ * @resource: generic resource associated with the MC portal if
+ * the MC portal came from a resource pool, or NULL if the MC portal
+ * is permanently bound to a device (e.g., a DPRC)
+ */
+struct fsl_mc_io {
+	struct device *dev;
+	uint32_t flags;
+	uint32_t portal_size;
+	phys_addr_t portal_phys_addr;
+	void __iomem *portal_virt_addr;
+	struct fsl_mc_resource *resource;
+};
+
+int __must_check fsl_create_mc_io(struct device *dev,
+				  phys_addr_t mc_portal_phys_addr,
+				  uint32_t mc_portal_size,
+				  struct fsl_mc_resource *resource,
+				  uint32_t flags, struct fsl_mc_io **new_mc_io);
+
+void fsl_destroy_mc_io(struct fsl_mc_io *mc_io);
+
+int mc_send_command(struct fsl_mc_io *mc_io, struct mc_command *cmd);
+
+#endif /* _FSL_MC_SYS_H */
diff --git a/drivers/staging/fsl-mc/include/mc.h b/drivers/staging/fsl-mc/include/mc.h
new file mode 100644
index 000000000000..fa02ef0529e7
--- /dev/null
+++ b/drivers/staging/fsl-mc/include/mc.h
@@ -0,0 +1,201 @@
+/*
+ * Freescale Management Complex (MC) bus public interface
+ *
+ * Copyright (C) 2014 Freescale Semiconductor, Inc.
+ * Author: German Rivera <German.Rivera@freescale.com>
+ *
+ * This file is licensed under the terms of the GNU General Public
+ * License version 2. This program is licensed "as is" without any
+ * warranty of any kind, whether express or implied.
+ */
+#ifndef _FSL_MC_H_
+#define _FSL_MC_H_
+
+#include <linux/device.h>
+#include <linux/mod_devicetable.h>
+#include <linux/list.h>
+#include "../include/dprc.h"
+
+#define FSL_MC_VENDOR_FREESCALE	0x1957
+
+struct fsl_mc_device;
+struct fsl_mc_io;
+
+/**
+ * struct fsl_mc_driver - MC object device driver object
+ * @driver: Generic device driver
+ * @match_id_table: table of supported device matching Ids
+ * @probe: Function called when a device is added
+ * @remove: Function called when a device is removed
+ * @shutdown: Function called at shutdown time to quiesce the device
+ * @suspend: Function called when a device is stopped
+ * @resume: Function called when a device is resumed
+ *
+ * Generic DPAA device driver object for device drivers that are registered
+ * with a DPRC bus. This structure is to be embedded in each device-specific
+ * driver structure.
+ */
+struct fsl_mc_driver {
+	struct device_driver driver;
+	const struct fsl_mc_device_match_id *match_id_table;
+	int (*probe)(struct fsl_mc_device *dev);
+	int (*remove)(struct fsl_mc_device *dev);
+	void (*shutdown)(struct fsl_mc_device *dev);
+	int (*suspend)(struct fsl_mc_device *dev, pm_message_t state);
+	int (*resume)(struct fsl_mc_device *dev);
+};
+
+#define to_fsl_mc_driver(_drv) \
+	container_of(_drv, struct fsl_mc_driver, driver)
+
+/**
+ * struct fsl_mc_device_match_id - MC object device Id entry for driver matching
+ * @vendor: vendor ID
+ * @obj_type: MC object type
+ * @ver_major: MC object version major number
+ * @ver_minor: MC object version minor number
+ *
+ * Type of entries in the "device Id" table for MC object devices supported by
+ * a MC object device driver. The last entry of the table has vendor set to 0x0
+ */
+struct fsl_mc_device_match_id {
+	uint16_t vendor;
+	const char obj_type[16];
+	uint32_t ver_major;
+	uint32_t ver_minor;
+};
+
+/**
+ * enum fsl_mc_pool_type - Types of allocatable MC bus resources
+ *
+ * Entries in these enum are used as indices in the array of resource
+ * pools of an fsl_mc_bus object.
+ */
+enum fsl_mc_pool_type {
+	FSL_MC_POOL_DPMCP = 0x0,    /* corresponds to "dpmcp" in the MC */
+	FSL_MC_POOL_DPBP,	    /* corresponds to "dpbp" in the MC */
+	FSL_MC_POOL_DPCON,	    /* corresponds to "dpcon" in the MC */
+
+	/*
+	 * NOTE: New resource pool types must be added before this entry
+	 */
+	FSL_MC_NUM_POOL_TYPES
+};
+
+/**
+ * struct fsl_mc_resource - MC generic resource
+ * @type: type of resource
+ * @id: unique MC resource Id within the resources of the same type
+ * @data: pointer to resource-specific data if the resource is currently
+ * allocated, or NULL if the resource is not currently allocated.
+ * @parent_pool: pointer to the parent resource pool from which this
+ * resource is allocated from.
+ * @node: Node in the free list of the corresponding resource pool
+ *
+ * NOTE: This structure is to be embedded as a field of specific
+ * MC resource structures.
+ */
+struct fsl_mc_resource {
+	enum fsl_mc_pool_type type;
+	int32_t id;
+	void *data;
+	struct fsl_mc_resource_pool *parent_pool;
+	struct list_head node;
+};
+
+/**
+ * Bit masks for a MC object device (struct fsl_mc_device) flags
+ */
+#define FSL_MC_IS_DPRC	0x0001
+
+/**
+ * Default DMA mask for devices on a fsl-mc bus
+ */
+#define FSL_MC_DEFAULT_DMA_MASK	(~0ULL)
+
+/**
+ * struct fsl_mc_device - MC object device object
+ * @dev: Linux driver model device object
+ * @dma_mask: Default DMA mask
+ * @flags: MC object device flags
+ * @icid: Isolation context ID for the device
+ * @mc_handle: MC handle for the corresponding MC object opened
+ * @mc_io: Pointer to MC IO object assigned to this device or
+ * NULL if none.
+ * @obj_desc: MC description of the DPAA device
+ * @regions: pointer to array of MMIO region entries
+ * @resource: generic resource associated with this MC object device, if any.
+ *
+ * Generic device object for MC object devices that are "attached" to a
+ * MC bus.
+ *
+ * NOTES:
+ * - For a non-DPRC object its icid is the same as its parent DPRC's icid.
+ * - The SMMU notifier callback gets invoked after device_add() has been
+ *   called for an MC object device, but before the device-specific probe
+ *   callback gets called.
+ * - DP_OBJ_DPRC objects are the only MC objects that have built-in MC
+ *   portals. For all other MC objects, their device drivers are responsible for
+ *   allocating MC portals for them by calling fsl_mc_portal_allocate().
+ * - Some types of MC objects (e.g., DP_OBJ_DPBP, DP_OBJ_DPCON) are
+ *   treated as resources that can be allocated/deallocated from the
+ *   corresponding resource pool in the object's parent DPRC, using the
+ *   fsl_mc_object_allocate()/fsl_mc_object_free() functions. These MC objects
+ *   are known as "allocatable" objects. For them, the corresponding
+ *   fsl_mc_device's 'resource' points to the associated resource object.
+ *   For MC objects that are not allocatable (e.g., DP_OBJ_DPRC, DP_OBJ_DPNI),
+ *   'resource' is NULL.
+ */
+struct fsl_mc_device {
+	struct device dev;
+	uint64_t dma_mask;
+	uint16_t flags;
+	uint16_t icid;
+	uint16_t mc_handle;
+	struct fsl_mc_io *mc_io;
+	struct dprc_obj_desc obj_desc;
+	struct resource *regions;
+	struct fsl_mc_resource *resource;
+};
+
+#define to_fsl_mc_device(_dev) \
+	container_of(_dev, struct fsl_mc_device, dev)
+
+/*
+ * module_fsl_mc_driver() - Helper macro for drivers that don't do
+ * anything special in module init/exit.  This eliminates a lot of
+ * boilerplate.  Each module may only use this macro once, and
+ * calling it replaces module_init() and module_exit()
+ */
+#define module_fsl_mc_driver(__fsl_mc_driver) \
+	module_driver(__fsl_mc_driver, fsl_mc_driver_register, \
+		      fsl_mc_driver_unregister)
+
+/*
+ * Macro to avoid include chaining to get THIS_MODULE
+ */
+#define fsl_mc_driver_register(drv) \
+	__fsl_mc_driver_register(drv, THIS_MODULE)
+
+int __must_check __fsl_mc_driver_register(struct fsl_mc_driver *fsl_mc_driver,
+					  struct module *owner);
+
+void fsl_mc_driver_unregister(struct fsl_mc_driver *driver);
+
+int __must_check fsl_mc_portal_allocate(struct fsl_mc_device *mc_dev,
+					uint16_t mc_io_flags,
+					struct fsl_mc_io **new_mc_io);
+
+void fsl_mc_portal_free(struct fsl_mc_io *mc_io);
+
+int fsl_mc_portal_reset(struct fsl_mc_io *mc_io);
+
+int __must_check fsl_mc_object_allocate(struct fsl_mc_device *mc_dev,
+					enum fsl_mc_pool_type pool_type,
+					struct fsl_mc_device **new_mc_adev);
+
+void fsl_mc_object_free(struct fsl_mc_device *mc_adev);
+
+extern struct bus_type fsl_mc_bus_type;
+
+#endif /* _FSL_MC_H_ */
diff --git a/drivers/staging/ft1000/ft1000-pcmcia/ft1000_cs.c b/drivers/staging/ft1000/ft1000-pcmcia/ft1000_cs.c
index 922478e1cb57..e5cc5bedf031 100644
--- a/drivers/staging/ft1000/ft1000-pcmcia/ft1000_cs.c
+++ b/drivers/staging/ft1000/ft1000-pcmcia/ft1000_cs.c
@@ -155,15 +155,4 @@ static struct pcmcia_driver ft1000_cs_driver = {
 	.resume		= ft1000_resume,
 };
 
-static int __init init_ft1000_cs(void)
-{
-	return pcmcia_register_driver(&ft1000_cs_driver);
-}
-
-static void __exit exit_ft1000_cs(void)
-{
-	pcmcia_unregister_driver(&ft1000_cs_driver);
-}
-
-module_init(init_ft1000_cs);
-module_exit(exit_ft1000_cs);
+module_pcmcia_driver(ft1000_cs_driver);
diff --git a/drivers/staging/ft1000/ft1000-pcmcia/ft1000_dnld.c b/drivers/staging/ft1000/ft1000-pcmcia/ft1000_dnld.c
index 06b0e9cfb9b1..83683e9a1451 100644
--- a/drivers/staging/ft1000/ft1000-pcmcia/ft1000_dnld.c
+++ b/drivers/staging/ft1000/ft1000-pcmcia/ft1000_dnld.c
@@ -116,7 +116,7 @@ struct dsp_image_info {
 
 void card_bootload(struct net_device *dev)
 {
-	struct ft1000_info *info = (struct ft1000_info *)netdev_priv(dev);
+	struct ft1000_info *info = netdev_priv(dev);
 	unsigned long flags;
 	u32 *pdata;
 	u32 size;
@@ -148,7 +148,7 @@ void card_bootload(struct net_device *dev)
 
 u16 get_handshake(struct net_device *dev, u16 expected_value)
 {
-	struct ft1000_info *info = (struct ft1000_info *)netdev_priv(dev);
+	struct ft1000_info *info = netdev_priv(dev);
 	u16 handshake;
 	u32 tempx;
 	int loopcnt;
@@ -182,7 +182,7 @@ u16 get_handshake(struct net_device *dev, u16 expected_value)
 
 void put_handshake(struct net_device *dev, u16 handshake_value)
 {
-	struct ft1000_info *info = (struct ft1000_info *)netdev_priv(dev);
+	struct ft1000_info *info = netdev_priv(dev);
 	u32 tempx;
 
 	if (info->AsicID == ELECTRABUZZ_ID) {
@@ -198,7 +198,7 @@ void put_handshake(struct net_device *dev, u16 handshake_value)
 
 u16 get_request_type(struct net_device *dev)
 {
-	struct ft1000_info *info = (struct ft1000_info *)netdev_priv(dev);
+	struct ft1000_info *info = netdev_priv(dev);
 	u16 request_type;
 	u32 tempx;
 
@@ -217,7 +217,7 @@ u16 get_request_type(struct net_device *dev)
 
 long get_request_value(struct net_device *dev)
 {
-	struct ft1000_info *info = (struct ft1000_info *)netdev_priv(dev);
+	struct ft1000_info *info = netdev_priv(dev);
 	long value;
 	u16 w_val;
 
@@ -246,7 +246,7 @@ long get_request_value(struct net_device *dev)
 
 void put_request_value(struct net_device *dev, long lvalue)
 {
-	struct ft1000_info *info = (struct ft1000_info *)netdev_priv(dev);
+	struct ft1000_info *info = netdev_priv(dev);
 	u16 size;
 	u32 tempx;
 
@@ -276,8 +276,8 @@ u16 hdr_checksum(struct pseudo_hdr *pHdr)
 	u16 *usPtr = (u16 *)pHdr;
 	u16 chksum;
 
-	chksum = ((((((usPtr[0] ^ usPtr[1]) ^ usPtr[2]) ^ usPtr[3]) ^
-		    usPtr[4]) ^ usPtr[5]) ^ usPtr[6]);
+	chksum = (((((usPtr[0] ^ usPtr[1]) ^ usPtr[2]) ^ usPtr[3]) ^
+		    usPtr[4]) ^ usPtr[5]) ^ usPtr[6];
 
 	return chksum;
 }
@@ -285,7 +285,7 @@ u16 hdr_checksum(struct pseudo_hdr *pHdr)
 int card_download(struct net_device *dev, const u8 *pFileStart,
 		  size_t FileLength)
 {
-	struct ft1000_info *info = (struct ft1000_info *)netdev_priv(dev);
+	struct ft1000_info *info = netdev_priv(dev);
 	int Status = SUCCESS;
 	u32 uiState;
 	u16 handshake;
@@ -713,10 +713,10 @@ int card_download(struct net_device *dev, const u8 *pFileStart,
 
 				/* Get buffer for provisioning data */
 				pbuffer =
-					kmalloc((usHdrLength + sizeof(struct pseudo_hdr)),
+					kmalloc(usHdrLength + sizeof(struct pseudo_hdr),
 						GFP_ATOMIC);
 				if (pbuffer) {
-					memcpy(pbuffer, (void *)pUcFile,
+					memcpy(pbuffer, pUcFile,
 					       (u32) (usHdrLength +
 						      sizeof(struct pseudo_hdr)));
 					/* link provisioning data */
diff --git a/drivers/staging/ft1000/ft1000-pcmcia/ft1000_hw.c b/drivers/staging/ft1000/ft1000-pcmcia/ft1000_hw.c
index 017c3b92f51b..eecfa377054d 100644
--- a/drivers/staging/ft1000/ft1000-pcmcia/ft1000_hw.c
+++ b/drivers/staging/ft1000/ft1000-pcmcia/ft1000_hw.c
@@ -28,8 +28,8 @@
 #include <linux/timer.h>
 #include <linux/interrupt.h>
 #include <linux/in.h>
-#include <asm/io.h>
-#include <asm/bitops.h>
+#include <linux/io.h>
+#include <linux/bitops.h>
 
 #include <linux/netdevice.h>
 #include <linux/etherdevice.h>
@@ -67,8 +67,7 @@ static void ft1000_disable_interrupts(struct net_device *dev);
 
 /* new kernel */
 MODULE_AUTHOR("");
-MODULE_DESCRIPTION
-("Support for Flarion Flash OFDM NIC Device. Support for PCMCIA when used with ft1000_cs.");
+MODULE_DESCRIPTION("Support for Flarion Flash OFDM NIC Device. Support for PCMCIA when used with ft1000_cs.");
 MODULE_LICENSE("GPL");
 MODULE_SUPPORTED_DEVICE("FT1000");
 
@@ -267,7 +266,8 @@ void ft1000_write_dpram_mag_32(struct net_device *dev, int offset, u32 value)
 /*---------------------------------------------------------------------------
 
   Function:   ft1000_enable_interrupts
-  Description: This function will enable interrupts base on the current interrupt mask.
+  Description: This function will enable interrupts base on the current
+	       interrupt mask.
   Input:
   dev    - device structure
   Output:
@@ -303,6 +303,41 @@ static void ft1000_disable_interrupts(struct net_device *dev)
 }
 
 /*---------------------------------------------------------------------------
+  Function:	ft1000_read_dsp_timer
+  Description:	This function reads the DSP timer and stores its value in the
+		DSP_TIME field of the ft1000_info struct passed as argument
+  Input:
+  dev    - device structure
+  info   - ft1000_info structure
+  Output:
+  None.
+
+  -------------------------------------------------------------------------*/
+static void ft1000_read_dsp_timer(struct net_device *dev,
+				  struct ft1000_info *info)
+{
+	if (info->AsicID == ELECTRABUZZ_ID) {
+		info->DSP_TIME[0] = ft1000_read_dpram(dev, FT1000_DSP_TIMER0);
+		info->DSP_TIME[1] = ft1000_read_dpram(dev, FT1000_DSP_TIMER1);
+		info->DSP_TIME[2] = ft1000_read_dpram(dev, FT1000_DSP_TIMER2);
+		info->DSP_TIME[3] = ft1000_read_dpram(dev, FT1000_DSP_TIMER3);
+	} else {
+		info->DSP_TIME[0] =
+			ft1000_read_dpram_mag_16(dev, FT1000_MAG_DSP_TIMER0,
+						 FT1000_MAG_DSP_TIMER0_INDX);
+		info->DSP_TIME[1] =
+			ft1000_read_dpram_mag_16(dev, FT1000_MAG_DSP_TIMER1,
+						 FT1000_MAG_DSP_TIMER1_INDX);
+		info->DSP_TIME[2] =
+			ft1000_read_dpram_mag_16(dev, FT1000_MAG_DSP_TIMER2,
+						 FT1000_MAG_DSP_TIMER2_INDX);
+		info->DSP_TIME[3] =
+			ft1000_read_dpram_mag_16(dev, FT1000_MAG_DSP_TIMER3,
+						 FT1000_MAG_DSP_TIMER3_INDX);
+	}
+}
+
+/*---------------------------------------------------------------------------
 
   Function:   ft1000_reset_asic
   Description: This function will call the Card Service function to reset the
@@ -327,7 +362,7 @@ static void ft1000_reset_asic(struct net_device *dev)
 	 */
 	if (info->AsicID == MAGNEMITE_ID) {
 		ft1000_write_reg(dev, FT1000_REG_RESET,
-				 (DSP_RESET_BIT | ASIC_RESET_BIT));
+				 DSP_RESET_BIT | ASIC_RESET_BIT);
 	}
 	mdelay(1);
 	if (info->AsicID == ELECTRABUZZ_ID) {
@@ -364,6 +399,7 @@ static int ft1000_reset_card(struct net_device *dev)
 	int i;
 	unsigned long flags;
 	struct prov_record *ptr;
+	struct prov_record *tmp;
 
 	info->CardReady = 0;
 	info->ProgConStat = 0;
@@ -373,9 +409,8 @@ static int ft1000_reset_card(struct net_device *dev)
 	/* del_timer(&poll_timer); */
 
 	/* Make sure we free any memory reserve for provisioning */
-	while (list_empty(&info->prov_list) == 0) {
+	list_for_each_entry_safe(ptr, tmp, &info->prov_list, list) {
 		pr_debug("deleting provisioning record\n");
-		ptr = list_entry(info->prov_list.next, struct prov_record, list);
 		list_del(&ptr->list);
 		kfree(ptr->pprov_data);
 		kfree(ptr);
@@ -387,7 +422,7 @@ static int ft1000_reset_card(struct net_device *dev)
 	} else {
 		pr_debug("resetting ASIC and DSP\n");
 		ft1000_write_reg(dev, FT1000_REG_RESET,
-				 (DSP_RESET_BIT | ASIC_RESET_BIT));
+				 DSP_RESET_BIT | ASIC_RESET_BIT);
 	}
 
 	/* Copy DSP session record into info block if this is not a coldstart */
@@ -406,7 +441,8 @@ static int ft1000_reset_card(struct net_device *dev)
 					 FT1000_DPRAM_MAG_RX_BASE);
 			for (i = 0; i < MAX_DSP_SESS_REC / 2; i++) {
 				info->DSPSess.MagRec[i] =
-					inl(dev->base_addr + FT1000_REG_MAG_DPDATA);
+					inl(dev->base_addr
+						+ FT1000_REG_MAG_DPDATA);
 			}
 		}
 		spin_unlock_irqrestore(&info->dpram_lock, flags);
@@ -435,11 +471,14 @@ static int ft1000_reset_card(struct net_device *dev)
 		mdelay(10);
 		pr_debug("Take DSP out of reset\n");
 
-		/* Wait for 0xfefe indicating dsp ready before starting download */
+		/*
+		 * Wait for 0xfefe indicating dsp ready before starting
+		 * download
+		 */
 		for (i = 0; i < 50; i++) {
-			tempword =
-				ft1000_read_dpram_mag_16(dev, FT1000_MAG_DPRAM_FEFE,
-							 FT1000_MAG_DPRAM_FEFE_INDX);
+			tempword = ft1000_read_dpram_mag_16(dev,
+						FT1000_MAG_DPRAM_FEFE,
+						FT1000_MAG_DPRAM_FEFE_INDX);
 			if (tempword == 0xfefe)
 				break;
 			mdelay(20);
@@ -459,16 +498,15 @@ static int ft1000_reset_card(struct net_device *dev)
 	if (card_download(dev, fw_entry->data, fw_entry->size)) {
 		pr_debug("card download unsuccessful\n");
 		return false;
-	} else {
-		pr_debug("card download successful\n");
 	}
+	pr_debug("card download successful\n");
 
 	mdelay(10);
 
 	if (info->AsicID == ELECTRABUZZ_ID) {
 		/*
-		 * Need to initialize the FIFO length counter to zero in order to sync up
-		 * with the DSP
+		 * Need to initialize the FIFO length counter to zero in order
+		 * to sync up with the DSP
 		 */
 		info->fifo_cnt = 0;
 		ft1000_write_dpram(dev, FT1000_FIFO_LEN, info->fifo_cnt);
@@ -578,33 +616,7 @@ static void ft1000_hbchk(u_long data)
 		}
 		if (tempword != ho) {
 			pr_info("heartbeat failed - no ho detected\n");
-			if (info->AsicID == ELECTRABUZZ_ID) {
-				info->DSP_TIME[0] =
-					ft1000_read_dpram(dev, FT1000_DSP_TIMER0);
-				info->DSP_TIME[1] =
-					ft1000_read_dpram(dev, FT1000_DSP_TIMER1);
-				info->DSP_TIME[2] =
-					ft1000_read_dpram(dev, FT1000_DSP_TIMER2);
-				info->DSP_TIME[3] =
-					ft1000_read_dpram(dev, FT1000_DSP_TIMER3);
-			} else {
-				info->DSP_TIME[0] =
-					ft1000_read_dpram_mag_16(dev,
-								 FT1000_MAG_DSP_TIMER0,
-								 FT1000_MAG_DSP_TIMER0_INDX);
-				info->DSP_TIME[1] =
-					ft1000_read_dpram_mag_16(dev,
-								 FT1000_MAG_DSP_TIMER1,
-								 FT1000_MAG_DSP_TIMER1_INDX);
-				info->DSP_TIME[2] =
-					ft1000_read_dpram_mag_16(dev,
-								 FT1000_MAG_DSP_TIMER2,
-								 FT1000_MAG_DSP_TIMER2_INDX);
-				info->DSP_TIME[3] =
-					ft1000_read_dpram_mag_16(dev,
-								 FT1000_MAG_DSP_TIMER3,
-								 FT1000_MAG_DSP_TIMER3_INDX);
-			}
+			ft1000_read_dsp_timer(dev, info);
 			info->DrvErrNum = DSP_HB_INFO;
 			if (ft1000_reset_card(dev) == 0) {
 				pr_info("Hardware Failure Detected - PC Card disabled\n");
@@ -625,33 +637,7 @@ static void ft1000_hbchk(u_long data)
 
 		if (tempword & FT1000_DB_HB) {
 			pr_info("heartbeat doorbell not clear by firmware\n");
-			if (info->AsicID == ELECTRABUZZ_ID) {
-				info->DSP_TIME[0] =
-					ft1000_read_dpram(dev, FT1000_DSP_TIMER0);
-				info->DSP_TIME[1] =
-					ft1000_read_dpram(dev, FT1000_DSP_TIMER1);
-				info->DSP_TIME[2] =
-					ft1000_read_dpram(dev, FT1000_DSP_TIMER2);
-				info->DSP_TIME[3] =
-					ft1000_read_dpram(dev, FT1000_DSP_TIMER3);
-			} else {
-				info->DSP_TIME[0] =
-					ft1000_read_dpram_mag_16(dev,
-								 FT1000_MAG_DSP_TIMER0,
-								 FT1000_MAG_DSP_TIMER0_INDX);
-				info->DSP_TIME[1] =
-					ft1000_read_dpram_mag_16(dev,
-								 FT1000_MAG_DSP_TIMER1,
-								 FT1000_MAG_DSP_TIMER1_INDX);
-				info->DSP_TIME[2] =
-					ft1000_read_dpram_mag_16(dev,
-								 FT1000_MAG_DSP_TIMER2,
-								 FT1000_MAG_DSP_TIMER2_INDX);
-				info->DSP_TIME[3] =
-					ft1000_read_dpram_mag_16(dev,
-								 FT1000_MAG_DSP_TIMER3,
-								 FT1000_MAG_DSP_TIMER3_INDX);
-			}
+			ft1000_read_dsp_timer(dev, info);
 			info->DrvErrNum = DSP_HB_INFO;
 			if (ft1000_reset_card(dev) == 0) {
 				pr_info("Hardware Failure Detected - PC Card disabled\n");
@@ -665,8 +651,8 @@ static void ft1000_hbchk(u_long data)
 			return;
 		}
 		/*
-		 * Set dedicated area to hi and ring appropriate doorbell according
-		 * to hi/ho heartbeat protocol
+		 * Set dedicated area to hi and ring appropriate doorbell
+		 * according to hi/ho heartbeat protocol
 		 */
 		if (info->AsicID == ELECTRABUZZ_ID) {
 			ft1000_write_dpram(dev, FT1000_HI_HO, hi);
@@ -688,44 +674,20 @@ static void ft1000_hbchk(u_long data)
 			if (info->AsicID == ELECTRABUZZ_ID)
 				ft1000_write_dpram(dev, FT1000_HI_HO, hi);
 			else
-				ft1000_write_dpram_mag_16(dev, FT1000_MAG_HI_HO, hi_mag, FT1000_MAG_HI_HO_INDX);
+				ft1000_write_dpram_mag_16(dev, FT1000_MAG_HI_HO,
+						hi_mag, FT1000_MAG_HI_HO_INDX);
 
 			if (info->AsicID == ELECTRABUZZ_ID)
 				tempword = ft1000_read_dpram(dev, FT1000_HI_HO);
 			else
-				tempword = ntohs(ft1000_read_dpram_mag_16(dev, FT1000_MAG_HI_HO, FT1000_MAG_HI_HO_INDX));
-
+				tempword = ntohs(ft1000_read_dpram_mag_16(dev,
+							FT1000_MAG_HI_HO,
+							FT1000_MAG_HI_HO_INDX));
 		}
 
 		if (tempword != hi) {
 			pr_info("heartbeat failed - cannot write hi into DPRAM\n");
-			if (info->AsicID == ELECTRABUZZ_ID) {
-				info->DSP_TIME[0] =
-					ft1000_read_dpram(dev, FT1000_DSP_TIMER0);
-				info->DSP_TIME[1] =
-					ft1000_read_dpram(dev, FT1000_DSP_TIMER1);
-				info->DSP_TIME[2] =
-					ft1000_read_dpram(dev, FT1000_DSP_TIMER2);
-				info->DSP_TIME[3] =
-					ft1000_read_dpram(dev, FT1000_DSP_TIMER3);
-			} else {
-				info->DSP_TIME[0] =
-					ft1000_read_dpram_mag_16(dev,
-								 FT1000_MAG_DSP_TIMER0,
-								 FT1000_MAG_DSP_TIMER0_INDX);
-				info->DSP_TIME[1] =
-					ft1000_read_dpram_mag_16(dev,
-								 FT1000_MAG_DSP_TIMER1,
-								 FT1000_MAG_DSP_TIMER1_INDX);
-				info->DSP_TIME[2] =
-					ft1000_read_dpram_mag_16(dev,
-								 FT1000_MAG_DSP_TIMER2,
-								 FT1000_MAG_DSP_TIMER2_INDX);
-				info->DSP_TIME[3] =
-					ft1000_read_dpram_mag_16(dev,
-								 FT1000_MAG_DSP_TIMER3,
-								 FT1000_MAG_DSP_TIMER3_INDX);
-			}
+			ft1000_read_dsp_timer(dev, info);
 			info->DrvErrNum = DSP_HB_INFO;
 			if (ft1000_reset_card(dev) == 0) {
 				pr_info("Hardware Failure Detected - PC Card disabled\n");
@@ -756,7 +718,8 @@ static void ft1000_hbchk(u_long data)
   Output:
 
   -------------------------------------------------------------------------*/
-static void ft1000_send_cmd(struct net_device *dev, u16 *ptempbuffer, int size, u16 qtype)
+static void ft1000_send_cmd(struct net_device *dev, u16 *ptempbuffer, int size,
+			    u16 qtype)
 {
 	struct ft1000_info *info = netdev_priv(dev);
 	int i;
@@ -850,74 +813,73 @@ static bool ft1000_receive_cmd(struct net_device *dev, u16 *pbuffer,
 	unsigned long flags;
 
 	if (info->AsicID == ELECTRABUZZ_ID) {
-		size = (ft1000_read_dpram(dev, *pnxtph)) + sizeof(struct pseudo_hdr);
+		size = ft1000_read_dpram(dev, *pnxtph)
+			+ sizeof(struct pseudo_hdr);
 	} else {
-		size =
-			ntohs(ft1000_read_dpram_mag_16
-			      (dev, FT1000_MAG_PH_LEN,
-			       FT1000_MAG_PH_LEN_INDX)) + sizeof(struct pseudo_hdr);
+		size = ntohs(ft1000_read_dpram_mag_16(dev, FT1000_MAG_PH_LEN,
+						      FT1000_MAG_PH_LEN_INDX))
+				+ sizeof(struct pseudo_hdr);
 	}
 	if (size > maxsz) {
 		pr_debug("Invalid command length = %d\n", size);
 		return false;
+	}
+	ppseudohdr = (u16 *)pbuffer;
+	spin_lock_irqsave(&info->dpram_lock, flags);
+	if (info->AsicID == ELECTRABUZZ_ID) {
+		ft1000_write_reg(dev, FT1000_REG_DPRAM_ADDR,
+				 FT1000_DPRAM_RX_BASE + 2);
+		for (i = 0; i <= (size >> 1); i++) {
+			tempword =
+				ft1000_read_reg(dev, FT1000_REG_DPRAM_DATA);
+			*pbuffer++ = ntohs(tempword);
+		}
 	} else {
-		ppseudohdr = (u16 *)pbuffer;
-		spin_lock_irqsave(&info->dpram_lock, flags);
-		if (info->AsicID == ELECTRABUZZ_ID) {
-			ft1000_write_reg(dev, FT1000_REG_DPRAM_ADDR,
-					 FT1000_DPRAM_RX_BASE + 2);
-			for (i = 0; i <= (size >> 1); i++) {
-				tempword =
-					ft1000_read_reg(dev, FT1000_REG_DPRAM_DATA);
-				*pbuffer++ = ntohs(tempword);
-			}
-		} else {
-			ft1000_write_reg(dev, FT1000_REG_DPRAM_ADDR,
-					 FT1000_DPRAM_MAG_RX_BASE);
-			*pbuffer = inw(dev->base_addr + FT1000_REG_MAG_DPDATAH);
-			pr_debug("received data = 0x%x\n", *pbuffer);
-			pbuffer++;
-			ft1000_write_reg(dev, FT1000_REG_DPRAM_ADDR,
-					 FT1000_DPRAM_MAG_RX_BASE + 1);
-			for (i = 0; i <= (size >> 2); i++) {
-				*pbuffer =
-					inw(dev->base_addr +
-					    FT1000_REG_MAG_DPDATAL);
-				pbuffer++;
-				*pbuffer =
-					inw(dev->base_addr +
-					    FT1000_REG_MAG_DPDATAH);
-				pbuffer++;
-			}
-			/* copy odd aligned word */
-			*pbuffer = inw(dev->base_addr + FT1000_REG_MAG_DPDATAL);
-			pr_debug("received data = 0x%x\n", *pbuffer);
+		ft1000_write_reg(dev, FT1000_REG_DPRAM_ADDR,
+				 FT1000_DPRAM_MAG_RX_BASE);
+		*pbuffer = inw(dev->base_addr + FT1000_REG_MAG_DPDATAH);
+		pr_debug("received data = 0x%x\n", *pbuffer);
+		pbuffer++;
+		ft1000_write_reg(dev, FT1000_REG_DPRAM_ADDR,
+				 FT1000_DPRAM_MAG_RX_BASE + 1);
+		for (i = 0; i <= (size >> 2); i++) {
+			*pbuffer =
+				inw(dev->base_addr +
+				    FT1000_REG_MAG_DPDATAL);
 			pbuffer++;
-			*pbuffer = inw(dev->base_addr + FT1000_REG_MAG_DPDATAH);
-			pr_debug("received data = 0x%x\n", *pbuffer);
+			*pbuffer =
+				inw(dev->base_addr +
+				    FT1000_REG_MAG_DPDATAH);
 			pbuffer++;
 		}
-		if (size & 0x0001) {
-			/* copy odd byte from fifo */
-			tempword = ft1000_read_reg(dev, FT1000_REG_DPRAM_DATA);
-			*pbuffer = ntohs(tempword);
-		}
-		spin_unlock_irqrestore(&info->dpram_lock, flags);
+		/* copy odd aligned word */
+		*pbuffer = inw(dev->base_addr + FT1000_REG_MAG_DPDATAL);
+		pr_debug("received data = 0x%x\n", *pbuffer);
+		pbuffer++;
+		*pbuffer = inw(dev->base_addr + FT1000_REG_MAG_DPDATAH);
+		pr_debug("received data = 0x%x\n", *pbuffer);
+		pbuffer++;
+	}
+	if (size & 0x0001) {
+		/* copy odd byte from fifo */
+		tempword = ft1000_read_reg(dev, FT1000_REG_DPRAM_DATA);
+		*pbuffer = ntohs(tempword);
+	}
+	spin_unlock_irqrestore(&info->dpram_lock, flags);
 
-		/*
-		 * Check if pseudo header checksum is good
-		 * Calculate pseudo header checksum
-		 */
-		tempword = *ppseudohdr++;
-		for (i = 1; i < 7; i++)
-			tempword ^= *ppseudohdr++;
-		if ((tempword != *ppseudohdr)) {
-			pr_debug("Pseudo header checksum mismatch\n");
-			/* Drop this message */
-			return false;
-		}
-		return true;
+	/*
+	 * Check if pseudo header checksum is good
+	 * Calculate pseudo header checksum
+	 */
+	tempword = *ppseudohdr++;
+	for (i = 1; i < 7; i++)
+		tempword ^= *ppseudohdr++;
+	if (tempword != *ppseudohdr) {
+		pr_debug("Pseudo header checksum mismatch\n");
+		/* Drop this message */
+		return false;
 	}
+	return true;
 }
 
 /*---------------------------------------------------------------------------
@@ -957,7 +919,10 @@ static void ft1000_proc_drvmsg(struct net_device *dev)
 
 	if (ft1000_receive_cmd(dev, &cmdbuffer[0], MAX_CMD_SQSIZE, &tempword)) {
 
-		/* Get the message type which is total_len + PSEUDO header + msgtype + message body */
+		/*
+		 * Get the message type which is total_len + PSEUDO header
+		 * + msgtype + message body
+		 */
 		pdrvmsg = (struct drv_msg *)&cmdbuffer[0];
 		msgtype = ntohs(pdrvmsg->type);
 		pr_debug("Command message type = 0x%x\n", msgtype);
@@ -968,8 +933,8 @@ static void ft1000_proc_drvmsg(struct net_device *dev)
 			while (list_empty(&info->prov_list) == 0) {
 				pr_debug("Sending a provisioning message\n");
 				/* Make sure SLOWQ doorbell is clear */
-				tempword =
-					ft1000_read_reg(dev, FT1000_REG_DOORBELL);
+				tempword = ft1000_read_reg(dev,
+							   FT1000_REG_DOORBELL);
 				i = 0;
 				while (tempword & FT1000_DB_DPRAM_TX) {
 					mdelay(5);
@@ -977,9 +942,8 @@ static void ft1000_proc_drvmsg(struct net_device *dev)
 					if (i == 10)
 						break;
 				}
-				ptr =
-					list_entry(info->prov_list.next,
-						   struct prov_record, list);
+				ptr = list_entry(info->prov_list.next,
+						 struct prov_record, list);
 				len = *(u16 *)ptr->pprov_data;
 				len = htons(len);
 
@@ -998,14 +962,15 @@ static void ft1000_proc_drvmsg(struct net_device *dev)
 						 ppseudo_hdr->checksum);
 				}
 
-				ft1000_send_cmd(dev, (u16 *)ptr->pprov_data, len, SLOWQ_TYPE);
+				ft1000_send_cmd(dev, (u16 *)ptr->pprov_data,
+						len, SLOWQ_TYPE);
 				list_del(&ptr->list);
 				kfree(ptr->pprov_data);
 				kfree(ptr);
 			}
 			/*
-			 * Indicate adapter is ready to take application messages after all
-			 * provisioning messages are sent
+			 * Indicate adapter is ready to take application
+			 * messages after all provisioning messages are sent
 			 */
 			info->CardReady = 1;
 			break;
@@ -1093,8 +1058,8 @@ static void ft1000_proc_drvmsg(struct net_device *dev)
 			tempword = ft1000_read_reg(dev, FT1000_REG_DOORBELL);
 			if (tempword & FT1000_DB_DPRAM_TX) {
 				mdelay(10);
-				tempword =
-					ft1000_read_reg(dev, FT1000_REG_DOORBELL);
+				tempword = ft1000_read_reg(dev,
+							   FT1000_REG_DOORBELL);
 				if (tempword & FT1000_DB_DPRAM_TX)
 					mdelay(10);
 			}
@@ -1129,7 +1094,9 @@ static void ft1000_proc_drvmsg(struct net_device *dev)
 				info->DSPInfoBlk[8] = 0x7200;
 				info->DSPInfoBlk[9] =
 					htons(info->DSPInfoBlklen);
-				ft1000_send_cmd(dev, (u16 *)info->DSPInfoBlk, (u16)(info->DSPInfoBlklen+4), 0);
+				ft1000_send_cmd(dev, info->DSPInfoBlk,
+						(u16)(info->DSPInfoBlklen+4),
+						0);
 			}
 
 			break;
@@ -1143,8 +1110,8 @@ static void ft1000_proc_drvmsg(struct net_device *dev)
 			tempword = ft1000_read_reg(dev, FT1000_REG_DOORBELL);
 			if (tempword & FT1000_DB_DPRAM_TX) {
 				mdelay(10);
-				tempword =
-					ft1000_read_reg(dev, FT1000_REG_DOORBELL);
+				tempword = ft1000_read_reg(dev,
+							   FT1000_REG_DOORBELL);
 				if (tempword & FT1000_DB_DPRAM_TX)
 					mdelay(10);
 			}
@@ -1190,7 +1157,8 @@ static void ft1000_proc_drvmsg(struct net_device *dev)
 				*pmsg++ = convert.wrd;
 				*pmsg++ = htons(info->DrvErrNum);
 
-				ft1000_send_cmd(dev, (u16 *)&tempbuffer[0], (u16)(0x0012), 0);
+				ft1000_send_cmd(dev, (u16 *)&tempbuffer[0],
+						(u16)(0x0012), 0);
 				info->DrvErrNum = 0;
 			}
 
@@ -1281,28 +1249,30 @@ static int ft1000_parse_dpram_msg(struct net_device *dev)
 				       FT1000_MAG_TOTAL_LEN_INDX));
 		}
 		pr_debug("total length = %d\n", total_len);
-		if ((total_len < MAX_CMD_SQSIZE) && (total_len > sizeof(struct pseudo_hdr))) {
+		if ((total_len < MAX_CMD_SQSIZE)
+				&& (total_len > sizeof(struct pseudo_hdr))) {
 			total_len += nxtph;
 			/*
-			 * ft1000_read_reg will return a value that needs to be byteswap
-			 * in order to get DSP_QID_OFFSET.
+			 * ft1000_read_reg will return a value that needs to be
+			 * byteswap in order to get DSP_QID_OFFSET.
 			 */
 			if (info->AsicID == ELECTRABUZZ_ID) {
-				portid =
-					(ft1000_read_dpram
-					 (dev,
-					  DSP_QID_OFFSET + FT1000_DPRAM_RX_BASE +
-					  2) >> 8) & 0xff;
+				portid = (ft1000_read_dpram(dev, DSP_QID_OFFSET
+						+ FT1000_DPRAM_RX_BASE + 2)
+						>> 8) & 0xff;
 			} else {
 				portid =
-					(ft1000_read_dpram_mag_16
+					ft1000_read_dpram_mag_16
 					 (dev, FT1000_MAG_PORT_ID,
-					  FT1000_MAG_PORT_ID_INDX) & 0xff);
+					  FT1000_MAG_PORT_ID_INDX) & 0xff;
 			}
 			pr_debug("DSP_QID = 0x%x\n", portid);
 
 			if (portid == DRIVERID) {
-				/* We are assumming one driver message from the DSP at a time. */
+				/*
+				 * We are assumming one driver message from the
+				 * DSP at a time.
+				 */
 				ft1000_proc_drvmsg(dev);
 			}
 		}
@@ -1311,29 +1281,7 @@ static int ft1000_parse_dpram_msg(struct net_device *dev)
 
 	if (doorbell & FT1000_DB_COND_RESET) {
 		/* Reset ASIC and DSP */
-		if (info->AsicID == ELECTRABUZZ_ID) {
-			info->DSP_TIME[0] =
-				ft1000_read_dpram(dev, FT1000_DSP_TIMER0);
-			info->DSP_TIME[1] =
-				ft1000_read_dpram(dev, FT1000_DSP_TIMER1);
-			info->DSP_TIME[2] =
-				ft1000_read_dpram(dev, FT1000_DSP_TIMER2);
-			info->DSP_TIME[3] =
-				ft1000_read_dpram(dev, FT1000_DSP_TIMER3);
-		} else {
-			info->DSP_TIME[0] =
-				ft1000_read_dpram_mag_16(dev, FT1000_MAG_DSP_TIMER0,
-							 FT1000_MAG_DSP_TIMER0_INDX);
-			info->DSP_TIME[1] =
-				ft1000_read_dpram_mag_16(dev, FT1000_MAG_DSP_TIMER1,
-							 FT1000_MAG_DSP_TIMER1_INDX);
-			info->DSP_TIME[2] =
-				ft1000_read_dpram_mag_16(dev, FT1000_MAG_DSP_TIMER2,
-							 FT1000_MAG_DSP_TIMER2_INDX);
-			info->DSP_TIME[3] =
-				ft1000_read_dpram_mag_16(dev, FT1000_MAG_DSP_TIMER3,
-							 FT1000_MAG_DSP_TIMER3_INDX);
-		}
+		ft1000_read_dsp_timer(dev, info);
 		info->DrvErrNum = DSP_CONDRESET_INFO;
 		pr_debug("DSP conditional reset requested\n");
 		ft1000_reset_card(dev);
@@ -1374,131 +1322,84 @@ static void ft1000_flush_fifo(struct net_device *dev, u16 DrvErrNum)
 	u16 tempword;
 
 	if (pcmcia->PktIntfErr > MAX_PH_ERR) {
-		if (info->AsicID == ELECTRABUZZ_ID) {
-			info->DSP_TIME[0] =
-				ft1000_read_dpram(dev, FT1000_DSP_TIMER0);
-			info->DSP_TIME[1] =
-				ft1000_read_dpram(dev, FT1000_DSP_TIMER1);
-			info->DSP_TIME[2] =
-				ft1000_read_dpram(dev, FT1000_DSP_TIMER2);
-			info->DSP_TIME[3] =
-				ft1000_read_dpram(dev, FT1000_DSP_TIMER3);
-		} else {
-			info->DSP_TIME[0] =
-				ft1000_read_dpram_mag_16(dev, FT1000_MAG_DSP_TIMER0,
-							 FT1000_MAG_DSP_TIMER0_INDX);
-			info->DSP_TIME[1] =
-				ft1000_read_dpram_mag_16(dev, FT1000_MAG_DSP_TIMER1,
-							 FT1000_MAG_DSP_TIMER1_INDX);
-			info->DSP_TIME[2] =
-				ft1000_read_dpram_mag_16(dev, FT1000_MAG_DSP_TIMER2,
-							 FT1000_MAG_DSP_TIMER2_INDX);
-			info->DSP_TIME[3] =
-				ft1000_read_dpram_mag_16(dev, FT1000_MAG_DSP_TIMER3,
-							 FT1000_MAG_DSP_TIMER3_INDX);
-		}
+		ft1000_read_dsp_timer(dev, info);
 		info->DrvErrNum = DrvErrNum;
 		ft1000_reset_card(dev);
 		return;
-	} else {
-		/* Flush corrupted pkt from FIFO */
-		i = 0;
-		do {
-			if (info->AsicID == ELECTRABUZZ_ID) {
-				tempword =
-					ft1000_read_reg(dev, FT1000_REG_DFIFO);
-				tempword =
-					ft1000_read_reg(dev, FT1000_REG_DFIFO_STAT);
-			} else {
-				templong =
-					inl(dev->base_addr + FT1000_REG_MAG_DFR);
+	}
+	/* Flush corrupted pkt from FIFO */
+	i = 0;
+	do {
+		if (info->AsicID == ELECTRABUZZ_ID) {
+			tempword =
+				ft1000_read_reg(dev, FT1000_REG_DFIFO);
+			tempword =
+				ft1000_read_reg(dev, FT1000_REG_DFIFO_STAT);
+		} else {
+			templong =
+				inl(dev->base_addr + FT1000_REG_MAG_DFR);
+			tempword =
+				inw(dev->base_addr + FT1000_REG_MAG_DFSR);
+		}
+		i++;
+		/*
+		 * This should never happen unless the ASIC is broken.
+		 * We must reset to recover.
+		 */
+		if ((i > 2048) || (tempword == 0)) {
+			ft1000_read_dsp_timer(dev, info);
+			if (tempword == 0) {
+				/*
+				 * Let's check if ASIC reads are still ok by
+				 * reading the Mask register which is never zero
+				 * at this point of the code.
+				 */
 				tempword =
-					inw(dev->base_addr + FT1000_REG_MAG_DFSR);
-			}
-			i++;
-			/*
-			 * This should never happen unless the ASIC is broken.
-			 * We must reset to recover.
-			 */
-			if ((i > 2048) || (tempword == 0)) {
-				if (info->AsicID == ELECTRABUZZ_ID) {
-					info->DSP_TIME[0] =
-						ft1000_read_dpram(dev,
-								  FT1000_DSP_TIMER0);
-					info->DSP_TIME[1] =
-						ft1000_read_dpram(dev,
-								  FT1000_DSP_TIMER1);
-					info->DSP_TIME[2] =
-						ft1000_read_dpram(dev,
-								  FT1000_DSP_TIMER2);
-					info->DSP_TIME[3] =
-						ft1000_read_dpram(dev,
-								  FT1000_DSP_TIMER3);
-				} else {
-					info->DSP_TIME[0] =
-						ft1000_read_dpram_mag_16(dev,
-									 FT1000_MAG_DSP_TIMER0,
-									 FT1000_MAG_DSP_TIMER0_INDX);
-					info->DSP_TIME[1] =
-						ft1000_read_dpram_mag_16(dev,
-									 FT1000_MAG_DSP_TIMER1,
-									 FT1000_MAG_DSP_TIMER1_INDX);
-					info->DSP_TIME[2] =
-						ft1000_read_dpram_mag_16(dev,
-									 FT1000_MAG_DSP_TIMER2,
-									 FT1000_MAG_DSP_TIMER2_INDX);
-					info->DSP_TIME[3] =
-						ft1000_read_dpram_mag_16(dev,
-									 FT1000_MAG_DSP_TIMER3,
-									 FT1000_MAG_DSP_TIMER3_INDX);
-				}
+					inw(dev->base_addr +
+					    FT1000_REG_SUP_IMASK);
 				if (tempword == 0) {
 					/*
-					 * Let's check if ASIC reads are still ok by reading the Mask register
-					 * which is never zero at this point of the code.
+					 * This indicates that we can not
+					 * communicate with the ASIC
 					 */
-					tempword =
-						inw(dev->base_addr +
-						    FT1000_REG_SUP_IMASK);
-					if (tempword == 0) {
-						/* This indicates that we can not communicate with the ASIC */
-						info->DrvErrNum =
-							FIFO_FLUSH_BADCNT;
-					} else {
-						/* Let's assume that we really flush the FIFO */
-						pcmcia->PktIntfErr++;
-						return;
-					}
+					info->DrvErrNum = FIFO_FLUSH_BADCNT;
 				} else {
-					info->DrvErrNum = FIFO_FLUSH_MAXLIMIT;
+					/*
+					 * Let's assume that we really flush
+					 * the FIFO
+					 */
+					pcmcia->PktIntfErr++;
+					return;
 				}
-				return;
+			} else {
+				info->DrvErrNum = FIFO_FLUSH_MAXLIMIT;
 			}
-			tempword = inw(dev->base_addr + FT1000_REG_SUP_STAT);
-		} while ((tempword & 0x03) != 0x03);
-		if (info->AsicID == ELECTRABUZZ_ID) {
-			i++;
-			pr_debug("Flushing FIFO complete = %x\n", tempword);
-			/* Flush last word in FIFO. */
-			tempword = ft1000_read_reg(dev, FT1000_REG_DFIFO);
-			/* Update FIFO counter for DSP */
-			i = i * 2;
-			pr_debug("Flush Data byte count to dsp = %d\n", i);
-			info->fifo_cnt += i;
-			ft1000_write_dpram(dev, FT1000_FIFO_LEN,
-					   info->fifo_cnt);
-		} else {
-			pr_debug("Flushing FIFO complete = %x\n", tempword);
-			/* Flush last word in FIFO */
-			templong = inl(dev->base_addr + FT1000_REG_MAG_DFR);
-			tempword = inw(dev->base_addr + FT1000_REG_SUP_STAT);
-			pr_debug("FT1000_REG_SUP_STAT = 0x%x\n", tempword);
-			tempword = inw(dev->base_addr + FT1000_REG_MAG_DFSR);
-			pr_debug("FT1000_REG_MAG_DFSR = 0x%x\n", tempword);
+			return;
 		}
-		if (DrvErrNum)
-			pcmcia->PktIntfErr++;
+		tempword = inw(dev->base_addr + FT1000_REG_SUP_STAT);
+	} while ((tempword & 0x03) != 0x03);
+	if (info->AsicID == ELECTRABUZZ_ID) {
+		i++;
+		pr_debug("Flushing FIFO complete = %x\n", tempword);
+		/* Flush last word in FIFO. */
+		tempword = ft1000_read_reg(dev, FT1000_REG_DFIFO);
+		/* Update FIFO counter for DSP */
+		i = i * 2;
+		pr_debug("Flush Data byte count to dsp = %d\n", i);
+		info->fifo_cnt += i;
+		ft1000_write_dpram(dev, FT1000_FIFO_LEN,
+				   info->fifo_cnt);
+	} else {
+		pr_debug("Flushing FIFO complete = %x\n", tempword);
+		/* Flush last word in FIFO */
+		templong = inl(dev->base_addr + FT1000_REG_MAG_DFR);
+		tempword = inw(dev->base_addr + FT1000_REG_SUP_STAT);
+		pr_debug("FT1000_REG_SUP_STAT = 0x%x\n", tempword);
+		tempword = inw(dev->base_addr + FT1000_REG_MAG_DFSR);
+		pr_debug("FT1000_REG_MAG_DFSR = 0x%x\n", tempword);
 	}
+	if (DrvErrNum)
+		pcmcia->PktIntfErr++;
 }
 
 /*---------------------------------------------------------------------------
@@ -1552,7 +1453,6 @@ static int ft1000_copy_up_pkt(struct net_device *dev)
 	skb = dev_alloc_skb(len + 12 + 2);
 
 	if (skb == NULL) {
-		pr_debug("No Network buffers available\n");
 		/* Read High word to complete 32 bit access */
 		if (info->AsicID == MAGNEMITE_ID)
 			tempword = ft1000_read_reg(dev, FT1000_REG_MAG_DFRH);
@@ -1673,7 +1573,8 @@ static int ft1000_copy_up_pkt(struct net_device *dev)
 	info->stats.rx_bytes += (len + 12);
 
 	if (info->AsicID == ELECTRABUZZ_ID) {
-		/* track how many bytes have been read from FIFO - round up to 16 bit word */
+		/* track how many bytes have been read from FIFO - round up to
+		 * 16 bit word */
 		tempword = len + 16;
 		if (tempword & 0x01)
 			tempword++;
@@ -1737,9 +1638,11 @@ static int ft1000_copy_down_pkt(struct net_device *dev, u16 *packet, u16 len)
 	else
 		pseudo.blk.length = ntohs(len);
 
-	pseudo.blk.source = DSPID;	/* Need to swap to get in correct order */
+	pseudo.blk.source = DSPID;	/* Need to swap to get in correct
+					   order */
 	pseudo.blk.destination = HOSTID;
-	pseudo.blk.portdest = NETWORKID;	/* Need to swap to get in correct order */
+	pseudo.blk.portdest = NETWORKID;	/* Need to swap to get in
+						   correct order */
 	pseudo.blk.portsrc = DSPAIRID;
 	pseudo.blk.sh_str_id = 0;
 	pseudo.blk.control = 0;
@@ -1843,7 +1746,8 @@ static int ft1000_open(struct net_device *dev)
 {
 	ft1000_reset_card(dev);
 
-	/* schedule ft1000_hbchk to perform periodic heartbeat checks on DSP and ASIC */
+	/* schedule ft1000_hbchk to perform periodic heartbeat checks on DSP
+	 * and ASIC */
 	init_timer(&poll_timer);
 	poll_timer.expires = jiffies + (2 * HZ);
 	poll_timer.data = (u_long)dev;
@@ -1928,7 +1832,8 @@ static irqreturn_t ft1000_interrupt(int irq, void *dev_id)
 	/* Read interrupt type */
 	inttype = ft1000_read_reg(dev, FT1000_REG_SUP_ISR);
 
-	/* Make sure we process all interrupt before leaving the ISR due to the edge trigger interrupt type */
+	/* Make sure we process all interrupt before leaving the ISR due to the
+	 * edge trigger interrupt type */
 	while (inttype) {
 		if (inttype & ISR_DOORBELL_PEND)
 			ft1000_parse_dpram_msg(dev);
@@ -1938,21 +1843,18 @@ static irqreturn_t ft1000_interrupt(int irq, void *dev_id)
 
 			cnt = 0;
 			do {
-				/* Check if we have packets in the Downlink FIFO */
+				/* Check if we have packets in the Downlink
+				 * FIFO */
 				if (info->AsicID == ELECTRABUZZ_ID) {
-					tempword =
-						ft1000_read_reg(dev,
-								FT1000_REG_DFIFO_STAT);
+					tempword = ft1000_read_reg(dev,
+							FT1000_REG_DFIFO_STAT);
 				} else {
-					tempword =
-						ft1000_read_reg(dev,
-								FT1000_REG_MAG_DFSR);
+					tempword = ft1000_read_reg(dev,
+							FT1000_REG_MAG_DFSR);
 				}
-				if (tempword & 0x1f) {
-					ft1000_copy_up_pkt(dev);
-				} else {
+				if (!(tempword & 0x1f))
 					break;
-				}
+				ft1000_copy_up_pkt(dev);
 				cnt++;
 			} while (cnt < MAX_RCV_LOOP);
 
@@ -1975,6 +1877,7 @@ void stop_ft1000_card(struct net_device *dev)
 {
 	struct ft1000_info *info = netdev_priv(dev);
 	struct prov_record *ptr;
+	struct prov_record *tmp;
 	/* int cnt; */
 
 	info->CardReady = 0;
@@ -1983,8 +1886,7 @@ void stop_ft1000_card(struct net_device *dev)
 	ft1000_disable_interrupts(dev);
 
 	/* Make sure we free any memory reserve for provisioning */
-	while (list_empty(&info->prov_list) == 0) {
-		ptr = list_entry(info->prov_list.next, struct prov_record, list);
+	list_for_each_entry_safe(ptr, tmp, &info->prov_list, list) {
 		list_del(&ptr->list);
 		kfree(ptr->pprov_data);
 		kfree(ptr);
@@ -2008,6 +1910,7 @@ static void ft1000_get_drvinfo(struct net_device *dev,
 			       struct ethtool_drvinfo *info)
 {
 	struct ft1000_info *ft_info;
+
 	ft_info = netdev_priv(dev);
 
 	strlcpy(info->driver, "ft1000", sizeof(info->driver));
@@ -2038,8 +1941,7 @@ struct net_device *init_ft1000_card(struct pcmcia_device *link,
 	struct ft1000_pcmcia *pcmcia;
 	struct net_device *dev;
 
-	static const struct net_device_ops ft1000ops =		/* Slavius 21.10.2009 due to kernel changes */
-		{
+	static const struct net_device_ops ft1000ops = {
 			.ndo_open = &ft1000_open,
 			.ndo_stop = &ft1000_close,
 			.ndo_start_xmit = &ft1000_start_xmit,
@@ -2102,7 +2004,7 @@ struct net_device *init_ft1000_card(struct pcmcia_device *link,
 	/* dev->open = &ft1000_open; */
 	/* dev->stop = &ft1000_close; */
 
-	dev->netdev_ops = &ft1000ops;		/* Slavius 21.10.2009 due to kernel changes */
+	dev->netdev_ops = &ft1000ops;
 
 	pr_debug("device name = %s\n", dev->name);
 
@@ -2113,7 +2015,8 @@ struct net_device *init_ft1000_card(struct pcmcia_device *link,
 		goto err_dev;
 	}
 
-	if (request_irq(dev->irq, ft1000_interrupt, IRQF_SHARED, dev->name, dev)) {
+	if (request_irq(dev->irq, ft1000_interrupt, IRQF_SHARED, dev->name,
+			dev)) {
 		netdev_err(dev, "Could not request_irq\n");
 		goto err_dev;
 	}
@@ -2123,7 +2026,7 @@ struct net_device *init_ft1000_card(struct pcmcia_device *link,
 		goto err_irq;
 	}
 
-	if (register_netdev(dev) != 0) {
+	if (register_netdev(dev)) {
 		pr_debug("Could not register netdev\n");
 		goto err_reg;
 	}
@@ -2131,13 +2034,15 @@ struct net_device *init_ft1000_card(struct pcmcia_device *link,
 	info->AsicID = ft1000_read_reg(dev, FT1000_REG_ASIC_ID);
 	if (info->AsicID == ELECTRABUZZ_ID) {
 		pr_debug("ELECTRABUZZ ASIC\n");
-		if (request_firmware(&fw_entry, "ft1000.img", &link->dev) != 0) {
+		if (request_firmware(&fw_entry, "ft1000.img",
+				     &link->dev) != 0) {
 			pr_info("Could not open ft1000.img\n");
 			goto err_unreg;
 		}
 	} else {
 		pr_debug("MAGNEMITE ASIC\n");
-		if (request_firmware(&fw_entry, "ft2000.img", &link->dev) != 0) {
+		if (request_firmware(&fw_entry, "ft2000.img",
+				     &link->dev) != 0) {
 			pr_info("Could not open ft2000.img\n");
 			goto err_unreg;
 		}
diff --git a/drivers/staging/ft1000/ft1000-usb/ft1000_debug.c b/drivers/staging/ft1000/ft1000-usb/ft1000_debug.c
index c8d278229940..2d758fb26eac 100644
--- a/drivers/staging/ft1000/ft1000-usb/ft1000_debug.c
+++ b/drivers/staging/ft1000/ft1000-usb/ft1000_debug.c
@@ -190,7 +190,7 @@ int ft1000_create_dev(struct ft1000_usb *dev)
 	tmp->dent = dir;
 	tmp->file = file;
 	tmp->int_number = dev->CardNumber;
-	list_add(&(tmp->list), &(dev->nodes.list));
+	list_add(&tmp->list, &dev->nodes.list);
 
 	pr_debug("registered debugfs directory \"%s\"\n", dev->DeviceName);
 
@@ -301,7 +301,7 @@ static int ft1000_open(struct inode *inode, struct file *file)
 	struct ft1000_usb *dev = (struct ft1000_usb *)inode->i_private;
 	int i, num;
 
-	num = (MINOR(inode->i_rdev) & 0xf);
+	num = MINOR(inode->i_rdev) & 0xf;
 	pr_debug("minor number=%d\n", num);
 
 	info = file->private_data = netdev_priv(dev->net);
@@ -317,9 +317,8 @@ static int ft1000_open(struct inode *inode, struct file *file)
 
 	/* Search for available application info block */
 	for (i = 0; i < MAX_NUM_APP; i++) {
-		if ((dev->app_info[i].fileobject == NULL)) {
+		if ((dev->app_info[i].fileobject == NULL))
 			break;
-		}
 	}
 
 	/* Fail due to lack of application info block */
@@ -478,14 +477,14 @@ static long ft1000_ioctl(struct file *file, unsigned int command,
 		/* Connect Message */
 		pr_debug("IOCTL_FT1000_CONNECT\n");
 		ConnectionMsg[79] = 0xfc;
-		result = card_send_command(ft1000dev, (unsigned short *)ConnectionMsg, 0x4c);
+		result = card_send_command(ft1000dev, ConnectionMsg, 0x4c);
 
 		break;
 	case IOCTL_DISCONNECT:
 		/* Disconnect Message */
 		pr_debug("IOCTL_FT1000_DISCONNECT\n");
 		ConnectionMsg[79] = 0xfd;
-		result = card_send_command(ft1000dev, (unsigned short *)ConnectionMsg, 0x4c);
+		result = card_send_command(ft1000dev, ConnectionMsg, 0x4c);
 		break;
 	case IOCTL_GET_DSP_STAT_CMD:
 		/* pr_debug("IOCTL_FT1000_GET_DSP_STAT\n"); */
@@ -545,7 +544,7 @@ static long ft1000_ioctl(struct file *file, unsigned int command,
 		if (ft1000dev->fProvComplete == 0)
 			return -EACCES;
 
-		ft1000dev->fAppMsgPend = 1;
+		ft1000dev->fAppMsgPend = true;
 
 		if (info->CardReady) {
 
@@ -575,9 +574,8 @@ static long ft1000_ioctl(struct file *file, unsigned int command,
 			} else {
 				/* Check if this message came from a registered application */
 				for (i = 0; i < MAX_NUM_APP; i++) {
-					if (ft1000dev->app_info[i].fileobject == &file->f_owner) {
+					if (ft1000dev->app_info[i].fileobject == &file->f_owner)
 						break;
-					}
 				}
 				if (i == MAX_NUM_APP) {
 					pr_debug("No matching application fileobject\n");
@@ -629,9 +627,8 @@ static long ft1000_ioctl(struct file *file, unsigned int command,
 						pmsg = (u16 *)&dpram_data->pseudohdr;
 						ppseudo_hdr = (struct pseudo_hdr *)pmsg;
 						total_len = msgsz+2;
-						if (total_len & 0x1) {
+						if (total_len & 0x1)
 							total_len++;
-						}
 
 						/* Insert slow queue sequence number */
 						ppseudo_hdr->seq_num = info->squeseqnum++;
@@ -645,7 +642,7 @@ static long ft1000_ioctl(struct file *file, unsigned int command,
 						}
 						pmsg++;
 						ppseudo_hdr = (struct pseudo_hdr *)pmsg;
-						result = card_send_command(ft1000dev, (unsigned short *)dpram_data, total_len+2);
+						result = card_send_command(ft1000dev, dpram_data, total_len+2);
 
 
 						ft1000dev->app_info[app_index].nTxMsg++;
@@ -722,7 +719,7 @@ static long ft1000_ioctl(struct file *file, unsigned int command,
 		result = -ENOTTY;
 		break;
 	}
-	ft1000dev->fAppMsgPend = 0;
+	ft1000dev->fAppMsgPend = false;
 	return result;
 }
 
@@ -745,6 +742,7 @@ static int ft1000_release(struct inode *inode, struct file *file)
 	struct ft1000_usb *ft1000dev;
 	int i;
 	struct dpram_blk *pdpram_blk;
+	struct dpram_blk *tmp;
 
 	dev = file->private_data;
 	info = netdev_priv(dev);
@@ -766,9 +764,8 @@ static int ft1000_release(struct inode *inode, struct file *file)
 	if (i == MAX_NUM_APP)
 		return 0;
 
-	while (list_empty(&ft1000dev->app_info[i].app_sqlist) == 0) {
+	list_for_each_entry_safe(pdpram_blk, tmp, &ft1000dev->app_info[i].app_sqlist, list) {
 		pr_debug("Remove and free memory queue up on slow queue\n");
-		pdpram_blk = list_entry(ft1000dev->app_info[i].app_sqlist.next, struct dpram_blk, list);
 		list_del(&pdpram_blk->list);
 		ft1000_free_buffer(pdpram_blk, &freercvpool);
 	}
diff --git a/drivers/staging/ft1000/ft1000-usb/ft1000_download.c b/drivers/staging/ft1000/ft1000-usb/ft1000_download.c
index e8126325877b..5def347beb08 100644
--- a/drivers/staging/ft1000/ft1000-usb/ft1000_download.c
+++ b/drivers/staging/ft1000/ft1000-usb/ft1000_download.c
@@ -230,7 +230,7 @@ static u16 get_handshake_usb(struct ft1000_usb *ft1000dev, u16 expected_value)
 	while (loopcnt < 100) {
 		if (ft1000dev->usbboot == 2) {
 			status = ft1000_read_dpram32(ft1000dev, 0,
-						     (u8 *)&(ft1000dev->tempbuf[0]), 64);
+						     (u8 *)&ft1000dev->tempbuf[0], 64);
 			for (temp = 0; temp < 16; temp++) {
 				pr_debug("tempbuf %d = 0x%x\n",
 					 temp, ft1000dev->tempbuf[temp]);
@@ -368,8 +368,8 @@ static u16 hdr_checksum(struct pseudo_hdr *pHdr)
 	u16   chksum;
 
 
-	chksum = ((((((usPtr[0] ^ usPtr[1]) ^ usPtr[2]) ^ usPtr[3]) ^
-		    usPtr[4]) ^ usPtr[5]) ^ usPtr[6]);
+	chksum = (((((usPtr[0] ^ usPtr[1]) ^ usPtr[2]) ^ usPtr[3]) ^
+		    usPtr[4]) ^ usPtr[5]) ^ usPtr[6];
 
 	return chksum;
 }
@@ -538,7 +538,7 @@ static int write_blk_fifo(struct ft1000_usb *ft1000dev, u16 **pUsFile,
 			  usb_sndbulkpipe(ft1000dev->dev,
 					  ft1000dev->bulk_out_endpointAddr),
 			  ft1000dev->tx_buf, byte_length, usb_dnld_complete,
-			  (void *)ft1000dev);
+			  ft1000dev);
 
 	usb_submit_urb(ft1000dev->tx_urb, GFP_ATOMIC);
 
@@ -704,7 +704,7 @@ int scram_dnldr(struct ft1000_usb *ft1000dev, void *pFileStart,
 				case REQUEST_CODE_SEGMENT:
 					status = request_code_segment(ft1000dev,
 								      &s_file, &c_file,
-								      (const u8 *)boot_end,
+								      boot_end,
 								      true);
 					break;
 				default:
@@ -799,7 +799,7 @@ int scram_dnldr(struct ft1000_usb *ft1000dev, void *pFileStart,
 
 					status = request_code_segment(ft1000dev,
 								      &s_file, &c_file,
-								      (const u8 *)code_end,
+								      code_end,
 								      false);
 
 					break;
@@ -971,11 +971,11 @@ int scram_dnldr(struct ft1000_usb *ft1000dev, void *pFileStart,
 
 				/* Get buffer for provisioning data */
 				pbuffer =
-					kmalloc((pseudo_header_len +
-						 sizeof(struct pseudo_hdr)),
+					kmalloc(pseudo_header_len +
+						 sizeof(struct pseudo_hdr),
 						GFP_ATOMIC);
 				if (pbuffer) {
-					memcpy(pbuffer, (void *)c_file,
+					memcpy(pbuffer, c_file,
 					       (u32) (pseudo_header_len +
 						      sizeof(struct
 							     pseudo_hdr)));
diff --git a/drivers/staging/ft1000/ft1000-usb/ft1000_hw.c b/drivers/staging/ft1000/ft1000-usb/ft1000_hw.c
index f0ac43838461..e6fb066e0dd2 100644
--- a/drivers/staging/ft1000/ft1000-usb/ft1000_hw.c
+++ b/drivers/staging/ft1000/ft1000-usb/ft1000_hw.c
@@ -339,7 +339,7 @@ int card_send_command(struct ft1000_usb *ft1000dev, void *ptempbuffer,
 	commandbuf = kmalloc(size + 2, GFP_KERNEL);
 	if (!commandbuf)
 		return -ENOMEM;
-	memcpy((void *)commandbuf + 2, (void *)ptempbuffer, size);
+	memcpy((void *)commandbuf + 2, ptempbuffer, size);
 
 	if (temp & 0x0100)
 		usleep_range(900, 1100);
@@ -429,7 +429,7 @@ static void ft1000_reset_asic(struct net_device *dev)
 	/* Let's use the register provided by the Magnemite ASIC to reset the
 	 * ASIC and DSP.
 	 */
-	ft1000_write_register(ft1000dev, (DSP_RESET_BIT | ASIC_RESET_BIT),
+	ft1000_write_register(ft1000dev, DSP_RESET_BIT | ASIC_RESET_BIT,
 			      FT1000_REG_RESET);
 
 	mdelay(1);
@@ -451,16 +451,15 @@ static int ft1000_reset_card(struct net_device *dev)
 	struct ft1000_usb *ft1000dev = info->priv;
 	u16 tempword;
 	struct prov_record *ptr;
+	struct prov_record *tmp;
 
 	ft1000dev->fCondResetPend = true;
 	info->CardReady = 0;
 	ft1000dev->fProvComplete = false;
 
 	/* Make sure we free any memory reserve for provisioning */
-	while (list_empty(&info->prov_list) == 0) {
+	list_for_each_entry_safe(ptr, tmp, &info->prov_list, list) {
 		pr_debug("deleting provisioning record\n");
-		ptr =
-			list_entry(info->prov_list.next, struct prov_record, list);
 		list_del(&ptr->list);
 		kfree(ptr->pprov_data);
 		kfree(ptr);
@@ -542,7 +541,7 @@ static int ft1000_copy_down_pkt(struct net_device *netdev, u8 *packet, u16 len)
 		hdr.portdest ^ hdr.portsrc ^ hdr.sh_str_id ^ hdr.control;
 
 	memcpy(&pFt1000Dev->tx_buf[0], &hdr, sizeof(hdr));
-	memcpy(&(pFt1000Dev->tx_buf[sizeof(struct pseudo_hdr)]), packet, len);
+	memcpy(&pFt1000Dev->tx_buf[sizeof(struct pseudo_hdr)], packet, len);
 
 	netif_stop_queue(netdev);
 
@@ -551,7 +550,7 @@ static int ft1000_copy_down_pkt(struct net_device *netdev, u8 *packet, u16 len)
 			  usb_sndbulkpipe(pFt1000Dev->dev,
 					  pFt1000Dev->bulk_out_endpointAddr),
 			  pFt1000Dev->tx_buf, count,
-			  ft1000_usb_transmit_complete, (void *)pFt1000Dev);
+			  ft1000_usb_transmit_complete, pFt1000Dev);
 
 	t = (u8 *)pFt1000Dev->tx_urb->transfer_buffer;
 
@@ -606,7 +605,7 @@ static int ft1000_start_xmit(struct sk_buff *skb, struct net_device *dev)
 		goto err;
 	}
 
-	ft1000_copy_down_pkt(dev, (pdata + ENET_HEADER_SIZE - 2),
+	ft1000_copy_down_pkt(dev, pdata + ENET_HEADER_SIZE - 2,
 			     skb->len - ENET_HEADER_SIZE + 2);
 
 err:
@@ -1558,19 +1557,19 @@ int ft1000_poll(void *dev_id)
 				/* Reset ASIC and DSP */
 				status = ft1000_read_dpram16(dev,
 							     FT1000_MAG_DSP_TIMER0,
-							     (u8 *)&(info->DSP_TIME[0]),
+							     (u8 *)&info->DSP_TIME[0],
 							     FT1000_MAG_DSP_TIMER0_INDX);
 				status = ft1000_read_dpram16(dev,
 							     FT1000_MAG_DSP_TIMER1,
-							     (u8 *)&(info->DSP_TIME[1]),
+							     (u8 *)&info->DSP_TIME[1],
 							     FT1000_MAG_DSP_TIMER1_INDX);
 				status = ft1000_read_dpram16(dev,
 							     FT1000_MAG_DSP_TIMER2,
-							     (u8 *)&(info->DSP_TIME[2]),
+							     (u8 *)&info->DSP_TIME[2],
 							     FT1000_MAG_DSP_TIMER2_INDX);
 				status = ft1000_read_dpram16(dev,
 							     FT1000_MAG_DSP_TIMER3,
-							     (u8 *)&(info->DSP_TIME[3]),
+							     (u8 *)&info->DSP_TIME[3],
 							     FT1000_MAG_DSP_TIMER3_INDX);
 				info->CardReady = 0;
 				info->DrvErrNum = DSP_CONDRESET_INFO;
diff --git a/drivers/staging/ft1000/ft1000-usb/ft1000_usb.c b/drivers/staging/ft1000/ft1000-usb/ft1000_usb.c
index a6b55f42c07c..3b303b6b1b27 100644
--- a/drivers/staging/ft1000/ft1000-usb/ft1000_usb.c
+++ b/drivers/staging/ft1000/ft1000-usb/ft1000_usb.c
@@ -136,7 +136,7 @@ static int ft1000_probe(struct usb_interface *interface,
 
 	ret = request_firmware(&dsp_fw, "ft3000.img", &dev->dev);
 	if (ret < 0) {
-		pr_err("Error request_firmware()\n");
+		dev_err(interface->usb_dev, "Error request_firmware()\n");
 		goto err_fw;
 	}
 
@@ -164,7 +164,8 @@ static int ft1000_probe(struct usb_interface *interface,
 	pr_debug("pft1000info=%p\n", pft1000info);
 	ret = dsp_reload(ft1000dev);
 	if (ret) {
-		pr_err("Problem with DSP image loading\n");
+		dev_err(interface->usb_dev,
+			"Problem with DSP image loading\n");
 		goto err_load;
 	}
 
diff --git a/drivers/staging/fwserial/fwserial.c b/drivers/staging/fwserial/fwserial.c
index 73deae3cd9eb..fdb2418c5f88 100644
--- a/drivers/staging/fwserial/fwserial.c
+++ b/drivers/staging/fwserial/fwserial.c
@@ -1186,7 +1186,7 @@ static void fwtty_unthrottle(struct tty_struct *tty)
 {
 	struct fwtty_port *port = tty->driver_data;
 
-	fwtty_dbg(port, "CRTSCTS: %d\n", (C_CRTSCTS(tty) != 0));
+	fwtty_dbg(port, "CRTSCTS: %d\n", C_CRTSCTS(tty) != 0);
 
 	fwtty_profile_fifo(port, port->stats.unthrottle);
 
diff --git a/drivers/staging/gdm724x/gdm_lte.c b/drivers/staging/gdm724x/gdm_lte.c
index 7c4a77bb94aa..a8d2cffb411c 100644
--- a/drivers/staging/gdm724x/gdm_lte.c
+++ b/drivers/staging/gdm724x/gdm_lte.c
@@ -889,7 +889,7 @@ int register_lte_device(struct phy_dev *phy_dev,
 		/* Allocate netdev */
 		net = alloc_netdev(sizeof(struct nic), pdn_dev_name,
 				   NET_NAME_UNKNOWN, ether_setup);
-		if (net == NULL) {
+		if (!net) {
 			pr_err("alloc_netdev failed\n");
 			ret = -ENOMEM;
 			goto err;
diff --git a/drivers/staging/gdm724x/gdm_mux.c b/drivers/staging/gdm724x/gdm_mux.c
index d1ab996b3305..8199b0a697bb 100644
--- a/drivers/staging/gdm724x/gdm_mux.c
+++ b/drivers/staging/gdm724x/gdm_mux.c
@@ -270,7 +270,7 @@ static void gdm_mux_rcv_complete(struct urb *urb)
 
 	if (urb->status) {
 		if (mux_dev->usb_state == PM_NORMAL)
-			pr_err("%s: urb status error %d\n",
+			dev_err(&urb->dev->dev, "%s: urb status error %d\n",
 			       __func__, urb->status);
 		put_rx_struct(rx, r);
 	} else {
@@ -342,7 +342,7 @@ static void gdm_mux_send_complete(struct urb *urb)
 	struct mux_tx *t = urb->context;
 
 	if (urb->status == -ECONNRESET) {
-		pr_info("CONNRESET\n");
+		dev_info(&urb->dev->dev, "CONNRESET\n");
 		free_mux_tx(t);
 		return;
 	}
@@ -608,7 +608,7 @@ static int gdm_mux_suspend(struct usb_interface *intf, pm_message_t pm_msg)
 	rx = &mux_dev->rx;
 
 	if (mux_dev->usb_state != PM_NORMAL) {
-		pr_err("usb suspend - invalid state\n");
+		dev_err(intf->usb_dev, "usb suspend - invalid state\n");
 		return -1;
 	}
 
@@ -637,7 +637,7 @@ static int gdm_mux_resume(struct usb_interface *intf)
 	mux_dev = tty_dev->priv_dev;
 
 	if (mux_dev->usb_state != PM_SUSPEND) {
-		pr_err("usb resume - invalid state\n");
+		dev_err(intf->usb_dev, "usb resume - invalid state\n");
 		return -1;
 	}
 
diff --git a/drivers/staging/gdm724x/gdm_usb.c b/drivers/staging/gdm724x/gdm_usb.c
index d2a3b350ba6d..ed1a12f504e2 100644
--- a/drivers/staging/gdm724x/gdm_usb.c
+++ b/drivers/staging/gdm724x/gdm_usb.c
@@ -480,7 +480,7 @@ static void gdm_usb_rcv_complete(struct urb *urb)
 		spin_unlock_irqrestore(&rx->to_host_lock, flags);
 	} else {
 		if (urb->status && udev->usb_state == PM_NORMAL)
-			pr_err("%s: urb status error %d\n",
+			dev_err(&urb->dev->dev, "%s: urb status error %d\n",
 			       __func__, urb->status);
 
 		put_rx_struct(rx, r);
@@ -557,7 +557,7 @@ static void gdm_usb_send_complete(struct urb *urb)
 	unsigned long flags;
 
 	if (urb->status == -ECONNRESET) {
-		pr_info("CONNRESET\n");
+		dev_info(&urb->dev->dev, "CONNRESET\n");
 		return;
 	}
 
@@ -590,7 +590,8 @@ static int send_tx_packet(struct usb_device *usbdev, struct usb_tx *t, u32 len)
 	ret = usb_submit_urb(t->urb, GFP_ATOMIC);
 
 	if (ret)
-		pr_err("usb_submit_urb failed: %d\n", ret);
+		dev_err(&usbdev->dev, "usb_submit_urb failed: %d\n",
+			ret);
 
 	usb_mark_last_busy(usbdev);
 
@@ -848,7 +849,7 @@ static int gdm_usb_probe(struct usb_interface *intf,
 	udev->usbdev = usbdev;
 	ret = init_usb(udev);
 	if (ret < 0) {
-		pr_err("init_usb func failed\n");
+		dev_err(intf->usb_dev, "init_usb func failed\n");
 		goto err_init_usb;
 	}
 	udev->intf = intf;
@@ -867,7 +868,7 @@ static int gdm_usb_probe(struct usb_interface *intf,
 
 	ret = request_mac_address(udev);
 	if (ret < 0) {
-		pr_err("request Mac address failed\n");
+		dev_err(intf->usb_dev, "request Mac address failed\n");
 		goto err_mac_address;
 	}
 
@@ -928,7 +929,7 @@ static int gdm_usb_suspend(struct usb_interface *intf, pm_message_t pm_msg)
 	udev = phy_dev->priv_dev;
 	rx = &udev->rx;
 	if (udev->usb_state != PM_NORMAL) {
-		pr_err("usb suspend - invalid state\n");
+		dev_err(intf->usb_dev, "usb suspend - invalid state\n");
 		return -1;
 	}
 
@@ -961,7 +962,7 @@ static int gdm_usb_resume(struct usb_interface *intf)
 	rx = &udev->rx;
 
 	if (udev->usb_state != PM_SUSPEND) {
-		pr_err("usb resume - invalid state\n");
+		dev_err(intf->usb_dev, "usb resume - invalid state\n");
 		return -1;
 	}
 	udev->usb_state = PM_NORMAL;
diff --git a/drivers/staging/gdm72xx/gdm_qos.c b/drivers/staging/gdm72xx/gdm_qos.c
index af24c57b8232..96bf2bf87ff4 100644
--- a/drivers/staging/gdm72xx/gdm_qos.c
+++ b/drivers/staging/gdm72xx/gdm_qos.c
@@ -54,8 +54,7 @@ static void *alloc_qos_entry(void)
 	}
 	spin_unlock_irqrestore(&qos_free_list.lock, flags);
 
-	entry = kmalloc(sizeof(*entry), GFP_ATOMIC);
-	return entry;
+	return kmalloc(sizeof(*entry), GFP_ATOMIC);
 }
 
 static void free_qos_entry(void *entry)
diff --git a/drivers/staging/gdm72xx/gdm_sdio.c b/drivers/staging/gdm72xx/gdm_sdio.c
index 7a0a0f221418..a5fd0794842e 100644
--- a/drivers/staging/gdm72xx/gdm_sdio.c
+++ b/drivers/staging/gdm72xx/gdm_sdio.c
@@ -223,8 +223,7 @@ static void send_sdio_pkt(struct sdio_func *func, u8 *data, int len)
 		if (ret < 0) {
 			if (ret != -ENOMEDIUM)
 				dev_err(&func->dev,
-					"gdmwms: %s error: ret = %d\n",
-					__func__, ret);
+					"gdmwms:  error: ret = %d\n", ret);
 			goto end_io;
 		}
 	}
@@ -237,8 +236,7 @@ static void send_sdio_pkt(struct sdio_func *func, u8 *data, int len)
 		if (ret < 0) {
 			if (ret != -ENOMEDIUM)
 				dev_err(&func->dev,
-					"gdmwms: %s error: ret = %d\n",
-					__func__, ret);
+					"gdmwms:  error: ret = %d\n", ret);
 			goto end_io;
 		}
 	}
diff --git a/drivers/staging/gdm72xx/gdm_wimax.c b/drivers/staging/gdm72xx/gdm_wimax.c
index 9cab54bfa6f4..61d168e82011 100644
--- a/drivers/staging/gdm72xx/gdm_wimax.c
+++ b/drivers/staging/gdm72xx/gdm_wimax.c
@@ -129,11 +129,11 @@ static void __gdm_wimax_event_send(struct work_struct *work)
 	int idx;
 	unsigned long flags;
 	struct evt_entry *e;
+	struct evt_entry *tmp;
 
 	spin_lock_irqsave(&wm_event.evt_lock, flags);
 
-	while (!list_empty(&wm_event.evtq)) {
-		e = list_entry(wm_event.evtq.next, struct evt_entry, list);
+	list_for_each_entry_safe(e, tmp, &wm_event.evtq, list) {
 		spin_unlock_irqrestore(&wm_event.evt_lock, flags);
 
 		if (sscanf(e->dev->name, "wm%d", &idx) == 1)
@@ -749,7 +749,7 @@ int register_wimax_device(struct phy_dev *phy_dev, struct device *pdev)
 	dev = alloc_netdev(sizeof(*nic), "wm%d", NET_NAME_UNKNOWN,
 			   ether_setup);
 
-	if (dev == NULL) {
+	if (!dev) {
 		pr_err("alloc_etherdev failed\n");
 		return -ENOMEM;
 	}
diff --git a/drivers/staging/goldfish/goldfish_audio.c b/drivers/staging/goldfish/goldfish_audio.c
index f200359c4443..702ae04df912 100644
--- a/drivers/staging/goldfish/goldfish_audio.c
+++ b/drivers/staging/goldfish/goldfish_audio.c
@@ -40,12 +40,12 @@ struct goldfish_audio {
 	spinlock_t lock;
 	wait_queue_head_t wait;
 
-	char __iomem *buffer_virt;      /* combined buffer virtual address */
+	char *buffer_virt;		/* combined buffer virtual address */
 	unsigned long buffer_phys;      /* combined buffer physical address */
 
-	char __iomem *write_buffer1;    /* write buffer 1 virtual address */
-	char __iomem *write_buffer2;    /* write buffer 2 virtual address */
-	char __iomem *read_buffer;      /* read buffer virtual address */
+	char *write_buffer1;		/* write buffer 1 virtual address */
+	char *write_buffer2;		/* write buffer 2 virtual address */
+	char *read_buffer;		/* read buffer virtual address */
 	int buffer_status;
 	int read_supported;         /* true if we have audio input support */
 };
@@ -125,8 +125,8 @@ static ssize_t goldfish_audio_read(struct file *fp, char __user *buf,
 		length = (count > READ_BUFFER_SIZE ? READ_BUFFER_SIZE : count);
 		AUDIO_WRITE(data, AUDIO_START_READ, length);
 
-		wait_event_interruptible(data->wait, (data->buffer_status &
-					 AUDIO_INT_READ_BUFFER_FULL));
+		wait_event_interruptible(data->wait, data->buffer_status &
+					 AUDIO_INT_READ_BUFFER_FULL);
 
 		length = AUDIO_READ(data, AUDIO_READ_BUFFER_AVAILABLE);
 
@@ -147,16 +147,16 @@ static ssize_t goldfish_audio_write(struct file *fp, const char __user *buf,
 	struct goldfish_audio *data = fp->private_data;
 	unsigned long irq_flags;
 	ssize_t result = 0;
-	char __iomem *kbuf;
+	char *kbuf;
 
 	while (count > 0) {
 		ssize_t copy = count;
 
 		if (copy > WRITE_BUFFER_SIZE)
 			copy = WRITE_BUFFER_SIZE;
-		wait_event_interruptible(data->wait, (data->buffer_status &
+		wait_event_interruptible(data->wait, data->buffer_status &
 					(AUDIO_INT_WRITE_BUFFER_1_EMPTY |
-					AUDIO_INT_WRITE_BUFFER_2_EMPTY)));
+					AUDIO_INT_WRITE_BUFFER_2_EMPTY));
 
 		if ((data->buffer_status & AUDIO_INT_WRITE_BUFFER_1_EMPTY) != 0)
 			kbuf = data->write_buffer1;
@@ -273,7 +273,7 @@ static int goldfish_audio_probe(struct platform_device *pdev)
 	dma_addr_t buf_addr;
 
 	data = devm_kzalloc(&pdev->dev, sizeof(*data), GFP_KERNEL);
-	if (data == NULL)
+	if (!data)
 		return -ENOMEM;
 	spin_lock_init(&data->lock);
 	init_waitqueue_head(&data->wait);
diff --git a/drivers/staging/goldfish/goldfish_nand.c b/drivers/staging/goldfish/goldfish_nand.c
index d68f216a7e77..213877a2c430 100644
--- a/drivers/staging/goldfish/goldfish_nand.c
+++ b/drivers/staging/goldfish/goldfish_nand.c
@@ -330,7 +330,7 @@ static int goldfish_nand_init_device(struct platform_device *pdev,
 	mtd->priv = nand;
 
 	name = devm_kzalloc(&pdev->dev, name_len + 1, GFP_KERNEL);
-	if (name == NULL)
+	if (!name)
 		return -ENOMEM;
 	mtd->name = name;
 
@@ -383,7 +383,7 @@ static int goldfish_nand_probe(struct platform_device *pdev)
 		return -ENODEV;
 
 	base = devm_ioremap(&pdev->dev, r->start, PAGE_SIZE);
-	if (base == NULL)
+	if (!base)
 		return -ENOMEM;
 
 	version = readl(base + NAND_VERSION);
@@ -399,7 +399,7 @@ static int goldfish_nand_probe(struct platform_device *pdev)
 
 	nand = devm_kzalloc(&pdev->dev, sizeof(*nand) +
 				sizeof(struct mtd_info) * num_dev, GFP_KERNEL);
-	if (nand == NULL)
+	if (!nand)
 		return -ENOMEM;
 
 	mutex_init(&nand->lock);
diff --git a/drivers/staging/gs_fpgaboot/gs_fpgaboot.c b/drivers/staging/gs_fpgaboot/gs_fpgaboot.c
index 6da72858d28c..a3a10f9a2a2b 100644
--- a/drivers/staging/gs_fpgaboot/gs_fpgaboot.c
+++ b/drivers/staging/gs_fpgaboot/gs_fpgaboot.c
@@ -17,7 +17,6 @@
 #include <linux/kernel.h>
 #include <linux/init.h>
 #include <linux/module.h>
-#include <linux/moduleparam.h>
 #include <linux/types.h>
 #include <linux/device.h>
 #include <linux/string.h>
diff --git a/drivers/staging/i2o/bus-osm.c b/drivers/staging/i2o/bus-osm.c
index 7aa0339aea05..43e357eeeb67 100644
--- a/drivers/staging/i2o/bus-osm.c
+++ b/drivers/staging/i2o/bus-osm.c
@@ -69,7 +69,8 @@ static ssize_t i2o_bus_store_scan(struct device *d,
 	struct i2o_device *i2o_dev = to_i2o_device(d);
 	int rc;
 
-	if ((rc = i2o_bus_scan(i2o_dev)))
+	rc = i2o_bus_scan(i2o_dev);
+	if (rc)
 		osm_warn("bus scan failed %d\n", rc);
 
 	return count;
diff --git a/drivers/staging/i2o/config-osm.c b/drivers/staging/i2o/config-osm.c
index 519f52f9f688..45091ac66154 100644
--- a/drivers/staging/i2o/config-osm.c
+++ b/drivers/staging/i2o/config-osm.c
@@ -19,7 +19,7 @@
 #include <linux/namei.h>
 #include <linux/fs.h>
 
-#include <asm/uaccess.h>
+#include <linux/uaccess.h>
 
 #define OSM_NAME	"config-osm"
 #define OSM_VERSION	"1.323"
diff --git a/drivers/staging/i2o/debug.c b/drivers/staging/i2o/debug.c
index 7a16114ed8ea..12b783b2a86c 100644
--- a/drivers/staging/i2o/debug.c
+++ b/drivers/staging/i2o/debug.c
@@ -5,7 +5,7 @@
 
 static void i2o_report_util_cmd(u8 cmd);
 static void i2o_report_exec_cmd(u8 cmd);
-static void i2o_report_fail_status(u8 req_status, u32 * msg);
+static void i2o_report_fail_status(u8 req_status, u32 *msg);
 static void i2o_report_common_status(u8 req_status);
 static void i2o_report_common_dsc(u16 detailed_status);
 
@@ -22,7 +22,7 @@ void i2o_report_status(const char *severity, const char *str,
 	u16 detailed_status = msg[4] & 0xFFFF;
 
 	if (cmd == I2O_CMD_UTIL_EVT_REGISTER)
-		return;		// No status in this reply
+		return;		/* No status in this reply */
 
 	printk("%s%s: ", severity, str);
 
@@ -54,6 +54,7 @@ void i2o_dump_message(struct i2o_message *m)
 #ifdef DEBUG
 	u32 *msg = (u32 *) m;
 	int i;
+
 	printk(KERN_INFO "Dumping I2O message size %d @ %p\n",
 	       msg[0] >> 16 & 0xffff, msg);
 	for (i = 0; i < ((msg[0] >> 16) & 0xffff); i++)
@@ -66,7 +67,7 @@ void i2o_dump_message(struct i2o_message *m)
  * Following fail status are common to all classes.
  * The preserved message must be handled in the reply handler.
  */
-static void i2o_report_fail_status(u8 req_status, u32 * msg)
+static void i2o_report_fail_status(u8 req_status, u32 *msg)
 {
 	static char *FAIL_STATUS[] = {
 		"0x80",		/* not used */
diff --git a/drivers/staging/i2o/device.c b/drivers/staging/i2o/device.c
index 2af22553dd4e..e47496cb0ac2 100644
--- a/drivers/staging/i2o/device.c
+++ b/drivers/staging/i2o/device.c
@@ -565,10 +565,8 @@ int i2o_parm_table_get(struct i2o_device *dev, int oper, int group,
 		size += 4 - size % 4;
 
 	opblk = kmalloc(size, GFP_KERNEL);
-	if (opblk == NULL) {
-		printk(KERN_ERR "i2o: no memory for query buffer.\n");
+	if (opblk == NULL)
 		return -ENOMEM;
-	}
 
 	opblk[0] = 1;		/* operation count */
 	opblk[1] = 0;		/* pad */
diff --git a/drivers/staging/i2o/driver.c b/drivers/staging/i2o/driver.c
index 111c3edde035..06119bb3eb5f 100644
--- a/drivers/staging/i2o/driver.c
+++ b/drivers/staging/i2o/driver.c
@@ -102,8 +102,7 @@ int i2o_driver_register(struct i2o_driver *drv)
 
 	for (i = 0; i2o_drivers[i]; i++)
 		if (i >= i2o_max_drivers) {
-			osm_err("too many drivers registered, increase "
-				"max_drivers\n");
+			osm_err("too many drivers registered, increase max_drivers\n");
 			spin_unlock_irqrestore(&i2o_drivers_lock, flags);
 			rc = -EFAULT;
 			goto out;
@@ -244,8 +243,8 @@ int i2o_driver_dispatch(struct i2o_controller *c, u32 m)
 	}
 
 	if (unlikely(!drv->reply)) {
-		osm_debug("%s: Reply to driver %s, but no reply function"
-			  " defined!\n", c->name, drv->name);
+		osm_debug("%s: Reply to driver %s, but no reply function defined!\n",
+			c->name, drv->name);
 		return -EIO;
 	}
 
diff --git a/drivers/staging/i2o/exec-osm.c b/drivers/staging/i2o/exec-osm.c
index 16d857d5e655..dce16e425a6e 100644
--- a/drivers/staging/i2o/exec-osm.c
+++ b/drivers/staging/i2o/exec-osm.c
@@ -507,8 +507,8 @@ static int i2o_exec_reply(struct i2o_controller *c, u32 m,
 	 * to aid in debugging.
 	 *
 	 */
-	printk(KERN_WARNING "%s: Unsolicited message reply sent to core!"
-	       "Message dumped to syslog\n", c->name);
+	printk(KERN_WARNING "%s: Unsolicited message reply sent to core! Message dumped to syslog\n",
+			c->name);
 	i2o_dump_message(msg);
 
 	return -EFAULT;
diff --git a/drivers/staging/i2o/i2o_block.c b/drivers/staging/i2o/i2o_block.c
index 0a13c64ce000..406758f755ee 100644
--- a/drivers/staging/i2o/i2o_block.c
+++ b/drivers/staging/i2o/i2o_block.c
@@ -871,13 +871,13 @@ static int i2o_block_transfer(struct request *req)
 
 	return 0;
 
-      context_remove:
+context_remove:
 	i2o_cntxt_list_remove(c, req);
 
-      nop_msg:
+nop_msg:
 	i2o_msg_nop(c, msg);
 
-      exit:
+exit:
 	return rc;
 };
 
@@ -1002,13 +1002,13 @@ static struct i2o_block_device *i2o_block_device_alloc(void)
 
 	return dev;
 
-      cleanup_queue:
+cleanup_queue:
 	put_disk(gd);
 
-      cleanup_dev:
+cleanup_dev:
 	kfree(dev);
 
-      exit:
+exit:
 	return ERR_PTR(rc);
 };
 
@@ -1028,7 +1028,7 @@ static int i2o_block_probe(struct device *dev)
 	struct i2o_block_device *i2o_blk_dev;
 	struct gendisk *gd;
 	struct request_queue *queue;
-	static int unit = 0;
+	static int unit;
 	int rc;
 	u64 size;
 	u32 blocksize;
@@ -1115,10 +1115,10 @@ static int i2o_block_probe(struct device *dev)
 
 	return 0;
 
-      claim_release:
+claim_release:
 	i2o_device_claim_release(i2o_dev);
 
-      exit:
+exit:
 	return rc;
 };
 
@@ -1187,16 +1187,16 @@ static int __init i2o_block_init(void)
 
 	return 0;
 
-      unregister_blkdev:
+unregister_blkdev:
 	unregister_blkdev(I2O_MAJOR, "i2o_block");
 
-      free_mempool:
+free_mempool:
 	mempool_destroy(i2o_blk_req_pool.pool);
 
-      free_slab:
+free_slab:
 	kmem_cache_destroy(i2o_blk_req_pool.slab);
 
-      exit:
+exit:
 	return rc;
 };
 
diff --git a/drivers/staging/i2o/i2o_config.c b/drivers/staging/i2o/i2o_config.c
index 04bd3b6de401..cd7ca5eb18ff 100644
--- a/drivers/staging/i2o/i2o_config.c
+++ b/drivers/staging/i2o/i2o_config.c
@@ -34,8 +34,7 @@
 #include <linux/mutex.h>
 #include <linux/compat.h>
 #include <linux/slab.h>
-
-#include <asm/uaccess.h>
+#include <linux/uaccess.h>
 
 #include "core.h"
 
@@ -65,7 +64,7 @@ struct i2o_cfg_info {
 	struct i2o_cfg_info *next;
 };
 static struct i2o_cfg_info *open_files = NULL;
-static ulong i2o_cfg_info_id = 0;
+static ulong i2o_cfg_info_id;
 
 static int i2o_cfg_getiops(unsigned long arg)
 {
diff --git a/drivers/staging/i2o/i2o_proc.c b/drivers/staging/i2o/i2o_proc.c
index ad84f3304f3c..780fee3224ea 100644
--- a/drivers/staging/i2o/i2o_proc.c
+++ b/drivers/staging/i2o/i2o_proc.c
@@ -48,9 +48,9 @@
 #include <linux/errno.h>
 #include <linux/spinlock.h>
 #include <linux/workqueue.h>
+#include <linux/uaccess.h>
 
 #include <asm/io.h>
-#include <asm/uaccess.h>
 #include <asm/byteorder.h>
 
 /* Structure used to define /proc entries */
@@ -85,9 +85,8 @@ static int print_serial_number(struct seq_file *seq, u8 * serialno, int max_len)
 	switch (serialno[0]) {
 	case I2O_SNFORMAT_BINARY:	/* Binary */
 		seq_printf(seq, "0x");
-		for (i = 0; i < serialno[1]; i++) {
+		for (i = 0; i < serialno[1]; i++)
 			seq_printf(seq, "%02X", serialno[2 + i]);
-		}
 		break;
 
 	case I2O_SNFORMAT_ASCII:	/* ASCII */
@@ -101,9 +100,8 @@ static int print_serial_number(struct seq_file *seq, u8 * serialno, int max_len)
 			seq_printf(seq, "%s", &serialno[2]);
 		} else {
 			/* print chars for specified length */
-			for (i = 0; i < serialno[1]; i++) {
+			for (i = 0; i < serialno[1]; i++)
 				seq_printf(seq, "%c", serialno[2 + i]);
-			}
 		}
 		break;
 
@@ -266,16 +264,22 @@ static int i2o_report_query_status(struct seq_file *seq, int block_status,
 {
 	switch (block_status) {
 	case -ETIMEDOUT:
-		return seq_printf(seq, "Timeout reading group %s.\n", group);
+		seq_printf(seq, "Timeout reading group %s.\n", group);
+		break;
 	case -ENOMEM:
-		return seq_printf(seq, "No free memory to read the table.\n");
+		seq_puts(seq, "No free memory to read the table.\n");
+		break;
 	case -I2O_PARAMS_STATUS_INVALID_GROUP_ID:
-		return seq_printf(seq, "Group %s not supported.\n", group);
+		seq_printf(seq, "Group %s not supported.\n", group);
+		break;
 	default:
-		return seq_printf(seq,
-				  "Error reading group %s. BlockStatus 0x%02X\n",
-				  group, -block_status);
+		seq_printf(seq,
+			   "Error reading group %s. BlockStatus 0x%02X\n",
+			   group, -block_status);
+		break;
 	}
+
+	return 0;
 }
 
 static char *bus_strings[] = {
diff --git a/drivers/staging/i2o/iop.c b/drivers/staging/i2o/iop.c
index 52334fc8b547..23bdbe4aa480 100644
--- a/drivers/staging/i2o/iop.c
+++ b/drivers/staging/i2o/iop.c
@@ -1042,7 +1042,7 @@ static void i2o_iop_release(struct device *dev)
  */
 struct i2o_controller *i2o_iop_alloc(void)
 {
-	static int unit = 0;	/* 0 and 1 are NULL IOP and Local Host */
+	static int unit;	/* 0 and 1 are NULL IOP and Local Host */
 	struct i2o_controller *c;
 	char poolname[32];
 
@@ -1096,7 +1096,8 @@ int i2o_iop_add(struct i2o_controller *c)
 {
 	int rc;
 
-	if ((rc = device_add(&c->device))) {
+	rc = device_add(&c->device);
+	if (rc) {
 		osm_err("%s: could not add controller\n", c->name);
 		goto iop_reset;
 	}
@@ -1105,24 +1106,28 @@ int i2o_iop_add(struct i2o_controller *c)
 	osm_info("%s: This may take a few minutes if there are many devices\n",
 		 c->name);
 
-	if ((rc = i2o_iop_activate(c))) {
+	rc = i2o_iop_activate(c);
+	if (rc) {
 		osm_err("%s: could not activate controller\n", c->name);
 		goto device_del;
 	}
 
 	osm_debug("%s: building sys table...\n", c->name);
 
-	if ((rc = i2o_systab_build()))
+	rc = i2o_systab_build();
+	if (rc)
 		goto device_del;
 
 	osm_debug("%s: online controller...\n", c->name);
 
-	if ((rc = i2o_iop_online(c)))
+	rc = i2o_iop_online(c);
+	if (rc)
 		goto device_del;
 
 	osm_debug("%s: getting LCT...\n", c->name);
 
-	if ((rc = i2o_exec_lct_get(c)))
+	rc = i2o_exec_lct_get(c);
+	if (rc)
 		goto device_del;
 
 	list_add(&c->list, &i2o_controllers);
@@ -1192,13 +1197,16 @@ static int __init i2o_iop_init(void)
 
 	printk(KERN_INFO OSM_DESCRIPTION " v" OSM_VERSION "\n");
 
-	if ((rc = i2o_driver_init()))
+	rc = i2o_driver_init();
+	if (rc)
 		goto exit;
 
-	if ((rc = i2o_exec_init()))
+	rc = i2o_exec_init();
+	if (rc)
 		goto driver_exit;
 
-	if ((rc = i2o_pci_init()))
+	rc = i2o_pci_init();
+	if (rc)
 		goto exec_exit;
 
 	return 0;
diff --git a/drivers/staging/i2o/memory.c b/drivers/staging/i2o/memory.c
index 8f9509d275a4..78b702c18537 100644
--- a/drivers/staging/i2o/memory.c
+++ b/drivers/staging/i2o/memory.c
@@ -270,10 +270,9 @@ EXPORT_SYMBOL_GPL(i2o_dma_realloc);
 int i2o_pool_alloc(struct i2o_pool *pool, const char *name,
 				 size_t size, int min_nr)
 {
-	pool->name = kmalloc(strlen(name) + 1, GFP_KERNEL);
+	pool->name = kstrdup(name, GFP_KERNEL);
 	if (!pool->name)
 		goto exit;
-	strcpy(pool->name, name);
 
 	pool->slab =
 	    kmem_cache_create(pool->name, size, 0, SLAB_HWCACHE_ALIGN, NULL);
diff --git a/drivers/staging/i2o/pci.c b/drivers/staging/i2o/pci.c
index b3b8a61dd4a6..49804c9cf74f 100644
--- a/drivers/staging/i2o/pci.c
+++ b/drivers/staging/i2o/pci.c
@@ -329,7 +329,8 @@ static int i2o_pci_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 		return -ENODEV;
 	}
 
-	if ((rc = pci_enable_device(pdev))) {
+	rc = pci_enable_device(pdev);
+	if (rc) {
 		printk(KERN_WARNING "i2o: couldn't enable device %s\n",
 		       pci_name(pdev));
 		return rc;
@@ -410,7 +411,8 @@ static int i2o_pci_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 #endif
 	}
 
-	if ((rc = i2o_pci_alloc(c))) {
+	rc = i2o_pci_alloc(c);
+	if (rc) {
 		printk(KERN_ERR "%s: DMA / IO allocation for I2O controller "
 		       "failed\n", c->name);
 		goto free_controller;
@@ -422,7 +424,8 @@ static int i2o_pci_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 		goto free_pci;
 	}
 
-	if ((rc = i2o_iop_add(c)))
+	rc = i2o_iop_add(c);
+	if (rc)
 		goto uninstall;
 
 	if (i960)
diff --git a/drivers/staging/iio/Documentation/generic_buffer.c b/drivers/staging/iio/Documentation/generic_buffer.c
deleted file mode 100644
index de4647e2495e..000000000000
--- a/drivers/staging/iio/Documentation/generic_buffer.c
+++ /dev/null
@@ -1,360 +0,0 @@
-/* Industrialio buffer test code.
- *
- * Copyright (c) 2008 Jonathan Cameron
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms of the GNU General Public License version 2 as published by
- * the Free Software Foundation.
- *
- * This program is primarily intended as an example application.
- * Reads the current buffer setup from sysfs and starts a short capture
- * from the specified device, pretty printing the result after appropriate
- * conversion.
- *
- * Command line parameters
- * generic_buffer -n <device_name> -t <trigger_name>
- * If trigger name is not specified the program assumes you want a dataready
- * trigger associated with the device and goes looking for it.
- *
- */
-
-#define _GNU_SOURCE
-
-#include <unistd.h>
-#include <dirent.h>
-#include <fcntl.h>
-#include <stdio.h>
-#include <errno.h>
-#include <sys/stat.h>
-#include <sys/dir.h>
-#include <linux/types.h>
-#include <string.h>
-#include <poll.h>
-#include <endian.h>
-#include <getopt.h>
-#include <inttypes.h>
-#include "iio_utils.h"
-
-/**
- * size_from_channelarray() - calculate the storage size of a scan
- * @channels:		the channel info array
- * @num_channels:	number of channels
- *
- * Has the side effect of filling the channels[i].location values used
- * in processing the buffer output.
- **/
-int size_from_channelarray(struct iio_channel_info *channels, int num_channels)
-{
-	int bytes = 0;
-	int i = 0;
-
-	while (i < num_channels) {
-		if (bytes % channels[i].bytes == 0)
-			channels[i].location = bytes;
-		else
-			channels[i].location = bytes - bytes%channels[i].bytes
-				+ channels[i].bytes;
-		bytes = channels[i].location + channels[i].bytes;
-		i++;
-	}
-	return bytes;
-}
-
-void print2byte(int input, struct iio_channel_info *info)
-{
-	/* First swap if incorrect endian */
-	if (info->be)
-		input = be16toh((uint16_t)input);
-	else
-		input = le16toh((uint16_t)input);
-
-	/*
-	 * Shift before conversion to avoid sign extension
-	 * of left aligned data
-	 */
-	input = input >> info->shift;
-	if (info->is_signed) {
-		int16_t val = input;
-
-		val &= (1 << info->bits_used) - 1;
-		val = (int16_t)(val << (16 - info->bits_used)) >>
-			(16 - info->bits_used);
-		printf("%05f ", ((float)val + info->offset)*info->scale);
-	} else {
-		uint16_t val = input;
-
-		val &= (1 << info->bits_used) - 1;
-		printf("%05f ", ((float)val + info->offset)*info->scale);
-	}
-}
-/**
- * process_scan() - print out the values in SI units
- * @data:		pointer to the start of the scan
- * @channels:		information about the channels. Note
- *  size_from_channelarray must have been called first to fill the
- *  location offsets.
- * @num_channels:	number of channels
- **/
-void process_scan(char *data,
-		  struct iio_channel_info *channels,
-		  int num_channels)
-{
-	int k;
-
-	for (k = 0; k < num_channels; k++)
-		switch (channels[k].bytes) {
-			/* only a few cases implemented so far */
-		case 2:
-			print2byte(*(uint16_t *)(data + channels[k].location),
-				   &channels[k]);
-			break;
-		case 4:
-			if (!channels[k].is_signed) {
-				uint32_t val = *(uint32_t *)
-					(data + channels[k].location);
-				printf("%05f ", ((float)val +
-						 channels[k].offset)*
-				       channels[k].scale);
-
-			}
-			break;
-		case 8:
-			if (channels[k].is_signed) {
-				int64_t val = *(int64_t *)
-					(data +
-					 channels[k].location);
-				if ((val >> channels[k].bits_used) & 1)
-					val = (val & channels[k].mask) |
-						~channels[k].mask;
-				/* special case for timestamp */
-				if (channels[k].scale == 1.0f &&
-				    channels[k].offset == 0.0f)
-					printf("%" PRId64 " ", val);
-				else
-					printf("%05f ", ((float)val +
-							 channels[k].offset)*
-					       channels[k].scale);
-			}
-			break;
-		default:
-			break;
-		}
-	printf("\n");
-}
-
-int main(int argc, char **argv)
-{
-	unsigned long num_loops = 2;
-	unsigned long timedelay = 1000000;
-	unsigned long buf_len = 128;
-
-	int ret, c, i, j, toread;
-	int fp;
-
-	int num_channels;
-	char *trigger_name = NULL, *device_name = NULL;
-	char *dev_dir_name, *buf_dir_name;
-
-	int datardytrigger = 1;
-	char *data;
-	ssize_t read_size;
-	int dev_num, trig_num;
-	char *buffer_access;
-	int scan_size;
-	int noevents = 0;
-	int notrigger = 0;
-	char *dummy;
-
-	struct iio_channel_info *channels;
-
-	while ((c = getopt(argc, argv, "l:w:c:et:n:g")) != -1) {
-		switch (c) {
-		case 'n':
-			device_name = optarg;
-			break;
-		case 't':
-			trigger_name = optarg;
-			datardytrigger = 0;
-			break;
-		case 'e':
-			noevents = 1;
-			break;
-		case 'c':
-			num_loops = strtoul(optarg, &dummy, 10);
-			break;
-		case 'w':
-			timedelay = strtoul(optarg, &dummy, 10);
-			break;
-		case 'l':
-			buf_len = strtoul(optarg, &dummy, 10);
-			break;
-		case 'g':
-			notrigger = 1;
-			break;
-		case '?':
-			return -1;
-		}
-	}
-
-	if (device_name == NULL)
-		return -1;
-
-	/* Find the device requested */
-	dev_num = find_type_by_name(device_name, "iio:device");
-	if (dev_num < 0) {
-		printf("Failed to find the %s\n", device_name);
-		ret = -ENODEV;
-		goto error_ret;
-	}
-	printf("iio device number being used is %d\n", dev_num);
-
-	asprintf(&dev_dir_name, "%siio:device%d", iio_dir, dev_num);
-
-	if (!notrigger) {
-		if (trigger_name == NULL) {
-			/*
-			 * Build the trigger name. If it is device associated
-			 * its name is <device_name>_dev[n] where n matches
-			 * the device number found above.
-			 */
-			ret = asprintf(&trigger_name,
-				       "%s-dev%d", device_name, dev_num);
-			if (ret < 0) {
-				ret = -ENOMEM;
-				goto error_ret;
-			}
-		}
-
-		/* Verify the trigger exists */
-		trig_num = find_type_by_name(trigger_name, "trigger");
-		if (trig_num < 0) {
-			printf("Failed to find the trigger %s\n", trigger_name);
-			ret = -ENODEV;
-			goto error_free_triggername;
-		}
-		printf("iio trigger number being used is %d\n", trig_num);
-	} else
-		printf("trigger-less mode selected\n");
-
-	/*
-	 * Parse the files in scan_elements to identify what channels are
-	 * present
-	 */
-	ret = build_channel_array(dev_dir_name, &channels, &num_channels);
-	if (ret) {
-		printf("Problem reading scan element information\n");
-		printf("diag %s\n", dev_dir_name);
-		goto error_free_triggername;
-	}
-
-	/*
-	 * Construct the directory name for the associated buffer.
-	 * As we know that the lis3l02dq has only one buffer this may
-	 * be built rather than found.
-	 */
-	ret = asprintf(&buf_dir_name,
-		       "%siio:device%d/buffer", iio_dir, dev_num);
-	if (ret < 0) {
-		ret = -ENOMEM;
-		goto error_free_triggername;
-	}
-
-	if (!notrigger) {
-		printf("%s %s\n", dev_dir_name, trigger_name);
-		/* Set the device trigger to be the data ready trigger found
-		 * above */
-		ret = write_sysfs_string_and_verify("trigger/current_trigger",
-						    dev_dir_name,
-						    trigger_name);
-		if (ret < 0) {
-			printf("Failed to write current_trigger file\n");
-			goto error_free_buf_dir_name;
-		}
-	}
-
-	/* Setup ring buffer parameters */
-	ret = write_sysfs_int("length", buf_dir_name, buf_len);
-	if (ret < 0)
-		goto error_free_buf_dir_name;
-
-	/* Enable the buffer */
-	ret = write_sysfs_int("enable", buf_dir_name, 1);
-	if (ret < 0)
-		goto error_free_buf_dir_name;
-	scan_size = size_from_channelarray(channels, num_channels);
-	data = malloc(scan_size*buf_len);
-	if (!data) {
-		ret = -ENOMEM;
-		goto error_free_buf_dir_name;
-	}
-
-	ret = asprintf(&buffer_access, "/dev/iio:device%d", dev_num);
-	if (ret < 0) {
-		ret = -ENOMEM;
-		goto error_free_data;
-	}
-
-	/* Attempt to open non blocking the access dev */
-	fp = open(buffer_access, O_RDONLY | O_NONBLOCK);
-	if (fp == -1) { /* If it isn't there make the node */
-		printf("Failed to open %s\n", buffer_access);
-		ret = -errno;
-		goto error_free_buffer_access;
-	}
-
-	/* Wait for events 10 times */
-	for (j = 0; j < num_loops; j++) {
-		if (!noevents) {
-			struct pollfd pfd = {
-				.fd = fp,
-				.events = POLLIN,
-			};
-
-			poll(&pfd, 1, -1);
-			toread = buf_len;
-
-		} else {
-			usleep(timedelay);
-			toread = 64;
-		}
-
-		read_size = read(fp,
-				 data,
-				 toread*scan_size);
-		if (read_size < 0) {
-			if (errno == -EAGAIN) {
-				printf("nothing available\n");
-				continue;
-			} else
-				break;
-		}
-		for (i = 0; i < read_size/scan_size; i++)
-			process_scan(data + scan_size*i,
-				     channels,
-				     num_channels);
-	}
-
-	/* Stop the buffer */
-	ret = write_sysfs_int("enable", buf_dir_name, 0);
-	if (ret < 0)
-		goto error_close_buffer_access;
-
-	if (!notrigger)
-		/* Disconnect the trigger - just write a dummy name. */
-		write_sysfs_string("trigger/current_trigger",
-				   dev_dir_name, "NULL");
-
-error_close_buffer_access:
-	close(fp);
-error_free_data:
-	free(data);
-error_free_buffer_access:
-	free(buffer_access);
-error_free_buf_dir_name:
-	free(buf_dir_name);
-error_free_triggername:
-	if (datardytrigger)
-		free(trigger_name);
-error_ret:
-	return ret;
-}
diff --git a/drivers/staging/iio/Documentation/iio_event_monitor.c b/drivers/staging/iio/Documentation/iio_event_monitor.c
deleted file mode 100644
index 72c96aa6e992..000000000000
--- a/drivers/staging/iio/Documentation/iio_event_monitor.c
+++ /dev/null
@@ -1,308 +0,0 @@
-/* Industrialio event test code.
- *
- * Copyright (c) 2011-2012 Lars-Peter Clausen <lars@metafoo.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 primarily intended as an example application.
- * Reads the current buffer setup from sysfs and starts a short capture
- * from the specified device, pretty printing the result after appropriate
- * conversion.
- *
- * Usage:
- *	iio_event_monitor <device_name>
- *
- */
-
-#define _GNU_SOURCE
-
-#include <unistd.h>
-#include <stdbool.h>
-#include <stdio.h>
-#include <errno.h>
-#include <string.h>
-#include <poll.h>
-#include <fcntl.h>
-#include <sys/ioctl.h>
-#include "iio_utils.h"
-#include <linux/iio/events.h>
-
-static const char * const iio_chan_type_name_spec[] = {
-	[IIO_VOLTAGE] = "voltage",
-	[IIO_CURRENT] = "current",
-	[IIO_POWER] = "power",
-	[IIO_ACCEL] = "accel",
-	[IIO_ANGL_VEL] = "anglvel",
-	[IIO_MAGN] = "magn",
-	[IIO_LIGHT] = "illuminance",
-	[IIO_INTENSITY] = "intensity",
-	[IIO_PROXIMITY] = "proximity",
-	[IIO_TEMP] = "temp",
-	[IIO_INCLI] = "incli",
-	[IIO_ROT] = "rot",
-	[IIO_ANGL] = "angl",
-	[IIO_TIMESTAMP] = "timestamp",
-	[IIO_CAPACITANCE] = "capacitance",
-	[IIO_ALTVOLTAGE] = "altvoltage",
-	[IIO_CCT] = "cct",
-	[IIO_PRESSURE] = "pressure",
-	[IIO_HUMIDITYRELATIVE] = "humidityrelative",
-	[IIO_ACTIVITY] = "activity",
-	[IIO_STEPS] = "steps",
-};
-
-static const char * const iio_ev_type_text[] = {
-	[IIO_EV_TYPE_THRESH] = "thresh",
-	[IIO_EV_TYPE_MAG] = "mag",
-	[IIO_EV_TYPE_ROC] = "roc",
-	[IIO_EV_TYPE_THRESH_ADAPTIVE] = "thresh_adaptive",
-	[IIO_EV_TYPE_MAG_ADAPTIVE] = "mag_adaptive",
-	[IIO_EV_TYPE_CHANGE] = "change",
-};
-
-static const char * const iio_ev_dir_text[] = {
-	[IIO_EV_DIR_EITHER] = "either",
-	[IIO_EV_DIR_RISING] = "rising",
-	[IIO_EV_DIR_FALLING] = "falling"
-};
-
-static const char * const iio_modifier_names[] = {
-	[IIO_MOD_X] = "x",
-	[IIO_MOD_Y] = "y",
-	[IIO_MOD_Z] = "z",
-	[IIO_MOD_X_AND_Y] = "x&y",
-	[IIO_MOD_X_AND_Z] = "x&z",
-	[IIO_MOD_Y_AND_Z] = "y&z",
-	[IIO_MOD_X_AND_Y_AND_Z] = "x&y&z",
-	[IIO_MOD_X_OR_Y] = "x|y",
-	[IIO_MOD_X_OR_Z] = "x|z",
-	[IIO_MOD_Y_OR_Z] = "y|z",
-	[IIO_MOD_X_OR_Y_OR_Z] = "x|y|z",
-	[IIO_MOD_LIGHT_BOTH] = "both",
-	[IIO_MOD_LIGHT_IR] = "ir",
-	[IIO_MOD_ROOT_SUM_SQUARED_X_Y] = "sqrt(x^2+y^2)",
-	[IIO_MOD_SUM_SQUARED_X_Y_Z] = "x^2+y^2+z^2",
-	[IIO_MOD_LIGHT_CLEAR] = "clear",
-	[IIO_MOD_LIGHT_RED] = "red",
-	[IIO_MOD_LIGHT_GREEN] = "green",
-	[IIO_MOD_LIGHT_BLUE] = "blue",
-	[IIO_MOD_QUATERNION] = "quaternion",
-	[IIO_MOD_TEMP_AMBIENT] = "ambient",
-	[IIO_MOD_TEMP_OBJECT] = "object",
-	[IIO_MOD_NORTH_MAGN] = "from_north_magnetic",
-	[IIO_MOD_NORTH_TRUE] = "from_north_true",
-	[IIO_MOD_NORTH_MAGN_TILT_COMP] = "from_north_magnetic_tilt_comp",
-	[IIO_MOD_NORTH_TRUE_TILT_COMP] = "from_north_true_tilt_comp",
-	[IIO_MOD_RUNNING] = "running",
-	[IIO_MOD_JOGGING] = "jogging",
-	[IIO_MOD_WALKING] = "walking",
-	[IIO_MOD_STILL] = "still",
-};
-
-static bool event_is_known(struct iio_event_data *event)
-{
-	enum iio_chan_type type = IIO_EVENT_CODE_EXTRACT_CHAN_TYPE(event->id);
-	enum iio_modifier mod = IIO_EVENT_CODE_EXTRACT_MODIFIER(event->id);
-	enum iio_event_type ev_type = IIO_EVENT_CODE_EXTRACT_TYPE(event->id);
-	enum iio_event_direction dir = IIO_EVENT_CODE_EXTRACT_DIR(event->id);
-
-	switch (type) {
-	case IIO_VOLTAGE:
-	case IIO_CURRENT:
-	case IIO_POWER:
-	case IIO_ACCEL:
-	case IIO_ANGL_VEL:
-	case IIO_MAGN:
-	case IIO_LIGHT:
-	case IIO_INTENSITY:
-	case IIO_PROXIMITY:
-	case IIO_TEMP:
-	case IIO_INCLI:
-	case IIO_ROT:
-	case IIO_ANGL:
-	case IIO_TIMESTAMP:
-	case IIO_CAPACITANCE:
-	case IIO_ALTVOLTAGE:
-	case IIO_CCT:
-	case IIO_PRESSURE:
-	case IIO_HUMIDITYRELATIVE:
-	case IIO_ACTIVITY:
-	case IIO_STEPS:
-		break;
-	default:
-		return false;
-	}
-
-	switch (mod) {
-	case IIO_NO_MOD:
-	case IIO_MOD_X:
-	case IIO_MOD_Y:
-	case IIO_MOD_Z:
-	case IIO_MOD_X_AND_Y:
-	case IIO_MOD_X_AND_Z:
-	case IIO_MOD_Y_AND_Z:
-	case IIO_MOD_X_AND_Y_AND_Z:
-	case IIO_MOD_X_OR_Y:
-	case IIO_MOD_X_OR_Z:
-	case IIO_MOD_Y_OR_Z:
-	case IIO_MOD_X_OR_Y_OR_Z:
-	case IIO_MOD_LIGHT_BOTH:
-	case IIO_MOD_LIGHT_IR:
-	case IIO_MOD_ROOT_SUM_SQUARED_X_Y:
-	case IIO_MOD_SUM_SQUARED_X_Y_Z:
-	case IIO_MOD_LIGHT_CLEAR:
-	case IIO_MOD_LIGHT_RED:
-	case IIO_MOD_LIGHT_GREEN:
-	case IIO_MOD_LIGHT_BLUE:
-	case IIO_MOD_QUATERNION:
-	case IIO_MOD_TEMP_AMBIENT:
-	case IIO_MOD_TEMP_OBJECT:
-	case IIO_MOD_NORTH_MAGN:
-	case IIO_MOD_NORTH_TRUE:
-	case IIO_MOD_NORTH_MAGN_TILT_COMP:
-	case IIO_MOD_NORTH_TRUE_TILT_COMP:
-	case IIO_MOD_RUNNING:
-	case IIO_MOD_JOGGING:
-	case IIO_MOD_WALKING:
-	case IIO_MOD_STILL:
-		break;
-	default:
-		return false;
-	}
-
-	switch (ev_type) {
-	case IIO_EV_TYPE_THRESH:
-	case IIO_EV_TYPE_MAG:
-	case IIO_EV_TYPE_ROC:
-	case IIO_EV_TYPE_THRESH_ADAPTIVE:
-	case IIO_EV_TYPE_MAG_ADAPTIVE:
-	case IIO_EV_TYPE_CHANGE:
-		break;
-	default:
-		return false;
-	}
-
-	switch (dir) {
-	case IIO_EV_DIR_EITHER:
-	case IIO_EV_DIR_RISING:
-	case IIO_EV_DIR_FALLING:
-	case IIO_EV_DIR_NONE:
-		break;
-	default:
-		return false;
-	}
-
-	return true;
-}
-
-static void print_event(struct iio_event_data *event)
-{
-	enum iio_chan_type type = IIO_EVENT_CODE_EXTRACT_CHAN_TYPE(event->id);
-	enum iio_modifier mod = IIO_EVENT_CODE_EXTRACT_MODIFIER(event->id);
-	enum iio_event_type ev_type = IIO_EVENT_CODE_EXTRACT_TYPE(event->id);
-	enum iio_event_direction dir = IIO_EVENT_CODE_EXTRACT_DIR(event->id);
-	int chan = IIO_EVENT_CODE_EXTRACT_CHAN(event->id);
-	int chan2 = IIO_EVENT_CODE_EXTRACT_CHAN2(event->id);
-	bool diff = IIO_EVENT_CODE_EXTRACT_DIFF(event->id);
-
-	if (!event_is_known(event)) {
-		printf("Unknown event: time: %lld, id: %llx\n",
-				event->timestamp, event->id);
-		return;
-	}
-
-	printf("Event: time: %lld, ", event->timestamp);
-
-	if (mod != IIO_NO_MOD) {
-		printf("type: %s(%s), ",
-			iio_chan_type_name_spec[type],
-			iio_modifier_names[mod]);
-	} else {
-		printf("type: %s, ",
-			iio_chan_type_name_spec[type]);
-	}
-
-	if (diff && chan >= 0 && chan2 >= 0)
-		printf("channel: %d-%d, ", chan, chan2);
-	else if (chan >= 0)
-		printf("channel: %d, ", chan);
-
-	printf("evtype: %s", iio_ev_type_text[ev_type]);
-
-	if (dir != IIO_EV_DIR_NONE)
-		printf(", direction: %s", iio_ev_dir_text[dir]);
-	printf("\n");
-}
-
-int main(int argc, char **argv)
-{
-	struct iio_event_data event;
-	const char *device_name;
-	char *chrdev_name;
-	int ret;
-	int dev_num;
-	int fd, event_fd;
-
-	if (argc <= 1) {
-		printf("Usage: %s <device_name>\n", argv[0]);
-		return -1;
-	}
-
-	device_name = argv[1];
-
-	dev_num = find_type_by_name(device_name, "iio:device");
-	if (dev_num >= 0) {
-		printf("Found IIO device with name %s with device number %d\n",
-			device_name, dev_num);
-		ret = asprintf(&chrdev_name, "/dev/iio:device%d", dev_num);
-		if (ret < 0) {
-			ret = -ENOMEM;
-			goto error_ret;
-		}
-	} else {
-		/* If we can't find a IIO device by name assume device_name is a
-		   IIO chrdev */
-		chrdev_name = strdup(device_name);
-	}
-
-	fd = open(chrdev_name, 0);
-	if (fd == -1) {
-		fprintf(stdout, "Failed to open %s\n", chrdev_name);
-		ret = -errno;
-		goto error_free_chrdev_name;
-	}
-
-	ret = ioctl(fd, IIO_GET_EVENT_FD_IOCTL, &event_fd);
-
-	close(fd);
-
-	if (ret == -1 || event_fd == -1) {
-		fprintf(stdout, "Failed to retrieve event fd\n");
-		ret = -errno;
-		goto error_free_chrdev_name;
-	}
-
-	while (true) {
-		ret = read(event_fd, &event, sizeof(event));
-		if (ret == -1) {
-			if (errno == EAGAIN) {
-				printf("nothing available\n");
-				continue;
-			} else {
-				perror("Failed to read event from device");
-				ret = -errno;
-				break;
-			}
-		}
-
-		print_event(&event);
-	}
-
-	close(event_fd);
-error_free_chrdev_name:
-	free(chrdev_name);
-error_ret:
-	return ret;
-}
diff --git a/drivers/staging/iio/Documentation/iio_utils.h b/drivers/staging/iio/Documentation/iio_utils.h
deleted file mode 100644
index 568eff06f803..000000000000
--- a/drivers/staging/iio/Documentation/iio_utils.h
+++ /dev/null
@@ -1,683 +0,0 @@
-/* IIO - useful set of util functionality
- *
- * Copyright (c) 2008 Jonathan Cameron
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms of the GNU General Public License version 2 as published by
- * the Free Software Foundation.
- */
-
-#include <string.h>
-#include <stdlib.h>
-#include <ctype.h>
-#include <stdio.h>
-#include <stdint.h>
-#include <dirent.h>
-#include <errno.h>
-
-/* Made up value to limit allocation sizes */
-#define IIO_MAX_NAME_LENGTH 30
-
-#define FORMAT_SCAN_ELEMENTS_DIR "%s/scan_elements"
-#define FORMAT_TYPE_FILE "%s_type"
-
-const char *iio_dir = "/sys/bus/iio/devices/";
-
-/**
- * iioutils_break_up_name() - extract generic name from full channel name
- * @full_name: the full channel name
- * @generic_name: the output generic channel name
- **/
-inline int iioutils_break_up_name(const char *full_name,
-				  char **generic_name)
-{
-	char *current;
-	char *w, *r;
-	char *working;
-
-	current = strdup(full_name);
-	working = strtok(current, "_\0");
-	w = working;
-	r = working;
-
-	while (*r != '\0') {
-		if (!isdigit(*r)) {
-			*w = *r;
-			w++;
-		}
-		r++;
-	}
-	*w = '\0';
-	*generic_name = strdup(working);
-	free(current);
-
-	return 0;
-}
-
-/**
- * struct iio_channel_info - information about a given channel
- * @name: channel name
- * @generic_name: general name for channel type
- * @scale: scale factor to be applied for conversion to si units
- * @offset: offset to be applied for conversion to si units
- * @index: the channel index in the buffer output
- * @bytes: number of bytes occupied in buffer output
- * @mask: a bit mask for the raw output
- * @is_signed: is the raw value stored signed
- * @enabled: is this channel enabled
- **/
-struct iio_channel_info {
-	char *name;
-	char *generic_name;
-	float scale;
-	float offset;
-	unsigned index;
-	unsigned bytes;
-	unsigned bits_used;
-	unsigned shift;
-	uint64_t mask;
-	unsigned be;
-	unsigned is_signed;
-	unsigned location;
-};
-
-/**
- * iioutils_get_type() - find and process _type attribute data
- * @is_signed: output whether channel is signed
- * @bytes: output how many bytes the channel storage occupies
- * @mask: output a bit mask for the raw data
- * @be: big endian
- * @device_dir: the iio device directory
- * @name: the channel name
- * @generic_name: the channel type name
- **/
-inline int iioutils_get_type(unsigned *is_signed,
-			     unsigned *bytes,
-			     unsigned *bits_used,
-			     unsigned *shift,
-			     uint64_t *mask,
-			     unsigned *be,
-			     const char *device_dir,
-			     const char *name,
-			     const char *generic_name)
-{
-	FILE *sysfsfp;
-	int ret;
-	DIR *dp;
-	char *scan_el_dir, *builtname, *builtname_generic, *filename = 0;
-	char signchar, endianchar;
-	unsigned padint;
-	const struct dirent *ent;
-
-	ret = asprintf(&scan_el_dir, FORMAT_SCAN_ELEMENTS_DIR, device_dir);
-	if (ret < 0) {
-		ret = -ENOMEM;
-		goto error_ret;
-	}
-	ret = asprintf(&builtname, FORMAT_TYPE_FILE, name);
-	if (ret < 0) {
-		ret = -ENOMEM;
-		goto error_free_scan_el_dir;
-	}
-	ret = asprintf(&builtname_generic, FORMAT_TYPE_FILE, generic_name);
-	if (ret < 0) {
-		ret = -ENOMEM;
-		goto error_free_builtname;
-	}
-
-	dp = opendir(scan_el_dir);
-	if (dp == NULL) {
-		ret = -errno;
-		goto error_free_builtname_generic;
-	}
-	while (ent = readdir(dp), ent != NULL)
-		/*
-		 * Do we allow devices to override a generic name with
-		 * a specific one?
-		 */
-		if ((strcmp(builtname, ent->d_name) == 0) ||
-		    (strcmp(builtname_generic, ent->d_name) == 0)) {
-			ret = asprintf(&filename,
-				       "%s/%s", scan_el_dir, ent->d_name);
-			if (ret < 0) {
-				ret = -ENOMEM;
-				goto error_closedir;
-			}
-			sysfsfp = fopen(filename, "r");
-			if (sysfsfp == NULL) {
-				printf("failed to open %s\n", filename);
-				ret = -errno;
-				goto error_free_filename;
-			}
-
-			ret = fscanf(sysfsfp,
-				     "%ce:%c%u/%u>>%u",
-				     &endianchar,
-				     &signchar,
-				     bits_used,
-				     &padint, shift);
-			if (ret < 0) {
-				printf("failed to pass scan type description\n");
-				ret = -errno;
-				goto error_close_sysfsfp;
-			}
-			*be = (endianchar == 'b');
-			*bytes = padint / 8;
-			if (*bits_used == 64)
-				*mask = ~0;
-			else
-				*mask = (1 << *bits_used) - 1;
-			if (signchar == 's')
-				*is_signed = 1;
-			else
-				*is_signed = 0;
-			fclose(sysfsfp);
-			free(filename);
-
-			filename = 0;
-			sysfsfp = 0;
-		}
-error_close_sysfsfp:
-	if (sysfsfp)
-		fclose(sysfsfp);
-error_free_filename:
-	if (filename)
-		free(filename);
-error_closedir:
-	closedir(dp);
-error_free_builtname_generic:
-	free(builtname_generic);
-error_free_builtname:
-	free(builtname);
-error_free_scan_el_dir:
-	free(scan_el_dir);
-error_ret:
-	return ret;
-}
-
-inline int iioutils_get_param_float(float *output,
-				    const char *param_name,
-				    const char *device_dir,
-				    const char *name,
-				    const char *generic_name)
-{
-	FILE *sysfsfp;
-	int ret;
-	DIR *dp;
-	char *builtname, *builtname_generic;
-	char *filename = NULL;
-	const struct dirent *ent;
-
-	ret = asprintf(&builtname, "%s_%s", name, param_name);
-	if (ret < 0) {
-		ret = -ENOMEM;
-		goto error_ret;
-	}
-	ret = asprintf(&builtname_generic,
-		       "%s_%s", generic_name, param_name);
-	if (ret < 0) {
-		ret = -ENOMEM;
-		goto error_free_builtname;
-	}
-	dp = opendir(device_dir);
-	if (dp == NULL) {
-		ret = -errno;
-		goto error_free_builtname_generic;
-	}
-	while (ent = readdir(dp), ent != NULL)
-		if ((strcmp(builtname, ent->d_name) == 0) ||
-		    (strcmp(builtname_generic, ent->d_name) == 0)) {
-			ret = asprintf(&filename,
-				       "%s/%s", device_dir, ent->d_name);
-			if (ret < 0) {
-				ret = -ENOMEM;
-				goto error_closedir;
-			}
-			sysfsfp = fopen(filename, "r");
-			if (!sysfsfp) {
-				ret = -errno;
-				goto error_free_filename;
-			}
-			fscanf(sysfsfp, "%f", output);
-			break;
-		}
-error_free_filename:
-	if (filename)
-		free(filename);
-error_closedir:
-	closedir(dp);
-error_free_builtname_generic:
-	free(builtname_generic);
-error_free_builtname:
-	free(builtname);
-error_ret:
-	return ret;
-}
-
-/**
- * bsort_channel_array_by_index() - reorder so that the array is in index order
- *
- **/
-
-inline void bsort_channel_array_by_index(struct iio_channel_info **ci_array,
-					 int cnt)
-{
-
-	struct iio_channel_info temp;
-	int x, y;
-
-	for (x = 0; x < cnt; x++)
-		for (y = 0; y < (cnt - 1); y++)
-			if ((*ci_array)[y].index > (*ci_array)[y+1].index) {
-				temp = (*ci_array)[y + 1];
-				(*ci_array)[y + 1] = (*ci_array)[y];
-				(*ci_array)[y] = temp;
-			}
-}
-
-/**
- * build_channel_array() - function to figure out what channels are present
- * @device_dir: the IIO device directory in sysfs
- * @
- **/
-inline int build_channel_array(const char *device_dir,
-			      struct iio_channel_info **ci_array,
-			      int *counter)
-{
-	DIR *dp;
-	FILE *sysfsfp;
-	int count, i;
-	struct iio_channel_info *current;
-	int ret;
-	const struct dirent *ent;
-	char *scan_el_dir;
-	char *filename;
-
-	*counter = 0;
-	ret = asprintf(&scan_el_dir, FORMAT_SCAN_ELEMENTS_DIR, device_dir);
-	if (ret < 0) {
-		ret = -ENOMEM;
-		goto error_ret;
-	}
-	dp = opendir(scan_el_dir);
-	if (dp == NULL) {
-		ret = -errno;
-		goto error_free_name;
-	}
-	while (ent = readdir(dp), ent != NULL)
-		if (strcmp(ent->d_name + strlen(ent->d_name) - strlen("_en"),
-			   "_en") == 0) {
-			ret = asprintf(&filename,
-				       "%s/%s", scan_el_dir, ent->d_name);
-			if (ret < 0) {
-				ret = -ENOMEM;
-				goto error_close_dir;
-			}
-			sysfsfp = fopen(filename, "r");
-			if (sysfsfp == NULL) {
-				ret = -errno;
-				free(filename);
-				goto error_close_dir;
-			}
-			fscanf(sysfsfp, "%i", &ret);
-			if (ret == 1)
-				(*counter)++;
-			fclose(sysfsfp);
-			free(filename);
-		}
-	*ci_array = malloc(sizeof(**ci_array) * (*counter));
-	if (*ci_array == NULL) {
-		ret = -ENOMEM;
-		goto error_close_dir;
-	}
-	seekdir(dp, 0);
-	count = 0;
-	while (ent = readdir(dp), ent != NULL) {
-		if (strcmp(ent->d_name + strlen(ent->d_name) - strlen("_en"),
-			   "_en") == 0) {
-			int current_enabled = 0;
-
-			current = &(*ci_array)[count++];
-			ret = asprintf(&filename,
-				       "%s/%s", scan_el_dir, ent->d_name);
-			if (ret < 0) {
-				ret = -ENOMEM;
-				/* decrement count to avoid freeing name */
-				count--;
-				goto error_cleanup_array;
-			}
-			sysfsfp = fopen(filename, "r");
-			if (sysfsfp == NULL) {
-				free(filename);
-				ret = -errno;
-				goto error_cleanup_array;
-			}
-			fscanf(sysfsfp, "%i", &current_enabled);
-			fclose(sysfsfp);
-
-			if (!current_enabled) {
-				free(filename);
-				count--;
-				continue;
-			}
-
-			current->scale = 1.0;
-			current->offset = 0;
-			current->name = strndup(ent->d_name,
-						strlen(ent->d_name) -
-						strlen("_en"));
-			if (current->name == NULL) {
-				free(filename);
-				ret = -ENOMEM;
-				goto error_cleanup_array;
-			}
-			/* Get the generic and specific name elements */
-			ret = iioutils_break_up_name(current->name,
-						     &current->generic_name);
-			if (ret) {
-				free(filename);
-				goto error_cleanup_array;
-			}
-			ret = asprintf(&filename,
-				       "%s/%s_index",
-				       scan_el_dir,
-				       current->name);
-			if (ret < 0) {
-				free(filename);
-				ret = -ENOMEM;
-				goto error_cleanup_array;
-			}
-			sysfsfp = fopen(filename, "r");
-			fscanf(sysfsfp, "%u", &current->index);
-			fclose(sysfsfp);
-			free(filename);
-			/* Find the scale */
-			ret = iioutils_get_param_float(&current->scale,
-						       "scale",
-						       device_dir,
-						       current->name,
-						       current->generic_name);
-			if (ret < 0)
-				goto error_cleanup_array;
-			ret = iioutils_get_param_float(&current->offset,
-						       "offset",
-						       device_dir,
-						       current->name,
-						       current->generic_name);
-			if (ret < 0)
-				goto error_cleanup_array;
-			ret = iioutils_get_type(&current->is_signed,
-						&current->bytes,
-						&current->bits_used,
-						&current->shift,
-						&current->mask,
-						&current->be,
-						device_dir,
-						current->name,
-						current->generic_name);
-		}
-	}
-
-	closedir(dp);
-	/* reorder so that the array is in index order */
-	bsort_channel_array_by_index(ci_array, *counter);
-
-	return 0;
-
-error_cleanup_array:
-	for (i = count - 1;  i >= 0; i--)
-		free((*ci_array)[i].name);
-	free(*ci_array);
-error_close_dir:
-	closedir(dp);
-error_free_name:
-	free(scan_el_dir);
-error_ret:
-	return ret;
-}
-
-/**
- * find_type_by_name() - function to match top level types by name
- * @name: top level type instance name
- * @type: the type of top level instance being sort
- *
- * Typical types this is used for are device and trigger.
- **/
-inline int find_type_by_name(const char *name, const char *type)
-{
-	const struct dirent *ent;
-	int number, numstrlen;
-
-	FILE *nameFile;
-	DIR *dp;
-	char thisname[IIO_MAX_NAME_LENGTH];
-	char *filename;
-
-	dp = opendir(iio_dir);
-	if (dp == NULL) {
-		printf("No industrialio devices available\n");
-		return -ENODEV;
-	}
-
-	while (ent = readdir(dp), ent != NULL) {
-		if (strcmp(ent->d_name, ".") != 0 &&
-			strcmp(ent->d_name, "..") != 0 &&
-			strlen(ent->d_name) > strlen(type) &&
-			strncmp(ent->d_name, type, strlen(type)) == 0) {
-			numstrlen = sscanf(ent->d_name + strlen(type),
-					   "%d",
-					   &number);
-			/* verify the next character is not a colon */
-			if (strncmp(ent->d_name + strlen(type) + numstrlen,
-					":",
-					1) != 0) {
-				filename = malloc(strlen(iio_dir)
-						+ strlen(type)
-						+ numstrlen
-						+ 6);
-				if (filename == NULL) {
-					closedir(dp);
-					return -ENOMEM;
-				}
-				sprintf(filename, "%s%s%d/name",
-					iio_dir,
-					type,
-					number);
-				nameFile = fopen(filename, "r");
-				if (!nameFile) {
-					free(filename);
-					continue;
-				}
-				free(filename);
-				fscanf(nameFile, "%s", thisname);
-				fclose(nameFile);
-				if (strcmp(name, thisname) == 0) {
-					closedir(dp);
-					return number;
-				}
-			}
-		}
-	}
-	closedir(dp);
-	return -ENODEV;
-}
-
-inline int _write_sysfs_int(char *filename, char *basedir, int val, int verify)
-{
-	int ret = 0;
-	FILE *sysfsfp;
-	int test;
-	char *temp = malloc(strlen(basedir) + strlen(filename) + 2);
-
-	if (temp == NULL)
-		return -ENOMEM;
-	sprintf(temp, "%s/%s", basedir, filename);
-	sysfsfp = fopen(temp, "w");
-	if (sysfsfp == NULL) {
-		printf("failed to open %s\n", temp);
-		ret = -errno;
-		goto error_free;
-	}
-	fprintf(sysfsfp, "%d", val);
-	fclose(sysfsfp);
-	if (verify) {
-		sysfsfp = fopen(temp, "r");
-		if (sysfsfp == NULL) {
-			printf("failed to open %s\n", temp);
-			ret = -errno;
-			goto error_free;
-		}
-		fscanf(sysfsfp, "%d", &test);
-		fclose(sysfsfp);
-		if (test != val) {
-			printf("Possible failure in int write %d to %s%s\n",
-				val,
-				basedir,
-				filename);
-			ret = -1;
-		}
-	}
-error_free:
-	free(temp);
-	return ret;
-}
-
-int write_sysfs_int(char *filename, char *basedir, int val)
-{
-	return _write_sysfs_int(filename, basedir, val, 0);
-}
-
-int write_sysfs_int_and_verify(char *filename, char *basedir, int val)
-{
-	return _write_sysfs_int(filename, basedir, val, 1);
-}
-
-int _write_sysfs_string(char *filename, char *basedir, char *val, int verify)
-{
-	int ret = 0;
-	FILE  *sysfsfp;
-	char *temp = malloc(strlen(basedir) + strlen(filename) + 2);
-
-	if (temp == NULL) {
-		printf("Memory allocation failed\n");
-		return -ENOMEM;
-	}
-	sprintf(temp, "%s/%s", basedir, filename);
-	sysfsfp = fopen(temp, "w");
-	if (sysfsfp == NULL) {
-		printf("Could not open %s\n", temp);
-		ret = -errno;
-		goto error_free;
-	}
-	fprintf(sysfsfp, "%s", val);
-	fclose(sysfsfp);
-	if (verify) {
-		sysfsfp = fopen(temp, "r");
-		if (sysfsfp == NULL) {
-			printf("could not open file to verify\n");
-			ret = -errno;
-			goto error_free;
-		}
-		fscanf(sysfsfp, "%s", temp);
-		fclose(sysfsfp);
-		if (strcmp(temp, val) != 0) {
-			printf("Possible failure in string write of %s "
-				"Should be %s "
-				"written to %s\%s\n",
-				temp,
-				val,
-				basedir,
-				filename);
-			ret = -1;
-		}
-	}
-error_free:
-	free(temp);
-
-	return ret;
-}
-
-/**
- * write_sysfs_string_and_verify() - string write, readback and verify
- * @filename: name of file to write to
- * @basedir: the sysfs directory in which the file is to be found
- * @val: the string to write
- **/
-int write_sysfs_string_and_verify(char *filename, char *basedir, char *val)
-{
-	return _write_sysfs_string(filename, basedir, val, 1);
-}
-
-int write_sysfs_string(char *filename, char *basedir, char *val)
-{
-	return _write_sysfs_string(filename, basedir, val, 0);
-}
-
-int read_sysfs_posint(char *filename, char *basedir)
-{
-	int ret;
-	FILE  *sysfsfp;
-	char *temp = malloc(strlen(basedir) + strlen(filename) + 2);
-
-	if (temp == NULL) {
-		printf("Memory allocation failed");
-		return -ENOMEM;
-	}
-	sprintf(temp, "%s/%s", basedir, filename);
-	sysfsfp = fopen(temp, "r");
-	if (sysfsfp == NULL) {
-		ret = -errno;
-		goto error_free;
-	}
-	fscanf(sysfsfp, "%d\n", &ret);
-	fclose(sysfsfp);
-error_free:
-	free(temp);
-	return ret;
-}
-
-int read_sysfs_float(char *filename, char *basedir, float *val)
-{
-	int ret = 0;
-	FILE  *sysfsfp;
-	char *temp = malloc(strlen(basedir) + strlen(filename) + 2);
-
-	if (temp == NULL) {
-		printf("Memory allocation failed");
-		return -ENOMEM;
-	}
-	sprintf(temp, "%s/%s", basedir, filename);
-	sysfsfp = fopen(temp, "r");
-	if (sysfsfp == NULL) {
-		ret = -errno;
-		goto error_free;
-	}
-	fscanf(sysfsfp, "%f\n", val);
-	fclose(sysfsfp);
-error_free:
-	free(temp);
-	return ret;
-}
-
-int read_sysfs_string(const char *filename, const char *basedir, char *str)
-{
-	int ret = 0;
-	FILE  *sysfsfp;
-	char *temp = malloc(strlen(basedir) + strlen(filename) + 2);
-
-	if (temp == NULL) {
-		printf("Memory allocation failed");
-		return -ENOMEM;
-	}
-	sprintf(temp, "%s/%s", basedir, filename);
-	sysfsfp = fopen(temp, "r");
-	if (sysfsfp == NULL) {
-		ret = -errno;
-		goto error_free;
-	}
-	fscanf(sysfsfp, "%s\n", str);
-	fclose(sysfsfp);
-error_free:
-	free(temp);
-	return ret;
-}
diff --git a/drivers/staging/iio/Documentation/lsiio.c b/drivers/staging/iio/Documentation/lsiio.c
deleted file mode 100644
index 98a0de098130..000000000000
--- a/drivers/staging/iio/Documentation/lsiio.c
+++ /dev/null
@@ -1,163 +0,0 @@
-/*
- * Industrial I/O utilities - lsiio.c
- *
- * Copyright (c) 2010 Manuel Stahl <manuel.stahl@iis.fraunhofer.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.
- */
-
-#include <string.h>
-#include <dirent.h>
-#include <stdio.h>
-#include <errno.h>
-#include <stdint.h>
-#include <stdlib.h>
-#include <unistd.h>
-#include <sys/types.h>
-#include <sys/stat.h>
-#include <sys/dir.h>
-#include "iio_utils.h"
-
-
-static enum verbosity {
-	VERBLEVEL_DEFAULT,	/* 0 gives lspci behaviour */
-	VERBLEVEL_SENSORS,	/* 1 lists sensors */
-} verblevel = VERBLEVEL_DEFAULT;
-
-const char *type_device = "iio:device";
-const char *type_trigger = "trigger";
-
-
-static inline int check_prefix(const char *str, const char *prefix)
-{
-	return strlen(str) > strlen(prefix) &&
-		strncmp(str, prefix, strlen(prefix)) == 0;
-}
-
-static inline int check_postfix(const char *str, const char *postfix)
-{
-	return strlen(str) > strlen(postfix) &&
-		strcmp(str + strlen(str) - strlen(postfix), postfix) == 0;
-}
-
-static int dump_channels(const char *dev_dir_name)
-{
-	DIR *dp;
-	const struct dirent *ent;
-
-	dp = opendir(dev_dir_name);
-	if (dp == NULL)
-		return -errno;
-	while (ent = readdir(dp), ent != NULL)
-		if (check_prefix(ent->d_name, "in_") &&
-		    check_postfix(ent->d_name, "_raw")) {
-			printf("   %-10s\n", ent->d_name);
-		}
-
-	return 0;
-}
-
-static int dump_one_device(const char *dev_dir_name)
-{
-	char name[IIO_MAX_NAME_LENGTH];
-	int dev_idx;
-	int retval;
-
-	retval = sscanf(dev_dir_name + strlen(iio_dir) + strlen(type_device),
-			"%i", &dev_idx);
-	if (retval != 1)
-		return -EINVAL;
-	read_sysfs_string("name", dev_dir_name, name);
-	printf("Device %03d: %s\n", dev_idx, name);
-
-	if (verblevel >= VERBLEVEL_SENSORS)
-		return dump_channels(dev_dir_name);
-	return 0;
-}
-
-static int dump_one_trigger(const char *dev_dir_name)
-{
-	char name[IIO_MAX_NAME_LENGTH];
-	int dev_idx;
-	int retval;
-
-	retval = sscanf(dev_dir_name + strlen(iio_dir) + strlen(type_trigger),
-			"%i", &dev_idx);
-	if (retval != 1)
-		return -EINVAL;
-	read_sysfs_string("name", dev_dir_name, name);
-	printf("Trigger %03d: %s\n", dev_idx, name);
-	return 0;
-}
-
-static void dump_devices(void)
-{
-	const struct dirent *ent;
-	int number, numstrlen;
-
-	FILE *nameFile;
-	DIR *dp;
-	char thisname[IIO_MAX_NAME_LENGTH];
-	char *filename;
-
-	dp = opendir(iio_dir);
-	if (dp == NULL) {
-		printf("No industrial I/O devices available\n");
-		return;
-	}
-
-	while (ent = readdir(dp), ent != NULL) {
-		if (check_prefix(ent->d_name, type_device)) {
-			char *dev_dir_name;
-
-			asprintf(&dev_dir_name, "%s%s", iio_dir, ent->d_name);
-			dump_one_device(dev_dir_name);
-			free(dev_dir_name);
-			if (verblevel >= VERBLEVEL_SENSORS)
-				printf("\n");
-		}
-	}
-	rewinddir(dp);
-	while (ent = readdir(dp), ent != NULL) {
-		if (check_prefix(ent->d_name, type_trigger)) {
-			char *dev_dir_name;
-
-			asprintf(&dev_dir_name, "%s%s", iio_dir, ent->d_name);
-			dump_one_trigger(dev_dir_name);
-			free(dev_dir_name);
-		}
-	}
-	closedir(dp);
-}
-
-int main(int argc, char **argv)
-{
-	int c, err = 0;
-
-	while ((c = getopt(argc, argv, "d:D:v")) != EOF) {
-		switch (c) {
-		case 'v':
-			verblevel++;
-			break;
-
-		case '?':
-		default:
-			err++;
-			break;
-		}
-	}
-	if (err || argc > optind) {
-		fprintf(stderr, "Usage: lsiio [options]...\n"
-			"List industrial I/O devices\n"
-			"  -v, --verbose\n"
-			"      Increase verbosity (may be given multiple times)\n"
-			);
-		exit(1);
-	}
-
-	dump_devices();
-
-	return 0;
-}
diff --git a/drivers/staging/iio/Kconfig b/drivers/staging/iio/Kconfig
index 24183028bd71..6d5b38d69578 100644
--- a/drivers/staging/iio/Kconfig
+++ b/drivers/staging/iio/Kconfig
@@ -38,6 +38,7 @@ config IIO_SIMPLE_DUMMY_EVENTS
 config IIO_SIMPLE_DUMMY_BUFFER
 	bool "Buffered capture support"
 	select IIO_BUFFER
+	select IIO_TRIGGER
 	select IIO_KFIFO_BUF
 	help
 	  Add buffered data capture to the simple dummy driver.
diff --git a/drivers/staging/iio/accel/adis16201.h b/drivers/staging/iio/accel/adis16201.h
index 8747de5a9805..e6b8c9af6e22 100644
--- a/drivers/staging/iio/accel/adis16201.h
+++ b/drivers/staging/iio/accel/adis16201.h
@@ -34,24 +34,24 @@
 #define ADIS16201_GLOB_CMD       0x3E /* Operation, system command register */
 
 /* MSC_CTRL */
-#define ADIS16201_MSC_CTRL_SELF_TEST_EN	        (1 << 8)  /* Self-test enable */
-#define ADIS16201_MSC_CTRL_DATA_RDY_EN	        (1 << 2)  /* Data-ready enable: 1 = enabled, 0 = disabled */
-#define ADIS16201_MSC_CTRL_ACTIVE_HIGH	        (1 << 1)  /* Data-ready polarity: 1 = active high, 0 = active low */
-#define ADIS16201_MSC_CTRL_DATA_RDY_DIO1	(1 << 0)  /* Data-ready line selection: 1 = DIO1, 0 = DIO0 */
+#define ADIS16201_MSC_CTRL_SELF_TEST_EN	        BIT(8)  /* Self-test enable */
+#define ADIS16201_MSC_CTRL_DATA_RDY_EN	        BIT(2)  /* Data-ready enable: 1 = enabled, 0 = disabled */
+#define ADIS16201_MSC_CTRL_ACTIVE_HIGH	        BIT(1)  /* Data-ready polarity: 1 = active high, 0 = active low */
+#define ADIS16201_MSC_CTRL_DATA_RDY_DIO1	BIT(0)  /* Data-ready line selection: 1 = DIO1, 0 = DIO0 */
 
 /* DIAG_STAT */
-#define ADIS16201_DIAG_STAT_ALARM2        (1<<9) /* Alarm 2 status: 1 = alarm active, 0 = alarm inactive */
-#define ADIS16201_DIAG_STAT_ALARM1        (1<<8) /* Alarm 1 status: 1 = alarm active, 0 = alarm inactive */
+#define ADIS16201_DIAG_STAT_ALARM2        BIT(9) /* Alarm 2 status: 1 = alarm active, 0 = alarm inactive */
+#define ADIS16201_DIAG_STAT_ALARM1        BIT(8) /* Alarm 1 status: 1 = alarm active, 0 = alarm inactive */
 #define ADIS16201_DIAG_STAT_SPI_FAIL_BIT   3 /* SPI communications failure */
 #define ADIS16201_DIAG_STAT_FLASH_UPT_BIT  2 /* Flash update failure */
 #define ADIS16201_DIAG_STAT_POWER_HIGH_BIT 1 /* Power supply above 3.625 V */
 #define ADIS16201_DIAG_STAT_POWER_LOW_BIT  0 /* Power supply below 3.15 V */
 
 /* GLOB_CMD */
-#define ADIS16201_GLOB_CMD_SW_RESET	(1<<7)
-#define ADIS16201_GLOB_CMD_FACTORY_CAL	(1<<1)
+#define ADIS16201_GLOB_CMD_SW_RESET	BIT(7)
+#define ADIS16201_GLOB_CMD_FACTORY_CAL	BIT(1)
 
-#define ADIS16201_ERROR_ACTIVE          (1<<14)
+#define ADIS16201_ERROR_ACTIVE          BIT(14)
 
 enum adis16201_scan {
 	ADIS16201_SCAN_ACC_X,
diff --git a/drivers/staging/iio/accel/adis16201_core.c b/drivers/staging/iio/accel/adis16201_core.c
index 7eae5fd0f393..10db685813c9 100644
--- a/drivers/staging/iio/accel/adis16201_core.c
+++ b/drivers/staging/iio/accel/adis16201_core.c
@@ -135,14 +135,14 @@ static const struct iio_chan_spec adis16201_channels[] = {
 	ADIS_SUPPLY_CHAN(ADIS16201_SUPPLY_OUT, ADIS16201_SCAN_SUPPLY, 0, 12),
 	ADIS_TEMP_CHAN(ADIS16201_TEMP_OUT, ADIS16201_SCAN_TEMP, 0, 12),
 	ADIS_ACCEL_CHAN(X, ADIS16201_XACCL_OUT, ADIS16201_SCAN_ACC_X,
-		BIT(IIO_CHAN_INFO_CALIBBIAS), 0, 14),
+			BIT(IIO_CHAN_INFO_CALIBBIAS), 0, 14),
 	ADIS_ACCEL_CHAN(Y, ADIS16201_YACCL_OUT, ADIS16201_SCAN_ACC_Y,
-		BIT(IIO_CHAN_INFO_CALIBBIAS), 0, 14),
+			BIT(IIO_CHAN_INFO_CALIBBIAS), 0, 14),
 	ADIS_AUX_ADC_CHAN(ADIS16201_AUX_ADC, ADIS16201_SCAN_AUX_ADC, 0, 12),
 	ADIS_INCLI_CHAN(X, ADIS16201_XINCL_OUT, ADIS16201_SCAN_INCLI_X,
-		BIT(IIO_CHAN_INFO_CALIBBIAS), 0, 14),
+			BIT(IIO_CHAN_INFO_CALIBBIAS), 0, 14),
 	ADIS_INCLI_CHAN(X, ADIS16201_YINCL_OUT, ADIS16201_SCAN_INCLI_Y,
-		BIT(IIO_CHAN_INFO_CALIBBIAS), 0, 14),
+			BIT(IIO_CHAN_INFO_CALIBBIAS), 0, 14),
 	IIO_CHAN_SOFT_TIMESTAMP(7)
 };
 
diff --git a/drivers/staging/iio/accel/adis16203.h b/drivers/staging/iio/accel/adis16203.h
index acc688d7ea9e..6426e38bf006 100644
--- a/drivers/staging/iio/accel/adis16203.h
+++ b/drivers/staging/iio/accel/adis16203.h
@@ -25,16 +25,16 @@
 #define ADIS16203_GLOB_CMD       0x3E /* Operation, system command register */
 
 /* MSC_CTRL */
-#define ADIS16203_MSC_CTRL_PWRUP_SELF_TEST	(1 << 10) /* Self-test at power-on: 1 = disabled, 0 = enabled */
-#define ADIS16203_MSC_CTRL_REVERSE_ROT_EN	(1 << 9)  /* Reverses rotation of both inclination outputs */
-#define ADIS16203_MSC_CTRL_SELF_TEST_EN	        (1 << 8)  /* Self-test enable */
-#define ADIS16203_MSC_CTRL_DATA_RDY_EN	        (1 << 2)  /* Data-ready enable: 1 = enabled, 0 = disabled */
-#define ADIS16203_MSC_CTRL_ACTIVE_HIGH	        (1 << 1)  /* Data-ready polarity: 1 = active high, 0 = active low */
-#define ADIS16203_MSC_CTRL_DATA_RDY_DIO1	(1 << 0)  /* Data-ready line selection: 1 = DIO1, 0 = DIO0 */
+#define ADIS16203_MSC_CTRL_PWRUP_SELF_TEST	BIT(10) /* Self-test at power-on: 1 = disabled, 0 = enabled */
+#define ADIS16203_MSC_CTRL_REVERSE_ROT_EN	BIT(9)  /* Reverses rotation of both inclination outputs */
+#define ADIS16203_MSC_CTRL_SELF_TEST_EN	        BIT(8)  /* Self-test enable */
+#define ADIS16203_MSC_CTRL_DATA_RDY_EN	        BIT(2)  /* Data-ready enable: 1 = enabled, 0 = disabled */
+#define ADIS16203_MSC_CTRL_ACTIVE_HIGH	        BIT(1)  /* Data-ready polarity: 1 = active high, 0 = active low */
+#define ADIS16203_MSC_CTRL_DATA_RDY_DIO1	BIT(0)  /* Data-ready line selection: 1 = DIO1, 0 = DIO0 */
 
 /* DIAG_STAT */
-#define ADIS16203_DIAG_STAT_ALARM2        (1<<9) /* Alarm 2 status: 1 = alarm active, 0 = alarm inactive */
-#define ADIS16203_DIAG_STAT_ALARM1        (1<<8) /* Alarm 1 status: 1 = alarm active, 0 = alarm inactive */
+#define ADIS16203_DIAG_STAT_ALARM2        BIT(9) /* Alarm 2 status: 1 = alarm active, 0 = alarm inactive */
+#define ADIS16203_DIAG_STAT_ALARM1        BIT(8) /* Alarm 1 status: 1 = alarm active, 0 = alarm inactive */
 #define ADIS16203_DIAG_STAT_SELFTEST_FAIL_BIT 5 /* Self-test diagnostic error flag */
 #define ADIS16203_DIAG_STAT_SPI_FAIL_BIT      3 /* SPI communications failure */
 #define ADIS16203_DIAG_STAT_FLASH_UPT_BIT     2 /* Flash update failure */
@@ -42,11 +42,11 @@
 #define ADIS16203_DIAG_STAT_POWER_LOW_BIT     0 /* Power supply below 3.15 V */
 
 /* GLOB_CMD */
-#define ADIS16203_GLOB_CMD_SW_RESET	(1<<7)
-#define ADIS16203_GLOB_CMD_CLEAR_STAT	(1<<4)
-#define ADIS16203_GLOB_CMD_FACTORY_CAL	(1<<1)
+#define ADIS16203_GLOB_CMD_SW_RESET	BIT(7)
+#define ADIS16203_GLOB_CMD_CLEAR_STAT	BIT(4)
+#define ADIS16203_GLOB_CMD_FACTORY_CAL	BIT(1)
 
-#define ADIS16203_ERROR_ACTIVE          (1<<14)
+#define ADIS16203_ERROR_ACTIVE          BIT(14)
 
 enum adis16203_scan {
 	ADIS16203_SCAN_INCLI_X,
diff --git a/drivers/staging/iio/accel/adis16203_core.c b/drivers/staging/iio/accel/adis16203_core.c
index fbbe93f26bbe..fb593d23d5bc 100644
--- a/drivers/staging/iio/accel/adis16203_core.c
+++ b/drivers/staging/iio/accel/adis16203_core.c
@@ -104,10 +104,10 @@ static const struct iio_chan_spec adis16203_channels[] = {
 	ADIS_SUPPLY_CHAN(ADIS16203_SUPPLY_OUT, ADIS16203_SCAN_SUPPLY, 0, 12),
 	ADIS_AUX_ADC_CHAN(ADIS16203_AUX_ADC, ADIS16203_SCAN_AUX_ADC, 0, 12),
 	ADIS_INCLI_CHAN(X, ADIS16203_XINCL_OUT, ADIS16203_SCAN_INCLI_X,
-		BIT(IIO_CHAN_INFO_CALIBBIAS), 0, 14),
+			BIT(IIO_CHAN_INFO_CALIBBIAS), 0, 14),
 	/* Fixme: Not what it appears to be - see data sheet */
 	ADIS_INCLI_CHAN(Y, ADIS16203_YINCL_OUT, ADIS16203_SCAN_INCLI_Y,
-		0, 0, 14),
+			0, 0, 14),
 	ADIS_TEMP_CHAN(ADIS16203_TEMP_OUT, ADIS16203_SCAN_TEMP, 0, 12),
 	IIO_CHAN_SOFT_TIMESTAMP(5),
 };
diff --git a/drivers/staging/iio/accel/adis16204.h b/drivers/staging/iio/accel/adis16204.h
index 9ff950c1e8db..0b23f0b5c52f 100644
--- a/drivers/staging/iio/accel/adis16204.h
+++ b/drivers/staging/iio/accel/adis16204.h
@@ -33,15 +33,15 @@
 #define ADIS16204_GLOB_CMD       0x3E /* Operation, system command register */
 
 /* MSC_CTRL */
-#define ADIS16204_MSC_CTRL_PWRUP_SELF_TEST	(1 << 10) /* Self-test at power-on: 1 = disabled, 0 = enabled */
-#define ADIS16204_MSC_CTRL_SELF_TEST_EN	        (1 << 8)  /* Self-test enable */
-#define ADIS16204_MSC_CTRL_DATA_RDY_EN	        (1 << 2)  /* Data-ready enable: 1 = enabled, 0 = disabled */
-#define ADIS16204_MSC_CTRL_ACTIVE_HIGH	        (1 << 1)  /* Data-ready polarity: 1 = active high, 0 = active low */
-#define ADIS16204_MSC_CTRL_DATA_RDY_DIO2	(1 << 0)  /* Data-ready line selection: 1 = DIO2, 0 = DIO1 */
+#define ADIS16204_MSC_CTRL_PWRUP_SELF_TEST	BIT(10) /* Self-test at power-on: 1 = disabled, 0 = enabled */
+#define ADIS16204_MSC_CTRL_SELF_TEST_EN	        BIT(8)  /* Self-test enable */
+#define ADIS16204_MSC_CTRL_DATA_RDY_EN	        BIT(2)  /* Data-ready enable: 1 = enabled, 0 = disabled */
+#define ADIS16204_MSC_CTRL_ACTIVE_HIGH	        BIT(1)  /* Data-ready polarity: 1 = active high, 0 = active low */
+#define ADIS16204_MSC_CTRL_DATA_RDY_DIO2	BIT(0)  /* Data-ready line selection: 1 = DIO2, 0 = DIO1 */
 
 /* DIAG_STAT */
-#define ADIS16204_DIAG_STAT_ALARM2        (1<<9) /* Alarm 2 status: 1 = alarm active, 0 = alarm inactive */
-#define ADIS16204_DIAG_STAT_ALARM1        (1<<8) /* Alarm 1 status: 1 = alarm active, 0 = alarm inactive */
+#define ADIS16204_DIAG_STAT_ALARM2        BIT(9) /* Alarm 2 status: 1 = alarm active, 0 = alarm inactive */
+#define ADIS16204_DIAG_STAT_ALARM1        BIT(8) /* Alarm 1 status: 1 = alarm active, 0 = alarm inactive */
 #define ADIS16204_DIAG_STAT_SELFTEST_FAIL_BIT 5 /* Self-test diagnostic error flag: 1 = error condition,
 						0 = normal operation */
 #define ADIS16204_DIAG_STAT_SPI_FAIL_BIT      3 /* SPI communications failure */
@@ -50,11 +50,11 @@
 #define ADIS16204_DIAG_STAT_POWER_LOW_BIT     0 /* Power supply below 2.975 V */
 
 /* GLOB_CMD */
-#define ADIS16204_GLOB_CMD_SW_RESET	(1<<7)
-#define ADIS16204_GLOB_CMD_CLEAR_STAT	(1<<4)
-#define ADIS16204_GLOB_CMD_FACTORY_CAL	(1<<1)
+#define ADIS16204_GLOB_CMD_SW_RESET	BIT(7)
+#define ADIS16204_GLOB_CMD_CLEAR_STAT	BIT(4)
+#define ADIS16204_GLOB_CMD_FACTORY_CAL	BIT(1)
 
-#define ADIS16204_ERROR_ACTIVE          (1<<14)
+#define ADIS16204_ERROR_ACTIVE          BIT(14)
 
 enum adis16204_scan {
 	ADIS16204_SCAN_ACC_X,
diff --git a/drivers/staging/iio/accel/adis16204_core.c b/drivers/staging/iio/accel/adis16204_core.c
index 4c8acbc2e44e..ea0ac2467ac2 100644
--- a/drivers/staging/iio/accel/adis16204_core.c
+++ b/drivers/staging/iio/accel/adis16204_core.c
@@ -141,11 +141,13 @@ static const struct iio_chan_spec adis16204_channels[] = {
 	ADIS_AUX_ADC_CHAN(ADIS16204_AUX_ADC, ADIS16204_SCAN_AUX_ADC, 0, 12),
 	ADIS_TEMP_CHAN(ADIS16204_TEMP_OUT, ADIS16204_SCAN_TEMP, 0, 12),
 	ADIS_ACCEL_CHAN(X, ADIS16204_XACCL_OUT, ADIS16204_SCAN_ACC_X,
-		BIT(IIO_CHAN_INFO_CALIBBIAS) | BIT(IIO_CHAN_INFO_PEAK), 0, 14),
+			BIT(IIO_CHAN_INFO_CALIBBIAS) | BIT(IIO_CHAN_INFO_PEAK),
+			0, 14),
 	ADIS_ACCEL_CHAN(Y, ADIS16204_YACCL_OUT, ADIS16204_SCAN_ACC_Y,
-		BIT(IIO_CHAN_INFO_CALIBBIAS) | BIT(IIO_CHAN_INFO_PEAK), 0, 14),
+			BIT(IIO_CHAN_INFO_CALIBBIAS) | BIT(IIO_CHAN_INFO_PEAK),
+			0, 14),
 	ADIS_ACCEL_CHAN(ROOT_SUM_SQUARED_X_Y, ADIS16204_XY_RSS_OUT,
-		ADIS16204_SCAN_ACC_XY, BIT(IIO_CHAN_INFO_PEAK), 0, 14),
+			ADIS16204_SCAN_ACC_XY, BIT(IIO_CHAN_INFO_PEAK), 0, 14),
 	IIO_CHAN_SOFT_TIMESTAMP(5),
 };
 
diff --git a/drivers/staging/iio/accel/adis16209.h b/drivers/staging/iio/accel/adis16209.h
index ad3945a06292..813698d18ec8 100644
--- a/drivers/staging/iio/accel/adis16209.h
+++ b/drivers/staging/iio/accel/adis16209.h
@@ -60,21 +60,21 @@
 
 /* MSC_CTRL */
 /* Self-test at power-on: 1 = disabled, 0 = enabled */
-#define ADIS16209_MSC_CTRL_PWRUP_SELF_TEST	(1 << 10)
+#define ADIS16209_MSC_CTRL_PWRUP_SELF_TEST	BIT(10)
 /* Self-test enable */
-#define ADIS16209_MSC_CTRL_SELF_TEST_EN	        (1 << 8)
+#define ADIS16209_MSC_CTRL_SELF_TEST_EN	        BIT(8)
 /* Data-ready enable: 1 = enabled, 0 = disabled */
-#define ADIS16209_MSC_CTRL_DATA_RDY_EN	        (1 << 2)
+#define ADIS16209_MSC_CTRL_DATA_RDY_EN	        BIT(2)
 /* Data-ready polarity: 1 = active high, 0 = active low */
-#define ADIS16209_MSC_CTRL_ACTIVE_HIGH	        (1 << 1)
+#define ADIS16209_MSC_CTRL_ACTIVE_HIGH	        BIT(1)
 /* Data-ready line selection: 1 = DIO2, 0 = DIO1 */
-#define ADIS16209_MSC_CTRL_DATA_RDY_DIO2	(1 << 0)
+#define ADIS16209_MSC_CTRL_DATA_RDY_DIO2	BIT(0)
 
 /* DIAG_STAT */
 /* Alarm 2 status: 1 = alarm active, 0 = alarm inactive */
-#define ADIS16209_DIAG_STAT_ALARM2        (1<<9)
+#define ADIS16209_DIAG_STAT_ALARM2        BIT(9)
 /* Alarm 1 status: 1 = alarm active, 0 = alarm inactive */
-#define ADIS16209_DIAG_STAT_ALARM1        (1<<8)
+#define ADIS16209_DIAG_STAT_ALARM1        BIT(8)
 /* Self-test diagnostic error flag: 1 = error condition, 0 = normal operation */
 #define ADIS16209_DIAG_STAT_SELFTEST_FAIL_BIT	5
 /* SPI communications failure */
@@ -87,11 +87,11 @@
 #define ADIS16209_DIAG_STAT_POWER_LOW_BIT	0
 
 /* GLOB_CMD */
-#define ADIS16209_GLOB_CMD_SW_RESET	(1<<7)
-#define ADIS16209_GLOB_CMD_CLEAR_STAT	(1<<4)
-#define ADIS16209_GLOB_CMD_FACTORY_CAL	(1<<1)
+#define ADIS16209_GLOB_CMD_SW_RESET	BIT(7)
+#define ADIS16209_GLOB_CMD_CLEAR_STAT	BIT(4)
+#define ADIS16209_GLOB_CMD_FACTORY_CAL	BIT(1)
 
-#define ADIS16209_ERROR_ACTIVE          (1<<14)
+#define ADIS16209_ERROR_ACTIVE          BIT(14)
 
 #define ADIS16209_SCAN_SUPPLY	0
 #define ADIS16209_SCAN_ACC_X	1
diff --git a/drivers/staging/iio/accel/adis16209_core.c b/drivers/staging/iio/accel/adis16209_core.c
index b2c7aeda75f0..d1dc1a3cb3ce 100644
--- a/drivers/staging/iio/accel/adis16209_core.c
+++ b/drivers/staging/iio/accel/adis16209_core.c
@@ -134,14 +134,14 @@ static const struct iio_chan_spec adis16209_channels[] = {
 	ADIS_SUPPLY_CHAN(ADIS16209_SUPPLY_OUT, ADIS16209_SCAN_SUPPLY, 0, 14),
 	ADIS_TEMP_CHAN(ADIS16209_TEMP_OUT, ADIS16209_SCAN_TEMP, 0, 12),
 	ADIS_ACCEL_CHAN(X, ADIS16209_XACCL_OUT, ADIS16209_SCAN_ACC_X,
-		BIT(IIO_CHAN_INFO_CALIBBIAS), 0, 14),
+			BIT(IIO_CHAN_INFO_CALIBBIAS), 0, 14),
 	ADIS_ACCEL_CHAN(Y, ADIS16209_YACCL_OUT, ADIS16209_SCAN_ACC_Y,
-		BIT(IIO_CHAN_INFO_CALIBBIAS), 0, 14),
+			BIT(IIO_CHAN_INFO_CALIBBIAS), 0, 14),
 	ADIS_AUX_ADC_CHAN(ADIS16209_AUX_ADC, ADIS16209_SCAN_AUX_ADC, 0, 12),
 	ADIS_INCLI_CHAN(X, ADIS16209_XINCL_OUT, ADIS16209_SCAN_INCLI_X,
-		0, 0, 14),
+			0, 0, 14),
 	ADIS_INCLI_CHAN(Y, ADIS16209_YINCL_OUT, ADIS16209_SCAN_INCLI_Y,
-		0, 0, 14),
+			0, 0, 14),
 	ADIS_ROT_CHAN(X, ADIS16209_ROT_OUT, ADIS16209_SCAN_ROT, 0, 0, 14),
 	IIO_CHAN_SOFT_TIMESTAMP(8)
 };
@@ -178,7 +178,6 @@ static const struct adis_data adis16209_data = {
 		BIT(ADIS16209_DIAG_STAT_POWER_LOW_BIT),
 };
 
-
 static int adis16209_probe(struct spi_device *spi)
 {
 	int ret;
diff --git a/drivers/staging/iio/accel/adis16220.h b/drivers/staging/iio/accel/adis16220.h
index a894ad7fb26d..eab86331124f 100644
--- a/drivers/staging/iio/accel/adis16220.h
+++ b/drivers/staging/iio/accel/adis16220.h
@@ -71,35 +71,35 @@
 #define ADIS16220_CAPTURE_SIZE  2048
 
 /* MSC_CTRL */
-#define ADIS16220_MSC_CTRL_SELF_TEST_EN	        (1 << 8)
-#define ADIS16220_MSC_CTRL_POWER_SUP_COM_AIN1	(1 << 1)
-#define ADIS16220_MSC_CTRL_POWER_SUP_COM_AIN2	(1 << 0)
+#define ADIS16220_MSC_CTRL_SELF_TEST_EN	        BIT(8)
+#define ADIS16220_MSC_CTRL_POWER_SUP_COM_AIN1	BIT(1)
+#define ADIS16220_MSC_CTRL_POWER_SUP_COM_AIN2	BIT(0)
 
 /* DIO_CTRL */
-#define ADIS16220_MSC_CTRL_DIO2_BUSY_IND     (3<<4)
-#define ADIS16220_MSC_CTRL_DIO1_BUSY_IND     (3<<2)
-#define ADIS16220_MSC_CTRL_DIO2_ACT_HIGH     (1<<1)
-#define ADIS16220_MSC_CTRL_DIO1_ACT_HIGH     (1<<0)
+#define ADIS16220_MSC_CTRL_DIO2_BUSY_IND     (BIT(5) | BIT(4))
+#define ADIS16220_MSC_CTRL_DIO1_BUSY_IND     (BIT(3) | BIT(2))
+#define ADIS16220_MSC_CTRL_DIO2_ACT_HIGH     BIT(1)
+#define ADIS16220_MSC_CTRL_DIO1_ACT_HIGH     BIT(0)
 
 /* DIAG_STAT */
 /* AIN2 sample > ALM_MAG2 */
-#define ADIS16220_DIAG_STAT_ALM_MAG2    (1<<14)
+#define ADIS16220_DIAG_STAT_ALM_MAG2    BIT(14)
 /* AIN1 sample > ALM_MAG1 */
-#define ADIS16220_DIAG_STAT_ALM_MAG1    (1<<13)
+#define ADIS16220_DIAG_STAT_ALM_MAG1    BIT(13)
 /* Acceleration sample > ALM_MAGA */
-#define ADIS16220_DIAG_STAT_ALM_MAGA    (1<<12)
+#define ADIS16220_DIAG_STAT_ALM_MAGA    BIT(12)
 /* Error condition programmed into ALM_MAGS[11:0] and ALM_CTRL[5:4] is true */
-#define ADIS16220_DIAG_STAT_ALM_MAGS    (1<<11)
+#define ADIS16220_DIAG_STAT_ALM_MAGS    BIT(11)
 /* |Peak value in AIN2 data capture| > ALM_MAG2 */
-#define ADIS16220_DIAG_STAT_PEAK_AIN2   (1<<10)
+#define ADIS16220_DIAG_STAT_PEAK_AIN2   BIT(10)
 /* |Peak value in AIN1 data capture| > ALM_MAG1 */
-#define ADIS16220_DIAG_STAT_PEAK_AIN1   (1<<9)
+#define ADIS16220_DIAG_STAT_PEAK_AIN1   BIT(9)
 /* |Peak value in acceleration data capture| > ALM_MAGA */
-#define ADIS16220_DIAG_STAT_PEAK_ACCEL  (1<<8)
+#define ADIS16220_DIAG_STAT_PEAK_ACCEL  BIT(8)
 /* Data ready, capture complete */
-#define ADIS16220_DIAG_STAT_DATA_RDY    (1<<7)
-#define ADIS16220_DIAG_STAT_FLASH_CHK	(1<<6)
-#define ADIS16220_DIAG_STAT_SELF_TEST	(1<<5)
+#define ADIS16220_DIAG_STAT_DATA_RDY    BIT(7)
+#define ADIS16220_DIAG_STAT_FLASH_CHK	BIT(6)
+#define ADIS16220_DIAG_STAT_SELF_TEST	BIT(5)
 /* Capture period violation/interruption */
 #define ADIS16220_DIAG_STAT_VIOLATION_BIT	4
 /* SPI communications failure */
@@ -112,9 +112,9 @@
 #define ADIS16220_DIAG_STAT_POWER_LOW_BIT	0
 
 /* GLOB_CMD */
-#define ADIS16220_GLOB_CMD_SW_RESET	(1<<7)
-#define ADIS16220_GLOB_CMD_SELF_TEST	(1<<2)
-#define ADIS16220_GLOB_CMD_PWR_DOWN	(1<<1)
+#define ADIS16220_GLOB_CMD_SW_RESET	BIT(7)
+#define ADIS16220_GLOB_CMD_SELF_TEST	BIT(2)
+#define ADIS16220_GLOB_CMD_PWR_DOWN	BIT(1)
 
 #define ADIS16220_MAX_TX 2048
 #define ADIS16220_MAX_RX 2048
diff --git a/drivers/staging/iio/accel/adis16220_core.c b/drivers/staging/iio/accel/adis16220_core.c
index d478f5130a0f..e46a91c69a31 100644
--- a/drivers/staging/iio/accel/adis16220_core.c
+++ b/drivers/staging/iio/accel/adis16220_core.c
@@ -21,29 +21,28 @@
 #include "adis16220.h"
 
 static ssize_t adis16220_read_16bit(struct device *dev,
-		struct device_attribute *attr,
-		char *buf)
+				    struct device_attribute *attr,
+				    char *buf)
 {
 	struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
 	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct adis16220_state *st = iio_priv(indio_dev);
 	ssize_t ret;
-	s16 val = 0;
+	u16 val;
 
 	/* Take the iio_dev status lock */
 	mutex_lock(&indio_dev->mlock);
-	ret = adis_read_reg_16(&st->adis, this_attr->address,
-					(u16 *)&val);
+	ret = adis_read_reg_16(&st->adis, this_attr->address, &val);
 	mutex_unlock(&indio_dev->mlock);
 	if (ret)
 		return ret;
-	return sprintf(buf, "%d\n", val);
+	return sprintf(buf, "%u\n", val);
 }
 
 static ssize_t adis16220_write_16bit(struct device *dev,
-		struct device_attribute *attr,
-		const char *buf,
-		size_t len)
+				     struct device_attribute *attr,
+				     const char *buf,
+				     size_t len)
 {
 	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
@@ -76,8 +75,8 @@ static int adis16220_capture(struct iio_dev *indio_dev)
 }
 
 static ssize_t adis16220_write_capture(struct device *dev,
-		struct device_attribute *attr,
-		const char *buf, size_t len)
+				       struct device_attribute *attr,
+				       const char *buf, size_t len)
 {
 	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	bool val;
@@ -96,10 +95,10 @@ static ssize_t adis16220_write_capture(struct device *dev,
 }
 
 static ssize_t adis16220_capture_buffer_read(struct iio_dev *indio_dev,
-					char *buf,
-					loff_t off,
-					size_t count,
-					int addr)
+					     char *buf,
+					     loff_t off,
+					     size_t count,
+					     int addr)
 {
 	struct adis16220_state *st = iio_priv(indio_dev);
 	struct spi_transfer xfers[] = {
@@ -131,15 +130,14 @@ static ssize_t adis16220_capture_buffer_read(struct iio_dev *indio_dev,
 
 	/* write the begin position of capture buffer */
 	ret = adis_write_reg_16(&st->adis,
-					ADIS16220_CAPT_PNTR,
-					off > 1);
+				ADIS16220_CAPT_PNTR,
+				off > 1);
 	if (ret)
 		return -EIO;
 
 	/* read count/2 values from capture buffer */
 	mutex_lock(&st->buf_lock);
 
-
 	for (i = 0; i < count; i += 2) {
 		st->tx[i] = ADIS_READ_REG(addr);
 		st->tx[i + 1] = 0;
@@ -148,7 +146,6 @@ static ssize_t adis16220_capture_buffer_read(struct iio_dev *indio_dev,
 
 	ret = spi_sync_transfer(st->adis.spi, xfers, ARRAY_SIZE(xfers));
 	if (ret) {
-
 		mutex_unlock(&st->buf_lock);
 		return -EIO;
 	}
@@ -182,9 +179,9 @@ static struct bin_attribute accel_bin = {
 };
 
 static ssize_t adis16220_adc1_bin_read(struct file *filp, struct kobject *kobj,
-				struct bin_attribute *attr,
-				char *buf, loff_t off,
-				size_t count)
+				       struct bin_attribute *attr,
+				       char *buf, loff_t off,
+				       size_t count)
 {
 	struct iio_dev *indio_dev = dev_to_iio_dev(kobj_to_dev(kobj));
 
@@ -203,9 +200,9 @@ static struct bin_attribute adc1_bin = {
 };
 
 static ssize_t adis16220_adc2_bin_read(struct file *filp, struct kobject *kobj,
-				struct bin_attribute *attr,
-				char *buf, loff_t off,
-				size_t count)
+				       struct bin_attribute *attr,
+				       char *buf, loff_t off,
+				       size_t count)
 {
 	struct iio_dev *indio_dev = dev_to_iio_dev(kobj_to_dev(kobj));
 
@@ -214,7 +211,6 @@ static ssize_t adis16220_adc2_bin_read(struct file *filp, struct kobject *kobj,
 					ADIS16220_CAPT_BUF2);
 }
 
-
 static struct bin_attribute adc2_bin = {
 	.attr = {
 		.name = "in1_bin",
diff --git a/drivers/staging/iio/accel/adis16240.h b/drivers/staging/iio/accel/adis16240.h
index d442d49f51f4..66b5ad2f42c5 100644
--- a/drivers/staging/iio/accel/adis16240.h
+++ b/drivers/staging/iio/accel/adis16240.h
@@ -74,31 +74,31 @@
 
 /* MSC_CTRL */
 /* Enables sum-of-squares output (XYZPEAK_OUT) */
-#define ADIS16240_MSC_CTRL_XYZPEAK_OUT_EN	(1 << 15)
+#define ADIS16240_MSC_CTRL_XYZPEAK_OUT_EN	BIT(15)
 /* Enables peak tracking output (XPEAK_OUT, YPEAK_OUT, and ZPEAK_OUT) */
-#define ADIS16240_MSC_CTRL_X_Y_ZPEAK_OUT_EN	(1 << 14)
+#define ADIS16240_MSC_CTRL_X_Y_ZPEAK_OUT_EN	BIT(14)
 /* Self-test enable: 1 = apply electrostatic force, 0 = disabled */
-#define ADIS16240_MSC_CTRL_SELF_TEST_EN	        (1 << 8)
+#define ADIS16240_MSC_CTRL_SELF_TEST_EN	        BIT(8)
 /* Data-ready enable: 1 = enabled, 0 = disabled */
-#define ADIS16240_MSC_CTRL_DATA_RDY_EN	        (1 << 2)
+#define ADIS16240_MSC_CTRL_DATA_RDY_EN	        BIT(2)
 /* Data-ready polarity: 1 = active high, 0 = active low */
-#define ADIS16240_MSC_CTRL_ACTIVE_HIGH	        (1 << 1)
+#define ADIS16240_MSC_CTRL_ACTIVE_HIGH	        BIT(1)
 /* Data-ready line selection: 1 = DIO2, 0 = DIO1 */
-#define ADIS16240_MSC_CTRL_DATA_RDY_DIO2	(1 << 0)
+#define ADIS16240_MSC_CTRL_DATA_RDY_DIO2	BIT(0)
 
 /* DIAG_STAT */
 /* Alarm 2 status: 1 = alarm active, 0 = alarm inactive */
-#define ADIS16240_DIAG_STAT_ALARM2      (1<<9)
+#define ADIS16240_DIAG_STAT_ALARM2      BIT(9)
 /* Alarm 1 status: 1 = alarm active, 0 = alarm inactive */
-#define ADIS16240_DIAG_STAT_ALARM1      (1<<8)
+#define ADIS16240_DIAG_STAT_ALARM1      BIT(8)
 /* Capture buffer full: 1 = capture buffer is full */
-#define ADIS16240_DIAG_STAT_CPT_BUF_FUL (1<<7)
+#define ADIS16240_DIAG_STAT_CPT_BUF_FUL BIT(7)
 /* Flash test, checksum flag: 1 = mismatch, 0 = match */
-#define ADIS16240_DIAG_STAT_CHKSUM      (1<<6)
+#define ADIS16240_DIAG_STAT_CHKSUM      BIT(6)
 /* Power-on, self-test flag: 1 = failure, 0 = pass */
 #define ADIS16240_DIAG_STAT_PWRON_FAIL_BIT  5
 /* Power-on self-test: 1 = in-progress, 0 = complete */
-#define ADIS16240_DIAG_STAT_PWRON_BUSY  (1<<4)
+#define ADIS16240_DIAG_STAT_PWRON_BUSY  BIT(4)
 /* SPI communications failure */
 #define ADIS16240_DIAG_STAT_SPI_FAIL_BIT	3
 /* Flash update failure */
@@ -109,11 +109,11 @@
 #define ADIS16240_DIAG_STAT_POWER_LOW_BIT	0
 
 /* GLOB_CMD */
-#define ADIS16240_GLOB_CMD_RESUME	(1<<8)
-#define ADIS16240_GLOB_CMD_SW_RESET	(1<<7)
-#define ADIS16240_GLOB_CMD_STANDBY	(1<<2)
+#define ADIS16240_GLOB_CMD_RESUME	BIT(8)
+#define ADIS16240_GLOB_CMD_SW_RESET	BIT(7)
+#define ADIS16240_GLOB_CMD_STANDBY	BIT(2)
 
-#define ADIS16240_ERROR_ACTIVE          (1<<14)
+#define ADIS16240_ERROR_ACTIVE          BIT(14)
 
 /* At the moment triggers are only used for ring buffer
  * filling. This may change!
diff --git a/drivers/staging/iio/accel/adis16240_core.c b/drivers/staging/iio/accel/adis16240_core.c
index 3f46086eab3a..cb074e864408 100644
--- a/drivers/staging/iio/accel/adis16240_core.c
+++ b/drivers/staging/iio/accel/adis16240_core.c
@@ -27,9 +27,9 @@
 #include "adis16240.h"
 
 static ssize_t adis16240_spi_read_signed(struct device *dev,
-		struct device_attribute *attr,
-		char *buf,
-		unsigned bits)
+					 struct device_attribute *attr,
+					 char *buf,
+					 unsigned bits)
 {
 	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct adis *st = iio_priv(indio_dev);
@@ -39,20 +39,20 @@ static ssize_t adis16240_spi_read_signed(struct device *dev,
 	struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
 
 	ret = adis_read_reg_16(st,
-					this_attr->address, (u16 *)&val);
+			       this_attr->address, (u16 *)&val);
 	if (ret)
 		return ret;
 
 	if (val & ADIS16240_ERROR_ACTIVE)
 		adis_check_status(st);
 
-	val = ((s16)(val << shift) >> shift);
+	val = (s16)(val << shift) >> shift;
 	return sprintf(buf, "%d\n", val);
 }
 
 static ssize_t adis16240_read_12bit_signed(struct device *dev,
-		struct device_attribute *attr,
-		char *buf)
+					   struct device_attribute *attr,
+					   char *buf)
 {
 	ssize_t ret;
 	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
@@ -176,11 +176,14 @@ static const struct iio_chan_spec adis16240_channels[] = {
 	ADIS_SUPPLY_CHAN(ADIS16240_SUPPLY_OUT, ADIS16240_SCAN_SUPPLY, 0, 10),
 	ADIS_AUX_ADC_CHAN(ADIS16240_AUX_ADC, ADIS16240_SCAN_AUX_ADC, 0, 10),
 	ADIS_ACCEL_CHAN(X, ADIS16240_XACCL_OUT, ADIS16240_SCAN_ACC_X,
-		BIT(IIO_CHAN_INFO_CALIBBIAS) | BIT(IIO_CHAN_INFO_PEAK), 0, 10),
+			BIT(IIO_CHAN_INFO_CALIBBIAS) | BIT(IIO_CHAN_INFO_PEAK),
+			0, 10),
 	ADIS_ACCEL_CHAN(Y, ADIS16240_YACCL_OUT, ADIS16240_SCAN_ACC_Y,
-		BIT(IIO_CHAN_INFO_CALIBBIAS) | BIT(IIO_CHAN_INFO_PEAK), 0, 10),
+			BIT(IIO_CHAN_INFO_CALIBBIAS) | BIT(IIO_CHAN_INFO_PEAK),
+			0, 10),
 	ADIS_ACCEL_CHAN(Z, ADIS16240_ZACCL_OUT, ADIS16240_SCAN_ACC_Z,
-		BIT(IIO_CHAN_INFO_CALIBBIAS) | BIT(IIO_CHAN_INFO_PEAK), 0, 10),
+			BIT(IIO_CHAN_INFO_CALIBBIAS) | BIT(IIO_CHAN_INFO_PEAK),
+			0, 10),
 	ADIS_TEMP_CHAN(ADIS16240_TEMP_OUT, ADIS16240_SCAN_TEMP, 0, 10),
 	IIO_CHAN_SOFT_TIMESTAMP(6)
 };
diff --git a/drivers/staging/iio/accel/lis3l02dq.h b/drivers/staging/iio/accel/lis3l02dq.h
index 0a8ea8270866..3f24c629be6f 100644
--- a/drivers/staging/iio/accel/lis3l02dq.h
+++ b/drivers/staging/iio/accel/lis3l02dq.h
@@ -194,6 +194,7 @@ irqreturn_t lis3l02dq_data_rdy_trig_poll(int irq, void *private);
 static inline void lis3l02dq_remove_trigger(struct iio_dev *indio_dev)
 {
 }
+
 static inline int lis3l02dq_probe_trigger(struct iio_dev *indio_dev)
 {
 	return 0;
@@ -203,6 +204,7 @@ static int lis3l02dq_configure_buffer(struct iio_dev *indio_dev)
 {
 	return 0;
 }
+
 static inline void lis3l02dq_unconfigure_buffer(struct iio_dev *indio_dev)
 {
 }
diff --git a/drivers/staging/iio/accel/lis3l02dq_core.c b/drivers/staging/iio/accel/lis3l02dq_core.c
index b78c9c5d5588..ebcab56c81b9 100644
--- a/drivers/staging/iio/accel/lis3l02dq_core.c
+++ b/drivers/staging/iio/accel/lis3l02dq_core.c
@@ -558,13 +558,11 @@ static const struct iio_chan_spec lis3l02dq_channels[] = {
 	IIO_CHAN_SOFT_TIMESTAMP(3)
 };
 
-
 static int lis3l02dq_read_event_config(struct iio_dev *indio_dev,
 				       const struct iio_chan_spec *chan,
 				       enum iio_event_type type,
 				       enum iio_event_direction dir)
 {
-
 	u8 val;
 	int ret;
 	u8 mask = (1 << (chan->channel2*2 + (dir == IIO_EV_DIR_RISING)));
@@ -656,8 +654,8 @@ static int lis3l02dq_write_event_config(struct iio_dev *indio_dev,
 			(control | LIS3L02DQ_REG_CTRL_2_ENABLE_INTERRUPT) :
 			(control & ~LIS3L02DQ_REG_CTRL_2_ENABLE_INTERRUPT);
 		ret = lis3l02dq_spi_write_reg_8(indio_dev,
-					       LIS3L02DQ_REG_CTRL_2_ADDR,
-					       control);
+						LIS3L02DQ_REG_CTRL_2_ADDR,
+						control);
 		if (ret)
 			goto error_ret;
 	}
diff --git a/drivers/staging/iio/accel/lis3l02dq_ring.c b/drivers/staging/iio/accel/lis3l02dq_ring.c
index 1fd90090a633..b892f2cf5f9e 100644
--- a/drivers/staging/iio/accel/lis3l02dq_ring.c
+++ b/drivers/staging/iio/accel/lis3l02dq_ring.c
@@ -110,7 +110,7 @@ static int lis3l02dq_read_all(struct iio_dev *indio_dev, u8 *rx_array)
 }
 
 static int lis3l02dq_get_buffer_element(struct iio_dev *indio_dev,
-				u8 *buf)
+					u8 *buf)
 {
 	int ret, i;
 	u8 *rx_array;
@@ -118,8 +118,8 @@ static int lis3l02dq_get_buffer_element(struct iio_dev *indio_dev,
 	int scan_count = bitmap_weight(indio_dev->active_scan_mask,
 				       indio_dev->masklength);
 
-	rx_array = kzalloc(4 * scan_count, GFP_KERNEL);
-	if (rx_array == NULL)
+	rx_array = kcalloc(4, scan_count, GFP_KERNEL);
+	if (!rx_array)
 		return -ENOMEM;
 	ret = lis3l02dq_read_all(indio_dev, rx_array);
 	if (ret < 0) {
@@ -142,7 +142,7 @@ static irqreturn_t lis3l02dq_trigger_handler(int irq, void *p)
 	char *data;
 
 	data = kmalloc(indio_dev->scan_bytes, GFP_KERNEL);
-	if (data == NULL)
+	if (!data)
 		goto done;
 
 	if (!bitmap_empty(indio_dev->active_scan_mask, indio_dev->masklength))
@@ -330,19 +330,21 @@ static int lis3l02dq_buffer_postenable(struct iio_dev *indio_dev)
 	if (test_bit(0, indio_dev->active_scan_mask)) {
 		t |= LIS3L02DQ_REG_CTRL_1_AXES_X_ENABLE;
 		oneenabled = true;
-	} else
+	} else {
 		t &= ~LIS3L02DQ_REG_CTRL_1_AXES_X_ENABLE;
+	}
 	if (test_bit(1, indio_dev->active_scan_mask)) {
 		t |= LIS3L02DQ_REG_CTRL_1_AXES_Y_ENABLE;
 		oneenabled = true;
-	} else
+	} else {
 		t &= ~LIS3L02DQ_REG_CTRL_1_AXES_Y_ENABLE;
+	}
 	if (test_bit(2, indio_dev->active_scan_mask)) {
 		t |= LIS3L02DQ_REG_CTRL_1_AXES_Z_ENABLE;
 		oneenabled = true;
-	} else
+	} else {
 		t &= ~LIS3L02DQ_REG_CTRL_1_AXES_Z_ENABLE;
-
+	}
 	if (!oneenabled) /* what happens in this case is unknown */
 		return -EINVAL;
 	ret = lis3l02dq_spi_write_reg_8(indio_dev,
@@ -410,7 +412,7 @@ int lis3l02dq_configure_buffer(struct iio_dev *indio_dev)
 						 "lis3l02dq_consumer%d",
 						 indio_dev->id);
 
-	if (indio_dev->pollfunc == NULL) {
+	if (!indio_dev->pollfunc) {
 		ret = -ENOMEM;
 		goto error_iio_sw_rb_free;
 	}
diff --git a/drivers/staging/iio/accel/sca3000.h b/drivers/staging/iio/accel/sca3000.h
index b284e5a6cac1..9c8a9587df7d 100644
--- a/drivers/staging/iio/accel/sca3000.h
+++ b/drivers/staging/iio/accel/sca3000.h
@@ -38,6 +38,9 @@
  * Can probably alleviate this by reading the interrupt register on start, but
  * that is really just brushing the problem under the carpet.
  */
+#ifndef _SCA3000
+#define _SCA3000
+
 #define SCA3000_WRITE_REG(a) (((a) << 2) | 0x02)
 #define SCA3000_READ_REG(a) ((a) << 2)
 
@@ -272,4 +275,4 @@ static inline void sca3000_ring_int_process(u8 val, void *ring)
 }
 
 #endif
-
+#endif /* _SCA3000 */
diff --git a/drivers/staging/iio/accel/sca3000_core.c b/drivers/staging/iio/accel/sca3000_core.c
index 31fb2182c198..b614f272b5f4 100644
--- a/drivers/staging/iio/accel/sca3000_core.c
+++ b/drivers/staging/iio/accel/sca3000_core.c
@@ -870,7 +870,7 @@ static ssize_t sca3000_query_free_fall_mode(struct device *dev,
 					    struct device_attribute *attr,
 					    char *buf)
 {
-	int ret, len;
+	int ret;
 	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct sca3000_state *st = iio_priv(indio_dev);
 	int val;
@@ -881,9 +881,7 @@ static ssize_t sca3000_query_free_fall_mode(struct device *dev,
 	mutex_unlock(&st->lock);
 	if (ret < 0)
 		return ret;
-	len = sprintf(buf, "%d\n",
-		      !!(val & SCA3000_FREE_FALL_DETECT));
-	return len;
+	return sprintf(buf, "%d\n", !!(val & SCA3000_FREE_FALL_DETECT));
 }
 
 /**
diff --git a/drivers/staging/iio/accel/sca3000_ring.c b/drivers/staging/iio/accel/sca3000_ring.c
index f76a26885808..8589eade1057 100644
--- a/drivers/staging/iio/accel/sca3000_ring.c
+++ b/drivers/staging/iio/accel/sca3000_ring.c
@@ -129,9 +129,9 @@ error_ret:
 	return ret ? ret : num_read;
 }
 
-static bool sca3000_ring_buf_data_available(struct iio_buffer *r)
+static size_t sca3000_ring_buf_data_available(struct iio_buffer *r)
 {
-	return r->stufftoread;
+	return r->stufftoread ? r->watermark : 0;
 }
 
 /**
diff --git a/drivers/staging/iio/adc/ad7192.c b/drivers/staging/iio/adc/ad7192.c
index 6f8ce6c6574b..fe56fb6c7d30 100644
--- a/drivers/staging/iio/adc/ad7192.c
+++ b/drivers/staging/iio/adc/ad7192.c
@@ -41,32 +41,32 @@
 				   * (RW, 16-bit (AD7792)/24-bit (AD7192)) */
 
 /* Communications Register Bit Designations (AD7192_REG_COMM) */
-#define AD7192_COMM_WEN		(1 << 7) /* Write Enable */
-#define AD7192_COMM_WRITE	(0 << 6) /* Write Operation */
-#define AD7192_COMM_READ	(1 << 6) /* Read Operation */
+#define AD7192_COMM_WEN		BIT(7) /* Write Enable */
+#define AD7192_COMM_WRITE	0 /* Write Operation */
+#define AD7192_COMM_READ	BIT(6) /* Read Operation */
 #define AD7192_COMM_ADDR(x)	(((x) & 0x7) << 3) /* Register Address */
-#define AD7192_COMM_CREAD	(1 << 2) /* Continuous Read of Data Register */
+#define AD7192_COMM_CREAD	BIT(2) /* Continuous Read of Data Register */
 
 /* Status Register Bit Designations (AD7192_REG_STAT) */
-#define AD7192_STAT_RDY		(1 << 7) /* Ready */
-#define AD7192_STAT_ERR		(1 << 6) /* Error (Overrange, Underrange) */
-#define AD7192_STAT_NOREF	(1 << 5) /* Error no external reference */
-#define AD7192_STAT_PARITY	(1 << 4) /* Parity */
-#define AD7192_STAT_CH3		(1 << 2) /* Channel 3 */
-#define AD7192_STAT_CH2		(1 << 1) /* Channel 2 */
-#define AD7192_STAT_CH1		(1 << 0) /* Channel 1 */
+#define AD7192_STAT_RDY		BIT(7) /* Ready */
+#define AD7192_STAT_ERR		BIT(6) /* Error (Overrange, Underrange) */
+#define AD7192_STAT_NOREF	BIT(5) /* Error no external reference */
+#define AD7192_STAT_PARITY	BIT(4) /* Parity */
+#define AD7192_STAT_CH3		BIT(2) /* Channel 3 */
+#define AD7192_STAT_CH2		BIT(1) /* Channel 2 */
+#define AD7192_STAT_CH1		BIT(0) /* Channel 1 */
 
 /* Mode Register Bit Designations (AD7192_REG_MODE) */
 #define AD7192_MODE_SEL(x)	(((x) & 0x7) << 21) /* Operation Mode Select */
 #define AD7192_MODE_SEL_MASK	(0x7 << 21) /* Operation Mode Select Mask */
-#define AD7192_MODE_DAT_STA	(1 << 20) /* Status Register transmission */
+#define AD7192_MODE_DAT_STA	BIT(20) /* Status Register transmission */
 #define AD7192_MODE_CLKSRC(x)	(((x) & 0x3) << 18) /* Clock Source Select */
-#define AD7192_MODE_SINC3	(1 << 15) /* SINC3 Filter Select */
-#define AD7192_MODE_ACX		(1 << 14) /* AC excitation enable(AD7195 only)*/
-#define AD7192_MODE_ENPAR	(1 << 13) /* Parity Enable */
-#define AD7192_MODE_CLKDIV	(1 << 12) /* Clock divide by 2 (AD7190/2 only)*/
-#define AD7192_MODE_SCYCLE	(1 << 11) /* Single cycle conversion */
-#define AD7192_MODE_REJ60	(1 << 10) /* 50/60Hz notch filter */
+#define AD7192_MODE_SINC3	BIT(15) /* SINC3 Filter Select */
+#define AD7192_MODE_ACX		BIT(14) /* AC excitation enable(AD7195 only)*/
+#define AD7192_MODE_ENPAR	BIT(13) /* Parity Enable */
+#define AD7192_MODE_CLKDIV	BIT(12) /* Clock divide by 2 (AD7190/2 only)*/
+#define AD7192_MODE_SCYCLE	BIT(11) /* Single cycle conversion */
+#define AD7192_MODE_REJ60	BIT(10) /* 50/60Hz notch filter */
 #define AD7192_MODE_RATE(x)	((x) & 0x3FF) /* Filter Update Rate Select */
 
 /* Mode Register: AD7192_MODE_SEL options */
@@ -91,14 +91,14 @@
 
 /* Configuration Register Bit Designations (AD7192_REG_CONF) */
 
-#define AD7192_CONF_CHOP	(1 << 23) /* CHOP enable */
-#define AD7192_CONF_REFSEL	(1 << 20) /* REFIN1/REFIN2 Reference Select */
+#define AD7192_CONF_CHOP	BIT(23) /* CHOP enable */
+#define AD7192_CONF_REFSEL	BIT(20) /* REFIN1/REFIN2 Reference Select */
 #define AD7192_CONF_CHAN(x)	(((1 << (x)) & 0xFF) << 8) /* Channel select */
 #define AD7192_CONF_CHAN_MASK	(0xFF << 8) /* Channel select mask */
-#define AD7192_CONF_BURN	(1 << 7) /* Burnout current enable */
-#define AD7192_CONF_REFDET	(1 << 6) /* Reference detect enable */
-#define AD7192_CONF_BUF		(1 << 4) /* Buffered Mode Enable */
-#define AD7192_CONF_UNIPOLAR	(1 << 3) /* Unipolar/Bipolar Enable */
+#define AD7192_CONF_BURN	BIT(7) /* Burnout current enable */
+#define AD7192_CONF_REFDET	BIT(6) /* Reference detect enable */
+#define AD7192_CONF_BUF		BIT(4) /* Buffered Mode Enable */
+#define AD7192_CONF_UNIPOLAR	BIT(3) /* Unipolar/Bipolar Enable */
 #define AD7192_CONF_GAIN(x)	((x) & 0x7) /* Gain Select */
 
 #define AD7192_CH_AIN1P_AIN2M	0 /* AIN1(+) - AIN2(-) */
@@ -117,13 +117,13 @@
 #define AD7192_ID_MASK		0x0F
 
 /* GPOCON Register Bit Designations (AD7192_REG_GPOCON) */
-#define AD7192_GPOCON_BPDSW	(1 << 6) /* Bridge power-down switch enable */
-#define AD7192_GPOCON_GP32EN	(1 << 5) /* Digital Output P3 and P2 enable */
-#define AD7192_GPOCON_GP10EN	(1 << 4) /* Digital Output P1 and P0 enable */
-#define AD7192_GPOCON_P3DAT	(1 << 3) /* P3 state */
-#define AD7192_GPOCON_P2DAT	(1 << 2) /* P2 state */
-#define AD7192_GPOCON_P1DAT	(1 << 1) /* P1 state */
-#define AD7192_GPOCON_P0DAT	(1 << 0) /* P0 state */
+#define AD7192_GPOCON_BPDSW	BIT(6) /* Bridge power-down switch enable */
+#define AD7192_GPOCON_GP32EN	BIT(5) /* Digital Output P3 and P2 enable */
+#define AD7192_GPOCON_GP10EN	BIT(4) /* Digital Output P1 and P0 enable */
+#define AD7192_GPOCON_P3DAT	BIT(3) /* P3 state */
+#define AD7192_GPOCON_P2DAT	BIT(2) /* P2 state */
+#define AD7192_GPOCON_P1DAT	BIT(1) /* P1 state */
+#define AD7192_GPOCON_P0DAT	BIT(0) /* P0 state */
 
 #define AD7192_INT_FREQ_MHz	4915200
 
diff --git a/drivers/staging/iio/adc/ad7280a.c b/drivers/staging/iio/adc/ad7280a.c
index 4d4870787eed..d98e229c46bf 100644
--- a/drivers/staging/iio/adc/ad7280a.c
+++ b/drivers/staging/iio/adc/ad7280a.c
@@ -55,37 +55,37 @@
 #define AD7280A_CNVST_CONTROL		0x1D /* D7 to D0, Read/write */
 
 /* Bits and Masks */
-#define AD7280A_CTRL_HB_CONV_INPUT_ALL			(0 << 6)
-#define AD7280A_CTRL_HB_CONV_INPUT_6CELL_AUX1_3_4	(1 << 6)
-#define AD7280A_CTRL_HB_CONV_INPUT_6CELL		(2 << 6)
-#define AD7280A_CTRL_HB_CONV_INPUT_SELF_TEST		(3 << 6)
-#define AD7280A_CTRL_HB_CONV_RES_READ_ALL		(0 << 4)
-#define AD7280A_CTRL_HB_CONV_RES_READ_6CELL_AUX1_3_4	(1 << 4)
-#define AD7280A_CTRL_HB_CONV_RES_READ_6CELL		(2 << 4)
-#define AD7280A_CTRL_HB_CONV_RES_READ_NO		(3 << 4)
-#define AD7280A_CTRL_HB_CONV_START_CNVST		(0 << 3)
-#define AD7280A_CTRL_HB_CONV_START_CS			(1 << 3)
-#define AD7280A_CTRL_HB_CONV_AVG_DIS			(0 << 1)
-#define AD7280A_CTRL_HB_CONV_AVG_2			(1 << 1)
-#define AD7280A_CTRL_HB_CONV_AVG_4			(2 << 1)
-#define AD7280A_CTRL_HB_CONV_AVG_8			(3 << 1)
+#define AD7280A_CTRL_HB_CONV_INPUT_ALL			0
+#define AD7280A_CTRL_HB_CONV_INPUT_6CELL_AUX1_3_4	BIT(6)
+#define AD7280A_CTRL_HB_CONV_INPUT_6CELL		BIT(7)
+#define AD7280A_CTRL_HB_CONV_INPUT_SELF_TEST		(BIT(7) | BIT(6))
+#define AD7280A_CTRL_HB_CONV_RES_READ_ALL		0
+#define AD7280A_CTRL_HB_CONV_RES_READ_6CELL_AUX1_3_4	BIT(4)
+#define AD7280A_CTRL_HB_CONV_RES_READ_6CELL		BIT(5)
+#define AD7280A_CTRL_HB_CONV_RES_READ_NO		(BIT(5) | BIT(4))
+#define AD7280A_CTRL_HB_CONV_START_CNVST		0
+#define AD7280A_CTRL_HB_CONV_START_CS			BIT(3)
+#define AD7280A_CTRL_HB_CONV_AVG_DIS			0
+#define AD7280A_CTRL_HB_CONV_AVG_2			BIT(1)
+#define AD7280A_CTRL_HB_CONV_AVG_4			BIT(2)
+#define AD7280A_CTRL_HB_CONV_AVG_8			(BIT(2) | BIT(1))
 #define AD7280A_CTRL_HB_CONV_AVG(x)			((x) << 1)
-#define AD7280A_CTRL_HB_PWRDN_SW			(1 << 0)
+#define AD7280A_CTRL_HB_PWRDN_SW			BIT(0)
 
-#define AD7280A_CTRL_LB_SWRST				(1 << 7)
-#define AD7280A_CTRL_LB_ACQ_TIME_400ns			(0 << 5)
-#define AD7280A_CTRL_LB_ACQ_TIME_800ns			(1 << 5)
-#define AD7280A_CTRL_LB_ACQ_TIME_1200ns			(2 << 5)
-#define AD7280A_CTRL_LB_ACQ_TIME_1600ns			(3 << 5)
+#define AD7280A_CTRL_LB_SWRST				BIT(7)
+#define AD7280A_CTRL_LB_ACQ_TIME_400ns			0
+#define AD7280A_CTRL_LB_ACQ_TIME_800ns			BIT(5)
+#define AD7280A_CTRL_LB_ACQ_TIME_1200ns			BIT(6)
+#define AD7280A_CTRL_LB_ACQ_TIME_1600ns			(BIT(6) | BIT(5))
 #define AD7280A_CTRL_LB_ACQ_TIME(x)			((x) << 5)
-#define AD7280A_CTRL_LB_MUST_SET			(1 << 4)
-#define AD7280A_CTRL_LB_THERMISTOR_EN			(1 << 3)
-#define AD7280A_CTRL_LB_LOCK_DEV_ADDR			(1 << 2)
-#define AD7280A_CTRL_LB_INC_DEV_ADDR			(1 << 1)
-#define AD7280A_CTRL_LB_DAISY_CHAIN_RB_EN		(1 << 0)
+#define AD7280A_CTRL_LB_MUST_SET			BIT(4)
+#define AD7280A_CTRL_LB_THERMISTOR_EN			BIT(3)
+#define AD7280A_CTRL_LB_LOCK_DEV_ADDR			BIT(2)
+#define AD7280A_CTRL_LB_INC_DEV_ADDR			BIT(1)
+#define AD7280A_CTRL_LB_DAISY_CHAIN_RB_EN		BIT(0)
 
-#define AD7280A_ALERT_GEN_STATIC_HIGH			(1 << 6)
-#define AD7280A_ALERT_RELAY_SIG_CHAIN_DOWN		(3 << 6)
+#define AD7280A_ALERT_GEN_STATIC_HIGH			BIT(6)
+#define AD7280A_ALERT_RELAY_SIG_CHAIN_DOWN		(BIT(7) | BIT(6))
 
 #define AD7280A_ALL_CELLS				(0xAD << 16)
 
@@ -547,8 +547,9 @@ static int ad7280_attr_init(struct ad7280_state *st)
 {
 	int dev, ch, cnt;
 
-	st->iio_attr = kzalloc(sizeof(*st->iio_attr) * (st->slave_num + 1) *
-				AD7280A_CELLS_PER_DEV * 2, GFP_KERNEL);
+	st->iio_attr = kcalloc(2, sizeof(*st->iio_attr) *
+			       (st->slave_num + 1) * AD7280A_CELLS_PER_DEV,
+			       GFP_KERNEL);
 	if (st->iio_attr == NULL)
 		return -ENOMEM;
 
diff --git a/drivers/staging/iio/adc/ad7280a.h b/drivers/staging/iio/adc/ad7280a.h
index 20400b0045e5..732347a9bce4 100644
--- a/drivers/staging/iio/adc/ad7280a.h
+++ b/drivers/staging/iio/adc/ad7280a.h
@@ -23,10 +23,10 @@
 #define AD7280A_CONV_AVG_4			2
 #define AD7280A_CONV_AVG_8			3
 
-#define AD7280A_ALERT_REMOVE_VIN5		(1 << 2)
-#define AD7280A_ALERT_REMOVE_VIN4_VIN5		(2 << 2)
-#define AD7280A_ALERT_REMOVE_AUX5		(1 << 0)
-#define AD7280A_ALERT_REMOVE_AUX4_AUX5		(2 << 0)
+#define AD7280A_ALERT_REMOVE_VIN5		BIT(2)
+#define AD7280A_ALERT_REMOVE_VIN4_VIN5		BIT(3)
+#define AD7280A_ALERT_REMOVE_AUX5		BIT(0)
+#define AD7280A_ALERT_REMOVE_AUX4_AUX5		BIT(1)
 
 struct ad7280_platform_data {
 	unsigned acquisition_time;
diff --git a/drivers/staging/iio/adc/ad7606_ring.c b/drivers/staging/iio/adc/ad7606_ring.c
index 3bf174cb19b1..a6f8eb11242c 100644
--- a/drivers/staging/iio/adc/ad7606_ring.c
+++ b/drivers/staging/iio/adc/ad7606_ring.c
@@ -50,7 +50,7 @@ static void ad7606_poll_bh_to_ring(struct work_struct *work_s)
 	int ret;
 
 	buf = kzalloc(indio_dev->scan_bytes, GFP_KERNEL);
-	if (buf == NULL)
+	if (!buf)
 		return;
 
 	if (gpio_is_valid(st->pdata->gpio_frstdata)) {
diff --git a/drivers/staging/iio/adc/ad7780.c b/drivers/staging/iio/adc/ad7780.c
index 273add3ed63f..9f03fe3ee3d9 100644
--- a/drivers/staging/iio/adc/ad7780.c
+++ b/drivers/staging/iio/adc/ad7780.c
@@ -24,14 +24,14 @@
 
 #include "ad7780.h"
 
-#define AD7780_RDY	(1 << 7)
-#define AD7780_FILTER	(1 << 6)
-#define AD7780_ERR	(1 << 5)
-#define AD7780_ID1	(1 << 4)
-#define AD7780_ID0	(1 << 3)
-#define AD7780_GAIN	(1 << 2)
-#define AD7780_PAT1	(1 << 1)
-#define AD7780_PAT0	(1 << 0)
+#define AD7780_RDY	BIT(7)
+#define AD7780_FILTER	BIT(6)
+#define AD7780_ERR	BIT(5)
+#define AD7780_ID1	BIT(4)
+#define AD7780_ID0	BIT(3)
+#define AD7780_GAIN	BIT(2)
+#define AD7780_PAT1	BIT(1)
+#define AD7780_PAT0	BIT(0)
 
 struct ad7780_chip_info {
 	struct iio_chan_spec	channel;
@@ -169,7 +169,7 @@ static int ad7780_probe(struct spi_device *spi)
 	int ret, voltage_uv = 0;
 
 	indio_dev = devm_iio_device_alloc(&spi->dev, sizeof(*st));
-	if (indio_dev == NULL)
+	if (!indio_dev)
 		return -ENOMEM;
 
 	st = iio_priv(indio_dev);
diff --git a/drivers/staging/iio/adc/mxs-lradc.c b/drivers/staging/iio/adc/mxs-lradc.c
index d9d6fad7cb00..d7c5223f1c3e 100644
--- a/drivers/staging/iio/adc/mxs-lradc.c
+++ b/drivers/staging/iio/adc/mxs-lradc.c
@@ -214,11 +214,17 @@ struct mxs_lradc {
 	unsigned long		is_divided;
 
 	/*
-	 * Touchscreen LRADC channels receives a private slot in the CTRL4
-	 * register, the slot #7. Therefore only 7 slots instead of 8 in the
-	 * CTRL4 register can be mapped to LRADC channels when using the
-	 * touchscreen.
-	 *
+	 * When the touchscreen is enabled, we give it two private virtual
+	 * channels: #6 and #7. This means that only 6 virtual channels (instead
+	 * of 8) will be available for buffered capture.
+	 */
+#define TOUCHSCREEN_VCHANNEL1		7
+#define TOUCHSCREEN_VCHANNEL2		6
+#define BUFFER_VCHANS_LIMITED		0x3f
+#define BUFFER_VCHANS_ALL		0xff
+	u8			buffer_vchans;
+
+	/*
 	 * Furthermore, certain LRADC channels are shared between touchscreen
 	 * and/or touch-buttons and generic LRADC block. Therefore when using
 	 * either of these, these channels are not available for the regular
@@ -237,7 +243,7 @@ struct mxs_lradc {
 	 * be sampled as regular LRADC channels. The driver will refuse any
 	 * attempt to sample these channels.
 	 */
-#define CHAN_MASK_TOUCHBUTTON		(0x3 << 0)
+#define CHAN_MASK_TOUCHBUTTON		(BIT(1) | BIT(0))
 #define CHAN_MASK_TOUCHSCREEN_4WIRE	(0xf << 2)
 #define CHAN_MASK_TOUCHSCREEN_5WIRE	(0x1f << 2)
 	enum mxs_lradc_ts	use_touchscreen;
@@ -262,20 +268,20 @@ struct mxs_lradc {
 };
 
 #define	LRADC_CTRL0				0x00
-# define LRADC_CTRL0_MX28_TOUCH_DETECT_ENABLE	(1 << 23)
-# define LRADC_CTRL0_MX28_TOUCH_SCREEN_TYPE	(1 << 22)
-# define LRADC_CTRL0_MX28_YNNSW	/* YM */	(1 << 21)
-# define LRADC_CTRL0_MX28_YPNSW	/* YP */	(1 << 20)
-# define LRADC_CTRL0_MX28_YPPSW	/* YP */	(1 << 19)
-# define LRADC_CTRL0_MX28_XNNSW	/* XM */	(1 << 18)
-# define LRADC_CTRL0_MX28_XNPSW	/* XM */	(1 << 17)
-# define LRADC_CTRL0_MX28_XPPSW	/* XP */	(1 << 16)
-
-# define LRADC_CTRL0_MX23_TOUCH_DETECT_ENABLE	(1 << 20)
-# define LRADC_CTRL0_MX23_YM			(1 << 19)
-# define LRADC_CTRL0_MX23_XM			(1 << 18)
-# define LRADC_CTRL0_MX23_YP			(1 << 17)
-# define LRADC_CTRL0_MX23_XP			(1 << 16)
+# define LRADC_CTRL0_MX28_TOUCH_DETECT_ENABLE	BIT(23)
+# define LRADC_CTRL0_MX28_TOUCH_SCREEN_TYPE	BIT(22)
+# define LRADC_CTRL0_MX28_YNNSW	/* YM */	BIT(21)
+# define LRADC_CTRL0_MX28_YPNSW	/* YP */	BIT(20)
+# define LRADC_CTRL0_MX28_YPPSW	/* YP */	BIT(19)
+# define LRADC_CTRL0_MX28_XNNSW	/* XM */	BIT(18)
+# define LRADC_CTRL0_MX28_XNPSW	/* XM */	BIT(17)
+# define LRADC_CTRL0_MX28_XPPSW	/* XP */	BIT(16)
+
+# define LRADC_CTRL0_MX23_TOUCH_DETECT_ENABLE	BIT(20)
+# define LRADC_CTRL0_MX23_YM			BIT(19)
+# define LRADC_CTRL0_MX23_XM			BIT(18)
+# define LRADC_CTRL0_MX23_YP			BIT(17)
+# define LRADC_CTRL0_MX23_XP			BIT(16)
 
 # define LRADC_CTRL0_MX28_PLATE_MASK \
 		(LRADC_CTRL0_MX28_TOUCH_DETECT_ENABLE | \
@@ -289,12 +295,12 @@ struct mxs_lradc {
 		LRADC_CTRL0_MX23_YP | LRADC_CTRL0_MX23_XP)
 
 #define	LRADC_CTRL1				0x10
-#define	LRADC_CTRL1_TOUCH_DETECT_IRQ_EN		(1 << 24)
+#define	LRADC_CTRL1_TOUCH_DETECT_IRQ_EN		BIT(24)
 #define	LRADC_CTRL1_LRADC_IRQ_EN(n)		(1 << ((n) + 16))
 #define	LRADC_CTRL1_MX28_LRADC_IRQ_EN_MASK	(0x1fff << 16)
 #define	LRADC_CTRL1_MX23_LRADC_IRQ_EN_MASK	(0x01ff << 16)
 #define	LRADC_CTRL1_LRADC_IRQ_EN_OFFSET		16
-#define	LRADC_CTRL1_TOUCH_DETECT_IRQ		(1 << 8)
+#define	LRADC_CTRL1_TOUCH_DETECT_IRQ		BIT(8)
 #define	LRADC_CTRL1_LRADC_IRQ(n)		(1 << (n))
 #define	LRADC_CTRL1_MX28_LRADC_IRQ_MASK		0x1fff
 #define	LRADC_CTRL1_MX23_LRADC_IRQ_MASK		0x01ff
@@ -302,13 +308,13 @@ struct mxs_lradc {
 
 #define	LRADC_CTRL2				0x20
 #define	LRADC_CTRL2_DIVIDE_BY_TWO_OFFSET	24
-#define	LRADC_CTRL2_TEMPSENSE_PWD		(1 << 15)
+#define	LRADC_CTRL2_TEMPSENSE_PWD		BIT(15)
 
 #define	LRADC_STATUS				0x40
-#define	LRADC_STATUS_TOUCH_DETECT_RAW		(1 << 0)
+#define	LRADC_STATUS_TOUCH_DETECT_RAW		BIT(0)
 
 #define	LRADC_CH(n)				(0x50 + (0x10 * (n)))
-#define	LRADC_CH_ACCUMULATE			(1 << 29)
+#define	LRADC_CH_ACCUMULATE			BIT(29)
 #define	LRADC_CH_NUM_SAMPLES_MASK		(0x1f << 24)
 #define	LRADC_CH_NUM_SAMPLES_OFFSET		24
 #define	LRADC_CH_NUM_SAMPLES(x) \
@@ -342,6 +348,9 @@ struct mxs_lradc {
 #define	LRADC_CTRL4				0x140
 #define	LRADC_CTRL4_LRADCSELECT_MASK(n)		(0xf << ((n) * 4))
 #define	LRADC_CTRL4_LRADCSELECT_OFFSET(n)	((n) * 4)
+#define	LRADC_CTRL4_LRADCSELECT(n, x) \
+				(((x) << LRADC_CTRL4_LRADCSELECT_OFFSET(n)) & \
+				LRADC_CTRL4_LRADCSELECT_MASK(n))
 
 #define LRADC_RESOLUTION			12
 #define LRADC_SINGLE_SAMPLE_MASK		((1 << LRADC_RESOLUTION) - 1)
@@ -416,6 +425,14 @@ static bool mxs_lradc_check_touch_event(struct mxs_lradc *lradc)
 					LRADC_STATUS_TOUCH_DETECT_RAW);
 }
 
+static void mxs_lradc_map_channel(struct mxs_lradc *lradc, unsigned vch,
+				  unsigned ch)
+{
+	mxs_lradc_reg_clear(lradc, LRADC_CTRL4_LRADCSELECT_MASK(vch),
+				LRADC_CTRL4);
+	mxs_lradc_reg_set(lradc, LRADC_CTRL4_LRADCSELECT(vch, ch), LRADC_CTRL4);
+}
+
 static void mxs_lradc_setup_ts_channel(struct mxs_lradc *lradc, unsigned ch)
 {
 	/*
@@ -450,12 +467,8 @@ static void mxs_lradc_setup_ts_channel(struct mxs_lradc *lradc, unsigned ch)
 		LRADC_DELAY_DELAY(lradc->over_sample_delay - 1),
 			LRADC_DELAY(3));
 
-	mxs_lradc_reg_clear(lradc, LRADC_CTRL1_LRADC_IRQ(2) |
-			LRADC_CTRL1_LRADC_IRQ(3) | LRADC_CTRL1_LRADC_IRQ(4) |
-			LRADC_CTRL1_LRADC_IRQ(5), LRADC_CTRL1);
+	mxs_lradc_reg_clear(lradc, LRADC_CTRL1_LRADC_IRQ(ch), LRADC_CTRL1);
 
-	/* wake us again, when the complete conversion is done */
-	mxs_lradc_reg_set(lradc, LRADC_CTRL1_LRADC_IRQ_EN(ch), LRADC_CTRL1);
 	/*
 	 * after changing the touchscreen plates setting
 	 * the signals need some initial time to settle. Start the
@@ -464,7 +477,7 @@ static void mxs_lradc_setup_ts_channel(struct mxs_lradc *lradc, unsigned ch)
 	 */
 	mxs_lradc_reg_wrt(lradc,
 		LRADC_DELAY_TRIGGER(0) | /* don't trigger ADC */
-		LRADC_DELAY_TRIGGER_DELAYS(1 << 3) | /* trigger DELAY unit#3 */
+		LRADC_DELAY_TRIGGER_DELAYS(BIT(3)) | /* trigger DELAY unit#3 */
 		LRADC_DELAY_KICK |
 		LRADC_DELAY_DELAY(lradc->settling_delay),
 			LRADC_DELAY(2));
@@ -509,12 +522,8 @@ static void mxs_lradc_setup_ts_pressure(struct mxs_lradc *lradc, unsigned ch1,
 		LRADC_DELAY_DELAY(lradc->over_sample_delay - 1),
 					LRADC_DELAY(3));
 
-	mxs_lradc_reg_clear(lradc, LRADC_CTRL1_LRADC_IRQ(2) |
-			LRADC_CTRL1_LRADC_IRQ(3) | LRADC_CTRL1_LRADC_IRQ(4) |
-			LRADC_CTRL1_LRADC_IRQ(5), LRADC_CTRL1);
+	mxs_lradc_reg_clear(lradc, LRADC_CTRL1_LRADC_IRQ(ch2), LRADC_CTRL1);
 
-	/* wake us again, when the conversions are done */
-	mxs_lradc_reg_set(lradc, LRADC_CTRL1_LRADC_IRQ_EN(ch2), LRADC_CTRL1);
 	/*
 	 * after changing the touchscreen plates setting
 	 * the signals need some initial time to settle. Start the
@@ -523,7 +532,7 @@ static void mxs_lradc_setup_ts_pressure(struct mxs_lradc *lradc, unsigned ch1,
 	 */
 	mxs_lradc_reg_wrt(lradc,
 		LRADC_DELAY_TRIGGER(0) | /* don't trigger ADC */
-		LRADC_DELAY_TRIGGER_DELAYS(1 << 3) | /* trigger DELAY unit#3 */
+		LRADC_DELAY_TRIGGER_DELAYS(BIT(3)) | /* trigger DELAY unit#3 */
 		LRADC_DELAY_KICK |
 		LRADC_DELAY_DELAY(lradc->settling_delay), LRADC_DELAY(2));
 }
@@ -580,36 +589,6 @@ static unsigned mxs_lradc_read_ts_pressure(struct mxs_lradc *lradc,
 #define TS_CH_XM 4
 #define TS_CH_YM 5
 
-static int mxs_lradc_read_ts_channel(struct mxs_lradc *lradc)
-{
-	u32 reg;
-	int val;
-
-	reg = readl(lradc->base + LRADC_CTRL1);
-
-	/* only channels 3 to 5 are of interest here */
-	if (reg & LRADC_CTRL1_LRADC_IRQ(TS_CH_YP)) {
-		mxs_lradc_reg_clear(lradc, LRADC_CTRL1_LRADC_IRQ_EN(TS_CH_YP) |
-			LRADC_CTRL1_LRADC_IRQ(TS_CH_YP), LRADC_CTRL1);
-		val = mxs_lradc_read_raw_channel(lradc, TS_CH_YP);
-	} else if (reg & LRADC_CTRL1_LRADC_IRQ(TS_CH_XM)) {
-		mxs_lradc_reg_clear(lradc, LRADC_CTRL1_LRADC_IRQ_EN(TS_CH_XM) |
-			LRADC_CTRL1_LRADC_IRQ(TS_CH_XM), LRADC_CTRL1);
-		val = mxs_lradc_read_raw_channel(lradc, TS_CH_XM);
-	} else if (reg & LRADC_CTRL1_LRADC_IRQ(TS_CH_YM)) {
-		mxs_lradc_reg_clear(lradc, LRADC_CTRL1_LRADC_IRQ_EN(TS_CH_YM) |
-			LRADC_CTRL1_LRADC_IRQ(TS_CH_YM), LRADC_CTRL1);
-		val = mxs_lradc_read_raw_channel(lradc, TS_CH_YM);
-	} else {
-		return -EIO;
-	}
-
-	mxs_lradc_reg_wrt(lradc, 0, LRADC_DELAY(2));
-	mxs_lradc_reg_wrt(lradc, 0, LRADC_DELAY(3));
-
-	return val;
-}
-
 /*
  * YP(open)--+-------------+
  *           |             |--+
@@ -653,7 +632,8 @@ static void mxs_lradc_prepare_x_pos(struct mxs_lradc *lradc)
 	mxs_lradc_reg_set(lradc, mxs_lradc_drive_x_plate(lradc), LRADC_CTRL0);
 
 	lradc->cur_plate = LRADC_SAMPLE_X;
-	mxs_lradc_setup_ts_channel(lradc, TS_CH_YP);
+	mxs_lradc_map_channel(lradc, TOUCHSCREEN_VCHANNEL1, TS_CH_YP);
+	mxs_lradc_setup_ts_channel(lradc, TOUCHSCREEN_VCHANNEL1);
 }
 
 /*
@@ -674,7 +654,8 @@ static void mxs_lradc_prepare_y_pos(struct mxs_lradc *lradc)
 	mxs_lradc_reg_set(lradc, mxs_lradc_drive_y_plate(lradc), LRADC_CTRL0);
 
 	lradc->cur_plate = LRADC_SAMPLE_Y;
-	mxs_lradc_setup_ts_channel(lradc, TS_CH_XM);
+	mxs_lradc_map_channel(lradc, TOUCHSCREEN_VCHANNEL1, TS_CH_XM);
+	mxs_lradc_setup_ts_channel(lradc, TOUCHSCREEN_VCHANNEL1);
 }
 
 /*
@@ -695,7 +676,10 @@ static void mxs_lradc_prepare_pressure(struct mxs_lradc *lradc)
 	mxs_lradc_reg_set(lradc, mxs_lradc_drive_pressure(lradc), LRADC_CTRL0);
 
 	lradc->cur_plate = LRADC_SAMPLE_PRESSURE;
-	mxs_lradc_setup_ts_pressure(lradc, TS_CH_XP, TS_CH_YM);
+	mxs_lradc_map_channel(lradc, TOUCHSCREEN_VCHANNEL1, TS_CH_YM);
+	mxs_lradc_map_channel(lradc, TOUCHSCREEN_VCHANNEL2, TS_CH_XP);
+	mxs_lradc_setup_ts_pressure(lradc, TOUCHSCREEN_VCHANNEL2,
+						TOUCHSCREEN_VCHANNEL1);
 }
 
 static void mxs_lradc_enable_touch_detection(struct mxs_lradc *lradc)
@@ -708,6 +692,19 @@ static void mxs_lradc_enable_touch_detection(struct mxs_lradc *lradc)
 	mxs_lradc_reg_set(lradc, LRADC_CTRL1_TOUCH_DETECT_IRQ_EN, LRADC_CTRL1);
 }
 
+static void mxs_lradc_start_touch_event(struct mxs_lradc *lradc)
+{
+	mxs_lradc_reg_clear(lradc, LRADC_CTRL1_TOUCH_DETECT_IRQ_EN,
+				LRADC_CTRL1);
+	mxs_lradc_reg_set(lradc,
+		LRADC_CTRL1_LRADC_IRQ_EN(TOUCHSCREEN_VCHANNEL1), LRADC_CTRL1);
+	/*
+	 * start with the Y-pos, because it uses nearly the same plate
+	 * settings like the touch detection
+	 */
+	mxs_lradc_prepare_y_pos(lradc);
+}
+
 static void mxs_lradc_report_ts_event(struct mxs_lradc *lradc)
 {
 	input_report_abs(lradc->ts_input, ABS_X, lradc->ts_x_pos);
@@ -725,10 +722,12 @@ static void mxs_lradc_complete_touch_event(struct mxs_lradc *lradc)
 	 * start a dummy conversion to burn time to settle the signals
 	 * note: we are not interested in the conversion's value
 	 */
-	mxs_lradc_reg_wrt(lradc, 0, LRADC_CH(5));
-	mxs_lradc_reg_clear(lradc, LRADC_CTRL1_LRADC_IRQ(5), LRADC_CTRL1);
-	mxs_lradc_reg_set(lradc, LRADC_CTRL1_LRADC_IRQ_EN(5), LRADC_CTRL1);
-	mxs_lradc_reg_wrt(lradc, LRADC_DELAY_TRIGGER(1 << 5) |
+	mxs_lradc_reg_wrt(lradc, 0, LRADC_CH(TOUCHSCREEN_VCHANNEL1));
+	mxs_lradc_reg_clear(lradc,
+		LRADC_CTRL1_LRADC_IRQ(TOUCHSCREEN_VCHANNEL1) |
+		LRADC_CTRL1_LRADC_IRQ(TOUCHSCREEN_VCHANNEL2), LRADC_CTRL1);
+	mxs_lradc_reg_wrt(lradc,
+		LRADC_DELAY_TRIGGER(1 << TOUCHSCREEN_VCHANNEL1) |
 		LRADC_DELAY_KICK | LRADC_DELAY_DELAY(10), /* waste 5 ms */
 			LRADC_DELAY(2));
 }
@@ -760,59 +759,45 @@ static void mxs_lradc_finish_touch_event(struct mxs_lradc *lradc, bool valid)
 
 	/* if it is released, wait for the next touch via IRQ */
 	lradc->cur_plate = LRADC_TOUCH;
-	mxs_lradc_reg_clear(lradc, LRADC_CTRL1_TOUCH_DETECT_IRQ, LRADC_CTRL1);
+	mxs_lradc_reg_wrt(lradc, 0, LRADC_DELAY(2));
+	mxs_lradc_reg_wrt(lradc, 0, LRADC_DELAY(3));
+	mxs_lradc_reg_clear(lradc, LRADC_CTRL1_TOUCH_DETECT_IRQ |
+		LRADC_CTRL1_LRADC_IRQ_EN(TOUCHSCREEN_VCHANNEL1) |
+		LRADC_CTRL1_LRADC_IRQ(TOUCHSCREEN_VCHANNEL1), LRADC_CTRL1);
 	mxs_lradc_reg_set(lradc, LRADC_CTRL1_TOUCH_DETECT_IRQ_EN, LRADC_CTRL1);
 }
 
 /* touchscreen's state machine */
 static void mxs_lradc_handle_touch(struct mxs_lradc *lradc)
 {
-	int val;
-
 	switch (lradc->cur_plate) {
 	case LRADC_TOUCH:
-		/*
-		 * start with the Y-pos, because it uses nearly the same plate
-		 * settings like the touch detection
-		 */
-		if (mxs_lradc_check_touch_event(lradc)) {
-			mxs_lradc_reg_clear(lradc,
-					LRADC_CTRL1_TOUCH_DETECT_IRQ_EN,
-					LRADC_CTRL1);
-			mxs_lradc_prepare_y_pos(lradc);
-		}
+		if (mxs_lradc_check_touch_event(lradc))
+			mxs_lradc_start_touch_event(lradc);
 		mxs_lradc_reg_clear(lradc, LRADC_CTRL1_TOUCH_DETECT_IRQ,
 					LRADC_CTRL1);
 		return;
 
 	case LRADC_SAMPLE_Y:
-		val = mxs_lradc_read_ts_channel(lradc);
-		if (val < 0) {
-			mxs_lradc_enable_touch_detection(lradc); /* re-start */
-			return;
-		}
-		lradc->ts_y_pos = val;
+		lradc->ts_y_pos = mxs_lradc_read_raw_channel(lradc,
+							TOUCHSCREEN_VCHANNEL1);
 		mxs_lradc_prepare_x_pos(lradc);
 		return;
 
 	case LRADC_SAMPLE_X:
-		val = mxs_lradc_read_ts_channel(lradc);
-		if (val < 0) {
-			mxs_lradc_enable_touch_detection(lradc); /* re-start */
-			return;
-		}
-		lradc->ts_x_pos = val;
+		lradc->ts_x_pos = mxs_lradc_read_raw_channel(lradc,
+							TOUCHSCREEN_VCHANNEL1);
 		mxs_lradc_prepare_pressure(lradc);
 		return;
 
 	case LRADC_SAMPLE_PRESSURE:
-		lradc->ts_pressure =
-			mxs_lradc_read_ts_pressure(lradc, TS_CH_XP, TS_CH_YM);
+		lradc->ts_pressure = mxs_lradc_read_ts_pressure(lradc,
+							TOUCHSCREEN_VCHANNEL2,
+							TOUCHSCREEN_VCHANNEL1);
 		mxs_lradc_complete_touch_event(lradc);
 		return;
 
 	case LRADC_SAMPLE_VALID:
-		val = mxs_lradc_read_ts_channel(lradc); /* ignore the value */
 		mxs_lradc_finish_touch_event(lradc, 1);
 		break;
 	}
@@ -844,9 +829,9 @@ static int mxs_lradc_read_single(struct iio_dev *iio_dev, int chan, int *val)
 	 * used if doing raw sampling.
 	 */
 	if (lradc->soc == IMX28_LRADC)
-		mxs_lradc_reg_clear(lradc, LRADC_CTRL1_MX28_LRADC_IRQ_EN_MASK,
+		mxs_lradc_reg_clear(lradc, LRADC_CTRL1_LRADC_IRQ_EN(0),
 			LRADC_CTRL1);
-	mxs_lradc_reg_clear(lradc, 0xff, LRADC_CTRL0);
+	mxs_lradc_reg_clear(lradc, 0x1, LRADC_CTRL0);
 
 	/* Enable / disable the divider per requirement */
 	if (test_bit(chan, &lradc->is_divided))
@@ -865,7 +850,7 @@ static int mxs_lradc_read_single(struct iio_dev *iio_dev, int chan, int *val)
 
 	/* Enable the IRQ and start sampling the channel. */
 	mxs_lradc_reg_set(lradc, LRADC_CTRL1_LRADC_IRQ_EN(0), LRADC_CTRL1);
-	mxs_lradc_reg_set(lradc, 1 << 0, LRADC_CTRL0);
+	mxs_lradc_reg_set(lradc, BIT(0), LRADC_CTRL0);
 
 	/* Wait for completion on the channel, 1 second max. */
 	ret = wait_for_completion_killable_timeout(&lradc->completion, HZ);
@@ -1008,7 +993,7 @@ static ssize_t mxs_lradc_show_scale_available_ch(struct device *dev,
 	int i, len = 0;
 
 	for (i = 0; i < ARRAY_SIZE(lradc->scale_avail[ch]); i++)
-		len += sprintf(buf + len, "%d.%09u ",
+		len += sprintf(buf + len, "%u.%09u ",
 			       lradc->scale_avail[ch][i].integer,
 			       lradc->scale_avail[ch][i].nano);
 
@@ -1090,9 +1075,8 @@ static void mxs_lradc_disable_ts(struct mxs_lradc *lradc)
 {
 	/* stop all interrupts from firing */
 	mxs_lradc_reg_clear(lradc, LRADC_CTRL1_TOUCH_DETECT_IRQ_EN |
-		LRADC_CTRL1_LRADC_IRQ_EN(2) | LRADC_CTRL1_LRADC_IRQ_EN(3) |
-		LRADC_CTRL1_LRADC_IRQ_EN(4) | LRADC_CTRL1_LRADC_IRQ_EN(5),
-		LRADC_CTRL1);
+		LRADC_CTRL1_LRADC_IRQ_EN(TOUCHSCREEN_VCHANNEL1) |
+		LRADC_CTRL1_LRADC_IRQ_EN(TOUCHSCREEN_VCHANNEL2), LRADC_CTRL1);
 
 	/* Power-down touchscreen touch-detect circuitry. */
 	mxs_lradc_reg_clear(lradc, mxs_lradc_plate_mask(lradc), LRADC_CTRL0);
@@ -1158,26 +1142,31 @@ static irqreturn_t mxs_lradc_handle_irq(int irq, void *data)
 	struct iio_dev *iio = data;
 	struct mxs_lradc *lradc = iio_priv(iio);
 	unsigned long reg = readl(lradc->base + LRADC_CTRL1);
+	uint32_t clr_irq = mxs_lradc_irq_mask(lradc);
 	const uint32_t ts_irq_mask =
 		LRADC_CTRL1_TOUCH_DETECT_IRQ |
-		LRADC_CTRL1_LRADC_IRQ(2) |
-		LRADC_CTRL1_LRADC_IRQ(3) |
-		LRADC_CTRL1_LRADC_IRQ(4) |
-		LRADC_CTRL1_LRADC_IRQ(5);
+		LRADC_CTRL1_LRADC_IRQ(TOUCHSCREEN_VCHANNEL1) |
+		LRADC_CTRL1_LRADC_IRQ(TOUCHSCREEN_VCHANNEL2);
 
 	if (!(reg & mxs_lradc_irq_mask(lradc)))
 		return IRQ_NONE;
 
-	if (lradc->use_touchscreen && (reg & ts_irq_mask))
+	if (lradc->use_touchscreen && (reg & ts_irq_mask)) {
 		mxs_lradc_handle_touch(lradc);
 
-	if (iio_buffer_enabled(iio))
-		iio_trigger_poll(iio->trig);
-	else if (reg & LRADC_CTRL1_LRADC_IRQ(0))
+		/* Make sure we don't clear the next conversion's interrupt. */
+		clr_irq &= ~(LRADC_CTRL1_LRADC_IRQ(TOUCHSCREEN_VCHANNEL1) |
+				LRADC_CTRL1_LRADC_IRQ(TOUCHSCREEN_VCHANNEL2));
+	}
+
+	if (iio_buffer_enabled(iio)) {
+		if (reg & lradc->buffer_vchans)
+			iio_trigger_poll(iio->trig);
+	} else if (reg & LRADC_CTRL1_LRADC_IRQ(0)) {
 		complete(&lradc->completion);
+	}
 
-	mxs_lradc_reg_clear(lradc, reg & mxs_lradc_irq_mask(lradc),
-			LRADC_CTRL1);
+	mxs_lradc_reg_clear(lradc, reg & clr_irq, LRADC_CTRL1);
 
 	return IRQ_HANDLED;
 }
@@ -1289,9 +1278,10 @@ static int mxs_lradc_buffer_preenable(struct iio_dev *iio)
 	}
 
 	if (lradc->soc == IMX28_LRADC)
-		mxs_lradc_reg_clear(lradc, LRADC_CTRL1_MX28_LRADC_IRQ_EN_MASK,
-							LRADC_CTRL1);
-	mxs_lradc_reg_clear(lradc, 0xff, LRADC_CTRL0);
+		mxs_lradc_reg_clear(lradc,
+			lradc->buffer_vchans << LRADC_CTRL1_LRADC_IRQ_EN_OFFSET,
+			LRADC_CTRL1);
+	mxs_lradc_reg_clear(lradc, lradc->buffer_vchans, LRADC_CTRL0);
 
 	for_each_set_bit(chan, iio->active_scan_mask, LRADC_MAX_TOTAL_CHANS) {
 		ctrl4_set |= chan << LRADC_CTRL4_LRADCSELECT_OFFSET(ofs);
@@ -1324,10 +1314,11 @@ static int mxs_lradc_buffer_postdisable(struct iio_dev *iio)
 	mxs_lradc_reg_clear(lradc, LRADC_DELAY_TRIGGER_LRADCS_MASK |
 					LRADC_DELAY_KICK, LRADC_DELAY(0));
 
-	mxs_lradc_reg_clear(lradc, 0xff, LRADC_CTRL0);
+	mxs_lradc_reg_clear(lradc, lradc->buffer_vchans, LRADC_CTRL0);
 	if (lradc->soc == IMX28_LRADC)
-		mxs_lradc_reg_clear(lradc, LRADC_CTRL1_MX28_LRADC_IRQ_EN_MASK,
-					LRADC_CTRL1);
+		mxs_lradc_reg_clear(lradc,
+			lradc->buffer_vchans << LRADC_CTRL1_LRADC_IRQ_EN_OFFSET,
+			LRADC_CTRL1);
 
 	kfree(lradc->buffer);
 	mutex_unlock(&lradc->lock);
@@ -1353,7 +1344,7 @@ static bool mxs_lradc_validate_scan_mask(struct iio_dev *iio,
 	if (lradc->use_touchbutton)
 		rsvd_chans++;
 	if (lradc->use_touchscreen)
-		rsvd_chans++;
+		rsvd_chans += 2;
 
 	/* Test for attempts to map channels with special mode of operation. */
 	if (bitmap_intersects(mask, &rsvd_mask, LRADC_MAX_TOTAL_CHANS))
@@ -1413,6 +1404,13 @@ static const struct iio_chan_spec mxs_lradc_chan_spec[] = {
 		.channel = 8,
 		.scan_type = {.sign = 'u', .realbits = 18, .storagebits = 32,},
 	},
+	/* Hidden channel to keep indexes */
+	{
+		.type = IIO_TEMP,
+		.indexed = 1,
+		.scan_index = -1,
+		.channel = 9,
+	},
 	MXS_ADC_CHAN(10, IIO_VOLTAGE),	/* VDDIO */
 	MXS_ADC_CHAN(11, IIO_VOLTAGE),	/* VTH */
 	MXS_ADC_CHAN(12, IIO_VOLTAGE),	/* VDDA */
@@ -1583,6 +1581,11 @@ static int mxs_lradc_probe(struct platform_device *pdev)
 
 	touch_ret = mxs_lradc_probe_touchscreen(lradc, node);
 
+	if (touch_ret == 0)
+		lradc->buffer_vchans = BUFFER_VCHANS_LIMITED;
+	else
+		lradc->buffer_vchans = BUFFER_VCHANS_ALL;
+
 	/* Grab all IRQ sources */
 	for (i = 0; i < of_cfg->irq_count; i++) {
 		lradc->irq[i] = platform_get_irq(pdev, i);
diff --git a/drivers/staging/iio/adc/spear_adc.c b/drivers/staging/iio/adc/spear_adc.c
index 8ad71691fc09..c5382374ddf6 100644
--- a/drivers/staging/iio/adc/spear_adc.c
+++ b/drivers/staging/iio/adc/spear_adc.c
@@ -29,11 +29,11 @@
 #define SPEAR_ADC_CLK_HIGH(x)		(((x) & 0xf) << 4)
 
 /* Bit definitions for SPEAR_ADC_STATUS */
-#define SPEAR_ADC_STATUS_START_CONVERSION	(1 << 0)
+#define SPEAR_ADC_STATUS_START_CONVERSION	BIT(0)
 #define SPEAR_ADC_STATUS_CHANNEL_NUM(x)		((x) << 1)
-#define SPEAR_ADC_STATUS_ADC_ENABLE		(1 << 4)
+#define SPEAR_ADC_STATUS_ADC_ENABLE		BIT(4)
 #define SPEAR_ADC_STATUS_AVG_SAMPLE(x)		((x) << 5)
-#define SPEAR_ADC_STATUS_VREF_INTERNAL		(1 << 9)
+#define SPEAR_ADC_STATUS_VREF_INTERNAL		BIT(9)
 
 #define SPEAR_ADC_DATA_MASK		0x03ff
 #define SPEAR_ADC_DATA_BITS		10
diff --git a/drivers/staging/iio/frequency/ad9832.c b/drivers/staging/iio/frequency/ad9832.c
index cf68159a5848..a861fe0149b1 100644
--- a/drivers/staging/iio/frequency/ad9832.c
+++ b/drivers/staging/iio/frequency/ad9832.c
@@ -24,8 +24,8 @@
 
 static unsigned long ad9832_calc_freqreg(unsigned long mclk, unsigned long fout)
 {
-	unsigned long long freqreg = (u64) fout *
-				     (u64) ((u64) 1L << AD9832_FREQ_BITS);
+	unsigned long long freqreg = (u64)fout *
+				     (u64)((u64)1L << AD9832_FREQ_BITS);
 	do_div(freqreg, mclk);
 	return freqreg;
 }
@@ -59,7 +59,7 @@ static int ad9832_write_frequency(struct ad9832_state *st,
 static int ad9832_write_phase(struct ad9832_state *st,
 			      unsigned long addr, unsigned long phase)
 {
-	if (phase > (1 << AD9832_PHASE_BITS))
+	if (phase > BIT(AD9832_PHASE_BITS))
 		return -EINVAL;
 
 	st->phase_data[0] = cpu_to_be16((AD9832_CMD_PHA8BITSW << CMD_SHIFT) |
@@ -86,7 +86,7 @@ static ssize_t ad9832_write(struct device *dev, struct device_attribute *attr,
 		goto error_ret;
 
 	mutex_lock(&indio_dev->mlock);
-	switch ((u32) this_attr->address) {
+	switch ((u32)this_attr->address) {
 	case AD9832_FREQ0HM:
 	case AD9832_FREQ1HM:
 		ret = ad9832_write_frequency(st, this_attr->address, val);
@@ -220,7 +220,7 @@ static int ad9832_probe(struct spi_device *spi)
 	}
 
 	indio_dev = devm_iio_device_alloc(&spi->dev, sizeof(*st));
-	if (indio_dev == NULL) {
+	if (!indio_dev) {
 		ret = -ENOMEM;
 		goto error_disable_reg;
 	}
diff --git a/drivers/staging/iio/frequency/ad9832.h b/drivers/staging/iio/frequency/ad9832.h
index 386f4dc8c9a1..d32323b46be6 100644
--- a/drivers/staging/iio/frequency/ad9832.h
+++ b/drivers/staging/iio/frequency/ad9832.h
@@ -42,13 +42,13 @@
 #define AD9832_CMD_SYNCSELSRC	0x8
 #define AD9832_CMD_SLEEPRESCLR	0xC
 
-#define AD9832_FREQ		(1 << 11)
+#define AD9832_FREQ		BIT(11)
 #define AD9832_PHASE(x)		(((x) & 3) << 9)
-#define AD9832_SYNC		(1 << 13)
-#define AD9832_SELSRC		(1 << 12)
-#define AD9832_SLEEP		(1 << 13)
-#define AD9832_RESET		(1 << 12)
-#define AD9832_CLR		(1 << 11)
+#define AD9832_SYNC		BIT(13)
+#define AD9832_SELSRC		BIT(12)
+#define AD9832_SLEEP		BIT(13)
+#define AD9832_RESET		BIT(12)
+#define AD9832_CLR		BIT(11)
 #define CMD_SHIFT		12
 #define ADD_SHIFT		8
 #define AD9832_FREQ_BITS	32
diff --git a/drivers/staging/iio/frequency/ad9834.c b/drivers/staging/iio/frequency/ad9834.c
index 5c803191c2ce..d02bb44fb8fc 100644
--- a/drivers/staging/iio/frequency/ad9834.c
+++ b/drivers/staging/iio/frequency/ad9834.c
@@ -27,7 +27,7 @@
 
 static unsigned int ad9834_calc_freqreg(unsigned long mclk, unsigned long fout)
 {
-	unsigned long long freqreg = (u64) fout * (u64) (1 << AD9834_FREQ_BITS);
+	unsigned long long freqreg = (u64)fout * (u64)BIT(AD9834_FREQ_BITS);
 
 	do_div(freqreg, mclk);
 	return freqreg;
@@ -53,9 +53,9 @@ static int ad9834_write_frequency(struct ad9834_state *st,
 }
 
 static int ad9834_write_phase(struct ad9834_state *st,
-				  unsigned long addr, unsigned long phase)
+			      unsigned long addr, unsigned long phase)
 {
-	if (phase > (1 << AD9834_PHASE_BITS))
+	if (phase > BIT(AD9834_PHASE_BITS))
 		return -EINVAL;
 	st->data = cpu_to_be16(addr | phase);
 
@@ -63,9 +63,9 @@ static int ad9834_write_phase(struct ad9834_state *st,
 }
 
 static ssize_t ad9834_write(struct device *dev,
-		struct device_attribute *attr,
-		const char *buf,
-		size_t len)
+			    struct device_attribute *attr,
+			    const char *buf,
+			    size_t len)
 {
 	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ad9834_state *st = iio_priv(indio_dev);
@@ -78,7 +78,7 @@ static ssize_t ad9834_write(struct device *dev,
 		goto error_ret;
 
 	mutex_lock(&indio_dev->mlock);
-	switch ((u32) this_attr->address) {
+	switch ((u32)this_attr->address) {
 	case AD9834_REG_FREQ0:
 	case AD9834_REG_FREQ1:
 		ret = ad9834_write_frequency(st, this_attr->address, val);
@@ -111,9 +111,9 @@ static ssize_t ad9834_write(struct device *dev,
 		break;
 	case AD9834_FSEL:
 	case AD9834_PSEL:
-		if (val == 0)
+		if (val == 0) {
 			st->control &= ~(this_attr->address | AD9834_PIN_SW);
-		else if (val == 1) {
+		} else if (val == 1) {
 			st->control |= this_attr->address;
 			st->control &= ~AD9834_PIN_SW;
 		} else {
@@ -142,9 +142,9 @@ error_ret:
 }
 
 static ssize_t ad9834_store_wavetype(struct device *dev,
-				 struct device_attribute *attr,
-				 const char *buf,
-				 size_t len)
+				     struct device_attribute *attr,
+				     const char *buf,
+				     size_t len)
 {
 	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ad9834_state *st = iio_priv(indio_dev);
@@ -154,7 +154,7 @@ static ssize_t ad9834_store_wavetype(struct device *dev,
 
 	mutex_lock(&indio_dev->mlock);
 
-	switch ((u32) this_attr->address) {
+	switch ((u32)this_attr->address) {
 	case 0:
 		if (sysfs_streq(buf, "sine")) {
 			st->control &= ~AD9834_MODE;
@@ -179,7 +179,7 @@ static ssize_t ad9834_store_wavetype(struct device *dev,
 		break;
 	case 1:
 		if (sysfs_streq(buf, "square") &&
-			!(st->control & AD9834_MODE)) {
+		    !(st->control & AD9834_MODE)) {
 			st->control &= ~AD9834_MODE;
 			st->control |= AD9834_OPBITEN;
 		} else {
@@ -200,9 +200,10 @@ static ssize_t ad9834_store_wavetype(struct device *dev,
 	return ret ? ret : len;
 }
 
-static ssize_t ad9834_show_out0_wavetype_available(struct device *dev,
-						struct device_attribute *attr,
-						char *buf)
+static
+ssize_t ad9834_show_out0_wavetype_available(struct device *dev,
+					    struct device_attribute *attr,
+					    char *buf)
 {
 	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ad9834_state *st = iio_priv(indio_dev);
@@ -218,13 +219,13 @@ static ssize_t ad9834_show_out0_wavetype_available(struct device *dev,
 	return sprintf(buf, "%s\n", str);
 }
 
-
 static IIO_DEVICE_ATTR(out_altvoltage0_out0_wavetype_available, S_IRUGO,
 		       ad9834_show_out0_wavetype_available, NULL, 0);
 
-static ssize_t ad9834_show_out1_wavetype_available(struct device *dev,
-						struct device_attribute *attr,
-						char *buf)
+static
+ssize_t ad9834_show_out1_wavetype_available(struct device *dev,
+					    struct device_attribute *attr,
+					    char *buf)
 {
 	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ad9834_state *st = iio_priv(indio_dev);
@@ -336,7 +337,7 @@ static int ad9834_probe(struct spi_device *spi)
 	}
 
 	indio_dev = devm_iio_device_alloc(&spi->dev, sizeof(*st));
-	if (indio_dev == NULL) {
+	if (!indio_dev) {
 		ret = -ENOMEM;
 		goto error_disable_reg;
 	}
diff --git a/drivers/staging/iio/frequency/ad9834.h b/drivers/staging/iio/frequency/ad9834.h
index 8ca6e52bae6b..40fdd5da7bd0 100644
--- a/drivers/staging/iio/frequency/ad9834.h
+++ b/drivers/staging/iio/frequency/ad9834.h
@@ -10,31 +10,31 @@
 
 /* Registers */
 
-#define AD9834_REG_CMD		(0 << 14)
-#define AD9834_REG_FREQ0	(1 << 14)
-#define AD9834_REG_FREQ1	(2 << 14)
-#define AD9834_REG_PHASE0	(6 << 13)
-#define AD9834_REG_PHASE1	(7 << 13)
+#define AD9834_REG_CMD		0
+#define AD9834_REG_FREQ0	BIT(14)
+#define AD9834_REG_FREQ1	BIT(15)
+#define AD9834_REG_PHASE0	(BIT(15) | BIT(14))
+#define AD9834_REG_PHASE1	(BIT(15) | BIT(14) | BIT(13))
 
 /* Command Control Bits */
 
-#define AD9834_B28		(1 << 13)
-#define AD9834_HLB		(1 << 12)
-#define AD9834_FSEL		(1 << 11)
-#define AD9834_PSEL		(1 << 10)
-#define AD9834_PIN_SW		(1 << 9)
-#define AD9834_RESET		(1 << 8)
-#define AD9834_SLEEP1		(1 << 7)
-#define AD9834_SLEEP12		(1 << 6)
-#define AD9834_OPBITEN		(1 << 5)
-#define AD9834_SIGN_PIB		(1 << 4)
-#define AD9834_DIV2		(1 << 3)
-#define AD9834_MODE		(1 << 1)
+#define AD9834_B28		BIT(13)
+#define AD9834_HLB		BIT(12)
+#define AD9834_FSEL		BIT(11)
+#define AD9834_PSEL		BIT(10)
+#define AD9834_PIN_SW		BIT(9)
+#define AD9834_RESET		BIT(8)
+#define AD9834_SLEEP1		BIT(7)
+#define AD9834_SLEEP12		BIT(6)
+#define AD9834_OPBITEN		BIT(5)
+#define AD9834_SIGN_PIB		BIT(4)
+#define AD9834_DIV2		BIT(3)
+#define AD9834_MODE		BIT(1)
 
 #define AD9834_FREQ_BITS	28
 #define AD9834_PHASE_BITS	12
 
-#define RES_MASK(bits)	((1 << (bits)) - 1)
+#define RES_MASK(bits)	(BIT(bits) - 1)
 
 /**
  * struct ad9834_state - driver instance specific data
@@ -69,7 +69,6 @@ struct ad9834_state {
 	__be16				freq_data[2];
 };
 
-
 /*
  * TODO: struct ad7887_platform_data needs to go into include/linux/iio
  */
diff --git a/drivers/staging/iio/frequency/dds.h b/drivers/staging/iio/frequency/dds.h
index 611e2b0cfc4c..fe53e7324c94 100644
--- a/drivers/staging/iio/frequency/dds.h
+++ b/drivers/staging/iio/frequency/dds.h
@@ -5,6 +5,8 @@
  *
  * Licensed under the GPL-2 or later.
  */
+#ifndef IIO_DDS_H_
+#define IIO_DDS_H_
 
 /**
  * /sys/bus/iio/devices/.../out_altvoltageX_frequencyY
@@ -108,3 +110,5 @@
 #define IIO_CONST_ATTR_OUT_WAVETYPES_AVAILABLE(_channel, _output, _modes)\
 	IIO_CONST_ATTR(							\
 	out_altvoltage##_channel##_out##_output##_wavetype_available, _modes)
+
+#endif /* IIO_DDS_H_ */
diff --git a/drivers/staging/iio/iio_dummy_evgen.c b/drivers/staging/iio/iio_dummy_evgen.c
index 59ad5a3efe9c..0c9c86d7b509 100644
--- a/drivers/staging/iio/iio_dummy_evgen.c
+++ b/drivers/staging/iio/iio_dummy_evgen.c
@@ -72,7 +72,7 @@ static int iio_dummy_evgen_create(void)
 	int ret, i;
 
 	iio_evgen = kzalloc(sizeof(*iio_evgen), GFP_KERNEL);
-	if (iio_evgen == NULL)
+	if (!iio_evgen)
 		return -ENOMEM;
 
 	iio_evgen->base = irq_alloc_descs(-1, 0, IIO_EVENTGEN_NO, 0);
@@ -105,7 +105,7 @@ int iio_dummy_evgen_get_irq(void)
 {
 	int i, ret = 0;
 
-	if (iio_evgen == NULL)
+	if (!iio_evgen)
 		return -ENODEV;
 
 	mutex_lock(&iio_evgen->lock);
diff --git a/drivers/staging/iio/iio_simple_dummy.c b/drivers/staging/iio/iio_simple_dummy.c
index e4520213f627..b47bf9fb64be 100644
--- a/drivers/staging/iio/iio_simple_dummy.c
+++ b/drivers/staging/iio/iio_simple_dummy.c
@@ -17,7 +17,6 @@
 #include <linux/kernel.h>
 #include <linux/slab.h>
 #include <linux/module.h>
-#include <linux/moduleparam.h>
 
 #include <linux/iio/iio.h>
 #include <linux/iio/sysfs.h>
@@ -589,7 +588,7 @@ static int iio_dummy_probe(int index)
 	 * for chip specific state information.
 	 */
 	indio_dev = iio_device_alloc(sizeof(*st));
-	if (indio_dev == NULL) {
+	if (!indio_dev) {
 		ret = -ENOMEM;
 		goto error_ret;
 	}
diff --git a/drivers/staging/iio/iio_simple_dummy_buffer.c b/drivers/staging/iio/iio_simple_dummy_buffer.c
index 360a4c980722..a651b8922d0a 100644
--- a/drivers/staging/iio/iio_simple_dummy_buffer.c
+++ b/drivers/staging/iio/iio_simple_dummy_buffer.c
@@ -50,7 +50,7 @@ static irqreturn_t iio_simple_dummy_trigger_h(int irq, void *p)
 	u16 *data;
 
 	data = kmalloc(indio_dev->scan_bytes, GFP_KERNEL);
-	if (data == NULL)
+	if (!data)
 		goto done;
 
 	if (!bitmap_empty(indio_dev->active_scan_mask, indio_dev->masklength)) {
@@ -122,7 +122,7 @@ int iio_simple_dummy_configure_buffer(struct iio_dev *indio_dev)
 
 	/* Allocate a buffer to use - here a kfifo */
 	buffer = iio_kfifo_allocate();
-	if (buffer == NULL) {
+	if (!buffer) {
 		ret = -ENOMEM;
 		goto error_ret;
 	}
@@ -161,7 +161,7 @@ int iio_simple_dummy_configure_buffer(struct iio_dev *indio_dev)
 						 "iio_simple_dummy_consumer%d",
 						 indio_dev->id);
 
-	if (indio_dev->pollfunc == NULL) {
+	if (!indio_dev->pollfunc) {
 		ret = -ENOMEM;
 		goto error_free_buffer;
 	}
diff --git a/drivers/staging/iio/impedance-analyzer/ad5933.c b/drivers/staging/iio/impedance-analyzer/ad5933.c
index 79194399e040..c18109c55497 100644
--- a/drivers/staging/iio/impedance-analyzer/ad5933.c
+++ b/drivers/staging/iio/impedance-analyzer/ad5933.c
@@ -703,7 +703,7 @@ static int ad5933_probe(struct i2c_client *client,
 	struct iio_dev *indio_dev;
 
 	indio_dev = devm_iio_device_alloc(&client->dev, sizeof(*st));
-	if (indio_dev == NULL)
+	if (!indio_dev)
 		return -ENOMEM;
 
 	st = iio_priv(indio_dev);
diff --git a/drivers/staging/iio/light/isl29028.c b/drivers/staging/iio/light/isl29028.c
index 6440e3b293ca..e5b2fdc2334b 100644
--- a/drivers/staging/iio/light/isl29028.c
+++ b/drivers/staging/iio/light/isl29028.c
@@ -441,15 +441,15 @@ static int isl29028_chip_init(struct isl29028_chip *chip)
 
 	ret = isl29028_set_proxim_sampling(chip, chip->prox_sampling);
 	if (ret < 0) {
-		dev_err(chip->dev, "%s(): setting the proximity, err = %d\n",
-			__func__, ret);
+		dev_err(chip->dev, "setting the proximity, err = %d\n",
+			ret);
 		return ret;
 	}
 
 	ret = isl29028_set_als_scale(chip, chip->lux_scale);
 	if (ret < 0)
-		dev_err(chip->dev, "%s(): setting als scale failed, err = %d\n",
-			__func__, ret);
+		dev_err(chip->dev,
+			"setting als scale failed, err = %d\n", ret);
 	return ret;
 }
 
diff --git a/drivers/staging/iio/light/tsl2583.c b/drivers/staging/iio/light/tsl2583.c
index 8afae8e33d56..b5e1b8b0a6f0 100644
--- a/drivers/staging/iio/light/tsl2583.c
+++ b/drivers/staging/iio/light/tsl2583.c
@@ -471,14 +471,12 @@ static int taos_chip_on(struct iio_dev *indio_dev)
 static int taos_chip_off(struct iio_dev *indio_dev)
 {
 	struct tsl2583_chip *chip = iio_priv(indio_dev);
-	int ret;
 
 	/* turn device off */
 	chip->taos_chip_status = TSL258X_CHIP_SUSPENDED;
-	ret = i2c_smbus_write_byte_data(chip->client,
+	return i2c_smbus_write_byte_data(chip->client,
 					TSL258X_CMD_REG | TSL258X_CNTRL,
 					0x00);
-	return ret;
 }
 
 /* Sysfs Interface Functions */
diff --git a/drivers/staging/iio/light/tsl2x7x_core.c b/drivers/staging/iio/light/tsl2x7x_core.c
index 4a5dc26fed4c..010e607dd880 100644
--- a/drivers/staging/iio/light/tsl2x7x_core.c
+++ b/drivers/staging/iio/light/tsl2x7x_core.c
@@ -301,8 +301,7 @@ tsl2x7x_i2c_read(struct i2c_client *client, u8 reg, u8 *val)
 	/* select register to write */
 	ret = i2c_smbus_write_byte(client, (TSL2X7X_CMD_REG | reg));
 	if (ret < 0) {
-		dev_err(&client->dev, "%s: failed to write register %x\n"
-				, __func__, reg);
+		dev_err(&client->dev, "failed to write register %x\n", reg);
 		return ret;
 	}
 
@@ -311,8 +310,7 @@ tsl2x7x_i2c_read(struct i2c_client *client, u8 reg, u8 *val)
 	if (ret >= 0)
 		*val = (u8)ret;
 	else
-		dev_err(&client->dev, "%s: failed to read register %x\n"
-						, __func__, reg);
+		dev_err(&client->dev, "failed to read register %x\n", reg);
 
 	return ret;
 }
@@ -377,7 +375,7 @@ static int tsl2x7x_get_lux(struct iio_dev *indio_dev)
 			&buf[i]);
 		if (ret < 0) {
 			dev_err(&chip->client->dev,
-				"%s: failed to read. err=%x\n", __func__, ret);
+				"failed to read. err=%x\n", ret);
 			goto out_unlock;
 		}
 	}
@@ -389,8 +387,7 @@ static int tsl2x7x_get_lux(struct iio_dev *indio_dev)
 				TSL2X7X_CMD_ALS_INT_CLR));
 	if (ret < 0) {
 		dev_err(&chip->client->dev,
-		"%s: i2c_write_command failed - err = %d\n",
-			__func__, ret);
+			"i2c_write_command failed - err = %d\n", ret);
 		goto out_unlock; /* have no data, so return failure */
 	}
 
@@ -493,8 +490,7 @@ static int tsl2x7x_get_prox(struct iio_dev *indio_dev)
 	ret = tsl2x7x_i2c_read(chip->client,
 		(TSL2X7X_CMD_REG | TSL2X7X_STATUS), &status);
 	if (ret < 0) {
-		dev_err(&chip->client->dev,
-		"%s: i2c err=%d\n", __func__, ret);
+		dev_err(&chip->client->dev, "i2c err=%d\n", ret);
 		goto prox_poll_err;
 	}
 
@@ -583,8 +579,7 @@ static int tsl2x7x_als_calibrate(struct iio_dev *indio_dev)
 			(TSL2X7X_CMD_REG | TSL2X7X_CNTRL));
 	if (ret < 0) {
 		dev_err(&chip->client->dev,
-		"%s: failed to write CNTRL register, ret=%d\n",
-		__func__, ret);
+			"failed to write CNTRL register, ret=%d\n", ret);
 		return ret;
 	}
 
@@ -600,8 +595,7 @@ static int tsl2x7x_als_calibrate(struct iio_dev *indio_dev)
 			(TSL2X7X_CMD_REG | TSL2X7X_CNTRL));
 	if (ret < 0) {
 		dev_err(&chip->client->dev,
-			"%s: failed to write ctrl reg: ret=%d\n",
-			__func__, ret);
+			"failed to write ctrl reg: ret=%d\n", ret);
 		return ret;
 	}
 
@@ -619,8 +613,8 @@ static int tsl2x7x_als_calibrate(struct iio_dev *indio_dev)
 		return lux_val;
 	}
 
-	gain_trim_val =  (((chip->tsl2x7x_settings.als_cal_target)
-			* chip->tsl2x7x_settings.als_gain_trim) / lux_val);
+	gain_trim_val =  ((chip->tsl2x7x_settings.als_cal_target)
+			* chip->tsl2x7x_settings.als_gain_trim) / lux_val;
 	if ((gain_trim_val < 250) || (gain_trim_val > 4000))
 		return -ERANGE;
 
@@ -720,7 +714,7 @@ static int tsl2x7x_chip_on(struct iio_dev *indio_dev)
 				TSL2X7X_CMD_REG + i, *dev_reg++);
 		if (ret < 0) {
 			dev_err(&chip->client->dev,
-			"%s: failed on write to reg %d.\n", __func__, i);
+				"failed on write to reg %d.\n", i);
 			return ret;
 		}
 	}
@@ -871,8 +865,8 @@ static void tsl2x7x_prox_cal(struct iio_dev *indio_dev)
 
 	if (chip->tsl2x7x_settings.prox_max_samples_cal > MAX_SAMPLES_CAL) {
 		dev_err(&chip->client->dev,
-			"%s: max prox samples cal is too big: %d\n",
-			__func__, chip->tsl2x7x_settings.prox_max_samples_cal);
+			"max prox samples cal is too big: %d\n",
+			chip->tsl2x7x_settings.prox_max_samples_cal);
 		chip->tsl2x7x_settings.prox_max_samples_cal = MAX_SAMPLES_CAL;
 	}
 
@@ -1563,8 +1557,8 @@ static irqreturn_t tsl2x7x_event_handler(int irq, void *private)
 		TSL2X7X_CMD_PROXALS_INT_CLR);
 	if (ret < 0)
 		dev_err(&chip->client->dev,
-			"%s: Failed to clear irq from event handler. err = %d\n",
-			__func__, ret);
+			"Failed to clear irq from event handler. err = %d\n",
+			ret);
 
 	return IRQ_HANDLED;
 }
@@ -1893,8 +1887,8 @@ static int tsl2x7x_probe(struct i2c_client *clientp,
 
 	ret = i2c_smbus_write_byte(clientp, (TSL2X7X_CMD_REG | TSL2X7X_CNTRL));
 	if (ret < 0) {
-		dev_err(&clientp->dev, "%s: write to cmd reg failed. err = %d\n",
-				__func__, ret);
+		dev_err(&clientp->dev, "write to cmd reg failed. err = %d\n",
+			ret);
 		return ret;
 	}
 
diff --git a/drivers/staging/iio/magnetometer/hmc5843.h b/drivers/staging/iio/magnetometer/hmc5843.h
index b784e3eb4591..f3d0da2fe458 100644
--- a/drivers/staging/iio/magnetometer/hmc5843.h
+++ b/drivers/staging/iio/magnetometer/hmc5843.h
@@ -10,7 +10,6 @@
  *
  * */
 
-
 #ifndef HMC5843_CORE_H
 #define HMC5843_CORE_H
 
@@ -32,16 +31,24 @@ enum hmc5843_ids {
 	HMC5983_ID,
 };
 
+/**
+ * struct hcm5843_data	- device specific data
+ * @dev:		actual device
+ * @lock:		update and read regmap data
+ * @regmap:		hardware access register maps
+ * @variant:		describe chip variants
+ * @buffer:		3x 16-bit channels + padding + 64-bit timestamp
+ **/
 struct hmc5843_data {
 	struct device *dev;
 	struct mutex lock;
 	struct regmap *regmap;
 	const struct hmc5843_chip_info *variant;
-	__be16 buffer[8]; /* 3x 16-bit channels + padding + 64-bit timestamp */
+	__be16 buffer[8];
 };
 
 int hmc5843_common_probe(struct device *dev, struct regmap *regmap,
-		enum hmc5843_ids id);
+			 enum hmc5843_ids id);
 int hmc5843_common_remove(struct device *dev);
 
 int hmc5843_common_suspend(struct device *dev);
diff --git a/drivers/staging/iio/magnetometer/hmc5843_core.c b/drivers/staging/iio/magnetometer/hmc5843_core.c
index fd171d8b38fb..fffca3a9f637 100644
--- a/drivers/staging/iio/magnetometer/hmc5843_core.c
+++ b/drivers/staging/iio/magnetometer/hmc5843_core.c
@@ -1,25 +1,25 @@
-/*  Copyright (C) 2010 Texas Instruments
-    Author: Shubhrajyoti Datta <shubhrajyoti@ti.com>
-    Acknowledgement: Jonathan Cameron <jic23@kernel.org> for valuable inputs.
-
-    Support for HMC5883 and HMC5883L by Peter Meerwald <pmeerw@pmeerw.net>.
-
-    Split to multiple files by Josef Gajdusek <atx@atx.name> - 2014
-
-    This program is free software; you can redistribute it and/or modify
-    it under the terms of the 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.
-*/
+/*
+ * Device driver for the the HMC5843 multi-chip module designed
+ * for low field magnetic sensing.
+ *
+ * Copyright (C) 2010 Texas Instruments
+ *
+ * Author: Shubhrajyoti Datta <shubhrajyoti@ti.com>
+ * Acknowledgment: Jonathan Cameron <jic23@kernel.org> for valuable inputs.
+ * Support for HMC5883 and HMC5883L by Peter Meerwald <pmeerw@pmeerw.net>.
+ * Split to multiple files by Josef Gajdusek <atx@atx.name> - 2014
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
 
 #include <linux/module.h>
 #include <linux/regmap.h>
@@ -122,7 +122,7 @@ static s32 hmc5843_set_mode(struct hmc5843_data *data, u8 operating_mode)
 
 	mutex_lock(&data->lock);
 	ret = regmap_update_bits(data->regmap, HMC5843_MODE_REG,
-			HMC5843_MODE_MASK, operating_mode);
+				 HMC5843_MODE_MASK, operating_mode);
 	mutex_unlock(&data->lock);
 
 	return ret;
@@ -165,7 +165,7 @@ static int hmc5843_read_measurement(struct hmc5843_data *data,
 		return ret;
 	}
 	ret = regmap_bulk_read(data->regmap, HMC5843_DATA_OUT_MSB_REGS,
-			values, sizeof(values));
+			       values, sizeof(values));
 	mutex_unlock(&data->lock);
 	if (ret < 0)
 		return ret;
@@ -198,15 +198,16 @@ static int hmc5843_set_meas_conf(struct hmc5843_data *data, u8 meas_conf)
 
 	mutex_lock(&data->lock);
 	ret = regmap_update_bits(data->regmap, HMC5843_CONFIG_REG_A,
-			HMC5843_MEAS_CONF_MASK, meas_conf);
+				 HMC5843_MEAS_CONF_MASK, meas_conf);
 	mutex_unlock(&data->lock);
 
 	return ret;
 }
 
-static ssize_t hmc5843_show_measurement_configuration(struct device *dev,
-						struct device_attribute *attr,
-						char *buf)
+static
+ssize_t hmc5843_show_measurement_configuration(struct device *dev,
+					       struct device_attribute *attr,
+					       char *buf)
 {
 	struct hmc5843_data *data = iio_priv(dev_to_iio_dev(dev));
 	unsigned int val;
@@ -220,10 +221,11 @@ static ssize_t hmc5843_show_measurement_configuration(struct device *dev,
 	return sprintf(buf, "%d\n", val);
 }
 
-static ssize_t hmc5843_set_measurement_configuration(struct device *dev,
-						struct device_attribute *attr,
-						const char *buf,
-						size_t count)
+static
+ssize_t hmc5843_set_measurement_configuration(struct device *dev,
+					      struct device_attribute *attr,
+					      const char *buf,
+					      size_t count)
 {
 	struct hmc5843_data *data = iio_priv(dev_to_iio_dev(dev));
 	unsigned long meas_conf = 0;
@@ -246,8 +248,9 @@ static IIO_DEVICE_ATTR(meas_conf,
 			hmc5843_set_measurement_configuration,
 			0);
 
-static ssize_t hmc5843_show_samp_freq_avail(struct device *dev,
-				struct device_attribute *attr, char *buf)
+static
+ssize_t hmc5843_show_samp_freq_avail(struct device *dev,
+				     struct device_attribute *attr, char *buf)
 {
 	struct hmc5843_data *data = iio_priv(dev_to_iio_dev(dev));
 	size_t len = 0;
@@ -272,20 +275,21 @@ static int hmc5843_set_samp_freq(struct hmc5843_data *data, u8 rate)
 
 	mutex_lock(&data->lock);
 	ret = regmap_update_bits(data->regmap, HMC5843_CONFIG_REG_A,
-			HMC5843_RATE_MASK, rate << HMC5843_RATE_OFFSET);
+				 HMC5843_RATE_MASK,
+				 rate << HMC5843_RATE_OFFSET);
 	mutex_unlock(&data->lock);
 
 	return ret;
 }
 
 static int hmc5843_get_samp_freq_index(struct hmc5843_data *data,
-				   int val, int val2)
+				       int val, int val2)
 {
 	int i;
 
 	for (i = 0; i < data->variant->n_regval_to_samp_freq; i++)
 		if (val == data->variant->regval_to_samp_freq[i][0] &&
-			val2 == data->variant->regval_to_samp_freq[i][1])
+		    val2 == data->variant->regval_to_samp_freq[i][1])
 			return i;
 
 	return -EINVAL;
@@ -297,15 +301,16 @@ static int hmc5843_set_range_gain(struct hmc5843_data *data, u8 range)
 
 	mutex_lock(&data->lock);
 	ret = regmap_update_bits(data->regmap, HMC5843_CONFIG_REG_B,
-			HMC5843_RANGE_GAIN_MASK,
-			range << HMC5843_RANGE_GAIN_OFFSET);
+				 HMC5843_RANGE_GAIN_MASK,
+				 range << HMC5843_RANGE_GAIN_OFFSET);
 	mutex_unlock(&data->lock);
 
 	return ret;
 }
 
 static ssize_t hmc5843_show_scale_avail(struct device *dev,
-				struct device_attribute *attr, char *buf)
+					struct device_attribute *attr,
+					char *buf)
 {
 	struct hmc5843_data *data = iio_priv(dev_to_iio_dev(dev));
 
@@ -396,7 +401,8 @@ static int hmc5843_write_raw(struct iio_dev *indio_dev,
 }
 
 static int hmc5843_write_raw_get_fmt(struct iio_dev *indio_dev,
-			       struct iio_chan_spec const *chan, long mask)
+				     struct iio_chan_spec const *chan,
+				     long mask)
 {
 	switch (mask) {
 	case IIO_CHAN_INFO_SAMP_FREQ:
@@ -423,14 +429,14 @@ static irqreturn_t hmc5843_trigger_handler(int irq, void *p)
 	}
 
 	ret = regmap_bulk_read(data->regmap, HMC5843_DATA_OUT_MSB_REGS,
-			data->buffer, 3 * sizeof(__be16));
+			       data->buffer, 3 * sizeof(__be16));
 
 	mutex_unlock(&data->lock);
 	if (ret < 0)
 		goto done;
 
 	iio_push_to_buffers_with_timestamp(indio_dev, data->buffer,
-		iio_get_time_ns());
+					   iio_get_time_ns());
 
 done:
 	iio_trigger_notify_done(indio_dev->trig);
@@ -526,7 +532,7 @@ static int hmc5843_init(struct hmc5843_data *data)
 	u8 id[3];
 
 	ret = regmap_bulk_read(data->regmap, HMC5843_ID_REG,
-			id, ARRAY_SIZE(id));
+			       id, ARRAY_SIZE(id));
 	if (ret < 0)
 		return ret;
 	if (id[0] != 'H' || id[1] != '4' || id[2] != '3') {
@@ -556,7 +562,6 @@ static const struct iio_info hmc5843_info = {
 
 static const unsigned long hmc5843_scan_masks[] = {0x7, 0};
 
-
 int hmc5843_common_suspend(struct device *dev)
 {
 	return hmc5843_set_mode(iio_priv(dev_get_drvdata(dev)),
@@ -567,19 +572,19 @@ EXPORT_SYMBOL(hmc5843_common_suspend);
 int hmc5843_common_resume(struct device *dev)
 {
 	return hmc5843_set_mode(iio_priv(dev_get_drvdata(dev)),
-			HMC5843_MODE_SLEEP);
+				HMC5843_MODE_SLEEP);
 }
 EXPORT_SYMBOL(hmc5843_common_resume);
 
 int hmc5843_common_probe(struct device *dev, struct regmap *regmap,
-		enum hmc5843_ids id)
+			 enum hmc5843_ids id)
 {
 	struct hmc5843_data *data;
 	struct iio_dev *indio_dev;
 	int ret;
 
 	indio_dev = devm_iio_device_alloc(dev, sizeof(*data));
-	if (indio_dev == NULL)
+	if (!indio_dev)
 		return -ENOMEM;
 
 	dev_set_drvdata(dev, indio_dev);
@@ -592,6 +597,7 @@ int hmc5843_common_probe(struct device *dev, struct regmap *regmap,
 	mutex_init(&data->lock);
 
 	indio_dev->dev.parent = dev;
+	indio_dev->name = dev->driver->name;
 	indio_dev->info = &hmc5843_info;
 	indio_dev->modes = INDIO_DIRECT_MODE;
 	indio_dev->channels = data->variant->channels;
@@ -603,9 +609,9 @@ int hmc5843_common_probe(struct device *dev, struct regmap *regmap,
 		return ret;
 
 	ret = iio_triggered_buffer_setup(indio_dev, NULL,
-		hmc5843_trigger_handler, NULL);
+					 hmc5843_trigger_handler, NULL);
 	if (ret < 0)
-		return ret;
+		goto buffer_setup_err;
 
 	ret = iio_device_register(indio_dev);
 	if (ret < 0)
@@ -615,6 +621,8 @@ int hmc5843_common_probe(struct device *dev, struct regmap *regmap,
 
 buffer_cleanup:
 	iio_triggered_buffer_cleanup(indio_dev);
+buffer_setup_err:
+	hmc5843_set_mode(iio_priv(indio_dev), HMC5843_MODE_SLEEP);
 	return ret;
 }
 EXPORT_SYMBOL(hmc5843_common_probe);
diff --git a/drivers/staging/iio/magnetometer/hmc5843_i2c.c b/drivers/staging/iio/magnetometer/hmc5843_i2c.c
index 6acd614cdbc6..ff08667fa2f6 100644
--- a/drivers/staging/iio/magnetometer/hmc5843_i2c.c
+++ b/drivers/staging/iio/magnetometer/hmc5843_i2c.c
@@ -19,48 +19,48 @@
 #include "hmc5843.h"
 
 static const struct regmap_range hmc5843_readable_ranges[] = {
-		regmap_reg_range(0, HMC5843_ID_END),
+	regmap_reg_range(0, HMC5843_ID_END),
 };
 
-static struct regmap_access_table hmc5843_readable_table = {
-		.yes_ranges = hmc5843_readable_ranges,
-		.n_yes_ranges = ARRAY_SIZE(hmc5843_readable_ranges),
+static const struct regmap_access_table hmc5843_readable_table = {
+	.yes_ranges = hmc5843_readable_ranges,
+	.n_yes_ranges = ARRAY_SIZE(hmc5843_readable_ranges),
 };
 
 static const struct regmap_range hmc5843_writable_ranges[] = {
-		regmap_reg_range(0, HMC5843_MODE_REG),
+	regmap_reg_range(0, HMC5843_MODE_REG),
 };
 
-static struct regmap_access_table hmc5843_writable_table = {
-		.yes_ranges = hmc5843_writable_ranges,
-		.n_yes_ranges = ARRAY_SIZE(hmc5843_writable_ranges),
+static const struct regmap_access_table hmc5843_writable_table = {
+	.yes_ranges = hmc5843_writable_ranges,
+	.n_yes_ranges = ARRAY_SIZE(hmc5843_writable_ranges),
 };
 
 static const struct regmap_range hmc5843_volatile_ranges[] = {
-		regmap_reg_range(HMC5843_DATA_OUT_MSB_REGS, HMC5843_STATUS_REG),
+	regmap_reg_range(HMC5843_DATA_OUT_MSB_REGS, HMC5843_STATUS_REG),
 };
 
-static struct regmap_access_table hmc5843_volatile_table = {
-		.yes_ranges = hmc5843_volatile_ranges,
-		.n_yes_ranges = ARRAY_SIZE(hmc5843_volatile_ranges),
+static const struct regmap_access_table hmc5843_volatile_table = {
+	.yes_ranges = hmc5843_volatile_ranges,
+	.n_yes_ranges = ARRAY_SIZE(hmc5843_volatile_ranges),
 };
 
-static struct regmap_config hmc5843_i2c_regmap_config = {
-		.reg_bits = 8,
-		.val_bits = 8,
+static const struct regmap_config hmc5843_i2c_regmap_config = {
+	.reg_bits = 8,
+	.val_bits = 8,
 
-		.rd_table = &hmc5843_readable_table,
-		.wr_table = &hmc5843_writable_table,
-		.volatile_table = &hmc5843_volatile_table,
+	.rd_table = &hmc5843_readable_table,
+	.wr_table = &hmc5843_writable_table,
+	.volatile_table = &hmc5843_volatile_table,
 
-		.cache_type = REGCACHE_RBTREE,
+	.cache_type = REGCACHE_RBTREE,
 };
 
-static int hmc5843_i2c_probe(struct i2c_client *client,
-			 const struct i2c_device_id *id)
+static int hmc5843_i2c_probe(struct i2c_client *cli,
+			     const struct i2c_device_id *id)
 {
-	return hmc5843_common_probe(&client->dev,
-			devm_regmap_init_i2c(client, &hmc5843_i2c_regmap_config),
+	return hmc5843_common_probe(&cli->dev,
+			devm_regmap_init_i2c(cli, &hmc5843_i2c_regmap_config),
 			id->driver_data);
 }
 
diff --git a/drivers/staging/iio/magnetometer/hmc5843_spi.c b/drivers/staging/iio/magnetometer/hmc5843_spi.c
index 98c4b57101c9..8e658f736e1f 100644
--- a/drivers/staging/iio/magnetometer/hmc5843_spi.c
+++ b/drivers/staging/iio/magnetometer/hmc5843_spi.c
@@ -19,7 +19,7 @@ static const struct regmap_range hmc5843_readable_ranges[] = {
 		regmap_reg_range(0, HMC5843_ID_END),
 };
 
-static struct regmap_access_table hmc5843_readable_table = {
+static const struct regmap_access_table hmc5843_readable_table = {
 		.yes_ranges = hmc5843_readable_ranges,
 		.n_yes_ranges = ARRAY_SIZE(hmc5843_readable_ranges),
 };
@@ -28,7 +28,7 @@ static const struct regmap_range hmc5843_writable_ranges[] = {
 		regmap_reg_range(0, HMC5843_MODE_REG),
 };
 
-static struct regmap_access_table hmc5843_writable_table = {
+static const struct regmap_access_table hmc5843_writable_table = {
 		.yes_ranges = hmc5843_writable_ranges,
 		.n_yes_ranges = ARRAY_SIZE(hmc5843_writable_ranges),
 };
@@ -37,12 +37,12 @@ static const struct regmap_range hmc5843_volatile_ranges[] = {
 		regmap_reg_range(HMC5843_DATA_OUT_MSB_REGS, HMC5843_STATUS_REG),
 };
 
-static struct regmap_access_table hmc5843_volatile_table = {
+static const struct regmap_access_table hmc5843_volatile_table = {
 		.yes_ranges = hmc5843_volatile_ranges,
 		.n_yes_ranges = ARRAY_SIZE(hmc5843_volatile_ranges),
 };
 
-static struct regmap_config hmc5843_spi_regmap_config = {
+static const struct regmap_config hmc5843_spi_regmap_config = {
 		.reg_bits = 8,
 		.val_bits = 8,
 
diff --git a/drivers/staging/iio/meter/ade7753.c b/drivers/staging/iio/meter/ade7753.c
index 78e8f560eeec..ffc7f0ddff14 100644
--- a/drivers/staging/iio/meter/ade7753.c
+++ b/drivers/staging/iio/meter/ade7753.c
@@ -411,7 +411,7 @@ static ssize_t ade7753_write_frequency(struct device *dev,
 
 	mutex_lock(&indio_dev->mlock);
 
-	t = (27900 / val);
+	t = 27900 / val;
 	if (t > 0)
 		t--;
 
@@ -517,11 +517,7 @@ static int ade7753_probe(struct spi_device *spi)
 	if (ret)
 		return ret;
 
-	ret = iio_device_register(indio_dev);
-	if (ret)
-		return ret;
-
-	return 0;
+	return iio_device_register(indio_dev);
 }
 
 /* fixme, confirm ordering in this function */
diff --git a/drivers/staging/iio/meter/ade7754.c b/drivers/staging/iio/meter/ade7754.c
index 81f67318974a..f12b2e50329b 100644
--- a/drivers/staging/iio/meter/ade7754.c
+++ b/drivers/staging/iio/meter/ade7754.c
@@ -216,9 +216,13 @@ error_ret:
 
 static int ade7754_reset(struct device *dev)
 {
+	int ret;
 	u8 val;
 
-	ade7754_spi_read_reg_8(dev, ADE7754_OPMODE, &val);
+	ret = ade7754_spi_read_reg_8(dev, ADE7754_OPMODE, &val);
+	if (ret < 0)
+		return ret;
+
 	val |= 1 << 6; /* Software Chip Reset */
 	return ade7754_spi_write_reg_8(dev, ADE7754_OPMODE, val);
 }
@@ -362,9 +366,16 @@ error_ret:
 /* Power down the device */
 static int ade7754_stop_device(struct device *dev)
 {
+	int ret;
 	u8 val;
 
-	ade7754_spi_read_reg_8(dev, ADE7754_OPMODE, &val);
+	ret = ade7754_spi_read_reg_8(dev, ADE7754_OPMODE, &val);
+	if (ret < 0) {
+		dev_err(dev, "unable to power down the device, error: %d",
+			ret);
+		return ret;
+	}
+
 	val |= 7 << 3;  /* ADE7754 powered down */
 	return ade7754_spi_write_reg_8(dev, ADE7754_OPMODE, val);
 }
@@ -432,7 +443,7 @@ static ssize_t ade7754_write_frequency(struct device *dev,
 
 	mutex_lock(&indio_dev->mlock);
 
-	t = (26000 / val);
+	t = 26000 / val;
 	if (t > 0)
 		t--;
 
@@ -539,12 +550,15 @@ static int ade7754_probe(struct spi_device *spi)
 	/* Get the device into a sane initial state */
 	ret = ade7754_initial_setup(indio_dev);
 	if (ret)
-		return ret;
+		goto powerdown_on_error;
 	ret = iio_device_register(indio_dev);
 	if (ret)
-		return ret;
+		goto powerdown_on_error;
+	return ret;
 
-	return 0;
+powerdown_on_error:
+	ade7754_stop_device(&indio_dev->dev);
+	return ret;
 }
 
 /* fixme, confirm ordering in this function */
diff --git a/drivers/staging/iio/meter/ade7758_core.c b/drivers/staging/iio/meter/ade7758_core.c
index 70e96b20c2eb..77141ae1349d 100644
--- a/drivers/staging/iio/meter/ade7758_core.c
+++ b/drivers/staging/iio/meter/ade7758_core.c
@@ -303,14 +303,15 @@ static int ade7758_reset(struct device *dev)
 	int ret;
 	u8 val;
 
-	ade7758_spi_read_reg_8(dev,
-			ADE7758_OPMODE,
-			&val);
+	ret = ade7758_spi_read_reg_8(dev, ADE7758_OPMODE, &val);
+	if (ret < 0) {
+		dev_err(dev, "Failed to read opmode reg\n");
+		return ret;
+	}
 	val |= 1 << 6; /* Software Chip Reset */
-	ret = ade7758_spi_write_reg_8(dev,
-			ADE7758_OPMODE,
-			val);
-
+	ret = ade7758_spi_write_reg_8(dev, ADE7758_OPMODE, val);
+	if (ret < 0)
+		dev_err(dev, "Failed to write opmode reg\n");
 	return ret;
 }
 
@@ -444,14 +445,15 @@ static int ade7758_stop_device(struct device *dev)
 	int ret;
 	u8 val;
 
-	ade7758_spi_read_reg_8(dev,
-			ADE7758_OPMODE,
-			&val);
+	ret = ade7758_spi_read_reg_8(dev, ADE7758_OPMODE, &val);
+	if (ret < 0) {
+		dev_err(dev, "Failed to read opmode reg\n");
+		return ret;
+	}
 	val |= 7 << 3;  /* ADE7758 powered down */
-	ret = ade7758_spi_write_reg_8(dev,
-			ADE7758_OPMODE,
-			val);
-
+	ret = ade7758_spi_write_reg_8(dev, ADE7758_OPMODE, val);
+	if (ret < 0)
+		dev_err(dev, "Failed to write opmode reg\n");
 	return ret;
 }
 
@@ -483,7 +485,7 @@ static ssize_t ade7758_read_frequency(struct device *dev,
 		struct device_attribute *attr,
 		char *buf)
 {
-	int ret, len = 0;
+	int ret;
 	u8 t;
 	int sps;
 
@@ -496,8 +498,7 @@ static ssize_t ade7758_read_frequency(struct device *dev,
 	t = (t >> 5) & 0x3;
 	sps = 26040 / (1 << t);
 
-	len = sprintf(buf, "%d SPS\n", sps);
-	return len;
+	return sprintf(buf, "%d SPS\n", sps);
 }
 
 static ssize_t ade7758_write_frequency(struct device *dev,
@@ -832,7 +833,7 @@ static int ade7758_probe(struct spi_device *spi)
 	if (!st->rx)
 		return -ENOMEM;
 	st->tx = kcalloc(ADE7758_MAX_TX, sizeof(*st->tx), GFP_KERNEL);
-	if (st->tx == NULL) {
+	if (!st->tx) {
 		ret = -ENOMEM;
 		goto error_free_rx;
 	}
diff --git a/drivers/staging/iio/meter/ade7758_ring.c b/drivers/staging/iio/meter/ade7758_ring.c
index 3792b5761645..9a24e0226f8b 100644
--- a/drivers/staging/iio/meter/ade7758_ring.c
+++ b/drivers/staging/iio/meter/ade7758_ring.c
@@ -119,10 +119,8 @@ int ade7758_configure_ring(struct iio_dev *indio_dev)
 	int ret = 0;
 
 	buffer = iio_kfifo_allocate();
-	if (!buffer) {
-		ret = -ENOMEM;
-		return ret;
-	}
+	if (!buffer)
+		return -ENOMEM;
 
 	iio_device_attach_buffer(indio_dev, buffer);
 
@@ -134,7 +132,7 @@ int ade7758_configure_ring(struct iio_dev *indio_dev)
 						 indio_dev,
 						 "ade7759_consumer%d",
 						 indio_dev->id);
-	if (indio_dev->pollfunc == NULL) {
+	if (!indio_dev->pollfunc) {
 		ret = -ENOMEM;
 		goto error_iio_kfifo_free;
 	}
diff --git a/drivers/staging/iio/meter/ade7758_trigger.c b/drivers/staging/iio/meter/ade7758_trigger.c
index 6f45ce0478d7..5b35a7f08f4f 100644
--- a/drivers/staging/iio/meter/ade7758_trigger.c
+++ b/drivers/staging/iio/meter/ade7758_trigger.c
@@ -66,7 +66,7 @@ int ade7758_probe_trigger(struct iio_dev *indio_dev)
 	st->trig = iio_trigger_alloc("%s-dev%d",
 					spi_get_device_id(st->us)->name,
 					indio_dev->id);
-	if (st->trig == NULL) {
+	if (!st->trig) {
 		ret = -ENOMEM;
 		goto error_ret;
 	}
diff --git a/drivers/staging/iio/meter/ade7759.c b/drivers/staging/iio/meter/ade7759.c
index b0c7dbc8a428..dbceda1e67ea 100644
--- a/drivers/staging/iio/meter/ade7759.c
+++ b/drivers/staging/iio/meter/ade7759.c
@@ -218,15 +218,16 @@ static int ade7759_reset(struct device *dev)
 	int ret;
 	u16 val;
 
-	ade7759_spi_read_reg_16(dev,
+	ret = ade7759_spi_read_reg_16(dev,
 			ADE7759_MODE,
 			&val);
+	if (ret < 0)
+		return ret;
+
 	val |= 1 << 6; /* Software Chip Reset */
-	ret = ade7759_spi_write_reg_16(dev,
+	return ade7759_spi_write_reg_16(dev,
 			ADE7759_MODE,
 			val);
-
-	return ret;
 }
 
 static IIO_DEV_ATTR_AENERGY(ade7759_read_40bit, ADE7759_AENERGY);
@@ -301,11 +302,18 @@ error_ret:
 /* Power down the device */
 static int ade7759_stop_device(struct device *dev)
 {
+	int ret;
 	u16 val;
 
-	ade7759_spi_read_reg_16(dev,
+	ret = ade7759_spi_read_reg_16(dev,
 			ADE7759_MODE,
 			&val);
+	if (ret < 0) {
+		dev_err(dev, "unable to power down the device, error: %d\n",
+			ret);
+		return ret;
+	}
+
 	val |= 1 << 4;  /* AD converters can be turned off */
 
 	return ade7759_spi_write_reg_16(dev, ADE7759_MODE, val);
@@ -374,7 +382,7 @@ static ssize_t ade7759_write_frequency(struct device *dev,
 
 	mutex_lock(&indio_dev->mlock);
 
-	t = (27900 / val);
+	t = 27900 / val;
 	if (t > 0)
 		t--;
 
@@ -465,11 +473,7 @@ static int ade7759_probe(struct spi_device *spi)
 	if (ret)
 		return ret;
 
-	ret = iio_device_register(indio_dev);
-	if (ret)
-		return ret;
-
-	return 0;
+	return iio_device_register(indio_dev);
 }
 
 /* fixme, confirm ordering in this function */
diff --git a/drivers/staging/iio/meter/ade7854-i2c.c b/drivers/staging/iio/meter/ade7854-i2c.c
index 5b33c7f1aa91..07cfe28b24e2 100644
--- a/drivers/staging/iio/meter/ade7854-i2c.c
+++ b/drivers/staging/iio/meter/ade7854-i2c.c
@@ -16,8 +16,8 @@
 #include "ade7854.h"
 
 static int ade7854_i2c_write_reg_8(struct device *dev,
-		u16 reg_address,
-		u8 value)
+				   u16 reg_address,
+				   u8 value)
 {
 	int ret;
 	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
@@ -35,8 +35,8 @@ static int ade7854_i2c_write_reg_8(struct device *dev,
 }
 
 static int ade7854_i2c_write_reg_16(struct device *dev,
-		u16 reg_address,
-		u16 value)
+				    u16 reg_address,
+				    u16 value)
 {
 	int ret;
 	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
@@ -55,8 +55,8 @@ static int ade7854_i2c_write_reg_16(struct device *dev,
 }
 
 static int ade7854_i2c_write_reg_24(struct device *dev,
-		u16 reg_address,
-		u32 value)
+				    u16 reg_address,
+				    u32 value)
 {
 	int ret;
 	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
@@ -76,8 +76,8 @@ static int ade7854_i2c_write_reg_24(struct device *dev,
 }
 
 static int ade7854_i2c_write_reg_32(struct device *dev,
-		u16 reg_address,
-		u32 value)
+				    u16 reg_address,
+				    u32 value)
 {
 	int ret;
 	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
@@ -98,8 +98,8 @@ static int ade7854_i2c_write_reg_32(struct device *dev,
 }
 
 static int ade7854_i2c_read_reg_8(struct device *dev,
-		u16 reg_address,
-		u8 *val)
+				  u16 reg_address,
+				  u8 *val)
 {
 	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ade7854_state *st = iio_priv(indio_dev);
@@ -124,8 +124,8 @@ out:
 }
 
 static int ade7854_i2c_read_reg_16(struct device *dev,
-		u16 reg_address,
-		u16 *val)
+				   u16 reg_address,
+				   u16 *val)
 {
 	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ade7854_state *st = iio_priv(indio_dev);
@@ -150,8 +150,8 @@ out:
 }
 
 static int ade7854_i2c_read_reg_24(struct device *dev,
-		u16 reg_address,
-		u32 *val)
+				   u16 reg_address,
+				   u32 *val)
 {
 	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ade7854_state *st = iio_priv(indio_dev);
@@ -176,8 +176,8 @@ out:
 }
 
 static int ade7854_i2c_read_reg_32(struct device *dev,
-		u16 reg_address,
-		u32 *val)
+				   u16 reg_address,
+				   u32 *val)
 {
 	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ade7854_state *st = iio_priv(indio_dev);
@@ -195,21 +195,21 @@ static int ade7854_i2c_read_reg_32(struct device *dev,
 	if (ret)
 		goto out;
 
-	*val = (st->rx[0] << 24) | (st->rx[1] << 16) | (st->rx[2] << 8) | st->rx[3];
+	*val = (st->rx[0] << 24) | (st->rx[1] << 16) |
+		(st->rx[2] << 8) | st->rx[3];
 out:
 	mutex_unlock(&st->buf_lock);
 	return ret;
 }
 
 static int ade7854_i2c_probe(struct i2c_client *client,
-		const struct i2c_device_id *id)
+			     const struct i2c_device_id *id)
 {
-	int ret;
 	struct ade7854_state *st;
 	struct iio_dev *indio_dev;
 
 	indio_dev = devm_iio_device_alloc(&client->dev, sizeof(*st));
-	if (indio_dev == NULL)
+	if (!indio_dev)
 		return -ENOMEM;
 	st = iio_priv(indio_dev);
 	i2c_set_clientdata(client, indio_dev);
@@ -224,9 +224,7 @@ static int ade7854_i2c_probe(struct i2c_client *client,
 	st->i2c = client;
 	st->irq = client->irq;
 
-	ret = ade7854_probe(indio_dev, &client->dev);
-
-	return ret;
+	return ade7854_probe(indio_dev, &client->dev);
 }
 
 static int ade7854_i2c_remove(struct i2c_client *client)
diff --git a/drivers/staging/iio/meter/ade7854-spi.c b/drivers/staging/iio/meter/ade7854-spi.c
index 94f73bbbc0fd..9b255a5f62c3 100644
--- a/drivers/staging/iio/meter/ade7854-spi.c
+++ b/drivers/staging/iio/meter/ade7854-spi.c
@@ -274,7 +274,6 @@ error_ret:
 
 static int ade7854_spi_probe(struct spi_device *spi)
 {
-	int ret;
 	struct ade7854_state *st;
 	struct iio_dev *indio_dev;
 
@@ -294,10 +293,7 @@ static int ade7854_spi_probe(struct spi_device *spi)
 	st->irq = spi->irq;
 	st->spi = spi;
 
-
-	ret = ade7854_probe(indio_dev, &spi->dev);
-
-	return ret;
+	return ade7854_probe(indio_dev, &spi->dev);
 }
 
 static int ade7854_spi_remove(struct spi_device *spi)
diff --git a/drivers/staging/iio/meter/meter.h b/drivers/staging/iio/meter/meter.h
index 8f0de02839b7..dfba510f29be 100644
--- a/drivers/staging/iio/meter/meter.h
+++ b/drivers/staging/iio/meter/meter.h
@@ -1,3 +1,6 @@
+#ifndef _METER_H
+#define _METER_H
+
 #include <linux/iio/sysfs.h>
 
 /* metering ic types of attribute */
@@ -394,3 +397,4 @@
 #define IIO_EVENT_ATTR_VPKLVL_EXC(_evlist, _show, _store, _mask) \
 	IIO_EVENT_ATTR_SH(vpklvl_exc, _evlist, _show, _store, _mask)
 
+#endif /* _METER_H */
diff --git a/drivers/staging/iio/resolver/ad2s1200.c b/drivers/staging/iio/resolver/ad2s1200.c
index 017d2f8379b7..c17893b4918c 100644
--- a/drivers/staging/iio/resolver/ad2s1200.c
+++ b/drivers/staging/iio/resolver/ad2s1200.c
@@ -18,6 +18,7 @@
 #include <linux/delay.h>
 #include <linux/gpio.h>
 #include <linux/module.h>
+#include <linux/bitops.h>
 
 #include <linux/iio/iio.h>
 #include <linux/iio/sysfs.h>
@@ -68,7 +69,7 @@ static int ad2s1200_read_raw(struct iio_dev *indio_dev,
 		break;
 	case IIO_ANGL_VEL:
 		vel = (((s16)(st->rx[0])) << 4) | ((st->rx[1] & 0xF0) >> 4);
-		vel = (vel << 4) >> 4;
+		vel = sign_extend32(vel, 11);
 		*val = vel;
 		break;
 	default:
diff --git a/drivers/staging/iio/resolver/ad2s1210.c b/drivers/staging/iio/resolver/ad2s1210.c
index b4c14ba5fdee..7bc3e4a73834 100644
--- a/drivers/staging/iio/resolver/ad2s1210.c
+++ b/drivers/staging/iio/resolver/ad2s1210.c
@@ -151,7 +151,7 @@ int ad2s1210_update_frequency_control_word(struct ad2s1210_state *st)
 
 	fcw = (unsigned char)(st->fexcit * (1 << 15) / st->fclkin);
 	if (fcw < AD2S1210_MIN_FCW || fcw > AD2S1210_MAX_FCW) {
-		pr_err("ad2s1210: FCW out of range\n");
+		dev_err(&st->sdev->dev, "ad2s1210: FCW out of range\n");
 		return -ERANGE;
 	}
 
@@ -198,7 +198,7 @@ static ssize_t ad2s1210_show_fclkin(struct device *dev,
 {
 	struct ad2s1210_state *st = iio_priv(dev_to_iio_dev(dev));
 
-	return sprintf(buf, "%d\n", st->fclkin);
+	return sprintf(buf, "%u\n", st->fclkin);
 }
 
 static ssize_t ad2s1210_store_fclkin(struct device *dev,
@@ -214,7 +214,7 @@ static ssize_t ad2s1210_store_fclkin(struct device *dev,
 	if (ret)
 		return ret;
 	if (fclkin < AD2S1210_MIN_CLKIN || fclkin > AD2S1210_MAX_CLKIN) {
-		pr_err("ad2s1210: fclkin out of range\n");
+		dev_err(dev, "ad2s1210: fclkin out of range\n");
 		return -EINVAL;
 	}
 
@@ -237,7 +237,7 @@ static ssize_t ad2s1210_show_fexcit(struct device *dev,
 {
 	struct ad2s1210_state *st = iio_priv(dev_to_iio_dev(dev));
 
-	return sprintf(buf, "%d\n", st->fexcit);
+	return sprintf(buf, "%u\n", st->fexcit);
 }
 
 static ssize_t ad2s1210_store_fexcit(struct device *dev,
@@ -252,7 +252,8 @@ static ssize_t ad2s1210_store_fexcit(struct device *dev,
 	if (ret < 0)
 		return ret;
 	if (fexcit < AD2S1210_MIN_EXCIT || fexcit > AD2S1210_MAX_EXCIT) {
-		pr_err("ad2s1210: excitation frequency out of range\n");
+		dev_err(dev,
+			"ad2s1210: excitation frequency out of range\n");
 		return -EINVAL;
 	}
 	mutex_lock(&st->lock);
@@ -307,7 +308,8 @@ static ssize_t ad2s1210_store_control(struct device *dev,
 		goto error_ret;
 	if (ret & AD2S1210_MSB_IS_HIGH) {
 		ret = -EIO;
-		pr_err("ad2s1210: write control register fail\n");
+		dev_err(dev,
+			"ad2s1210: write control register fail\n");
 		goto error_ret;
 	}
 	st->resolution
@@ -315,7 +317,7 @@ static ssize_t ad2s1210_store_control(struct device *dev,
 	if (st->pdata->gpioin) {
 		data = ad2s1210_read_resolution_pin(st);
 		if (data != st->resolution)
-			pr_warn("ad2s1210: resolution settings not match\n");
+			dev_warn(dev, "ad2s1210: resolution settings not match\n");
 	} else
 		ad2s1210_set_resolution_pin(st);
 
@@ -346,7 +348,7 @@ static ssize_t ad2s1210_store_resolution(struct device *dev,
 
 	ret = kstrtou8(buf, 10, &udata);
 	if (ret || udata < 10 || udata > 16) {
-		pr_err("ad2s1210: resolution out of range\n");
+		dev_err(dev, "ad2s1210: resolution out of range\n");
 		return -EINVAL;
 	}
 	mutex_lock(&st->lock);
@@ -368,7 +370,7 @@ static ssize_t ad2s1210_store_resolution(struct device *dev,
 	data = ret;
 	if (data & AD2S1210_MSB_IS_HIGH) {
 		ret = -EIO;
-		pr_err("ad2s1210: setting resolution fail\n");
+		dev_err(dev, "ad2s1210: setting resolution fail\n");
 		goto error_ret;
 	}
 	st->resolution
@@ -376,7 +378,7 @@ static ssize_t ad2s1210_store_resolution(struct device *dev,
 	if (st->pdata->gpioin) {
 		data = ad2s1210_read_resolution_pin(st);
 		if (data != st->resolution)
-			pr_warn("ad2s1210: resolution settings not match\n");
+			dev_warn(dev, "ad2s1210: resolution settings not match\n");
 	} else
 		ad2s1210_set_resolution_pin(st);
 	ret = len;
diff --git a/drivers/staging/iio/resolver/ad2s1210.h b/drivers/staging/iio/resolver/ad2s1210.h
index aec0bdca16a4..c7158f6e61c2 100644
--- a/drivers/staging/iio/resolver/ad2s1210.h
+++ b/drivers/staging/iio/resolver/ad2s1210.h
@@ -8,6 +8,8 @@
  * it under the terms of the GNU General Public License version 2 as
  * published by the Free Software Foundation.
  */
+#ifndef _AD2S1210_H
+#define _AD2S1210_H
 
 struct ad2s1210_platform_data {
 	unsigned sample;
@@ -15,3 +17,4 @@ struct ad2s1210_platform_data {
 	unsigned res[2];
 	bool gpioin;
 };
+#endif /* _AD2S1210_H */
diff --git a/drivers/staging/iio/trigger/iio-trig-bfin-timer.c b/drivers/staging/iio/trigger/iio-trig-bfin-timer.c
index 2af8d677d4ed..3c1c8c6c4a6c 100644
--- a/drivers/staging/iio/trigger/iio-trig-bfin-timer.c
+++ b/drivers/staging/iio/trigger/iio-trig-bfin-timer.c
@@ -183,7 +183,7 @@ static int iio_bfin_tmr_trigger_probe(struct platform_device *pdev)
 	int ret;
 
 	st = devm_kzalloc(&pdev->dev, sizeof(*st), GFP_KERNEL);
-	if (st == NULL)
+	if (!st)
 		return -ENOMEM;
 
 	st->irq = platform_get_irq(pdev, 0);
diff --git a/drivers/staging/iio/trigger/iio-trig-periodic-rtc.c b/drivers/staging/iio/trigger/iio-trig-periodic-rtc.c
index a24caf73ae0b..0c1976ddee74 100644
--- a/drivers/staging/iio/trigger/iio-trig-periodic-rtc.c
+++ b/drivers/staging/iio/trigger/iio-trig-periodic-rtc.c
@@ -24,7 +24,7 @@ static DEFINE_MUTEX(iio_prtc_trigger_list_lock);
 
 struct iio_prtc_trigger_info {
 	struct rtc_device *rtc;
-	int frequency;
+	unsigned int frequency;
 	struct rtc_task task;
 	bool state;
 };
@@ -36,7 +36,7 @@ static int iio_trig_periodic_rtc_set_state(struct iio_trigger *trig, bool state)
 
 	if (trig_info->frequency == 0 && state)
 		return -EINVAL;
-	dev_dbg(&trig_info->rtc->dev, "trigger frequency is %d\n",
+	dev_dbg(&trig_info->rtc->dev, "trigger frequency is %u\n",
 			trig_info->frequency);
 	ret = rtc_irq_set_state(trig_info->rtc, &trig_info->task, state);
 	if (ret == 0)
@@ -62,10 +62,10 @@ static ssize_t iio_trig_periodic_write_freq(struct device *dev,
 {
 	struct iio_trigger *trig = to_iio_trigger(dev);
 	struct iio_prtc_trigger_info *trig_info = iio_trigger_get_drvdata(trig);
-	int val;
+	unsigned int val;
 	int ret;
 
-	ret = kstrtoint(buf, 10, &val);
+	ret = kstrtouint(buf, 10, &val);
 	if (ret)
 		goto error_ret;
 
@@ -74,10 +74,8 @@ static ssize_t iio_trig_periodic_write_freq(struct device *dev,
 		if (ret == 0 && trig_info->state && trig_info->frequency == 0)
 			ret = rtc_irq_set_state(trig_info->rtc,
 						&trig_info->task, 1);
-	} else if (val == 0) {
-		ret = rtc_irq_set_state(trig_info->rtc, &trig_info->task, 0);
 	} else
-		ret = -EINVAL;
+		ret = rtc_irq_set_state(trig_info->rtc, &trig_info->task, 0);
 	if (ret)
 		goto error_ret;
 
@@ -126,7 +124,7 @@ static int iio_trig_periodic_rtc_probe(struct platform_device *dev)
 	int i, ret;
 
 	for (i = 0;; i++) {
-		if (pdata[i] == NULL)
+		if (!pdata[i])
 			break;
 		trig = iio_trigger_alloc("periodic%s", pdata[i]);
 		if (!trig) {
@@ -144,7 +142,7 @@ static int iio_trig_periodic_rtc_probe(struct platform_device *dev)
 		trig->ops = &iio_prtc_trigger_ops;
 		/* RTC access */
 		trig_info->rtc = rtc_class_open(pdata[i]);
-		if (trig_info->rtc == NULL) {
+		if (!trig_info->rtc) {
 			ret = -EINVAL;
 			goto error_free_trig_info;
 		}
diff --git a/drivers/staging/lustre/include/linux/libcfs/libcfs_debug.h b/drivers/staging/lustre/include/linux/libcfs/libcfs_debug.h
index 2e5a9e5965b1..8251ac932e37 100644
--- a/drivers/staging/lustre/include/linux/libcfs/libcfs_debug.h
+++ b/drivers/staging/lustre/include/linux/libcfs/libcfs_debug.h
@@ -196,7 +196,7 @@ do {								\
 	       .msg_fn     = __func__,				\
 	       .msg_line   = __LINE__,				\
 	       .msg_cdls   = (cdls)	 };			\
-	dataname.msg_mask   = (mask);
+	dataname.msg_mask   = (mask)
 
 /**
  * Filters out logging messages based on mask and subsystem.
@@ -244,12 +244,12 @@ do {									\
 
 int libcfs_debug_msg(struct libcfs_debug_msg_data *msgdata,
 			    const char *format1, ...)
-	__attribute__ ((format (printf, 2, 3)));
+	__printf(2, 3);
 
 int libcfs_debug_vmsg2(struct libcfs_debug_msg_data *msgdata,
 			      const char *format1,
 			      va_list args, const char *format2, ...)
-	__attribute__ ((format (printf, 4, 5)));
+	__printf(4, 5);
 
 /* other external symbols that tracefile provides: */
 int cfs_trace_copyin_string(char *knl_buffer, int knl_buffer_nob,
diff --git a/drivers/staging/lustre/include/linux/libcfs/libcfs_hash.h b/drivers/staging/lustre/include/linux/libcfs/libcfs_hash.h
index 808e49411a30..c40814591189 100644
--- a/drivers/staging/lustre/include/linux/libcfs/libcfs_hash.h
+++ b/drivers/staging/lustre/include/linux/libcfs/libcfs_hash.h
@@ -469,7 +469,7 @@ cfs_hash_key(struct cfs_hash *hs, struct hlist_node *hnode)
 static inline void
 cfs_hash_keycpy(struct cfs_hash *hs, struct hlist_node *hnode, void *key)
 {
-        if (hs->hs_ops->hs_keycpy)
+	if (hs->hs_ops->hs_keycpy)
 		hs->hs_ops->hs_keycpy(hnode, key);
 }
 
@@ -785,8 +785,8 @@ static inline void __cfs_hash_set_theta(struct cfs_hash *hs, int min, int max)
 
 /* Generic debug formatting routines mainly for proc handler */
 struct seq_file;
-int cfs_hash_debug_header(struct seq_file *m);
-int cfs_hash_debug_str(struct cfs_hash *hs, struct seq_file *m);
+void cfs_hash_debug_header(struct seq_file *m);
+void cfs_hash_debug_str(struct cfs_hash *hs, struct seq_file *m);
 
 /*
  * Generic djb2 hash algorithm for character arrays.
diff --git a/drivers/staging/lustre/include/linux/libcfs/libcfs_kernelcomm.h b/drivers/staging/lustre/include/linux/libcfs/libcfs_kernelcomm.h
index f19a121a37cd..a989d2666230 100644
--- a/drivers/staging/lustre/include/linux/libcfs/libcfs_kernelcomm.h
+++ b/drivers/staging/lustre/include/linux/libcfs/libcfs_kernelcomm.h
@@ -58,7 +58,7 @@ struct kuc_hdr {
 	__u8  kuc_flags;
 	__u16 kuc_msgtype;    /* Message type or opcode, transport-specific */
 	__u16 kuc_msglen;     /* Including header */
-} __attribute__((aligned(sizeof(__u64))));
+} __aligned(sizeof(__u64));
 
 #define KUC_CHANGELOG_MSG_MAXSIZE (sizeof(struct kuc_hdr)+CR_MAXSIZE)
 
@@ -107,7 +107,7 @@ typedef struct lustre_kernelcomm {
 	__u32 lk_group;
 	__u32 lk_data;
 	__u32 lk_flags;
-} __attribute__((packed)) lustre_kernelcomm;
+} __packed lustre_kernelcomm;
 
 /* Userspace methods */
 int libcfs_ukuc_start(lustre_kernelcomm *l, int groups);
diff --git a/drivers/staging/lustre/include/linux/libcfs/libcfs_private.h b/drivers/staging/lustre/include/linux/libcfs/libcfs_private.h
index 3d86fb5b5481..fef882530455 100644
--- a/drivers/staging/lustre/include/linux/libcfs/libcfs_private.h
+++ b/drivers/staging/lustre/include/linux/libcfs/libcfs_private.h
@@ -335,8 +335,8 @@ do {							    \
 #define LASSERT_ATOMIC_ZERO(a)		  LASSERT_ATOMIC_EQ(a, 0)
 #define LASSERT_ATOMIC_POS(a)		   LASSERT_ATOMIC_GT(a, 0)
 
-#define CFS_ALLOC_PTR(ptr)      LIBCFS_ALLOC(ptr, sizeof(*(ptr)));
-#define CFS_FREE_PTR(ptr)       LIBCFS_FREE(ptr, sizeof(*(ptr)));
+#define CFS_ALLOC_PTR(ptr)      LIBCFS_ALLOC(ptr, sizeof(*(ptr)))
+#define CFS_FREE_PTR(ptr)       LIBCFS_FREE(ptr, sizeof(*(ptr)))
 
 /*
  * percpu partition lock
diff --git a/drivers/staging/lustre/lnet/klnds/o2iblnd/o2iblnd.c b/drivers/staging/lustre/lnet/klnds/o2iblnd/o2iblnd.c
index 651016919669..3bad441de8dc 100644
--- a/drivers/staging/lustre/lnet/klnds/o2iblnd/o2iblnd.c
+++ b/drivers/staging/lustre/lnet/klnds/o2iblnd/o2iblnd.c
@@ -53,8 +53,7 @@ static lnd_t the_o2iblnd = {
 
 kib_data_t	      kiblnd_data;
 
-static __u32
-kiblnd_cksum(void *ptr, int nob)
+static __u32 kiblnd_cksum(void *ptr, int nob)
 {
 	char  *c  = ptr;
 	__u32  sum = 0;
@@ -66,8 +65,7 @@ kiblnd_cksum(void *ptr, int nob)
 	return (sum == 0) ? 1 : sum;
 }
 
-static char *
-kiblnd_msgtype2str(int type)
+static char *kiblnd_msgtype2str(int type)
 {
 	switch (type) {
 	case IBLND_MSG_CONNREQ:
@@ -105,8 +103,7 @@ kiblnd_msgtype2str(int type)
 	}
 }
 
-static int
-kiblnd_msgtype2size(int type)
+static int kiblnd_msgtype2size(int type)
 {
 	const int hdr_size = offsetof(kib_msg_t, ibm_u);
 
@@ -139,15 +136,14 @@ kiblnd_msgtype2size(int type)
 	}
 }
 
-static int
-kiblnd_unpack_rd(kib_msg_t *msg, int flip)
+static int kiblnd_unpack_rd(kib_msg_t *msg, int flip)
 {
 	kib_rdma_desc_t   *rd;
 	int		nob;
 	int		n;
 	int		i;
 
-	LASSERT (msg->ibm_type == IBLND_MSG_GET_REQ ||
+	LASSERT(msg->ibm_type == IBLND_MSG_GET_REQ ||
 		 msg->ibm_type == IBLND_MSG_PUT_ACK);
 
 	rd = msg->ibm_type == IBLND_MSG_GET_REQ ?
@@ -167,7 +163,7 @@ kiblnd_unpack_rd(kib_msg_t *msg, int flip)
 		return 1;
 	}
 
-	nob = offsetof (kib_msg_t, ibm_u) +
+	nob = offsetof(kib_msg_t, ibm_u) +
 	      kiblnd_rd_msg_size(rd, msg->ibm_type, n);
 
 	if (msg->ibm_nob < nob) {
@@ -187,9 +183,8 @@ kiblnd_unpack_rd(kib_msg_t *msg, int flip)
 	return 0;
 }
 
-void
-kiblnd_pack_msg (lnet_ni_t *ni, kib_msg_t *msg, int version,
-		 int credits, lnet_nid_t dstnid, __u64 dststamp)
+void kiblnd_pack_msg(lnet_ni_t *ni, kib_msg_t *msg, int version,
+		     int credits, lnet_nid_t dstnid, __u64 dststamp)
 {
 	kib_net_t *net = ni->ni_data;
 
@@ -212,8 +207,7 @@ kiblnd_pack_msg (lnet_ni_t *ni, kib_msg_t *msg, int version,
 	}
 }
 
-int
-kiblnd_unpack_msg(kib_msg_t *msg, int nob)
+int kiblnd_unpack_msg(kib_msg_t *msg, int nob)
 {
 	const int hdr_size = offsetof(kib_msg_t, ibm_u);
 	__u32     msg_cksum;
@@ -269,8 +263,8 @@ kiblnd_unpack_msg(kib_msg_t *msg, int nob)
 	if (flip) {
 		/* leave magic unflipped as a clue to peer endianness */
 		msg->ibm_version = version;
-		CLASSERT (sizeof(msg->ibm_type) == 1);
-		CLASSERT (sizeof(msg->ibm_credits) == 1);
+		CLASSERT(sizeof(msg->ibm_type) == 1);
+		CLASSERT(sizeof(msg->ibm_credits) == 1);
 		msg->ibm_nob     = msg_nob;
 		__swab64s(&msg->ibm_srcnid);
 		__swab64s(&msg->ibm_srcstamp);
@@ -324,8 +318,7 @@ kiblnd_unpack_msg(kib_msg_t *msg, int nob)
 	return 0;
 }
 
-int
-kiblnd_create_peer(lnet_ni_t *ni, kib_peer_t **peerp, lnet_nid_t nid)
+int kiblnd_create_peer(lnet_ni_t *ni, kib_peer_t **peerp, lnet_nid_t nid)
 {
 	kib_peer_t	*peer;
 	kib_net_t	*net = ni->ni_data;
@@ -356,7 +349,7 @@ kiblnd_create_peer(lnet_ni_t *ni, kib_peer_t **peerp, lnet_nid_t nid)
 	write_lock_irqsave(&kiblnd_data.kib_global_lock, flags);
 
 	/* always called with a ref on ni, which prevents ni being shutdown */
-	LASSERT (net->ibn_shutdown == 0);
+	LASSERT(net->ibn_shutdown == 0);
 
 	/* npeers only grows with the global lock held */
 	atomic_inc(&net->ibn_npeers);
@@ -367,18 +360,17 @@ kiblnd_create_peer(lnet_ni_t *ni, kib_peer_t **peerp, lnet_nid_t nid)
 	return 0;
 }
 
-void
-kiblnd_destroy_peer (kib_peer_t *peer)
+void kiblnd_destroy_peer(kib_peer_t *peer)
 {
 	kib_net_t *net = peer->ibp_ni->ni_data;
 
-	LASSERT (net != NULL);
-	LASSERT (atomic_read(&peer->ibp_refcount) == 0);
-	LASSERT (!kiblnd_peer_active(peer));
-	LASSERT (peer->ibp_connecting == 0);
-	LASSERT (peer->ibp_accepting == 0);
-	LASSERT (list_empty(&peer->ibp_conns));
-	LASSERT (list_empty(&peer->ibp_tx_queue));
+	LASSERT(net != NULL);
+	LASSERT(atomic_read(&peer->ibp_refcount) == 0);
+	LASSERT(!kiblnd_peer_active(peer));
+	LASSERT(peer->ibp_connecting == 0);
+	LASSERT(peer->ibp_accepting == 0);
+	LASSERT(list_empty(&peer->ibp_conns));
+	LASSERT(list_empty(&peer->ibp_tx_queue));
 
 	LIBCFS_FREE(peer, sizeof(*peer));
 
@@ -389,8 +381,7 @@ kiblnd_destroy_peer (kib_peer_t *peer)
 	atomic_dec(&net->ibn_npeers);
 }
 
-kib_peer_t *
-kiblnd_find_peer_locked (lnet_nid_t nid)
+kib_peer_t *kiblnd_find_peer_locked(lnet_nid_t nid)
 {
 	/* the caller is responsible for accounting the additional reference
 	 * that this creates */
@@ -398,11 +389,11 @@ kiblnd_find_peer_locked (lnet_nid_t nid)
 	struct list_head       *tmp;
 	kib_peer_t       *peer;
 
-	list_for_each (tmp, peer_list) {
+	list_for_each(tmp, peer_list) {
 
 		peer = list_entry(tmp, kib_peer_t, ibp_list);
 
-		LASSERT (peer->ibp_connecting > 0 || /* creating conns */
+		LASSERT(peer->ibp_connecting > 0 || /* creating conns */
 			 peer->ibp_accepting > 0 ||
 			 !list_empty(&peer->ibp_conns));  /* active conn */
 
@@ -418,20 +409,18 @@ kiblnd_find_peer_locked (lnet_nid_t nid)
 	return NULL;
 }
 
-void
-kiblnd_unlink_peer_locked (kib_peer_t *peer)
+void kiblnd_unlink_peer_locked(kib_peer_t *peer)
 {
-	LASSERT (list_empty(&peer->ibp_conns));
+	LASSERT(list_empty(&peer->ibp_conns));
 
-	LASSERT (kiblnd_peer_active(peer));
+	LASSERT(kiblnd_peer_active(peer));
 	list_del_init(&peer->ibp_list);
 	/* lose peerlist's ref */
 	kiblnd_peer_decref(peer);
 }
 
-static int
-kiblnd_get_peer_info(lnet_ni_t *ni, int index,
-		      lnet_nid_t *nidp, int *count)
+static int kiblnd_get_peer_info(lnet_ni_t *ni, int index,
+				lnet_nid_t *nidp, int *count)
 {
 	kib_peer_t	    *peer;
 	struct list_head	    *ptmp;
@@ -442,10 +431,10 @@ kiblnd_get_peer_info(lnet_ni_t *ni, int index,
 
 	for (i = 0; i < kiblnd_data.kib_peer_hash_size; i++) {
 
-		list_for_each (ptmp, &kiblnd_data.kib_peers[i]) {
+		list_for_each(ptmp, &kiblnd_data.kib_peers[i]) {
 
 			peer = list_entry(ptmp, kib_peer_t, ibp_list);
-			LASSERT (peer->ibp_connecting > 0 ||
+			LASSERT(peer->ibp_connecting > 0 ||
 				 peer->ibp_accepting > 0 ||
 				 !list_empty(&peer->ibp_conns));
 
@@ -468,8 +457,7 @@ kiblnd_get_peer_info(lnet_ni_t *ni, int index,
 	return -ENOENT;
 }
 
-static void
-kiblnd_del_peer_locked(kib_peer_t *peer)
+static void kiblnd_del_peer_locked(kib_peer_t *peer)
 {
 	struct list_head	   *ctmp;
 	struct list_head	   *cnxt;
@@ -478,7 +466,7 @@ kiblnd_del_peer_locked(kib_peer_t *peer)
 	if (list_empty(&peer->ibp_conns)) {
 		kiblnd_unlink_peer_locked(peer);
 	} else {
-		list_for_each_safe (ctmp, cnxt, &peer->ibp_conns) {
+		list_for_each_safe(ctmp, cnxt, &peer->ibp_conns) {
 			conn = list_entry(ctmp, kib_conn_t, ibc_list);
 
 			kiblnd_close_conn_locked(conn, 0);
@@ -489,10 +477,9 @@ kiblnd_del_peer_locked(kib_peer_t *peer)
 	 * last ref on it. */
 }
 
-static int
-kiblnd_del_peer(lnet_ni_t *ni, lnet_nid_t nid)
+static int kiblnd_del_peer(lnet_ni_t *ni, lnet_nid_t nid)
 {
-	LIST_HEAD	 (zombies);
+	LIST_HEAD(zombies);
 	struct list_head	    *ptmp;
 	struct list_head	    *pnxt;
 	kib_peer_t	    *peer;
@@ -512,9 +499,9 @@ kiblnd_del_peer(lnet_ni_t *ni, lnet_nid_t nid)
 	}
 
 	for (i = lo; i <= hi; i++) {
-		list_for_each_safe (ptmp, pnxt, &kiblnd_data.kib_peers[i]) {
+		list_for_each_safe(ptmp, pnxt, &kiblnd_data.kib_peers[i]) {
 			peer = list_entry(ptmp, kib_peer_t, ibp_list);
-			LASSERT (peer->ibp_connecting > 0 ||
+			LASSERT(peer->ibp_connecting > 0 ||
 				 peer->ibp_accepting > 0 ||
 				 !list_empty(&peer->ibp_conns));
 
@@ -525,7 +512,7 @@ kiblnd_del_peer(lnet_ni_t *ni, lnet_nid_t nid)
 				continue;
 
 			if (!list_empty(&peer->ibp_tx_queue)) {
-				LASSERT (list_empty(&peer->ibp_conns));
+				LASSERT(list_empty(&peer->ibp_conns));
 
 				list_splice_init(&peer->ibp_tx_queue,
 						     &zombies);
@@ -543,8 +530,7 @@ kiblnd_del_peer(lnet_ni_t *ni, lnet_nid_t nid)
 	return rc;
 }
 
-static kib_conn_t *
-kiblnd_get_conn_by_idx(lnet_ni_t *ni, int index)
+static kib_conn_t *kiblnd_get_conn_by_idx(lnet_ni_t *ni, int index)
 {
 	kib_peer_t	    *peer;
 	struct list_head	    *ptmp;
@@ -556,25 +542,26 @@ kiblnd_get_conn_by_idx(lnet_ni_t *ni, int index)
 	read_lock_irqsave(&kiblnd_data.kib_global_lock, flags);
 
 	for (i = 0; i < kiblnd_data.kib_peer_hash_size; i++) {
-		list_for_each (ptmp, &kiblnd_data.kib_peers[i]) {
+		list_for_each(ptmp, &kiblnd_data.kib_peers[i]) {
 
 			peer = list_entry(ptmp, kib_peer_t, ibp_list);
-			LASSERT (peer->ibp_connecting > 0 ||
+			LASSERT(peer->ibp_connecting > 0 ||
 				 peer->ibp_accepting > 0 ||
 				 !list_empty(&peer->ibp_conns));
 
 			if (peer->ibp_ni != ni)
 				continue;
 
-			list_for_each (ctmp, &peer->ibp_conns) {
+			list_for_each(ctmp, &peer->ibp_conns) {
 				if (index-- > 0)
 					continue;
 
 				conn = list_entry(ctmp, kib_conn_t,
 						      ibc_list);
 				kiblnd_conn_addref(conn);
-				read_unlock_irqrestore(&kiblnd_data.kib_global_lock,
-						       flags);
+				read_unlock_irqrestore(
+					&kiblnd_data.kib_global_lock,
+					flags);
 				return conn;
 			}
 		}
@@ -584,8 +571,7 @@ kiblnd_get_conn_by_idx(lnet_ni_t *ni, int index)
 	return NULL;
 }
 
-int
-kiblnd_translate_mtu(int value)
+int kiblnd_translate_mtu(int value)
 {
 	switch (value) {
 	default:
@@ -605,8 +591,7 @@ kiblnd_translate_mtu(int value)
 	}
 }
 
-static void
-kiblnd_setup_mtu_locked(struct rdma_cm_id *cmid)
+static void kiblnd_setup_mtu_locked(struct rdma_cm_id *cmid)
 {
 	int	   mtu;
 
@@ -615,13 +600,12 @@ kiblnd_setup_mtu_locked(struct rdma_cm_id *cmid)
 		return;
 
 	mtu = kiblnd_translate_mtu(*kiblnd_tunables.kib_ib_mtu);
-	LASSERT (mtu >= 0);
+	LASSERT(mtu >= 0);
 	if (mtu != 0)
 		cmid->route.path_rec->mtu = mtu;
 }
 
-static int
-kiblnd_get_completion_vector(kib_conn_t *conn, int cpt)
+static int kiblnd_get_completion_vector(kib_conn_t *conn, int cpt)
 {
 	cpumask_t	*mask;
 	int		vectors;
@@ -638,8 +622,8 @@ kiblnd_get_completion_vector(kib_conn_t *conn, int cpt)
 		return 0;
 
 	/* hash NID to CPU id in this partition... */
-	off = do_div(nid, cpus_weight(*mask));
-	for_each_cpu_mask(i, *mask) {
+	off = do_div(nid, cpumask_weight(mask));
+	for_each_cpu(i, mask) {
 		if (off-- == 0)
 			return i % vectors;
 	}
@@ -648,9 +632,8 @@ kiblnd_get_completion_vector(kib_conn_t *conn, int cpt)
 	return 1;
 }
 
-kib_conn_t *
-kiblnd_create_conn(kib_peer_t *peer, struct rdma_cm_id *cmid,
-		   int state, int version)
+kib_conn_t *kiblnd_create_conn(kib_peer_t *peer, struct rdma_cm_id *cmid,
+				int state, int version)
 {
 	/* CAVEAT EMPTOR:
 	 * If the new conn is created successfully it takes over the caller's
@@ -835,7 +818,7 @@ kiblnd_create_conn(kib_peer_t *peer, struct rdma_cm_id *cmid,
 	}
 
 	/* Init successful! */
-	LASSERT (state == IBLND_CONN_ACTIVE_CONNECT ||
+	LASSERT(state == IBLND_CONN_ACTIVE_CONNECT ||
 		 state == IBLND_CONN_PASSIVE_WAIT);
 	conn->ibc_state = state;
 
@@ -851,23 +834,22 @@ kiblnd_create_conn(kib_peer_t *peer, struct rdma_cm_id *cmid,
 	return NULL;
 }
 
-void
-kiblnd_destroy_conn (kib_conn_t *conn)
+void kiblnd_destroy_conn(kib_conn_t *conn)
 {
 	struct rdma_cm_id *cmid = conn->ibc_cmid;
 	kib_peer_t	*peer = conn->ibc_peer;
 	int		rc;
 
-	LASSERT (!in_interrupt());
-	LASSERT (atomic_read(&conn->ibc_refcount) == 0);
-	LASSERT (list_empty(&conn->ibc_early_rxs));
-	LASSERT (list_empty(&conn->ibc_tx_noops));
-	LASSERT (list_empty(&conn->ibc_tx_queue));
-	LASSERT (list_empty(&conn->ibc_tx_queue_rsrvd));
-	LASSERT (list_empty(&conn->ibc_tx_queue_nocred));
-	LASSERT (list_empty(&conn->ibc_active_txs));
-	LASSERT (conn->ibc_noops_posted == 0);
-	LASSERT (conn->ibc_nsends_posted == 0);
+	LASSERT(!in_interrupt());
+	LASSERT(atomic_read(&conn->ibc_refcount) == 0);
+	LASSERT(list_empty(&conn->ibc_early_rxs));
+	LASSERT(list_empty(&conn->ibc_tx_noops));
+	LASSERT(list_empty(&conn->ibc_tx_queue));
+	LASSERT(list_empty(&conn->ibc_tx_queue_rsrvd));
+	LASSERT(list_empty(&conn->ibc_tx_queue_nocred));
+	LASSERT(list_empty(&conn->ibc_active_txs));
+	LASSERT(conn->ibc_noops_posted == 0);
+	LASSERT(conn->ibc_nsends_posted == 0);
 
 	switch (conn->ibc_state) {
 	default:
@@ -876,7 +858,7 @@ kiblnd_destroy_conn (kib_conn_t *conn)
 
 	case IBLND_CONN_DISCONNECTED:
 		/* connvars should have been freed already */
-		LASSERT (conn->ibc_connvars == NULL);
+		LASSERT(conn->ibc_connvars == NULL);
 		break;
 
 	case IBLND_CONN_INIT:
@@ -898,7 +880,8 @@ kiblnd_destroy_conn (kib_conn_t *conn)
 
 	if (conn->ibc_rxs != NULL) {
 		LIBCFS_FREE(conn->ibc_rxs,
-			    IBLND_RX_MSGS(conn->ibc_version) * sizeof(kib_rx_t));
+			    IBLND_RX_MSGS(conn->ibc_version)
+			      * sizeof(kib_rx_t));
 	}
 
 	if (conn->ibc_connvars != NULL)
@@ -919,15 +902,14 @@ kiblnd_destroy_conn (kib_conn_t *conn)
 	LIBCFS_FREE(conn, sizeof(*conn));
 }
 
-int
-kiblnd_close_peer_conns_locked (kib_peer_t *peer, int why)
+int kiblnd_close_peer_conns_locked(kib_peer_t *peer, int why)
 {
 	kib_conn_t	     *conn;
 	struct list_head	     *ctmp;
 	struct list_head	     *cnxt;
 	int		     count = 0;
 
-	list_for_each_safe (ctmp, cnxt, &peer->ibp_conns) {
+	list_for_each_safe(ctmp, cnxt, &peer->ibp_conns) {
 		conn = list_entry(ctmp, kib_conn_t, ibc_list);
 
 		CDEBUG(D_NET, "Closing conn -> %s, version: %x, reason: %d\n",
@@ -941,23 +923,23 @@ kiblnd_close_peer_conns_locked (kib_peer_t *peer, int why)
 	return count;
 }
 
-int
-kiblnd_close_stale_conns_locked (kib_peer_t *peer,
-				 int version, __u64 incarnation)
+int kiblnd_close_stale_conns_locked(kib_peer_t *peer,
+				     int version, __u64 incarnation)
 {
 	kib_conn_t	     *conn;
 	struct list_head	     *ctmp;
 	struct list_head	     *cnxt;
 	int		     count = 0;
 
-	list_for_each_safe (ctmp, cnxt, &peer->ibp_conns) {
+	list_for_each_safe(ctmp, cnxt, &peer->ibp_conns) {
 		conn = list_entry(ctmp, kib_conn_t, ibc_list);
 
 		if (conn->ibc_version     == version &&
 		    conn->ibc_incarnation == incarnation)
 			continue;
 
-		CDEBUG(D_NET, "Closing stale conn -> %s version: %x, incarnation:%#llx(%x, %#llx)\n",
+		CDEBUG(D_NET,
+		       "Closing stale conn -> %s version: %x, incarnation:%#llx(%x, %#llx)\n",
 		       libcfs_nid2str(peer->ibp_nid),
 		       conn->ibc_version, conn->ibc_incarnation,
 		       version, incarnation);
@@ -969,8 +951,7 @@ kiblnd_close_stale_conns_locked (kib_peer_t *peer,
 	return count;
 }
 
-static int
-kiblnd_close_matching_conns(lnet_ni_t *ni, lnet_nid_t nid)
+static int kiblnd_close_matching_conns(lnet_ni_t *ni, lnet_nid_t nid)
 {
 	kib_peer_t	     *peer;
 	struct list_head	     *ptmp;
@@ -991,10 +972,10 @@ kiblnd_close_matching_conns(lnet_ni_t *ni, lnet_nid_t nid)
 	}
 
 	for (i = lo; i <= hi; i++) {
-		list_for_each_safe (ptmp, pnxt, &kiblnd_data.kib_peers[i]) {
+		list_for_each_safe(ptmp, pnxt, &kiblnd_data.kib_peers[i]) {
 
 			peer = list_entry(ptmp, kib_peer_t, ibp_list);
-			LASSERT (peer->ibp_connecting > 0 ||
+			LASSERT(peer->ibp_connecting > 0 ||
 				 peer->ibp_accepting > 0 ||
 				 !list_empty(&peer->ibp_conns));
 
@@ -1017,8 +998,7 @@ kiblnd_close_matching_conns(lnet_ni_t *ni, lnet_nid_t nid)
 	return (count == 0) ? -ENOENT : 0;
 }
 
-int
-kiblnd_ctl(lnet_ni_t *ni, unsigned int cmd, void *arg)
+int kiblnd_ctl(lnet_ni_t *ni, unsigned int cmd, void *arg)
 {
 	struct libcfs_ioctl_data *data = arg;
 	int		       rc = -EINVAL;
@@ -1049,7 +1029,7 @@ kiblnd_ctl(lnet_ni_t *ni, unsigned int cmd, void *arg)
 			break;
 		}
 
-		LASSERT (conn->ibc_cmid != NULL);
+		LASSERT(conn->ibc_cmid != NULL);
 		data->ioc_nid = conn->ibc_peer->ibp_nid;
 		if (conn->ibc_cmid->route.path_rec == NULL)
 			data->ioc_u32[0] = 0; /* iWarp has no path MTU */
@@ -1071,8 +1051,7 @@ kiblnd_ctl(lnet_ni_t *ni, unsigned int cmd, void *arg)
 	return rc;
 }
 
-void
-kiblnd_query (lnet_ni_t *ni, lnet_nid_t nid, unsigned long *when)
+void kiblnd_query(lnet_ni_t *ni, lnet_nid_t nid, unsigned long *when)
 {
 	unsigned long	last_alive = 0;
 	unsigned long	now = cfs_time_current();
@@ -1084,7 +1063,7 @@ kiblnd_query (lnet_ni_t *ni, lnet_nid_t nid, unsigned long *when)
 
 	peer = kiblnd_find_peer_locked(nid);
 	if (peer != NULL) {
-		LASSERT (peer->ibp_connecting > 0 || /* creating conns */
+		LASSERT(peer->ibp_connecting > 0 || /* creating conns */
 			 peer->ibp_accepting > 0 ||
 			 !list_empty(&peer->ibp_conns));  /* active conn */
 		last_alive = peer->ibp_last_alive;
@@ -1103,11 +1082,9 @@ kiblnd_query (lnet_ni_t *ni, lnet_nid_t nid, unsigned long *when)
 	CDEBUG(D_NET, "Peer %s %p, alive %ld secs ago\n",
 	       libcfs_nid2str(nid), peer,
 	       last_alive ? cfs_duration_sec(now - last_alive) : -1);
-	return;
 }
 
-void
-kiblnd_free_pages(kib_pages_t *p)
+void kiblnd_free_pages(kib_pages_t *p)
 {
 	int	npages = p->ibp_npages;
 	int	i;
@@ -1120,8 +1097,7 @@ kiblnd_free_pages(kib_pages_t *p)
 	LIBCFS_FREE(p, offsetof(kib_pages_t, ibp_pages[npages]));
 }
 
-int
-kiblnd_alloc_pages(kib_pages_t **pp, int cpt, int npages)
+int kiblnd_alloc_pages(kib_pages_t **pp, int cpt, int npages)
 {
 	kib_pages_t	*p;
 	int		i;
@@ -1151,19 +1127,18 @@ kiblnd_alloc_pages(kib_pages_t **pp, int cpt, int npages)
 	return 0;
 }
 
-void
-kiblnd_unmap_rx_descs(kib_conn_t *conn)
+void kiblnd_unmap_rx_descs(kib_conn_t *conn)
 {
 	kib_rx_t *rx;
 	int       i;
 
-	LASSERT (conn->ibc_rxs != NULL);
-	LASSERT (conn->ibc_hdev != NULL);
+	LASSERT(conn->ibc_rxs != NULL);
+	LASSERT(conn->ibc_hdev != NULL);
 
 	for (i = 0; i < IBLND_RX_MSGS(conn->ibc_version); i++) {
 		rx = &conn->ibc_rxs[i];
 
-		LASSERT (rx->rx_nob >= 0); /* not posted */
+		LASSERT(rx->rx_nob >= 0); /* not posted */
 
 		kiblnd_dma_unmap_single(conn->ibc_hdev->ibh_ibdev,
 					KIBLND_UNMAP_ADDR(rx, rx_msgunmap,
@@ -1176,8 +1151,7 @@ kiblnd_unmap_rx_descs(kib_conn_t *conn)
 	conn->ibc_rx_pages = NULL;
 }
 
-void
-kiblnd_map_rx_descs(kib_conn_t *conn)
+void kiblnd_map_rx_descs(kib_conn_t *conn)
 {
 	kib_rx_t       *rx;
 	struct page    *pg;
@@ -1194,9 +1168,10 @@ kiblnd_map_rx_descs(kib_conn_t *conn)
 		rx->rx_msg = (kib_msg_t *)(((char *)page_address(pg)) + pg_off);
 
 		rx->rx_msgaddr = kiblnd_dma_map_single(conn->ibc_hdev->ibh_ibdev,
-						       rx->rx_msg, IBLND_MSG_SIZE,
+						       rx->rx_msg,
+						       IBLND_MSG_SIZE,
 						       DMA_FROM_DEVICE);
-		LASSERT (!kiblnd_dma_mapping_error(conn->ibc_hdev->ibh_ibdev,
+		LASSERT(!kiblnd_dma_mapping_error(conn->ibc_hdev->ibh_ibdev,
 						   rx->rx_msgaddr));
 		KIBLND_UNMAP_ADDR_SET(rx, rx_msgunmap, rx->rx_msgaddr);
 
@@ -1205,24 +1180,23 @@ kiblnd_map_rx_descs(kib_conn_t *conn)
 		       lnet_page2phys(pg) + pg_off);
 
 		pg_off += IBLND_MSG_SIZE;
-		LASSERT (pg_off <= PAGE_SIZE);
+		LASSERT(pg_off <= PAGE_SIZE);
 
 		if (pg_off == PAGE_SIZE) {
 			pg_off = 0;
 			ipg++;
-			LASSERT (ipg <= IBLND_RX_MSG_PAGES(conn->ibc_version));
+			LASSERT(ipg <= IBLND_RX_MSG_PAGES(conn->ibc_version));
 		}
 	}
 }
 
-static void
-kiblnd_unmap_tx_pool(kib_tx_pool_t *tpo)
+static void kiblnd_unmap_tx_pool(kib_tx_pool_t *tpo)
 {
 	kib_hca_dev_t  *hdev = tpo->tpo_hdev;
 	kib_tx_t       *tx;
 	int	     i;
 
-	LASSERT (tpo->tpo_pool.po_allocated == 0);
+	LASSERT(tpo->tpo_pool.po_allocated == 0);
 
 	if (hdev == NULL)
 		return;
@@ -1239,8 +1213,7 @@ kiblnd_unmap_tx_pool(kib_tx_pool_t *tpo)
 	tpo->tpo_hdev = NULL;
 }
 
-static kib_hca_dev_t *
-kiblnd_current_hdev(kib_dev_t *dev)
+static kib_hca_dev_t *kiblnd_current_hdev(kib_dev_t *dev)
 {
 	kib_hca_dev_t *hdev;
 	unsigned long  flags;
@@ -1265,8 +1238,7 @@ kiblnd_current_hdev(kib_dev_t *dev)
 	return hdev;
 }
 
-static void
-kiblnd_map_tx_pool(kib_tx_pool_t *tpo)
+static void kiblnd_map_tx_pool(kib_tx_pool_t *tpo)
 {
 	kib_pages_t    *txpgs = tpo->tpo_tx_pages;
 	kib_pool_t     *pool  = &tpo->tpo_pool;
@@ -1278,15 +1250,15 @@ kiblnd_map_tx_pool(kib_tx_pool_t *tpo)
 	int	     ipage;
 	int	     i;
 
-	LASSERT (net != NULL);
+	LASSERT(net != NULL);
 
 	dev = net->ibn_dev;
 
 	/* pre-mapped messages are not bigger than 1 page */
-	CLASSERT (IBLND_MSG_SIZE <= PAGE_SIZE);
+	CLASSERT(IBLND_MSG_SIZE <= PAGE_SIZE);
 
 	/* No fancy arithmetic when we do the buffer calculations */
-	CLASSERT (PAGE_SIZE % IBLND_MSG_SIZE == 0);
+	CLASSERT(PAGE_SIZE % IBLND_MSG_SIZE == 0);
 
 	tpo->tpo_hdev = kiblnd_current_hdev(dev);
 
@@ -1300,29 +1272,28 @@ kiblnd_map_tx_pool(kib_tx_pool_t *tpo)
 		tx->tx_msgaddr = kiblnd_dma_map_single(
 			tpo->tpo_hdev->ibh_ibdev, tx->tx_msg,
 			IBLND_MSG_SIZE, DMA_TO_DEVICE);
-		LASSERT (!kiblnd_dma_mapping_error(tpo->tpo_hdev->ibh_ibdev,
+		LASSERT(!kiblnd_dma_mapping_error(tpo->tpo_hdev->ibh_ibdev,
 						   tx->tx_msgaddr));
 		KIBLND_UNMAP_ADDR_SET(tx, tx_msgunmap, tx->tx_msgaddr);
 
 		list_add(&tx->tx_list, &pool->po_free_list);
 
 		page_offset += IBLND_MSG_SIZE;
-		LASSERT (page_offset <= PAGE_SIZE);
+		LASSERT(page_offset <= PAGE_SIZE);
 
 		if (page_offset == PAGE_SIZE) {
 			page_offset = 0;
 			ipage++;
-			LASSERT (ipage <= txpgs->ibp_npages);
+			LASSERT(ipage <= txpgs->ibp_npages);
 		}
 	}
 }
 
-struct ib_mr *
-kiblnd_find_dma_mr(kib_hca_dev_t *hdev, __u64 addr, __u64 size)
+struct ib_mr *kiblnd_find_dma_mr(kib_hca_dev_t *hdev, __u64 addr, __u64 size)
 {
 	__u64   index;
 
-	LASSERT (hdev->ibh_mrs[0] != NULL);
+	LASSERT(hdev->ibh_mrs[0] != NULL);
 
 	if (hdev->ibh_nmrs == 1)
 		return hdev->ibh_mrs[0];
@@ -1336,14 +1307,13 @@ kiblnd_find_dma_mr(kib_hca_dev_t *hdev, __u64 addr, __u64 size)
 	return NULL;
 }
 
-struct ib_mr *
-kiblnd_find_rd_dma_mr(kib_hca_dev_t *hdev, kib_rdma_desc_t *rd)
+struct ib_mr *kiblnd_find_rd_dma_mr(kib_hca_dev_t *hdev, kib_rdma_desc_t *rd)
 {
 	struct ib_mr *prev_mr;
 	struct ib_mr *mr;
 	int	   i;
 
-	LASSERT (hdev->ibh_mrs[0] != NULL);
+	LASSERT(hdev->ibh_mrs[0] != NULL);
 
 	if (*kiblnd_tunables.kib_map_on_demand > 0 &&
 	    *kiblnd_tunables.kib_map_on_demand <= rd->rd_nfrags)
@@ -1370,10 +1340,9 @@ kiblnd_find_rd_dma_mr(kib_hca_dev_t *hdev, kib_rdma_desc_t *rd)
 	return mr;
 }
 
-static void
-kiblnd_destroy_fmr_pool(kib_fmr_pool_t *pool)
+static void kiblnd_destroy_fmr_pool(kib_fmr_pool_t *pool)
 {
-	LASSERT (pool->fpo_map_count == 0);
+	LASSERT(pool->fpo_map_count == 0);
 
 	if (pool->fpo_fmr_pool != NULL)
 		ib_destroy_fmr_pool(pool->fpo_fmr_pool);
@@ -1384,8 +1353,7 @@ kiblnd_destroy_fmr_pool(kib_fmr_pool_t *pool)
 	LIBCFS_FREE(pool, sizeof(kib_fmr_pool_t));
 }
 
-static void
-kiblnd_destroy_fmr_pool_list(struct list_head *head)
+static void kiblnd_destroy_fmr_pool_list(struct list_head *head)
 {
 	kib_fmr_pool_t *pool;
 
@@ -1410,8 +1378,8 @@ static int kiblnd_fmr_flush_trigger(int ncpts)
 	return max(IBLND_FMR_POOL_FLUSH, size);
 }
 
-static int
-kiblnd_create_fmr_pool(kib_fmr_poolset_t *fps, kib_fmr_pool_t **pp_fpo)
+static int kiblnd_create_fmr_pool(kib_fmr_poolset_t *fps,
+				  kib_fmr_pool_t **pp_fpo)
 {
 	/* FMR pool for RDMA */
 	kib_dev_t	       *dev = fps->fps_net->ibn_dev;
@@ -1451,8 +1419,8 @@ kiblnd_create_fmr_pool(kib_fmr_poolset_t *fps, kib_fmr_pool_t **pp_fpo)
 	return 0;
 }
 
-static void
-kiblnd_fail_fmr_poolset(kib_fmr_poolset_t *fps, struct list_head *zombies)
+static void kiblnd_fail_fmr_poolset(kib_fmr_poolset_t *fps,
+				    struct list_head *zombies)
 {
 	if (fps->fps_net == NULL) /* intialized? */
 		return;
@@ -1473,8 +1441,7 @@ kiblnd_fail_fmr_poolset(kib_fmr_poolset_t *fps, struct list_head *zombies)
 	spin_unlock(&fps->fps_lock);
 }
 
-static void
-kiblnd_fini_fmr_poolset(kib_fmr_poolset_t *fps)
+static void kiblnd_fini_fmr_poolset(kib_fmr_poolset_t *fps)
 {
 	if (fps->fps_net != NULL) { /* initialized? */
 		kiblnd_destroy_fmr_pool_list(&fps->fps_failed_pool_list);
@@ -1482,9 +1449,9 @@ kiblnd_fini_fmr_poolset(kib_fmr_poolset_t *fps)
 	}
 }
 
-static int
-kiblnd_init_fmr_poolset(kib_fmr_poolset_t *fps, int cpt, kib_net_t *net,
-			int pool_size, int flush_trigger)
+static int kiblnd_init_fmr_poolset(kib_fmr_poolset_t *fps, int cpt,
+				   kib_net_t *net, int pool_size,
+				   int flush_trigger)
 {
 	kib_fmr_pool_t *fpo;
 	int	     rc;
@@ -1506,8 +1473,7 @@ kiblnd_init_fmr_poolset(kib_fmr_poolset_t *fps, int cpt, kib_net_t *net,
 	return rc;
 }
 
-static int
-kiblnd_fmr_pool_is_idle(kib_fmr_pool_t *fpo, unsigned long now)
+static int kiblnd_fmr_pool_is_idle(kib_fmr_pool_t *fpo, unsigned long now)
 {
 	if (fpo->fpo_map_count != 0) /* still in use */
 		return 0;
@@ -1516,10 +1482,9 @@ kiblnd_fmr_pool_is_idle(kib_fmr_pool_t *fpo, unsigned long now)
 	return cfs_time_aftereq(now, fpo->fpo_deadline);
 }
 
-void
-kiblnd_fmr_pool_unmap(kib_fmr_t *fmr, int status)
+void kiblnd_fmr_pool_unmap(kib_fmr_t *fmr, int status)
 {
-	LIST_HEAD     (zombies);
+	LIST_HEAD(zombies);
 	kib_fmr_pool_t    *fpo = fmr->fmr_pool;
 	kib_fmr_poolset_t *fps = fpo->fpo_owner;
 	unsigned long	 now = cfs_time_current();
@@ -1527,11 +1492,11 @@ kiblnd_fmr_pool_unmap(kib_fmr_t *fmr, int status)
 	int		rc;
 
 	rc = ib_fmr_pool_unmap(fmr->fmr_pfmr);
-	LASSERT (rc == 0);
+	LASSERT(rc == 0);
 
 	if (status != 0) {
 		rc = ib_flush_fmr_pool(fpo->fpo_fmr_pool);
-		LASSERT (rc == 0);
+		LASSERT(rc == 0);
 	}
 
 	fmr->fmr_pool = NULL;
@@ -1556,9 +1521,8 @@ kiblnd_fmr_pool_unmap(kib_fmr_t *fmr, int status)
 		kiblnd_destroy_fmr_pool_list(&zombies);
 }
 
-int
-kiblnd_fmr_pool_map(kib_fmr_poolset_t *fps, __u64 *pages, int npages,
-		    __u64 iov, kib_fmr_t *fmr)
+int kiblnd_fmr_pool_map(kib_fmr_poolset_t *fps, __u64 *pages, int npages,
+			__u64 iov, kib_fmr_t *fmr)
 {
 	struct ib_pool_fmr *pfmr;
 	kib_fmr_pool_t     *fpo;
@@ -1597,7 +1561,8 @@ kiblnd_fmr_pool_map(kib_fmr_poolset_t *fps, __u64 *pages, int npages,
 
 	if (fps->fps_increasing) {
 		spin_unlock(&fps->fps_lock);
-		CDEBUG(D_NET, "Another thread is allocating new FMR pool, waiting for her to complete\n");
+		CDEBUG(D_NET,
+			"Another thread is allocating new FMR pool, waiting for her to complete\n");
 		schedule();
 		goto again;
 
@@ -1627,17 +1592,15 @@ kiblnd_fmr_pool_map(kib_fmr_poolset_t *fps, __u64 *pages, int npages,
 	goto again;
 }
 
-static void
-kiblnd_fini_pool(kib_pool_t *pool)
+static void kiblnd_fini_pool(kib_pool_t *pool)
 {
-	LASSERT (list_empty(&pool->po_free_list));
-	LASSERT (pool->po_allocated == 0);
+	LASSERT(list_empty(&pool->po_free_list));
+	LASSERT(pool->po_allocated == 0);
 
 	CDEBUG(D_NET, "Finalize %s pool\n", pool->po_owner->ps_name);
 }
 
-static void
-kiblnd_init_pool(kib_poolset_t *ps, kib_pool_t *pool, int size)
+static void kiblnd_init_pool(kib_poolset_t *ps, kib_pool_t *pool, int size)
 {
 	CDEBUG(D_NET, "Initialize %s pool\n", ps->ps_name);
 
@@ -1648,8 +1611,7 @@ kiblnd_init_pool(kib_poolset_t *ps, kib_pool_t *pool, int size)
 	pool->po_size     = size;
 }
 
-static void
-kiblnd_destroy_pool_list(struct list_head *head)
+static void kiblnd_destroy_pool_list(struct list_head *head)
 {
 	kib_pool_t *pool;
 
@@ -1657,13 +1619,12 @@ kiblnd_destroy_pool_list(struct list_head *head)
 		pool = list_entry(head->next, kib_pool_t, po_list);
 		list_del(&pool->po_list);
 
-		LASSERT (pool->po_owner != NULL);
+		LASSERT(pool->po_owner != NULL);
 		pool->po_owner->ps_pool_destroy(pool);
 	}
 }
 
-static void
-kiblnd_fail_poolset(kib_poolset_t *ps, struct list_head *zombies)
+static void kiblnd_fail_poolset(kib_poolset_t *ps, struct list_head *zombies)
 {
 	if (ps->ps_net == NULL) /* intialized? */
 		return;
@@ -1682,8 +1643,7 @@ kiblnd_fail_poolset(kib_poolset_t *ps, struct list_head *zombies)
 	spin_unlock(&ps->ps_lock);
 }
 
-static void
-kiblnd_fini_poolset(kib_poolset_t *ps)
+static void kiblnd_fini_poolset(kib_poolset_t *ps)
 {
 	if (ps->ps_net != NULL) { /* initialized? */
 		kiblnd_destroy_pool_list(&ps->ps_failed_pool_list);
@@ -1691,13 +1651,12 @@ kiblnd_fini_poolset(kib_poolset_t *ps)
 	}
 }
 
-static int
-kiblnd_init_poolset(kib_poolset_t *ps, int cpt,
-		    kib_net_t *net, char *name, int size,
-		    kib_ps_pool_create_t po_create,
-		    kib_ps_pool_destroy_t po_destroy,
-		    kib_ps_node_init_t nd_init,
-		    kib_ps_node_fini_t nd_fini)
+static int kiblnd_init_poolset(kib_poolset_t *ps, int cpt,
+			       kib_net_t *net, char *name, int size,
+			       kib_ps_pool_create_t po_create,
+			       kib_ps_pool_destroy_t po_destroy,
+			       kib_ps_node_init_t nd_init,
+			       kib_ps_node_fini_t nd_fini)
 {
 	kib_pool_t	*pool;
 	int		rc;
@@ -1727,8 +1686,7 @@ kiblnd_init_poolset(kib_poolset_t *ps, int cpt,
 	return rc;
 }
 
-static int
-kiblnd_pool_is_idle(kib_pool_t *pool, unsigned long now)
+static int kiblnd_pool_is_idle(kib_pool_t *pool, unsigned long now)
 {
 	if (pool->po_allocated != 0) /* still in use */
 		return 0;
@@ -1737,10 +1695,9 @@ kiblnd_pool_is_idle(kib_pool_t *pool, unsigned long now)
 	return cfs_time_aftereq(now, pool->po_deadline);
 }
 
-void
-kiblnd_pool_free_node(kib_pool_t *pool, struct list_head *node)
+void kiblnd_pool_free_node(kib_pool_t *pool, struct list_head *node)
 {
-	LIST_HEAD  (zombies);
+	LIST_HEAD(zombies);
 	kib_poolset_t  *ps = pool->po_owner;
 	kib_pool_t     *tmp;
 	unsigned long      now = cfs_time_current();
@@ -1750,7 +1707,7 @@ kiblnd_pool_free_node(kib_pool_t *pool, struct list_head *node)
 	if (ps->ps_node_fini != NULL)
 		ps->ps_node_fini(pool, node);
 
-	LASSERT (pool->po_allocated > 0);
+	LASSERT(pool->po_allocated > 0);
 	list_add(node, &pool->po_free_list);
 	pool->po_allocated--;
 
@@ -1768,8 +1725,7 @@ kiblnd_pool_free_node(kib_pool_t *pool, struct list_head *node)
 		kiblnd_destroy_pool_list(&zombies);
 }
 
-struct list_head *
-kiblnd_pool_alloc_node(kib_poolset_t *ps)
+struct list_head *kiblnd_pool_alloc_node(kib_poolset_t *ps)
 {
 	struct list_head	    *node;
 	kib_pool_t	    *pool;
@@ -1831,8 +1787,7 @@ kiblnd_pool_alloc_node(kib_poolset_t *ps)
 	goto again;
 }
 
-void
-kiblnd_pmr_pool_unmap(kib_phys_mr_t *pmr)
+void kiblnd_pmr_pool_unmap(kib_phys_mr_t *pmr)
 {
 	kib_pmr_pool_t      *ppo = pmr->pmr_pool;
 	struct ib_mr	*mr  = pmr->pmr_mr;
@@ -1843,8 +1798,7 @@ kiblnd_pmr_pool_unmap(kib_phys_mr_t *pmr)
 		ib_dereg_mr(mr);
 }
 
-int
-kiblnd_pmr_pool_map(kib_pmr_poolset_t *pps, kib_hca_dev_t *hdev,
+int kiblnd_pmr_pool_map(kib_pmr_poolset_t *pps, kib_hca_dev_t *hdev,
 		    kib_rdma_desc_t *rd, __u64 *iova, kib_phys_mr_t **pp_pmr)
 {
 	kib_phys_mr_t *pmr;
@@ -1889,19 +1843,16 @@ kiblnd_pmr_pool_map(kib_pmr_poolset_t *pps, kib_hca_dev_t *hdev,
 	return rc;
 }
 
-static void
-kiblnd_destroy_pmr_pool(kib_pool_t *pool)
+static void kiblnd_destroy_pmr_pool(kib_pool_t *pool)
 {
 	kib_pmr_pool_t *ppo = container_of(pool, kib_pmr_pool_t, ppo_pool);
 	kib_phys_mr_t  *pmr;
+	kib_phys_mr_t *tmp;
 
-	LASSERT (pool->po_allocated == 0);
-
-	while (!list_empty(&pool->po_free_list)) {
-		pmr = list_entry(pool->po_free_list.next,
-				     kib_phys_mr_t, pmr_list);
+	LASSERT(pool->po_allocated == 0);
 
-		LASSERT (pmr->pmr_mr == NULL);
+	list_for_each_entry_safe(pmr, tmp, &pool->po_free_list, pmr_list) {
+		LASSERT(pmr->pmr_mr == NULL);
 		list_del(&pmr->pmr_list);
 
 		if (pmr->pmr_ipb != NULL) {
@@ -1927,8 +1878,8 @@ static inline int kiblnd_pmr_pool_size(int ncpts)
 	return max(IBLND_PMR_POOL, size);
 }
 
-static int
-kiblnd_create_pmr_pool(kib_poolset_t *ps, int size, kib_pool_t **pp_po)
+static int kiblnd_create_pmr_pool(kib_poolset_t *ps, int size,
+				  kib_pool_t **pp_po)
 {
 	struct kib_pmr_pool	*ppo;
 	struct kib_pool		*pool;
@@ -1970,13 +1921,12 @@ kiblnd_create_pmr_pool(kib_poolset_t *ps, int size, kib_pool_t **pp_po)
 	return 0;
 }
 
-static void
-kiblnd_destroy_tx_pool(kib_pool_t *pool)
+static void kiblnd_destroy_tx_pool(kib_pool_t *pool)
 {
 	kib_tx_pool_t  *tpo = container_of(pool, kib_tx_pool_t, tpo_pool);
 	int	     i;
 
-	LASSERT (pool->po_allocated == 0);
+	LASSERT(pool->po_allocated == 0);
 
 	if (tpo->tpo_tx_pages != NULL) {
 		kiblnd_unmap_tx_pool(tpo);
@@ -2026,8 +1976,8 @@ static int kiblnd_tx_pool_size(int ncpts)
 	return max(IBLND_TX_POOL, ntx);
 }
 
-static int
-kiblnd_create_tx_pool(kib_poolset_t *ps, int size, kib_pool_t **pp_po)
+static int kiblnd_create_tx_pool(kib_poolset_t *ps, int size,
+				 kib_pool_t **pp_po)
 {
 	int	    i;
 	int	    npg;
@@ -2110,8 +2060,7 @@ kiblnd_create_tx_pool(kib_poolset_t *ps, int size, kib_pool_t **pp_po)
 	return -ENOMEM;
 }
 
-static void
-kiblnd_tx_init(kib_pool_t *pool, struct list_head *node)
+static void kiblnd_tx_init(kib_pool_t *pool, struct list_head *node)
 {
 	kib_tx_poolset_t *tps = container_of(pool->po_owner, kib_tx_poolset_t,
 					     tps_poolset);
@@ -2120,8 +2069,7 @@ kiblnd_tx_init(kib_pool_t *pool, struct list_head *node)
 	tx->tx_cookie = tps->tps_next_tx_cookie++;
 }
 
-static void
-kiblnd_net_fini_pools(kib_net_t *net)
+static void kiblnd_net_fini_pools(kib_net_t *net)
 {
 	int	i;
 
@@ -2162,8 +2110,7 @@ kiblnd_net_fini_pools(kib_net_t *net)
 	}
 }
 
-static int
-kiblnd_net_init_pools(kib_net_t *net, __u32 *cpts, int ncpts)
+static int kiblnd_net_init_pools(kib_net_t *net, __u32 *cpts, int ncpts)
 {
 	unsigned long	flags;
 	int		cpt;
@@ -2291,8 +2238,7 @@ kiblnd_net_init_pools(kib_net_t *net, __u32 *cpts, int ncpts)
 	return rc;
 }
 
-static int
-kiblnd_hdev_get_attr(kib_hca_dev_t *hdev)
+static int kiblnd_hdev_get_attr(kib_hca_dev_t *hdev)
 {
 	struct ib_device_attr *attr;
 	int		    rc;
@@ -2336,8 +2282,7 @@ kiblnd_hdev_get_attr(kib_hca_dev_t *hdev)
 	return -EINVAL;
 }
 
-static void
-kiblnd_hdev_cleanup_mrs(kib_hca_dev_t *hdev)
+static void kiblnd_hdev_cleanup_mrs(kib_hca_dev_t *hdev)
 {
 	int     i;
 
@@ -2356,8 +2301,7 @@ kiblnd_hdev_cleanup_mrs(kib_hca_dev_t *hdev)
 	hdev->ibh_nmrs = 0;
 }
 
-void
-kiblnd_hdev_destroy(kib_hca_dev_t *hdev)
+void kiblnd_hdev_destroy(kib_hca_dev_t *hdev)
 {
 	kiblnd_hdev_cleanup_mrs(hdev);
 
@@ -2370,8 +2314,7 @@ kiblnd_hdev_destroy(kib_hca_dev_t *hdev)
 	LIBCFS_FREE(hdev, sizeof(*hdev));
 }
 
-static int
-kiblnd_hdev_setup_mrs(kib_hca_dev_t *hdev)
+static int kiblnd_hdev_setup_mrs(kib_hca_dev_t *hdev)
 {
 	struct ib_mr *mr;
 	int	   i;
@@ -2442,7 +2385,7 @@ kiblnd_hdev_setup_mrs(kib_hca_dev_t *hdev)
 			return PTR_ERR(mr);
 		}
 
-		LASSERT (iova == ipb.addr);
+		LASSERT(iova == ipb.addr);
 
 		hdev->ibh_mrs[i] = mr;
 	}
@@ -2454,14 +2397,14 @@ out:
 	return 0;
 }
 
-static int
-kiblnd_dummy_callback(struct rdma_cm_id *cmid, struct rdma_cm_event *event)
-{       /* DUMMY */
+/* DUMMY */
+static int kiblnd_dummy_callback(struct rdma_cm_id *cmid,
+				 struct rdma_cm_event *event)
+{
 	return 0;
 }
 
-static int
-kiblnd_dev_need_failover(kib_dev_t *dev)
+static int kiblnd_dev_need_failover(kib_dev_t *dev)
 {
 	struct rdma_cm_id  *cmid;
 	struct sockaddr_in  srcaddr;
@@ -2516,12 +2459,11 @@ kiblnd_dev_need_failover(kib_dev_t *dev)
 	return 1;
 }
 
-int
-kiblnd_dev_failover(kib_dev_t *dev)
+int kiblnd_dev_failover(kib_dev_t *dev)
 {
-	LIST_HEAD      (zombie_tpo);
-	LIST_HEAD      (zombie_ppo);
-	LIST_HEAD      (zombie_fpo);
+	LIST_HEAD(zombie_tpo);
+	LIST_HEAD(zombie_ppo);
+	LIST_HEAD(zombie_fpo);
 	struct rdma_cm_id  *cmid  = NULL;
 	kib_hca_dev_t      *hdev  = NULL;
 	kib_hca_dev_t      *old;
@@ -2532,7 +2474,7 @@ kiblnd_dev_failover(kib_dev_t *dev)
 	int		 rc = 0;
 	int		    i;
 
-	LASSERT (*kiblnd_tunables.kib_dev_failover > 1 ||
+	LASSERT(*kiblnd_tunables.kib_dev_failover > 1 ||
 		 dev->ibd_can_failover ||
 		 dev->ibd_hdev == NULL);
 
@@ -2655,11 +2597,10 @@ kiblnd_dev_failover(kib_dev_t *dev)
 	return rc;
 }
 
-void
-kiblnd_destroy_dev (kib_dev_t *dev)
+void kiblnd_destroy_dev(kib_dev_t *dev)
 {
-	LASSERT (dev->ibd_nnets == 0);
-	LASSERT (list_empty(&dev->ibd_nets));
+	LASSERT(dev->ibd_nnets == 0);
+	LASSERT(list_empty(&dev->ibd_nets));
 
 	list_del(&dev->ibd_fail_list);
 	list_del(&dev->ibd_list);
@@ -2670,8 +2611,7 @@ kiblnd_destroy_dev (kib_dev_t *dev)
 	LIBCFS_FREE(dev, sizeof(*dev));
 }
 
-static kib_dev_t *
-kiblnd_create_dev(char *ifname)
+static kib_dev_t *kiblnd_create_dev(char *ifname)
 {
 	struct net_device *netdev;
 	kib_dev_t	 *dev;
@@ -2723,13 +2663,12 @@ kiblnd_create_dev(char *ifname)
 	return dev;
 }
 
-static void
-kiblnd_base_shutdown(void)
+static void kiblnd_base_shutdown(void)
 {
 	struct kib_sched_info	*sched;
 	int			i;
 
-	LASSERT (list_empty(&kiblnd_data.kib_devs));
+	LASSERT(list_empty(&kiblnd_data.kib_devs));
 
 	CDEBUG(D_MALLOC, "before LND base cleanup: kmem %d\n",
 	       atomic_read(&libcfs_kmemory));
@@ -2740,12 +2679,11 @@ kiblnd_base_shutdown(void)
 
 	case IBLND_INIT_ALL:
 	case IBLND_INIT_DATA:
-		LASSERT (kiblnd_data.kib_peers != NULL);
-		for (i = 0; i < kiblnd_data.kib_peer_hash_size; i++) {
-			LASSERT (list_empty(&kiblnd_data.kib_peers[i]));
-		}
-		LASSERT (list_empty(&kiblnd_data.kib_connd_zombies));
-		LASSERT (list_empty(&kiblnd_data.kib_connd_conns));
+		LASSERT(kiblnd_data.kib_peers != NULL);
+		for (i = 0; i < kiblnd_data.kib_peer_hash_size; i++)
+			LASSERT(list_empty(&kiblnd_data.kib_peers[i]));
+		LASSERT(list_empty(&kiblnd_data.kib_connd_zombies));
+		LASSERT(list_empty(&kiblnd_data.kib_connd_conns));
 
 		/* flag threads to terminate; wake and wait for them to die */
 		kiblnd_data.kib_shutdown = 1;
@@ -2762,7 +2700,8 @@ kiblnd_base_shutdown(void)
 		i = 2;
 		while (atomic_read(&kiblnd_data.kib_nthreads) != 0) {
 			i++;
-			CDEBUG(((i & (-i)) == i) ? D_WARNING : D_NET, /* power of 2? */
+			/* power of 2 ? */
+			CDEBUG(((i & (-i)) == i) ? D_WARNING : D_NET,
 			       "Waiting for %d threads to terminate\n",
 			       atomic_read(&kiblnd_data.kib_nthreads));
 			set_current_state(TASK_UNINTERRUPTIBLE);
@@ -2791,8 +2730,7 @@ kiblnd_base_shutdown(void)
 	module_put(THIS_MODULE);
 }
 
-void
-kiblnd_shutdown (lnet_ni_t *ni)
+void kiblnd_shutdown(lnet_ni_t *ni)
 {
 	kib_net_t	*net = ni->ni_data;
 	rwlock_t     *g_lock = &kiblnd_data.kib_global_lock;
@@ -2842,7 +2780,7 @@ kiblnd_shutdown (lnet_ni_t *ni)
 		/* fall through */
 
 	case IBLND_INIT_NOTHING:
-		LASSERT (atomic_read(&net->ibn_nconns) == 0);
+		LASSERT(atomic_read(&net->ibn_nconns) == 0);
 
 		if (net->ibn_dev != NULL &&
 		    net->ibn_dev->ibd_nnets == 0)
@@ -2862,20 +2800,19 @@ kiblnd_shutdown (lnet_ni_t *ni)
 out:
 	if (list_empty(&kiblnd_data.kib_devs))
 		kiblnd_base_shutdown();
-	return;
 }
 
-static int
-kiblnd_base_startup(void)
+static int kiblnd_base_startup(void)
 {
 	struct kib_sched_info	*sched;
 	int			rc;
 	int			i;
 
-	LASSERT (kiblnd_data.kib_init == IBLND_INIT_NOTHING);
+	LASSERT(kiblnd_data.kib_init == IBLND_INIT_NOTHING);
 
 	try_module_get(THIS_MODULE);
-	memset(&kiblnd_data, 0, sizeof(kiblnd_data)); /* zero pointers, flags etc */
+	/* zero pointers, flags etc */
+	memset(&kiblnd_data, 0, sizeof(kiblnd_data));
 
 	rwlock_init(&kiblnd_data.kib_global_lock);
 
@@ -2886,9 +2823,8 @@ kiblnd_base_startup(void)
 	LIBCFS_ALLOC(kiblnd_data.kib_peers,
 		     sizeof(struct list_head) *
 			    kiblnd_data.kib_peer_hash_size);
-	if (kiblnd_data.kib_peers == NULL) {
+	if (kiblnd_data.kib_peers == NULL)
 		goto failed;
-	}
 	for (i = 0; i < kiblnd_data.kib_peer_hash_size; i++)
 		INIT_LIST_HEAD(&kiblnd_data.kib_peers[i]);
 
@@ -2955,8 +2891,7 @@ kiblnd_base_startup(void)
 	return -ENETDOWN;
 }
 
-static int
-kiblnd_start_schedulers(struct kib_sched_info *sched)
+static int kiblnd_start_schedulers(struct kib_sched_info *sched)
 {
 	int	rc = 0;
 	int	nthrs;
@@ -2974,12 +2909,13 @@ kiblnd_start_schedulers(struct kib_sched_info *sched)
 	} else {
 		LASSERT(sched->ibs_nthreads <= sched->ibs_nthreads_max);
 		/* increase one thread if there is new interface */
-		nthrs = (sched->ibs_nthreads < sched->ibs_nthreads_max);
+		nthrs = sched->ibs_nthreads < sched->ibs_nthreads_max;
 	}
 
 	for (i = 0; i < nthrs; i++) {
 		long	id;
 		char	name[20];
+
 		id = KIB_THREAD_ID(sched->ibs_cpt, sched->ibs_nthreads + i);
 		snprintf(name, sizeof(name), "kiblnd_sd_%02ld_%02ld",
 			 KIB_THREAD_CPT(id), KIB_THREAD_TID(id));
@@ -2996,8 +2932,8 @@ kiblnd_start_schedulers(struct kib_sched_info *sched)
 	return rc;
 }
 
-static int
-kiblnd_dev_start_threads(kib_dev_t *dev, int newdev, __u32 *cpts, int ncpts)
+static int kiblnd_dev_start_threads(kib_dev_t *dev, int newdev, __u32 *cpts,
+				    int ncpts)
 {
 	int	cpt;
 	int	rc;
@@ -3022,8 +2958,7 @@ kiblnd_dev_start_threads(kib_dev_t *dev, int newdev, __u32 *cpts, int ncpts)
 	return 0;
 }
 
-static kib_dev_t *
-kiblnd_dev_search(char *ifname)
+static kib_dev_t *kiblnd_dev_search(char *ifname)
 {
 	kib_dev_t	*alias = NULL;
 	kib_dev_t	*dev;
@@ -3055,8 +2990,7 @@ kiblnd_dev_search(char *ifname)
 	return alias;
 }
 
-int
-kiblnd_startup (lnet_ni_t *ni)
+int kiblnd_startup(lnet_ni_t *ni)
 {
 	char		     *ifname;
 	kib_dev_t		*ibdev = NULL;
@@ -3066,7 +3000,7 @@ kiblnd_startup (lnet_ni_t *ni)
 	int		       rc;
 	int			  newdev;
 
-	LASSERT (ni->ni_lnd == &the_o2iblnd);
+	LASSERT(ni->ni_lnd == &the_o2iblnd);
 
 	if (kiblnd_data.kib_init == IBLND_INIT_NOTHING) {
 		rc = kiblnd_base_startup();
@@ -3090,7 +3024,7 @@ kiblnd_startup (lnet_ni_t *ni)
 	if (ni->ni_interfaces[0] != NULL) {
 		/* Use the IPoIB interface specified in 'networks=' */
 
-		CLASSERT (LNET_MAX_INTERFACES > 1);
+		CLASSERT(LNET_MAX_INTERFACES > 1);
 		if (ni->ni_interfaces[1] != NULL) {
 			CERROR("Multiple interfaces not supported\n");
 			goto failed;
@@ -3150,22 +3084,22 @@ net_failed:
 	return -ENETDOWN;
 }
 
-static void __exit
-kiblnd_module_fini (void)
+static void __exit kiblnd_module_fini(void)
 {
 	lnet_unregister_lnd(&the_o2iblnd);
 }
 
-static int __init
-kiblnd_module_init (void)
+static int __init kiblnd_module_init(void)
 {
 	int    rc;
 
-	CLASSERT (sizeof(kib_msg_t) <= IBLND_MSG_SIZE);
-	CLASSERT (offsetof(kib_msg_t, ibm_u.get.ibgm_rd.rd_frags[IBLND_MAX_RDMA_FRAGS])
-		  <= IBLND_MSG_SIZE);
-	CLASSERT (offsetof(kib_msg_t, ibm_u.putack.ibpam_rd.rd_frags[IBLND_MAX_RDMA_FRAGS])
-		  <= IBLND_MSG_SIZE);
+	CLASSERT(sizeof(kib_msg_t) <= IBLND_MSG_SIZE);
+	CLASSERT(offsetof(kib_msg_t,
+		ibm_u.get.ibgm_rd.rd_frags[IBLND_MAX_RDMA_FRAGS])
+		<= IBLND_MSG_SIZE);
+	CLASSERT(offsetof(kib_msg_t,
+		ibm_u.putack.ibpam_rd.rd_frags[IBLND_MAX_RDMA_FRAGS])
+		<= IBLND_MSG_SIZE);
 
 	rc = kiblnd_tunables_init();
 	if (rc != 0)
diff --git a/drivers/staging/lustre/lnet/klnds/o2iblnd/o2iblnd.h b/drivers/staging/lustre/lnet/klnds/o2iblnd/o2iblnd.h
index ab128dee9483..cd664d025f41 100644
--- a/drivers/staging/lustre/lnet/klnds/o2iblnd/o2iblnd.h
+++ b/drivers/staging/lustre/lnet/klnds/o2iblnd/o2iblnd.h
@@ -46,8 +46,8 @@
 #include <linux/errno.h>
 #include <linux/unistd.h>
 #include <linux/uio.h>
+#include <linux/uaccess.h>
 
-#include <asm/uaccess.h>
 #include <asm/io.h>
 
 #include <linux/fs.h>
diff --git a/drivers/staging/lustre/lnet/klnds/o2iblnd/o2iblnd_cb.c b/drivers/staging/lustre/lnet/klnds/o2iblnd/o2iblnd_cb.c
index 48d885dc51d9..dbf3749831f9 100644
--- a/drivers/staging/lustre/lnet/klnds/o2iblnd/o2iblnd_cb.c
+++ b/drivers/staging/lustre/lnet/klnds/o2iblnd/o2iblnd_cb.c
@@ -41,19 +41,19 @@
 #include "o2iblnd.h"
 
 static void
-kiblnd_tx_done (lnet_ni_t *ni, kib_tx_t *tx)
+kiblnd_tx_done(lnet_ni_t *ni, kib_tx_t *tx)
 {
 	lnet_msg_t *lntmsg[2];
 	kib_net_t  *net = ni->ni_data;
 	int	 rc;
 	int	 i;
 
-	LASSERT (net != NULL);
-	LASSERT (!in_interrupt());
-	LASSERT (!tx->tx_queued);	       /* mustn't be queued for sending */
-	LASSERT (tx->tx_sending == 0);	  /* mustn't be awaiting sent callback */
-	LASSERT (!tx->tx_waiting);	      /* mustn't be awaiting peer response */
-	LASSERT (tx->tx_pool != NULL);
+	LASSERT(net != NULL);
+	LASSERT(!in_interrupt());
+	LASSERT(!tx->tx_queued);	       /* mustn't be queued for sending */
+	LASSERT(tx->tx_sending == 0);	  /* mustn't be awaiting sent callback */
+	LASSERT(!tx->tx_waiting);	      /* mustn't be awaiting peer response */
+	LASSERT(tx->tx_pool != NULL);
 
 	kiblnd_unmap_tx(ni, tx);
 
@@ -63,7 +63,7 @@ kiblnd_tx_done (lnet_ni_t *ni, kib_tx_t *tx)
 	rc = tx->tx_status;
 
 	if (tx->tx_conn != NULL) {
-		LASSERT (ni == tx->tx_conn->ibc_peer->ibp_ni);
+		LASSERT(ni == tx->tx_conn->ibc_peer->ibp_ni);
 
 		kiblnd_conn_decref(tx->tx_conn);
 		tx->tx_conn = NULL;
@@ -84,12 +84,12 @@ kiblnd_tx_done (lnet_ni_t *ni, kib_tx_t *tx)
 }
 
 void
-kiblnd_txlist_done (lnet_ni_t *ni, struct list_head *txlist, int status)
+kiblnd_txlist_done(lnet_ni_t *ni, struct list_head *txlist, int status)
 {
 	kib_tx_t *tx;
 
-	while (!list_empty (txlist)) {
-		tx = list_entry (txlist->next, kib_tx_t, tx_list);
+	while (!list_empty(txlist)) {
+		tx = list_entry(txlist->next, kib_tx_t, tx_list);
 
 		list_del(&tx->tx_list);
 		/* complete now */
@@ -113,16 +113,16 @@ kiblnd_get_idle_tx(lnet_ni_t *ni, lnet_nid_t target)
 		return NULL;
 	tx = container_of(node, kib_tx_t, tx_list);
 
-	LASSERT (tx->tx_nwrq == 0);
-	LASSERT (!tx->tx_queued);
-	LASSERT (tx->tx_sending == 0);
-	LASSERT (!tx->tx_waiting);
-	LASSERT (tx->tx_status == 0);
-	LASSERT (tx->tx_conn == NULL);
-	LASSERT (tx->tx_lntmsg[0] == NULL);
-	LASSERT (tx->tx_lntmsg[1] == NULL);
-	LASSERT (tx->tx_u.pmr == NULL);
-	LASSERT (tx->tx_nfrags == 0);
+	LASSERT(tx->tx_nwrq == 0);
+	LASSERT(!tx->tx_queued);
+	LASSERT(tx->tx_sending == 0);
+	LASSERT(!tx->tx_waiting);
+	LASSERT(tx->tx_status == 0);
+	LASSERT(tx->tx_conn == NULL);
+	LASSERT(tx->tx_lntmsg[0] == NULL);
+	LASSERT(tx->tx_lntmsg[1] == NULL);
+	LASSERT(tx->tx_u.pmr == NULL);
+	LASSERT(tx->tx_nfrags == 0);
 
 	return tx;
 }
@@ -143,7 +143,7 @@ kiblnd_drop_rx(kib_rx_t *rx)
 }
 
 int
-kiblnd_post_rx (kib_rx_t *rx, int credit)
+kiblnd_post_rx(kib_rx_t *rx, int credit)
 {
 	kib_conn_t	 *conn = rx->rx_conn;
 	kib_net_t	  *net = conn->ibc_peer->ibp_ni->ni_data;
@@ -151,14 +151,14 @@ kiblnd_post_rx (kib_rx_t *rx, int credit)
 	struct ib_mr       *mr;
 	int		 rc;
 
-	LASSERT (net != NULL);
-	LASSERT (!in_interrupt());
-	LASSERT (credit == IBLND_POSTRX_NO_CREDIT ||
-		 credit == IBLND_POSTRX_PEER_CREDIT ||
-		 credit == IBLND_POSTRX_RSRVD_CREDIT);
+	LASSERT(net != NULL);
+	LASSERT(!in_interrupt());
+	LASSERT(credit == IBLND_POSTRX_NO_CREDIT ||
+		credit == IBLND_POSTRX_PEER_CREDIT ||
+		credit == IBLND_POSTRX_RSRVD_CREDIT);
 
 	mr = kiblnd_find_dma_mr(conn->ibc_hdev, rx->rx_msgaddr, IBLND_MSG_SIZE);
-	LASSERT (mr != NULL);
+	LASSERT(mr != NULL);
 
 	rx->rx_sge.lkey   = mr->lkey;
 	rx->rx_sge.addr   = rx->rx_msgaddr;
@@ -169,8 +169,8 @@ kiblnd_post_rx (kib_rx_t *rx, int credit)
 	rx->rx_wrq.num_sge = 1;
 	rx->rx_wrq.wr_id = kiblnd_ptr2wreqid(rx, IBLND_WID_RX);
 
-	LASSERT (conn->ibc_state >= IBLND_CONN_INIT);
-	LASSERT (rx->rx_nob >= 0);	      /* not posted */
+	LASSERT(conn->ibc_state >= IBLND_CONN_INIT);
+	LASSERT(rx->rx_nob >= 0);	      /* not posted */
 
 	if (conn->ibc_state > IBLND_CONN_ESTABLISHED) {
 		kiblnd_drop_rx(rx);	     /* No more posts for this rx */
@@ -217,8 +217,8 @@ kiblnd_find_waiting_tx_locked(kib_conn_t *conn, int txtype, __u64 cookie)
 	list_for_each(tmp, &conn->ibc_active_txs) {
 		kib_tx_t *tx = list_entry(tmp, kib_tx_t, tx_list);
 
-		LASSERT (!tx->tx_queued);
-		LASSERT (tx->tx_sending != 0 || tx->tx_waiting);
+		LASSERT(!tx->tx_queued);
+		LASSERT(tx->tx_sending != 0 || tx->tx_waiting);
 
 		if (tx->tx_cookie != cookie)
 			continue;
@@ -293,7 +293,7 @@ kiblnd_send_completion(kib_conn_t *conn, int type, int status, __u64 cookie)
 }
 
 static void
-kiblnd_handle_rx (kib_rx_t *rx)
+kiblnd_handle_rx(kib_rx_t *rx)
 {
 	kib_msg_t    *msg = rx->rx_msg;
 	kib_conn_t   *conn = rx->rx_conn;
@@ -304,11 +304,11 @@ kiblnd_handle_rx (kib_rx_t *rx)
 	int	   rc2;
 	int	   post_credit;
 
-	LASSERT (conn->ibc_state >= IBLND_CONN_ESTABLISHED);
+	LASSERT(conn->ibc_state >= IBLND_CONN_ESTABLISHED);
 
-	CDEBUG (D_NET, "Received %x[%d] from %s\n",
-		msg->ibm_type, credits,
-		libcfs_nid2str(conn->ibc_peer->ibp_nid));
+	CDEBUG(D_NET, "Received %x[%d] from %s\n",
+	       msg->ibm_type, credits,
+	       libcfs_nid2str(conn->ibc_peer->ibp_nid));
 
 	if (credits != 0) {
 		/* Have I received credits that will let me send? */
@@ -377,8 +377,8 @@ kiblnd_handle_rx (kib_rx_t *rx)
 		break;
 
 	case IBLND_MSG_PUT_NAK:
-		CWARN ("PUT_NACK from %s\n",
-		       libcfs_nid2str(conn->ibc_peer->ibp_nid));
+		CWARN("PUT_NACK from %s\n",
+		      libcfs_nid2str(conn->ibc_peer->ibp_nid));
 		post_credit = IBLND_POSTRX_RSRVD_CREDIT;
 		kiblnd_handle_completion(conn, IBLND_MSG_PUT_REQ,
 					 msg->ibm_u.completion.ibcm_status,
@@ -402,7 +402,7 @@ kiblnd_handle_rx (kib_rx_t *rx)
 			break;
 		}
 
-		LASSERT (tx->tx_waiting);
+		LASSERT(tx->tx_waiting);
 		/* CAVEAT EMPTOR: I could be racing with tx_complete, but...
 		 * (a) I can overwrite tx_msg since my peer has received it!
 		 * (b) tx_waiting set tells tx_complete() it's not done. */
@@ -454,7 +454,7 @@ kiblnd_handle_rx (kib_rx_t *rx)
 }
 
 static void
-kiblnd_rx_complete (kib_rx_t *rx, int status, int nob)
+kiblnd_rx_complete(kib_rx_t *rx, int status, int nob)
 {
 	kib_msg_t    *msg = rx->rx_msg;
 	kib_conn_t   *conn = rx->rx_conn;
@@ -463,8 +463,8 @@ kiblnd_rx_complete (kib_rx_t *rx, int status, int nob)
 	int	   rc;
 	int	   err = -EIO;
 
-	LASSERT (net != NULL);
-	LASSERT (rx->rx_nob < 0);	       /* was posted */
+	LASSERT(net != NULL);
+	LASSERT(rx->rx_nob < 0);	       /* was posted */
 	rx->rx_nob = 0;			 /* isn't now */
 
 	if (conn->ibc_state > IBLND_CONN_ESTABLISHED)
@@ -476,12 +476,12 @@ kiblnd_rx_complete (kib_rx_t *rx, int status, int nob)
 		goto failed;
 	}
 
-	LASSERT (nob >= 0);
+	LASSERT(nob >= 0);
 	rx->rx_nob = nob;
 
 	rc = kiblnd_unpack_msg(msg, rx->rx_nob);
 	if (rc != 0) {
-		CERROR ("Error %d unpacking rx from %s\n",
+		CERROR("Error %d unpacking rx from %s\n",
 			rc, libcfs_nid2str(conn->ibc_peer->ibp_nid));
 		goto failed;
 	}
@@ -490,7 +490,7 @@ kiblnd_rx_complete (kib_rx_t *rx, int status, int nob)
 	    msg->ibm_dstnid != ni->ni_nid ||
 	    msg->ibm_srcstamp != conn->ibc_incarnation ||
 	    msg->ibm_dststamp != net->ibn_incarnation) {
-		CERROR ("Stale rx from %s\n",
+		CERROR("Stale rx from %s\n",
 			libcfs_nid2str(conn->ibc_peer->ibp_nid));
 		err = -ESTALE;
 		goto failed;
@@ -525,13 +525,13 @@ kiblnd_rx_complete (kib_rx_t *rx, int status, int nob)
 }
 
 static struct page *
-kiblnd_kvaddr_to_page (unsigned long vaddr)
+kiblnd_kvaddr_to_page(unsigned long vaddr)
 {
 	struct page *page;
 
 	if (is_vmalloc_addr((void *)vaddr)) {
-		page = vmalloc_to_page ((void *)vaddr);
-		LASSERT (page != NULL);
+		page = vmalloc_to_page((void *)vaddr);
+		LASSERT(page != NULL);
 		return page;
 	}
 #ifdef CONFIG_HIGHMEM
@@ -542,8 +542,8 @@ kiblnd_kvaddr_to_page (unsigned long vaddr)
 		LBUG();
 	}
 #endif
-	page = virt_to_page (vaddr);
-	LASSERT (page != NULL);
+	page = virt_to_page(vaddr);
+	LASSERT(page != NULL);
 	return page;
 }
 
@@ -567,7 +567,7 @@ kiblnd_fmr_map_tx(kib_net_t *net, kib_tx_t *tx, kib_rdma_desc_t *rd, int nob)
 	for (i = 0, npages = 0; i < rd->rd_nfrags; i++) {
 		for (size = 0; size <  rd->rd_frags[i].rf_nob;
 			       size += hdev->ibh_page_size) {
-			pages[npages ++] = (rd->rd_frags[i].rf_addr &
+			pages[npages++] = (rd->rd_frags[i].rf_addr &
 					    hdev->ibh_page_mask) + size;
 		}
 	}
@@ -577,7 +577,7 @@ kiblnd_fmr_map_tx(kib_net_t *net, kib_tx_t *tx, kib_rdma_desc_t *rd, int nob)
 	fps = net->ibn_fmr_ps[cpt];
 	rc = kiblnd_fmr_pool_map(fps, pages, npages, 0, &tx->tx_u.fmr);
 	if (rc != 0) {
-		CERROR ("Can't map %d pages: %d\n", npages, rc);
+		CERROR("Can't map %d pages: %d\n", npages, rc);
 		return rc;
 	}
 
@@ -706,26 +706,26 @@ kiblnd_setup_rd_iov(lnet_ni_t *ni, kib_tx_t *tx, kib_rdma_desc_t *rd,
 	int		 fragnob;
 	int		 page_offset;
 
-	LASSERT (nob > 0);
-	LASSERT (niov > 0);
-	LASSERT (net != NULL);
+	LASSERT(nob > 0);
+	LASSERT(niov > 0);
+	LASSERT(net != NULL);
 
 	while (offset >= iov->iov_len) {
 		offset -= iov->iov_len;
 		niov--;
 		iov++;
-		LASSERT (niov > 0);
+		LASSERT(niov > 0);
 	}
 
 	sg = tx->tx_frags;
 	do {
-		LASSERT (niov > 0);
+		LASSERT(niov > 0);
 
 		vaddr = ((unsigned long)iov->iov_base) + offset;
 		page_offset = vaddr & (PAGE_SIZE - 1);
 		page = kiblnd_kvaddr_to_page(vaddr);
 		if (page == NULL) {
-			CERROR ("Can't find page\n");
+			CERROR("Can't find page\n");
 			return -EFAULT;
 		}
 
@@ -749,7 +749,7 @@ kiblnd_setup_rd_iov(lnet_ni_t *ni, kib_tx_t *tx, kib_rdma_desc_t *rd,
 }
 
 static int
-kiblnd_setup_rd_kiov (lnet_ni_t *ni, kib_tx_t *tx, kib_rdma_desc_t *rd,
+kiblnd_setup_rd_kiov(lnet_ni_t *ni, kib_tx_t *tx, kib_rdma_desc_t *rd,
 		      int nkiov, lnet_kiov_t *kiov, int offset, int nob)
 {
 	kib_net_t	  *net = ni->ni_data;
@@ -758,20 +758,20 @@ kiblnd_setup_rd_kiov (lnet_ni_t *ni, kib_tx_t *tx, kib_rdma_desc_t *rd,
 
 	CDEBUG(D_NET, "niov %d offset %d nob %d\n", nkiov, offset, nob);
 
-	LASSERT (nob > 0);
-	LASSERT (nkiov > 0);
-	LASSERT (net != NULL);
+	LASSERT(nob > 0);
+	LASSERT(nkiov > 0);
+	LASSERT(net != NULL);
 
 	while (offset >= kiov->kiov_len) {
 		offset -= kiov->kiov_len;
 		nkiov--;
 		kiov++;
-		LASSERT (nkiov > 0);
+		LASSERT(nkiov > 0);
 	}
 
 	sg = tx->tx_frags;
 	do {
-		LASSERT (nkiov > 0);
+		LASSERT(nkiov > 0);
 
 		fragnob = min((int)(kiov->kiov_len - offset), nob);
 
@@ -789,7 +789,7 @@ kiblnd_setup_rd_kiov (lnet_ni_t *ni, kib_tx_t *tx, kib_rdma_desc_t *rd,
 }
 
 static int
-kiblnd_post_tx_locked (kib_conn_t *conn, kib_tx_t *tx, int credit)
+kiblnd_post_tx_locked(kib_conn_t *conn, kib_tx_t *tx, int credit)
 	__releases(conn->ibc_lock)
 	__acquires(conn->ibc_lock)
 {
@@ -800,16 +800,16 @@ kiblnd_post_tx_locked (kib_conn_t *conn, kib_tx_t *tx, int credit)
 	int		done;
 	struct ib_send_wr *bad_wrq;
 
-	LASSERT (tx->tx_queued);
+	LASSERT(tx->tx_queued);
 	/* We rely on this for QP sizing */
-	LASSERT (tx->tx_nwrq > 0);
-	LASSERT (tx->tx_nwrq <= 1 + IBLND_RDMA_FRAGS(ver));
+	LASSERT(tx->tx_nwrq > 0);
+	LASSERT(tx->tx_nwrq <= 1 + IBLND_RDMA_FRAGS(ver));
 
-	LASSERT (credit == 0 || credit == 1);
-	LASSERT (conn->ibc_outstanding_credits >= 0);
-	LASSERT (conn->ibc_outstanding_credits <= IBLND_MSG_QUEUE_SIZE(ver));
-	LASSERT (conn->ibc_credits >= 0);
-	LASSERT (conn->ibc_credits <= IBLND_MSG_QUEUE_SIZE(ver));
+	LASSERT(credit == 0 || credit == 1);
+	LASSERT(conn->ibc_outstanding_credits >= 0);
+	LASSERT(conn->ibc_outstanding_credits <= IBLND_MSG_QUEUE_SIZE(ver));
+	LASSERT(conn->ibc_credits >= 0);
+	LASSERT(conn->ibc_credits <= IBLND_MSG_QUEUE_SIZE(ver));
 
 	if (conn->ibc_nsends_posted == IBLND_CONCURRENT_SENDS(ver)) {
 		/* tx completions outstanding... */
@@ -923,7 +923,7 @@ kiblnd_post_tx_locked (kib_conn_t *conn, kib_tx_t *tx, int credit)
 }
 
 void
-kiblnd_check_sends (kib_conn_t *conn)
+kiblnd_check_sends(kib_conn_t *conn)
 {
 	int	ver = conn->ibc_version;
 	lnet_ni_t *ni = conn->ibc_peer->ibp_ni;
@@ -938,10 +938,10 @@ kiblnd_check_sends (kib_conn_t *conn)
 
 	spin_lock(&conn->ibc_lock);
 
-	LASSERT (conn->ibc_nsends_posted <= IBLND_CONCURRENT_SENDS(ver));
-	LASSERT (!IBLND_OOB_CAPABLE(ver) ||
+	LASSERT(conn->ibc_nsends_posted <= IBLND_CONCURRENT_SENDS(ver));
+	LASSERT(!IBLND_OOB_CAPABLE(ver) ||
 		 conn->ibc_noops_posted <= IBLND_OOB_MSGS(ver));
-	LASSERT (conn->ibc_reserved_credits >= 0);
+	LASSERT(conn->ibc_reserved_credits >= 0);
 
 	while (conn->ibc_reserved_credits > 0 &&
 	       !list_empty(&conn->ibc_tx_queue_rsrvd)) {
@@ -974,7 +974,7 @@ kiblnd_check_sends (kib_conn_t *conn)
 			tx = list_entry(conn->ibc_tx_queue_nocred.next,
 					    kib_tx_t, tx_list);
 		} else if (!list_empty(&conn->ibc_tx_noops)) {
-			LASSERT (!IBLND_OOB_CAPABLE(ver));
+			LASSERT(!IBLND_OOB_CAPABLE(ver));
 			credit = 1;
 			tx = list_entry(conn->ibc_tx_noops.next,
 					kib_tx_t, tx_list);
@@ -995,13 +995,13 @@ kiblnd_check_sends (kib_conn_t *conn)
 }
 
 static void
-kiblnd_tx_complete (kib_tx_t *tx, int status)
+kiblnd_tx_complete(kib_tx_t *tx, int status)
 {
 	int	   failed = (status != IB_WC_SUCCESS);
 	kib_conn_t   *conn = tx->tx_conn;
 	int	   idle;
 
-	LASSERT (tx->tx_sending > 0);
+	LASSERT(tx->tx_sending > 0);
 
 	if (failed) {
 		if (conn->ibc_state == IBLND_CONN_ESTABLISHED)
@@ -1049,22 +1049,22 @@ kiblnd_tx_complete (kib_tx_t *tx, int status)
 }
 
 void
-kiblnd_init_tx_msg (lnet_ni_t *ni, kib_tx_t *tx, int type, int body_nob)
+kiblnd_init_tx_msg(lnet_ni_t *ni, kib_tx_t *tx, int type, int body_nob)
 {
 	kib_hca_dev_t     *hdev = tx->tx_pool->tpo_hdev;
 	struct ib_sge     *sge = &tx->tx_sge[tx->tx_nwrq];
 	struct ib_send_wr *wrq = &tx->tx_wrq[tx->tx_nwrq];
-	int		nob = offsetof (kib_msg_t, ibm_u) + body_nob;
+	int		nob = offsetof(kib_msg_t, ibm_u) + body_nob;
 	struct ib_mr      *mr;
 
-	LASSERT (tx->tx_nwrq >= 0);
-	LASSERT (tx->tx_nwrq < IBLND_MAX_RDMA_FRAGS + 1);
-	LASSERT (nob <= IBLND_MSG_SIZE);
+	LASSERT(tx->tx_nwrq >= 0);
+	LASSERT(tx->tx_nwrq < IBLND_MAX_RDMA_FRAGS + 1);
+	LASSERT(nob <= IBLND_MSG_SIZE);
 
 	kiblnd_init_msg(tx->tx_msg, type, body_nob);
 
 	mr = kiblnd_find_dma_mr(hdev, tx->tx_msgaddr, nob);
-	LASSERT (mr != NULL);
+	LASSERT(mr != NULL);
 
 	sge->lkey   = mr->lkey;
 	sge->addr   = tx->tx_msgaddr;
@@ -1083,7 +1083,7 @@ kiblnd_init_tx_msg (lnet_ni_t *ni, kib_tx_t *tx, int type, int body_nob)
 }
 
 int
-kiblnd_init_rdma (kib_conn_t *conn, kib_tx_t *tx, int type,
+kiblnd_init_rdma(kib_conn_t *conn, kib_tx_t *tx, int type,
 		  int resid, kib_rdma_desc_t *dstrd, __u64 dstcookie)
 {
 	kib_msg_t	 *ibmsg = tx->tx_msg;
@@ -1095,9 +1095,9 @@ kiblnd_init_rdma (kib_conn_t *conn, kib_tx_t *tx, int type,
 	int		dstidx;
 	int		wrknob;
 
-	LASSERT (!in_interrupt());
-	LASSERT (tx->tx_nwrq == 0);
-	LASSERT (type == IBLND_MSG_GET_DONE ||
+	LASSERT(!in_interrupt());
+	LASSERT(tx->tx_nwrq == 0);
+	LASSERT(type == IBLND_MSG_GET_DONE ||
 		 type == IBLND_MSG_PUT_DONE);
 
 	srcidx = dstidx = 0;
@@ -1162,19 +1162,19 @@ kiblnd_init_rdma (kib_conn_t *conn, kib_tx_t *tx, int type,
 	ibmsg->ibm_u.completion.ibcm_status = rc;
 	ibmsg->ibm_u.completion.ibcm_cookie = dstcookie;
 	kiblnd_init_tx_msg(conn->ibc_peer->ibp_ni, tx,
-			   type, sizeof (kib_completion_msg_t));
+			   type, sizeof(kib_completion_msg_t));
 
 	return rc;
 }
 
 void
-kiblnd_queue_tx_locked (kib_tx_t *tx, kib_conn_t *conn)
+kiblnd_queue_tx_locked(kib_tx_t *tx, kib_conn_t *conn)
 {
 	struct list_head   *q;
 
-	LASSERT (tx->tx_nwrq > 0);	      /* work items set up */
-	LASSERT (!tx->tx_queued);	       /* not queued for sending already */
-	LASSERT (conn->ibc_state >= IBLND_CONN_ESTABLISHED);
+	LASSERT(tx->tx_nwrq > 0);	      /* work items set up */
+	LASSERT(!tx->tx_queued);	       /* not queued for sending already */
+	LASSERT(conn->ibc_state >= IBLND_CONN_ESTABLISHED);
 
 	tx->tx_queued = 1;
 	tx->tx_deadline = jiffies + (*kiblnd_tunables.kib_timeout * HZ);
@@ -1182,11 +1182,11 @@ kiblnd_queue_tx_locked (kib_tx_t *tx, kib_conn_t *conn)
 	if (tx->tx_conn == NULL) {
 		kiblnd_conn_addref(conn);
 		tx->tx_conn = conn;
-		LASSERT (tx->tx_msg->ibm_type != IBLND_MSG_PUT_DONE);
+		LASSERT(tx->tx_msg->ibm_type != IBLND_MSG_PUT_DONE);
 	} else {
 		/* PUT_DONE first attached to conn as a PUT_REQ */
-		LASSERT (tx->tx_conn == conn);
-		LASSERT (tx->tx_msg->ibm_type == IBLND_MSG_PUT_DONE);
+		LASSERT(tx->tx_conn == conn);
+		LASSERT(tx->tx_msg->ibm_type == IBLND_MSG_PUT_DONE);
 	}
 
 	switch (tx->tx_msg->ibm_type) {
@@ -1221,7 +1221,7 @@ kiblnd_queue_tx_locked (kib_tx_t *tx, kib_conn_t *conn)
 }
 
 void
-kiblnd_queue_tx (kib_tx_t *tx, kib_conn_t *conn)
+kiblnd_queue_tx(kib_tx_t *tx, kib_conn_t *conn)
 {
 	spin_lock(&conn->ibc_lock);
 	kiblnd_queue_tx_locked(tx, conn);
@@ -1268,7 +1268,7 @@ static int kiblnd_resolve_addr(struct rdma_cm_id *cmid,
 }
 
 static void
-kiblnd_connect_peer (kib_peer_t *peer)
+kiblnd_connect_peer(kib_peer_t *peer)
 {
 	struct rdma_cm_id *cmid;
 	kib_dev_t	 *dev;
@@ -1277,8 +1277,8 @@ kiblnd_connect_peer (kib_peer_t *peer)
 	struct sockaddr_in dstaddr;
 	int		rc;
 
-	LASSERT (net != NULL);
-	LASSERT (peer->ibp_connecting > 0);
+	LASSERT(net != NULL);
+	LASSERT(peer->ibp_connecting > 0);
 
 	cmid = kiblnd_rdma_create_id(kiblnd_cm_callback, peer, RDMA_PS_TCP,
 				     IB_QPT_RC);
@@ -1318,7 +1318,7 @@ kiblnd_connect_peer (kib_peer_t *peer)
 		goto failed2;
 	}
 
-	LASSERT (cmid->device != NULL);
+	LASSERT(cmid->device != NULL);
 	CDEBUG(D_NET, "%s: connection bound to %s:%pI4h:%s\n",
 	       libcfs_nid2str(peer->ibp_nid), dev->ibd_ifname,
 	       &dev->ibd_ifip, cmid->device->name);
@@ -1333,7 +1333,7 @@ kiblnd_connect_peer (kib_peer_t *peer)
 }
 
 void
-kiblnd_launch_tx (lnet_ni_t *ni, kib_tx_t *tx, lnet_nid_t nid)
+kiblnd_launch_tx(lnet_ni_t *ni, kib_tx_t *tx, lnet_nid_t nid)
 {
 	kib_peer_t	*peer;
 	kib_peer_t	*peer2;
@@ -1345,8 +1345,8 @@ kiblnd_launch_tx (lnet_ni_t *ni, kib_tx_t *tx, lnet_nid_t nid)
 	/* If I get here, I've committed to send, so I complete the tx with
 	 * failure on any problems */
 
-	LASSERT (tx == NULL || tx->tx_conn == NULL); /* only set when assigned a conn */
-	LASSERT (tx == NULL || tx->tx_nwrq > 0);     /* work items have been set up */
+	LASSERT(tx == NULL || tx->tx_conn == NULL); /* only set when assigned a conn */
+	LASSERT(tx == NULL || tx->tx_nwrq > 0);     /* work items have been set up */
 
 	/* First time, just use a read lock since I expect to find my peer
 	 * connected */
@@ -1374,7 +1374,7 @@ kiblnd_launch_tx (lnet_ni_t *ni, kib_tx_t *tx, lnet_nid_t nid)
 	if (peer != NULL) {
 		if (list_empty(&peer->ibp_conns)) {
 			/* found a peer, but it's still connecting... */
-			LASSERT (peer->ibp_connecting != 0 ||
+			LASSERT(peer->ibp_connecting != 0 ||
 				 peer->ibp_accepting != 0);
 			if (tx != NULL)
 				list_add_tail(&tx->tx_list,
@@ -1413,7 +1413,7 @@ kiblnd_launch_tx (lnet_ni_t *ni, kib_tx_t *tx, lnet_nid_t nid)
 	if (peer2 != NULL) {
 		if (list_empty(&peer2->ibp_conns)) {
 			/* found a peer, but it's still connecting... */
-			LASSERT (peer2->ibp_connecting != 0 ||
+			LASSERT(peer2->ibp_connecting != 0 ||
 				 peer2->ibp_accepting != 0);
 			if (tx != NULL)
 				list_add_tail(&tx->tx_list,
@@ -1435,11 +1435,11 @@ kiblnd_launch_tx (lnet_ni_t *ni, kib_tx_t *tx, lnet_nid_t nid)
 	}
 
 	/* Brand new peer */
-	LASSERT (peer->ibp_connecting == 0);
+	LASSERT(peer->ibp_connecting == 0);
 	peer->ibp_connecting = 1;
 
 	/* always called with a ref on ni, which prevents ni being shutdown */
-	LASSERT (((kib_net_t *)ni->ni_data)->ibn_shutdown == 0);
+	LASSERT(((kib_net_t *)ni->ni_data)->ibn_shutdown == 0);
 
 	if (tx != NULL)
 		list_add_tail(&tx->tx_list, &peer->ibp_tx_queue);
@@ -1454,7 +1454,7 @@ kiblnd_launch_tx (lnet_ni_t *ni, kib_tx_t *tx, lnet_nid_t nid)
 }
 
 int
-kiblnd_send (lnet_ni_t *ni, void *private, lnet_msg_t *lntmsg)
+kiblnd_send(lnet_ni_t *ni, void *private, lnet_msg_t *lntmsg)
 {
 	lnet_hdr_t       *hdr = &lntmsg->msg_hdr;
 	int	       type = lntmsg->msg_type;
@@ -1476,13 +1476,13 @@ kiblnd_send (lnet_ni_t *ni, void *private, lnet_msg_t *lntmsg)
 	CDEBUG(D_NET, "sending %d bytes in %d frags to %s\n",
 	       payload_nob, payload_niov, libcfs_id2str(target));
 
-	LASSERT (payload_nob == 0 || payload_niov > 0);
-	LASSERT (payload_niov <= LNET_MAX_IOV);
+	LASSERT(payload_nob == 0 || payload_niov > 0);
+	LASSERT(payload_niov <= LNET_MAX_IOV);
 
 	/* Thread context */
-	LASSERT (!in_interrupt());
+	LASSERT(!in_interrupt());
 	/* payload is either all vaddrs or all pages */
-	LASSERT (!(payload_kiov != NULL && payload_iov != NULL));
+	LASSERT(!(payload_kiov != NULL && payload_iov != NULL));
 
 	switch (type) {
 	default:
@@ -1490,7 +1490,7 @@ kiblnd_send (lnet_ni_t *ni, void *private, lnet_msg_t *lntmsg)
 		return -EIO;
 
 	case LNET_MSG_ACK:
-		LASSERT (payload_nob == 0);
+		LASSERT(payload_nob == 0);
 		break;
 
 	case LNET_MSG_GET:
@@ -1592,12 +1592,12 @@ kiblnd_send (lnet_ni_t *ni, void *private, lnet_msg_t *lntmsg)
 
 	/* send IMMEDIATE */
 
-	LASSERT (offsetof(kib_msg_t, ibm_u.immediate.ibim_payload[payload_nob])
+	LASSERT(offsetof(kib_msg_t, ibm_u.immediate.ibim_payload[payload_nob])
 		 <= IBLND_MSG_SIZE);
 
 	tx = kiblnd_get_idle_tx(ni, target.nid);
 	if (tx == NULL) {
-		CERROR ("Can't send %d to %s: tx descs exhausted\n",
+		CERROR("Can't send %d to %s: tx descs exhausted\n",
 			type, libcfs_nid2str(target.nid));
 		return -ENOMEM;
 	}
@@ -1625,7 +1625,7 @@ kiblnd_send (lnet_ni_t *ni, void *private, lnet_msg_t *lntmsg)
 }
 
 static void
-kiblnd_reply (lnet_ni_t *ni, kib_rx_t *rx, lnet_msg_t *lntmsg)
+kiblnd_reply(lnet_ni_t *ni, kib_rx_t *rx, lnet_msg_t *lntmsg)
 {
 	lnet_process_id_t target = lntmsg->msg_target;
 	unsigned int      niov = lntmsg->msg_niov;
@@ -1687,7 +1687,7 @@ kiblnd_reply (lnet_ni_t *ni, kib_rx_t *rx, lnet_msg_t *lntmsg)
 }
 
 int
-kiblnd_recv (lnet_ni_t *ni, void *private, lnet_msg_t *lntmsg, int delayed,
+kiblnd_recv(lnet_ni_t *ni, void *private, lnet_msg_t *lntmsg, int delayed,
 	     unsigned int niov, struct kvec *iov, lnet_kiov_t *kiov,
 	     unsigned int offset, unsigned int mlen, unsigned int rlen)
 {
@@ -1700,10 +1700,10 @@ kiblnd_recv (lnet_ni_t *ni, void *private, lnet_msg_t *lntmsg, int delayed,
 	int	  post_credit = IBLND_POSTRX_PEER_CREDIT;
 	int	  rc = 0;
 
-	LASSERT (mlen <= rlen);
-	LASSERT (!in_interrupt());
+	LASSERT(mlen <= rlen);
+	LASSERT(!in_interrupt());
 	/* Either all pages or all vaddrs */
-	LASSERT (!(kiov != NULL && iov != NULL));
+	LASSERT(!(kiov != NULL && iov != NULL));
 
 	switch (rxmsg->ibm_type) {
 	default:
@@ -1712,7 +1712,7 @@ kiblnd_recv (lnet_ni_t *ni, void *private, lnet_msg_t *lntmsg, int delayed,
 	case IBLND_MSG_IMMEDIATE:
 		nob = offsetof(kib_msg_t, ibm_u.immediate.ibim_payload[rlen]);
 		if (nob > rx->rx_nob) {
-			CERROR ("Immediate message from %s too big: %d(%d)\n",
+			CERROR("Immediate message from %s too big: %d(%d)\n",
 				libcfs_nid2str(rxmsg->ibm_u.immediate.ibim_hdr.src_nid),
 				nob, rx->rx_nob);
 			rc = -EPROTO;
@@ -1729,7 +1729,7 @@ kiblnd_recv (lnet_ni_t *ni, void *private, lnet_msg_t *lntmsg, int delayed,
 					   IBLND_MSG_SIZE, rxmsg,
 					   offsetof(kib_msg_t, ibm_u.immediate.ibim_payload),
 					   mlen);
-		lnet_finalize (ni, lntmsg, 0);
+		lnet_finalize(ni, lntmsg, 0);
 		break;
 
 	case IBLND_MSG_PUT_REQ:
@@ -1812,13 +1812,13 @@ kiblnd_thread_start(int (*fn)(void *arg), void *arg, char *name)
 }
 
 static void
-kiblnd_thread_fini (void)
+kiblnd_thread_fini(void)
 {
-	atomic_dec (&kiblnd_data.kib_nthreads);
+	atomic_dec(&kiblnd_data.kib_nthreads);
 }
 
 void
-kiblnd_peer_alive (kib_peer_t *peer)
+kiblnd_peer_alive(kib_peer_t *peer)
 {
 	/* This is racy, but everyone's only writing cfs_time_current() */
 	peer->ibp_last_alive = cfs_time_current();
@@ -1826,7 +1826,7 @@ kiblnd_peer_alive (kib_peer_t *peer)
 }
 
 static void
-kiblnd_peer_notify (kib_peer_t *peer)
+kiblnd_peer_notify(kib_peer_t *peer)
 {
 	int	   error = 0;
 	unsigned long    last_alive = 0;
@@ -1852,7 +1852,7 @@ kiblnd_peer_notify (kib_peer_t *peer)
 }
 
 void
-kiblnd_close_conn_locked (kib_conn_t *conn, int error)
+kiblnd_close_conn_locked(kib_conn_t *conn, int error)
 {
 	/* This just does the immediate housekeeping.  'error' is zero for a
 	 * normal shutdown which can happen only after the connection has been
@@ -1864,7 +1864,7 @@ kiblnd_close_conn_locked (kib_conn_t *conn, int error)
 	kib_dev_t	*dev;
 	unsigned long     flags;
 
-	LASSERT (error != 0 || conn->ibc_state >= IBLND_CONN_ESTABLISHED);
+	LASSERT(error != 0 || conn->ibc_state >= IBLND_CONN_ESTABLISHED);
 
 	if (error != 0 && conn->ibc_comms_error == 0)
 		conn->ibc_comms_error = error;
@@ -1894,7 +1894,7 @@ kiblnd_close_conn_locked (kib_conn_t *conn, int error)
 	list_del(&conn->ibc_list);
 	/* connd (see below) takes over ibc_list's ref */
 
-	if (list_empty (&peer->ibp_conns) &&    /* no more conns */
+	if (list_empty(&peer->ibp_conns) &&    /* no more conns */
 	    kiblnd_peer_active(peer)) {	 /* still in peer table */
 		kiblnd_unlink_peer_locked(peer);
 
@@ -1936,14 +1936,13 @@ kiblnd_handle_early_rxs(kib_conn_t *conn)
 {
 	unsigned long    flags;
 	kib_rx_t	*rx;
+	kib_rx_t *tmp;
 
 	LASSERT(!in_interrupt());
 	LASSERT(conn->ibc_state >= IBLND_CONN_ESTABLISHED);
 
 	write_lock_irqsave(&kiblnd_data.kib_global_lock, flags);
-	while (!list_empty(&conn->ibc_early_rxs)) {
-		rx = list_entry(conn->ibc_early_rxs.next,
-				    kib_rx_t, rx_list);
+	list_for_each_entry_safe(rx, tmp, &conn->ibc_early_rxs, rx_list) {
 		list_del(&rx->rx_list);
 		write_unlock_irqrestore(&kiblnd_data.kib_global_lock, flags);
 
@@ -1957,22 +1956,22 @@ kiblnd_handle_early_rxs(kib_conn_t *conn)
 static void
 kiblnd_abort_txs(kib_conn_t *conn, struct list_head *txs)
 {
-	LIST_HEAD       (zombies);
+	LIST_HEAD(zombies);
 	struct list_head	  *tmp;
 	struct list_head	  *nxt;
 	kib_tx_t	    *tx;
 
 	spin_lock(&conn->ibc_lock);
 
-	list_for_each_safe (tmp, nxt, txs) {
-		tx = list_entry (tmp, kib_tx_t, tx_list);
+	list_for_each_safe(tmp, nxt, txs) {
+		tx = list_entry(tmp, kib_tx_t, tx_list);
 
 		if (txs == &conn->ibc_active_txs) {
-			LASSERT (!tx->tx_queued);
-			LASSERT (tx->tx_waiting ||
+			LASSERT(!tx->tx_queued);
+			LASSERT(tx->tx_waiting ||
 				 tx->tx_sending != 0);
 		} else {
-			LASSERT (tx->tx_queued);
+			LASSERT(tx->tx_queued);
 		}
 
 		tx->tx_status = -ECONNABORTED;
@@ -1980,8 +1979,8 @@ kiblnd_abort_txs(kib_conn_t *conn, struct list_head *txs)
 
 		if (tx->tx_sending == 0) {
 			tx->tx_queued = 0;
-			list_del (&tx->tx_list);
-			list_add (&tx->tx_list, &zombies);
+			list_del(&tx->tx_list);
+			list_add(&tx->tx_list, &zombies);
 		}
 	}
 
@@ -1991,10 +1990,10 @@ kiblnd_abort_txs(kib_conn_t *conn, struct list_head *txs)
 }
 
 static void
-kiblnd_finalise_conn (kib_conn_t *conn)
+kiblnd_finalise_conn(kib_conn_t *conn)
 {
-	LASSERT (!in_interrupt());
-	LASSERT (conn->ibc_state > IBLND_CONN_INIT);
+	LASSERT(!in_interrupt());
+	LASSERT(conn->ibc_state > IBLND_CONN_INIT);
 
 	kiblnd_set_conn_state(conn, IBLND_CONN_DISCONNECTED);
 
@@ -2016,21 +2015,21 @@ kiblnd_finalise_conn (kib_conn_t *conn)
 }
 
 void
-kiblnd_peer_connect_failed (kib_peer_t *peer, int active, int error)
+kiblnd_peer_connect_failed(kib_peer_t *peer, int active, int error)
 {
-	LIST_HEAD    (zombies);
+	LIST_HEAD(zombies);
 	unsigned long     flags;
 
-	LASSERT (error != 0);
-	LASSERT (!in_interrupt());
+	LASSERT(error != 0);
+	LASSERT(!in_interrupt());
 
 	write_lock_irqsave(&kiblnd_data.kib_global_lock, flags);
 
 	if (active) {
-		LASSERT (peer->ibp_connecting > 0);
+		LASSERT(peer->ibp_connecting > 0);
 		peer->ibp_connecting--;
 	} else {
-		LASSERT (peer->ibp_accepting > 0);
+		LASSERT(peer->ibp_accepting > 0);
 		peer->ibp_accepting--;
 	}
 
@@ -2053,14 +2052,14 @@ kiblnd_peer_connect_failed (kib_peer_t *peer, int active, int error)
 		peer->ibp_error = error;
 	} else {
 		/* Can't have blocked transmits if there are connections */
-		LASSERT (list_empty(&peer->ibp_tx_queue));
+		LASSERT(list_empty(&peer->ibp_tx_queue));
 	}
 
 	write_unlock_irqrestore(&kiblnd_data.kib_global_lock, flags);
 
 	kiblnd_peer_notify(peer);
 
-	if (list_empty (&zombies))
+	if (list_empty(&zombies))
 		return;
 
 	CNETERR("Deleting messages for %s: connection failed\n",
@@ -2074,6 +2073,7 @@ kiblnd_connreq_done(kib_conn_t *conn, int status)
 {
 	kib_peer_t	*peer = conn->ibc_peer;
 	kib_tx_t	  *tx;
+	kib_tx_t *tmp;
 	struct list_head	 txs;
 	unsigned long      flags;
 	int		active;
@@ -2084,8 +2084,8 @@ kiblnd_connreq_done(kib_conn_t *conn, int status)
 	       libcfs_nid2str(peer->ibp_nid), active,
 	       conn->ibc_version, status);
 
-	LASSERT (!in_interrupt());
-	LASSERT ((conn->ibc_state == IBLND_CONN_ACTIVE_CONNECT &&
+	LASSERT(!in_interrupt());
+	LASSERT((conn->ibc_state == IBLND_CONN_ACTIVE_CONNECT &&
 		  peer->ibp_connecting > 0) ||
 		 (conn->ibc_state == IBLND_CONN_PASSIVE_WAIT &&
 		  peer->ibp_accepting > 0));
@@ -2150,8 +2150,7 @@ kiblnd_connreq_done(kib_conn_t *conn, int status)
 
 	/* Schedule blocked txs */
 	spin_lock(&conn->ibc_lock);
-	while (!list_empty(&txs)) {
-		tx = list_entry(txs.next, kib_tx_t, tx_list);
+	list_for_each_entry_safe(tx, tmp, &txs, tx_list) {
 		list_del(&tx->tx_list);
 
 		kiblnd_queue_tx_locked(tx, conn);
@@ -2176,7 +2175,7 @@ kiblnd_reject(struct rdma_cm_id *cmid, kib_rej_t *rej)
 }
 
 static int
-kiblnd_passive_connect (struct rdma_cm_id *cmid, void *priv, int priv_nob)
+kiblnd_passive_connect(struct rdma_cm_id *cmid, void *priv, int priv_nob)
 {
 	rwlock_t		*g_lock = &kiblnd_data.kib_global_lock;
 	kib_msg_t	     *reqmsg = priv;
@@ -2194,11 +2193,11 @@ kiblnd_passive_connect (struct rdma_cm_id *cmid, void *priv, int priv_nob)
 	unsigned long	  flags;
 	int		    rc;
 	struct sockaddr_in    *peer_addr;
-	LASSERT (!in_interrupt());
+	LASSERT(!in_interrupt());
 
 	/* cmid inherits 'context' from the corresponding listener id */
 	ibdev = (kib_dev_t *)cmid->context;
-	LASSERT (ibdev != NULL);
+	LASSERT(ibdev != NULL);
 
 	memset(&rej, 0, sizeof(rej));
 	rej.ibr_magic		= IBLND_MSG_MAGIC;
@@ -2366,8 +2365,8 @@ kiblnd_passive_connect (struct rdma_cm_id *cmid, void *priv, int priv_nob)
 		peer = peer2;
 	} else {
 		/* Brand new peer */
-		LASSERT (peer->ibp_accepting == 0);
-		LASSERT (peer->ibp_version == 0 &&
+		LASSERT(peer->ibp_accepting == 0);
+		LASSERT(peer->ibp_version == 0 &&
 			 peer->ibp_incarnation == 0);
 
 		peer->ibp_accepting   = 1;
@@ -2375,7 +2374,7 @@ kiblnd_passive_connect (struct rdma_cm_id *cmid, void *priv, int priv_nob)
 		peer->ibp_incarnation = reqmsg->ibm_srcstamp;
 
 		/* I have a ref on ni that prevents it being shutdown */
-		LASSERT (net->ibn_shutdown == 0);
+		LASSERT(net->ibn_shutdown == 0);
 
 		kiblnd_peer_addref(peer);
 		list_add_tail(&peer->ibp_list, kiblnd_nid2peerlist(nid));
@@ -2397,7 +2396,7 @@ kiblnd_passive_connect (struct rdma_cm_id *cmid, void *priv, int priv_nob)
 	conn->ibc_incarnation      = reqmsg->ibm_srcstamp;
 	conn->ibc_credits	  = IBLND_MSG_QUEUE_SIZE(version);
 	conn->ibc_reserved_credits = IBLND_MSG_QUEUE_SIZE(version);
-	LASSERT (conn->ibc_credits + conn->ibc_reserved_credits + IBLND_OOB_MSGS(version)
+	LASSERT(conn->ibc_credits + conn->ibc_reserved_credits + IBLND_OOB_MSGS(version)
 		 <= IBLND_RX_MSGS(version));
 
 	ackmsg = &conn->ibc_connvars->cv_msg;
@@ -2449,7 +2448,7 @@ kiblnd_passive_connect (struct rdma_cm_id *cmid, void *priv, int priv_nob)
 }
 
 static void
-kiblnd_reconnect (kib_conn_t *conn, int version,
+kiblnd_reconnect(kib_conn_t *conn, int version,
 		  __u64 incarnation, int why, kib_connparams_t *cp)
 {
 	kib_peer_t    *peer = conn->ibc_peer;
@@ -2457,8 +2456,8 @@ kiblnd_reconnect (kib_conn_t *conn, int version,
 	int	    retry = 0;
 	unsigned long  flags;
 
-	LASSERT (conn->ibc_state == IBLND_CONN_ACTIVE_CONNECT);
-	LASSERT (peer->ibp_connecting > 0);     /* 'conn' at least */
+	LASSERT(conn->ibc_state == IBLND_CONN_ACTIVE_CONNECT);
+	LASSERT(peer->ibp_connecting > 0);     /* 'conn' at least */
 
 	write_lock_irqsave(&kiblnd_data.kib_global_lock, flags);
 
@@ -2504,20 +2503,20 @@ kiblnd_reconnect (kib_conn_t *conn, int version,
 	CNETERR("%s: retrying (%s), %x, %x, queue_dep: %d, max_frag: %d, msg_size: %d\n",
 		libcfs_nid2str(peer->ibp_nid),
 		reason, IBLND_MSG_VERSION, version,
-		cp != NULL? cp->ibcp_queue_depth :IBLND_MSG_QUEUE_SIZE(version),
-		cp != NULL? cp->ibcp_max_frags   : IBLND_RDMA_FRAGS(version),
-		cp != NULL? cp->ibcp_max_msg_size: IBLND_MSG_SIZE);
+		cp != NULL ? cp->ibcp_queue_depth  : IBLND_MSG_QUEUE_SIZE(version),
+		cp != NULL ? cp->ibcp_max_frags    : IBLND_RDMA_FRAGS(version),
+		cp != NULL ? cp->ibcp_max_msg_size : IBLND_MSG_SIZE);
 
 	kiblnd_connect_peer(peer);
 }
 
 static void
-kiblnd_rejected (kib_conn_t *conn, int reason, void *priv, int priv_nob)
+kiblnd_rejected(kib_conn_t *conn, int reason, void *priv, int priv_nob)
 {
 	kib_peer_t    *peer = conn->ibc_peer;
 
-	LASSERT (!in_interrupt());
-	LASSERT (conn->ibc_state == IBLND_CONN_ACTIVE_CONNECT);
+	LASSERT(!in_interrupt());
+	LASSERT(conn->ibc_state == IBLND_CONN_ACTIVE_CONNECT);
 
 	switch (reason) {
 	case IB_CM_REJ_STALE_CONN:
@@ -2651,7 +2650,7 @@ kiblnd_rejected (kib_conn_t *conn, int reason, void *priv, int priv_nob)
 }
 
 static void
-kiblnd_check_connreply (kib_conn_t *conn, void *priv, int priv_nob)
+kiblnd_check_connreply(kib_conn_t *conn, void *priv, int priv_nob)
 {
 	kib_peer_t    *peer = conn->ibc_peer;
 	lnet_ni_t     *ni   = peer->ibp_ni;
@@ -2661,7 +2660,7 @@ kiblnd_check_connreply (kib_conn_t *conn, void *priv, int priv_nob)
 	int	    rc   = kiblnd_unpack_msg(msg, priv_nob);
 	unsigned long  flags;
 
-	LASSERT (net != NULL);
+	LASSERT(net != NULL);
 
 	if (rc != 0) {
 		CERROR("Can't unpack connack from %s: %d\n",
@@ -2730,7 +2729,7 @@ kiblnd_check_connreply (kib_conn_t *conn, void *priv, int priv_nob)
 	conn->ibc_incarnation      = msg->ibm_srcstamp;
 	conn->ibc_credits	  =
 	conn->ibc_reserved_credits = IBLND_MSG_QUEUE_SIZE(ver);
-	LASSERT (conn->ibc_credits + conn->ibc_reserved_credits + IBLND_OOB_MSGS(ver)
+	LASSERT(conn->ibc_credits + conn->ibc_reserved_credits + IBLND_OOB_MSGS(ver)
 		 <= IBLND_RX_MSGS(ver));
 
 	kiblnd_connreq_done(conn, 0);
@@ -2742,13 +2741,13 @@ kiblnd_check_connreply (kib_conn_t *conn, void *priv, int priv_nob)
 	 * kiblnd_connreq_done(0) moves the conn state to ESTABLISHED, but then
 	 * immediately tears it down. */
 
-	LASSERT (rc != 0);
+	LASSERT(rc != 0);
 	conn->ibc_comms_error = rc;
 	kiblnd_connreq_done(conn, 0);
 }
 
 static int
-kiblnd_active_connect (struct rdma_cm_id *cmid)
+kiblnd_active_connect(struct rdma_cm_id *cmid)
 {
 	kib_peer_t	      *peer = (kib_peer_t *)cmid->context;
 	kib_conn_t	      *conn;
@@ -2913,7 +2912,7 @@ kiblnd_cm_callback(struct rdma_cm_id *cmid, struct rdma_cm_event *event)
 			LBUG();
 
 		case IBLND_CONN_PASSIVE_WAIT:
-			CERROR ("%s: REJECTED %d\n",
+			CERROR("%s: REJECTED %d\n",
 				libcfs_nid2str(conn->ibc_peer->ibp_nid),
 				event->status);
 			kiblnd_connreq_done(conn, -ECONNRESET);
@@ -2987,17 +2986,17 @@ kiblnd_check_txs_locked(kib_conn_t *conn, struct list_head *txs)
 	kib_tx_t	  *tx;
 	struct list_head	*ttmp;
 
-	list_for_each (ttmp, txs) {
-		tx = list_entry (ttmp, kib_tx_t, tx_list);
+	list_for_each(ttmp, txs) {
+		tx = list_entry(ttmp, kib_tx_t, tx_list);
 
 		if (txs != &conn->ibc_active_txs) {
-			LASSERT (tx->tx_queued);
+			LASSERT(tx->tx_queued);
 		} else {
-			LASSERT (!tx->tx_queued);
-			LASSERT (tx->tx_waiting || tx->tx_sending != 0);
+			LASSERT(!tx->tx_queued);
+			LASSERT(tx->tx_waiting || tx->tx_sending != 0);
 		}
 
-		if (cfs_time_aftereq (jiffies, tx->tx_deadline)) {
+		if (cfs_time_aftereq(jiffies, tx->tx_deadline)) {
 			CERROR("Timed out tx: %s, %lu seconds\n",
 			       kiblnd_queue2str(conn, txs),
 			       cfs_duration_sec(jiffies - tx->tx_deadline));
@@ -3019,14 +3018,15 @@ kiblnd_conn_timed_out_locked(kib_conn_t *conn)
 }
 
 static void
-kiblnd_check_conns (int idx)
+kiblnd_check_conns(int idx)
 {
-	LIST_HEAD (closes);
-	LIST_HEAD (checksends);
+	LIST_HEAD(closes);
+	LIST_HEAD(checksends);
 	struct list_head    *peers = &kiblnd_data.kib_peers[idx];
 	struct list_head    *ptmp;
 	kib_peer_t    *peer;
 	kib_conn_t    *conn;
+	kib_conn_t *tmp;
 	struct list_head    *ctmp;
 	unsigned long  flags;
 
@@ -3035,16 +3035,16 @@ kiblnd_check_conns (int idx)
 	 * take a look... */
 	read_lock_irqsave(&kiblnd_data.kib_global_lock, flags);
 
-	list_for_each (ptmp, peers) {
-		peer = list_entry (ptmp, kib_peer_t, ibp_list);
+	list_for_each(ptmp, peers) {
+		peer = list_entry(ptmp, kib_peer_t, ibp_list);
 
-		list_for_each (ctmp, &peer->ibp_conns) {
+		list_for_each(ctmp, &peer->ibp_conns) {
 			int timedout;
 			int sendnoop;
 
 			conn = list_entry(ctmp, kib_conn_t, ibc_list);
 
-			LASSERT (conn->ibc_state == IBLND_CONN_ESTABLISHED);
+			LASSERT(conn->ibc_state == IBLND_CONN_ESTABLISHED);
 
 			spin_lock(&conn->ibc_lock);
 
@@ -3080,9 +3080,7 @@ kiblnd_check_conns (int idx)
 	/* Handle timeout by closing the whole
 	 * connection. We can only be sure RDMA activity
 	 * has ceased once the QP has been modified. */
-	while (!list_empty(&closes)) {
-		conn = list_entry(closes.next,
-				      kib_conn_t, ibc_connd_list);
+	list_for_each_entry_safe(conn, tmp, &closes, ibc_connd_list) {
 		list_del(&conn->ibc_connd_list);
 		kiblnd_close_conn(conn, -ETIMEDOUT);
 		kiblnd_conn_decref(conn);
@@ -3101,11 +3099,11 @@ kiblnd_check_conns (int idx)
 }
 
 static void
-kiblnd_disconnect_conn (kib_conn_t *conn)
+kiblnd_disconnect_conn(kib_conn_t *conn)
 {
-	LASSERT (!in_interrupt());
-	LASSERT (current == kiblnd_data.kib_connd);
-	LASSERT (conn->ibc_state == IBLND_CONN_CLOSING);
+	LASSERT(!in_interrupt());
+	LASSERT(current == kiblnd_data.kib_connd);
+	LASSERT(conn->ibc_state == IBLND_CONN_CLOSING);
 
 	rdma_disconnect(conn->ibc_cmid);
 	kiblnd_finalise_conn(conn);
@@ -3114,7 +3112,7 @@ kiblnd_disconnect_conn (kib_conn_t *conn)
 }
 
 int
-kiblnd_connd (void *arg)
+kiblnd_connd(void *arg)
 {
 	wait_queue_t     wait;
 	unsigned long      flags;
@@ -3125,7 +3123,7 @@ kiblnd_connd (void *arg)
 	int		peer_index = 0;
 	unsigned long      deadline = jiffies;
 
-	cfs_block_allsigs ();
+	cfs_block_allsigs();
 
 	init_waitqueue_entry(&wait, current);
 	kiblnd_data.kib_connd = current;
@@ -3136,7 +3134,7 @@ kiblnd_connd (void *arg)
 
 		dropped_lock = 0;
 
-		if (!list_empty (&kiblnd_data.kib_connd_zombies)) {
+		if (!list_empty(&kiblnd_data.kib_connd_zombies)) {
 			conn = list_entry(kiblnd_data. \
 					      kib_connd_zombies.next,
 					      kib_conn_t, ibc_list);
@@ -3239,7 +3237,7 @@ kiblnd_qp_event(struct ib_event *event, void *arg)
 }
 
 static void
-kiblnd_complete (struct ib_wc *wc)
+kiblnd_complete(struct ib_wc *wc)
 {
 	switch (kiblnd_wreqid2type(wc->wr_id)) {
 	default:
@@ -3440,9 +3438,9 @@ kiblnd_failover_thread(void *arg)
 	unsigned long      flags;
 	int		rc;
 
-	LASSERT (*kiblnd_tunables.kib_dev_failover != 0);
+	LASSERT(*kiblnd_tunables.kib_dev_failover != 0);
 
-	cfs_block_allsigs ();
+	cfs_block_allsigs();
 
 	init_waitqueue_entry(&wait, current);
 	write_lock_irqsave(glock, flags);
@@ -3469,7 +3467,7 @@ kiblnd_failover_thread(void *arg)
 
 			write_lock_irqsave(glock, flags);
 
-			LASSERT (dev->ibd_failover);
+			LASSERT(dev->ibd_failover);
 			dev->ibd_failover = 0;
 			if (rc >= 0) { /* Device is OK or failover succeed */
 				dev->ibd_next_failover = cfs_time_shift(3);
diff --git a/drivers/staging/lustre/lnet/klnds/o2iblnd/o2iblnd_modparams.c b/drivers/staging/lustre/lnet/klnds/o2iblnd/o2iblnd_modparams.c
index 8b4a8e9a29b4..eedf01afd57f 100644
--- a/drivers/staging/lustre/lnet/klnds/o2iblnd/o2iblnd_modparams.c
+++ b/drivers/staging/lustre/lnet/klnds/o2iblnd/o2iblnd_modparams.c
@@ -44,7 +44,7 @@ static int service = 987;
 module_param(service, int, 0444);
 MODULE_PARM_DESC(service, "service number (within RDMA_PS_TCP)");
 
-static int cksum = 0;
+static int cksum;
 module_param(cksum, int, 0644);
 MODULE_PARM_DESC(cksum, "set non-zero to enable message (not RDMA) checksums");
 
@@ -72,11 +72,11 @@ static int peer_credits = 8;
 module_param(peer_credits, int, 0444);
 MODULE_PARM_DESC(peer_credits, "# concurrent sends to 1 peer");
 
-static int peer_credits_hiw = 0;
+static int peer_credits_hiw;
 module_param(peer_credits_hiw, int, 0444);
 MODULE_PARM_DESC(peer_credits_hiw, "when eagerly to return credits");
 
-static int peer_buffer_credits = 0;
+static int peer_buffer_credits;
 module_param(peer_buffer_credits, int, 0444);
 MODULE_PARM_DESC(peer_buffer_credits, "# per-peer router buffer credits");
 
@@ -100,15 +100,15 @@ static int keepalive = 100;
 module_param(keepalive, int, 0644);
 MODULE_PARM_DESC(keepalive, "Idle time in seconds before sending a keepalive");
 
-static int ib_mtu = 0;
+static int ib_mtu;
 module_param(ib_mtu, int, 0444);
 MODULE_PARM_DESC(ib_mtu, "IB MTU 256/512/1024/2048/4096");
 
-static int concurrent_sends = 0;
+static int concurrent_sends;
 module_param(concurrent_sends, int, 0444);
 MODULE_PARM_DESC(concurrent_sends, "send work-queue sizing");
 
-static int map_on_demand = 0;
+static int map_on_demand;
 module_param(map_on_demand, int, 0444);
 MODULE_PARM_DESC(map_on_demand, "map on demand");
 
@@ -136,12 +136,12 @@ MODULE_PARM_DESC(pmr_pool_size, "size of MR cache pmr pool on each CPT");
  * 1: enable failover if necessary
  * 2: force to failover (for debug)
  */
-static int dev_failover = 0;
+static int dev_failover;
 module_param(dev_failover, int, 0444);
 MODULE_PARM_DESC(dev_failover, "HCA failover for bonding (0 off, 1 on, other values reserved)");
 
 
-static int require_privileged_port = 0;
+static int require_privileged_port;
 module_param(require_privileged_port, int, 0644);
 MODULE_PARM_DESC(require_privileged_port, "require privileged port when accepting connection");
 
@@ -177,7 +177,7 @@ kib_tunables_t kiblnd_tunables = {
 };
 
 int
-kiblnd_tunables_init (void)
+kiblnd_tunables_init(void)
 {
 	if (kiblnd_translate_mtu(*kiblnd_tunables.kib_ib_mtu) < 0) {
 		CERROR("Invalid ib_mtu %d, expected 256/512/1024/2048/4096\n",
diff --git a/drivers/staging/lustre/lnet/klnds/socklnd/socklnd.c b/drivers/staging/lustre/lnet/klnds/socklnd/socklnd.c
index 5956dbac5d04..7586b7e4040b 100644
--- a/drivers/staging/lustre/lnet/klnds/socklnd/socklnd.c
+++ b/drivers/staging/lustre/lnet/klnds/socklnd/socklnd.c
@@ -65,15 +65,15 @@ ksocknal_ip2iface(lnet_ni_t *ni, __u32 ip)
 }
 
 static ksock_route_t *
-ksocknal_create_route (__u32 ipaddr, int port)
+ksocknal_create_route(__u32 ipaddr, int port)
 {
 	ksock_route_t *route;
 
-	LIBCFS_ALLOC (route, sizeof (*route));
+	LIBCFS_ALLOC(route, sizeof(*route));
 	if (route == NULL)
 		return NULL;
 
-	atomic_set (&route->ksnr_refcount, 1);
+	atomic_set(&route->ksnr_refcount, 1);
 	route->ksnr_peer = NULL;
 	route->ksnr_retry_interval = 0;	 /* OK to connect at any time */
 	route->ksnr_ipaddr = ipaddr;
@@ -89,43 +89,43 @@ ksocknal_create_route (__u32 ipaddr, int port)
 }
 
 void
-ksocknal_destroy_route (ksock_route_t *route)
+ksocknal_destroy_route(ksock_route_t *route)
 {
-	LASSERT (atomic_read(&route->ksnr_refcount) == 0);
+	LASSERT(atomic_read(&route->ksnr_refcount) == 0);
 
 	if (route->ksnr_peer != NULL)
 		ksocknal_peer_decref(route->ksnr_peer);
 
-	LIBCFS_FREE (route, sizeof (*route));
+	LIBCFS_FREE(route, sizeof(*route));
 }
 
 static int
-ksocknal_create_peer (ksock_peer_t **peerp, lnet_ni_t *ni, lnet_process_id_t id)
+ksocknal_create_peer(ksock_peer_t **peerp, lnet_ni_t *ni, lnet_process_id_t id)
 {
 	ksock_net_t   *net = ni->ni_data;
 	ksock_peer_t  *peer;
 
-	LASSERT (id.nid != LNET_NID_ANY);
-	LASSERT (id.pid != LNET_PID_ANY);
-	LASSERT (!in_interrupt());
+	LASSERT(id.nid != LNET_NID_ANY);
+	LASSERT(id.pid != LNET_PID_ANY);
+	LASSERT(!in_interrupt());
 
-	LIBCFS_ALLOC (peer, sizeof (*peer));
+	LIBCFS_ALLOC(peer, sizeof(*peer));
 	if (peer == NULL)
 		return -ENOMEM;
 
 	peer->ksnp_ni = ni;
 	peer->ksnp_id = id;
-	atomic_set (&peer->ksnp_refcount, 1);   /* 1 ref for caller */
+	atomic_set(&peer->ksnp_refcount, 1);   /* 1 ref for caller */
 	peer->ksnp_closing = 0;
 	peer->ksnp_accepting = 0;
 	peer->ksnp_proto = NULL;
 	peer->ksnp_last_alive = 0;
 	peer->ksnp_zc_next_cookie = SOCKNAL_KEEPALIVE_PING + 1;
 
-	INIT_LIST_HEAD (&peer->ksnp_conns);
-	INIT_LIST_HEAD (&peer->ksnp_routes);
-	INIT_LIST_HEAD (&peer->ksnp_tx_queue);
-	INIT_LIST_HEAD (&peer->ksnp_zc_req_list);
+	INIT_LIST_HEAD(&peer->ksnp_conns);
+	INIT_LIST_HEAD(&peer->ksnp_routes);
+	INIT_LIST_HEAD(&peer->ksnp_tx_queue);
+	INIT_LIST_HEAD(&peer->ksnp_zc_req_list);
 	spin_lock_init(&peer->ksnp_lock);
 
 	spin_lock_bh(&net->ksnn_lock);
@@ -147,21 +147,21 @@ ksocknal_create_peer (ksock_peer_t **peerp, lnet_ni_t *ni, lnet_process_id_t id)
 }
 
 void
-ksocknal_destroy_peer (ksock_peer_t *peer)
+ksocknal_destroy_peer(ksock_peer_t *peer)
 {
 	ksock_net_t    *net = peer->ksnp_ni->ni_data;
 
-	CDEBUG (D_NET, "peer %s %p deleted\n",
+	CDEBUG(D_NET, "peer %s %p deleted\n",
 		libcfs_id2str(peer->ksnp_id), peer);
 
-	LASSERT (atomic_read (&peer->ksnp_refcount) == 0);
-	LASSERT (peer->ksnp_accepting == 0);
-	LASSERT (list_empty (&peer->ksnp_conns));
-	LASSERT (list_empty (&peer->ksnp_routes));
-	LASSERT (list_empty (&peer->ksnp_tx_queue));
-	LASSERT (list_empty (&peer->ksnp_zc_req_list));
+	LASSERT(atomic_read(&peer->ksnp_refcount) == 0);
+	LASSERT(peer->ksnp_accepting == 0);
+	LASSERT(list_empty(&peer->ksnp_conns));
+	LASSERT(list_empty(&peer->ksnp_routes));
+	LASSERT(list_empty(&peer->ksnp_tx_queue));
+	LASSERT(list_empty(&peer->ksnp_zc_req_list));
 
-	LIBCFS_FREE (peer, sizeof (*peer));
+	LIBCFS_FREE(peer, sizeof(*peer));
 
 	/* NB a peer's connections and routes keep a reference on their peer
 	 * until they are destroyed, so we can be assured that _all_ state to
@@ -173,17 +173,17 @@ ksocknal_destroy_peer (ksock_peer_t *peer)
 }
 
 ksock_peer_t *
-ksocknal_find_peer_locked (lnet_ni_t *ni, lnet_process_id_t id)
+ksocknal_find_peer_locked(lnet_ni_t *ni, lnet_process_id_t id)
 {
 	struct list_head       *peer_list = ksocknal_nid2peerlist(id.nid);
 	struct list_head       *tmp;
 	ksock_peer_t     *peer;
 
-	list_for_each (tmp, peer_list) {
+	list_for_each(tmp, peer_list) {
 
-		peer = list_entry (tmp, ksock_peer_t, ksnp_list);
+		peer = list_entry(tmp, ksock_peer_t, ksnp_list);
 
-		LASSERT (!peer->ksnp_closing);
+		LASSERT(!peer->ksnp_closing);
 
 		if (peer->ksnp_ni != ni)
 			continue;
@@ -201,7 +201,7 @@ ksocknal_find_peer_locked (lnet_ni_t *ni, lnet_process_id_t id)
 }
 
 ksock_peer_t *
-ksocknal_find_peer (lnet_ni_t *ni, lnet_process_id_t id)
+ksocknal_find_peer(lnet_ni_t *ni, lnet_process_id_t id)
 {
 	ksock_peer_t     *peer;
 
@@ -215,37 +215,37 @@ ksocknal_find_peer (lnet_ni_t *ni, lnet_process_id_t id)
 }
 
 static void
-ksocknal_unlink_peer_locked (ksock_peer_t *peer)
+ksocknal_unlink_peer_locked(ksock_peer_t *peer)
 {
 	int		i;
 	__u32	      ip;
 	ksock_interface_t *iface;
 
 	for (i = 0; i < peer->ksnp_n_passive_ips; i++) {
-		LASSERT (i < LNET_MAX_INTERFACES);
+		LASSERT(i < LNET_MAX_INTERFACES);
 		ip = peer->ksnp_passive_ips[i];
 
 		iface = ksocknal_ip2iface(peer->ksnp_ni, ip);
 		/* All IPs in peer->ksnp_passive_ips[] come from the
 		 * interface list, therefore the call must succeed. */
-		LASSERT (iface != NULL);
+		LASSERT(iface != NULL);
 
 		CDEBUG(D_NET, "peer=%p iface=%p ksni_nroutes=%d\n",
 		       peer, iface, iface->ksni_nroutes);
 		iface->ksni_npeers--;
 	}
 
-	LASSERT (list_empty(&peer->ksnp_conns));
-	LASSERT (list_empty(&peer->ksnp_routes));
-	LASSERT (!peer->ksnp_closing);
+	LASSERT(list_empty(&peer->ksnp_conns));
+	LASSERT(list_empty(&peer->ksnp_routes));
+	LASSERT(!peer->ksnp_closing);
 	peer->ksnp_closing = 1;
-	list_del (&peer->ksnp_list);
+	list_del(&peer->ksnp_list);
 	/* lose peerlist's ref */
 	ksocknal_peer_decref(peer);
 }
 
 static int
-ksocknal_get_peer_info (lnet_ni_t *ni, int index,
+ksocknal_get_peer_info(lnet_ni_t *ni, int index,
 			lnet_process_id_t *id, __u32 *myip, __u32 *peer_ip,
 			int *port, int *conn_count, int *share_count)
 {
@@ -261,8 +261,8 @@ ksocknal_get_peer_info (lnet_ni_t *ni, int index,
 
 	for (i = 0; i < ksocknal_data.ksnd_peer_hash_size; i++) {
 
-		list_for_each (ptmp, &ksocknal_data.ksnd_peers[i]) {
-			peer = list_entry (ptmp, ksock_peer_t, ksnp_list);
+		list_for_each(ptmp, &ksocknal_data.ksnd_peers[i]) {
+			peer = list_entry(ptmp, ksock_peer_t, ksnp_list);
 
 			if (peer->ksnp_ni != ni)
 				continue;
@@ -296,7 +296,7 @@ ksocknal_get_peer_info (lnet_ni_t *ni, int index,
 				goto out;
 			}
 
-			list_for_each (rtmp, &peer->ksnp_routes) {
+			list_for_each(rtmp, &peer->ksnp_routes) {
 				if (index-- > 0)
 					continue;
 
@@ -364,17 +364,17 @@ ksocknal_associate_route_conn_locked(ksock_route_t *route, ksock_conn_t *conn)
 }
 
 static void
-ksocknal_add_route_locked (ksock_peer_t *peer, ksock_route_t *route)
+ksocknal_add_route_locked(ksock_peer_t *peer, ksock_route_t *route)
 {
 	struct list_head	*tmp;
 	ksock_conn_t      *conn;
 	ksock_route_t     *route2;
 
-	LASSERT (!peer->ksnp_closing);
-	LASSERT (route->ksnr_peer == NULL);
-	LASSERT (!route->ksnr_scheduled);
-	LASSERT (!route->ksnr_connecting);
-	LASSERT (route->ksnr_connected == 0);
+	LASSERT(!peer->ksnp_closing);
+	LASSERT(route->ksnr_peer == NULL);
+	LASSERT(!route->ksnr_scheduled);
+	LASSERT(!route->ksnr_connecting);
+	LASSERT(route->ksnr_connected == 0);
 
 	/* LASSERT(unique) */
 	list_for_each(tmp, &peer->ksnp_routes) {
@@ -405,7 +405,7 @@ ksocknal_add_route_locked (ksock_peer_t *peer, ksock_route_t *route)
 }
 
 static void
-ksocknal_del_route_locked (ksock_route_t *route)
+ksocknal_del_route_locked(ksock_route_t *route)
 {
 	ksock_peer_t      *peer = route->ksnr_peer;
 	ksock_interface_t *iface;
@@ -413,16 +413,16 @@ ksocknal_del_route_locked (ksock_route_t *route)
 	struct list_head	*ctmp;
 	struct list_head	*cnxt;
 
-	LASSERT (!route->ksnr_deleted);
+	LASSERT(!route->ksnr_deleted);
 
 	/* Close associated conns */
-	list_for_each_safe (ctmp, cnxt, &peer->ksnp_conns) {
+	list_for_each_safe(ctmp, cnxt, &peer->ksnp_conns) {
 		conn = list_entry(ctmp, ksock_conn_t, ksnc_list);
 
 		if (conn->ksnc_route != route)
 			continue;
 
-		ksocknal_close_conn_locked (conn, 0);
+		ksocknal_close_conn_locked(conn, 0);
 	}
 
 	if (route->ksnr_myipaddr != 0) {
@@ -433,19 +433,19 @@ ksocknal_del_route_locked (ksock_route_t *route)
 	}
 
 	route->ksnr_deleted = 1;
-	list_del (&route->ksnr_list);
+	list_del(&route->ksnr_list);
 	ksocknal_route_decref(route);	     /* drop peer's ref */
 
-	if (list_empty (&peer->ksnp_routes) &&
-	    list_empty (&peer->ksnp_conns)) {
+	if (list_empty(&peer->ksnp_routes) &&
+	    list_empty(&peer->ksnp_conns)) {
 		/* I've just removed the last route to a peer with no active
 		 * connections */
-		ksocknal_unlink_peer_locked (peer);
+		ksocknal_unlink_peer_locked(peer);
 	}
 }
 
 int
-ksocknal_add_peer (lnet_ni_t *ni, lnet_process_id_t id, __u32 ipaddr, int port)
+ksocknal_add_peer(lnet_ni_t *ni, lnet_process_id_t id, __u32 ipaddr, int port)
 {
 	struct list_head	*tmp;
 	ksock_peer_t      *peer;
@@ -463,7 +463,7 @@ ksocknal_add_peer (lnet_ni_t *ni, lnet_process_id_t id, __u32 ipaddr, int port)
 	if (rc != 0)
 		return rc;
 
-	route = ksocknal_create_route (ipaddr, port);
+	route = ksocknal_create_route(ipaddr, port);
 	if (route == NULL) {
 		ksocknal_peer_decref(peer);
 		return -ENOMEM;
@@ -472,20 +472,20 @@ ksocknal_add_peer (lnet_ni_t *ni, lnet_process_id_t id, __u32 ipaddr, int port)
 	write_lock_bh(&ksocknal_data.ksnd_global_lock);
 
 	/* always called with a ref on ni, so shutdown can't have started */
-	LASSERT (((ksock_net_t *) ni->ni_data)->ksnn_shutdown == 0);
+	LASSERT(((ksock_net_t *) ni->ni_data)->ksnn_shutdown == 0);
 
-	peer2 = ksocknal_find_peer_locked (ni, id);
+	peer2 = ksocknal_find_peer_locked(ni, id);
 	if (peer2 != NULL) {
 		ksocknal_peer_decref(peer);
 		peer = peer2;
 	} else {
 		/* peer table takes my ref on peer */
-		list_add_tail (&peer->ksnp_list,
-				   ksocknal_nid2peerlist (id.nid));
+		list_add_tail(&peer->ksnp_list,
+				   ksocknal_nid2peerlist(id.nid));
 	}
 
 	route2 = NULL;
-	list_for_each (tmp, &peer->ksnp_routes) {
+	list_for_each(tmp, &peer->ksnp_routes) {
 		route2 = list_entry(tmp, ksock_route_t, ksnr_list);
 
 		if (route2->ksnr_ipaddr == ipaddr)
@@ -507,7 +507,7 @@ ksocknal_add_peer (lnet_ni_t *ni, lnet_process_id_t id, __u32 ipaddr, int port)
 }
 
 static void
-ksocknal_del_peer_locked (ksock_peer_t *peer, __u32 ip)
+ksocknal_del_peer_locked(ksock_peer_t *peer, __u32 ip)
 {
 	ksock_conn_t     *conn;
 	ksock_route_t    *route;
@@ -515,12 +515,12 @@ ksocknal_del_peer_locked (ksock_peer_t *peer, __u32 ip)
 	struct list_head       *nxt;
 	int	       nshared;
 
-	LASSERT (!peer->ksnp_closing);
+	LASSERT(!peer->ksnp_closing);
 
 	/* Extra ref prevents peer disappearing until I'm done with it */
 	ksocknal_peer_addref(peer);
 
-	list_for_each_safe (tmp, nxt, &peer->ksnp_routes) {
+	list_for_each_safe(tmp, nxt, &peer->ksnp_routes) {
 		route = list_entry(tmp, ksock_route_t, ksnr_list);
 
 		/* no match */
@@ -529,11 +529,11 @@ ksocknal_del_peer_locked (ksock_peer_t *peer, __u32 ip)
 
 		route->ksnr_share_count = 0;
 		/* This deletes associated conns too */
-		ksocknal_del_route_locked (route);
+		ksocknal_del_route_locked(route);
 	}
 
 	nshared = 0;
-	list_for_each_safe (tmp, nxt, &peer->ksnp_routes) {
+	list_for_each_safe(tmp, nxt, &peer->ksnp_routes) {
 		route = list_entry(tmp, ksock_route_t, ksnr_list);
 		nshared += route->ksnr_share_count;
 	}
@@ -542,15 +542,15 @@ ksocknal_del_peer_locked (ksock_peer_t *peer, __u32 ip)
 		/* remove everything else if there are no explicit entries
 		 * left */
 
-		list_for_each_safe (tmp, nxt, &peer->ksnp_routes) {
+		list_for_each_safe(tmp, nxt, &peer->ksnp_routes) {
 			route = list_entry(tmp, ksock_route_t, ksnr_list);
 
 			/* we should only be removing auto-entries */
 			LASSERT(route->ksnr_share_count == 0);
-			ksocknal_del_route_locked (route);
+			ksocknal_del_route_locked(route);
 		}
 
-		list_for_each_safe (tmp, nxt, &peer->ksnp_conns) {
+		list_for_each_safe(tmp, nxt, &peer->ksnp_conns) {
 			conn = list_entry(tmp, ksock_conn_t, ksnc_list);
 
 			ksocknal_close_conn_locked(conn, 0);
@@ -562,9 +562,9 @@ ksocknal_del_peer_locked (ksock_peer_t *peer, __u32 ip)
 }
 
 static int
-ksocknal_del_peer (lnet_ni_t *ni, lnet_process_id_t id, __u32 ip)
+ksocknal_del_peer(lnet_ni_t *ni, lnet_process_id_t id, __u32 ip)
 {
-	LIST_HEAD     (zombies);
+	LIST_HEAD(zombies);
 	struct list_head	*ptmp;
 	struct list_head	*pnxt;
 	ksock_peer_t      *peer;
@@ -583,9 +583,9 @@ ksocknal_del_peer (lnet_ni_t *ni, lnet_process_id_t id, __u32 ip)
 	}
 
 	for (i = lo; i <= hi; i++) {
-		list_for_each_safe (ptmp, pnxt,
+		list_for_each_safe(ptmp, pnxt,
 					&ksocknal_data.ksnd_peers[i]) {
-			peer = list_entry (ptmp, ksock_peer_t, ksnp_list);
+			peer = list_entry(ptmp, ksock_peer_t, ksnp_list);
 
 			if (peer->ksnp_ni != ni)
 				continue;
@@ -596,12 +596,12 @@ ksocknal_del_peer (lnet_ni_t *ni, lnet_process_id_t id, __u32 ip)
 
 			ksocknal_peer_addref(peer);     /* a ref for me... */
 
-			ksocknal_del_peer_locked (peer, ip);
+			ksocknal_del_peer_locked(peer, ip);
 
 			if (peer->ksnp_closing &&
 			    !list_empty(&peer->ksnp_tx_queue)) {
-				LASSERT (list_empty(&peer->ksnp_conns));
-				LASSERT (list_empty(&peer->ksnp_routes));
+				LASSERT(list_empty(&peer->ksnp_conns));
+				LASSERT(list_empty(&peer->ksnp_routes));
 
 				list_splice_init(&peer->ksnp_tx_queue,
 						     &zombies);
@@ -621,7 +621,7 @@ ksocknal_del_peer (lnet_ni_t *ni, lnet_process_id_t id, __u32 ip)
 }
 
 static ksock_conn_t *
-ksocknal_get_conn_by_idx (lnet_ni_t *ni, int index)
+ksocknal_get_conn_by_idx(lnet_ni_t *ni, int index)
 {
 	ksock_peer_t      *peer;
 	struct list_head	*ptmp;
@@ -632,19 +632,19 @@ ksocknal_get_conn_by_idx (lnet_ni_t *ni, int index)
 	read_lock(&ksocknal_data.ksnd_global_lock);
 
 	for (i = 0; i < ksocknal_data.ksnd_peer_hash_size; i++) {
-		list_for_each (ptmp, &ksocknal_data.ksnd_peers[i]) {
-			peer = list_entry (ptmp, ksock_peer_t, ksnp_list);
+		list_for_each(ptmp, &ksocknal_data.ksnd_peers[i]) {
+			peer = list_entry(ptmp, ksock_peer_t, ksnp_list);
 
-			LASSERT (!peer->ksnp_closing);
+			LASSERT(!peer->ksnp_closing);
 
 			if (peer->ksnp_ni != ni)
 				continue;
 
-			list_for_each (ctmp, &peer->ksnp_conns) {
+			list_for_each(ctmp, &peer->ksnp_conns) {
 				if (index-- > 0)
 					continue;
 
-				conn = list_entry (ctmp, ksock_conn_t,
+				conn = list_entry(ctmp, ksock_conn_t,
 						       ksnc_list);
 				ksocknal_conn_addref(conn);
 				read_unlock(&ksocknal_data.ksnd_global_lock);
@@ -681,7 +681,7 @@ ksocknal_choose_scheduler_locked(unsigned int cpt)
 }
 
 static int
-ksocknal_local_ipvec (lnet_ni_t *ni, __u32 *ipaddrs)
+ksocknal_local_ipvec(lnet_ni_t *ni, __u32 *ipaddrs)
 {
 	ksock_net_t       *net = ni->ni_data;
 	int		i;
@@ -690,7 +690,7 @@ ksocknal_local_ipvec (lnet_ni_t *ni, __u32 *ipaddrs)
 	read_lock(&ksocknal_data.ksnd_global_lock);
 
 	nip = net->ksnn_ninterfaces;
-	LASSERT (nip <= LNET_MAX_INTERFACES);
+	LASSERT(nip <= LNET_MAX_INTERFACES);
 
 	/* Only offer interfaces for additional connections if I have
 	 * more than one. */
@@ -701,7 +701,7 @@ ksocknal_local_ipvec (lnet_ni_t *ni, __u32 *ipaddrs)
 
 	for (i = 0; i < nip; i++) {
 		ipaddrs[i] = net->ksnn_interfaces[i].ksni_ipaddr;
-		LASSERT (ipaddrs[i] != 0);
+		LASSERT(ipaddrs[i] != 0);
 	}
 
 	read_unlock(&ksocknal_data.ksnd_global_lock);
@@ -709,7 +709,7 @@ ksocknal_local_ipvec (lnet_ni_t *ni, __u32 *ipaddrs)
 }
 
 static int
-ksocknal_match_peerip (ksock_interface_t *iface, __u32 *ips, int nips)
+ksocknal_match_peerip(ksock_interface_t *iface, __u32 *ips, int nips)
 {
 	int   best_netmatch = 0;
 	int   best_xor      = 0;
@@ -722,7 +722,7 @@ ksocknal_match_peerip (ksock_interface_t *iface, __u32 *ips, int nips)
 		if (ips[i] == 0)
 			continue;
 
-		this_xor = (ips[i] ^ iface->ksni_ipaddr);
+		this_xor = ips[i] ^ iface->ksni_ipaddr;
 		this_netmatch = ((this_xor & iface->ksni_netmask) == 0) ? 1 : 0;
 
 		if (!(best < 0 ||
@@ -736,7 +736,7 @@ ksocknal_match_peerip (ksock_interface_t *iface, __u32 *ips, int nips)
 		best_xor = this_xor;
 	}
 
-	LASSERT (best >= 0);
+	LASSERT(best >= 0);
 	return best;
 }
 
@@ -767,8 +767,8 @@ ksocknal_select_ips(ksock_peer_t *peer, __u32 *peerips, int n_peerips)
 
 	write_lock_bh(global_lock);
 
-	LASSERT (n_peerips <= LNET_MAX_INTERFACES);
-	LASSERT (net->ksnn_ninterfaces <= LNET_MAX_INTERFACES);
+	LASSERT(n_peerips <= LNET_MAX_INTERFACES);
+	LASSERT(net->ksnn_ninterfaces <= LNET_MAX_INTERFACES);
 
 	/* Only match interfaces for additional connections
 	 * if I have > 1 interface */
@@ -791,7 +791,7 @@ ksocknal_select_ips(ksock_peer_t *peer, __u32 *peerips, int n_peerips)
 
 		} else {
 			/* choose a new interface */
-			LASSERT (i == peer->ksnp_n_passive_ips);
+			LASSERT(i == peer->ksnp_n_passive_ips);
 
 			best_iface = NULL;
 			best_netmatch = 0;
@@ -809,7 +809,7 @@ ksocknal_select_ips(ksock_peer_t *peer, __u32 *peerips, int n_peerips)
 					continue;
 
 				k = ksocknal_match_peerip(iface, peerips, n_peerips);
-				xor = (ip ^ peerips[k]);
+				xor = ip ^ peerips[k];
 				this_netmatch = ((xor & iface->ksni_netmask) == 0) ? 1 : 0;
 
 				if (!(best_iface == NULL ||
@@ -874,7 +874,7 @@ ksocknal_create_routes(ksock_peer_t *peer, int port,
 		return;
 	}
 
-	LASSERT (npeer_ipaddrs <= LNET_MAX_INTERFACES);
+	LASSERT(npeer_ipaddrs <= LNET_MAX_INTERFACES);
 
 	for (i = 0; i < npeer_ipaddrs; i++) {
 		if (newroute != NULL) {
@@ -911,7 +911,7 @@ ksocknal_create_routes(ksock_peer_t *peer, int port,
 		best_nroutes = 0;
 		best_netmatch = 0;
 
-		LASSERT (net->ksnn_ninterfaces <= LNET_MAX_INTERFACES);
+		LASSERT(net->ksnn_ninterfaces <= LNET_MAX_INTERFACES);
 
 		/* Select interface to connect from */
 		for (j = 0; j < net->ksnn_ninterfaces; j++) {
@@ -961,7 +961,7 @@ ksocknal_create_routes(ksock_peer_t *peer, int port,
 }
 
 int
-ksocknal_accept (lnet_ni_t *ni, struct socket *sock)
+ksocknal_accept(lnet_ni_t *ni, struct socket *sock)
 {
 	ksock_connreq_t    *cr;
 	int		 rc;
@@ -969,7 +969,7 @@ ksocknal_accept (lnet_ni_t *ni, struct socket *sock)
 	int		 peer_port;
 
 	rc = libcfs_sock_getaddr(sock, 1, &peer_ip, &peer_port);
-	LASSERT (rc == 0);		      /* we succeeded before */
+	LASSERT(rc == 0);		      /* we succeeded before */
 
 	LIBCFS_ALLOC(cr, sizeof(*cr));
 	if (cr == NULL) {
@@ -992,11 +992,11 @@ ksocknal_accept (lnet_ni_t *ni, struct socket *sock)
 }
 
 static int
-ksocknal_connecting (ksock_peer_t *peer, __u32 ipaddr)
+ksocknal_connecting(ksock_peer_t *peer, __u32 ipaddr)
 {
 	ksock_route_t   *route;
 
-	list_for_each_entry (route, &peer->ksnp_routes, ksnr_list) {
+	list_for_each_entry(route, &peer->ksnp_routes, ksnr_list) {
 
 		if (route->ksnr_ipaddr == ipaddr)
 			return route->ksnr_connecting;
@@ -1005,11 +1005,11 @@ ksocknal_connecting (ksock_peer_t *peer, __u32 ipaddr)
 }
 
 int
-ksocknal_create_conn (lnet_ni_t *ni, ksock_route_t *route,
+ksocknal_create_conn(lnet_ni_t *ni, ksock_route_t *route,
 		      struct socket *sock, int type)
 {
 	rwlock_t		*global_lock = &ksocknal_data.ksnd_global_lock;
-	LIST_HEAD     (zombies);
+	LIST_HEAD(zombies);
 	lnet_process_id_t  peerid;
 	struct list_head	*tmp;
 	__u64	      incarnation;
@@ -1028,7 +1028,7 @@ ksocknal_create_conn (lnet_ni_t *ni, ksock_route_t *route,
 
 	active = (route != NULL);
 
-	LASSERT (active == (type != SOCKLND_CONN_NONE));
+	LASSERT(active == (type != SOCKLND_CONN_NONE));
 
 	LIBCFS_ALLOC(conn, sizeof(*conn));
 	if (conn == NULL) {
@@ -1041,19 +1041,19 @@ ksocknal_create_conn (lnet_ni_t *ni, ksock_route_t *route,
 	conn->ksnc_sock = sock;
 	/* 2 ref, 1 for conn, another extra ref prevents socket
 	 * being closed before establishment of connection */
-	atomic_set (&conn->ksnc_sock_refcount, 2);
+	atomic_set(&conn->ksnc_sock_refcount, 2);
 	conn->ksnc_type = type;
 	ksocknal_lib_save_callback(sock, conn);
-	atomic_set (&conn->ksnc_conn_refcount, 1); /* 1 ref for me */
+	atomic_set(&conn->ksnc_conn_refcount, 1); /* 1 ref for me */
 
 	conn->ksnc_rx_ready = 0;
 	conn->ksnc_rx_scheduled = 0;
 
-	INIT_LIST_HEAD (&conn->ksnc_tx_queue);
+	INIT_LIST_HEAD(&conn->ksnc_tx_queue);
 	conn->ksnc_tx_ready = 0;
 	conn->ksnc_tx_scheduled = 0;
 	conn->ksnc_tx_carrier = NULL;
-	atomic_set (&conn->ksnc_tx_nob, 0);
+	atomic_set(&conn->ksnc_tx_nob, 0);
 
 	LIBCFS_ALLOC(hello, offsetof(ksock_hello_msg_t,
 				     kshm_ips[LNET_MAX_INTERFACES]));
@@ -1063,7 +1063,7 @@ ksocknal_create_conn (lnet_ni_t *ni, ksock_route_t *route,
 	}
 
 	/* stash conn's local and remote addrs */
-	rc = ksocknal_lib_get_conn_addrs (conn);
+	rc = ksocknal_lib_get_conn_addrs(conn);
 	if (rc != 0)
 		goto failed_1;
 
@@ -1094,7 +1094,7 @@ ksocknal_create_conn (lnet_ni_t *ni, ksock_route_t *route,
 #endif
 		}
 
-		rc = ksocknal_send_hello (ni, conn, peerid.nid, hello);
+		rc = ksocknal_send_hello(ni, conn, peerid.nid, hello);
 		if (rc != 0)
 			goto failed_1;
 	} else {
@@ -1105,13 +1105,13 @@ ksocknal_create_conn (lnet_ni_t *ni, ksock_route_t *route,
 		conn->ksnc_proto = NULL;
 	}
 
-	rc = ksocknal_recv_hello (ni, conn, hello, &peerid, &incarnation);
+	rc = ksocknal_recv_hello(ni, conn, hello, &peerid, &incarnation);
 	if (rc < 0)
 		goto failed_1;
 
-	LASSERT (rc == 0 || active);
-	LASSERT (conn->ksnc_proto != NULL);
-	LASSERT (peerid.nid != LNET_NID_ANY);
+	LASSERT(rc == 0 || active);
+	LASSERT(conn->ksnc_proto != NULL);
+	LASSERT(peerid.nid != LNET_NID_ANY);
 
 	cpt = lnet_cpt_of_nid(peerid.nid);
 
@@ -1126,7 +1126,7 @@ ksocknal_create_conn (lnet_ni_t *ni, ksock_route_t *route,
 		write_lock_bh(global_lock);
 
 		/* called with a ref on ni, so shutdown can't have started */
-		LASSERT (((ksock_net_t *) ni->ni_data)->ksnn_shutdown == 0);
+		LASSERT(((ksock_net_t *) ni->ni_data)->ksnn_shutdown == 0);
 
 		peer2 = ksocknal_find_peer_locked(ni, peerid);
 		if (peer2 == NULL) {
@@ -1166,7 +1166,7 @@ ksocknal_create_conn (lnet_ni_t *ni, ksock_route_t *route,
 		 * NB recv_hello may have returned EPROTO to signal my peer
 		 * wants a different protocol than the one I asked for.
 		 */
-		LASSERT (list_empty(&peer->ksnp_conns));
+		LASSERT(list_empty(&peer->ksnp_conns));
 
 		peer->ksnp_proto = conn->ksnc_proto;
 		peer->ksnp_incarnation = incarnation;
@@ -1211,7 +1211,7 @@ ksocknal_create_conn (lnet_ni_t *ni, ksock_route_t *route,
 
 			/* Reply on a passive connection attempt so the peer
 			 * realises we're connected. */
-			LASSERT (rc == 0);
+			LASSERT(rc == 0);
 			if (!active)
 				rc = EALREADY;
 
@@ -1235,7 +1235,7 @@ ksocknal_create_conn (lnet_ni_t *ni, ksock_route_t *route,
 	 * create an association.  This allows incoming connections created
 	 * by routes in my peer to match my own route entries so I don't
 	 * continually create duplicate routes. */
-	list_for_each (tmp, &peer->ksnp_routes) {
+	list_for_each(tmp, &peer->ksnp_routes) {
 		route = list_entry(tmp, ksock_route_t, ksnr_list);
 
 		if (route->ksnr_ipaddr != conn->ksnc_ipaddr)
@@ -1260,7 +1260,7 @@ ksocknal_create_conn (lnet_ni_t *ni, ksock_route_t *route,
 	conn->ksnc_tx_deadline = cfs_time_shift(*ksocknal_tunables.ksnd_timeout);
 	mb();   /* order with adding to peer's conn list */
 
-	list_add (&conn->ksnc_list, &peer->ksnp_conns);
+	list_add(&conn->ksnc_list, &peer->ksnp_conns);
 	ksocknal_conn_addref(conn);
 
 	ksocknal_new_packet(conn, 0);
@@ -1272,8 +1272,8 @@ ksocknal_create_conn (lnet_ni_t *ni, ksock_route_t *route,
 		if (conn->ksnc_proto->pro_match_tx(conn, tx, tx->tx_nonblk) == SOCKNAL_MATCH_NO)
 				continue;
 
-		list_del (&tx->tx_list);
-		ksocknal_queue_tx_locked (tx, conn);
+		list_del(&tx->tx_list);
+		ksocknal_queue_tx_locked(tx, conn);
 	}
 
 	write_unlock_bh(global_lock);
@@ -1343,8 +1343,8 @@ ksocknal_create_conn (lnet_ni_t *ni, ksock_route_t *route,
 
  failed_2:
 	if (!peer->ksnp_closing &&
-	    list_empty (&peer->ksnp_conns) &&
-	    list_empty (&peer->ksnp_routes)) {
+	    list_empty(&peer->ksnp_conns) &&
+	    list_empty(&peer->ksnp_routes)) {
 		list_add(&zombies, &peer->ksnp_tx_queue);
 		list_del_init(&peer->ksnp_tx_queue);
 		ksocknal_unlink_peer_locked(peer);
@@ -1383,7 +1383,7 @@ ksocknal_create_conn (lnet_ni_t *ni, ksock_route_t *route,
 		LIBCFS_FREE(hello, offsetof(ksock_hello_msg_t,
 					    kshm_ips[LNET_MAX_INTERFACES]));
 
-	LIBCFS_FREE (conn, sizeof(*conn));
+	LIBCFS_FREE(conn, sizeof(*conn));
 
  failed_0:
 	libcfs_sock_release(sock);
@@ -1391,7 +1391,7 @@ ksocknal_create_conn (lnet_ni_t *ni, ksock_route_t *route,
 }
 
 void
-ksocknal_close_conn_locked (ksock_conn_t *conn, int error)
+ksocknal_close_conn_locked(ksock_conn_t *conn, int error)
 {
 	/* This just does the immmediate housekeeping, and queues the
 	 * connection for the reaper to terminate.
@@ -1401,18 +1401,18 @@ ksocknal_close_conn_locked (ksock_conn_t *conn, int error)
 	ksock_conn_t      *conn2;
 	struct list_head	*tmp;
 
-	LASSERT (peer->ksnp_error == 0);
-	LASSERT (!conn->ksnc_closing);
+	LASSERT(peer->ksnp_error == 0);
+	LASSERT(!conn->ksnc_closing);
 	conn->ksnc_closing = 1;
 
 	/* ksnd_deathrow_conns takes over peer's ref */
-	list_del (&conn->ksnc_list);
+	list_del(&conn->ksnc_list);
 
 	route = conn->ksnc_route;
 	if (route != NULL) {
 		/* dissociate conn from route... */
-		LASSERT (!route->ksnr_deleted);
-		LASSERT ((route->ksnr_connected & (1 << conn->ksnc_type)) != 0);
+		LASSERT(!route->ksnr_deleted);
+		LASSERT((route->ksnr_connected & (1 << conn->ksnc_type)) != 0);
 
 		conn2 = NULL;
 		list_for_each(tmp, &peer->ksnp_conns) {
@@ -1431,19 +1431,19 @@ ksocknal_close_conn_locked (ksock_conn_t *conn, int error)
 
 #if 0	   /* irrelevant with only eager routes */
 		/* make route least favourite */
-		list_del (&route->ksnr_list);
-		list_add_tail (&route->ksnr_list, &peer->ksnp_routes);
+		list_del(&route->ksnr_list);
+		list_add_tail(&route->ksnr_list, &peer->ksnp_routes);
 #endif
 		ksocknal_route_decref(route);     /* drop conn's ref on route */
 	}
 
-	if (list_empty (&peer->ksnp_conns)) {
+	if (list_empty(&peer->ksnp_conns)) {
 		/* No more connections to this peer */
 
 		if (!list_empty(&peer->ksnp_tx_queue)) {
 			ksock_tx_t *tx;
 
-			LASSERT (conn->ksnc_proto == &ksocknal_protocol_v3x);
+			LASSERT(conn->ksnc_proto == &ksocknal_protocol_v3x);
 
 			/* throw them to the last connection...,
 			 * these TXs will be send to /dev/null by scheduler */
@@ -1460,10 +1460,10 @@ ksocknal_close_conn_locked (ksock_conn_t *conn, int error)
 		peer->ksnp_proto = NULL;	/* renegotiate protocol version */
 		peer->ksnp_error = error;       /* stash last conn close reason */
 
-		if (list_empty (&peer->ksnp_routes)) {
+		if (list_empty(&peer->ksnp_routes)) {
 			/* I've just closed last conn belonging to a
 			 * peer with no routes to it */
-			ksocknal_unlink_peer_locked (peer);
+			ksocknal_unlink_peer_locked(peer);
 		}
 	}
 
@@ -1477,7 +1477,7 @@ ksocknal_close_conn_locked (ksock_conn_t *conn, int error)
 }
 
 void
-ksocknal_peer_failed (ksock_peer_t *peer)
+ksocknal_peer_failed(ksock_peer_t *peer)
 {
 	int	notify = 0;
 	unsigned long last_alive = 0;
@@ -1499,7 +1499,7 @@ ksocknal_peer_failed (ksock_peer_t *peer)
 	read_unlock(&ksocknal_data.ksnd_global_lock);
 
 	if (notify)
-		lnet_notify (peer->ksnp_ni, peer->ksnp_id.nid, 0,
+		lnet_notify(peer->ksnp_ni, peer->ksnp_id.nid, 0,
 			     last_alive);
 }
 
@@ -1509,11 +1509,11 @@ ksocknal_finalize_zcreq(ksock_conn_t *conn)
 	ksock_peer_t     *peer = conn->ksnc_peer;
 	ksock_tx_t       *tx;
 	ksock_tx_t       *tmp;
-	LIST_HEAD    (zlist);
+	LIST_HEAD(zlist);
 
 	/* NB safe to finalize TXs because closing of socket will
 	 * abort all buffered data */
-	LASSERT (conn->ksnc_sock == NULL);
+	LASSERT(conn->ksnc_sock == NULL);
 
 	spin_lock(&peer->ksnp_lock);
 
@@ -1521,7 +1521,7 @@ ksocknal_finalize_zcreq(ksock_conn_t *conn)
 		if (tx->tx_conn != conn)
 			continue;
 
-		LASSERT (tx->tx_msg.ksm_zc_cookies[0] != 0);
+		LASSERT(tx->tx_msg.ksm_zc_cookies[0] != 0);
 
 		tx->tx_msg.ksm_zc_cookies[0] = 0;
 		tx->tx_zc_aborted = 1; /* mark it as not-acked */
@@ -1540,7 +1540,7 @@ ksocknal_finalize_zcreq(ksock_conn_t *conn)
 }
 
 void
-ksocknal_terminate_conn (ksock_conn_t *conn)
+ksocknal_terminate_conn(ksock_conn_t *conn)
 {
 	/* This gets called by the reaper (guaranteed thread context) to
 	 * disengage the socket from its callbacks and close it.
@@ -1560,13 +1560,13 @@ ksocknal_terminate_conn (ksock_conn_t *conn)
 
 	if (!conn->ksnc_tx_scheduled &&
 	    !list_empty(&conn->ksnc_tx_queue)) {
-		list_add_tail (&conn->ksnc_tx_list,
+		list_add_tail(&conn->ksnc_tx_list,
 			       &sched->kss_tx_conns);
 		conn->ksnc_tx_scheduled = 1;
 		/* extra ref for scheduler */
 		ksocknal_conn_addref(conn);
 
-		wake_up (&sched->kss_waitq);
+		wake_up(&sched->kss_waitq);
 	}
 
 	spin_unlock_bh(&sched->kss_lock);
@@ -1582,7 +1582,7 @@ ksocknal_terminate_conn (ksock_conn_t *conn)
 
 	if (peer->ksnp_error != 0) {
 		/* peer's last conn closed in error */
-		LASSERT (list_empty (&peer->ksnp_conns));
+		LASSERT(list_empty(&peer->ksnp_conns));
 		failed = 1;
 		peer->ksnp_error = 0;     /* avoid multiple notifications */
 	}
@@ -1601,7 +1601,7 @@ ksocknal_terminate_conn (ksock_conn_t *conn)
 }
 
 void
-ksocknal_queue_zombie_conn (ksock_conn_t *conn)
+ksocknal_queue_zombie_conn(ksock_conn_t *conn)
 {
 	/* Queue the conn for the reaper to destroy */
 
@@ -1615,20 +1615,20 @@ ksocknal_queue_zombie_conn (ksock_conn_t *conn)
 }
 
 void
-ksocknal_destroy_conn (ksock_conn_t *conn)
+ksocknal_destroy_conn(ksock_conn_t *conn)
 {
 	unsigned long      last_rcv;
 
 	/* Final coup-de-grace of the reaper */
-	CDEBUG (D_NET, "connection %p\n", conn);
+	CDEBUG(D_NET, "connection %p\n", conn);
 
-	LASSERT (atomic_read (&conn->ksnc_conn_refcount) == 0);
-	LASSERT (atomic_read (&conn->ksnc_sock_refcount) == 0);
-	LASSERT (conn->ksnc_sock == NULL);
-	LASSERT (conn->ksnc_route == NULL);
-	LASSERT (!conn->ksnc_tx_scheduled);
-	LASSERT (!conn->ksnc_rx_scheduled);
-	LASSERT (list_empty(&conn->ksnc_tx_queue));
+	LASSERT(atomic_read(&conn->ksnc_conn_refcount) == 0);
+	LASSERT(atomic_read(&conn->ksnc_sock_refcount) == 0);
+	LASSERT(conn->ksnc_sock == NULL);
+	LASSERT(conn->ksnc_route == NULL);
+	LASSERT(!conn->ksnc_tx_scheduled);
+	LASSERT(!conn->ksnc_rx_scheduled);
+	LASSERT(list_empty(&conn->ksnc_tx_queue));
 
 	/* complete current receive if any */
 	switch (conn->ksnc_rx_state) {
@@ -1641,7 +1641,7 @@ ksocknal_destroy_conn (ksock_conn_t *conn)
 		       conn->ksnc_rx_nob_wanted, conn->ksnc_rx_nob_left,
 		       cfs_duration_sec(cfs_time_sub(cfs_time_current(),
 						     last_rcv)));
-		lnet_finalize (conn->ksnc_peer->ksnp_ni,
+		lnet_finalize(conn->ksnc_peer->ksnp_ni,
 			       conn->ksnc_cookie, -EIO);
 		break;
 	case SOCKNAL_RX_LNET_HEADER:
@@ -1665,30 +1665,30 @@ ksocknal_destroy_conn (ksock_conn_t *conn)
 			       &conn->ksnc_ipaddr, conn->ksnc_port);
 	       break;
 	default:
-		LBUG ();
+		LBUG();
 		break;
 	}
 
 	ksocknal_peer_decref(conn->ksnc_peer);
 
-	LIBCFS_FREE (conn, sizeof (*conn));
+	LIBCFS_FREE(conn, sizeof(*conn));
 }
 
 int
-ksocknal_close_peer_conns_locked (ksock_peer_t *peer, __u32 ipaddr, int why)
+ksocknal_close_peer_conns_locked(ksock_peer_t *peer, __u32 ipaddr, int why)
 {
 	ksock_conn_t       *conn;
 	struct list_head	 *ctmp;
 	struct list_head	 *cnxt;
 	int		 count = 0;
 
-	list_for_each_safe (ctmp, cnxt, &peer->ksnp_conns) {
-		conn = list_entry (ctmp, ksock_conn_t, ksnc_list);
+	list_for_each_safe(ctmp, cnxt, &peer->ksnp_conns) {
+		conn = list_entry(ctmp, ksock_conn_t, ksnc_list);
 
 		if (ipaddr == 0 ||
 		    conn->ksnc_ipaddr == ipaddr) {
 			count++;
-			ksocknal_close_conn_locked (conn, why);
+			ksocknal_close_conn_locked(conn, why);
 		}
 	}
 
@@ -1696,7 +1696,7 @@ ksocknal_close_peer_conns_locked (ksock_peer_t *peer, __u32 ipaddr, int why)
 }
 
 int
-ksocknal_close_conn_and_siblings (ksock_conn_t *conn, int why)
+ksocknal_close_conn_and_siblings(ksock_conn_t *conn, int why)
 {
 	ksock_peer_t     *peer = conn->ksnc_peer;
 	__u32	     ipaddr = conn->ksnc_ipaddr;
@@ -1704,7 +1704,7 @@ ksocknal_close_conn_and_siblings (ksock_conn_t *conn, int why)
 
 	write_lock_bh(&ksocknal_data.ksnd_global_lock);
 
-	count = ksocknal_close_peer_conns_locked (peer, ipaddr, why);
+	count = ksocknal_close_peer_conns_locked(peer, ipaddr, why);
 
 	write_unlock_bh(&ksocknal_data.ksnd_global_lock);
 
@@ -1712,7 +1712,7 @@ ksocknal_close_conn_and_siblings (ksock_conn_t *conn, int why)
 }
 
 int
-ksocknal_close_matching_conns (lnet_process_id_t id, __u32 ipaddr)
+ksocknal_close_matching_conns(lnet_process_id_t id, __u32 ipaddr)
 {
 	ksock_peer_t       *peer;
 	struct list_head	 *ptmp;
@@ -1732,16 +1732,16 @@ ksocknal_close_matching_conns (lnet_process_id_t id, __u32 ipaddr)
 	}
 
 	for (i = lo; i <= hi; i++) {
-		list_for_each_safe (ptmp, pnxt,
+		list_for_each_safe(ptmp, pnxt,
 					&ksocknal_data.ksnd_peers[i]) {
 
-			peer = list_entry (ptmp, ksock_peer_t, ksnp_list);
+			peer = list_entry(ptmp, ksock_peer_t, ksnp_list);
 
 			if (!((id.nid == LNET_NID_ANY || id.nid == peer->ksnp_id.nid) &&
 			      (id.pid == LNET_PID_ANY || id.pid == peer->ksnp_id.pid)))
 				continue;
 
-			count += ksocknal_close_peer_conns_locked (peer, ipaddr, 0);
+			count += ksocknal_close_peer_conns_locked(peer, ipaddr, 0);
 		}
 	}
 
@@ -1758,7 +1758,7 @@ ksocknal_close_matching_conns (lnet_process_id_t id, __u32 ipaddr)
 }
 
 void
-ksocknal_notify (lnet_ni_t *ni, lnet_nid_t gw_nid, int alive)
+ksocknal_notify(lnet_ni_t *ni, lnet_nid_t gw_nid, int alive)
 {
 	/* The router is telling me she's been notified of a change in
 	 * gateway state.... */
@@ -1767,12 +1767,12 @@ ksocknal_notify (lnet_ni_t *ni, lnet_nid_t gw_nid, int alive)
 	id.nid = gw_nid;
 	id.pid = LNET_PID_ANY;
 
-	CDEBUG (D_NET, "gw %s %s\n", libcfs_nid2str(gw_nid),
+	CDEBUG(D_NET, "gw %s %s\n", libcfs_nid2str(gw_nid),
 		alive ? "up" : "down");
 
 	if (!alive) {
 		/* If the gateway crashed, close all open connections... */
-		ksocknal_close_matching_conns (id, 0);
+		ksocknal_close_matching_conns(id, 0);
 		return;
 	}
 
@@ -1781,7 +1781,7 @@ ksocknal_notify (lnet_ni_t *ni, lnet_nid_t gw_nid, int alive)
 }
 
 void
-ksocknal_query (lnet_ni_t *ni, lnet_nid_t nid, unsigned long *when)
+ksocknal_query(lnet_ni_t *ni, lnet_nid_t nid, unsigned long *when)
 {
 	int		connect = 1;
 	unsigned long	 last_alive = 0;
@@ -1798,7 +1798,7 @@ ksocknal_query (lnet_ni_t *ni, lnet_nid_t nid, unsigned long *when)
 		ksock_conn_t     *conn;
 		int	       bufnob;
 
-		list_for_each (tmp, &peer->ksnp_conns) {
+		list_for_each(tmp, &peer->ksnp_conns) {
 			conn = list_entry(tmp, ksock_conn_t, ksnc_list);
 			bufnob = conn->ksnc_sock->sk->sk_wmem_queued;
 
@@ -1842,7 +1842,7 @@ ksocknal_query (lnet_ni_t *ni, lnet_nid_t nid, unsigned long *when)
 }
 
 static void
-ksocknal_push_peer (ksock_peer_t *peer)
+ksocknal_push_peer(ksock_peer_t *peer)
 {
 	int	       index;
 	int	       i;
@@ -1855,9 +1855,9 @@ ksocknal_push_peer (ksock_peer_t *peer)
 		i = 0;
 		conn = NULL;
 
-		list_for_each (tmp, &peer->ksnp_conns) {
+		list_for_each(tmp, &peer->ksnp_conns) {
 			if (i++ == index) {
-				conn = list_entry (tmp, ksock_conn_t,
+				conn = list_entry(tmp, ksock_conn_t,
 						       ksnc_list);
 				ksocknal_conn_addref(conn);
 				break;
@@ -1869,13 +1869,13 @@ ksocknal_push_peer (ksock_peer_t *peer)
 		if (conn == NULL)
 			break;
 
-		ksocknal_lib_push_conn (conn);
+		ksocknal_lib_push_conn(conn);
 		ksocknal_conn_decref(conn);
 	}
 }
 
 static int
-ksocknal_push (lnet_ni_t *ni, lnet_process_id_t id)
+ksocknal_push(lnet_ni_t *ni, lnet_process_id_t id)
 {
 	ksock_peer_t      *peer;
 	struct list_head	*tmp;
@@ -1891,7 +1891,7 @@ ksocknal_push (lnet_ni_t *ni, lnet_process_id_t id)
 			index = 0;
 			peer = NULL;
 
-			list_for_each (tmp, &ksocknal_data.ksnd_peers[i]) {
+			list_for_each(tmp, &ksocknal_data.ksnd_peers[i]) {
 				peer = list_entry(tmp, ksock_peer_t,
 						      ksnp_list);
 
@@ -1913,7 +1913,7 @@ ksocknal_push (lnet_ni_t *ni, lnet_process_id_t id)
 
 			if (peer != NULL) {
 				rc = 0;
-				ksocknal_push_peer (peer);
+				ksocknal_push_peer(peer);
 				ksocknal_peer_decref(peer);
 			}
 		}
@@ -2005,7 +2005,7 @@ ksocknal_peer_del_interface_locked(ksock_peer_t *peer, __u32 ipaddr)
 		}
 
 	list_for_each_safe(tmp, nxt, &peer->ksnp_routes) {
-		route = list_entry (tmp, ksock_route_t, ksnr_list);
+		route = list_entry(tmp, ksock_route_t, ksnr_list);
 
 		if (route->ksnr_myipaddr != ipaddr)
 			continue;
@@ -2022,7 +2022,7 @@ ksocknal_peer_del_interface_locked(ksock_peer_t *peer, __u32 ipaddr)
 		conn = list_entry(tmp, ksock_conn_t, ksnc_list);
 
 		if (conn->ksnc_myipaddr == ipaddr)
-			ksocknal_close_conn_locked (conn, 0);
+			ksocknal_close_conn_locked(conn, 0);
 	}
 }
 
@@ -2139,21 +2139,21 @@ ksocknal_ctl(lnet_ni_t *ni, unsigned int cmd, void *arg)
 	case IOC_LIBCFS_ADD_PEER:
 		id.nid = data->ioc_nid;
 		id.pid = LUSTRE_SRV_LNET_PID;
-		return ksocknal_add_peer (ni, id,
+		return ksocknal_add_peer(ni, id,
 					  data->ioc_u32[0], /* IP */
 					  data->ioc_u32[1]); /* port */
 
 	case IOC_LIBCFS_DEL_PEER:
 		id.nid = data->ioc_nid;
 		id.pid = LNET_PID_ANY;
-		return ksocknal_del_peer (ni, id,
+		return ksocknal_del_peer(ni, id,
 					  data->ioc_u32[0]); /* IP */
 
 	case IOC_LIBCFS_GET_CONN: {
 		int	   txmem;
 		int	   rxmem;
 		int	   nagle;
-		ksock_conn_t *conn = ksocknal_get_conn_by_idx (ni, data->ioc_count);
+		ksock_conn_t *conn = ksocknal_get_conn_by_idx(ni, data->ioc_count);
 
 		if (conn == NULL)
 			return -ENOENT;
@@ -2177,7 +2177,7 @@ ksocknal_ctl(lnet_ni_t *ni, unsigned int cmd, void *arg)
 	case IOC_LIBCFS_CLOSE_CONNECTION:
 		id.nid = data->ioc_nid;
 		id.pid = LNET_PID_ANY;
-		return ksocknal_close_matching_conns (id,
+		return ksocknal_close_matching_conns(id,
 						      data->ioc_u32[0]);
 
 	case IOC_LIBCFS_REGISTER_MYNID:
@@ -2202,9 +2202,9 @@ ksocknal_ctl(lnet_ni_t *ni, unsigned int cmd, void *arg)
 }
 
 static void
-ksocknal_free_buffers (void)
+ksocknal_free_buffers(void)
 {
-	LASSERT (atomic_read(&ksocknal_data.ksnd_nactive_txs) == 0);
+	LASSERT(atomic_read(&ksocknal_data.ksnd_nactive_txs) == 0);
 
 	if (ksocknal_data.ksnd_sched_info != NULL) {
 		struct ksock_sched_info	*info;
@@ -2220,8 +2220,8 @@ ksocknal_free_buffers (void)
 		cfs_percpt_free(ksocknal_data.ksnd_sched_info);
 	}
 
-	LIBCFS_FREE (ksocknal_data.ksnd_peers,
-		     sizeof (struct list_head) *
+	LIBCFS_FREE(ksocknal_data.ksnd_peers,
+		     sizeof(struct list_head) *
 		     ksocknal_data.ksnd_peer_hash_size);
 
 	spin_lock(&ksocknal_data.ksnd_tx_lock);
@@ -2253,25 +2253,25 @@ ksocknal_base_shutdown(void)
 	int			j;
 
 	CDEBUG(D_MALLOC, "before NAL cleanup: kmem %d\n",
-	       atomic_read (&libcfs_kmemory));
-	LASSERT (ksocknal_data.ksnd_nnets == 0);
+	       atomic_read(&libcfs_kmemory));
+	LASSERT(ksocknal_data.ksnd_nnets == 0);
 
 	switch (ksocknal_data.ksnd_init) {
 	default:
-		LASSERT (0);
+		LASSERT(0);
 
 	case SOCKNAL_INIT_ALL:
 	case SOCKNAL_INIT_DATA:
-		LASSERT (ksocknal_data.ksnd_peers != NULL);
+		LASSERT(ksocknal_data.ksnd_peers != NULL);
 		for (i = 0; i < ksocknal_data.ksnd_peer_hash_size; i++) {
-			LASSERT (list_empty (&ksocknal_data.ksnd_peers[i]));
+			LASSERT(list_empty(&ksocknal_data.ksnd_peers[i]));
 		}
 
 		LASSERT(list_empty(&ksocknal_data.ksnd_nets));
-		LASSERT (list_empty (&ksocknal_data.ksnd_enomem_conns));
-		LASSERT (list_empty (&ksocknal_data.ksnd_zombie_conns));
-		LASSERT (list_empty (&ksocknal_data.ksnd_connd_connreqs));
-		LASSERT (list_empty (&ksocknal_data.ksnd_connd_routes));
+		LASSERT(list_empty(&ksocknal_data.ksnd_enomem_conns));
+		LASSERT(list_empty(&ksocknal_data.ksnd_zombie_conns));
+		LASSERT(list_empty(&ksocknal_data.ksnd_connd_connreqs));
+		LASSERT(list_empty(&ksocknal_data.ksnd_connd_routes));
 
 		if (ksocknal_data.ksnd_sched_info != NULL) {
 			cfs_percpt_for_each(info, i,
@@ -2332,13 +2332,13 @@ ksocknal_base_shutdown(void)
 	}
 
 	CDEBUG(D_MALLOC, "after NAL cleanup: kmem %d\n",
-	       atomic_read (&libcfs_kmemory));
+	       atomic_read(&libcfs_kmemory));
 
 	module_put(THIS_MODULE);
 }
 
 static __u64
-ksocknal_new_incarnation (void)
+ksocknal_new_incarnation(void)
 {
 
 	/* The incarnation number is the time this module loaded and it
@@ -2354,14 +2354,14 @@ ksocknal_base_startup(void)
 	int			rc;
 	int			i;
 
-	LASSERT (ksocknal_data.ksnd_init == SOCKNAL_INIT_NOTHING);
-	LASSERT (ksocknal_data.ksnd_nnets == 0);
+	LASSERT(ksocknal_data.ksnd_init == SOCKNAL_INIT_NOTHING);
+	LASSERT(ksocknal_data.ksnd_nnets == 0);
 
-	memset (&ksocknal_data, 0, sizeof (ksocknal_data)); /* zero pointers */
+	memset(&ksocknal_data, 0, sizeof(ksocknal_data)); /* zero pointers */
 
 	ksocknal_data.ksnd_peer_hash_size = SOCKNAL_PEER_HASH_SIZE;
-	LIBCFS_ALLOC (ksocknal_data.ksnd_peers,
-		      sizeof (struct list_head) *
+	LIBCFS_ALLOC(ksocknal_data.ksnd_peers,
+		      sizeof(struct list_head) *
 		      ksocknal_data.ksnd_peer_hash_size);
 	if (ksocknal_data.ksnd_peers == NULL)
 		return -ENOMEM;
@@ -2373,18 +2373,18 @@ ksocknal_base_startup(void)
 	INIT_LIST_HEAD(&ksocknal_data.ksnd_nets);
 
 	spin_lock_init(&ksocknal_data.ksnd_reaper_lock);
-	INIT_LIST_HEAD (&ksocknal_data.ksnd_enomem_conns);
-	INIT_LIST_HEAD (&ksocknal_data.ksnd_zombie_conns);
-	INIT_LIST_HEAD (&ksocknal_data.ksnd_deathrow_conns);
+	INIT_LIST_HEAD(&ksocknal_data.ksnd_enomem_conns);
+	INIT_LIST_HEAD(&ksocknal_data.ksnd_zombie_conns);
+	INIT_LIST_HEAD(&ksocknal_data.ksnd_deathrow_conns);
 	init_waitqueue_head(&ksocknal_data.ksnd_reaper_waitq);
 
 	spin_lock_init(&ksocknal_data.ksnd_connd_lock);
-	INIT_LIST_HEAD (&ksocknal_data.ksnd_connd_connreqs);
-	INIT_LIST_HEAD (&ksocknal_data.ksnd_connd_routes);
+	INIT_LIST_HEAD(&ksocknal_data.ksnd_connd_connreqs);
+	INIT_LIST_HEAD(&ksocknal_data.ksnd_connd_routes);
 	init_waitqueue_head(&ksocknal_data.ksnd_connd_waitq);
 
 	spin_lock_init(&ksocknal_data.ksnd_tx_lock);
-	INIT_LIST_HEAD (&ksocknal_data.ksnd_idle_noop_txs);
+	INIT_LIST_HEAD(&ksocknal_data.ksnd_idle_noop_txs);
 
 	/* NB memset above zeros whole of ksocknal_data */
 
@@ -2465,7 +2465,7 @@ ksocknal_base_startup(void)
 
 	rc = ksocknal_thread_start(ksocknal_reaper, NULL, "socknal_reaper");
 	if (rc != 0) {
-		CERROR ("Can't spawn socknal reaper: %d\n", rc);
+		CERROR("Can't spawn socknal reaper: %d\n", rc);
 		goto failed;
 	}
 
@@ -2480,7 +2480,7 @@ ksocknal_base_startup(void)
 }
 
 static void
-ksocknal_debug_peerhash (lnet_ni_t *ni)
+ksocknal_debug_peerhash(lnet_ni_t *ni)
 {
 	ksock_peer_t	*peer = NULL;
 	struct list_head	*tmp;
@@ -2489,8 +2489,8 @@ ksocknal_debug_peerhash (lnet_ni_t *ni)
 	read_lock(&ksocknal_data.ksnd_global_lock);
 
 	for (i = 0; i < ksocknal_data.ksnd_peer_hash_size; i++) {
-		list_for_each (tmp, &ksocknal_data.ksnd_peers[i]) {
-			peer = list_entry (tmp, ksock_peer_t, ksnp_list);
+		list_for_each(tmp, &ksocknal_data.ksnd_peers[i]) {
+			peer = list_entry(tmp, ksock_peer_t, ksnp_list);
 
 			if (peer->ksnp_ni == ni)
 				break;
@@ -2512,7 +2512,7 @@ ksocknal_debug_peerhash (lnet_ni_t *ni)
 		      !list_empty(&peer->ksnp_tx_queue),
 		      !list_empty(&peer->ksnp_zc_req_list));
 
-		list_for_each (tmp, &peer->ksnp_routes) {
+		list_for_each(tmp, &peer->ksnp_routes) {
 			route = list_entry(tmp, ksock_route_t, ksnr_list);
 			CWARN("Route: ref %d, schd %d, conn %d, cnted %d, del %d\n",
 			      atomic_read(&route->ksnr_refcount),
@@ -2520,9 +2520,9 @@ ksocknal_debug_peerhash (lnet_ni_t *ni)
 			      route->ksnr_connected, route->ksnr_deleted);
 		}
 
-		list_for_each (tmp, &peer->ksnp_conns) {
+		list_for_each(tmp, &peer->ksnp_conns) {
 			conn = list_entry(tmp, ksock_conn_t, ksnc_list);
-			CWARN ("Conn: ref %d, sref %d, t %d, c %d\n",
+			CWARN("Conn: ref %d, sref %d, t %d, c %d\n",
 			       atomic_read(&conn->ksnc_conn_refcount),
 			       atomic_read(&conn->ksnc_sock_refcount),
 			       conn->ksnc_type, conn->ksnc_closing);
@@ -2534,7 +2534,7 @@ ksocknal_debug_peerhash (lnet_ni_t *ni)
 }
 
 void
-ksocknal_shutdown (lnet_ni_t *ni)
+ksocknal_shutdown(lnet_ni_t *ni)
 {
 	ksock_net_t      *net = ni->ni_data;
 	int	       i;
@@ -2573,8 +2573,8 @@ ksocknal_shutdown (lnet_ni_t *ni)
 	spin_unlock_bh(&net->ksnn_lock);
 
 	for (i = 0; i < net->ksnn_ninterfaces; i++) {
-		LASSERT (net->ksnn_interfaces[i].ksni_npeers == 0);
-		LASSERT (net->ksnn_interfaces[i].ksni_nroutes == 0);
+		LASSERT(net->ksnn_interfaces[i].ksni_npeers == 0);
+		LASSERT(net->ksnn_interfaces[i].ksni_nroutes == 0);
 	}
 
 	list_del(&net->ksnn_list);
@@ -2757,13 +2757,13 @@ ksocknal_net_start_threads(ksock_net_t *net, __u32 *cpts, int ncpts)
 }
 
 int
-ksocknal_startup (lnet_ni_t *ni)
+ksocknal_startup(lnet_ni_t *ni)
 {
 	ksock_net_t  *net;
 	int	   rc;
 	int	   i;
 
-	LASSERT (ni->ni_lnd == &the_ksocklnd);
+	LASSERT(ni->ni_lnd == &the_ksocklnd);
 
 	if (ksocknal_data.ksnd_init == SOCKNAL_INIT_NOTHING) {
 		rc = ksocknal_base_startup();
@@ -2843,19 +2843,19 @@ ksocknal_startup (lnet_ni_t *ni)
 
 
 static void __exit
-ksocknal_module_fini (void)
+ksocknal_module_fini(void)
 {
 	lnet_unregister_lnd(&the_ksocklnd);
 }
 
 static int __init
-ksocknal_module_init (void)
+ksocknal_module_init(void)
 {
 	int    rc;
 
 	/* check ksnr_connected/connecting field large enough */
-	CLASSERT (SOCKLND_CONN_NTYPES <= 4);
-	CLASSERT (SOCKLND_CONN_ACK == SOCKLND_CONN_BULK_IN);
+	CLASSERT(SOCKLND_CONN_NTYPES <= 4);
+	CLASSERT(SOCKLND_CONN_ACK == SOCKLND_CONN_BULK_IN);
 
 	/* initialize the_ksocklnd */
 	the_ksocklnd.lnd_type     = SOCKLND;
diff --git a/drivers/staging/lustre/lnet/klnds/socklnd/socklnd.h b/drivers/staging/lustre/lnet/klnds/socklnd/socklnd.h
index 03488d289c74..c54c9955164e 100644
--- a/drivers/staging/lustre/lnet/klnds/socklnd/socklnd.h
+++ b/drivers/staging/lustre/lnet/klnds/socklnd/socklnd.h
@@ -406,7 +406,7 @@ ksocknal_route_mask(void)
 }
 
 static inline struct list_head *
-ksocknal_nid2peerlist (lnet_nid_t nid)
+ksocknal_nid2peerlist(lnet_nid_t nid)
 {
 	unsigned int hash = ((unsigned int)nid) % ksocknal_data.ksnd_peer_hash_size;
 
@@ -414,25 +414,25 @@ ksocknal_nid2peerlist (lnet_nid_t nid)
 }
 
 static inline void
-ksocknal_conn_addref (ksock_conn_t *conn)
+ksocknal_conn_addref(ksock_conn_t *conn)
 {
-	LASSERT (atomic_read(&conn->ksnc_conn_refcount) > 0);
+	LASSERT(atomic_read(&conn->ksnc_conn_refcount) > 0);
 	atomic_inc(&conn->ksnc_conn_refcount);
 }
 
-extern void ksocknal_queue_zombie_conn (ksock_conn_t *conn);
+extern void ksocknal_queue_zombie_conn(ksock_conn_t *conn);
 extern void ksocknal_finalize_zcreq(ksock_conn_t *conn);
 
 static inline void
-ksocknal_conn_decref (ksock_conn_t *conn)
+ksocknal_conn_decref(ksock_conn_t *conn)
 {
-	LASSERT (atomic_read(&conn->ksnc_conn_refcount) > 0);
+	LASSERT(atomic_read(&conn->ksnc_conn_refcount) > 0);
 	if (atomic_dec_and_test(&conn->ksnc_conn_refcount))
 		ksocknal_queue_zombie_conn(conn);
 }
 
 static inline int
-ksocknal_connsock_addref (ksock_conn_t *conn)
+ksocknal_connsock_addref(ksock_conn_t *conn)
 {
 	int   rc = -ESHUTDOWN;
 
@@ -448,11 +448,11 @@ ksocknal_connsock_addref (ksock_conn_t *conn)
 }
 
 static inline void
-ksocknal_connsock_decref (ksock_conn_t *conn)
+ksocknal_connsock_decref(ksock_conn_t *conn)
 {
-	LASSERT (atomic_read(&conn->ksnc_sock_refcount) > 0);
+	LASSERT(atomic_read(&conn->ksnc_sock_refcount) > 0);
 	if (atomic_dec_and_test(&conn->ksnc_sock_refcount)) {
-		LASSERT (conn->ksnc_closing);
+		LASSERT(conn->ksnc_closing);
 		libcfs_sock_release(conn->ksnc_sock);
 		conn->ksnc_sock = NULL;
 		ksocknal_finalize_zcreq(conn);
@@ -460,61 +460,61 @@ ksocknal_connsock_decref (ksock_conn_t *conn)
 }
 
 static inline void
-ksocknal_tx_addref (ksock_tx_t *tx)
+ksocknal_tx_addref(ksock_tx_t *tx)
 {
-	LASSERT (atomic_read(&tx->tx_refcount) > 0);
+	LASSERT(atomic_read(&tx->tx_refcount) > 0);
 	atomic_inc(&tx->tx_refcount);
 }
 
-extern void ksocknal_tx_prep (ksock_conn_t *, ksock_tx_t *tx);
-extern void ksocknal_tx_done (lnet_ni_t *ni, ksock_tx_t *tx);
+extern void ksocknal_tx_prep(ksock_conn_t *, ksock_tx_t *tx);
+extern void ksocknal_tx_done(lnet_ni_t *ni, ksock_tx_t *tx);
 
 static inline void
-ksocknal_tx_decref (ksock_tx_t *tx)
+ksocknal_tx_decref(ksock_tx_t *tx)
 {
-	LASSERT (atomic_read(&tx->tx_refcount) > 0);
+	LASSERT(atomic_read(&tx->tx_refcount) > 0);
 	if (atomic_dec_and_test(&tx->tx_refcount))
 		ksocknal_tx_done(NULL, tx);
 }
 
 static inline void
-ksocknal_route_addref (ksock_route_t *route)
+ksocknal_route_addref(ksock_route_t *route)
 {
-	LASSERT (atomic_read(&route->ksnr_refcount) > 0);
+	LASSERT(atomic_read(&route->ksnr_refcount) > 0);
 	atomic_inc(&route->ksnr_refcount);
 }
 
-extern void ksocknal_destroy_route (ksock_route_t *route);
+extern void ksocknal_destroy_route(ksock_route_t *route);
 
 static inline void
-ksocknal_route_decref (ksock_route_t *route)
+ksocknal_route_decref(ksock_route_t *route)
 {
-	LASSERT (atomic_read (&route->ksnr_refcount) > 0);
+	LASSERT(atomic_read(&route->ksnr_refcount) > 0);
 	if (atomic_dec_and_test(&route->ksnr_refcount))
-		ksocknal_destroy_route (route);
+		ksocknal_destroy_route(route);
 }
 
 static inline void
-ksocknal_peer_addref (ksock_peer_t *peer)
+ksocknal_peer_addref(ksock_peer_t *peer)
 {
-	LASSERT (atomic_read (&peer->ksnp_refcount) > 0);
+	LASSERT(atomic_read(&peer->ksnp_refcount) > 0);
 	atomic_inc(&peer->ksnp_refcount);
 }
 
-extern void ksocknal_destroy_peer (ksock_peer_t *peer);
+extern void ksocknal_destroy_peer(ksock_peer_t *peer);
 
 static inline void
-ksocknal_peer_decref (ksock_peer_t *peer)
+ksocknal_peer_decref(ksock_peer_t *peer)
 {
-	LASSERT (atomic_read (&peer->ksnp_refcount) > 0);
+	LASSERT(atomic_read(&peer->ksnp_refcount) > 0);
 	if (atomic_dec_and_test(&peer->ksnp_refcount))
-		ksocknal_destroy_peer (peer);
+		ksocknal_destroy_peer(peer);
 }
 
-int ksocknal_startup (lnet_ni_t *ni);
-void ksocknal_shutdown (lnet_ni_t *ni);
+int ksocknal_startup(lnet_ni_t *ni);
+void ksocknal_shutdown(lnet_ni_t *ni);
 int ksocknal_ctl(lnet_ni_t *ni, unsigned int cmd, void *arg);
-int ksocknal_send (lnet_ni_t *ni, void *private, lnet_msg_t *lntmsg);
+int ksocknal_send(lnet_ni_t *ni, void *private, lnet_msg_t *lntmsg);
 int ksocknal_recv(lnet_ni_t *ni, void *private, lnet_msg_t *lntmsg,
 		  int delayed, unsigned int niov,
 		  struct kvec *iov, lnet_kiov_t *kiov,
@@ -522,44 +522,44 @@ int ksocknal_recv(lnet_ni_t *ni, void *private, lnet_msg_t *lntmsg,
 int ksocknal_accept(lnet_ni_t *ni, struct socket *sock);
 
 extern int ksocknal_add_peer(lnet_ni_t *ni, lnet_process_id_t id, __u32 ip, int port);
-extern ksock_peer_t *ksocknal_find_peer_locked (lnet_ni_t *ni, lnet_process_id_t id);
-extern ksock_peer_t *ksocknal_find_peer (lnet_ni_t *ni, lnet_process_id_t id);
-extern void ksocknal_peer_failed (ksock_peer_t *peer);
-extern int ksocknal_create_conn (lnet_ni_t *ni, ksock_route_t *route,
+extern ksock_peer_t *ksocknal_find_peer_locked(lnet_ni_t *ni, lnet_process_id_t id);
+extern ksock_peer_t *ksocknal_find_peer(lnet_ni_t *ni, lnet_process_id_t id);
+extern void ksocknal_peer_failed(ksock_peer_t *peer);
+extern int ksocknal_create_conn(lnet_ni_t *ni, ksock_route_t *route,
 				 struct socket *sock, int type);
-extern void ksocknal_close_conn_locked (ksock_conn_t *conn, int why);
-extern void ksocknal_terminate_conn (ksock_conn_t *conn);
-extern void ksocknal_destroy_conn (ksock_conn_t *conn);
-extern int  ksocknal_close_peer_conns_locked (ksock_peer_t *peer,
+extern void ksocknal_close_conn_locked(ksock_conn_t *conn, int why);
+extern void ksocknal_terminate_conn(ksock_conn_t *conn);
+extern void ksocknal_destroy_conn(ksock_conn_t *conn);
+extern int  ksocknal_close_peer_conns_locked(ksock_peer_t *peer,
 					      __u32 ipaddr, int why);
-extern int ksocknal_close_conn_and_siblings (ksock_conn_t *conn, int why);
-extern int ksocknal_close_matching_conns (lnet_process_id_t id, __u32 ipaddr);
+extern int ksocknal_close_conn_and_siblings(ksock_conn_t *conn, int why);
+extern int ksocknal_close_matching_conns(lnet_process_id_t id, __u32 ipaddr);
 extern ksock_conn_t *ksocknal_find_conn_locked(ksock_peer_t *peer,
 					       ksock_tx_t *tx, int nonblk);
 
 extern int  ksocknal_launch_packet(lnet_ni_t *ni, ksock_tx_t *tx,
 				   lnet_process_id_t id);
 extern ksock_tx_t *ksocknal_alloc_tx(int type, int size);
-extern void ksocknal_free_tx (ksock_tx_t *tx);
+extern void ksocknal_free_tx(ksock_tx_t *tx);
 extern ksock_tx_t *ksocknal_alloc_tx_noop(__u64 cookie, int nonblk);
 extern void ksocknal_next_tx_carrier(ksock_conn_t *conn);
-extern void ksocknal_queue_tx_locked (ksock_tx_t *tx, ksock_conn_t *conn);
-extern void ksocknal_txlist_done (lnet_ni_t *ni, struct list_head *txlist,
+extern void ksocknal_queue_tx_locked(ksock_tx_t *tx, ksock_conn_t *conn);
+extern void ksocknal_txlist_done(lnet_ni_t *ni, struct list_head *txlist,
 				  int error);
-extern void ksocknal_notify (lnet_ni_t *ni, lnet_nid_t gw_nid, int alive);
-extern void ksocknal_query (struct lnet_ni *ni, lnet_nid_t nid, unsigned long *when);
+extern void ksocknal_notify(lnet_ni_t *ni, lnet_nid_t gw_nid, int alive);
+extern void ksocknal_query(struct lnet_ni *ni, lnet_nid_t nid, unsigned long *when);
 extern int ksocknal_thread_start(int (*fn)(void *arg), void *arg, char *name);
-extern void ksocknal_thread_fini (void);
-extern void ksocknal_launch_all_connections_locked (ksock_peer_t *peer);
-extern ksock_route_t *ksocknal_find_connectable_route_locked (ksock_peer_t *peer);
-extern ksock_route_t *ksocknal_find_connecting_route_locked (ksock_peer_t *peer);
-extern int ksocknal_new_packet (ksock_conn_t *conn, int skip);
-extern int ksocknal_scheduler (void *arg);
-extern int ksocknal_connd (void *arg);
-extern int ksocknal_reaper (void *arg);
-extern int ksocknal_send_hello (lnet_ni_t *ni, ksock_conn_t *conn,
+extern void ksocknal_thread_fini(void);
+extern void ksocknal_launch_all_connections_locked(ksock_peer_t *peer);
+extern ksock_route_t *ksocknal_find_connectable_route_locked(ksock_peer_t *peer);
+extern ksock_route_t *ksocknal_find_connecting_route_locked(ksock_peer_t *peer);
+extern int ksocknal_new_packet(ksock_conn_t *conn, int skip);
+extern int ksocknal_scheduler(void *arg);
+extern int ksocknal_connd(void *arg);
+extern int ksocknal_reaper(void *arg);
+extern int ksocknal_send_hello(lnet_ni_t *ni, ksock_conn_t *conn,
 				lnet_nid_t peer_nid, ksock_hello_msg_t *hello);
-extern int ksocknal_recv_hello (lnet_ni_t *ni, ksock_conn_t *conn,
+extern int ksocknal_recv_hello(lnet_ni_t *ni, ksock_conn_t *conn,
 				ksock_hello_msg_t *hello, lnet_process_id_t *id,
 				__u64 *incarnation);
 extern void ksocknal_read_callback(ksock_conn_t *conn);
@@ -569,15 +569,15 @@ extern int ksocknal_lib_zc_capable(ksock_conn_t *conn);
 extern void ksocknal_lib_save_callback(struct socket *sock, ksock_conn_t *conn);
 extern void ksocknal_lib_set_callback(struct socket *sock,  ksock_conn_t *conn);
 extern void ksocknal_lib_reset_callback(struct socket *sock, ksock_conn_t *conn);
-extern void ksocknal_lib_push_conn (ksock_conn_t *conn);
-extern int ksocknal_lib_get_conn_addrs (ksock_conn_t *conn);
-extern int ksocknal_lib_setup_sock (struct socket *so);
-extern int ksocknal_lib_send_iov (ksock_conn_t *conn, ksock_tx_t *tx);
-extern int ksocknal_lib_send_kiov (ksock_conn_t *conn, ksock_tx_t *tx);
-extern void ksocknal_lib_eager_ack (ksock_conn_t *conn);
-extern int ksocknal_lib_recv_iov (ksock_conn_t *conn);
-extern int ksocknal_lib_recv_kiov (ksock_conn_t *conn);
-extern int ksocknal_lib_get_conn_tunables (ksock_conn_t *conn, int *txmem,
+extern void ksocknal_lib_push_conn(ksock_conn_t *conn);
+extern int ksocknal_lib_get_conn_addrs(ksock_conn_t *conn);
+extern int ksocknal_lib_setup_sock(struct socket *so);
+extern int ksocknal_lib_send_iov(ksock_conn_t *conn, ksock_tx_t *tx);
+extern int ksocknal_lib_send_kiov(ksock_conn_t *conn, ksock_tx_t *tx);
+extern void ksocknal_lib_eager_ack(ksock_conn_t *conn);
+extern int ksocknal_lib_recv_iov(ksock_conn_t *conn);
+extern int ksocknal_lib_recv_kiov(ksock_conn_t *conn);
+extern int ksocknal_lib_get_conn_tunables(ksock_conn_t *conn, int *txmem,
 					   int *rxmem, int *nagle);
 
 extern int ksocknal_tunables_init(void);
diff --git a/drivers/staging/lustre/lnet/klnds/socklnd/socklnd_cb.c b/drivers/staging/lustre/lnet/klnds/socklnd/socklnd_cb.c
index 92760fe94184..fa7ad883bda9 100644
--- a/drivers/staging/lustre/lnet/klnds/socklnd/socklnd_cb.c
+++ b/drivers/staging/lustre/lnet/klnds/socklnd/socklnd_cb.c
@@ -1374,9 +1374,9 @@ ksocknal_sched_cansleep(ksock_sched_t *sched)
 
 	spin_lock_bh(&sched->kss_lock);
 
-	rc = (!ksocknal_data.ksnd_shuttingdown &&
+	rc = !ksocknal_data.ksnd_shuttingdown &&
 	      list_empty(&sched->kss_rx_conns) &&
-	      list_empty(&sched->kss_tx_conns));
+	      list_empty(&sched->kss_tx_conns);
 
 	spin_unlock_bh(&sched->kss_lock);
 	return rc;
diff --git a/drivers/staging/lustre/lnet/klnds/socklnd/socklnd_lib-linux.c b/drivers/staging/lustre/lnet/klnds/socklnd/socklnd_lib-linux.c
index 66cc509295e5..f5e8ab06070c 100644
--- a/drivers/staging/lustre/lnet/klnds/socklnd/socklnd_lib-linux.c
+++ b/drivers/staging/lustre/lnet/klnds/socklnd/socklnd_lib-linux.c
@@ -37,24 +37,24 @@
 #include "socklnd.h"
 
 int
-ksocknal_lib_get_conn_addrs (ksock_conn_t *conn)
+ksocknal_lib_get_conn_addrs(ksock_conn_t *conn)
 {
 	int rc = libcfs_sock_getaddr(conn->ksnc_sock, 1,
 				     &conn->ksnc_ipaddr,
 				     &conn->ksnc_port);
 
 	/* Didn't need the {get,put}connsock dance to deref ksnc_sock... */
-	LASSERT (!conn->ksnc_closing);
+	LASSERT(!conn->ksnc_closing);
 
 	if (rc != 0) {
-		CERROR ("Error %d getting sock peer IP\n", rc);
+		CERROR("Error %d getting sock peer IP\n", rc);
 		return rc;
 	}
 
 	rc = libcfs_sock_getaddr(conn->ksnc_sock, 0,
 				 &conn->ksnc_myipaddr, NULL);
 	if (rc != 0) {
-		CERROR ("Error %d getting sock local IP\n", rc);
+		CERROR("Error %d getting sock local IP\n", rc);
 		return rc;
 	}
 
@@ -75,7 +75,7 @@ ksocknal_lib_zc_capable(ksock_conn_t *conn)
 }
 
 int
-ksocknal_lib_send_iov (ksock_conn_t *conn, ksock_tx_t *tx)
+ksocknal_lib_send_iov(ksock_conn_t *conn, ksock_tx_t *tx)
 {
 	struct socket *sock = conn->ksnc_sock;
 	int	    nob;
@@ -117,7 +117,7 @@ ksocknal_lib_send_iov (ksock_conn_t *conn, ksock_tx_t *tx)
 }
 
 int
-ksocknal_lib_send_kiov (ksock_conn_t *conn, ksock_tx_t *tx)
+ksocknal_lib_send_kiov(ksock_conn_t *conn, ksock_tx_t *tx)
 {
 	struct socket *sock = conn->ksnc_sock;
 	lnet_kiov_t   *kiov = tx->tx_kiov;
@@ -125,7 +125,7 @@ ksocknal_lib_send_kiov (ksock_conn_t *conn, ksock_tx_t *tx)
 	int	    nob;
 
 	/* Not NOOP message */
-	LASSERT (tx->tx_lnetmsg != NULL);
+	LASSERT(tx->tx_lnetmsg != NULL);
 
 	/* NB we can't trust socket ops to either consume our iovs
 	 * or leave them alone. */
@@ -185,7 +185,7 @@ ksocknal_lib_send_kiov (ksock_conn_t *conn, ksock_tx_t *tx)
 }
 
 void
-ksocknal_lib_eager_ack (ksock_conn_t *conn)
+ksocknal_lib_eager_ack(ksock_conn_t *conn)
 {
 	int	    opt = 1;
 	struct socket *sock = conn->ksnc_sock;
@@ -196,11 +196,11 @@ ksocknal_lib_eager_ack (ksock_conn_t *conn)
 	 * peer. */
 
 	kernel_setsockopt(sock, SOL_TCP, TCP_QUICKACK,
-			       (char *)&opt, sizeof (opt));
+			       (char *)&opt, sizeof(opt));
 }
 
 int
-ksocknal_lib_recv_iov (ksock_conn_t *conn)
+ksocknal_lib_recv_iov(ksock_conn_t *conn)
 {
 #if SOCKNAL_SINGLE_FRAG_RX
 	struct kvec  scratch;
@@ -223,13 +223,13 @@ ksocknal_lib_recv_iov (ksock_conn_t *conn)
 
 	/* NB we can't trust socket ops to either consume our iovs
 	 * or leave them alone. */
-	LASSERT (niov > 0);
+	LASSERT(niov > 0);
 
 	for (nob = i = 0; i < niov; i++) {
 		scratchiov[i] = iov[i];
 		nob += scratchiov[i].iov_len;
 	}
-	LASSERT (nob <= conn->ksnc_rx_nob_wanted);
+	LASSERT(nob <= conn->ksnc_rx_nob_wanted);
 
 	rc = kernel_recvmsg(conn->ksnc_sock, &msg,
 		scratchiov, niov, nob, MSG_DONTWAIT);
@@ -243,7 +243,7 @@ ksocknal_lib_recv_iov (ksock_conn_t *conn)
 	if (saved_csum != 0) {
 		/* accumulate checksum */
 		for (i = 0, sum = rc; sum > 0; i++, sum -= fragnob) {
-			LASSERT (i < niov);
+			LASSERT(i < niov);
 
 			fragnob = iov[i].iov_len;
 			if (fragnob > sum)
@@ -278,7 +278,7 @@ ksocknal_lib_kiov_vmap(lnet_kiov_t *kiov, int niov,
 	if (!*ksocknal_tunables.ksnd_zc_recv || pages == NULL)
 		return NULL;
 
-	LASSERT (niov <= LNET_MAX_IOV);
+	LASSERT(niov <= LNET_MAX_IOV);
 
 	if (niov < 2 ||
 	    niov < *ksocknal_tunables.ksnd_zc_recv_min_nfrags)
@@ -304,7 +304,7 @@ ksocknal_lib_kiov_vmap(lnet_kiov_t *kiov, int niov,
 }
 
 int
-ksocknal_lib_recv_kiov (ksock_conn_t *conn)
+ksocknal_lib_recv_kiov(ksock_conn_t *conn)
 {
 #if SOCKNAL_SINGLE_FRAG_RX || !SOCKNAL_RISK_KMAP_DEADLOCK
 	struct kvec   scratch;
@@ -348,14 +348,14 @@ ksocknal_lib_recv_kiov (ksock_conn_t *conn)
 		n = niov;
 	}
 
-	LASSERT (nob <= conn->ksnc_rx_nob_wanted);
+	LASSERT(nob <= conn->ksnc_rx_nob_wanted);
 
 	rc = kernel_recvmsg(conn->ksnc_sock, &msg,
 			(struct kvec *)scratchiov, n, nob, MSG_DONTWAIT);
 
 	if (conn->ksnc_msg.ksm_csum != 0) {
 		for (i = 0, sum = rc; sum > 0; i++, sum -= fragnob) {
-			LASSERT (i < niov);
+			LASSERT(i < niov);
 
 			/* Dang! have to kmap again because I have nowhere to stash the
 			 * mapped address.  But by doing it while the page is still
@@ -423,7 +423,7 @@ ksocknal_lib_csum_tx(ksock_tx_t *tx)
 }
 
 int
-ksocknal_lib_get_conn_tunables (ksock_conn_t *conn, int *txmem, int *rxmem, int *nagle)
+ksocknal_lib_get_conn_tunables(ksock_conn_t *conn, int *txmem, int *rxmem, int *nagle)
 {
 	struct socket *sock = conn->ksnc_sock;
 	int	    len;
@@ -431,7 +431,7 @@ ksocknal_lib_get_conn_tunables (ksock_conn_t *conn, int *txmem, int *rxmem, int
 
 	rc = ksocknal_connsock_addref(conn);
 	if (rc != 0) {
-		LASSERT (conn->ksnc_closing);
+		LASSERT(conn->ksnc_closing);
 		*txmem = *rxmem = *nagle = 0;
 		return -ESHUTDOWN;
 	}
@@ -454,7 +454,7 @@ ksocknal_lib_get_conn_tunables (ksock_conn_t *conn, int *txmem, int *rxmem, int
 }
 
 int
-ksocknal_lib_setup_sock (struct socket *sock)
+ksocknal_lib_setup_sock(struct socket *sock)
 {
 	int	     rc;
 	int	     option;
@@ -473,17 +473,17 @@ ksocknal_lib_setup_sock (struct socket *sock)
 	linger.l_linger = 0;
 
 	rc = kernel_setsockopt(sock, SOL_SOCKET, SO_LINGER,
-			      (char *)&linger, sizeof (linger));
+			      (char *)&linger, sizeof(linger));
 	if (rc != 0) {
-		CERROR ("Can't set SO_LINGER: %d\n", rc);
+		CERROR("Can't set SO_LINGER: %d\n", rc);
 		return rc;
 	}
 
 	option = -1;
 	rc = kernel_setsockopt(sock, SOL_TCP, TCP_LINGER2,
-				    (char *)&option, sizeof (option));
+				    (char *)&option, sizeof(option));
 	if (rc != 0) {
-		CERROR ("Can't set SO_LINGER2: %d\n", rc);
+		CERROR("Can't set SO_LINGER2: %d\n", rc);
 		return rc;
 	}
 
@@ -491,9 +491,9 @@ ksocknal_lib_setup_sock (struct socket *sock)
 		option = 1;
 
 		rc = kernel_setsockopt(sock, SOL_TCP, TCP_NODELAY,
-					    (char *)&option, sizeof (option));
+					    (char *)&option, sizeof(option));
 		if (rc != 0) {
-			CERROR ("Can't disable nagle: %d\n", rc);
+			CERROR("Can't disable nagle: %d\n", rc);
 			return rc;
 		}
 	}
@@ -502,7 +502,7 @@ ksocknal_lib_setup_sock (struct socket *sock)
 				*ksocknal_tunables.ksnd_tx_buffer_size,
 				*ksocknal_tunables.ksnd_rx_buffer_size);
 	if (rc != 0) {
-		CERROR ("Can't set buffer tx %d, rx %d buffers: %d\n",
+		CERROR("Can't set buffer tx %d, rx %d buffers: %d\n",
 			*ksocknal_tunables.ksnd_tx_buffer_size,
 			*ksocknal_tunables.ksnd_rx_buffer_size, rc);
 		return rc;
@@ -519,9 +519,9 @@ ksocknal_lib_setup_sock (struct socket *sock)
 
 	option = (do_keepalive ? 1 : 0);
 	rc = kernel_setsockopt(sock, SOL_SOCKET, SO_KEEPALIVE,
-			      (char *)&option, sizeof (option));
+			      (char *)&option, sizeof(option));
 	if (rc != 0) {
-		CERROR ("Can't set SO_KEEPALIVE: %d\n", rc);
+		CERROR("Can't set SO_KEEPALIVE: %d\n", rc);
 		return rc;
 	}
 
@@ -529,23 +529,23 @@ ksocknal_lib_setup_sock (struct socket *sock)
 		return 0;
 
 	rc = kernel_setsockopt(sock, SOL_TCP, TCP_KEEPIDLE,
-				    (char *)&keep_idle, sizeof (keep_idle));
+				    (char *)&keep_idle, sizeof(keep_idle));
 	if (rc != 0) {
-		CERROR ("Can't set TCP_KEEPIDLE: %d\n", rc);
+		CERROR("Can't set TCP_KEEPIDLE: %d\n", rc);
 		return rc;
 	}
 
 	rc = kernel_setsockopt(sock, SOL_TCP, TCP_KEEPINTVL,
-				    (char *)&keep_intvl, sizeof (keep_intvl));
+				    (char *)&keep_intvl, sizeof(keep_intvl));
 	if (rc != 0) {
-		CERROR ("Can't set TCP_KEEPINTVL: %d\n", rc);
+		CERROR("Can't set TCP_KEEPINTVL: %d\n", rc);
 		return rc;
 	}
 
 	rc = kernel_setsockopt(sock, SOL_TCP, TCP_KEEPCNT,
-				    (char *)&keep_count, sizeof (keep_count));
+				    (char *)&keep_count, sizeof(keep_count));
 	if (rc != 0) {
-		CERROR ("Can't set TCP_KEEPCNT: %d\n", rc);
+		CERROR("Can't set TCP_KEEPCNT: %d\n", rc);
 		return rc;
 	}
 
@@ -553,7 +553,7 @@ ksocknal_lib_setup_sock (struct socket *sock)
 }
 
 void
-ksocknal_lib_push_conn (ksock_conn_t *conn)
+ksocknal_lib_push_conn(ksock_conn_t *conn)
 {
 	struct sock    *sk;
 	struct tcp_sock *tp;
@@ -568,29 +568,29 @@ ksocknal_lib_push_conn (ksock_conn_t *conn)
 	sk = conn->ksnc_sock->sk;
 	tp = tcp_sk(sk);
 
-	lock_sock (sk);
+	lock_sock(sk);
 	nonagle = tp->nonagle;
 	tp->nonagle = 1;
-	release_sock (sk);
+	release_sock(sk);
 
 	rc = kernel_setsockopt(conn->ksnc_sock, SOL_TCP, TCP_NODELAY,
-				      (char *)&val, sizeof (val));
-	LASSERT (rc == 0);
+				      (char *)&val, sizeof(val));
+	LASSERT(rc == 0);
 
-	lock_sock (sk);
+	lock_sock(sk);
 	tp->nonagle = nonagle;
-	release_sock (sk);
+	release_sock(sk);
 
 	ksocknal_connsock_decref(conn);
 }
 
-extern void ksocknal_read_callback (ksock_conn_t *conn);
-extern void ksocknal_write_callback (ksock_conn_t *conn);
+extern void ksocknal_read_callback(ksock_conn_t *conn);
+extern void ksocknal_write_callback(ksock_conn_t *conn);
 /*
  * socket call back in Linux
  */
 static void
-ksocknal_data_ready (struct sock *sk)
+ksocknal_data_ready(struct sock *sk)
 {
 	ksock_conn_t  *conn;
 
@@ -600,8 +600,8 @@ ksocknal_data_ready (struct sock *sk)
 
 	conn = sk->sk_user_data;
 	if (conn == NULL) {	     /* raced with ksocknal_terminate_conn */
-		LASSERT (sk->sk_data_ready != &ksocknal_data_ready);
-		sk->sk_data_ready (sk);
+		LASSERT(sk->sk_data_ready != &ksocknal_data_ready);
+		sk->sk_data_ready(sk);
 	} else
 		ksocknal_read_callback(conn);
 
@@ -609,7 +609,7 @@ ksocknal_data_ready (struct sock *sk)
 }
 
 static void
-ksocknal_write_space (struct sock *sk)
+ksocknal_write_space(struct sock *sk)
 {
 	ksock_conn_t  *conn;
 	int	    wspace;
@@ -629,12 +629,12 @@ ksocknal_write_space (struct sock *sk)
 				      " ready" : " blocked"),
 	       (conn == NULL) ? "" : (conn->ksnc_tx_scheduled ?
 				      " scheduled" : " idle"),
-	       (conn == NULL) ? "" : (list_empty (&conn->ksnc_tx_queue) ?
+	       (conn == NULL) ? "" : (list_empty(&conn->ksnc_tx_queue) ?
 				      " empty" : " queued"));
 
 	if (conn == NULL) {	     /* raced with ksocknal_terminate_conn */
-		LASSERT (sk->sk_write_space != &ksocknal_write_space);
-		sk->sk_write_space (sk);
+		LASSERT(sk->sk_write_space != &ksocknal_write_space);
+		sk->sk_write_space(sk);
 
 		read_unlock(&ksocknal_data.ksnd_global_lock);
 		return;
@@ -647,7 +647,7 @@ ksocknal_write_space (struct sock *sk)
 		 * ENOMEM check in ksocknal_transmit is race-free (think about
 		 * it). */
 
-		clear_bit (SOCK_NOSPACE, &sk->sk_socket->flags);
+		clear_bit(SOCK_NOSPACE, &sk->sk_socket->flags);
 	}
 
 	read_unlock(&ksocknal_data.ksnd_global_lock);
diff --git a/drivers/staging/lustre/lnet/klnds/socklnd/socklnd_lib-linux.h b/drivers/staging/lustre/lnet/klnds/socklnd/socklnd_lib-linux.h
index 7a793d2d3582..f5563881b25c 100644
--- a/drivers/staging/lustre/lnet/klnds/socklnd/socklnd_lib-linux.h
+++ b/drivers/staging/lustre/lnet/klnds/socklnd/socklnd_lib-linux.h
@@ -50,8 +50,8 @@
 #include <net/tcp.h>
 #include <linux/uio.h>
 #include <linux/if.h>
+#include <linux/uaccess.h>
 
-#include <asm/uaccess.h>
 #include <asm/irq.h>
 
 #include <linux/fs.h>
diff --git a/drivers/staging/lustre/lnet/klnds/socklnd/socklnd_modparams.c b/drivers/staging/lustre/lnet/klnds/socklnd/socklnd_modparams.c
index 66d78c9be650..86b88db1cf20 100644
--- a/drivers/staging/lustre/lnet/klnds/socklnd/socklnd_modparams.c
+++ b/drivers/staging/lustre/lnet/klnds/socklnd/socklnd_modparams.c
@@ -72,7 +72,7 @@ static int typed_conns = 1;
 module_param(typed_conns, int, 0444);
 MODULE_PARM_DESC(typed_conns, "use different sockets for bulk");
 
-static int min_bulk = (1<<10);
+static int min_bulk = 1<<10;
 module_param(min_bulk, int, 0644);
 MODULE_PARM_DESC(min_bulk, "smallest 'large' message");
 
@@ -122,7 +122,7 @@ static int nonblk_zcack = 1;
 module_param(nonblk_zcack, int, 0644);
 MODULE_PARM_DESC(nonblk_zcack, "always send ZC-ACK on non-blocking connection");
 
-static unsigned int zc_min_payload = (16 << 10);
+static unsigned int zc_min_payload = 16 << 10;
 module_param(zc_min_payload, int, 0644);
 MODULE_PARM_DESC(zc_min_payload, "minimum payload size to zero copy");
 
@@ -182,7 +182,7 @@ int ksocknal_tunables_init(void)
 #endif
 
 	if (*ksocknal_tunables.ksnd_zc_min_payload < (2 << 10))
-		*ksocknal_tunables.ksnd_zc_min_payload = (2 << 10);
+		*ksocknal_tunables.ksnd_zc_min_payload = 2 << 10;
 
 	return 0;
 };
diff --git a/drivers/staging/lustre/lnet/klnds/socklnd/socklnd_proto.c b/drivers/staging/lustre/lnet/klnds/socklnd/socklnd_proto.c
index b2f88eb47bba..8596581f54ff 100644
--- a/drivers/staging/lustre/lnet/klnds/socklnd/socklnd_proto.c
+++ b/drivers/staging/lustre/lnet/klnds/socklnd/socklnd_proto.c
@@ -55,8 +55,8 @@ ksocknal_next_tx_carrier(ksock_conn_t *conn)
 	ksock_tx_t     *tx = conn->ksnc_tx_carrier;
 
 	/* Called holding BH lock: conn->ksnc_scheduler->kss_lock */
-	LASSERT (!list_empty(&conn->ksnc_tx_queue));
-	LASSERT (tx != NULL);
+	LASSERT(!list_empty(&conn->ksnc_tx_queue));
+	LASSERT(tx != NULL);
 
 	/* Next TX that can carry ZC-ACK or LNet message */
 	if (tx->tx_list.next == &conn->ksnc_tx_queue) {
@@ -65,7 +65,7 @@ ksocknal_next_tx_carrier(ksock_conn_t *conn)
 	} else {
 		conn->ksnc_tx_carrier = list_entry(tx->tx_list.next,
 						       ksock_tx_t, tx_list);
-		LASSERT (conn->ksnc_tx_carrier->tx_msg.ksm_type == tx->tx_msg.ksm_type);
+		LASSERT(conn->ksnc_tx_carrier->tx_msg.ksm_type == tx->tx_msg.ksm_type);
 	}
 }
 
@@ -75,7 +75,7 @@ ksocknal_queue_tx_zcack_v2(ksock_conn_t *conn,
 {
 	ksock_tx_t *tx = conn->ksnc_tx_carrier;
 
-	LASSERT (tx_ack == NULL ||
+	LASSERT(tx_ack == NULL ||
 		 tx_ack->tx_msg.ksm_type == KSOCK_MSG_NOOP);
 
 	/*
@@ -139,7 +139,7 @@ ksocknal_queue_tx_msg_v2(ksock_conn_t *conn, ksock_tx_t *tx_msg)
 		return NULL;
 	}
 
-	LASSERT (tx->tx_msg.ksm_type == KSOCK_MSG_NOOP);
+	LASSERT(tx->tx_msg.ksm_type == KSOCK_MSG_NOOP);
 
 	/* There is a noop zc-ack can be piggybacked */
 	tx_msg->tx_msg.ksm_zc_cookies[1] = tx->tx_msg.ksm_zc_cookies[1];
@@ -162,7 +162,7 @@ ksocknal_queue_tx_zcack_v3(ksock_conn_t *conn,
 		return ksocknal_queue_tx_zcack_v2(conn, tx_ack, cookie);
 
 	/* non-blocking ZC-ACK (to router) */
-	LASSERT (tx_ack == NULL ||
+	LASSERT(tx_ack == NULL ||
 		 tx_ack->tx_msg.ksm_type == KSOCK_MSG_NOOP);
 
 	tx = conn->ksnc_tx_carrier;
@@ -185,7 +185,7 @@ ksocknal_queue_tx_zcack_v3(ksock_conn_t *conn,
 
 	if (tx->tx_msg.ksm_zc_cookies[1] == SOCKNAL_KEEPALIVE_PING) {
 		/* replace the keepalive PING with a real ACK */
-		LASSERT (tx->tx_msg.ksm_zc_cookies[0] == 0);
+		LASSERT(tx->tx_msg.ksm_zc_cookies[0] == 0);
 		tx->tx_msg.ksm_zc_cookies[1] = cookie;
 		return 1;
 	}
@@ -220,7 +220,7 @@ ksocknal_queue_tx_zcack_v3(ksock_conn_t *conn,
 		__u64   tmp = 0;
 
 		/* two separated cookies: (a+2, a) or (a+1, a) */
-		LASSERT (tx->tx_msg.ksm_zc_cookies[0] -
+		LASSERT(tx->tx_msg.ksm_zc_cookies[0] -
 			 tx->tx_msg.ksm_zc_cookies[1] <= 2);
 
 		if (tx->tx_msg.ksm_zc_cookies[0] -
@@ -408,7 +408,7 @@ ksocknal_handle_zcack(ksock_conn_t *conn, __u64 cookie1, __u64 cookie2)
 	ksock_peer_t      *peer = conn->ksnc_peer;
 	ksock_tx_t	*tx;
 	ksock_tx_t	*tmp;
-	LIST_HEAD     (zlist);
+	LIST_HEAD(zlist);
 	int		count;
 
 	if (cookie1 == 0)
@@ -450,7 +450,7 @@ ksocknal_handle_zcack(ksock_conn_t *conn, __u64 cookie1, __u64 cookie2)
 }
 
 static int
-ksocknal_send_hello_v1 (ksock_conn_t *conn, ksock_hello_msg_t *hello)
+ksocknal_send_hello_v1(ksock_conn_t *conn, ksock_hello_msg_t *hello)
 {
 	struct socket	*sock = conn->ksnc_sock;
 	lnet_hdr_t	  *hdr;
@@ -526,7 +526,7 @@ out:
 }
 
 static int
-ksocknal_send_hello_v2 (ksock_conn_t *conn, ksock_hello_msg_t *hello)
+ksocknal_send_hello_v2(ksock_conn_t *conn, ksock_hello_msg_t *hello)
 {
 	struct socket *sock = conn->ksnc_sock;
 	int	     rc;
@@ -584,12 +584,12 @@ ksocknal_recv_hello_v1(ksock_conn_t *conn, ksock_hello_msg_t *hello,
 	}
 
 	rc = libcfs_sock_read(sock, &hdr->src_nid,
-			      sizeof (*hdr) - offsetof (lnet_hdr_t, src_nid),
+			      sizeof(*hdr) - offsetof(lnet_hdr_t, src_nid),
 			      timeout);
 	if (rc != 0) {
 		CERROR("Error %d reading rest of HELLO hdr from %pI4h\n",
 			rc, &conn->ksnc_ipaddr);
-		LASSERT (rc < 0 && rc != -EALREADY);
+		LASSERT(rc < 0 && rc != -EALREADY);
 		goto out;
 	}
 
@@ -602,12 +602,12 @@ ksocknal_recv_hello_v1(ksock_conn_t *conn, ksock_hello_msg_t *hello,
 		goto out;
 	}
 
-	hello->kshm_src_nid	 = le64_to_cpu (hdr->src_nid);
-	hello->kshm_src_pid	 = le32_to_cpu (hdr->src_pid);
-	hello->kshm_src_incarnation = le64_to_cpu (hdr->msg.hello.incarnation);
-	hello->kshm_ctype	   = le32_to_cpu (hdr->msg.hello.type);
-	hello->kshm_nips	    = le32_to_cpu (hdr->payload_length) /
-					 sizeof (__u32);
+	hello->kshm_src_nid	 = le64_to_cpu(hdr->src_nid);
+	hello->kshm_src_pid	 = le32_to_cpu(hdr->src_pid);
+	hello->kshm_src_incarnation = le64_to_cpu(hdr->msg.hello.incarnation);
+	hello->kshm_ctype	   = le32_to_cpu(hdr->msg.hello.type);
+	hello->kshm_nips	    = le32_to_cpu(hdr->payload_length) /
+					 sizeof(__u32);
 
 	if (hello->kshm_nips > LNET_MAX_INTERFACES) {
 		CERROR("Bad nips %d from ip %pI4h\n",
@@ -645,7 +645,7 @@ out:
 }
 
 static int
-ksocknal_recv_hello_v2 (ksock_conn_t *conn, ksock_hello_msg_t *hello, int timeout)
+ksocknal_recv_hello_v2(ksock_conn_t *conn, ksock_hello_msg_t *hello, int timeout)
 {
 	struct socket   *sock = conn->ksnc_sock;
 	int		rc;
diff --git a/drivers/staging/lustre/lnet/lnet/api-ni.c b/drivers/staging/lustre/lnet/lnet/api-ni.c
index faceb9505d84..4a14e5109821 100644
--- a/drivers/staging/lustre/lnet/lnet/api-ni.c
+++ b/drivers/staging/lustre/lnet/lnet/api-ni.c
@@ -650,15 +650,19 @@ lnet_prepare(lnet_pid_t requested_pid)
 
 	recs = lnet_res_containers_create(LNET_COOKIE_TYPE_ME, LNET_FL_MAX_MES,
 					  sizeof(lnet_me_t));
-	if (recs == NULL)
+	if (recs == NULL) {
+		rc = -ENOMEM;
 		goto failed;
+	}
 
 	the_lnet.ln_me_containers = recs;
 
 	recs = lnet_res_containers_create(LNET_COOKIE_TYPE_MD, LNET_FL_MAX_MDS,
 					  sizeof(lnet_libmd_t));
-	if (recs == NULL)
+	if (recs == NULL) {
+		rc = -ENOMEM;
 		goto failed;
+	}
 
 	the_lnet.ln_md_containers = recs;
 
diff --git a/drivers/staging/lustre/lnet/lnet/config.c b/drivers/staging/lustre/lnet/lnet/config.c
index af171e25a5ec..2dc4c4a1afd0 100644
--- a/drivers/staging/lustre/lnet/lnet/config.c
+++ b/drivers/staging/lustre/lnet/lnet/config.c
@@ -37,11 +37,11 @@
 #define DEBUG_SUBSYSTEM S_LNET
 #include "../../include/linux/lnet/lib-lnet.h"
 
-typedef struct {			    /* tmp struct for parsing routes */
+struct lnet_text_buf_t {	    /* tmp struct for parsing routes */
 	struct list_head	 ltb_list;	/* stash on lists */
 	int		ltb_size;	/* allocated size */
 	char	       ltb_text[0];     /* text buffer */
-} lnet_text_buf_t;
+};
 
 static int lnet_tbnob;			/* track text buf allocation */
 #define LNET_MAX_TEXTBUF_NOB     (64<<10)	/* bound allocation */
@@ -365,14 +365,14 @@ lnet_parse_networks(struct list_head *nilist, char *networks)
 	return -EINVAL;
 }
 
-static lnet_text_buf_t *
+static struct lnet_text_buf_t *
 lnet_new_text_buf(int str_len)
 {
-	lnet_text_buf_t *ltb;
+	struct lnet_text_buf_t *ltb;
 	int	      nob;
 
 	/* NB allocate space for the terminating 0 */
-	nob = offsetof(lnet_text_buf_t, ltb_text[str_len + 1]);
+	nob = offsetof(struct lnet_text_buf_t, ltb_text[str_len + 1]);
 	if (nob > LNET_SINGLE_TEXTBUF_NOB) {
 		/* _way_ conservative for "route net gateway..." */
 		CERROR("text buffer too big\n");
@@ -395,7 +395,7 @@ lnet_new_text_buf(int str_len)
 }
 
 static void
-lnet_free_text_buf(lnet_text_buf_t *ltb)
+lnet_free_text_buf(struct lnet_text_buf_t *ltb)
 {
 	lnet_tbnob -= ltb->ltb_size;
 	LIBCFS_FREE(ltb, ltb->ltb_size);
@@ -404,10 +404,10 @@ lnet_free_text_buf(lnet_text_buf_t *ltb)
 static void
 lnet_free_text_bufs(struct list_head *tbs)
 {
-	lnet_text_buf_t  *ltb;
+	struct lnet_text_buf_t  *ltb;
 
 	while (!list_empty(tbs)) {
-		ltb = list_entry(tbs->next, lnet_text_buf_t, ltb_list);
+		ltb = list_entry(tbs->next, struct lnet_text_buf_t, ltb_list);
 
 		list_del(&ltb->ltb_list);
 		lnet_free_text_buf(ltb);
@@ -421,7 +421,7 @@ lnet_str2tbs_sep(struct list_head *tbs, char *str)
 	char	     *sep;
 	int	       nob;
 	int	       i;
-	lnet_text_buf_t  *ltb;
+	struct lnet_text_buf_t  *ltb;
 
 	INIT_LIST_HEAD(&pending);
 
@@ -479,7 +479,7 @@ lnet_expand1tb(struct list_head *list,
 {
 	int	      len1 = (int)(sep1 - str);
 	int	      len2 = strlen(sep2 + 1);
-	lnet_text_buf_t *ltb;
+	struct lnet_text_buf_t *ltb;
 
 	LASSERT(*sep1 == '[');
 	LASSERT(*sep2 == ']');
@@ -532,14 +532,18 @@ lnet_str2tbs_expand(struct list_head *tbs, char *str)
 		if (enditem == parsed)		/* no empty items */
 			goto failed;
 
-		if (sscanf(parsed, "%d-%d/%d%n", &lo, &hi, &stride, &scanned) < 3) {
+		if (sscanf(parsed, "%d-%d/%d%n", &lo, &hi,
+			   &stride, &scanned) < 3) {
 
 			if (sscanf(parsed, "%d-%d%n", &lo, &hi, &scanned) < 2) {
 
 				/* simple string enumeration */
-				if (lnet_expand1tb(&pending, str, sep, sep2,
-						   parsed, (int)(enditem - parsed)) != 0)
+				if (lnet_expand1tb(
+				     &pending, str, sep, sep2,
+				     parsed,
+				     (int)(enditem - parsed)) != 0) {
 					goto failed;
+				}
 
 				continue;
 			}
@@ -632,7 +636,7 @@ lnet_parse_route(char *str, int *im_a_router)
 	struct list_head       *tmp2;
 	__u32	     net;
 	lnet_nid_t	nid;
-	lnet_text_buf_t  *ltb;
+	struct lnet_text_buf_t  *ltb;
 	int	       rc;
 	char	     *sep;
 	char	     *token = str;
@@ -688,7 +692,8 @@ lnet_parse_route(char *str, int *im_a_router)
 		list_add_tail(tmp1, tmp2);
 
 		while (tmp1 != tmp2) {
-			ltb = list_entry(tmp1, lnet_text_buf_t, ltb_list);
+			ltb = list_entry(tmp1, struct lnet_text_buf_t,
+					 ltb_list);
 
 			rc = lnet_str2tbs_expand(tmp1->next, ltb->ltb_text);
 			if (rc < 0)
@@ -728,12 +733,13 @@ lnet_parse_route(char *str, int *im_a_router)
 	LASSERT(!list_empty(&gateways));
 
 	list_for_each(tmp1, &nets) {
-		ltb = list_entry(tmp1, lnet_text_buf_t, ltb_list);
+		ltb = list_entry(tmp1, struct lnet_text_buf_t, ltb_list);
 		net = libcfs_str2net(ltb->ltb_text);
 		LASSERT(net != LNET_NIDNET(LNET_NID_ANY));
 
 		list_for_each(tmp2, &gateways) {
-			ltb = list_entry(tmp2, lnet_text_buf_t, ltb_list);
+			ltb = list_entry(tmp2, struct lnet_text_buf_t,
+					 ltb_list);
 			nid = libcfs_str2nid(ltb->ltb_text);
 			LASSERT(nid != LNET_NID_ANY);
 
@@ -766,10 +772,10 @@ lnet_parse_route(char *str, int *im_a_router)
 static int
 lnet_parse_route_tbs(struct list_head *tbs, int *im_a_router)
 {
-	lnet_text_buf_t   *ltb;
+	struct lnet_text_buf_t   *ltb;
 
 	while (!list_empty(tbs)) {
-		ltb = list_entry(tbs->next, lnet_text_buf_t, ltb_list);
+		ltb = list_entry(tbs->next, struct lnet_text_buf_t, ltb_list);
 
 		if (lnet_parse_route(ltb->ltb_text, im_a_router) < 0) {
 			lnet_free_text_bufs(tbs);
@@ -903,8 +909,8 @@ lnet_splitnets(char *source, struct list_head *nets)
 	int	       offset = 0;
 	int	       offset2;
 	int	       len;
-	lnet_text_buf_t  *tb;
-	lnet_text_buf_t  *tb2;
+	struct lnet_text_buf_t  *tb;
+	struct lnet_text_buf_t  *tb2;
 	struct list_head       *t;
 	char	     *sep;
 	char	     *bracket;
@@ -913,7 +919,7 @@ lnet_splitnets(char *source, struct list_head *nets)
 	LASSERT(!list_empty(nets));
 	LASSERT(nets->next == nets->prev);     /* single entry */
 
-	tb = list_entry(nets->next, lnet_text_buf_t, ltb_list);
+	tb = list_entry(nets->next, struct lnet_text_buf_t, ltb_list);
 
 	for (;;) {
 		sep = strchr(tb->ltb_text, ',');
@@ -949,7 +955,7 @@ lnet_splitnets(char *source, struct list_head *nets)
 		}
 
 		list_for_each(t, nets) {
-			tb2 = list_entry(t, lnet_text_buf_t, ltb_list);
+			tb2 = list_entry(t, struct lnet_text_buf_t, ltb_list);
 
 			if (tb2 == tb)
 				continue;
@@ -988,8 +994,8 @@ lnet_match_networks(char **networksp, char *ip2nets, __u32 *ipaddrs, int nip)
 	struct list_head	  current_nets;
 	struct list_head	 *t;
 	struct list_head	 *t2;
-	lnet_text_buf_t    *tb;
-	lnet_text_buf_t    *tb2;
+	struct lnet_text_buf_t    *tb;
+	struct lnet_text_buf_t    *tb2;
 	__u32	       net1;
 	__u32	       net2;
 	int		 len;
@@ -1012,7 +1018,7 @@ lnet_match_networks(char **networksp, char *ip2nets, __u32 *ipaddrs, int nip)
 	rc = 0;
 
 	while (!list_empty(&raw_entries)) {
-		tb = list_entry(raw_entries.next, lnet_text_buf_t,
+		tb = list_entry(raw_entries.next, struct lnet_text_buf_t,
 				    ltb_list);
 
 		strncpy(source, tb->ltb_text, sizeof(source)-1);
@@ -1039,12 +1045,12 @@ lnet_match_networks(char **networksp, char *ip2nets, __u32 *ipaddrs, int nip)
 
 		dup = 0;
 		list_for_each(t, &current_nets) {
-			tb = list_entry(t, lnet_text_buf_t, ltb_list);
+			tb = list_entry(t, struct lnet_text_buf_t, ltb_list);
 			net1 = lnet_netspec2net(tb->ltb_text);
 			LASSERT(net1 != LNET_NIDNET(LNET_NID_ANY));
 
 			list_for_each(t2, &matched_nets) {
-				tb2 = list_entry(t2, lnet_text_buf_t,
+				tb2 = list_entry(t2, struct lnet_text_buf_t,
 						     ltb_list);
 				net2 = lnet_netspec2net(tb2->ltb_text);
 				LASSERT(net2 != LNET_NIDNET(LNET_NID_ANY));
@@ -1065,7 +1071,7 @@ lnet_match_networks(char **networksp, char *ip2nets, __u32 *ipaddrs, int nip)
 		}
 
 		list_for_each_safe(t, t2, &current_nets) {
-			tb = list_entry(t, lnet_text_buf_t, ltb_list);
+			tb = list_entry(t, struct lnet_text_buf_t, ltb_list);
 
 			list_del(&tb->ltb_list);
 			list_add_tail(&tb->ltb_list, &matched_nets);
diff --git a/drivers/staging/lustre/lnet/lnet/lib-eq.c b/drivers/staging/lustre/lnet/lnet/lib-eq.c
index 863cc3735edf..5470148f5b64 100644
--- a/drivers/staging/lustre/lnet/lnet/lib-eq.c
+++ b/drivers/staging/lustre/lnet/lnet/lib-eq.c
@@ -81,9 +81,8 @@ LNetEQAlloc(unsigned int count, lnet_eq_handler_t callback,
 
 	count = cfs_power2_roundup(count);
 
-	if (callback != LNET_EQ_HANDLER_NONE && count != 0) {
+	if (callback != LNET_EQ_HANDLER_NONE && count != 0)
 		CWARN("EQ callback is guaranteed to get every event, do you still want to set eqcount %d for polling event which will have locking overhead? Please contact with developer to confirm\n", count);
-	}
 
 	/* count can be 0 if only need callback, we can eliminate
 	 * overhead of enqueue event */
diff --git a/drivers/staging/lustre/lnet/lnet/lib-md.c b/drivers/staging/lustre/lnet/lnet/lib-md.c
index 3225c069637d..89d660fefd48 100644
--- a/drivers/staging/lustre/lnet/lnet/lib-md.c
+++ b/drivers/staging/lustre/lnet/lnet/lib-md.c
@@ -123,7 +123,7 @@ lnet_md_build(lnet_libmd_t *lmd, lnet_md_t *umd, int unlink)
 
 		lmd->md_length = total_length;
 
-		if ((umd->options & LNET_MD_MAX_SIZE) != 0 && /* max size used */
+		if ((umd->options & LNET_MD_MAX_SIZE) != 0 && /* use max size */
 		    (umd->max_size < 0 ||
 		     umd->max_size > total_length)) /* illegal max_size */
 			return -EINVAL;
diff --git a/drivers/staging/lustre/lnet/lnet/lib-move.c b/drivers/staging/lustre/lnet/lnet/lib-move.c
index 0f53c761f1a9..c2fb70e5fc4e 100644
--- a/drivers/staging/lustre/lnet/lnet/lib-move.c
+++ b/drivers/staging/lustre/lnet/lnet/lib-move.c
@@ -1530,7 +1530,7 @@ lnet_parse_reply(lnet_ni_t *ni, lnet_msg_t *msg)
 	LASSERT(md->md_offset == 0);
 
 	rlength = hdr->payload_length;
-	mlength = min_t(int, rlength, md->md_length);
+	mlength = min_t(uint, rlength, md->md_length);
 
 	if (mlength < rlength &&
 	    (md->md_options & LNET_MD_TRUNCATE) == 0) {
diff --git a/drivers/staging/lustre/lnet/lnet/peer.c b/drivers/staging/lustre/lnet/lnet/peer.c
index c93ae8510530..45b5742f1bd2 100644
--- a/drivers/staging/lustre/lnet/lnet/peer.c
+++ b/drivers/staging/lustre/lnet/lnet/peer.c
@@ -87,7 +87,7 @@ lnet_peer_tables_destroy(void)
 
 	cfs_percpt_for_each(ptable, i, the_lnet.ln_peer_tables) {
 		hash = ptable->pt_hash;
-		if (hash == NULL) /* not intialized */
+		if (hash == NULL) /* not initialized */
 			break;
 
 		LASSERT(list_empty(&ptable->pt_deathrow));
@@ -132,7 +132,7 @@ lnet_peer_tables_cleanup(void)
 	}
 
 	cfs_percpt_for_each(ptable, i, the_lnet.ln_peer_tables) {
-		LIST_HEAD	(deathrow);
+		LIST_HEAD(deathrow);
 		lnet_peer_t	*lp;
 
 		lnet_net_lock(i);
diff --git a/drivers/staging/lustre/lnet/lnet/router.c b/drivers/staging/lustre/lnet/lnet/router.c
index 52ec0ab7e3c3..8510bae4822a 100644
--- a/drivers/staging/lustre/lnet/lnet/router.c
+++ b/drivers/staging/lustre/lnet/lnet/router.c
@@ -107,7 +107,8 @@ lnet_peers_start_down(void)
 }
 
 void
-lnet_notify_locked(lnet_peer_t *lp, int notifylnd, int alive, unsigned long when)
+lnet_notify_locked(lnet_peer_t *lp, int notifylnd, int alive,
+		   unsigned long when)
 {
 	if (time_before(when, lp->lp_timestamp)) { /* out of date information */
 		CDEBUG(D_NET, "Out of date\n");
@@ -225,7 +226,7 @@ lnet_rtr_decref_locked(lnet_peer_t *lp)
 }
 
 lnet_remotenet_t *
-lnet_find_net_locked (__u32 net)
+lnet_find_net_locked(__u32 net)
 {
 	lnet_remotenet_t	*rnet;
 	struct list_head		*tmp;
@@ -269,12 +270,11 @@ static void lnet_shuffle_seed(void)
 	do_gettimeofday(&tv);
 	cfs_srand(tv.tv_sec ^ seed[0], tv.tv_usec ^ seed[1]);
 	seeded = 1;
-	return;
 }
 
 /* NB expects LNET_LOCK held */
 static void
-lnet_add_route_to_rnet (lnet_remotenet_t *rnet, lnet_route_t *route)
+lnet_add_route_to_rnet(lnet_remotenet_t *rnet, lnet_route_t *route)
 {
 	unsigned int      len = 0;
 	unsigned int      offset = 0;
@@ -282,13 +282,13 @@ lnet_add_route_to_rnet (lnet_remotenet_t *rnet, lnet_route_t *route)
 
 	lnet_shuffle_seed();
 
-	list_for_each (e, &rnet->lrn_routes) {
+	list_for_each(e, &rnet->lrn_routes) {
 		len++;
 	}
 
 	/* len+1 positions to add a new entry, also prevents division by 0 */
 	offset = cfs_rand() % (len + 1);
-	list_for_each (e, &rnet->lrn_routes) {
+	list_for_each(e, &rnet->lrn_routes) {
 		if (offset == 0)
 			break;
 		offset--;
@@ -354,17 +354,16 @@ lnet_add_route(__u32 net, unsigned int hops, lnet_nid_t gateway,
 		LIBCFS_FREE(route, sizeof(*route));
 		LIBCFS_FREE(rnet, sizeof(*rnet));
 
-		if (rc == -EHOSTUNREACH) { /* gateway is not on a local net */
+		if (rc == -EHOSTUNREACH) /* gateway is not on a local net */
 			return 0;	/* ignore the route entry */
-		} else {
-			CERROR("Error %d creating route %s %d %s\n", rc,
-			       libcfs_net2str(net), hops,
-			       libcfs_nid2str(gateway));
-		}
+		CERROR("Error %d creating route %s %d %s\n", rc,
+		       libcfs_net2str(net), hops,
+		       libcfs_nid2str(gateway));
+
 		return rc;
 	}
 
-	LASSERT (!the_lnet.ln_shutdown);
+	LASSERT(!the_lnet.ln_shutdown);
 
 	rnet2 = lnet_find_net_locked(net);
 	if (rnet2 == NULL) {
@@ -375,7 +374,7 @@ lnet_add_route(__u32 net, unsigned int hops, lnet_nid_t gateway,
 
 	/* Search for a duplicate route (it's a NOOP if it is) */
 	add_route = 1;
-	list_for_each (e, &rnet2->lrn_routes) {
+	list_for_each(e, &rnet2->lrn_routes) {
 		lnet_route_t *route2 = list_entry(e, lnet_route_t, lr_list);
 
 		if (route2->lr_gateway == route->lr_gateway) {
@@ -384,7 +383,7 @@ lnet_add_route(__u32 net, unsigned int hops, lnet_nid_t gateway,
 		}
 
 		/* our lookups must be true */
-		LASSERT (route2->lr_gateway->lp_nid != gateway);
+		LASSERT(route2->lr_gateway->lp_nid != gateway);
 	}
 
 	if (add_route) {
@@ -546,7 +545,7 @@ lnet_del_route(__u32 net, lnet_nid_t gw_nid)
 }
 
 void
-lnet_destroy_routes (void)
+lnet_destroy_routes(void)
 {
 	lnet_del_route(LNET_NIDNET(LNET_NID_ANY), LNET_NID_ANY);
 }
@@ -606,7 +605,6 @@ lnet_swap_pinginfo(lnet_ping_info_t *info)
 		__swab64s(&stat->ns_nid);
 		__swab32s(&stat->ns_status);
 	}
-	return;
 }
 
 /**
@@ -758,13 +756,13 @@ lnet_wait_known_routerstate(void)
 	struct list_head	  *entry;
 	int		  all_known;
 
-	LASSERT (the_lnet.ln_rc_state == LNET_RC_STATE_RUNNING);
+	LASSERT(the_lnet.ln_rc_state == LNET_RC_STATE_RUNNING);
 
 	for (;;) {
 		int	cpt = lnet_net_lock_current();
 
 		all_known = 1;
-		list_for_each (entry, &the_lnet.ln_routers) {
+		list_for_each(entry, &the_lnet.ln_routers) {
 			rtr = list_entry(entry, lnet_peer_t, lp_rtr_list);
 
 			if (rtr->lp_alive_count == 0) {
@@ -886,7 +884,7 @@ lnet_create_rc_data_locked(lnet_peer_t *gateway)
 	}
 	rcd->rcd_pinginfo = pi;
 
-	LASSERT (!LNetHandleIsInvalid(the_lnet.ln_rc_eqh));
+	LASSERT(!LNetHandleIsInvalid(the_lnet.ln_rc_eqh));
 	rc = LNetMDBind((lnet_md_t){.start     = pi,
 				    .user_ptr  = rcd,
 				    .length    = LNET_PINGINFO_SIZE,
@@ -929,7 +927,7 @@ lnet_create_rc_data_locked(lnet_peer_t *gateway)
 }
 
 static int
-lnet_router_check_interval (lnet_peer_t *rtr)
+lnet_router_check_interval(lnet_peer_t *rtr)
 {
 	int secs;
 
@@ -942,7 +940,7 @@ lnet_router_check_interval (lnet_peer_t *rtr)
 }
 
 static void
-lnet_ping_router_locked (lnet_peer_t *rtr)
+lnet_ping_router_locked(lnet_peer_t *rtr)
 {
 	lnet_rc_data_t *rcd = NULL;
 	unsigned long      now = cfs_time_current();
@@ -1010,7 +1008,6 @@ lnet_ping_router_locked (lnet_peer_t *rtr)
 	}
 
 	lnet_peer_decref_locked(rtr);
-	return;
 }
 
 int
@@ -1019,7 +1016,7 @@ lnet_router_checker_start(void)
 	int	  rc;
 	int	  eqsz;
 
-	LASSERT (the_lnet.ln_rc_state == LNET_RC_STATE_SHUTDOWN);
+	LASSERT(the_lnet.ln_rc_state == LNET_RC_STATE_SHUTDOWN);
 
 	if (check_routers_before_use &&
 	    dead_router_check_interval <= 0) {
@@ -1067,14 +1064,14 @@ lnet_router_checker_start(void)
 }
 
 void
-lnet_router_checker_stop (void)
+lnet_router_checker_stop(void)
 {
 	int rc;
 
 	if (the_lnet.ln_rc_state == LNET_RC_STATE_SHUTDOWN)
 		return;
 
-	LASSERT (the_lnet.ln_rc_state == LNET_RC_STATE_RUNNING);
+	LASSERT(the_lnet.ln_rc_state == LNET_RC_STATE_RUNNING);
 	the_lnet.ln_rc_state = LNET_RC_STATE_STOPPING;
 
 	/* block until event callback signals exit */
@@ -1082,8 +1079,7 @@ lnet_router_checker_stop (void)
 	LASSERT(the_lnet.ln_rc_state == LNET_RC_STATE_SHUTDOWN);
 
 	rc = LNetEQFree(the_lnet.ln_rc_eqh);
-	LASSERT (rc == 0);
-	return;
+	LASSERT(rc == 0);
 }
 
 static void
@@ -1178,7 +1174,7 @@ lnet_router_checker(void *arg)
 
 	cfs_block_allsigs();
 
-	LASSERT (the_lnet.ln_rc_state == LNET_RC_STATE_RUNNING);
+	LASSERT(the_lnet.ln_rc_state == LNET_RC_STATE_RUNNING);
 
 	while (the_lnet.ln_rc_state == LNET_RC_STATE_RUNNING) {
 		__u64	version;
@@ -1291,11 +1287,11 @@ lnet_rtrpool_free_bufs(lnet_rtrbufpool_t *rbp)
 	if (rbp->rbp_nbuffers == 0) /* not initialized or already freed */
 		return;
 
-	LASSERT (list_empty(&rbp->rbp_msgs));
-	LASSERT (rbp->rbp_credits == rbp->rbp_nbuffers);
+	LASSERT(list_empty(&rbp->rbp_msgs));
+	LASSERT(rbp->rbp_credits == rbp->rbp_nbuffers);
 
 	while (!list_empty(&rbp->rbp_bufs)) {
-		LASSERT (rbp->rbp_credits > 0);
+		LASSERT(rbp->rbp_credits > 0);
 
 		rb = list_entry(rbp->rbp_bufs.next,
 				    lnet_rtrbuf_t, rb_list);
@@ -1304,8 +1300,8 @@ lnet_rtrpool_free_bufs(lnet_rtrbufpool_t *rbp)
 		nbuffers++;
 	}
 
-	LASSERT (rbp->rbp_nbuffers == nbuffers);
-	LASSERT (rbp->rbp_credits == nbuffers);
+	LASSERT(rbp->rbp_nbuffers == nbuffers);
+	LASSERT(rbp->rbp_credits == nbuffers);
 
 	rbp->rbp_nbuffers = rbp->rbp_credits = 0;
 }
@@ -1317,7 +1313,7 @@ lnet_rtrpool_alloc_bufs(lnet_rtrbufpool_t *rbp, int nbufs, int cpt)
 	int	    i;
 
 	if (rbp->rbp_nbuffers != 0) {
-		LASSERT (rbp->rbp_nbuffers == nbufs);
+		LASSERT(rbp->rbp_nbuffers == nbufs);
 		return 0;
 	}
 
@@ -1337,10 +1333,10 @@ lnet_rtrpool_alloc_bufs(lnet_rtrbufpool_t *rbp, int nbufs, int cpt)
 
 		/* No allocation "under fire" */
 		/* Otherwise we'd need code to schedule blocked msgs etc */
-		LASSERT (!the_lnet.ln_routing);
+		LASSERT(!the_lnet.ln_routing);
 	}
 
-	LASSERT (rbp->rbp_credits == nbufs);
+	LASSERT(rbp->rbp_credits == nbufs);
 	return 0;
 }
 
@@ -1435,7 +1431,7 @@ int
 lnet_rtrpools_alloc(int im_a_router)
 {
 	lnet_rtrbufpool_t *rtrp;
-	int	large_pages = (LNET_MTU + PAGE_CACHE_SIZE - 1) >> PAGE_CACHE_SHIFT;
+	int	large_pages;
 	int	small_pages = 1;
 	int	nrb_tiny;
 	int	nrb_small;
@@ -1443,6 +1439,8 @@ lnet_rtrpools_alloc(int im_a_router)
 	int	rc;
 	int	i;
 
+	large_pages = (LNET_MTU + PAGE_CACHE_SIZE - 1) >> PAGE_CACHE_SHIFT;
+
 	if (!strcmp(forwarding, "")) {
 		/* not set either way */
 		if (!im_a_router)
@@ -1513,16 +1511,16 @@ lnet_notify(lnet_ni_t *ni, lnet_nid_t nid, int alive, unsigned long when)
 	unsigned long		now = cfs_time_current();
 	int			cpt = lnet_cpt_of_nid(nid);
 
-	LASSERT (!in_interrupt ());
+	LASSERT(!in_interrupt ());
 
-	CDEBUG (D_NET, "%s notifying %s: %s\n",
+	CDEBUG(D_NET, "%s notifying %s: %s\n",
 		(ni == NULL) ? "userspace" : libcfs_nid2str(ni->ni_nid),
 		libcfs_nid2str(nid),
 		alive ? "up" : "down");
 
 	if (ni != NULL &&
 	    LNET_NIDNET(ni->ni_nid) != LNET_NIDNET(nid)) {
-		CWARN ("Ignoring notification of %s %s by %s (different net)\n",
+		CWARN("Ignoring notification of %s %s by %s (different net)\n",
 			libcfs_nid2str(nid), alive ? "birth" : "death",
 			libcfs_nid2str(ni->ni_nid));
 		return -EINVAL;
@@ -1577,21 +1575,20 @@ lnet_notify(lnet_ni_t *ni, lnet_nid_t nid, int alive, unsigned long when)
 EXPORT_SYMBOL(lnet_notify);
 
 void
-lnet_get_tunables (void)
+lnet_get_tunables(void)
 {
-	return;
 }
 
 #else
 
 int
-lnet_notify (lnet_ni_t *ni, lnet_nid_t nid, int alive, unsigned long when)
+lnet_notify(lnet_ni_t *ni, lnet_nid_t nid, int alive, unsigned long when)
 {
 	return -EOPNOTSUPP;
 }
 
 void
-lnet_router_checker (void)
+lnet_router_checker(void)
 {
 	static time_t last;
 	static int    running;
@@ -1643,7 +1640,7 @@ lnet_router_checker (void)
 			abort();
 		}
 
-		LASSERT (rc == 1);
+		LASSERT(rc == 1);
 
 		lnet_router_checker_event(&ev);
 	}
@@ -1655,26 +1652,25 @@ lnet_router_checker (void)
 		return;
 	}
 
-	LASSERT (the_lnet.ln_rc_state == LNET_RC_STATE_RUNNING);
+	LASSERT(the_lnet.ln_rc_state == LNET_RC_STATE_RUNNING);
 
 	lnet_net_lock(0);
 
 	version = the_lnet.ln_routers_version;
-	list_for_each_entry (rtr, &the_lnet.ln_routers, lp_rtr_list) {
+	list_for_each_entry(rtr, &the_lnet.ln_routers, lp_rtr_list) {
 		lnet_ping_router_locked(rtr);
-		LASSERT (version == the_lnet.ln_routers_version);
+		LASSERT(version == the_lnet.ln_routers_version);
 	}
 
 	lnet_net_unlock(0);
 
 	running = 0; /* lock only needed for the recursion check */
-	return;
 }
 
 /* NB lnet_peers_start_down depends on me,
  * so must be called before any peer creation */
 void
-lnet_get_tunables (void)
+lnet_get_tunables(void)
 {
 	char *s;
 
diff --git a/drivers/staging/lustre/lnet/selftest/brw_test.c b/drivers/staging/lustre/lnet/selftest/brw_test.c
index 463da076fa70..658f4584fff8 100644
--- a/drivers/staging/lustre/lnet/selftest/brw_test.c
+++ b/drivers/staging/lustre/lnet/selftest/brw_test.c
@@ -175,7 +175,6 @@ brw_fill_page(struct page *pg, int pattern, __u64 magic)
 	}
 
 	LBUG();
-	return;
 }
 
 static int
diff --git a/drivers/staging/lustre/lnet/selftest/conctl.c b/drivers/staging/lustre/lnet/selftest/conctl.c
index fbff84cea52f..045fe295ad54 100644
--- a/drivers/staging/lustre/lnet/selftest/conctl.c
+++ b/drivers/staging/lustre/lnet/selftest/conctl.c
@@ -203,7 +203,7 @@ lst_group_add_ioctl(lstio_group_add_args_t *args)
 	if (args->lstio_grp_key != console_session.ses_key)
 		return -EACCES;
 
-	if (args->lstio_grp_namep == NULL||
+	if (args->lstio_grp_namep == NULL ||
 	    args->lstio_grp_nmlen <= 0 ||
 	    args->lstio_grp_nmlen > LST_NAME_SIZE)
 		return -EINVAL;
@@ -857,62 +857,62 @@ lstcon_ioctl_entry(unsigned int cmd, struct libcfs_ioctl_data *data)
 	memset(&console_session.ses_trans_stat, 0, sizeof(lstcon_trans_stat_t));
 
 	switch (opc) {
-		case LSTIO_SESSION_NEW:
-			rc = lst_session_new_ioctl((lstio_session_new_args_t *)buf);
-			break;
-		case LSTIO_SESSION_END:
-			rc = lst_session_end_ioctl((lstio_session_end_args_t *)buf);
-			break;
-		case LSTIO_SESSION_INFO:
-			rc = lst_session_info_ioctl((lstio_session_info_args_t *)buf);
-			break;
-		case LSTIO_DEBUG:
-			rc = lst_debug_ioctl((lstio_debug_args_t *)buf);
-			break;
-		case LSTIO_GROUP_ADD:
-			rc = lst_group_add_ioctl((lstio_group_add_args_t *)buf);
-			break;
-		case LSTIO_GROUP_DEL:
-			rc = lst_group_del_ioctl((lstio_group_del_args_t *)buf);
-			break;
-		case LSTIO_GROUP_UPDATE:
-			rc = lst_group_update_ioctl((lstio_group_update_args_t *)buf);
-			break;
-		case LSTIO_NODES_ADD:
-			rc = lst_nodes_add_ioctl((lstio_group_nodes_args_t *)buf);
-			break;
-		case LSTIO_GROUP_LIST:
-			rc = lst_group_list_ioctl((lstio_group_list_args_t *)buf);
-			break;
-		case LSTIO_GROUP_INFO:
-			rc = lst_group_info_ioctl((lstio_group_info_args_t *)buf);
-			break;
-		case LSTIO_BATCH_ADD:
-			rc = lst_batch_add_ioctl((lstio_batch_add_args_t *)buf);
-			break;
-		case LSTIO_BATCH_START:
-			rc = lst_batch_run_ioctl((lstio_batch_run_args_t *)buf);
-			break;
-		case LSTIO_BATCH_STOP:
-			rc = lst_batch_stop_ioctl((lstio_batch_stop_args_t *)buf);
-			break;
-		case LSTIO_BATCH_QUERY:
-			rc = lst_batch_query_ioctl((lstio_batch_query_args_t *)buf);
-			break;
-		case LSTIO_BATCH_LIST:
-			rc = lst_batch_list_ioctl((lstio_batch_list_args_t *)buf);
-			break;
-		case LSTIO_BATCH_INFO:
-			rc = lst_batch_info_ioctl((lstio_batch_info_args_t *)buf);
-			break;
-		case LSTIO_TEST_ADD:
-			rc = lst_test_add_ioctl((lstio_test_args_t *)buf);
-			break;
-		case LSTIO_STAT_QUERY:
-			rc = lst_stat_query_ioctl((lstio_stat_args_t *)buf);
-			break;
-		default:
-			rc = -EINVAL;
+	case LSTIO_SESSION_NEW:
+		rc = lst_session_new_ioctl((lstio_session_new_args_t *)buf);
+		break;
+	case LSTIO_SESSION_END:
+		rc = lst_session_end_ioctl((lstio_session_end_args_t *)buf);
+		break;
+	case LSTIO_SESSION_INFO:
+		rc = lst_session_info_ioctl((lstio_session_info_args_t *)buf);
+		break;
+	case LSTIO_DEBUG:
+		rc = lst_debug_ioctl((lstio_debug_args_t *)buf);
+		break;
+	case LSTIO_GROUP_ADD:
+		rc = lst_group_add_ioctl((lstio_group_add_args_t *)buf);
+		break;
+	case LSTIO_GROUP_DEL:
+		rc = lst_group_del_ioctl((lstio_group_del_args_t *)buf);
+		break;
+	case LSTIO_GROUP_UPDATE:
+		rc = lst_group_update_ioctl((lstio_group_update_args_t *)buf);
+		break;
+	case LSTIO_NODES_ADD:
+		rc = lst_nodes_add_ioctl((lstio_group_nodes_args_t *)buf);
+		break;
+	case LSTIO_GROUP_LIST:
+		rc = lst_group_list_ioctl((lstio_group_list_args_t *)buf);
+		break;
+	case LSTIO_GROUP_INFO:
+		rc = lst_group_info_ioctl((lstio_group_info_args_t *)buf);
+		break;
+	case LSTIO_BATCH_ADD:
+		rc = lst_batch_add_ioctl((lstio_batch_add_args_t *)buf);
+		break;
+	case LSTIO_BATCH_START:
+		rc = lst_batch_run_ioctl((lstio_batch_run_args_t *)buf);
+		break;
+	case LSTIO_BATCH_STOP:
+		rc = lst_batch_stop_ioctl((lstio_batch_stop_args_t *)buf);
+		break;
+	case LSTIO_BATCH_QUERY:
+		rc = lst_batch_query_ioctl((lstio_batch_query_args_t *)buf);
+		break;
+	case LSTIO_BATCH_LIST:
+		rc = lst_batch_list_ioctl((lstio_batch_list_args_t *)buf);
+		break;
+	case LSTIO_BATCH_INFO:
+		rc = lst_batch_info_ioctl((lstio_batch_info_args_t *)buf);
+		break;
+	case LSTIO_TEST_ADD:
+		rc = lst_test_add_ioctl((lstio_test_args_t *)buf);
+		break;
+	case LSTIO_STAT_QUERY:
+		rc = lst_stat_query_ioctl((lstio_stat_args_t *)buf);
+		break;
+	default:
+		rc = -EINVAL;
 	}
 
 	if (copy_to_user(data->ioc_pbuf2, &console_session.ses_trans_stat,
diff --git a/drivers/staging/lustre/lnet/selftest/console.c b/drivers/staging/lustre/lnet/selftest/console.c
index 1e0afc286847..2b5f53c7a730 100644
--- a/drivers/staging/lustre/lnet/selftest/console.c
+++ b/drivers/staging/lustre/lnet/selftest/console.c
@@ -64,7 +64,7 @@ lstcon_session_t	console_session;
 static void
 lstcon_node_get(lstcon_node_t *nd)
 {
-	LASSERT (nd->nd_ref >= 1);
+	LASSERT(nd->nd_ref >= 1);
 
 	nd->nd_ref++;
 }
@@ -75,7 +75,7 @@ lstcon_node_find(lnet_process_id_t id, lstcon_node_t **ndpp, int create)
 	lstcon_ndlink_t *ndl;
 	unsigned int     idx = LNET_NIDADDR(id.nid) % LST_GLOBAL_HASHSIZE;
 
-	LASSERT (id.nid != LNET_NID_ANY);
+	LASSERT(id.nid != LNET_NID_ANY);
 
 	list_for_each_entry(ndl, &console_session.ses_ndl_hash[idx], ndl_hlink) {
 		if (ndl->ndl_node->nd_id.nid != id.nid ||
@@ -119,15 +119,15 @@ lstcon_node_put(lstcon_node_t *nd)
 {
 	lstcon_ndlink_t  *ndl;
 
-	LASSERT (nd->nd_ref > 0);
+	LASSERT(nd->nd_ref > 0);
 
 	if (--nd->nd_ref > 0)
 		return;
 
 	ndl = (lstcon_ndlink_t *)(nd + 1);
 
-	LASSERT (!list_empty(&ndl->ndl_link));
-	LASSERT (!list_empty(&ndl->ndl_hlink));
+	LASSERT(!list_empty(&ndl->ndl_link));
+	LASSERT(!list_empty(&ndl->ndl_hlink));
 
 	/* remove from session */
 	list_del(&ndl->ndl_link);
@@ -184,8 +184,8 @@ lstcon_ndlink_find(struct list_head *hash,
 static void
 lstcon_ndlink_release(lstcon_ndlink_t *ndl)
 {
-	LASSERT (list_empty(&ndl->ndl_link));
-	LASSERT (!list_empty(&ndl->ndl_hlink));
+	LASSERT(list_empty(&ndl->ndl_link));
+	LASSERT(!list_empty(&ndl->ndl_hlink));
 
 	list_del(&ndl->ndl_hlink); /* delete from hash */
 	lstcon_node_put(ndl->ndl_node);
@@ -254,7 +254,7 @@ lstcon_group_decref(lstcon_group_t *grp)
 	lstcon_group_drain(grp, 0);
 
 	for (i = 0; i < LST_NODE_HASHSIZE; i++) {
-		LASSERT (list_empty(&grp->grp_ndl_hash[i]));
+		LASSERT(list_empty(&grp->grp_ndl_hash[i]));
 	}
 
 	LIBCFS_FREE(grp, offsetof(lstcon_group_t,
@@ -552,8 +552,8 @@ lstcon_nodes_add(char *name, int count, lnet_process_id_t *ids_up,
 	lstcon_group_t	 *grp;
 	int		     rc;
 
-	LASSERT (count > 0);
-	LASSERT (ids_up != NULL);
+	LASSERT(count > 0);
+	LASSERT(ids_up != NULL);
 
 	rc = lstcon_group_find(name, &grp);
 	if (rc != 0) {
@@ -726,8 +726,8 @@ lstcon_group_list(int index, int len, char *name_up)
 {
 	lstcon_group_t *grp;
 
-	LASSERT (index >= 0);
-	LASSERT (name_up != NULL);
+	LASSERT(index >= 0);
+	LASSERT(name_up != NULL);
 
 	list_for_each_entry(grp, &console_session.ses_grp_list, grp_link) {
 		if (index-- == 0) {
@@ -748,10 +748,10 @@ lstcon_nodes_getent(struct list_head *head, int *index_p,
 	int	       count = 0;
 	int	       index = 0;
 
-	LASSERT (index_p != NULL && count_p != NULL);
-	LASSERT (dents_up != NULL);
-	LASSERT (*index_p >= 0);
-	LASSERT (*count_p > 0);
+	LASSERT(index_p != NULL && count_p != NULL);
+	LASSERT(dents_up != NULL);
+	LASSERT(*index_p >= 0);
+	LASSERT(*count_p > 0);
 
 	list_for_each_entry(ndl, head, ndl_link) {
 		if (index++ < *index_p)
@@ -905,8 +905,8 @@ lstcon_batch_list(int index, int len, char *name_up)
 {
 	lstcon_batch_t    *bat;
 
-	LASSERT (name_up != NULL);
-	LASSERT (index >= 0);
+	LASSERT(name_up != NULL);
+	LASSERT(index >= 0);
 
 	list_for_each_entry(bat, &console_session.ses_bat_list, bat_link) {
 		if (index-- == 0) {
@@ -1093,7 +1093,7 @@ lstcon_batch_destroy(lstcon_batch_t *bat)
 	while (!list_empty(&bat->bat_test_list)) {
 		test = list_entry(bat->bat_test_list.next,
 				      lstcon_test_t, tes_link);
-		LASSERT (list_empty(&test->tes_trans_list));
+		LASSERT(list_empty(&test->tes_trans_list));
 
 		list_del(&test->tes_link);
 
@@ -1104,7 +1104,7 @@ lstcon_batch_destroy(lstcon_batch_t *bat)
 					   tes_param[test->tes_paramlen]));
 	}
 
-	LASSERT (list_empty(&bat->bat_trans_list));
+	LASSERT(list_empty(&bat->bat_trans_list));
 
 	while (!list_empty(&bat->bat_cli_list)) {
 		ndl = list_entry(bat->bat_cli_list.next,
@@ -1123,8 +1123,8 @@ lstcon_batch_destroy(lstcon_batch_t *bat)
 	}
 
 	for (i = 0; i < LST_NODE_HASHSIZE; i++) {
-		LASSERT (list_empty(&bat->bat_cli_hash[i]));
-		LASSERT (list_empty(&bat->bat_srv_hash[i]));
+		LASSERT(list_empty(&bat->bat_cli_hash[i]));
+		LASSERT(list_empty(&bat->bat_srv_hash[i]));
 	}
 
 	LIBCFS_FREE(bat->bat_cli_hash,
@@ -1144,10 +1144,10 @@ lstcon_testrpc_condition(int transop, lstcon_node_t *nd, void *arg)
 	struct list_head       *head;
 
 	test = (lstcon_test_t *)arg;
-	LASSERT (test != NULL);
+	LASSERT(test != NULL);
 
 	batch = test->tes_batch;
-	LASSERT (batch != NULL);
+	LASSERT(batch != NULL);
 
 	if (test->tes_oneside &&
 	    transop == LST_TRANS_TSBSRVADD)
@@ -1161,13 +1161,13 @@ lstcon_testrpc_condition(int transop, lstcon_node_t *nd, void *arg)
 		head = &batch->bat_cli_list;
 
 	} else {
-		LASSERT (transop == LST_TRANS_TSBSRVADD);
+		LASSERT(transop == LST_TRANS_TSBSRVADD);
 
 		hash = batch->bat_srv_hash;
 		head = &batch->bat_srv_list;
 	}
 
-	LASSERT (nd->nd_id.nid != LNET_NID_ANY);
+	LASSERT(nd->nd_id.nid != LNET_NID_ANY);
 
 	if (lstcon_ndlink_find(hash, nd->nd_id, &ndl, 1) != 0)
 		return -ENOMEM;
@@ -1186,8 +1186,8 @@ lstcon_test_nodes_add(lstcon_test_t *test, struct list_head *result_up)
 	int		     transop;
 	int		     rc;
 
-	LASSERT (test->tes_src_grp != NULL);
-	LASSERT (test->tes_dst_grp != NULL);
+	LASSERT(test->tes_src_grp != NULL);
+	LASSERT(test->tes_dst_grp != NULL);
 
 	transop = LST_TRANS_TSBSRVADD;
 	grp  = test->tes_dst_grp;
@@ -1382,7 +1382,7 @@ lstcon_tsbrpc_readent(int transop, srpc_msg_t *msg,
 {
 	srpc_batch_reply_t *rep = &msg->msg_body.bat_reply;
 
-	LASSERT (transop == LST_TRANS_TSBCLIQRY ||
+	LASSERT(transop == LST_TRANS_TSBCLIQRY ||
 		 transop == LST_TRANS_TSBSRVQRY);
 
 	/* positive errno, framework error code */
@@ -1691,7 +1691,7 @@ lstcon_new_session_id(lst_sid_t *sid)
 {
 	lnet_process_id_t      id;
 
-	LASSERT (console_session.ses_state == LST_SESSION_NONE);
+	LASSERT(console_session.ses_state == LST_SESSION_NONE);
 
 	LNetGetId(1, &id);
 	sid->ses_nid   = id.nid;
@@ -1806,7 +1806,7 @@ lstcon_session_end(void)
 	lstcon_batch_t     *bat;
 	int		 rc = 0;
 
-	LASSERT (console_session.ses_state == LST_SESSION_ACTIVE);
+	LASSERT(console_session.ses_state == LST_SESSION_ACTIVE);
 
 	rc = lstcon_rpc_trans_ndlist(&console_session.ses_ndl_list,
 				     NULL, LST_TRANS_SESEND, NULL,
@@ -1846,13 +1846,13 @@ lstcon_session_end(void)
 	while (!list_empty(&console_session.ses_grp_list)) {
 		grp = list_entry(console_session.ses_grp_list.next,
 				     lstcon_group_t, grp_link);
-		LASSERT (grp->grp_ref == 1);
+		LASSERT(grp->grp_ref == 1);
 
 		lstcon_group_put(grp);
 	}
 
 	/* all nodes should be released */
-	LASSERT (list_empty(&console_session.ses_ndl_list));
+	LASSERT(list_empty(&console_session.ses_ndl_list));
 
 	console_session.ses_shutdown = 0;
 	console_session.ses_expired  = 0;
@@ -1892,7 +1892,7 @@ lstcon_session_feats_check(unsigned feats)
 }
 
 static int
-lstcon_acceptor_handle (srpc_server_rpc_t *rpc)
+lstcon_acceptor_handle(srpc_server_rpc_t *rpc)
 {
 	srpc_msg_t	*rep  = &rpc->srpc_replymsg;
 	srpc_msg_t	*req  = &rpc->srpc_reqstbuf->buf_msg;
@@ -2026,7 +2026,7 @@ lstcon_console_init(void)
 	lstcon_init_acceptor_service();
 
 	rc = srpc_add_service(&lstcon_acceptor_service);
-	LASSERT (rc != -EBUSY);
+	LASSERT(rc != -EBUSY);
 	if (rc != 0) {
 		LIBCFS_FREE(console_session.ses_ndl_hash,
 			    sizeof(struct list_head) * LST_GLOBAL_HASHSIZE);
@@ -2078,13 +2078,13 @@ lstcon_console_fini(void)
 
 	mutex_unlock(&console_session.ses_mutex);
 
-	LASSERT (list_empty(&console_session.ses_ndl_list));
-	LASSERT (list_empty(&console_session.ses_grp_list));
-	LASSERT (list_empty(&console_session.ses_bat_list));
-	LASSERT (list_empty(&console_session.ses_trans_list));
+	LASSERT(list_empty(&console_session.ses_ndl_list));
+	LASSERT(list_empty(&console_session.ses_grp_list));
+	LASSERT(list_empty(&console_session.ses_bat_list));
+	LASSERT(list_empty(&console_session.ses_trans_list));
 
 	for (i = 0; i < LST_NODE_HASHSIZE; i++) {
-		LASSERT (list_empty(&console_session.ses_ndl_hash[i]));
+		LASSERT(list_empty(&console_session.ses_ndl_hash[i]));
 	}
 
 	LIBCFS_FREE(console_session.ses_ndl_hash,
diff --git a/drivers/staging/lustre/lnet/selftest/console.h b/drivers/staging/lustre/lnet/selftest/console.h
index f960174ceff8..e41ca89f10ba 100644
--- a/drivers/staging/lustre/lnet/selftest/console.h
+++ b/drivers/staging/lustre/lnet/selftest/console.h
@@ -182,6 +182,9 @@ lstcon_id2hash (lnet_process_id_t id, struct list_head *hash)
 	return &hash[idx];
 }
 
+int lstcon_console_init(void);
+int lstcon_ioctl_entry(unsigned int cmd, struct libcfs_ioctl_data *data);
+int lstcon_console_fini(void);
 extern int lstcon_session_match(lst_sid_t sid);
 extern int lstcon_session_new(char *name, int key, unsigned version,
 			      int timeout, int flags, lst_sid_t *sid_up);
diff --git a/drivers/staging/lustre/lnet/selftest/framework.c b/drivers/staging/lustre/lnet/selftest/framework.c
index 570914828b8c..a93a90de0f85 100644
--- a/drivers/staging/lustre/lnet/selftest/framework.c
+++ b/drivers/staging/lustre/lnet/selftest/framework.c
@@ -115,18 +115,18 @@ static struct smoketest_framework {
 } sfw_data;
 
 /* forward ref's */
-int sfw_stop_batch (sfw_batch_t *tsb, int force);
-void sfw_destroy_session (sfw_session_t *sn);
+int sfw_stop_batch(sfw_batch_t *tsb, int force);
+void sfw_destroy_session(sfw_session_t *sn);
 
 static inline sfw_test_case_t *
 sfw_find_test_case(int id)
 {
 	sfw_test_case_t *tsc;
 
-	LASSERT (id <= SRPC_SERVICE_MAX_ID);
-	LASSERT (id > SRPC_FRAMEWORK_SERVICE_MAX_ID);
+	LASSERT(id <= SRPC_SERVICE_MAX_ID);
+	LASSERT(id > SRPC_FRAMEWORK_SERVICE_MAX_ID);
 
-	list_for_each_entry (tsc, &sfw_data.fw_tests, tsc_list) {
+	list_for_each_entry(tsc, &sfw_data.fw_tests, tsc_list) {
 		if (tsc->tsc_srv_service->sv_id == id)
 			return tsc;
 	}
@@ -135,12 +135,12 @@ sfw_find_test_case(int id)
 }
 
 static int
-sfw_register_test (srpc_service_t *service, sfw_test_client_ops_t *cliops)
+sfw_register_test(srpc_service_t *service, sfw_test_client_ops_t *cliops)
 {
 	sfw_test_case_t *tsc;
 
 	if (sfw_find_test_case(service->sv_id) != NULL) {
-		CERROR ("Failed to register test %s (%d)\n",
+		CERROR("Failed to register test %s (%d)\n",
 			service->sv_name, service->sv_id);
 		return -EEXIST;
 	}
@@ -157,17 +157,17 @@ sfw_register_test (srpc_service_t *service, sfw_test_client_ops_t *cliops)
 }
 
 static void
-sfw_add_session_timer (void)
+sfw_add_session_timer(void)
 {
 	sfw_session_t *sn = sfw_data.fw_session;
 	stt_timer_t   *timer = &sn->sn_timer;
 
-	LASSERT (!sfw_data.fw_shuttingdown);
+	LASSERT(!sfw_data.fw_shuttingdown);
 
 	if (sn == NULL || sn->sn_timeout == 0)
 		return;
 
-	LASSERT (!sn->sn_timer_active);
+	LASSERT(!sn->sn_timer_active);
 
 	sn->sn_timer_active = 1;
 	timer->stt_expires = cfs_time_add(sn->sn_timeout,
@@ -177,14 +177,14 @@ sfw_add_session_timer (void)
 }
 
 static int
-sfw_del_session_timer (void)
+sfw_del_session_timer(void)
 {
 	sfw_session_t *sn = sfw_data.fw_session;
 
 	if (sn == NULL || !sn->sn_timer_active)
 		return 0;
 
-	LASSERT (sn->sn_timeout != 0);
+	LASSERT(sn->sn_timeout != 0);
 
 	if (stt_del_timer(&sn->sn_timer)) { /* timer defused */
 		sn->sn_timer_active = 0;
@@ -195,7 +195,7 @@ sfw_del_session_timer (void)
 }
 
 static void
-sfw_deactivate_session (void)
+sfw_deactivate_session(void)
 	__must_hold(&sfw_data.fw_lock)
 {
 	sfw_session_t *sn = sfw_data.fw_session;
@@ -205,7 +205,7 @@ sfw_deactivate_session (void)
 
 	if (sn == NULL) return;
 
-	LASSERT (!sn->sn_timer_active);
+	LASSERT(!sn->sn_timer_active);
 
 	sfw_data.fw_session = NULL;
 	atomic_inc(&sfw_data.fw_nzombies);
@@ -219,7 +219,7 @@ sfw_deactivate_session (void)
 
 	spin_lock(&sfw_data.fw_lock);
 
-	list_for_each_entry (tsb, &sn->sn_batches, bat_list) {
+	list_for_each_entry(tsb, &sn->sn_batches, bat_list) {
 		if (sfw_batch_active(tsb)) {
 			nactive++;
 			sfw_stop_batch(tsb, 1);
@@ -239,16 +239,16 @@ sfw_deactivate_session (void)
 
 
 static void
-sfw_session_expired (void *data)
+sfw_session_expired(void *data)
 {
 	sfw_session_t *sn = data;
 
 	spin_lock(&sfw_data.fw_lock);
 
-	LASSERT (sn->sn_timer_active);
-	LASSERT (sn == sfw_data.fw_session);
+	LASSERT(sn->sn_timer_active);
+	LASSERT(sn == sfw_data.fw_session);
 
-	CWARN ("Session expired! sid: %s-%llu, name: %s\n",
+	CWARN("Session expired! sid: %s-%llu, name: %s\n",
 	       libcfs_nid2str(sn->sn_id.ses_nid),
 	       sn->sn_id.ses_stamp, &sn->sn_name[0]);
 
@@ -290,7 +290,7 @@ sfw_server_rpc_done(struct srpc_server_rpc *rpc)
 	struct srpc_service	*sv	= rpc->srpc_scd->scd_svc;
 	int			status	= rpc->srpc_status;
 
-	CDEBUG (D_NET,
+	CDEBUG(D_NET,
 		"Incoming framework RPC done: service %s, peer %s, status %s:%d\n",
 		sv->sv_name, libcfs_id2str(rpc->srpc_peer),
 		swi_state2str(rpc->srpc_wi.swi_state),
@@ -302,13 +302,13 @@ sfw_server_rpc_done(struct srpc_server_rpc *rpc)
 }
 
 static void
-sfw_client_rpc_fini (srpc_client_rpc_t *rpc)
+sfw_client_rpc_fini(srpc_client_rpc_t *rpc)
 {
-	LASSERT (rpc->crpc_bulk.bk_niov == 0);
-	LASSERT (list_empty(&rpc->crpc_list));
-	LASSERT (atomic_read(&rpc->crpc_refcount) == 0);
+	LASSERT(rpc->crpc_bulk.bk_niov == 0);
+	LASSERT(list_empty(&rpc->crpc_list));
+	LASSERT(atomic_read(&rpc->crpc_refcount) == 0);
 
-	CDEBUG (D_NET,
+	CDEBUG(D_NET,
 		"Outgoing framework RPC done: service %d, peer %s, status %s:%d:%d\n",
 		rpc->crpc_service, libcfs_id2str(rpc->crpc_dest),
 		swi_state2str(rpc->crpc_wi.swi_state),
@@ -324,14 +324,14 @@ sfw_client_rpc_fini (srpc_client_rpc_t *rpc)
 }
 
 static sfw_batch_t *
-sfw_find_batch (lst_bid_t bid)
+sfw_find_batch(lst_bid_t bid)
 {
 	sfw_session_t *sn = sfw_data.fw_session;
 	sfw_batch_t   *bat;
 
-	LASSERT (sn != NULL);
+	LASSERT(sn != NULL);
 
-	list_for_each_entry (bat, &sn->sn_batches, bat_list) {
+	list_for_each_entry(bat, &sn->sn_batches, bat_list) {
 		if (bat->bat_id.bat_id == bid.bat_id)
 			return bat;
 	}
@@ -340,12 +340,12 @@ sfw_find_batch (lst_bid_t bid)
 }
 
 static sfw_batch_t *
-sfw_bid2batch (lst_bid_t bid)
+sfw_bid2batch(lst_bid_t bid)
 {
 	sfw_session_t *sn = sfw_data.fw_session;
 	sfw_batch_t   *bat;
 
-	LASSERT (sn != NULL);
+	LASSERT(sn != NULL);
 
 	bat = sfw_find_batch(bid);
 	if (bat != NULL)
@@ -366,7 +366,7 @@ sfw_bid2batch (lst_bid_t bid)
 }
 
 static int
-sfw_get_stats (srpc_stat_reqst_t *request, srpc_stat_reply_t *reply)
+sfw_get_stats(srpc_stat_reqst_t *request, srpc_stat_reply_t *reply)
 {
 	sfw_session_t  *sn = sfw_data.fw_session;
 	sfw_counters_t *cnt = &reply->str_fw;
@@ -399,7 +399,7 @@ sfw_get_stats (srpc_stat_reqst_t *request, srpc_stat_reply_t *reply)
 	cnt->zombie_sessions = atomic_read(&sfw_data.fw_nzombies);
 
 	cnt->active_batches = 0;
-	list_for_each_entry (bat, &sn->sn_batches, bat_list) {
+	list_for_each_entry(bat, &sn->sn_batches, bat_list) {
 		if (atomic_read(&bat->bat_nactive) > 0)
 			cnt->active_batches++;
 	}
@@ -456,7 +456,7 @@ sfw_make_session(srpc_mksn_reqst_t *request, srpc_mksn_reply_t *reply)
 	/* brand new or create by force */
 	LIBCFS_ALLOC(sn, sizeof(sfw_session_t));
 	if (sn == NULL) {
-		CERROR ("Dropping RPC (mksn) under memory pressure.\n");
+		CERROR("Dropping RPC (mksn) under memory pressure.\n");
 		return -ENOMEM;
 	}
 
@@ -478,7 +478,7 @@ sfw_make_session(srpc_mksn_reqst_t *request, srpc_mksn_reply_t *reply)
 }
 
 static int
-sfw_remove_session (srpc_rmsn_reqst_t *request, srpc_rmsn_reply_t *reply)
+sfw_remove_session(srpc_rmsn_reqst_t *request, srpc_rmsn_reply_t *reply)
 {
 	sfw_session_t *sn = sfw_data.fw_session;
 
@@ -510,7 +510,7 @@ sfw_remove_session (srpc_rmsn_reqst_t *request, srpc_rmsn_reply_t *reply)
 }
 
 static int
-sfw_debug_session (srpc_debug_reqst_t *request, srpc_debug_reply_t *reply)
+sfw_debug_session(srpc_debug_reqst_t *request, srpc_debug_reply_t *reply)
 {
 	sfw_session_t *sn = sfw_data.fw_session;
 
@@ -531,13 +531,13 @@ sfw_debug_session (srpc_debug_reqst_t *request, srpc_debug_reply_t *reply)
 }
 
 static void
-sfw_test_rpc_fini (srpc_client_rpc_t *rpc)
+sfw_test_rpc_fini(srpc_client_rpc_t *rpc)
 {
 	sfw_test_unit_t     *tsu = rpc->crpc_priv;
 	sfw_test_instance_t *tsi = tsu->tsu_instance;
 
 	/* Called with hold of tsi->tsi_lock */
-	LASSERT (list_empty(&rpc->crpc_list));
+	LASSERT(list_empty(&rpc->crpc_list));
 	list_add(&rpc->crpc_list, &tsi->tsi_free_rpcs);
 }
 
@@ -608,7 +608,7 @@ sfw_unload_test(struct sfw_test_instance *tsi)
 }
 
 static void
-sfw_destroy_test_instance (sfw_test_instance_t *tsi)
+sfw_destroy_test_instance(sfw_test_instance_t *tsi)
 {
 	srpc_client_rpc_t *rpc;
 	sfw_test_unit_t   *tsu;
@@ -617,9 +617,9 @@ sfw_destroy_test_instance (sfw_test_instance_t *tsi)
 
 	tsi->tsi_ops->tso_fini(tsi);
 
-	LASSERT (!tsi->tsi_stopping);
-	LASSERT (list_empty(&tsi->tsi_active_rpcs));
-	LASSERT (!sfw_test_active(tsi));
+	LASSERT(!tsi->tsi_stopping);
+	LASSERT(list_empty(&tsi->tsi_active_rpcs));
+	LASSERT(!sfw_test_active(tsi));
 
 	while (!list_empty(&tsi->tsi_units)) {
 		tsu = list_entry(tsi->tsi_units.next,
@@ -642,12 +642,12 @@ clean:
 }
 
 static void
-sfw_destroy_batch (sfw_batch_t *tsb)
+sfw_destroy_batch(sfw_batch_t *tsb)
 {
 	sfw_test_instance_t *tsi;
 
-	LASSERT (!sfw_batch_active(tsb));
-	LASSERT (list_empty(&tsb->bat_list));
+	LASSERT(!sfw_batch_active(tsb));
+	LASSERT(list_empty(&tsb->bat_list));
 
 	while (!list_empty(&tsb->bat_tests)) {
 		tsi = list_entry(tsb->bat_tests.next,
@@ -661,12 +661,12 @@ sfw_destroy_batch (sfw_batch_t *tsb)
 }
 
 void
-sfw_destroy_session (sfw_session_t *sn)
+sfw_destroy_session(sfw_session_t *sn)
 {
 	sfw_batch_t *batch;
 
-	LASSERT (list_empty(&sn->sn_list));
-	LASSERT (sn != sfw_data.fw_session);
+	LASSERT(list_empty(&sn->sn_list));
+	LASSERT(sn != sfw_data.fw_session);
 
 	while (!list_empty(&sn->sn_batches)) {
 		batch = list_entry(sn->sn_batches.next,
@@ -685,13 +685,13 @@ sfw_unpack_addtest_req(srpc_msg_t *msg)
 {
 	srpc_test_reqst_t *req = &msg->msg_body.tes_reqst;
 
-	LASSERT (msg->msg_type == SRPC_MSG_TEST_REQST);
-	LASSERT (req->tsr_is_client);
+	LASSERT(msg->msg_type == SRPC_MSG_TEST_REQST);
+	LASSERT(req->tsr_is_client);
 
 	if (msg->msg_magic == SRPC_MSG_MAGIC)
 		return; /* no flipping needed */
 
-	LASSERT (msg->msg_magic == __swab32(SRPC_MSG_MAGIC));
+	LASSERT(msg->msg_magic == __swab32(SRPC_MSG_MAGIC));
 
 	if (req->tsr_service == SRPC_SERVICE_BRW) {
 		if ((msg->msg_ses_feats & LST_FEAT_BULK_LEN) == 0) {
@@ -721,12 +721,12 @@ sfw_unpack_addtest_req(srpc_msg_t *msg)
 		return;
 	}
 
-	LBUG ();
+	LBUG();
 	return;
 }
 
 static int
-sfw_add_test_instance (sfw_batch_t *tsb, srpc_server_rpc_t *rpc)
+sfw_add_test_instance(sfw_batch_t *tsb, srpc_server_rpc_t *rpc)
 {
 	srpc_msg_t	  *msg = &rpc->srpc_reqstbuf->buf_msg;
 	srpc_test_reqst_t   *req = &msg->msg_body.tes_reqst;
@@ -739,7 +739,7 @@ sfw_add_test_instance (sfw_batch_t *tsb, srpc_server_rpc_t *rpc)
 
 	LIBCFS_ALLOC(tsi, sizeof(*tsi));
 	if (tsi == NULL) {
-		CERROR ("Can't allocate test instance for batch: %llu\n",
+		CERROR("Can't allocate test instance for batch: %llu\n",
 			tsb->bat_id.bat_id);
 		return -ENOMEM;
 	}
@@ -764,7 +764,7 @@ sfw_add_test_instance (sfw_batch_t *tsb, srpc_server_rpc_t *rpc)
 		return rc;
 	}
 
-	LASSERT (!sfw_batch_active(tsb));
+	LASSERT(!sfw_batch_active(tsb));
 
 	if (!tsi->tsi_is_client) {
 		/* it's test server, just add it to tsb */
@@ -772,8 +772,8 @@ sfw_add_test_instance (sfw_batch_t *tsb, srpc_server_rpc_t *rpc)
 		return 0;
 	}
 
-	LASSERT (bk != NULL);
-	LASSERT (bk->bk_niov * SFW_ID_PER_PAGE >= (unsigned int)ndest);
+	LASSERT(bk != NULL);
+	LASSERT(bk->bk_niov * SFW_ID_PER_PAGE >= (unsigned int)ndest);
 	LASSERT((unsigned int)bk->bk_len >=
 		sizeof(lnet_process_id_packed_t) * ndest);
 
@@ -786,7 +786,7 @@ sfw_add_test_instance (sfw_batch_t *tsb, srpc_server_rpc_t *rpc)
 		int		       j;
 
 		dests = page_address(bk->bk_iovs[i / SFW_ID_PER_PAGE].kiov_page);
-		LASSERT (dests != NULL);  /* my pages are within KVM always */
+		LASSERT(dests != NULL);  /* my pages are within KVM always */
 		id = dests[i % SFW_ID_PER_PAGE];
 		if (msg->msg_magic != SRPC_MSG_MAGIC)
 			sfw_unpack_id(id);
@@ -795,7 +795,7 @@ sfw_add_test_instance (sfw_batch_t *tsb, srpc_server_rpc_t *rpc)
 			LIBCFS_ALLOC(tsu, sizeof(sfw_test_unit_t));
 			if (tsu == NULL) {
 				rc = -ENOMEM;
-				CERROR ("Can't allocate tsu for %d\n",
+				CERROR("Can't allocate tsu for %d\n",
 					tsi->tsi_service);
 				goto error;
 			}
@@ -815,19 +815,19 @@ sfw_add_test_instance (sfw_batch_t *tsb, srpc_server_rpc_t *rpc)
 	}
 
 error:
-	LASSERT (rc != 0);
+	LASSERT(rc != 0);
 	sfw_destroy_test_instance(tsi);
 	return rc;
 }
 
 static void
-sfw_test_unit_done (sfw_test_unit_t *tsu)
+sfw_test_unit_done(sfw_test_unit_t *tsu)
 {
 	sfw_test_instance_t *tsi = tsu->tsu_instance;
 	sfw_batch_t	 *tsb = tsi->tsi_batch;
 	sfw_session_t       *sn = tsb->bat_session;
 
-	LASSERT (sfw_test_active(tsi));
+	LASSERT(sfw_test_active(tsi));
 
 	if (!atomic_dec_and_test(&tsi->tsi_nactive))
 		return;
@@ -847,9 +847,9 @@ sfw_test_unit_done (sfw_test_unit_t *tsu)
 		return;
 	}
 
-	LASSERT (!list_empty(&sn->sn_list)); /* I'm a zombie! */
+	LASSERT(!list_empty(&sn->sn_list)); /* I'm a zombie! */
 
-	list_for_each_entry (tsb, &sn->sn_batches, bat_list) {
+	list_for_each_entry(tsb, &sn->sn_batches, bat_list) {
 		if (sfw_batch_active(tsb)) {
 			spin_unlock(&sfw_data.fw_lock);
 			return;
@@ -864,7 +864,7 @@ sfw_test_unit_done (sfw_test_unit_t *tsu)
 }
 
 static void
-sfw_test_rpc_done (srpc_client_rpc_t *rpc)
+sfw_test_rpc_done(srpc_client_rpc_t *rpc)
 {
 	sfw_test_unit_t     *tsu = rpc->crpc_priv;
 	sfw_test_instance_t *tsi = tsu->tsu_instance;
@@ -874,8 +874,8 @@ sfw_test_rpc_done (srpc_client_rpc_t *rpc)
 
 	spin_lock(&tsi->tsi_lock);
 
-	LASSERT (sfw_test_active(tsi));
-	LASSERT (!list_empty(&rpc->crpc_list));
+	LASSERT(sfw_test_active(tsi));
+	LASSERT(!list_empty(&rpc->crpc_list));
 
 	list_del_init(&rpc->crpc_list);
 
@@ -909,13 +909,13 @@ sfw_create_test_rpc(sfw_test_unit_t *tsu, lnet_process_id_t peer,
 
 	spin_lock(&tsi->tsi_lock);
 
-	LASSERT (sfw_test_active(tsi));
+	LASSERT(sfw_test_active(tsi));
 
 	if (!list_empty(&tsi->tsi_free_rpcs)) {
 		/* pick request from buffer */
 		rpc = list_entry(tsi->tsi_free_rpcs.next,
 				     srpc_client_rpc_t, crpc_list);
-		LASSERT (nblk == rpc->crpc_bulk.bk_niov);
+		LASSERT(nblk == rpc->crpc_bulk.bk_niov);
 		list_del_init(&rpc->crpc_list);
 	}
 
@@ -943,20 +943,20 @@ sfw_create_test_rpc(sfw_test_unit_t *tsu, lnet_process_id_t peer,
 }
 
 static int
-sfw_run_test (swi_workitem_t *wi)
+sfw_run_test(swi_workitem_t *wi)
 {
 	sfw_test_unit_t     *tsu = wi->swi_workitem.wi_data;
 	sfw_test_instance_t *tsi = tsu->tsu_instance;
 	srpc_client_rpc_t   *rpc = NULL;
 
-	LASSERT (wi == &tsu->tsu_worker);
+	LASSERT(wi == &tsu->tsu_worker);
 
 	if (tsi->tsi_ops->tso_prep_rpc(tsu, tsu->tsu_dest, &rpc) != 0) {
-		LASSERT (rpc == NULL);
+		LASSERT(rpc == NULL);
 		goto test_done;
 	}
 
-	LASSERT (rpc != NULL);
+	LASSERT(rpc != NULL);
 
 	spin_lock(&tsi->tsi_lock);
 
@@ -993,7 +993,7 @@ test_done:
 }
 
 static int
-sfw_run_batch (sfw_batch_t *tsb)
+sfw_run_batch(sfw_batch_t *tsb)
 {
 	swi_workitem_t      *wi;
 	sfw_test_unit_t     *tsu;
@@ -1005,16 +1005,16 @@ sfw_run_batch (sfw_batch_t *tsb)
 		return 0;
 	}
 
-	list_for_each_entry (tsi, &tsb->bat_tests, tsi_list) {
+	list_for_each_entry(tsi, &tsb->bat_tests, tsi_list) {
 		if (!tsi->tsi_is_client) /* skip server instances */
 			continue;
 
-		LASSERT (!tsi->tsi_stopping);
-		LASSERT (!sfw_test_active(tsi));
+		LASSERT(!tsi->tsi_stopping);
+		LASSERT(!sfw_test_active(tsi));
 
 		atomic_inc(&tsb->bat_nactive);
 
-		list_for_each_entry (tsu, &tsi->tsi_units, tsu_list) {
+		list_for_each_entry(tsu, &tsi->tsi_units, tsu_list) {
 			atomic_inc(&tsi->tsi_nactive);
 			tsu->tsu_loop = tsi->tsi_loop;
 			wi = &tsu->tsu_worker;
@@ -1029,7 +1029,7 @@ sfw_run_batch (sfw_batch_t *tsb)
 }
 
 int
-sfw_stop_batch (sfw_batch_t *tsb, int force)
+sfw_stop_batch(sfw_batch_t *tsb, int force)
 {
 	sfw_test_instance_t *tsi;
 	srpc_client_rpc_t   *rpc;
@@ -1039,7 +1039,7 @@ sfw_stop_batch (sfw_batch_t *tsb, int force)
 		return 0;
 	}
 
-	list_for_each_entry (tsi, &tsb->bat_tests, tsi_list) {
+	list_for_each_entry(tsi, &tsb->bat_tests, tsi_list) {
 		spin_lock(&tsi->tsi_lock);
 
 		if (!tsi->tsi_is_client ||
@@ -1071,7 +1071,7 @@ sfw_stop_batch (sfw_batch_t *tsb, int force)
 }
 
 static int
-sfw_query_batch (sfw_batch_t *tsb, int testidx, srpc_batch_reply_t *reply)
+sfw_query_batch(sfw_batch_t *tsb, int testidx, srpc_batch_reply_t *reply)
 {
 	sfw_test_instance_t *tsi;
 
@@ -1083,7 +1083,7 @@ sfw_query_batch (sfw_batch_t *tsb, int testidx, srpc_batch_reply_t *reply)
 		return 0;
 	}
 
-	list_for_each_entry (tsi, &tsb->bat_tests, tsi_list) {
+	list_for_each_entry(tsi, &tsb->bat_tests, tsi_list) {
 		if (testidx-- > 1)
 			continue;
 
@@ -1095,7 +1095,7 @@ sfw_query_batch (sfw_batch_t *tsb, int testidx, srpc_batch_reply_t *reply)
 }
 
 void
-sfw_free_pages (srpc_server_rpc_t *rpc)
+sfw_free_pages(srpc_server_rpc_t *rpc)
 {
 	srpc_free_bulk(rpc->srpc_bulk);
 	rpc->srpc_bulk = NULL;
@@ -1116,7 +1116,7 @@ sfw_alloc_pages(struct srpc_server_rpc *rpc, int cpt, int npages, int len,
 }
 
 static int
-sfw_add_test (srpc_server_rpc_t *rpc)
+sfw_add_test(srpc_server_rpc_t *rpc)
 {
 	sfw_session_t     *sn = sfw_data.fw_session;
 	srpc_test_reply_t *reply = &rpc->srpc_replymsg.msg_body.tes_reply;
@@ -1147,7 +1147,7 @@ sfw_add_test (srpc_server_rpc_t *rpc)
 
 	bat = sfw_bid2batch(request->tsr_bid);
 	if (bat == NULL) {
-		CERROR ("Dropping RPC (%s) from %s under memory pressure.\n",
+		CERROR("Dropping RPC (%s) from %s under memory pressure.\n",
 			rpc->srpc_scd->scd_svc->sv_name,
 			libcfs_id2str(rpc->srpc_peer));
 		return -ENOMEM;
@@ -1175,7 +1175,7 @@ sfw_add_test (srpc_server_rpc_t *rpc)
 	}
 
 	rc = sfw_add_test_instance(bat, rpc);
-	CDEBUG (rc == 0 ? D_NET : D_WARNING,
+	CDEBUG(rc == 0 ? D_NET : D_WARNING,
 		"%s test: sv %d %s, loop %d, concur %d, ndest %d\n",
 		rc == 0 ? "Added" : "Failed to add", request->tsr_service,
 		request->tsr_is_client ? "client" : "server",
@@ -1186,7 +1186,7 @@ sfw_add_test (srpc_server_rpc_t *rpc)
 }
 
 static int
-sfw_control_batch (srpc_batch_reqst_t *request, srpc_batch_reply_t *reply)
+sfw_control_batch(srpc_batch_reqst_t *request, srpc_batch_reply_t *reply)
 {
 	sfw_session_t *sn = sfw_data.fw_session;
 	int	    rc = 0;
@@ -1285,7 +1285,7 @@ sfw_handle_server_rpc(struct srpc_server_rpc *rpc)
 
 	switch (sv->sv_id) {
 	default:
-		LBUG ();
+		LBUG();
 	case SRPC_SERVICE_TEST:
 		rc = sfw_add_test(rpc);
 		break;
@@ -1387,8 +1387,8 @@ sfw_create_rpc(lnet_process_id_t peer, int service,
 
 	spin_lock(&sfw_data.fw_lock);
 
-	LASSERT (!sfw_data.fw_shuttingdown);
-	LASSERT (service <= SRPC_FRAMEWORK_SERVICE_MAX_ID);
+	LASSERT(!sfw_data.fw_shuttingdown);
+	LASSERT(service <= SRPC_FRAMEWORK_SERVICE_MAX_ID);
 
 	if (nbulkiov == 0 && !list_empty(&sfw_data.fw_zombie_rpcs)) {
 		rpc = list_entry(sfw_data.fw_zombie_rpcs.next,
@@ -1416,13 +1416,13 @@ sfw_create_rpc(lnet_process_id_t peer, int service,
 }
 
 void
-sfw_unpack_message (srpc_msg_t *msg)
+sfw_unpack_message(srpc_msg_t *msg)
 {
 	if (msg->msg_magic == SRPC_MSG_MAGIC)
 		return; /* no flipping needed */
 
 	/* srpc module should guarantee I wouldn't get crap */
-	LASSERT (msg->msg_magic == __swab32(SRPC_MSG_MAGIC));
+	LASSERT(msg->msg_magic == __swab32(SRPC_MSG_MAGIC));
 
 	if (msg->msg_type == SRPC_MSG_STAT_REQST) {
 		srpc_stat_reqst_t *req = &msg->msg_body.stat_reqst;
@@ -1555,12 +1555,12 @@ sfw_unpack_message (srpc_msg_t *msg)
 		return;
 	}
 
-	LBUG ();
+	LBUG();
 	return;
 }
 
 void
-sfw_abort_rpc (srpc_client_rpc_t *rpc)
+sfw_abort_rpc(srpc_client_rpc_t *rpc)
 {
 	LASSERT(atomic_read(&rpc->crpc_refcount) > 0);
 	LASSERT(rpc->crpc_service <= SRPC_FRAMEWORK_SERVICE_MAX_ID);
@@ -1572,14 +1572,14 @@ sfw_abort_rpc (srpc_client_rpc_t *rpc)
 }
 
 void
-sfw_post_rpc (srpc_client_rpc_t *rpc)
+sfw_post_rpc(srpc_client_rpc_t *rpc)
 {
 	spin_lock(&rpc->crpc_lock);
 
-	LASSERT (!rpc->crpc_closed);
-	LASSERT (!rpc->crpc_aborted);
-	LASSERT (list_empty(&rpc->crpc_list));
-	LASSERT (!sfw_data.fw_shuttingdown);
+	LASSERT(!rpc->crpc_closed);
+	LASSERT(!rpc->crpc_aborted);
+	LASSERT(list_empty(&rpc->crpc_list));
+	LASSERT(!sfw_data.fw_shuttingdown);
 
 	rpc->crpc_timeout = rpc_timeout;
 	srpc_post_rpc(rpc);
@@ -1638,7 +1638,7 @@ extern void brw_init_test_service(void);
 
 
 int
-sfw_startup (void)
+sfw_startup(void)
 {
 	int	      i;
 	int	      rc;
@@ -1648,13 +1648,13 @@ sfw_startup (void)
 
 
 	if (session_timeout < 0) {
-		CERROR ("Session timeout must be non-negative: %d\n",
+		CERROR("Session timeout must be non-negative: %d\n",
 			session_timeout);
 		return -EINVAL;
 	}
 
 	if (rpc_timeout < 0) {
-		CERROR ("RPC timeout must be non-negative: %d\n",
+		CERROR("RPC timeout must be non-negative: %d\n",
 			rpc_timeout);
 		return -EINVAL;
 	}
@@ -1678,21 +1678,21 @@ sfw_startup (void)
 	brw_init_test_client();
 	brw_init_test_service();
 	rc = sfw_register_test(&brw_test_service, &brw_test_client);
-	LASSERT (rc == 0);
+	LASSERT(rc == 0);
 
 	ping_init_test_client();
 	ping_init_test_service();
 	rc = sfw_register_test(&ping_test_service, &ping_test_client);
-	LASSERT (rc == 0);
+	LASSERT(rc == 0);
 
 	error = 0;
-	list_for_each_entry (tsc, &sfw_data.fw_tests, tsc_list) {
+	list_for_each_entry(tsc, &sfw_data.fw_tests, tsc_list) {
 		sv = tsc->tsc_srv_service;
 
 		rc = srpc_add_service(sv);
-		LASSERT (rc != -EBUSY);
+		LASSERT(rc != -EBUSY);
 		if (rc != 0) {
-			CWARN ("Failed to add %s service: %d\n",
+			CWARN("Failed to add %s service: %d\n",
 			       sv->sv_name, rc);
 			error = rc;
 		}
@@ -1709,9 +1709,9 @@ sfw_startup (void)
 			sv->sv_bulk_ready = sfw_bulk_ready;
 
 		rc = srpc_add_service(sv);
-		LASSERT (rc != -EBUSY);
+		LASSERT(rc != -EBUSY);
 		if (rc != 0) {
-			CWARN ("Failed to add %s service: %d\n",
+			CWARN("Failed to add %s service: %d\n",
 			       sv->sv_name, rc);
 			error = rc;
 		}
@@ -1733,7 +1733,7 @@ sfw_startup (void)
 }
 
 void
-sfw_shutdown (void)
+sfw_shutdown(void)
 {
 	srpc_service_t	*sv;
 	sfw_test_case_t	*tsc;
@@ -1766,7 +1766,7 @@ sfw_shutdown (void)
 		srpc_remove_service(sv);
 	}
 
-	list_for_each_entry (tsc, &sfw_data.fw_tests, tsc_list) {
+	list_for_each_entry(tsc, &sfw_data.fw_tests, tsc_list) {
 		sv = tsc->tsc_srv_service;
 		srpc_shutdown_service(sv);
 		srpc_remove_service(sv);
diff --git a/drivers/staging/lustre/lnet/selftest/module.c b/drivers/staging/lustre/lnet/selftest/module.c
index faf409802372..7ad62f167cea 100644
--- a/drivers/staging/lustre/lnet/selftest/module.c
+++ b/drivers/staging/lustre/lnet/selftest/module.c
@@ -87,7 +87,6 @@ lnet_selftest_fini(void)
 	default:
 		LBUG();
 	}
-	return;
 }
 
 static int
diff --git a/drivers/staging/lustre/lnet/selftest/ping_test.c b/drivers/staging/lustre/lnet/selftest/ping_test.c
index d8c0df6e6852..644069a9fe4e 100644
--- a/drivers/staging/lustre/lnet/selftest/ping_test.c
+++ b/drivers/staging/lustre/lnet/selftest/ping_test.c
@@ -70,19 +70,19 @@ ping_client_init(sfw_test_instance_t *tsi)
 }
 
 static void
-ping_client_fini (sfw_test_instance_t *tsi)
+ping_client_fini(sfw_test_instance_t *tsi)
 {
 	sfw_session_t *sn = tsi->tsi_batch->bat_session;
 	int	    errors;
 
-	LASSERT (sn != NULL);
-	LASSERT (tsi->tsi_is_client);
+	LASSERT(sn != NULL);
+	LASSERT(tsi->tsi_is_client);
 
 	errors = atomic_read(&sn->sn_ping_errors);
 	if (errors)
-		CWARN ("%d pings have failed.\n", errors);
+		CWARN("%d pings have failed.\n", errors);
 	else
-		CDEBUG (D_NET, "Ping test finished OK.\n");
+		CDEBUG(D_NET, "Ping test finished OK.\n");
 }
 
 static int
@@ -118,7 +118,7 @@ ping_client_prep_rpc(sfw_test_unit_t *tsu,
 }
 
 static void
-ping_client_done_rpc (sfw_test_unit_t *tsu, srpc_client_rpc_t *rpc)
+ping_client_done_rpc(sfw_test_unit_t *tsu, srpc_client_rpc_t *rpc)
 {
 	sfw_test_instance_t *tsi = tsu->tsu_instance;
 	sfw_session_t       *sn = tsi->tsi_batch->bat_session;
@@ -126,12 +126,12 @@ ping_client_done_rpc (sfw_test_unit_t *tsu, srpc_client_rpc_t *rpc)
 	srpc_ping_reply_t   *reply = &rpc->crpc_replymsg.msg_body.ping_reply;
 	struct timeval       tv;
 
-	LASSERT (sn != NULL);
+	LASSERT(sn != NULL);
 
 	if (rpc->crpc_status != 0) {
 		if (!tsi->tsi_stopping) /* rpc could have been aborted */
 			atomic_inc(&sn->sn_ping_errors);
-		CERROR ("Unable to ping %s (%d): %d\n",
+		CERROR("Unable to ping %s (%d): %d\n",
 			libcfs_id2str(rpc->crpc_dest),
 			reqst->pnr_seq, rpc->crpc_status);
 		return;
@@ -146,7 +146,7 @@ ping_client_done_rpc (sfw_test_unit_t *tsu, srpc_client_rpc_t *rpc)
 	if (reply->pnr_magic != LST_PING_TEST_MAGIC) {
 		rpc->crpc_status = -EBADMSG;
 		atomic_inc(&sn->sn_ping_errors);
-		CERROR ("Bad magic %u from %s, %u expected.\n",
+		CERROR("Bad magic %u from %s, %u expected.\n",
 			reply->pnr_magic, libcfs_id2str(rpc->crpc_dest),
 			LST_PING_TEST_MAGIC);
 		return;
@@ -155,14 +155,14 @@ ping_client_done_rpc (sfw_test_unit_t *tsu, srpc_client_rpc_t *rpc)
 	if (reply->pnr_seq != reqst->pnr_seq) {
 		rpc->crpc_status = -EBADMSG;
 		atomic_inc(&sn->sn_ping_errors);
-		CERROR ("Bad seq %u from %s, %u expected.\n",
+		CERROR("Bad seq %u from %s, %u expected.\n",
 			reply->pnr_seq, libcfs_id2str(rpc->crpc_dest),
 			reqst->pnr_seq);
 		return;
 	}
 
 	cfs_fs_timeval(&tv);
-	CDEBUG (D_NET, "%d reply in %u usec\n", reply->pnr_seq,
+	CDEBUG(D_NET, "%d reply in %u usec\n", reply->pnr_seq,
 		(unsigned)((tv.tv_sec - (unsigned)reqst->pnr_time_sec) * 1000000
 			   + (tv.tv_usec - reqst->pnr_time_usec)));
 	return;
@@ -177,20 +177,20 @@ ping_server_handle(struct srpc_server_rpc *rpc)
 	srpc_ping_reqst_t *req = &reqstmsg->msg_body.ping_reqst;
 	srpc_ping_reply_t *rep = &rpc->srpc_replymsg.msg_body.ping_reply;
 
-	LASSERT (sv->sv_id == SRPC_SERVICE_PING);
+	LASSERT(sv->sv_id == SRPC_SERVICE_PING);
 
 	if (reqstmsg->msg_magic != SRPC_MSG_MAGIC) {
-		LASSERT (reqstmsg->msg_magic == __swab32(SRPC_MSG_MAGIC));
+		LASSERT(reqstmsg->msg_magic == __swab32(SRPC_MSG_MAGIC));
 
 		__swab32s(&req->pnr_seq);
 		__swab32s(&req->pnr_magic);
 		__swab64s(&req->pnr_time_sec);
 		__swab64s(&req->pnr_time_usec);
 	}
-	LASSERT (reqstmsg->msg_type == srpc_service2request(sv->sv_id));
+	LASSERT(reqstmsg->msg_type == srpc_service2request(sv->sv_id));
 
 	if (req->pnr_magic != LST_PING_TEST_MAGIC) {
-		CERROR ("Unexpected magic %08x from %s\n",
+		CERROR("Unexpected magic %08x from %s\n",
 			req->pnr_magic, libcfs_id2str(rpc->srpc_peer));
 		return -EINVAL;
 	}
diff --git a/drivers/staging/lustre/lnet/selftest/rpc.c b/drivers/staging/lustre/lnet/selftest/rpc.c
index 1f7d9a6248db..080788ab749e 100644
--- a/drivers/staging/lustre/lnet/selftest/rpc.c
+++ b/drivers/staging/lustre/lnet/selftest/rpc.c
@@ -71,16 +71,16 @@ srpc_serv_portal(int svc_id)
 }
 
 /* forward ref's */
-int srpc_handle_rpc (swi_workitem_t *wi);
+int srpc_handle_rpc(swi_workitem_t *wi);
 
-void srpc_get_counters (srpc_counters_t *cnt)
+void srpc_get_counters(srpc_counters_t *cnt)
 {
 	spin_lock(&srpc_data.rpc_glock);
 	*cnt = srpc_data.rpc_counters;
 	spin_unlock(&srpc_data.rpc_glock);
 }
 
-void srpc_set_counters (const srpc_counters_t *cnt)
+void srpc_set_counters(const srpc_counters_t *cnt)
 {
 	spin_lock(&srpc_data.rpc_glock);
 	srpc_data.rpc_counters = *cnt;
@@ -102,16 +102,17 @@ srpc_add_bulk_page(srpc_bulk_t *bk, struct page *pg, int i, int nob)
 }
 
 void
-srpc_free_bulk (srpc_bulk_t *bk)
+srpc_free_bulk(srpc_bulk_t *bk)
 {
 	int	 i;
 	struct page *pg;
 
-	LASSERT (bk != NULL);
+	LASSERT(bk != NULL);
 
 	for (i = 0; i < bk->bk_niov; i++) {
 		pg = bk->bk_iovs[i].kiov_page;
-		if (pg == NULL) break;
+		if (pg == NULL)
+			break;
 
 		__free_page(pg);
 	}
@@ -160,7 +161,7 @@ srpc_alloc_bulk(int cpt, unsigned bulk_npg, unsigned bulk_len, int sink)
 }
 
 static inline __u64
-srpc_next_id (void)
+srpc_next_id(void)
 {
 	__u64 id;
 
@@ -335,7 +336,7 @@ srpc_add_service(struct srpc_service *sv)
 }
 
 int
-srpc_remove_service (srpc_service_t *sv)
+srpc_remove_service(srpc_service_t *sv)
 {
 	int id = sv->sv_id;
 
@@ -363,8 +364,8 @@ srpc_post_passive_rdma(int portal, int local, __u64 matchbits, void *buf,
 	rc = LNetMEAttach(portal, peer, matchbits, 0, LNET_UNLINK,
 			  local ? LNET_INS_LOCAL : LNET_INS_AFTER, &meh);
 	if (rc != 0) {
-		CERROR ("LNetMEAttach failed: %d\n", rc);
-		LASSERT (rc == -ENOMEM);
+		CERROR("LNetMEAttach failed: %d\n", rc);
+		LASSERT(rc == -ENOMEM);
 		return -ENOMEM;
 	}
 
@@ -377,15 +378,15 @@ srpc_post_passive_rdma(int portal, int local, __u64 matchbits, void *buf,
 
 	rc = LNetMDAttach(meh, md, LNET_UNLINK, mdh);
 	if (rc != 0) {
-		CERROR ("LNetMDAttach failed: %d\n", rc);
-		LASSERT (rc == -ENOMEM);
+		CERROR("LNetMDAttach failed: %d\n", rc);
+		LASSERT(rc == -ENOMEM);
 
 		rc = LNetMEUnlink(meh);
-		LASSERT (rc == 0);
+		LASSERT(rc == 0);
 		return -ENOMEM;
 	}
 
-	CDEBUG (D_NET,
+	CDEBUG(D_NET,
 		"Posted passive RDMA: peer %s, portal %d, matchbits %#llx\n",
 		libcfs_id2str(peer), portal, matchbits);
 	return 0;
@@ -408,8 +409,8 @@ srpc_post_active_rdma(int portal, __u64 matchbits, void *buf, int len,
 
 	rc = LNetMDBind(md, LNET_UNLINK, mdh);
 	if (rc != 0) {
-		CERROR ("LNetMDBind failed: %d\n", rc);
-		LASSERT (rc == -ENOMEM);
+		CERROR("LNetMDBind failed: %d\n", rc);
+		LASSERT(rc == -ENOMEM);
 		return -ENOMEM;
 	}
 
@@ -420,13 +421,13 @@ srpc_post_active_rdma(int portal, __u64 matchbits, void *buf, int len,
 		rc = LNetPut(self, *mdh, LNET_NOACK_REQ, peer,
 			     portal, matchbits, 0, 0);
 	} else {
-		LASSERT ((options & LNET_MD_OP_GET) != 0);
+		LASSERT((options & LNET_MD_OP_GET) != 0);
 
 		rc = LNetGet(self, *mdh, peer, portal, matchbits, 0);
 	}
 
 	if (rc != 0) {
-		CERROR ("LNet%s(%s, %d, %lld) failed: %d\n",
+		CERROR("LNet%s(%s, %d, %lld) failed: %d\n",
 			((options & LNET_MD_OP_PUT) != 0) ? "Put" : "Get",
 			libcfs_id2str(peer), portal, matchbits, rc);
 
@@ -434,9 +435,9 @@ srpc_post_active_rdma(int portal, __u64 matchbits, void *buf, int len,
 		 * with failure, so fall through and return success here.
 		 */
 		rc = LNetMDUnlink(*mdh);
-		LASSERT (rc == 0);
+		LASSERT(rc == 0);
 	} else {
-		CDEBUG (D_NET,
+		CDEBUG(D_NET,
 			"Posted active RDMA: peer %s, portal %u, matchbits %#llx\n",
 			libcfs_id2str(peer), portal, matchbits);
 	}
@@ -788,7 +789,7 @@ srpc_shutdown_service(srpc_service_t *sv)
 }
 
 static int
-srpc_send_request (srpc_client_rpc_t *rpc)
+srpc_send_request(srpc_client_rpc_t *rpc)
 {
 	srpc_event_t *ev = &rpc->crpc_reqstev;
 	int	   rc;
@@ -801,14 +802,14 @@ srpc_send_request (srpc_client_rpc_t *rpc)
 				     &rpc->crpc_reqstmsg, sizeof(srpc_msg_t),
 				     &rpc->crpc_reqstmdh, ev);
 	if (rc != 0) {
-		LASSERT (rc == -ENOMEM);
+		LASSERT(rc == -ENOMEM);
 		ev->ev_fired = 1;  /* no more event expected */
 	}
 	return rc;
 }
 
 static int
-srpc_prepare_reply (srpc_client_rpc_t *rpc)
+srpc_prepare_reply(srpc_client_rpc_t *rpc)
 {
 	srpc_event_t *ev = &rpc->crpc_replyev;
 	__u64	*id = &rpc->crpc_reqstmsg.msg_body.reqst.rpyid;
@@ -825,14 +826,14 @@ srpc_prepare_reply (srpc_client_rpc_t *rpc)
 				    LNET_MD_OP_PUT, rpc->crpc_dest,
 				    &rpc->crpc_replymdh, ev);
 	if (rc != 0) {
-		LASSERT (rc == -ENOMEM);
+		LASSERT(rc == -ENOMEM);
 		ev->ev_fired = 1;  /* no more event expected */
 	}
 	return rc;
 }
 
 static int
-srpc_prepare_bulk (srpc_client_rpc_t *rpc)
+srpc_prepare_bulk(srpc_client_rpc_t *rpc)
 {
 	srpc_bulk_t  *bk = &rpc->crpc_bulk;
 	srpc_event_t *ev = &rpc->crpc_bulkev;
@@ -840,9 +841,10 @@ srpc_prepare_bulk (srpc_client_rpc_t *rpc)
 	int	   rc;
 	int	   opt;
 
-	LASSERT (bk->bk_niov <= LNET_MAX_IOV);
+	LASSERT(bk->bk_niov <= LNET_MAX_IOV);
 
-	if (bk->bk_niov == 0) return 0; /* nothing to do */
+	if (bk->bk_niov == 0)
+		return 0; /* nothing to do */
 
 	opt = bk->bk_sink ? LNET_MD_OP_PUT : LNET_MD_OP_GET;
 	opt |= LNET_MD_KIOV;
@@ -857,14 +859,14 @@ srpc_prepare_bulk (srpc_client_rpc_t *rpc)
 				    &bk->bk_iovs[0], bk->bk_niov, opt,
 				    rpc->crpc_dest, &bk->bk_mdh, ev);
 	if (rc != 0) {
-		LASSERT (rc == -ENOMEM);
+		LASSERT(rc == -ENOMEM);
 		ev->ev_fired = 1;  /* no more event expected */
 	}
 	return rc;
 }
 
 static int
-srpc_do_bulk (srpc_server_rpc_t *rpc)
+srpc_do_bulk(srpc_server_rpc_t *rpc)
 {
 	srpc_event_t  *ev = &rpc->srpc_ev;
 	srpc_bulk_t   *bk = rpc->srpc_bulk;
@@ -872,7 +874,7 @@ srpc_do_bulk (srpc_server_rpc_t *rpc)
 	int	    rc;
 	int	    opt;
 
-	LASSERT (bk != NULL);
+	LASSERT(bk != NULL);
 
 	opt = bk->bk_sink ? LNET_MD_OP_GET : LNET_MD_OP_PUT;
 	opt |= LNET_MD_KIOV;
@@ -898,11 +900,11 @@ srpc_server_rpc_done(srpc_server_rpc_t *rpc, int status)
 	struct srpc_service	*sv  = scd->scd_svc;
 	srpc_buffer_t		*buffer;
 
-	LASSERT (status != 0 || rpc->srpc_wi.swi_state == SWI_STATE_DONE);
+	LASSERT(status != 0 || rpc->srpc_wi.swi_state == SWI_STATE_DONE);
 
 	rpc->srpc_status = status;
 
-	CDEBUG_LIMIT (status == 0 ? D_NET : D_NETERROR,
+	CDEBUG_LIMIT(status == 0 ? D_NET : D_NETERROR,
 		"Server RPC %p done: service %s, peer %s, status %s:%d\n",
 		rpc, sv->sv_name, libcfs_id2str(rpc->srpc_peer),
 		swi_state2str(rpc->srpc_wi.swi_state), status);
@@ -985,7 +987,7 @@ srpc_handle_rpc(swi_workitem_t *wi)
 
 	switch (wi->swi_state) {
 	default:
-		LBUG ();
+		LBUG();
 	case SWI_STATE_NEWBORN: {
 		srpc_msg_t	   *msg;
 		srpc_generic_reply_t *reply;
@@ -1023,12 +1025,12 @@ srpc_handle_rpc(swi_workitem_t *wi)
 			if (rc == 0)
 				return 0; /* wait for bulk */
 
-			LASSERT (ev->ev_fired);
+			LASSERT(ev->ev_fired);
 			ev->ev_status = rc;
 		}
 	}
 	case SWI_STATE_BULK_STARTED:
-		LASSERT (rpc->srpc_bulk == NULL || ev->ev_fired);
+		LASSERT(rpc->srpc_bulk == NULL || ev->ev_fired);
 
 		if (rpc->srpc_bulk != NULL) {
 			rc = ev->ev_status;
@@ -1055,7 +1057,7 @@ srpc_handle_rpc(swi_workitem_t *wi)
 			       rpc, rpc->srpc_bulk, sv->sv_id);
 			CERROR("Event: status %d, type %d, lnet %d\n",
 			       ev->ev_status, ev->ev_type, ev->ev_lnet);
-			LASSERT (ev->ev_fired);
+			LASSERT(ev->ev_fired);
 		}
 
 		wi->swi_state = SWI_STATE_DONE;
@@ -1067,11 +1069,11 @@ srpc_handle_rpc(swi_workitem_t *wi)
 }
 
 static void
-srpc_client_rpc_expired (void *data)
+srpc_client_rpc_expired(void *data)
 {
 	srpc_client_rpc_t *rpc = data;
 
-	CWARN ("Client RPC expired: service %d, peer %s, timeout %d.\n",
+	CWARN("Client RPC expired: service %d, peer %s, timeout %d.\n",
 	       rpc->crpc_service, libcfs_id2str(rpc->crpc_dest),
 	       rpc->crpc_timeout);
 
@@ -1088,11 +1090,12 @@ srpc_client_rpc_expired (void *data)
 }
 
 inline void
-srpc_add_client_rpc_timer (srpc_client_rpc_t *rpc)
+srpc_add_client_rpc_timer(srpc_client_rpc_t *rpc)
 {
 	stt_timer_t *timer = &rpc->crpc_timer;
 
-	if (rpc->crpc_timeout == 0) return;
+	if (rpc->crpc_timeout == 0)
+		return;
 
 	INIT_LIST_HEAD(&timer->stt_list);
 	timer->stt_data    = rpc;
@@ -1109,7 +1112,7 @@ srpc_add_client_rpc_timer (srpc_client_rpc_t *rpc)
  * Upon exit the RPC expiry timer is not queued and the handler is not
  * running on any CPU. */
 static void
-srpc_del_client_rpc_timer (srpc_client_rpc_t *rpc)
+srpc_del_client_rpc_timer(srpc_client_rpc_t *rpc)
 {
 	/* timer not planted or already exploded */
 	if (rpc->crpc_timeout == 0)
@@ -1130,7 +1133,7 @@ srpc_del_client_rpc_timer (srpc_client_rpc_t *rpc)
 }
 
 static void
-srpc_client_rpc_done (srpc_client_rpc_t *rpc, int status)
+srpc_client_rpc_done(srpc_client_rpc_t *rpc, int status)
 {
 	swi_workitem_t *wi = &rpc->crpc_wi;
 
@@ -1144,7 +1147,7 @@ srpc_client_rpc_done (srpc_client_rpc_t *rpc, int status)
 
 	srpc_del_client_rpc_timer(rpc);
 
-	CDEBUG_LIMIT ((status == 0) ? D_NET : D_NETERROR,
+	CDEBUG_LIMIT((status == 0) ? D_NET : D_NETERROR,
 		"Client RPC done: service %d, peer %s, status %s:%d:%d\n",
 		rpc->crpc_service, libcfs_id2str(rpc->crpc_dest),
 		swi_state2str(wi->swi_state), rpc->crpc_aborted, status);
@@ -1157,7 +1160,7 @@ srpc_client_rpc_done (srpc_client_rpc_t *rpc, int status)
 	 *   scheduling me.
 	 * Cancel pending schedules and prevent future schedule attempts:
 	 */
-	LASSERT (!srpc_event_pending(rpc));
+	LASSERT(!srpc_event_pending(rpc));
 	swi_exit_workitem(wi);
 
 	spin_unlock(&rpc->crpc_lock);
@@ -1168,7 +1171,7 @@ srpc_client_rpc_done (srpc_client_rpc_t *rpc, int status)
 
 /* sends an outgoing RPC */
 int
-srpc_send_rpc (swi_workitem_t *wi)
+srpc_send_rpc(swi_workitem_t *wi)
 {
 	int		rc = 0;
 	srpc_client_rpc_t *rpc;
@@ -1179,8 +1182,8 @@ srpc_send_rpc (swi_workitem_t *wi)
 
 	rpc = wi->swi_workitem.wi_data;
 
-	LASSERT (rpc != NULL);
-	LASSERT (wi == &rpc->crpc_wi);
+	LASSERT(rpc != NULL);
+	LASSERT(wi == &rpc->crpc_wi);
 
 	reply = &rpc->crpc_replymsg;
 	do_bulk = rpc->crpc_bulk.bk_niov > 0;
@@ -1196,9 +1199,9 @@ srpc_send_rpc (swi_workitem_t *wi)
 
 	switch (wi->swi_state) {
 	default:
-		LBUG ();
+		LBUG();
 	case SWI_STATE_NEWBORN:
-		LASSERT (!srpc_event_pending(rpc));
+		LASSERT(!srpc_event_pending(rpc));
 
 		rc = srpc_prepare_reply(rpc);
 		if (rc != 0) {
@@ -1207,7 +1210,8 @@ srpc_send_rpc (swi_workitem_t *wi)
 		}
 
 		rc = srpc_prepare_bulk(rpc);
-		if (rc != 0) break;
+		if (rc != 0)
+			break;
 
 		wi->swi_state = SWI_STATE_REQUEST_SUBMITTED;
 		rc = srpc_send_request(rpc);
@@ -1217,20 +1221,24 @@ srpc_send_rpc (swi_workitem_t *wi)
 		/* CAVEAT EMPTOR: rqtev, rpyev, and bulkev may come in any
 		 * order; however, they're processed in a strict order:
 		 * rqt, rpy, and bulk. */
-		if (!rpc->crpc_reqstev.ev_fired) break;
+		if (!rpc->crpc_reqstev.ev_fired)
+			break;
 
 		rc = rpc->crpc_reqstev.ev_status;
-		if (rc != 0) break;
+		if (rc != 0)
+			break;
 
 		wi->swi_state = SWI_STATE_REQUEST_SENT;
 		/* perhaps more events, fall thru */
 	case SWI_STATE_REQUEST_SENT: {
 		srpc_msg_type_t type = srpc_service2reply(rpc->crpc_service);
 
-		if (!rpc->crpc_replyev.ev_fired) break;
+		if (!rpc->crpc_replyev.ev_fired)
+			break;
 
 		rc = rpc->crpc_replyev.ev_status;
-		if (rc != 0) break;
+		if (rc != 0)
+			break;
 
 		srpc_unpack_msg_hdr(reply);
 		if (reply->msg_type != type ||
@@ -1254,7 +1262,8 @@ srpc_send_rpc (swi_workitem_t *wi)
 		wi->swi_state = SWI_STATE_REPLY_RECEIVED;
 	}
 	case SWI_STATE_REPLY_RECEIVED:
-		if (do_bulk && !rpc->crpc_bulkev.ev_fired) break;
+		if (do_bulk && !rpc->crpc_bulkev.ev_fired)
+			break;
 
 		rc = do_bulk ? rpc->crpc_bulkev.ev_status : 0;
 
@@ -1292,7 +1301,7 @@ abort:
 }
 
 srpc_client_rpc_t *
-srpc_create_client_rpc (lnet_process_id_t peer, int service,
+srpc_create_client_rpc(lnet_process_id_t peer, int service,
 			int nbulkiov, int bulklen,
 			void (*rpc_done)(srpc_client_rpc_t *),
 			void (*rpc_fini)(srpc_client_rpc_t *), void *priv)
@@ -1311,15 +1320,15 @@ srpc_create_client_rpc (lnet_process_id_t peer, int service,
 
 /* called with rpc->crpc_lock held */
 void
-srpc_abort_rpc (srpc_client_rpc_t *rpc, int why)
+srpc_abort_rpc(srpc_client_rpc_t *rpc, int why)
 {
-	LASSERT (why != 0);
+	LASSERT(why != 0);
 
 	if (rpc->crpc_aborted || /* already aborted */
 	    rpc->crpc_closed)    /* callback imminent */
 		return;
 
-	CDEBUG (D_NET,
+	CDEBUG(D_NET,
 		"Aborting RPC: service %d, peer %s, state %s, why %d\n",
 		rpc->crpc_service, libcfs_id2str(rpc->crpc_dest),
 		swi_state2str(rpc->crpc_wi.swi_state), why);
@@ -1332,12 +1341,12 @@ srpc_abort_rpc (srpc_client_rpc_t *rpc, int why)
 
 /* called with rpc->crpc_lock held */
 void
-srpc_post_rpc (srpc_client_rpc_t *rpc)
+srpc_post_rpc(srpc_client_rpc_t *rpc)
 {
-	LASSERT (!rpc->crpc_aborted);
-	LASSERT (srpc_data.rpc_state == SRPC_STATE_RUNNING);
+	LASSERT(!rpc->crpc_aborted);
+	LASSERT(srpc_data.rpc_state == SRPC_STATE_RUNNING);
 
-	CDEBUG (D_NET, "Posting RPC: peer %s, service %d, timeout %d\n",
+	CDEBUG(D_NET, "Posting RPC: peer %s, service %d, timeout %d\n",
 		libcfs_id2str(rpc->crpc_dest), rpc->crpc_service,
 		rpc->crpc_timeout);
 
@@ -1403,7 +1412,7 @@ srpc_lnet_ev_handler(lnet_event_t *ev)
 	srpc_msg_t	*msg;
 	srpc_msg_type_t    type;
 
-	LASSERT (!in_interrupt());
+	LASSERT(!in_interrupt());
 
 	if (ev->status != 0) {
 		spin_lock(&srpc_data.rpc_glock);
@@ -1417,7 +1426,7 @@ srpc_lnet_ev_handler(lnet_event_t *ev)
 	default:
 		CERROR("Unknown event: status %d, type %d, lnet %d\n",
 		       rpcev->ev_status, rpcev->ev_type, rpcev->ev_lnet);
-		LBUG ();
+		LBUG();
 	case SRPC_REQUEST_SENT:
 		if (ev->status == 0 && ev->type != LNET_EVENT_UNLINK) {
 			spin_lock(&srpc_data.rpc_glock);
@@ -1436,7 +1445,7 @@ srpc_lnet_ev_handler(lnet_event_t *ev)
 			       &crpc->crpc_replyev, &crpc->crpc_bulkev);
 			CERROR("Bad event: status %d, type %d, lnet %d\n",
 			       rpcev->ev_status, rpcev->ev_type, rpcev->ev_lnet);
-			LBUG ();
+			LBUG();
 		}
 
 		spin_lock(&crpc->crpc_lock);
@@ -1458,10 +1467,10 @@ srpc_lnet_ev_handler(lnet_event_t *ev)
 
 		spin_lock(&scd->scd_lock);
 
-		LASSERT (ev->unlinked);
-		LASSERT (ev->type == LNET_EVENT_PUT ||
+		LASSERT(ev->unlinked);
+		LASSERT(ev->type == LNET_EVENT_PUT ||
 			 ev->type == LNET_EVENT_UNLINK);
-		LASSERT (ev->type != LNET_EVENT_UNLINK ||
+		LASSERT(ev->type != LNET_EVENT_UNLINK ||
 			 sv->sv_shuttingdown);
 
 		buffer = container_of(ev->md.start, srpc_buffer_t, buf_msg);
@@ -1536,7 +1545,7 @@ srpc_lnet_ev_handler(lnet_event_t *ev)
 		break;
 
 	case SRPC_BULK_GET_RPLD:
-		LASSERT (ev->type == LNET_EVENT_SEND ||
+		LASSERT(ev->type == LNET_EVENT_SEND ||
 			 ev->type == LNET_EVENT_REPLY ||
 			 ev->type == LNET_EVENT_UNLINK);
 
@@ -1574,7 +1583,7 @@ srpc_lnet_ev_handler(lnet_event_t *ev)
 
 
 int
-srpc_startup (void)
+srpc_startup(void)
 {
 	int rc;
 
@@ -1590,7 +1599,7 @@ srpc_startup (void)
 
 	rc = LNetNIInit(LUSTRE_SRV_LNET_PID);
 	if (rc < 0) {
-		CERROR ("LNetNIInit() has failed: %d\n", rc);
+		CERROR("LNetNIInit() has failed: %d\n", rc);
 		return rc;
 	}
 
@@ -1622,7 +1631,7 @@ bail:
 }
 
 void
-srpc_shutdown (void)
+srpc_shutdown(void)
 {
 	int i;
 	int rc;
@@ -1633,14 +1642,14 @@ srpc_shutdown (void)
 
 	switch (state) {
 	default:
-		LBUG ();
+		LBUG();
 	case SRPC_STATE_RUNNING:
 		spin_lock(&srpc_data.rpc_glock);
 
 		for (i = 0; i <= SRPC_SERVICE_MAX_ID; i++) {
 			srpc_service_t *sv = srpc_data.rpc_services[i];
 
-			LASSERTF (sv == NULL,
+			LASSERTF(sv == NULL,
 				  "service not empty: id %d, name %s\n",
 				  i, sv->sv_name);
 		}
@@ -1652,9 +1661,9 @@ srpc_shutdown (void)
 	case SRPC_STATE_EQ_INIT:
 		rc = LNetClearLazyPortal(SRPC_FRAMEWORK_REQUEST_PORTAL);
 		rc = LNetClearLazyPortal(SRPC_REQUEST_PORTAL);
-		LASSERT (rc == 0);
+		LASSERT(rc == 0);
 		rc = LNetEQFree(srpc_data.rpc_lnet_eq);
-		LASSERT (rc == 0); /* the EQ should have no user by now */
+		LASSERT(rc == 0); /* the EQ should have no user by now */
 
 	case SRPC_STATE_NI_INIT:
 		LNetNIFini();
diff --git a/drivers/staging/lustre/lustre/fid/lproc_fid.c b/drivers/staging/lustre/lustre/fid/lproc_fid.c
index 6a21f078fefa..783939dbd4db 100644
--- a/drivers/staging/lustre/lustre/fid/lproc_fid.c
+++ b/drivers/staging/lustre/lustre/fid/lproc_fid.c
@@ -120,15 +120,14 @@ static int
 lprocfs_fid_space_seq_show(struct seq_file *m, void *unused)
 {
 	struct lu_client_seq *seq = (struct lu_client_seq *)m->private;
-	int rc;
 
 	LASSERT(seq != NULL);
 
 	mutex_lock(&seq->lcs_mutex);
-	rc = seq_printf(m, "[%#llx - %#llx]:%x:%s\n", PRANGE(&seq->lcs_space));
+	seq_printf(m, "[%#llx - %#llx]:%x:%s\n", PRANGE(&seq->lcs_space));
 	mutex_unlock(&seq->lcs_mutex);
 
-	return rc;
+	return 0;
 }
 
 static ssize_t lprocfs_fid_width_seq_write(struct file *file,
@@ -170,30 +169,28 @@ static int
 lprocfs_fid_width_seq_show(struct seq_file *m, void *unused)
 {
 	struct lu_client_seq *seq = (struct lu_client_seq *)m->private;
-	int rc;
 
 	LASSERT(seq != NULL);
 
 	mutex_lock(&seq->lcs_mutex);
-	rc = seq_printf(m, "%llu\n", seq->lcs_width);
+	seq_printf(m, "%llu\n", seq->lcs_width);
 	mutex_unlock(&seq->lcs_mutex);
 
-	return rc;
+	return 0;
 }
 
 static int
 lprocfs_fid_fid_seq_show(struct seq_file *m, void *unused)
 {
 	struct lu_client_seq *seq = (struct lu_client_seq *)m->private;
-	int rc;
 
 	LASSERT(seq != NULL);
 
 	mutex_lock(&seq->lcs_mutex);
-	rc = seq_printf(m, DFID"\n", PFID(&seq->lcs_fid));
+	seq_printf(m, DFID "\n", PFID(&seq->lcs_fid));
 	mutex_unlock(&seq->lcs_mutex);
 
-	return rc;
+	return 0;
 }
 
 static int
@@ -201,17 +198,17 @@ lprocfs_fid_server_seq_show(struct seq_file *m, void *unused)
 {
 	struct lu_client_seq *seq = (struct lu_client_seq *)m->private;
 	struct client_obd *cli;
-	int rc;
 
 	LASSERT(seq != NULL);
 
 	if (seq->lcs_exp != NULL) {
 		cli = &seq->lcs_exp->exp_obd->u.cli;
-		rc = seq_printf(m, "%s\n", cli->cl_target_uuid.uuid);
+		seq_printf(m, "%s\n", cli->cl_target_uuid.uuid);
 	} else {
-		rc = seq_printf(m, "%s\n", seq->lcs_srv->lss_name);
+		seq_printf(m, "%s\n", seq->lcs_srv->lss_name);
 	}
-	return rc;
+
+	return 0;
 }
 
 LPROC_SEQ_FOPS(lprocfs_fid_space);
diff --git a/drivers/staging/lustre/lustre/fld/fld_internal.h b/drivers/staging/lustre/lustre/fld/fld_internal.h
index 6125bbe822b5..68bec7658463 100644
--- a/drivers/staging/lustre/lustre/fld/fld_internal.h
+++ b/drivers/staging/lustre/lustre/fld/fld_internal.h
@@ -142,7 +142,7 @@ extern struct lu_fld_hash fld_hash[];
 int fld_client_rpc(struct obd_export *exp,
 		   struct lu_seq_range *range, __u32 fld_op);
 
-#if defined (CONFIG_PROC_FS)
+#if defined(CONFIG_PROC_FS)
 extern struct lprocfs_vars fld_client_proc_list[];
 #endif
 
diff --git a/drivers/staging/lustre/lustre/fld/fld_request.c b/drivers/staging/lustre/lustre/fld/fld_request.c
index b8d17e109a96..6ac225e90ee0 100644
--- a/drivers/staging/lustre/lustre/fld/fld_request.c
+++ b/drivers/staging/lustre/lustre/fld/fld_request.c
@@ -217,10 +217,9 @@ int fld_client_add_target(struct lu_client_fld *fld,
 		CERROR("%s: Attempt to add target %s (idx %llu) on fly - skip it\n",
 			fld->lcf_name, name, tar->ft_idx);
 		return 0;
-	} else {
-		CDEBUG(D_INFO, "%s: Adding target %s (idx %llu)\n",
-		       fld->lcf_name, name, tar->ft_idx);
 	}
+	CDEBUG(D_INFO, "%s: Adding target %s (idx %llu)\n",
+			fld->lcf_name, name, tar->ft_idx);
 
 	OBD_ALLOC_PTR(target);
 	if (target == NULL)
@@ -280,7 +279,7 @@ EXPORT_SYMBOL(fld_client_del_target);
 
 static struct proc_dir_entry *fld_type_proc_dir;
 
-#if defined (CONFIG_PROC_FS)
+#if defined(CONFIG_PROC_FS)
 static int fld_client_proc_init(struct lu_client_fld *fld)
 {
 	int rc;
@@ -327,7 +326,6 @@ static int fld_client_proc_init(struct lu_client_fld *fld)
 
 void fld_client_proc_fini(struct lu_client_fld *fld)
 {
-	return;
 }
 #endif
 EXPORT_SYMBOL(fld_client_proc_fini);
diff --git a/drivers/staging/lustre/lustre/fld/lproc_fld.c b/drivers/staging/lustre/lustre/fld/lproc_fld.c
index 8c5a65704a37..f53fdcfae34e 100644
--- a/drivers/staging/lustre/lustre/fld/lproc_fld.c
+++ b/drivers/staging/lustre/lustre/fld/lproc_fld.c
@@ -168,4 +168,5 @@ struct lprocfs_vars fld_client_proc_list[] = {
 	{ "targets", &fld_proc_targets_fops },
 	{ "hash", &fld_proc_hash_fops },
 	{ "cache_flush", &fld_proc_cache_flush_fops },
-	{ NULL }};
+	{ NULL }
+};
diff --git a/drivers/staging/lustre/lustre/include/lprocfs_status.h b/drivers/staging/lustre/lustre/include/lprocfs_status.h
index 8a25cf6f6825..d030847e51ba 100644
--- a/drivers/staging/lustre/lustre/include/lprocfs_status.h
+++ b/drivers/staging/lustre/lustre/include/lprocfs_status.h
@@ -679,7 +679,7 @@ extern int lprocfs_seq_release(struct inode *, struct file *);
 	}				       \
 } while (0)
 #define LPROCFS_CLIMP_EXIT(obd)		 \
-	up_read(&(obd)->u.cli.cl_sem);
+	up_read(&(obd)->u.cli.cl_sem)
 
 
 /* write the name##_seq_show function, call LPROC_SEQ_FOPS_RO for read-only
@@ -723,7 +723,7 @@ static struct file_operations name##_fops = {				\
 		return lprocfs_wr_##type(file, buffer,			\
 					 count, seq->private);		\
 	}								\
-	LPROC_SEQ_FOPS(name##_##type);
+	LPROC_SEQ_FOPS(name##_##type)
 
 #define LPROC_SEQ_FOPS_WR_ONLY(name, type)				\
 	static ssize_t name##_##type##_write(struct file *file,		\
@@ -740,7 +740,7 @@ static struct file_operations name##_fops = {				\
 		.open	= name##_##type##_open,				\
 		.write	= name##_##type##_write,			\
 		.release = lprocfs_single_release,			\
-	};
+	}
 
 /* lproc_ptlrpc.c */
 struct ptlrpc_request;
diff --git a/drivers/staging/lustre/lustre/include/lu_object.h b/drivers/staging/lustre/lustre/include/lu_object.h
index 2ddb2b054d8d..c8cc48f00026 100644
--- a/drivers/staging/lustre/lustre/include/lu_object.h
+++ b/drivers/staging/lustre/lustre/include/lu_object.h
@@ -192,7 +192,7 @@ struct lu_object_conf {
  */
 typedef int (*lu_printer_t)(const struct lu_env *env,
 			    void *cookie, const char *format, ...)
-	__attribute__ ((format (printf, 3, 4)));
+	__printf(3, 4);
 
 /**
  * Operations specific for particular lu_object.
diff --git a/drivers/staging/lustre/lustre/include/lustre_dlm.h b/drivers/staging/lustre/lustre/include/lustre_dlm.h
index 83bc0a9d7d4c..bac9902b56bb 100644
--- a/drivers/staging/lustre/lustre/include/lustre_dlm.h
+++ b/drivers/staging/lustre/lustre/include/lustre_dlm.h
@@ -1075,7 +1075,7 @@ extern char *ldlm_it2str(int it);
 void _ldlm_lock_debug(struct ldlm_lock *lock,
 		      struct libcfs_debug_msg_data *data,
 		      const char *fmt, ...)
-	__attribute__ ((format (printf, 3, 4)));
+	__printf(3, 4);
 
 /**
  * Rate-limited version of lock printing function.
diff --git a/drivers/staging/lustre/lustre/include/lustre_import.h b/drivers/staging/lustre/lustre/include/lustre_import.h
index 51f3e98f94e2..dcc807676c49 100644
--- a/drivers/staging/lustre/lustre/include/lustre_import.h
+++ b/drivers/staging/lustre/lustre/include/lustre_import.h
@@ -218,6 +218,8 @@ struct obd_import {
 	atomic_t	      imp_timeouts;
 	/** Current import state */
 	enum lustre_imp_state     imp_state;
+	/** Last replay state */
+	enum lustre_imp_state	  imp_replay_state;
 	/** History of import states */
 	struct import_state_hist  imp_state_hist[IMP_STATE_HIST_LEN];
 	int		       imp_state_hist_idx;
diff --git a/drivers/staging/lustre/lustre/include/lustre_net.h b/drivers/staging/lustre/lustre/include/lustre_net.h
index 36396d1c94dc..e2805bd1acf1 100644
--- a/drivers/staging/lustre/lustre/include/lustre_net.h
+++ b/drivers/staging/lustre/lustre/include/lustre_net.h
@@ -1673,7 +1673,7 @@ ptlrpc_rqphase2str(struct ptlrpc_request *req)
 
 void _debug_req(struct ptlrpc_request *req,
 		struct libcfs_debug_msg_data *data, const char *fmt, ...)
-	__attribute__ ((format (printf, 3, 4)));
+	__printf(3, 4);
 
 /**
  * Helper that decides if we need to print request according to current debug
diff --git a/drivers/staging/lustre/lustre/include/obd_class.h b/drivers/staging/lustre/lustre/include/obd_class.h
index 4a29261c514d..34b5fa3f081c 100644
--- a/drivers/staging/lustre/lustre/include/obd_class.h
+++ b/drivers/staging/lustre/lustre/include/obd_class.h
@@ -72,6 +72,7 @@ extern int lustre_get_jobid(char *jobid);
 struct lu_device_type;
 
 /* genops.c */
+extern struct list_head obd_types;
 struct obd_export *class_conn2export(struct lustre_handle *);
 int class_register_type(struct obd_ops *, struct md_ops *,
 			struct lprocfs_vars *, const char *nm,
@@ -222,20 +223,20 @@ extern void (*class_export_dump_hook)(struct obd_export *);
 
 #endif
 
-#define class_export_rpc_inc(exp)				       \
-({								      \
-	atomic_inc(&(exp)->exp_rpc_count);			  \
-	CDEBUG(D_INFO, "RPC GETting export %p : new rpc_count %d\n",    \
-	       (exp), atomic_read(&(exp)->exp_rpc_count));	  \
-})
+static inline void class_export_rpc_inc(struct obd_export *exp)
+{
+	atomic_inc(&(exp)->exp_rpc_count);
+	CDEBUG(D_INFO, "RPC GETting export %p : new rpc_count %d\n",
+	       (exp), atomic_read(&(exp)->exp_rpc_count));
+}
 
-#define class_export_rpc_dec(exp)				       \
-({								      \
-	LASSERT_ATOMIC_POS(&exp->exp_rpc_count);			\
-	atomic_dec(&(exp)->exp_rpc_count);			  \
-	CDEBUG(D_INFO, "RPC PUTting export %p : new rpc_count %d\n",    \
-	       (exp), atomic_read(&(exp)->exp_rpc_count));	  \
-})
+static inline void class_export_rpc_dec(struct obd_export *exp)
+{
+	LASSERT_ATOMIC_POS(&exp->exp_rpc_count);
+	atomic_dec(&(exp)->exp_rpc_count);
+	CDEBUG(D_INFO, "RPC PUTting export %p : new rpc_count %d\n",
+	       (exp), atomic_read(&(exp)->exp_rpc_count));
+}
 
 #define class_export_lock_get(exp, lock)				\
 ({								      \
@@ -332,25 +333,29 @@ void obdo_le_to_cpu(struct obdo *dobdo, struct obdo *sobdo);
 
 /* Ensure obd_setup: used for cleanup which must be called
    while obd is stopping */
-#define OBD_CHECK_DEV(obd)				      \
-do {							    \
-	if (!(obd)) {					   \
-		CERROR("NULL device\n");			\
-		return -ENODEV;				\
-	}						       \
-} while (0)
+static inline int obd_check_dev(struct obd_device *obd)
+{
+	if (!obd) {
+		CERROR("NULL device\n");
+		return -ENODEV;
+	}
+	return 0;
+}
 
 /* ensure obd_setup and !obd_stopping */
-#define OBD_CHECK_DEV_ACTIVE(obd)			       \
-do {							    \
-	OBD_CHECK_DEV(obd);				     \
-	if (!(obd)->obd_set_up || (obd)->obd_stopping) {	\
-		CERROR("Device %d not setup\n",		 \
-		       (obd)->obd_minor);		       \
-		return -ENODEV;				\
-	}						       \
-} while (0)
+static inline int obd_check_dev_active(struct obd_device *obd)
+{
+	int rc;
 
+	rc = obd_check_dev(obd);
+	if (rc)
+		return rc;
+	if (!obd->obd_set_up || obd->obd_stopping) {
+		CERROR("Device %d not setup\n", obd->obd_minor);
+		return -ENODEV;
+	}
+	return rc;
+}
 
 #if defined (CONFIG_PROC_FS)
 #define OBD_COUNTER_OFFSET(op)				  \
@@ -593,7 +598,9 @@ static inline int obd_precleanup(struct obd_device *obd,
 	int rc;
 	DECLARE_LU_VARS(ldt, d);
 
-	OBD_CHECK_DEV(obd);
+	rc = obd_check_dev(obd);
+	if (rc)
+		return rc;
 	ldt = obd->obd_type->typ_lu;
 	d = obd->obd_lu_dev;
 	if (ldt != NULL && d != NULL) {
@@ -619,7 +626,9 @@ static inline int obd_cleanup(struct obd_device *obd)
 	int rc;
 	DECLARE_LU_VARS(ldt, d);
 
-	OBD_CHECK_DEV(obd);
+	rc = obd_check_dev(obd);
+	if (rc)
+		return rc;
 
 	ldt = obd->obd_type->typ_lu;
 	d = obd->obd_lu_dev;
@@ -667,7 +676,9 @@ obd_process_config(struct obd_device *obd, int datalen, void *data)
 	int rc;
 	DECLARE_LU_VARS(ldt, d);
 
-	OBD_CHECK_DEV(obd);
+	rc = obd_check_dev(obd);
+	if (rc)
+		return rc;
 
 	obd->obd_process_conf = 1;
 	ldt = obd->obd_type->typ_lu;
@@ -885,7 +896,9 @@ static inline int obd_add_conn(struct obd_import *imp, struct obd_uuid *uuid,
 	struct obd_device *obd = imp->imp_obd;
 	int rc;
 
-	OBD_CHECK_DEV_ACTIVE(obd);
+	rc = obd_check_dev_active(obd);
+	if (rc)
+		return rc;
 	OBD_CHECK_DT_OP(obd, add_conn, -EOPNOTSUPP);
 	OBD_COUNTER_INCREMENT(obd, add_conn);
 
@@ -898,7 +911,9 @@ static inline int obd_del_conn(struct obd_import *imp, struct obd_uuid *uuid)
 	struct obd_device *obd = imp->imp_obd;
 	int rc;
 
-	OBD_CHECK_DEV_ACTIVE(obd);
+	rc = obd_check_dev_active(obd);
+	if (rc)
+		return rc;
 	OBD_CHECK_DT_OP(obd, del_conn, -EOPNOTSUPP);
 	OBD_COUNTER_INCREMENT(obd, del_conn);
 
@@ -932,7 +947,9 @@ static inline int obd_connect(const struct lu_env *env,
 	__u64 ocf = data ? data->ocd_connect_flags : 0; /* for post-condition
 						   * check */
 
-	OBD_CHECK_DEV_ACTIVE(obd);
+	rc = obd_check_dev_active(obd);
+	if (rc)
+		return rc;
 	OBD_CHECK_DT_OP(obd, connect, -EOPNOTSUPP);
 	OBD_COUNTER_INCREMENT(obd, connect);
 
@@ -954,7 +971,9 @@ static inline int obd_reconnect(const struct lu_env *env,
 	__u64 ocf = d ? d->ocd_connect_flags : 0; /* for post-condition
 						   * check */
 
-	OBD_CHECK_DEV_ACTIVE(obd);
+	rc = obd_check_dev_active(obd);
+	if (rc)
+		return rc;
 	OBD_CHECK_DT_OP(obd, reconnect, 0);
 	OBD_COUNTER_INCREMENT(obd, reconnect);
 
@@ -1279,7 +1298,9 @@ static inline int obd_notify(struct obd_device *obd,
 {
 	int rc;
 
-	OBD_CHECK_DEV(obd);
+	rc = obd_check_dev(obd);
+	if (rc)
+		return rc;
 
 	/* the check for async_recov is a complete hack - I'm hereby
 	   overloading the meaning to also mean "this was called from
@@ -1380,7 +1401,11 @@ static inline int obd_health_check(const struct lu_env *env,
 static inline int obd_register_observer(struct obd_device *obd,
 					struct obd_device *observer)
 {
-	OBD_CHECK_DEV(obd);
+	int rc;
+
+	rc = obd_check_dev(obd);
+	if (rc)
+		return rc;
 	down_write(&obd->obd_observer_link_sem);
 	if (obd->obd_observer && observer) {
 		up_write(&obd->obd_observer_link_sem);
@@ -1895,6 +1920,8 @@ void class_exit_uuidlist(void);
 
 /* class_obd.c */
 extern char obd_jobid_node[];
+extern struct miscdevice obd_psdev;
+extern spinlock_t obd_types_lock;
 
 /* prng.c */
 #define ll_generate_random_uuid(uuid_out) cfs_get_random_bytes(uuid_out, sizeof(class_uuid_t))
diff --git a/drivers/staging/lustre/lustre/lclient/lcommon_cl.c b/drivers/staging/lustre/lustre/lclient/lcommon_cl.c
index 23095bb75226..ab6cb419302f 100644
--- a/drivers/staging/lustre/lustre/lclient/lcommon_cl.c
+++ b/drivers/staging/lustre/lustre/lclient/lcommon_cl.c
@@ -828,7 +828,8 @@ int ccc_prep_size(const struct lu_env *env, struct cl_object *obj,
 				 * --bug 17336 */
 				loff_t size = cl_isize_read(inode);
 				loff_t cur_index = start >> PAGE_CACHE_SHIFT;
-				loff_t size_index = ((size - 1) >> PAGE_CACHE_SHIFT);
+				loff_t size_index = (size - 1) >>
+						    PAGE_CACHE_SHIFT;
 
 				if ((size == 0 && cur_index != 0) ||
 				    size_index < cur_index)
@@ -1263,7 +1264,7 @@ __u32 cl_fid_build_gen(const struct lu_fid *fid)
 		return gen;
 	}
 
-	gen = (fid_flatten(fid) >> 32);
+	gen = fid_flatten(fid) >> 32;
 	return gen;
 }
 
diff --git a/drivers/staging/lustre/lustre/ldlm/ldlm_extent.c b/drivers/staging/lustre/lustre/ldlm/ldlm_extent.c
index a89eebaedabf..fd9b059361f9 100644
--- a/drivers/staging/lustre/lustre/ldlm/ldlm_extent.c
+++ b/drivers/staging/lustre/lustre/ldlm/ldlm_extent.c
@@ -151,7 +151,8 @@ static inline int lock_mode_to_index(ldlm_mode_t mode)
 
 	LASSERT(mode != 0);
 	LASSERT(IS_PO2(mode));
-	for (index = -1; mode; index++, mode >>= 1) ;
+	for (index = -1; mode; index++)
+		mode >>= 1;
 	LASSERT(index < LCK_MODE_NUM);
 	return index;
 }
diff --git a/drivers/staging/lustre/lustre/ldlm/ldlm_internal.h b/drivers/staging/lustre/lustre/ldlm/ldlm_internal.h
index 20e64cddb1f4..70b909f55861 100644
--- a/drivers/staging/lustre/lustre/ldlm/ldlm_internal.h
+++ b/drivers/staging/lustre/lustre/ldlm/ldlm_internal.h
@@ -131,12 +131,12 @@ struct ldlm_cb_set_arg {
 	union ldlm_gl_desc		*gl_desc; /* glimpse AST descriptor */
 };
 
-typedef enum {
+enum ldlm_desc_ast_t {
 	LDLM_WORK_BL_AST,
 	LDLM_WORK_CP_AST,
 	LDLM_WORK_REVOKE_AST,
 	LDLM_WORK_GL_AST
-} ldlm_desc_ast_t;
+};
 
 void ldlm_grant_lock(struct ldlm_lock *lock, struct list_head *work_list);
 int ldlm_fill_lvb(struct ldlm_lock *lock, struct req_capsule *pill,
@@ -155,7 +155,7 @@ void ldlm_lock_decref_internal_nolock(struct ldlm_lock *, __u32 mode);
 void ldlm_add_ast_work_item(struct ldlm_lock *lock, struct ldlm_lock *new,
 			    struct list_head *work_list);
 int ldlm_run_ast_work(struct ldlm_namespace *ns, struct list_head *rpc_list,
-		      ldlm_desc_ast_t ast_type);
+		      enum ldlm_desc_ast_t ast_type);
 int ldlm_work_gl_ast_lock(struct ptlrpc_request_set *rqset, void *opaq);
 int ldlm_lock_remove_from_lru(struct ldlm_lock *lock);
 int ldlm_lock_remove_from_lru_nolock(struct ldlm_lock *lock);
@@ -177,6 +177,10 @@ int ldlm_bl_to_thread_list(struct ldlm_namespace *ns,
 void ldlm_handle_bl_callback(struct ldlm_namespace *ns,
 			     struct ldlm_lock_desc *ld, struct ldlm_lock *lock);
 
+extern struct kmem_cache *ldlm_resource_slab;
+
+/* ldlm_lockd.c & ldlm_lock.c */
+extern struct kmem_cache *ldlm_lock_slab;
 
 /* ldlm_extent.c */
 void ldlm_extent_add_lock(struct ldlm_resource *res, struct ldlm_lock *lock);
@@ -207,9 +211,9 @@ struct ldlm_state {
 /* interval tree, for LDLM_EXTENT. */
 extern struct kmem_cache *ldlm_interval_slab; /* slab cache for ldlm_interval */
 extern void ldlm_interval_attach(struct ldlm_interval *n, struct ldlm_lock *l);
-extern struct ldlm_interval *ldlm_interval_detach(struct ldlm_lock *l);
-extern struct ldlm_interval *ldlm_interval_alloc(struct ldlm_lock *lock);
-extern void ldlm_interval_free(struct ldlm_interval *node);
+struct ldlm_interval *ldlm_interval_detach(struct ldlm_lock *l);
+struct ldlm_interval *ldlm_interval_alloc(struct ldlm_lock *lock);
+void ldlm_interval_free(struct ldlm_interval *node);
 /* this function must be called with res lock held */
 static inline struct ldlm_extent *
 ldlm_interval_extent(struct ldlm_interval *node)
diff --git a/drivers/staging/lustre/lustre/ldlm/ldlm_lock.c b/drivers/staging/lustre/lustre/ldlm/ldlm_lock.c
index 8191005464b1..84b111eb48fa 100644
--- a/drivers/staging/lustre/lustre/ldlm/ldlm_lock.c
+++ b/drivers/staging/lustre/lustre/ldlm/ldlm_lock.c
@@ -151,8 +151,6 @@ char *ldlm_it2str(int it)
 }
 EXPORT_SYMBOL(ldlm_it2str);
 
-extern struct kmem_cache *ldlm_lock_slab;
-
 
 void ldlm_register_intent(struct ldlm_namespace *ns, ldlm_res_policy arg)
 {
@@ -1805,7 +1803,7 @@ int ldlm_work_gl_ast_lock(struct ptlrpc_request_set *rqset, void *opaq)
  * one.
  */
 int ldlm_run_ast_work(struct ldlm_namespace *ns, struct list_head *rpc_list,
-		      ldlm_desc_ast_t ast_type)
+		      enum ldlm_desc_ast_t ast_type)
 {
 	struct ldlm_cb_set_arg *arg;
 	set_producer_func       work_ast_lock;
diff --git a/drivers/staging/lustre/lustre/ldlm/ldlm_lockd.c b/drivers/staging/lustre/lustre/ldlm/ldlm_lockd.c
index 98fbd3f7e751..08a91f5d91b1 100644
--- a/drivers/staging/lustre/lustre/ldlm/ldlm_lockd.c
+++ b/drivers/staging/lustre/lustre/ldlm/ldlm_lockd.c
@@ -55,8 +55,6 @@ static char *ldlm_cpts;
 module_param(ldlm_cpts, charp, 0444);
 MODULE_PARM_DESC(ldlm_cpts, "CPU partitions ldlm threads should run on");
 
-extern struct kmem_cache *ldlm_resource_slab;
-extern struct kmem_cache *ldlm_lock_slab;
 static struct mutex	ldlm_ref_mutex;
 static int ldlm_refcount;
 
@@ -154,7 +152,7 @@ void ldlm_handle_bl_callback(struct ldlm_namespace *ns,
 	if (lock->l_flags & LDLM_FL_CANCEL_ON_BLOCK)
 		lock->l_flags |= LDLM_FL_CANCEL;
 
-	do_ast = (!lock->l_readers && !lock->l_writers);
+	do_ast = !lock->l_readers && !lock->l_writers;
 	unlock_res_and_lock(lock);
 
 	if (do_ast) {
diff --git a/drivers/staging/lustre/lustre/ldlm/ldlm_pool.c b/drivers/staging/lustre/lustre/ldlm/ldlm_pool.c
index d20d277dc2f7..a9f4833e03e5 100644
--- a/drivers/staging/lustre/lustre/ldlm/ldlm_pool.c
+++ b/drivers/staging/lustre/lustre/ldlm/ldlm_pool.c
@@ -141,8 +141,6 @@
  */
 #define LDLM_POOL_SLV_SHIFT (10)
 
-extern struct proc_dir_entry *ldlm_ns_proc_dir;
-
 static inline __u64 dru(__u64 val, __u32 shift, int round_up)
 {
 	return (val + (round_up ? (1 << shift) - 1 : 0)) >> shift;
@@ -689,8 +687,8 @@ static int lprocfs_pool_state_seq_show(struct seq_file *m, void *unused)
 			      "  GP:  %d\n",
 			      grant_step, grant_plan);
 	}
-	seq_printf(m, "  GR:  %d\n" "  CR:  %d\n" "  GS:  %d\n"
-		      "  G:   %d\n" "  L:   %d\n",
+	seq_printf(m, "  GR:  %d\n  CR:  %d\n  GS:  %d\n"
+		      "  G:   %d\n  L:   %d\n",
 		      grant_rate, cancel_rate, grant_speed,
 		      granted, limit);
 
diff --git a/drivers/staging/lustre/lustre/ldlm/ldlm_request.c b/drivers/staging/lustre/lustre/ldlm/ldlm_request.c
index 287da325d928..4f713183145b 100644
--- a/drivers/staging/lustre/lustre/ldlm/ldlm_request.c
+++ b/drivers/staging/lustre/lustre/ldlm/ldlm_request.c
@@ -307,7 +307,7 @@ int ldlm_blocking_ast_nocheck(struct ldlm_lock *lock)
 	int do_ast;
 
 	lock->l_flags |= LDLM_FL_CBPENDING;
-	do_ast = (!lock->l_readers && !lock->l_writers);
+	do_ast = !lock->l_readers && !lock->l_writers;
 	unlock_res_and_lock(lock);
 
 	if (do_ast) {
@@ -1779,7 +1779,6 @@ int ldlm_cancel_resource_local(struct ldlm_resource *res,
 		if (opaque != NULL && lock->l_ast_data != opaque) {
 			LDLM_ERROR(lock, "data %p doesn't match opaque %p",
 				   lock->l_ast_data, opaque);
-			//LBUG();
 			continue;
 		}
 
diff --git a/drivers/staging/lustre/lustre/ldlm/ldlm_resource.c b/drivers/staging/lustre/lustre/ldlm/ldlm_resource.c
index c6f62a91b233..f750d42a7ad5 100644
--- a/drivers/staging/lustre/lustre/ldlm/ldlm_resource.c
+++ b/drivers/staging/lustre/lustre/ldlm/ldlm_resource.c
@@ -62,7 +62,7 @@ LIST_HEAD(ldlm_cli_active_namespace_list);
 LIST_HEAD(ldlm_cli_inactive_namespace_list);
 
 struct proc_dir_entry *ldlm_type_proc_dir = NULL;
-struct proc_dir_entry *ldlm_ns_proc_dir = NULL;
+static struct proc_dir_entry *ldlm_ns_proc_dir = NULL;
 struct proc_dir_entry *ldlm_svc_proc_dir = NULL;
 
 extern unsigned int ldlm_cancel_unused_locks_before_replay;
diff --git a/drivers/staging/lustre/lustre/libcfs/Makefile b/drivers/staging/lustre/lustre/libcfs/Makefile
index fcecbd2271af..2996a48a31fb 100644
--- a/drivers/staging/lustre/lustre/libcfs/Makefile
+++ b/drivers/staging/lustre/lustre/libcfs/Makefile
@@ -3,7 +3,7 @@ obj-$(CONFIG_LUSTRE_FS) += libcfs.o
 libcfs-linux-objs := linux-tracefile.o linux-debug.o
 libcfs-linux-objs += linux-prim.o linux-cpu.o
 libcfs-linux-objs += linux-tcpip.o
-libcfs-linux-objs += linux-proc.o linux-curproc.o
+libcfs-linux-objs += linux-curproc.o
 libcfs-linux-objs += linux-module.o
 libcfs-linux-objs += linux-crypto.o
 libcfs-linux-objs += linux-crypto-adler.o
diff --git a/drivers/staging/lustre/lustre/libcfs/debug.c b/drivers/staging/lustre/lustre/libcfs/debug.c
index 76c62e87a415..021c92fa0333 100644
--- a/drivers/staging/lustre/lustre/libcfs/debug.c
+++ b/drivers/staging/lustre/lustre/libcfs/debug.c
@@ -409,8 +409,8 @@ int libcfs_debug_init(unsigned long bufsize)
 	if (max > cfs_trace_max_debug_mb() || max < num_possible_cpus()) {
 		max = TCD_MAX_PAGES;
 	} else {
-		max = (max / num_possible_cpus());
-		max = max << (20 - PAGE_CACHE_SHIFT);
+		max = max / num_possible_cpus();
+		max <<= (20 - PAGE_CACHE_SHIFT);
 	}
 	rc = cfs_tracefile_init(max);
 
diff --git a/drivers/staging/lustre/lustre/libcfs/hash.c b/drivers/staging/lustre/lustre/libcfs/hash.c
index ec3a2a8b8b2c..a55567e0de9e 100644
--- a/drivers/staging/lustre/lustre/libcfs/hash.c
+++ b/drivers/staging/lustre/lustre/libcfs/hash.c
@@ -2008,13 +2008,10 @@ void cfs_hash_rehash_key(struct cfs_hash *hs, const void *old_key,
 }
 EXPORT_SYMBOL(cfs_hash_rehash_key);
 
-int cfs_hash_debug_header(struct seq_file *m)
+void cfs_hash_debug_header(struct seq_file *m)
 {
-	return seq_printf(m, "%-*s%6s%6s%6s%6s%6s%6s%6s%7s%8s%8s%8s%s\n",
-		 CFS_HASH_BIGNAME_LEN,
-		 "name", "cur", "min", "max", "theta", "t-min", "t-max",
-		 "flags", "rehash", "count", "maxdep", "maxdepb",
-		 " distribution");
+	seq_printf(m, "%-*s   cur   min   max theta t-min t-max flags rehash   count  maxdep maxdepb distribution\n",
+		   CFS_HASH_BIGNAME_LEN, "name");
 }
 EXPORT_SYMBOL(cfs_hash_debug_header);
 
@@ -2042,7 +2039,7 @@ cfs_hash_full_nbkt(struct cfs_hash *hs)
 	       CFS_HASH_RH_NBKT(hs) : CFS_HASH_NBKT(hs);
 }
 
-int cfs_hash_debug_str(struct cfs_hash *hs, struct seq_file *m)
+void cfs_hash_debug_str(struct cfs_hash *hs, struct seq_file *m)
 {
 	int		    dist[8] = { 0, };
 	int		    maxdep  = -1;
@@ -2097,7 +2094,5 @@ int cfs_hash_debug_str(struct cfs_hash *hs, struct seq_file *m)
 		seq_printf(m, "%d%c",  dist[i], (i == 7) ? '\n' : '/');
 
 	cfs_hash_unlock(hs, 0);
-
-	return 0;
 }
 EXPORT_SYMBOL(cfs_hash_debug_str);
diff --git a/drivers/staging/lustre/lustre/libcfs/linux/linux-cpu.c b/drivers/staging/lustre/lustre/libcfs/linux/linux-cpu.c
index 05f7595f18aa..cc3ab351943e 100644
--- a/drivers/staging/lustre/lustre/libcfs/linux/linux-cpu.c
+++ b/drivers/staging/lustre/lustre/libcfs/linux/linux-cpu.c
@@ -204,7 +204,7 @@ cfs_cpt_table_print(struct cfs_cpt_table *cptab, char *buf, int len)
 		}
 
 		tmp += rc;
-		for_each_cpu_mask(j, *cptab->ctb_parts[i].cpt_cpumask) {
+		for_each_cpu(j, cptab->ctb_parts[i].cpt_cpumask) {
 			rc = snprintf(tmp, len, "%d ", j);
 			len -= rc;
 			if (len <= 0) {
@@ -240,8 +240,8 @@ cfs_cpt_weight(struct cfs_cpt_table *cptab, int cpt)
 	LASSERT(cpt == CFS_CPT_ANY || (cpt >= 0 && cpt < cptab->ctb_nparts));
 
 	return cpt == CFS_CPT_ANY ?
-	       cpus_weight(*cptab->ctb_cpumask) :
-	       cpus_weight(*cptab->ctb_parts[cpt].cpt_cpumask);
+	       cpumask_weight(cptab->ctb_cpumask) :
+	       cpumask_weight(cptab->ctb_parts[cpt].cpt_cpumask);
 }
 EXPORT_SYMBOL(cfs_cpt_weight);
 
@@ -251,8 +251,10 @@ cfs_cpt_online(struct cfs_cpt_table *cptab, int cpt)
 	LASSERT(cpt == CFS_CPT_ANY || (cpt >= 0 && cpt < cptab->ctb_nparts));
 
 	return cpt == CFS_CPT_ANY ?
-	       any_online_cpu(*cptab->ctb_cpumask) != NR_CPUS :
-	       any_online_cpu(*cptab->ctb_parts[cpt].cpt_cpumask) != NR_CPUS;
+	       cpumask_any_and(cptab->ctb_cpumask,
+			       cpu_online_mask) < nr_cpu_ids :
+	       cpumask_any_and(cptab->ctb_parts[cpt].cpt_cpumask,
+			       cpu_online_mask) < nr_cpu_ids;
 }
 EXPORT_SYMBOL(cfs_cpt_online);
 
@@ -283,7 +285,7 @@ cfs_cpt_set_cpu(struct cfs_cpt_table *cptab, int cpt, int cpu)
 
 	LASSERT(cpt >= 0 && cpt < cptab->ctb_nparts);
 
-	if (cpu < 0 || cpu >= NR_CPUS || !cpu_online(cpu)) {
+	if (cpu < 0 || cpu >= nr_cpu_ids || !cpu_online(cpu)) {
 		CDEBUG(D_INFO, "CPU %d is invalid or it's offline\n", cpu);
 		return 0;
 	}
@@ -296,11 +298,11 @@ cfs_cpt_set_cpu(struct cfs_cpt_table *cptab, int cpt, int cpu)
 
 	cptab->ctb_cpu2cpt[cpu] = cpt;
 
-	LASSERT(!cpu_isset(cpu, *cptab->ctb_cpumask));
-	LASSERT(!cpu_isset(cpu, *cptab->ctb_parts[cpt].cpt_cpumask));
+	LASSERT(!cpumask_test_cpu(cpu, cptab->ctb_cpumask));
+	LASSERT(!cpumask_test_cpu(cpu, cptab->ctb_parts[cpt].cpt_cpumask));
 
-	cpu_set(cpu, *cptab->ctb_cpumask);
-	cpu_set(cpu, *cptab->ctb_parts[cpt].cpt_cpumask);
+	cpumask_set_cpu(cpu, cptab->ctb_cpumask);
+	cpumask_set_cpu(cpu, cptab->ctb_parts[cpt].cpt_cpumask);
 
 	node = cpu_to_node(cpu);
 
@@ -324,7 +326,7 @@ cfs_cpt_unset_cpu(struct cfs_cpt_table *cptab, int cpt, int cpu)
 
 	LASSERT(cpt == CFS_CPT_ANY || (cpt >= 0 && cpt < cptab->ctb_nparts));
 
-	if (cpu < 0 || cpu >= NR_CPUS) {
+	if (cpu < 0 || cpu >= nr_cpu_ids) {
 		CDEBUG(D_INFO, "Invalid CPU id %d\n", cpu);
 		return;
 	}
@@ -344,11 +346,11 @@ cfs_cpt_unset_cpu(struct cfs_cpt_table *cptab, int cpt, int cpu)
 		return;
 	}
 
-	LASSERT(cpu_isset(cpu, *cptab->ctb_parts[cpt].cpt_cpumask));
-	LASSERT(cpu_isset(cpu, *cptab->ctb_cpumask));
+	LASSERT(cpumask_test_cpu(cpu, cptab->ctb_parts[cpt].cpt_cpumask));
+	LASSERT(cpumask_test_cpu(cpu, cptab->ctb_cpumask));
 
-	cpu_clear(cpu, *cptab->ctb_parts[cpt].cpt_cpumask);
-	cpu_clear(cpu, *cptab->ctb_cpumask);
+	cpumask_clear_cpu(cpu, cptab->ctb_parts[cpt].cpt_cpumask);
+	cpumask_clear_cpu(cpu, cptab->ctb_cpumask);
 	cptab->ctb_cpu2cpt[cpu] = -1;
 
 	node = cpu_to_node(cpu);
@@ -356,22 +358,22 @@ cfs_cpt_unset_cpu(struct cfs_cpt_table *cptab, int cpt, int cpu)
 	LASSERT(node_isset(node, *cptab->ctb_parts[cpt].cpt_nodemask));
 	LASSERT(node_isset(node, *cptab->ctb_nodemask));
 
-	for_each_cpu_mask(i, *cptab->ctb_parts[cpt].cpt_cpumask) {
+	for_each_cpu(i, cptab->ctb_parts[cpt].cpt_cpumask) {
 		/* this CPT has other CPU belonging to this node? */
 		if (cpu_to_node(i) == node)
 			break;
 	}
 
-	if (i == NR_CPUS)
+	if (i >= nr_cpu_ids)
 		node_clear(node, *cptab->ctb_parts[cpt].cpt_nodemask);
 
-	for_each_cpu_mask(i, *cptab->ctb_cpumask) {
+	for_each_cpu(i, cptab->ctb_cpumask) {
 		/* this CPT-table has other CPU belonging to this node? */
 		if (cpu_to_node(i) == node)
 			break;
 	}
 
-	if (i == NR_CPUS)
+	if (i >= nr_cpu_ids)
 		node_clear(node, *cptab->ctb_nodemask);
 
 	return;
@@ -383,13 +385,14 @@ cfs_cpt_set_cpumask(struct cfs_cpt_table *cptab, int cpt, cpumask_t *mask)
 {
 	int	i;
 
-	if (cpus_weight(*mask) == 0 || any_online_cpu(*mask) == NR_CPUS) {
+	if (cpumask_weight(mask) == 0 ||
+	    cpumask_any_and(mask, cpu_online_mask) >= nr_cpu_ids) {
 		CDEBUG(D_INFO, "No online CPU is found in the CPU mask for CPU partition %d\n",
 		       cpt);
 		return 0;
 	}
 
-	for_each_cpu_mask(i, *mask) {
+	for_each_cpu(i, mask) {
 		if (!cfs_cpt_set_cpu(cptab, cpt, i))
 			return 0;
 	}
@@ -403,7 +406,7 @@ cfs_cpt_unset_cpumask(struct cfs_cpt_table *cptab, int cpt, cpumask_t *mask)
 {
 	int	i;
 
-	for_each_cpu_mask(i, *mask)
+	for_each_cpu(i, mask)
 		cfs_cpt_unset_cpu(cptab, cpt, i);
 }
 EXPORT_SYMBOL(cfs_cpt_unset_cpumask);
@@ -493,7 +496,7 @@ cfs_cpt_clear(struct cfs_cpt_table *cptab, int cpt)
 	}
 
 	for (; cpt <= last; cpt++) {
-		for_each_cpu_mask(i, *cptab->ctb_parts[cpt].cpt_cpumask)
+		for_each_cpu(i, cptab->ctb_parts[cpt].cpt_cpumask)
 			cfs_cpt_unset_cpu(cptab, cpt, i);
 	}
 }
@@ -554,7 +557,7 @@ EXPORT_SYMBOL(cfs_cpt_current);
 int
 cfs_cpt_of_cpu(struct cfs_cpt_table *cptab, int cpu)
 {
-	LASSERT(cpu >= 0 && cpu < NR_CPUS);
+	LASSERT(cpu >= 0 && cpu < nr_cpu_ids);
 
 	return cptab->ctb_cpu2cpt[cpu];
 }
@@ -578,14 +581,14 @@ cfs_cpt_bind(struct cfs_cpt_table *cptab, int cpt)
 		nodemask = cptab->ctb_parts[cpt].cpt_nodemask;
 	}
 
-	if (any_online_cpu(*cpumask) == NR_CPUS) {
+	if (cpumask_any_and(cpumask, cpu_online_mask) >= nr_cpu_ids) {
 		CERROR("No online CPU found in CPU partition %d, did someone do CPU hotplug on system? You might need to reload Lustre modules to keep system working well.\n",
 		       cpt);
 		return -EINVAL;
 	}
 
 	for_each_online_cpu(i) {
-		if (cpu_isset(i, *cpumask))
+		if (cpumask_test_cpu(i, cpumask))
 			continue;
 
 		rc = set_cpus_allowed_ptr(current, cpumask);
@@ -616,14 +619,14 @@ cfs_cpt_choose_ncpus(struct cfs_cpt_table *cptab, int cpt,
 
 	LASSERT(number > 0);
 
-	if (number >= cpus_weight(*node)) {
-		while (!cpus_empty(*node)) {
-			cpu = first_cpu(*node);
+	if (number >= cpumask_weight(node)) {
+		while (!cpumask_empty(node)) {
+			cpu = cpumask_first(node);
 
 			rc = cfs_cpt_set_cpu(cptab, cpt, cpu);
 			if (!rc)
 				return -EINVAL;
-			cpu_clear(cpu, *node);
+			cpumask_clear_cpu(cpu, node);
 		}
 		return 0;
 	}
@@ -636,27 +639,27 @@ cfs_cpt_choose_ncpus(struct cfs_cpt_table *cptab, int cpt,
 		goto out;
 	}
 
-	while (!cpus_empty(*node)) {
-		cpu = first_cpu(*node);
+	while (!cpumask_empty(node)) {
+		cpu = cpumask_first(node);
 
 		/* get cpumask for cores in the same socket */
 		cfs_cpu_core_siblings(cpu, socket);
-		cpus_and(*socket, *socket, *node);
+		cpumask_and(socket, socket, node);
 
-		LASSERT(!cpus_empty(*socket));
+		LASSERT(!cpumask_empty(socket));
 
-		while (!cpus_empty(*socket)) {
+		while (!cpumask_empty(socket)) {
 			int     i;
 
 			/* get cpumask for hts in the same core */
 			cfs_cpu_ht_siblings(cpu, core);
-			cpus_and(*core, *core, *node);
+			cpumask_and(core, core, node);
 
-			LASSERT(!cpus_empty(*core));
+			LASSERT(!cpumask_empty(core));
 
-			for_each_cpu_mask(i, *core) {
-				cpu_clear(i, *socket);
-				cpu_clear(i, *node);
+			for_each_cpu(i, core) {
+				cpumask_clear_cpu(i, socket);
+				cpumask_clear_cpu(i, node);
 
 				rc = cfs_cpt_set_cpu(cptab, cpt, i);
 				if (!rc) {
@@ -667,7 +670,7 @@ cfs_cpt_choose_ncpus(struct cfs_cpt_table *cptab, int cpt,
 				if (--number == 0)
 					goto out;
 			}
-			cpu = first_cpu(*socket);
+			cpu = cpumask_first(socket);
 		}
 	}
 
@@ -767,7 +770,7 @@ cfs_cpt_table_create(int ncpt)
 	for_each_online_node(i) {
 		cfs_node_to_cpumask(i, mask);
 
-		while (!cpus_empty(*mask)) {
+		while (!cpumask_empty(mask)) {
 			struct cfs_cpu_partition *part;
 			int    n;
 
@@ -776,24 +779,24 @@ cfs_cpt_table_create(int ncpt)
 
 			part = &cptab->ctb_parts[cpt];
 
-			n = num - cpus_weight(*part->cpt_cpumask);
+			n = num - cpumask_weight(part->cpt_cpumask);
 			LASSERT(n > 0);
 
 			rc = cfs_cpt_choose_ncpus(cptab, cpt, mask, n);
 			if (rc < 0)
 				goto failed;
 
-			LASSERT(num >= cpus_weight(*part->cpt_cpumask));
-			if (num == cpus_weight(*part->cpt_cpumask))
+			LASSERT(num >= cpumask_weight(part->cpt_cpumask));
+			if (num == cpumask_weight(part->cpt_cpumask))
 				cpt++;
 		}
 	}
 
 	if (cpt != ncpt ||
-	    num != cpus_weight(*cptab->ctb_parts[ncpt - 1].cpt_cpumask)) {
+	    num != cpumask_weight(cptab->ctb_parts[ncpt - 1].cpt_cpumask)) {
 		CERROR("Expect %d(%d) CPU partitions but got %d(%d), CPU hotplug/unplug while setting?\n",
 		       cptab->ctb_nparts, num, cpt,
-		       cpus_weight(*cptab->ctb_parts[ncpt - 1].cpt_cpumask));
+		       cpumask_weight(cptab->ctb_parts[ncpt - 1].cpt_cpumask));
 		goto failed;
 	}
 
@@ -845,7 +848,7 @@ cfs_cpt_table_create_pattern(char *pattern)
 		return NULL;
 	}
 
-	high = node ? MAX_NUMNODES - 1 : NR_CPUS - 1;
+	high = node ? MAX_NUMNODES - 1 : nr_cpu_ids - 1;
 
 	cptab = cfs_cpt_table_alloc(ncpt);
 	if (cptab == NULL) {
@@ -965,7 +968,8 @@ cfs_cpu_notify(struct notifier_block *self, unsigned long action, void *hcpu)
 		mutex_lock(&cpt_data.cpt_mutex);
 		/* if all HTs in a core are offline, it may break affinity */
 		cfs_cpu_ht_siblings(cpu, cpt_data.cpt_cpumask);
-		warn = any_online_cpu(*cpt_data.cpt_cpumask) >= nr_cpu_ids;
+		warn = cpumask_any_and(cpt_data.cpt_cpumask,
+				       cpu_online_mask) >= nr_cpu_ids;
 		mutex_unlock(&cpt_data.cpt_mutex);
 		CDEBUG(warn ? D_WARNING : D_INFO,
 		       "Lustre: can't support CPU plug-out well now, performance and stability could be impacted [CPU %u action: %lx]\n",
diff --git a/drivers/staging/lustre/lustre/libcfs/linux/linux-debug.c b/drivers/staging/lustre/lustre/libcfs/linux/linux-debug.c
index 12005a71aa73..4545d54f71c6 100644
--- a/drivers/staging/lustre/lustre/libcfs/linux/linux-debug.c
+++ b/drivers/staging/lustre/lustre/libcfs/linux/linux-debug.c
@@ -50,7 +50,7 @@
 #include <linux/interrupt.h>
 #include <linux/completion.h>
 #include <linux/fs.h>
-#include <asm/uaccess.h>
+#include <linux/uaccess.h>
 #include <linux/miscdevice.h>
 
 # define DEBUG_SUBSYSTEM S_LNET
diff --git a/drivers/staging/lustre/lustre/libcfs/linux/linux-module.c b/drivers/staging/lustre/lustre/libcfs/linux/linux-module.c
index a5effcd9c679..e962f89683a6 100644
--- a/drivers/staging/lustre/lustre/libcfs/linux/linux-module.c
+++ b/drivers/staging/lustre/lustre/libcfs/linux/linux-module.c
@@ -57,7 +57,7 @@ int libcfs_ioctl_getdata(char *buf, char *end, void *arg)
 		return -EINVAL;
 	}
 
-	if (hdr->ioc_len + buf >= end) {
+	if (hdr->ioc_len >= end - buf) {
 		CERROR("PORTALS: user buffer exceeds kernel buffer\n");
 		return -EINVAL;
 	}
diff --git a/drivers/staging/lustre/lustre/libcfs/linux/linux-prim.c b/drivers/staging/lustre/lustre/libcfs/linux/linux-prim.c
index 19f405e64e68..838f5f3bd6af 100644
--- a/drivers/staging/lustre/lustre/libcfs/linux/linux-prim.c
+++ b/drivers/staging/lustre/lustre/libcfs/linux/linux-prim.c
@@ -43,7 +43,7 @@
 #include "../../../include/linux/libcfs/libcfs.h"
 
 #if defined(CONFIG_KGDB)
-#include <asm/kgdb.h>
+#include <linux/kgdb.h>
 #endif
 
 /**
@@ -117,11 +117,12 @@ EXPORT_SYMBOL(cfs_timer_deadline);
 void cfs_enter_debugger(void)
 {
 #if defined(CONFIG_KGDB)
-//	BREAKPOINT();
+	/* BREAKPOINT(); */
 #else
 	/* nothing */
 #endif
 }
+EXPORT_SYMBOL(cfs_enter_debugger);
 
 
 sigset_t
@@ -138,6 +139,7 @@ cfs_block_allsigs(void)
 
 	return old;
 }
+EXPORT_SYMBOL(cfs_block_allsigs);
 
 sigset_t cfs_block_sigs(unsigned long sigs)
 {
@@ -151,6 +153,7 @@ sigset_t cfs_block_sigs(unsigned long sigs)
 	spin_unlock_irqrestore(&current->sighand->siglock, flags);
 	return old;
 }
+EXPORT_SYMBOL(cfs_block_sigs);
 
 /* Block all signals except for the @sigs */
 sigset_t cfs_block_sigsinv(unsigned long sigs)
@@ -166,9 +169,10 @@ sigset_t cfs_block_sigsinv(unsigned long sigs)
 
 	return old;
 }
+EXPORT_SYMBOL(cfs_block_sigsinv);
 
 void
-cfs_restore_sigs (sigset_t old)
+cfs_restore_sigs(sigset_t old)
 {
 	unsigned long  flags;
 
@@ -177,12 +181,14 @@ cfs_restore_sigs (sigset_t old)
 	recalc_sigpending();
 	spin_unlock_irqrestore(&current->sighand->siglock, flags);
 }
+EXPORT_SYMBOL(cfs_restore_sigs);
 
 int
 cfs_signal_pending(void)
 {
 	return signal_pending(current);
 }
+EXPORT_SYMBOL(cfs_signal_pending);
 
 void
 cfs_clear_sigpending(void)
@@ -193,25 +199,19 @@ cfs_clear_sigpending(void)
 	clear_tsk_thread_flag(current, TIF_SIGPENDING);
 	spin_unlock_irqrestore(&current->sighand->siglock, flags);
 }
+EXPORT_SYMBOL(cfs_clear_sigpending);
 
 int
 libcfs_arch_init(void)
 {
 	return 0;
 }
+EXPORT_SYMBOL(libcfs_arch_init);
 
 void
 libcfs_arch_cleanup(void)
 {
 	return;
 }
-
-EXPORT_SYMBOL(libcfs_arch_init);
 EXPORT_SYMBOL(libcfs_arch_cleanup);
-EXPORT_SYMBOL(cfs_enter_debugger);
-EXPORT_SYMBOL(cfs_block_allsigs);
-EXPORT_SYMBOL(cfs_block_sigs);
-EXPORT_SYMBOL(cfs_block_sigsinv);
-EXPORT_SYMBOL(cfs_restore_sigs);
-EXPORT_SYMBOL(cfs_signal_pending);
-EXPORT_SYMBOL(cfs_clear_sigpending);
+
diff --git a/drivers/staging/lustre/lustre/libcfs/linux/linux-proc.c b/drivers/staging/lustre/lustre/libcfs/linux/linux-proc.c
deleted file mode 100644
index c539e3741583..000000000000
--- a/drivers/staging/lustre/lustre/libcfs/linux/linux-proc.c
+++ /dev/null
@@ -1,577 +0,0 @@
-/*
- * GPL HEADER START
- *
- * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 only,
- * as published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * General Public License version 2 for more details (a copy is included
- * in the LICENSE file that accompanied this code).
- *
- * You should have received a copy of the GNU General Public License
- * version 2 along with this program; If not, see
- * http://www.sun.com/software/products/lustre/docs/GPLv2.pdf
- *
- * Please contact Sun Microsystems, Inc., 4150 Network Circle, Santa Clara,
- * CA 95054 USA or visit www.sun.com if you need additional information or
- * have any questions.
- *
- * GPL HEADER END
- */
-/*
- * Copyright (c) 2008, 2010, Oracle and/or its affiliates. All rights reserved.
- * Use is subject to license terms.
- *
- * Copyright (c) 2011, 2012, Intel Corporation.
- */
-/*
- * This file is part of Lustre, http://www.lustre.org/
- * Lustre is a trademark of Sun Microsystems, Inc.
- *
- * libcfs/libcfs/linux/linux-proc.c
- *
- * Author: Zach Brown <zab@zabbo.net>
- * Author: Peter J. Braam <braam@clusterfs.com>
- * Author: Phil Schwan <phil@clusterfs.com>
- */
-
-#include <linux/module.h>
-#include <linux/kernel.h>
-#include <linux/mm.h>
-#include <linux/string.h>
-#include <linux/stat.h>
-#include <linux/errno.h>
-#include <linux/unistd.h>
-#include <net/sock.h>
-#include <linux/uio.h>
-
-#include <asm/uaccess.h>
-
-#include <linux/fs.h>
-#include <linux/file.h>
-#include <linux/list.h>
-
-#include <linux/proc_fs.h>
-#include <linux/sysctl.h>
-
-# define DEBUG_SUBSYSTEM S_LNET
-
-#include "../../../include/linux/libcfs/libcfs.h"
-#include <asm/div64.h>
-#include "../tracefile.h"
-
-static struct ctl_table_header *lnet_table_header = NULL;
-extern char lnet_upcall[1024];
-/**
- * The path of debug log dump upcall script.
- */
-extern char lnet_debug_log_upcall[1024];
-
-#define CTL_LNET	(0x100)
-enum {
-	PSDEV_DEBUG = 1,	  /* control debugging */
-	PSDEV_SUBSYSTEM_DEBUG,    /* control debugging */
-	PSDEV_PRINTK,	     /* force all messages to console */
-	PSDEV_CONSOLE_RATELIMIT,  /* ratelimit console messages */
-	PSDEV_CONSOLE_MAX_DELAY_CS, /* maximum delay over which we skip messages */
-	PSDEV_CONSOLE_MIN_DELAY_CS, /* initial delay over which we skip messages */
-	PSDEV_CONSOLE_BACKOFF,    /* delay increase factor */
-	PSDEV_DEBUG_PATH,	 /* crashdump log location */
-	PSDEV_DEBUG_DUMP_PATH,    /* crashdump tracelog location */
-	PSDEV_CPT_TABLE,	  /* information about cpu partitions */
-	PSDEV_LNET_UPCALL,	/* User mode upcall script  */
-	PSDEV_LNET_MEMUSED,       /* bytes currently PORTAL_ALLOCated */
-	PSDEV_LNET_CATASTROPHE,   /* if we have LBUGged or panic'd */
-	PSDEV_LNET_PANIC_ON_LBUG, /* flag to panic on LBUG */
-	PSDEV_LNET_DUMP_KERNEL,   /* snapshot kernel debug buffer to file */
-	PSDEV_LNET_DAEMON_FILE,   /* spool kernel debug buffer to file */
-	PSDEV_LNET_DEBUG_MB,      /* size of debug buffer */
-	PSDEV_LNET_DEBUG_LOG_UPCALL, /* debug log upcall script */
-	PSDEV_LNET_WATCHDOG_RATELIMIT,  /* ratelimit watchdog messages  */
-	PSDEV_LNET_FORCE_LBUG,    /* hook to force an LBUG */
-	PSDEV_LNET_FAIL_LOC,      /* control test failures instrumentation */
-	PSDEV_LNET_FAIL_VAL,      /* userdata for fail loc */
-};
-
-static int proc_call_handler(void *data, int write, loff_t *ppos,
-		void __user *buffer, size_t *lenp,
-		int (*handler)(void *data, int write,
-		loff_t pos, void __user *buffer, int len))
-{
-	int rc = handler(data, write, *ppos, buffer, *lenp);
-
-	if (rc < 0)
-		return rc;
-
-	if (write) {
-		*ppos += *lenp;
-	} else {
-		*lenp = rc;
-		*ppos += rc;
-	}
-	return 0;
-}
-
-static int __proc_dobitmasks(void *data, int write,
-			     loff_t pos, void __user *buffer, int nob)
-{
-	const int     tmpstrlen = 512;
-	char	 *tmpstr;
-	int	   rc;
-	unsigned int *mask = data;
-	int	   is_subsys = (mask == &libcfs_subsystem_debug) ? 1 : 0;
-	int	   is_printk = (mask == &libcfs_printk) ? 1 : 0;
-
-	rc = cfs_trace_allocate_string_buffer(&tmpstr, tmpstrlen);
-	if (rc < 0)
-		return rc;
-
-	if (!write) {
-		libcfs_debug_mask2str(tmpstr, tmpstrlen, *mask, is_subsys);
-		rc = strlen(tmpstr);
-
-		if (pos >= rc) {
-			rc = 0;
-		} else {
-			rc = cfs_trace_copyout_string(buffer, nob,
-						      tmpstr + pos, "\n");
-		}
-	} else {
-		rc = cfs_trace_copyin_string(tmpstr, tmpstrlen, buffer, nob);
-		if (rc < 0) {
-			cfs_trace_free_string_buffer(tmpstr, tmpstrlen);
-			return rc;
-		}
-
-		rc = libcfs_debug_str2mask(mask, tmpstr, is_subsys);
-		/* Always print LBUG/LASSERT to console, so keep this mask */
-		if (is_printk)
-			*mask |= D_EMERG;
-	}
-
-	cfs_trace_free_string_buffer(tmpstr, tmpstrlen);
-	return rc;
-}
-
-static int proc_dobitmasks(struct ctl_table *table, int write,
-			   void __user *buffer, size_t *lenp, loff_t *ppos)
-{
-	return proc_call_handler(table->data, write, ppos, buffer, lenp,
-				 __proc_dobitmasks);
-}
-
-static int min_watchdog_ratelimit = 0;	  /* disable ratelimiting */
-static int max_watchdog_ratelimit = (24*60*60); /* limit to once per day */
-
-static int __proc_dump_kernel(void *data, int write,
-			      loff_t pos, void __user *buffer, int nob)
-{
-	if (!write)
-		return 0;
-
-	return cfs_trace_dump_debug_buffer_usrstr(buffer, nob);
-}
-
-static int proc_dump_kernel(struct ctl_table *table, int write,
-			    void __user *buffer, size_t *lenp, loff_t *ppos)
-{
-	return proc_call_handler(table->data, write, ppos, buffer, lenp,
-				 __proc_dump_kernel);
-}
-
-static int __proc_daemon_file(void *data, int write,
-			      loff_t pos, void __user *buffer, int nob)
-{
-	if (!write) {
-		int len = strlen(cfs_tracefile);
-
-		if (pos >= len)
-			return 0;
-
-		return cfs_trace_copyout_string(buffer, nob,
-						cfs_tracefile + pos, "\n");
-	}
-
-	return cfs_trace_daemon_command_usrstr(buffer, nob);
-}
-
-static int proc_daemon_file(struct ctl_table *table, int write,
-			    void __user *buffer, size_t *lenp, loff_t *ppos)
-{
-	return proc_call_handler(table->data, write, ppos, buffer, lenp,
-				 __proc_daemon_file);
-}
-
-static int __proc_debug_mb(void *data, int write,
-			   loff_t pos, void __user *buffer, int nob)
-{
-	if (!write) {
-		char tmpstr[32];
-		int  len = snprintf(tmpstr, sizeof(tmpstr), "%d",
-				    cfs_trace_get_debug_mb());
-
-		if (pos >= len)
-			return 0;
-
-		return cfs_trace_copyout_string(buffer, nob, tmpstr + pos,
-		       "\n");
-	}
-
-	return cfs_trace_set_debug_mb_usrstr(buffer, nob);
-}
-
-static int proc_debug_mb(struct ctl_table *table, int write,
-			 void __user *buffer, size_t *lenp, loff_t *ppos)
-{
-	return proc_call_handler(table->data, write, ppos, buffer, lenp,
-				 __proc_debug_mb);
-}
-
-static int proc_console_max_delay_cs(struct ctl_table *table, int write,
-				     void __user *buffer, size_t *lenp,
-				     loff_t *ppos)
-{
-	int rc, max_delay_cs;
-	struct ctl_table dummy = *table;
-	long d;
-
-	dummy.data = &max_delay_cs;
-	dummy.proc_handler = &proc_dointvec;
-
-	if (!write) { /* read */
-		max_delay_cs = cfs_duration_sec(libcfs_console_max_delay * 100);
-		rc = proc_dointvec(&dummy, write, buffer, lenp, ppos);
-		return rc;
-	}
-
-	/* write */
-	max_delay_cs = 0;
-	rc = proc_dointvec(&dummy, write, buffer, lenp, ppos);
-	if (rc < 0)
-		return rc;
-	if (max_delay_cs <= 0)
-		return -EINVAL;
-
-	d = cfs_time_seconds(max_delay_cs) / 100;
-	if (d == 0 || d < libcfs_console_min_delay)
-		return -EINVAL;
-	libcfs_console_max_delay = d;
-
-	return rc;
-}
-
-static int proc_console_min_delay_cs(struct ctl_table *table, int write,
-				     void __user *buffer, size_t *lenp,
-				     loff_t *ppos)
-{
-	int rc, min_delay_cs;
-	struct ctl_table dummy = *table;
-	long d;
-
-	dummy.data = &min_delay_cs;
-	dummy.proc_handler = &proc_dointvec;
-
-	if (!write) { /* read */
-		min_delay_cs = cfs_duration_sec(libcfs_console_min_delay * 100);
-		rc = proc_dointvec(&dummy, write, buffer, lenp, ppos);
-		return rc;
-	}
-
-	/* write */
-	min_delay_cs = 0;
-	rc = proc_dointvec(&dummy, write, buffer, lenp, ppos);
-	if (rc < 0)
-		return rc;
-	if (min_delay_cs <= 0)
-		return -EINVAL;
-
-	d = cfs_time_seconds(min_delay_cs) / 100;
-	if (d == 0 || d > libcfs_console_max_delay)
-		return -EINVAL;
-	libcfs_console_min_delay = d;
-
-	return rc;
-}
-
-static int proc_console_backoff(struct ctl_table *table, int write,
-				void __user *buffer, size_t *lenp, loff_t *ppos)
-{
-	int rc, backoff;
-	struct ctl_table dummy = *table;
-
-	dummy.data = &backoff;
-	dummy.proc_handler = &proc_dointvec;
-
-	if (!write) { /* read */
-		backoff= libcfs_console_backoff;
-		rc = proc_dointvec(&dummy, write, buffer, lenp, ppos);
-		return rc;
-	}
-
-	/* write */
-	backoff = 0;
-	rc = proc_dointvec(&dummy, write, buffer, lenp, ppos);
-	if (rc < 0)
-		return rc;
-	if (backoff <= 0)
-		return -EINVAL;
-
-	libcfs_console_backoff = backoff;
-
-	return rc;
-}
-
-static int libcfs_force_lbug(struct ctl_table *table, int write,
-			     void __user *buffer,
-			     size_t *lenp, loff_t *ppos)
-{
-	if (write)
-		LBUG();
-	return 0;
-}
-
-static int proc_fail_loc(struct ctl_table *table, int write,
-			 void __user *buffer,
-			 size_t *lenp, loff_t *ppos)
-{
-	int rc;
-	long old_fail_loc = cfs_fail_loc;
-
-	rc = proc_doulongvec_minmax(table, write, buffer, lenp, ppos);
-	if (old_fail_loc != cfs_fail_loc)
-		wake_up(&cfs_race_waitq);
-	return rc;
-}
-
-static int __proc_cpt_table(void *data, int write,
-			    loff_t pos, void __user *buffer, int nob)
-{
-	char *buf = NULL;
-	int   len = 4096;
-	int   rc  = 0;
-
-	if (write)
-		return -EPERM;
-
-	LASSERT(cfs_cpt_table != NULL);
-
-	while (1) {
-		LIBCFS_ALLOC(buf, len);
-		if (buf == NULL)
-			return -ENOMEM;
-
-		rc = cfs_cpt_table_print(cfs_cpt_table, buf, len);
-		if (rc >= 0)
-			break;
-
-		if (rc == -EFBIG) {
-			LIBCFS_FREE(buf, len);
-			len <<= 1;
-			continue;
-		}
-		goto out;
-	}
-
-	if (pos >= rc) {
-		rc = 0;
-		goto out;
-	}
-
-	rc = cfs_trace_copyout_string(buffer, nob, buf + pos, NULL);
- out:
-	if (buf != NULL)
-		LIBCFS_FREE(buf, len);
-	return rc;
-}
-
-static int proc_cpt_table(struct ctl_table *table, int write,
-			   void __user *buffer, size_t *lenp, loff_t *ppos)
-{
-	return proc_call_handler(table->data, write, ppos, buffer, lenp,
-				 __proc_cpt_table);
-}
-
-static struct ctl_table lnet_table[] = {
-	/*
-	 * NB No .strategy entries have been provided since sysctl(8) prefers
-	 * to go via /proc for portability.
-	 */
-	{
-		.procname = "debug",
-		.data     = &libcfs_debug,
-		.maxlen   = sizeof(int),
-		.mode     = 0644,
-		.proc_handler = &proc_dobitmasks,
-	},
-	{
-		.procname = "subsystem_debug",
-		.data     = &libcfs_subsystem_debug,
-		.maxlen   = sizeof(int),
-		.mode     = 0644,
-		.proc_handler = &proc_dobitmasks,
-	},
-	{
-		.procname = "printk",
-		.data     = &libcfs_printk,
-		.maxlen   = sizeof(int),
-		.mode     = 0644,
-		.proc_handler = &proc_dobitmasks,
-	},
-	{
-		.procname = "console_ratelimit",
-		.data     = &libcfs_console_ratelimit,
-		.maxlen   = sizeof(int),
-		.mode     = 0644,
-		.proc_handler = &proc_dointvec
-	},
-	{
-		.procname = "console_max_delay_centisecs",
-		.maxlen   = sizeof(int),
-		.mode     = 0644,
-		.proc_handler = &proc_console_max_delay_cs
-	},
-	{
-		.procname = "console_min_delay_centisecs",
-		.maxlen   = sizeof(int),
-		.mode     = 0644,
-		.proc_handler = &proc_console_min_delay_cs
-	},
-	{
-		.procname = "console_backoff",
-		.maxlen   = sizeof(int),
-		.mode     = 0644,
-		.proc_handler = &proc_console_backoff
-	},
-
-	{
-		.procname = "debug_path",
-		.data     = libcfs_debug_file_path_arr,
-		.maxlen   = sizeof(libcfs_debug_file_path_arr),
-		.mode     = 0644,
-		.proc_handler = &proc_dostring,
-	},
-
-	{
-		.procname = "cpu_partition_table",
-		.maxlen   = 128,
-		.mode     = 0444,
-		.proc_handler = &proc_cpt_table,
-	},
-
-	{
-		.procname = "upcall",
-		.data     = lnet_upcall,
-		.maxlen   = sizeof(lnet_upcall),
-		.mode     = 0644,
-		.proc_handler = &proc_dostring,
-	},
-	{
-		.procname = "debug_log_upcall",
-		.data     = lnet_debug_log_upcall,
-		.maxlen   = sizeof(lnet_debug_log_upcall),
-		.mode     = 0644,
-		.proc_handler = &proc_dostring,
-	},
-	{
-		.procname = "lnet_memused",
-		.data     = (int *)&libcfs_kmemory.counter,
-		.maxlen   = sizeof(int),
-		.mode     = 0444,
-		.proc_handler = &proc_dointvec,
-	},
-	{
-		.procname = "catastrophe",
-		.data     = &libcfs_catastrophe,
-		.maxlen   = sizeof(int),
-		.mode     = 0444,
-		.proc_handler = &proc_dointvec,
-	},
-	{
-		.procname = "panic_on_lbug",
-		.data     = &libcfs_panic_on_lbug,
-		.maxlen   = sizeof(int),
-		.mode     = 0644,
-		.proc_handler = &proc_dointvec,
-	},
-	{
-		.procname = "dump_kernel",
-		.maxlen   = 256,
-		.mode     = 0200,
-		.proc_handler = &proc_dump_kernel,
-	},
-	{
-		.procname = "daemon_file",
-		.mode     = 0644,
-		.maxlen   = 256,
-		.proc_handler = &proc_daemon_file,
-	},
-	{
-		.procname = "debug_mb",
-		.mode     = 0644,
-		.proc_handler = &proc_debug_mb,
-	},
-	{
-		.procname = "watchdog_ratelimit",
-		.data     = &libcfs_watchdog_ratelimit,
-		.maxlen   = sizeof(int),
-		.mode     = 0644,
-		.proc_handler = &proc_dointvec_minmax,
-		.extra1   = &min_watchdog_ratelimit,
-		.extra2   = &max_watchdog_ratelimit,
-	},
-	{
-		.procname = "force_lbug",
-		.data     = NULL,
-		.maxlen   = 0,
-		.mode     = 0200,
-		.proc_handler = &libcfs_force_lbug
-	},
-	{
-		.procname = "fail_loc",
-		.data     = &cfs_fail_loc,
-		.maxlen   = sizeof(cfs_fail_loc),
-		.mode     = 0644,
-		.proc_handler = &proc_fail_loc
-	},
-	{
-		.procname = "fail_val",
-		.data     = &cfs_fail_val,
-		.maxlen   = sizeof(int),
-		.mode     = 0644,
-		.proc_handler = &proc_dointvec
-	},
-	{
-	}
-};
-
-static struct ctl_table top_table[] = {
-	{
-		.procname = "lnet",
-		.mode     = 0555,
-		.data     = NULL,
-		.maxlen   = 0,
-		.child    = lnet_table,
-	},
-	{
-	}
-};
-
-int insert_proc(void)
-{
-	if (lnet_table_header == NULL)
-		lnet_table_header = register_sysctl_table(top_table);
-	return 0;
-}
-
-void remove_proc(void)
-{
-	if (lnet_table_header != NULL)
-		unregister_sysctl_table(lnet_table_header);
-
-	lnet_table_header = NULL;
-}
diff --git a/drivers/staging/lustre/lustre/libcfs/linux/linux-tcpip.c b/drivers/staging/lustre/lustre/libcfs/linux/linux-tcpip.c
index cd2fc01dea4c..f2462e7f04bc 100644
--- a/drivers/staging/lustre/lustre/libcfs/linux/linux-tcpip.c
+++ b/drivers/staging/lustre/lustre/libcfs/linux/linux-tcpip.c
@@ -543,19 +543,17 @@ libcfs_sock_accept (struct socket **newsockp, struct socket *sock)
 
 	newsock->ops = sock->ops;
 
-	set_current_state(TASK_INTERRUPTIBLE);
-	add_wait_queue(sk_sleep(sock->sk), &wait);
-
 	rc = sock->ops->accept(sock, newsock, O_NONBLOCK);
 	if (rc == -EAGAIN) {
 		/* Nothing ready, so wait for activity */
+		set_current_state(TASK_INTERRUPTIBLE);
+		add_wait_queue(sk_sleep(sock->sk), &wait);
 		schedule();
+		remove_wait_queue(sk_sleep(sock->sk), &wait);
+		set_current_state(TASK_RUNNING);
 		rc = sock->ops->accept(sock, newsock, O_NONBLOCK);
 	}
 
-	remove_wait_queue(sk_sleep(sock->sk), &wait);
-	set_current_state(TASK_RUNNING);
-
 	if (rc != 0)
 		goto failed;
 
diff --git a/drivers/staging/lustre/lustre/libcfs/module.c b/drivers/staging/lustre/lustre/libcfs/module.c
index 7dc77dd402b8..f0ee76abfd5a 100644
--- a/drivers/staging/lustre/lustre/libcfs/module.c
+++ b/drivers/staging/lustre/lustre/libcfs/module.c
@@ -33,15 +33,82 @@
  * This file is part of Lustre, http://www.lustre.org/
  * Lustre is a trademark of Sun Microsystems, Inc.
  */
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/mm.h>
+#include <linux/string.h>
+#include <linux/stat.h>
+#include <linux/errno.h>
+#include <linux/unistd.h>
+#include <net/sock.h>
+#include <linux/uio.h>
 
-#define DEBUG_SUBSYSTEM S_LNET
+#include <linux/uaccess.h>
+
+#include <linux/fs.h>
+#include <linux/file.h>
+#include <linux/list.h>
+
+#include <linux/proc_fs.h>
+#include <linux/sysctl.h>
+
+# define DEBUG_SUBSYSTEM S_LNET
 
 #include "../../include/linux/libcfs/libcfs.h"
+#include <asm/div64.h>
+
 #include "../../include/linux/libcfs/libcfs_crypto.h"
 #include "../../include/linux/lnet/lib-lnet.h"
 #include "../../include/linux/lnet/lnet.h"
 #include "tracefile.h"
 
+MODULE_AUTHOR("Peter J. Braam <braam@clusterfs.com>");
+MODULE_DESCRIPTION("Portals v3.1");
+MODULE_LICENSE("GPL");
+
+extern struct miscdevice libcfs_dev;
+extern struct rw_semaphore cfs_tracefile_sem;
+extern struct mutex cfs_trace_thread_mutex;
+extern struct cfs_wi_sched *cfs_sched_rehash;
+extern void libcfs_init_nidstrings(void);
+
+static int insert_proc(void);
+static void remove_proc(void);
+
+static struct ctl_table_header *lnet_table_header;
+extern char lnet_upcall[1024];
+/**
+ * The path of debug log dump upcall script.
+ */
+extern char lnet_debug_log_upcall[1024];
+
+#define CTL_LNET	(0x100)
+
+enum {
+	PSDEV_DEBUG = 1,	  /* control debugging */
+	PSDEV_SUBSYSTEM_DEBUG,    /* control debugging */
+	PSDEV_PRINTK,	     /* force all messages to console */
+	PSDEV_CONSOLE_RATELIMIT,  /* ratelimit console messages */
+	PSDEV_CONSOLE_MAX_DELAY_CS, /* maximum delay over which we skip messages */
+	PSDEV_CONSOLE_MIN_DELAY_CS, /* initial delay over which we skip messages */
+	PSDEV_CONSOLE_BACKOFF,    /* delay increase factor */
+	PSDEV_DEBUG_PATH,	 /* crashdump log location */
+	PSDEV_DEBUG_DUMP_PATH,    /* crashdump tracelog location */
+	PSDEV_CPT_TABLE,	  /* information about cpu partitions */
+	PSDEV_LNET_UPCALL,	/* User mode upcall script  */
+	PSDEV_LNET_MEMUSED,       /* bytes currently PORTAL_ALLOCated */
+	PSDEV_LNET_CATASTROPHE,   /* if we have LBUGged or panic'd */
+	PSDEV_LNET_PANIC_ON_LBUG, /* flag to panic on LBUG */
+	PSDEV_LNET_DUMP_KERNEL,   /* snapshot kernel debug buffer to file */
+	PSDEV_LNET_DAEMON_FILE,   /* spool kernel debug buffer to file */
+	PSDEV_LNET_DEBUG_MB,      /* size of debug buffer */
+	PSDEV_LNET_DEBUG_LOG_UPCALL, /* debug log upcall script */
+	PSDEV_LNET_WATCHDOG_RATELIMIT,  /* ratelimit watchdog messages  */
+	PSDEV_LNET_FORCE_LBUG,    /* hook to force an LBUG */
+	PSDEV_LNET_FAIL_LOC,      /* control test failures instrumentation */
+	PSDEV_LNET_FAIL_VAL,      /* userdata for fail loc */
+};
+
 static void kportal_memhog_free (struct libcfs_device_userstate *ldu)
 {
 	struct page **level0p = &ldu->ldu_memhog_root_page;
@@ -320,19 +387,6 @@ struct cfs_psdev_ops libcfs_psdev_ops = {
 	libcfs_ioctl
 };
 
-extern int insert_proc(void);
-extern void remove_proc(void);
-MODULE_AUTHOR("Peter J. Braam <braam@clusterfs.com>");
-MODULE_DESCRIPTION("Portals v3.1");
-MODULE_LICENSE("GPL");
-
-extern struct miscdevice libcfs_dev;
-extern struct rw_semaphore cfs_tracefile_sem;
-extern struct mutex cfs_trace_thread_mutex;
-extern struct cfs_wi_sched *cfs_sched_rehash;
-
-extern void libcfs_init_nidstrings(void);
-
 static int init_libcfs_module(void)
 {
 	int rc;
@@ -347,7 +401,7 @@ static int init_libcfs_module(void)
 
 	rc = libcfs_debug_init(5 * 1024 * 1024);
 	if (rc < 0) {
-		printk(KERN_ERR "LustreError: libcfs_debug_init: %d\n", rc);
+		pr_err("LustreError: libcfs_debug_init: %d\n", rc);
 		return rc;
 	}
 
@@ -433,12 +487,490 @@ static void exit_libcfs_module(void)
 
 	rc = libcfs_debug_cleanup();
 	if (rc)
-		printk(KERN_ERR "LustreError: libcfs_debug_cleanup: %d\n",
-		       rc);
+		pr_err("LustreError: libcfs_debug_cleanup: %d\n", rc);
 
 	libcfs_arch_cleanup();
 }
 
+static int proc_call_handler(void *data, int write, loff_t *ppos,
+		void __user *buffer, size_t *lenp,
+		int (*handler)(void *data, int write,
+		loff_t pos, void __user *buffer, int len))
+{
+	int rc = handler(data, write, *ppos, buffer, *lenp);
+
+	if (rc < 0)
+		return rc;
+
+	if (write) {
+		*ppos += *lenp;
+	} else {
+		*lenp = rc;
+		*ppos += rc;
+	}
+	return 0;
+}
+
+static int __proc_dobitmasks(void *data, int write,
+			     loff_t pos, void __user *buffer, int nob)
+{
+	const int     tmpstrlen = 512;
+	char	 *tmpstr;
+	int	   rc;
+	unsigned int *mask = data;
+	int	   is_subsys = (mask == &libcfs_subsystem_debug) ? 1 : 0;
+	int	   is_printk = (mask == &libcfs_printk) ? 1 : 0;
+
+	rc = cfs_trace_allocate_string_buffer(&tmpstr, tmpstrlen);
+	if (rc < 0)
+		return rc;
+
+	if (!write) {
+		libcfs_debug_mask2str(tmpstr, tmpstrlen, *mask, is_subsys);
+		rc = strlen(tmpstr);
+
+		if (pos >= rc) {
+			rc = 0;
+		} else {
+			rc = cfs_trace_copyout_string(buffer, nob,
+						      tmpstr + pos, "\n");
+		}
+	} else {
+		rc = cfs_trace_copyin_string(tmpstr, tmpstrlen, buffer, nob);
+		if (rc < 0) {
+			cfs_trace_free_string_buffer(tmpstr, tmpstrlen);
+			return rc;
+		}
+
+		rc = libcfs_debug_str2mask(mask, tmpstr, is_subsys);
+		/* Always print LBUG/LASSERT to console, so keep this mask */
+		if (is_printk)
+			*mask |= D_EMERG;
+	}
+
+	cfs_trace_free_string_buffer(tmpstr, tmpstrlen);
+	return rc;
+}
+
+static int proc_dobitmasks(struct ctl_table *table, int write,
+			   void __user *buffer, size_t *lenp, loff_t *ppos)
+{
+	return proc_call_handler(table->data, write, ppos, buffer, lenp,
+				 __proc_dobitmasks);
+}
+
+static int min_watchdog_ratelimit;	  /* disable ratelimiting */
+static int max_watchdog_ratelimit = (24*60*60); /* limit to once per day */
+
+static int __proc_dump_kernel(void *data, int write,
+			      loff_t pos, void __user *buffer, int nob)
+{
+	if (!write)
+		return 0;
+
+	return cfs_trace_dump_debug_buffer_usrstr(buffer, nob);
+}
+
+static int proc_dump_kernel(struct ctl_table *table, int write,
+			    void __user *buffer, size_t *lenp, loff_t *ppos)
+{
+	return proc_call_handler(table->data, write, ppos, buffer, lenp,
+				 __proc_dump_kernel);
+}
+
+static int __proc_daemon_file(void *data, int write,
+			      loff_t pos, void __user *buffer, int nob)
+{
+	if (!write) {
+		int len = strlen(cfs_tracefile);
+
+		if (pos >= len)
+			return 0;
+
+		return cfs_trace_copyout_string(buffer, nob,
+						cfs_tracefile + pos, "\n");
+	}
+
+	return cfs_trace_daemon_command_usrstr(buffer, nob);
+}
+
+static int proc_daemon_file(struct ctl_table *table, int write,
+			    void __user *buffer, size_t *lenp, loff_t *ppos)
+{
+	return proc_call_handler(table->data, write, ppos, buffer, lenp,
+				 __proc_daemon_file);
+}
+
+static int __proc_debug_mb(void *data, int write,
+			   loff_t pos, void __user *buffer, int nob)
+{
+	if (!write) {
+		char tmpstr[32];
+		int  len = snprintf(tmpstr, sizeof(tmpstr), "%d",
+				    cfs_trace_get_debug_mb());
+
+		if (pos >= len)
+			return 0;
+
+		return cfs_trace_copyout_string(buffer, nob, tmpstr + pos,
+		       "\n");
+	}
+
+	return cfs_trace_set_debug_mb_usrstr(buffer, nob);
+}
+
+static int proc_debug_mb(struct ctl_table *table, int write,
+			 void __user *buffer, size_t *lenp, loff_t *ppos)
+{
+	return proc_call_handler(table->data, write, ppos, buffer, lenp,
+				 __proc_debug_mb);
+}
+
+static int proc_console_max_delay_cs(struct ctl_table *table, int write,
+				     void __user *buffer, size_t *lenp,
+				     loff_t *ppos)
+{
+	int rc, max_delay_cs;
+	struct ctl_table dummy = *table;
+	long d;
+
+	dummy.data = &max_delay_cs;
+	dummy.proc_handler = &proc_dointvec;
+
+	if (!write) { /* read */
+		max_delay_cs = cfs_duration_sec(libcfs_console_max_delay * 100);
+		rc = proc_dointvec(&dummy, write, buffer, lenp, ppos);
+		return rc;
+	}
+
+	/* write */
+	max_delay_cs = 0;
+	rc = proc_dointvec(&dummy, write, buffer, lenp, ppos);
+	if (rc < 0)
+		return rc;
+	if (max_delay_cs <= 0)
+		return -EINVAL;
+
+	d = cfs_time_seconds(max_delay_cs) / 100;
+	if (d == 0 || d < libcfs_console_min_delay)
+		return -EINVAL;
+	libcfs_console_max_delay = d;
+
+	return rc;
+}
+
+static int proc_console_min_delay_cs(struct ctl_table *table, int write,
+				     void __user *buffer, size_t *lenp,
+				     loff_t *ppos)
+{
+	int rc, min_delay_cs;
+	struct ctl_table dummy = *table;
+	long d;
+
+	dummy.data = &min_delay_cs;
+	dummy.proc_handler = &proc_dointvec;
+
+	if (!write) { /* read */
+		min_delay_cs = cfs_duration_sec(libcfs_console_min_delay * 100);
+		rc = proc_dointvec(&dummy, write, buffer, lenp, ppos);
+		return rc;
+	}
+
+	/* write */
+	min_delay_cs = 0;
+	rc = proc_dointvec(&dummy, write, buffer, lenp, ppos);
+	if (rc < 0)
+		return rc;
+	if (min_delay_cs <= 0)
+		return -EINVAL;
+
+	d = cfs_time_seconds(min_delay_cs) / 100;
+	if (d == 0 || d > libcfs_console_max_delay)
+		return -EINVAL;
+	libcfs_console_min_delay = d;
+
+	return rc;
+}
+
+static int proc_console_backoff(struct ctl_table *table, int write,
+				void __user *buffer, size_t *lenp, loff_t *ppos)
+{
+	int rc, backoff;
+	struct ctl_table dummy = *table;
+
+	dummy.data = &backoff;
+	dummy.proc_handler = &proc_dointvec;
+
+	if (!write) { /* read */
+		backoff = libcfs_console_backoff;
+		rc = proc_dointvec(&dummy, write, buffer, lenp, ppos);
+		return rc;
+	}
+
+	/* write */
+	backoff = 0;
+	rc = proc_dointvec(&dummy, write, buffer, lenp, ppos);
+	if (rc < 0)
+		return rc;
+	if (backoff <= 0)
+		return -EINVAL;
+
+	libcfs_console_backoff = backoff;
+
+	return rc;
+}
+
+static int libcfs_force_lbug(struct ctl_table *table, int write,
+			     void __user *buffer,
+			     size_t *lenp, loff_t *ppos)
+{
+	if (write)
+		LBUG();
+	return 0;
+}
+
+static int proc_fail_loc(struct ctl_table *table, int write,
+			 void __user *buffer,
+			 size_t *lenp, loff_t *ppos)
+{
+	int rc;
+	long old_fail_loc = cfs_fail_loc;
+
+	rc = proc_doulongvec_minmax(table, write, buffer, lenp, ppos);
+	if (old_fail_loc != cfs_fail_loc)
+		wake_up(&cfs_race_waitq);
+	return rc;
+}
+
+static int __proc_cpt_table(void *data, int write,
+			    loff_t pos, void __user *buffer, int nob)
+{
+	char *buf = NULL;
+	int   len = 4096;
+	int   rc  = 0;
+
+	if (write)
+		return -EPERM;
+
+	LASSERT(cfs_cpt_table != NULL);
+
+	while (1) {
+		LIBCFS_ALLOC(buf, len);
+		if (buf == NULL)
+			return -ENOMEM;
+
+		rc = cfs_cpt_table_print(cfs_cpt_table, buf, len);
+		if (rc >= 0)
+			break;
+
+		if (rc == -EFBIG) {
+			LIBCFS_FREE(buf, len);
+			len <<= 1;
+			continue;
+		}
+		goto out;
+	}
+
+	if (pos >= rc) {
+		rc = 0;
+		goto out;
+	}
+
+	rc = cfs_trace_copyout_string(buffer, nob, buf + pos, NULL);
+ out:
+	if (buf != NULL)
+		LIBCFS_FREE(buf, len);
+	return rc;
+}
+
+static int proc_cpt_table(struct ctl_table *table, int write,
+			   void __user *buffer, size_t *lenp, loff_t *ppos)
+{
+	return proc_call_handler(table->data, write, ppos, buffer, lenp,
+				 __proc_cpt_table);
+}
+
+static struct ctl_table lnet_table[] = {
+	/*
+	 * NB No .strategy entries have been provided since sysctl(8) prefers
+	 * to go via /proc for portability.
+	 */
+	{
+		.procname = "debug",
+		.data     = &libcfs_debug,
+		.maxlen   = sizeof(int),
+		.mode     = 0644,
+		.proc_handler = &proc_dobitmasks,
+	},
+	{
+		.procname = "subsystem_debug",
+		.data     = &libcfs_subsystem_debug,
+		.maxlen   = sizeof(int),
+		.mode     = 0644,
+		.proc_handler = &proc_dobitmasks,
+	},
+	{
+		.procname = "printk",
+		.data     = &libcfs_printk,
+		.maxlen   = sizeof(int),
+		.mode     = 0644,
+		.proc_handler = &proc_dobitmasks,
+	},
+	{
+		.procname = "console_ratelimit",
+		.data     = &libcfs_console_ratelimit,
+		.maxlen   = sizeof(int),
+		.mode     = 0644,
+		.proc_handler = &proc_dointvec
+	},
+	{
+		.procname = "console_max_delay_centisecs",
+		.maxlen   = sizeof(int),
+		.mode     = 0644,
+		.proc_handler = &proc_console_max_delay_cs
+	},
+	{
+		.procname = "console_min_delay_centisecs",
+		.maxlen   = sizeof(int),
+		.mode     = 0644,
+		.proc_handler = &proc_console_min_delay_cs
+	},
+	{
+		.procname = "console_backoff",
+		.maxlen   = sizeof(int),
+		.mode     = 0644,
+		.proc_handler = &proc_console_backoff
+	},
+
+	{
+		.procname = "debug_path",
+		.data     = libcfs_debug_file_path_arr,
+		.maxlen   = sizeof(libcfs_debug_file_path_arr),
+		.mode     = 0644,
+		.proc_handler = &proc_dostring,
+	},
+
+	{
+		.procname = "cpu_partition_table",
+		.maxlen   = 128,
+		.mode     = 0444,
+		.proc_handler = &proc_cpt_table,
+	},
+
+	{
+		.procname = "upcall",
+		.data     = lnet_upcall,
+		.maxlen   = sizeof(lnet_upcall),
+		.mode     = 0644,
+		.proc_handler = &proc_dostring,
+	},
+	{
+		.procname = "debug_log_upcall",
+		.data     = lnet_debug_log_upcall,
+		.maxlen   = sizeof(lnet_debug_log_upcall),
+		.mode     = 0644,
+		.proc_handler = &proc_dostring,
+	},
+	{
+		.procname = "lnet_memused",
+		.data     = (int *)&libcfs_kmemory.counter,
+		.maxlen   = sizeof(int),
+		.mode     = 0444,
+		.proc_handler = &proc_dointvec,
+	},
+	{
+		.procname = "catastrophe",
+		.data     = &libcfs_catastrophe,
+		.maxlen   = sizeof(int),
+		.mode     = 0444,
+		.proc_handler = &proc_dointvec,
+	},
+	{
+		.procname = "panic_on_lbug",
+		.data     = &libcfs_panic_on_lbug,
+		.maxlen   = sizeof(int),
+		.mode     = 0644,
+		.proc_handler = &proc_dointvec,
+	},
+	{
+		.procname = "dump_kernel",
+		.maxlen   = 256,
+		.mode     = 0200,
+		.proc_handler = &proc_dump_kernel,
+	},
+	{
+		.procname = "daemon_file",
+		.mode     = 0644,
+		.maxlen   = 256,
+		.proc_handler = &proc_daemon_file,
+	},
+	{
+		.procname = "debug_mb",
+		.mode     = 0644,
+		.proc_handler = &proc_debug_mb,
+	},
+	{
+		.procname = "watchdog_ratelimit",
+		.data     = &libcfs_watchdog_ratelimit,
+		.maxlen   = sizeof(int),
+		.mode     = 0644,
+		.proc_handler = &proc_dointvec_minmax,
+		.extra1   = &min_watchdog_ratelimit,
+		.extra2   = &max_watchdog_ratelimit,
+	},
+	{
+		.procname = "force_lbug",
+		.data     = NULL,
+		.maxlen   = 0,
+		.mode     = 0200,
+		.proc_handler = &libcfs_force_lbug
+	},
+	{
+		.procname = "fail_loc",
+		.data     = &cfs_fail_loc,
+		.maxlen   = sizeof(cfs_fail_loc),
+		.mode     = 0644,
+		.proc_handler = &proc_fail_loc
+	},
+	{
+		.procname = "fail_val",
+		.data     = &cfs_fail_val,
+		.maxlen   = sizeof(int),
+		.mode     = 0644,
+		.proc_handler = &proc_dointvec
+	},
+	{
+	}
+};
+
+static struct ctl_table top_table[] = {
+	{
+		.procname = "lnet",
+		.mode     = 0555,
+		.data     = NULL,
+		.maxlen   = 0,
+		.child    = lnet_table,
+	},
+	{
+	}
+};
+
+static int insert_proc(void)
+{
+	if (lnet_table_header == NULL)
+		lnet_table_header = register_sysctl_table(top_table);
+	return 0;
+}
+
+static void remove_proc(void)
+{
+	if (lnet_table_header != NULL)
+		unregister_sysctl_table(lnet_table_header);
+
+	lnet_table_header = NULL;
+}
+
 MODULE_VERSION("1.0.0");
+
 module_init(init_libcfs_module);
 module_exit(exit_libcfs_module);
diff --git a/drivers/staging/lustre/lustre/libcfs/tracefile.c b/drivers/staging/lustre/lustre/libcfs/tracefile.c
index eb65b50f832d..c86394f7f4d9 100644
--- a/drivers/staging/lustre/lustre/libcfs/tracefile.c
+++ b/drivers/staging/lustre/lustre/libcfs/tracefile.c
@@ -53,7 +53,7 @@ char cfs_tracefile[TRACEFILE_NAME_SIZE];
 long long cfs_tracefile_size = CFS_TRACEFILE_SIZE;
 static struct tracefiled_ctl trace_tctl;
 struct mutex cfs_trace_thread_mutex;
-static int thread_running = 0;
+static int thread_running;
 
 static atomic_t cfs_tage_allocated = ATOMIC_INIT(0);
 
@@ -224,8 +224,7 @@ static struct cfs_trace_page *cfs_trace_get_tage(struct cfs_trace_cpu_data *tcd,
 	 */
 
 	if (len > PAGE_CACHE_SIZE) {
-		printk(KERN_ERR
-		       "cowardly refusing to write %lu bytes in a page\n", len);
+		pr_err("cowardly refusing to write %lu bytes in a page\n", len);
 		return NULL;
 	}
 
@@ -688,8 +687,8 @@ int cfs_tracefile_dump_all_pages(char *filename)
 	if (IS_ERR(filp)) {
 		rc = PTR_ERR(filp);
 		filp = NULL;
-		printk(KERN_ERR "LustreError: can't open %s for dump: rc %d\n",
-		      filename, rc);
+		pr_err("LustreError: can't open %s for dump: rc %d\n",
+			filename, rc);
 		goto out;
 	}
 
@@ -726,7 +725,7 @@ int cfs_tracefile_dump_all_pages(char *filename)
 	MMSPACE_CLOSE;
 	rc = vfs_fsync(filp, 1);
 	if (rc)
-		printk(KERN_ERR "sync returns %d\n", rc);
+		pr_err("sync returns %d\n", rc);
 close:
 	filp_close(filp, NULL);
 out:
@@ -1048,22 +1047,21 @@ static int tracefiled(void *arg)
 			int i;
 
 			printk(KERN_ALERT "Lustre: trace pages aren't empty\n");
-			printk(KERN_ERR "total cpus(%d): ",
-			       num_possible_cpus());
+			pr_err("total cpus(%d): ",
+				num_possible_cpus());
 			for (i = 0; i < num_possible_cpus(); i++)
 				if (cpu_online(i))
-					printk(KERN_ERR "%d(on) ", i);
+					pr_cont("%d(on) ", i);
 				else
-					printk(KERN_ERR "%d(off) ", i);
-			printk(KERN_ERR "\n");
+					pr_cont("%d(off) ", i);
+			pr_cont("\n");
 
 			i = 0;
 			list_for_each_entry_safe(tage, tmp, &pc.pc_pages,
 						     linkage)
-				printk(KERN_ERR "page %d belongs to cpu %d\n",
-				       ++i, tage->cpu);
-			printk(KERN_ERR "There are %d pages unwritten\n",
-			       i);
+				pr_err("page %d belongs to cpu %d\n",
+					++i, tage->cpu);
+			pr_err("There are %d pages unwritten\n", i);
 		}
 		__LASSERT(list_empty(&pc.pc_pages));
 end_loop:
diff --git a/drivers/staging/lustre/lustre/libcfs/workitem.c b/drivers/staging/lustre/lustre/libcfs/workitem.c
index c4afaeaf8310..48009b775845 100644
--- a/drivers/staging/lustre/lustre/libcfs/workitem.c
+++ b/drivers/staging/lustre/lustre/libcfs/workitem.c
@@ -442,7 +442,7 @@ cfs_wi_startup(void)
 }
 
 void
-cfs_wi_shutdown (void)
+cfs_wi_shutdown(void)
 {
 	struct cfs_wi_sched	*sched;
 
diff --git a/drivers/staging/lustre/lustre/llite/dcache.c b/drivers/staging/lustre/lustre/llite/dcache.c
index ddf1fa9f67f8..fe1fd05423e9 100644
--- a/drivers/staging/lustre/lustre/llite/dcache.c
+++ b/drivers/staging/lustre/lustre/llite/dcache.c
@@ -128,7 +128,7 @@ static int find_cbdata(struct inode *inode)
 	rc = md_find_cbdata(sbi->ll_md_exp, ll_inode2fid(inode),
 			    return_if_equal, NULL);
 	if (rc != 0)
-		 return rc;
+		return rc;
 
 	lsm = ccc_inode_lsm_get(inode);
 	if (lsm == NULL)
@@ -339,13 +339,10 @@ static int ll_revalidate_dentry(struct dentry *dentry,
  */
 static int ll_revalidate_nd(struct dentry *dentry, unsigned int flags)
 {
-	int rc;
-
 	CDEBUG(D_VFSTRACE, "VFS Op:name=%pd, flags=%u\n",
 	       dentry, flags);
 
-	rc = ll_revalidate_dentry(dentry, flags);
-	return rc;
+	return ll_revalidate_dentry(dentry, flags);
 }
 
 
diff --git a/drivers/staging/lustre/lustre/llite/dir.c b/drivers/staging/lustre/lustre/llite/dir.c
index a18201913273..a5bc694dcb64 100644
--- a/drivers/staging/lustre/lustre/llite/dir.c
+++ b/drivers/staging/lustre/lustre/llite/dir.c
@@ -41,7 +41,7 @@
 #include <linux/fs.h>
 #include <linux/pagemap.h>
 #include <linux/mm.h>
-#include <asm/uaccess.h>
+#include <linux/uaccess.h>
 #include <linux/buffer_head.h>   /* for wait_on_buffer */
 #include <linux/pagevec.h>
 #include <linux/prefetch.h>
@@ -1518,6 +1518,7 @@ out_rmdir:
 			lump = (struct lov_user_md *)arg;
 		} else {
 			struct lov_user_mds_data *lmdp;
+
 			lmdp = (struct lov_user_mds_data *)arg;
 			lump = &lmdp->lmd_lmm;
 		}
@@ -1909,21 +1910,21 @@ static loff_t ll_dir_seek(struct file *file, loff_t offset, int origin)
 
 	mutex_lock(&inode->i_mutex);
 	switch (origin) {
-		case SEEK_SET:
-			break;
-		case SEEK_CUR:
-			offset += file->f_pos;
-			break;
-		case SEEK_END:
-			if (offset > 0)
-				goto out;
-			if (api32)
-				offset += LL_DIR_END_OFF_32BIT;
-			else
-				offset += LL_DIR_END_OFF;
-			break;
-		default:
+	case SEEK_SET:
+		break;
+	case SEEK_CUR:
+		offset += file->f_pos;
+		break;
+	case SEEK_END:
+		if (offset > 0)
 			goto out;
+		if (api32)
+			offset += LL_DIR_END_OFF_32BIT;
+		else
+			offset += LL_DIR_END_OFF;
+		break;
+	default:
+		goto out;
 	}
 
 	if (offset >= 0 &&
diff --git a/drivers/staging/lustre/lustre/llite/file.c b/drivers/staging/lustre/lustre/llite/file.c
index 5ebee6ca0a10..85e74d18d1c7 100644
--- a/drivers/staging/lustre/lustre/llite/file.c
+++ b/drivers/staging/lustre/lustre/llite/file.c
@@ -161,7 +161,7 @@ static int ll_close_inode_openhandle(struct obd_export *md_exp,
 		op_data->op_lease_handle = och->och_lease_handle;
 		op_data->op_attr.ia_valid |= ATTR_SIZE | ATTR_BLOCKS;
 	}
-	epoch_close = (op_data->op_flags & MF_EPOCH_CLOSE);
+	epoch_close = op_data->op_flags & MF_EPOCH_CLOSE;
 	rc = md_close(md_exp, op_data, och->och_mod, &req);
 	if (rc == -EAGAIN) {
 		/* This close must have the epoch closed. */
@@ -197,6 +197,7 @@ static int ll_close_inode_openhandle(struct obd_export *md_exp,
 	}
 	if (rc == 0 && op_data->op_bias & MDS_HSM_RELEASE) {
 		struct mdt_body *body;
+
 		body = req_capsule_server_get(&req->rq_pill, &RMF_MDT_BODY);
 		if (!(body->valid & OBD_MD_FLRELEASED))
 			rc = -EBUSY;
@@ -269,7 +270,7 @@ static int ll_md_close(struct obd_export *md_exp, struct inode *inode,
 	int lockmode;
 	__u64 flags = LDLM_FL_BLOCK_GRANTED | LDLM_FL_TEST_LOCK;
 	struct lustre_handle lockh;
-	ldlm_policy_data_t policy = {.l_inodebits={MDS_INODELOCK_OPEN}};
+	ldlm_policy_data_t policy = {.l_inodebits = {MDS_INODELOCK_OPEN}};
 	int rc = 0;
 
 	/* clear group lock, if present */
@@ -692,7 +693,7 @@ restart:
 out_och_free:
 	if (rc) {
 		if (och_p && *och_p) {
-			OBD_FREE(*och_p, sizeof (struct obd_client_handle));
+			OBD_FREE(*och_p, sizeof(struct obd_client_handle));
 			*och_p = NULL; /* OBD_FREE writes some magic there */
 			(*och_usecount)--;
 		}
@@ -1711,6 +1712,12 @@ static int ll_do_fiemap(struct inode *inode, struct ll_user_fiemap *fiemap,
 	fm_key.oa.o_oi = lsm->lsm_oi;
 	fm_key.oa.o_valid = OBD_MD_FLID | OBD_MD_FLGROUP;
 
+	if (i_size_read(inode) == 0) {
+		rc = ll_glimpse_size(inode);
+		if (rc)
+			goto out;
+	}
+
 	obdo_from_inode(&fm_key.oa, inode, OBD_MD_FLSIZE);
 	obdo_set_parent_fid(&fm_key.oa, &ll_i2info(inode)->lli_fid);
 	/* If filesize is 0, then there would be no objects for mapping */
@@ -2821,7 +2828,7 @@ int ll_have_md_lock(struct inode *inode, __u64 *bits,  ldlm_mode_t l_req_mode)
 	int i;
 
 	if (!inode)
-	       return 0;
+		return 0;
 
 	fid = &ll_i2info(inode)->lli_fid;
 	CDEBUG(D_INFO, "trying to match res "DFID" mode %s\n", PFID(fid),
@@ -3233,6 +3240,7 @@ void *ll_iocontrol_register(llioc_callback_t cb, int count, unsigned int *cmd)
 
 	return in_data;
 }
+EXPORT_SYMBOL(ll_iocontrol_register);
 
 void ll_iocontrol_unregister(void *magic)
 {
@@ -3257,8 +3265,6 @@ void ll_iocontrol_unregister(void *magic)
 
 	CWARN("didn't find iocontrol register block with magic: %p\n", magic);
 }
-
-EXPORT_SYMBOL(ll_iocontrol_register);
 EXPORT_SYMBOL(ll_iocontrol_unregister);
 
 static enum llioc_iter
diff --git a/drivers/staging/lustre/lustre/llite/llite_close.c b/drivers/staging/lustre/lustre/llite/llite_close.c
index 21b4a5026776..a94ba02ccf02 100644
--- a/drivers/staging/lustre/lustre/llite/llite_close.c
+++ b/drivers/staging/lustre/lustre/llite/llite_close.c
@@ -284,10 +284,8 @@ static void ll_done_writing(struct inode *inode)
 	LASSERT(exp_connect_som(ll_i2mdexp(inode)));
 
 	op_data = kzalloc(sizeof(*op_data), GFP_NOFS);
-	if (!op_data) {
-		CERROR("can't allocate op_data\n");
+	if (!op_data)
 		return;
-	}
 
 	ll_prepare_done_writing(inode, op_data, &och);
 	/* If there is no @och, we do not do D_W yet. */
diff --git a/drivers/staging/lustre/lustre/llite/llite_lib.c b/drivers/staging/lustre/lustre/llite/llite_lib.c
index 0c1b583a4ea1..bf1ec277a1dc 100644
--- a/drivers/staging/lustre/lustre/llite/llite_lib.c
+++ b/drivers/staging/lustre/lustre/llite/llite_lib.c
@@ -87,11 +87,10 @@ static struct ll_sb_info *ll_init_sbi(void)
 
 	si_meminfo(&si);
 	pages = si.totalram - si.totalhigh;
-	if (pages >> (20 - PAGE_CACHE_SHIFT) < 512) {
+	if (pages >> (20 - PAGE_CACHE_SHIFT) < 512)
 		lru_page_max = pages / 2;
-	} else {
+	else
 		lru_page_max = (pages / 4) * 3;
-	}
 
 	/* initialize lru data */
 	atomic_set(&sbi->ll_cache.ccc_users, 0);
@@ -621,7 +620,7 @@ int ll_get_max_mdsize(struct ll_sb_info *sbi, int *lmmsize)
 	rc = obd_get_info(NULL, sbi->ll_md_exp, sizeof(KEY_MAX_EASIZE),
 			  KEY_MAX_EASIZE, &size, lmmsize, NULL);
 	if (rc)
-		CERROR("Get max mdsize error rc %d \n", rc);
+		CERROR("Get max mdsize error rc %d\n", rc);
 
 	return rc;
 }
@@ -978,19 +977,17 @@ int ll_fill_super(struct super_block *sb, struct vfsmount *mnt)
 	CDEBUG(D_CONFIG, "Found profile %s: mdc=%s osc=%s\n", profilenm,
 	       lprof->lp_md, lprof->lp_dt);
 
-	dt = kzalloc(strlen(lprof->lp_dt) + instlen + 2, GFP_NOFS);
+	dt = kasprintf(GFP_NOFS, "%s-%p", lprof->lp_dt, cfg->cfg_instance);
 	if (!dt) {
 		err = -ENOMEM;
 		goto out_free;
 	}
-	sprintf(dt, "%s-%p", lprof->lp_dt, cfg->cfg_instance);
 
-	md = kzalloc(strlen(lprof->lp_md) + instlen + 2, GFP_NOFS);
+	md = kasprintf(GFP_NOFS, "%s-%p", lprof->lp_md, cfg->cfg_instance);
 	if (!md) {
 		err = -ENOMEM;
 		goto out_free;
 	}
-	sprintf(md, "%s-%p", lprof->lp_md, cfg->cfg_instance);
 
 	/* connections, registrations, sb setup */
 	err = client_common_fill_super(sb, md, dt, mnt);
@@ -1432,7 +1429,7 @@ int ll_setattr_raw(struct dentry *dentry, struct iattr *attr, bool hsm_import)
 
 	if (attr->ia_valid & (ATTR_SIZE |
 			      ATTR_ATIME | ATTR_ATIME_SET |
-			      ATTR_MTIME | ATTR_MTIME_SET))
+			      ATTR_MTIME | ATTR_MTIME_SET)) {
 		/* For truncate and utimes sending attributes to OSTs, setting
 		 * mtime/atime to the past will be performed under PW [0:EOF]
 		 * extent lock (new_size:EOF for truncate).  It may seem
@@ -1444,6 +1441,7 @@ int ll_setattr_raw(struct dentry *dentry, struct iattr *attr, bool hsm_import)
 		rc = ll_setattr_ost(inode, attr);
 		if (attr->ia_valid & ATTR_SIZE)
 			up_write(&lli->lli_trunc_sem);
+	}
 out:
 	if (op_data) {
 		if (op_data->op_ioepoch) {
@@ -1608,7 +1606,7 @@ void ll_update_inode(struct inode *inode, struct lustre_md *md)
 	struct lov_stripe_md *lsm = md->lsm;
 	struct ll_sb_info *sbi = ll_i2sbi(inode);
 
-	LASSERT ((lsm != NULL) == ((body->valid & OBD_MD_FLEASIZE) != 0));
+	LASSERT((lsm != NULL) == ((body->valid & OBD_MD_FLEASIZE) != 0));
 	if (lsm != NULL) {
 		if (!lli->lli_has_smd &&
 		    !(sbi->ll_flags & LL_SBI_LAYOUT_LOCK))
@@ -2152,7 +2150,8 @@ int ll_process_config(struct lustre_cfg *lcfg)
 	ptr = strrchr(lustre_cfg_string(lcfg, 0), '-');
 	if (!ptr || !*(++ptr))
 		return -EINVAL;
-	if (sscanf(ptr, "%lx", &x) != 1)
+	rc = kstrtoul(ptr, 16, &x);
+	if (rc != 0)
 		return -EINVAL;
 	sb = (void *)x;
 	/* This better be a real Lustre superblock! */
diff --git a/drivers/staging/lustre/lustre/llite/llite_mmap.c b/drivers/staging/lustre/lustre/llite/llite_mmap.c
index 479bf428780c..a90214bb84dd 100644
--- a/drivers/staging/lustre/lustre/llite/llite_mmap.c
+++ b/drivers/staging/lustre/lustre/llite/llite_mmap.c
@@ -40,7 +40,7 @@
 #include <linux/stat.h>
 #include <linux/errno.h>
 #include <linux/unistd.h>
-#include <asm/uaccess.h>
+#include <linux/uaccess.h>
 
 #include <linux/fs.h>
 #include <linux/pagemap.h>
@@ -312,7 +312,7 @@ static int ll_fault0(struct vm_area_struct *vma, struct vm_fault *vmf)
 		vio->u.fault.ft_vmpage    = NULL;
 		vio->u.fault.fault.ft_vmf = vmf;
 		vio->u.fault.fault.ft_flags = 0;
-		vio->u.fault.fault.ft_flags_valid = 0;
+		vio->u.fault.fault.ft_flags_valid = false;
 
 		result = cl_io_loop(env, io);
 
diff --git a/drivers/staging/lustre/lustre/llite/lloop.c b/drivers/staging/lustre/lustre/llite/lloop.c
index 031248840642..413a8408e3f5 100644
--- a/drivers/staging/lustre/lustre/llite/lloop.c
+++ b/drivers/staging/lustre/lustre/llite/lloop.c
@@ -100,8 +100,7 @@
 #include <linux/highmem.h>
 #include <linux/gfp.h>
 #include <linux/pagevec.h>
-
-#include <asm/uaccess.h>
+#include <linux/uaccess.h>
 
 #include "../include/lustre_lib.h"
 #include "../include/lustre_lite.h"
@@ -349,7 +348,7 @@ static void loop_make_request(struct request_queue *q, struct bio *old_bio)
 	       old_bio->bi_iter.bi_size);
 
 	spin_lock_irq(&lo->lo_lock);
-	inactive = (lo->lo_state != LLOOP_BOUND);
+	inactive = lo->lo_state != LLOOP_BOUND;
 	spin_unlock_irq(&lo->lo_lock);
 	if (inactive)
 		goto err;
diff --git a/drivers/staging/lustre/lustre/llite/lproc_llite.c b/drivers/staging/lustre/lustre/llite/lproc_llite.c
index aaa13bd3e8de..83a9b85474e1 100644
--- a/drivers/staging/lustre/lustre/llite/lproc_llite.c
+++ b/drivers/staging/lustre/lustre/llite/lproc_llite.c
@@ -59,7 +59,7 @@ static int ll_blksize_seq_show(struct seq_file *m, void *v)
 				cfs_time_shift_64(-OBD_STATFS_CACHE_SECONDS),
 				OBD_STATFS_NODELAY);
 	if (!rc)
-	      rc = seq_printf(m, "%u\n", osfs.os_bsize);
+		seq_printf(m, "%u\n", osfs.os_bsize);
 
 	return rc;
 }
@@ -82,8 +82,9 @@ static int ll_kbytestotal_seq_show(struct seq_file *m, void *v)
 		while (blk_size >>= 1)
 			result <<= 1;
 
-		rc = seq_printf(m, "%llu\n", result);
+		seq_printf(m, "%llu\n", result);
 	}
+
 	return rc;
 }
 LPROC_SEQ_FOPS_RO(ll_kbytestotal);
@@ -105,8 +106,9 @@ static int ll_kbytesfree_seq_show(struct seq_file *m, void *v)
 		while (blk_size >>= 1)
 			result <<= 1;
 
-		rc = seq_printf(m, "%llu\n", result);
+		seq_printf(m, "%llu\n", result);
 	}
+
 	return rc;
 }
 LPROC_SEQ_FOPS_RO(ll_kbytesfree);
@@ -128,8 +130,9 @@ static int ll_kbytesavail_seq_show(struct seq_file *m, void *v)
 		while (blk_size >>= 1)
 			result <<= 1;
 
-		rc = seq_printf(m, "%llu\n", result);
+		seq_printf(m, "%llu\n", result);
 	}
+
 	return rc;
 }
 LPROC_SEQ_FOPS_RO(ll_kbytesavail);
@@ -145,7 +148,8 @@ static int ll_filestotal_seq_show(struct seq_file *m, void *v)
 				cfs_time_shift_64(-OBD_STATFS_CACHE_SECONDS),
 				OBD_STATFS_NODELAY);
 	if (!rc)
-		 rc = seq_printf(m, "%llu\n", osfs.os_files);
+		seq_printf(m, "%llu\n", osfs.os_files);
+
 	return rc;
 }
 LPROC_SEQ_FOPS_RO(ll_filestotal);
@@ -161,7 +165,8 @@ static int ll_filesfree_seq_show(struct seq_file *m, void *v)
 				cfs_time_shift_64(-OBD_STATFS_CACHE_SECONDS),
 				OBD_STATFS_NODELAY);
 	if (!rc)
-		 rc = seq_printf(m, "%llu\n", osfs.os_ffree);
+		seq_printf(m, "%llu\n", osfs.os_ffree);
+
 	return rc;
 }
 LPROC_SEQ_FOPS_RO(ll_filesfree);
@@ -169,16 +174,15 @@ LPROC_SEQ_FOPS_RO(ll_filesfree);
 static int ll_client_type_seq_show(struct seq_file *m, void *v)
 {
 	struct ll_sb_info *sbi = ll_s2sbi((struct super_block *)m->private);
-	int rc;
 
 	LASSERT(sbi != NULL);
 
 	if (sbi->ll_flags & LL_SBI_RMT_CLIENT)
-		rc = seq_printf(m, "remote client\n");
+		seq_puts(m, "remote client\n");
 	else
-		rc = seq_printf(m, "local client\n");
+		seq_puts(m, "local client\n");
 
-	return rc;
+	return 0;
 }
 LPROC_SEQ_FOPS_RO(ll_client_type);
 
@@ -187,7 +191,8 @@ static int ll_fstype_seq_show(struct seq_file *m, void *v)
 	struct super_block *sb = (struct super_block *)m->private;
 
 	LASSERT(sb != NULL);
-	return seq_printf(m, "%s\n", sb->s_type->name);
+	seq_printf(m, "%s\n", sb->s_type->name);
+	return 0;
 }
 LPROC_SEQ_FOPS_RO(ll_fstype);
 
@@ -196,7 +201,8 @@ static int ll_sb_uuid_seq_show(struct seq_file *m, void *v)
 	struct super_block *sb = (struct super_block *)m->private;
 
 	LASSERT(sb != NULL);
-	return seq_printf(m, "%s\n", ll_s2sbi(sb)->ll_sb_uuid.uuid);
+	seq_printf(m, "%s\n", ll_s2sbi(sb)->ll_sb_uuid.uuid);
+	return 0;
 }
 LPROC_SEQ_FOPS_RO(ll_sb_uuid);
 
@@ -353,17 +359,18 @@ static int ll_max_cached_mb_seq_show(struct seq_file *m, void *v)
 
 	max_cached_mb = cache->ccc_lru_max >> shift;
 	unused_mb = atomic_read(&cache->ccc_lru_left) >> shift;
-	return seq_printf(m,
-			"users: %d\n"
-			"max_cached_mb: %d\n"
-			"used_mb: %d\n"
-			"unused_mb: %d\n"
-			"reclaim_count: %u\n",
-			atomic_read(&cache->ccc_users),
-			max_cached_mb,
-			max_cached_mb - unused_mb,
-			unused_mb,
-			cache->ccc_lru_shrinkers);
+	seq_printf(m,
+		   "users: %d\n"
+		   "max_cached_mb: %d\n"
+		   "used_mb: %d\n"
+		   "unused_mb: %d\n"
+		   "reclaim_count: %u\n",
+		   atomic_read(&cache->ccc_users),
+		   max_cached_mb,
+		   max_cached_mb - unused_mb,
+		   unused_mb,
+		   cache->ccc_lru_shrinkers);
+	return 0;
 }
 
 static ssize_t ll_max_cached_mb_seq_write(struct file *file,
@@ -467,7 +474,8 @@ static int ll_checksum_seq_show(struct seq_file *m, void *v)
 	struct super_block *sb = m->private;
 	struct ll_sb_info *sbi = ll_s2sbi(sb);
 
-	return seq_printf(m, "%u\n", (sbi->ll_flags & LL_SBI_CHECKSUM) ? 1 : 0);
+	seq_printf(m, "%u\n", (sbi->ll_flags & LL_SBI_CHECKSUM) ? 1 : 0);
+	return 0;
 }
 
 static ssize_t ll_checksum_seq_write(struct file *file,
@@ -503,7 +511,8 @@ static int ll_max_rw_chunk_seq_show(struct seq_file *m, void *v)
 {
 	struct super_block *sb = m->private;
 
-	return seq_printf(m, "%lu\n", ll_s2sbi(sb)->ll_max_rw_chunk);
+	seq_printf(m, "%lu\n", ll_s2sbi(sb)->ll_max_rw_chunk);
+	return 0;
 }
 
 static ssize_t ll_max_rw_chunk_seq_write(struct file *file,
@@ -525,15 +534,14 @@ static int ll_rd_track_id(struct seq_file *m, enum stats_track_type type)
 {
 	struct super_block *sb = m->private;
 
-	if (ll_s2sbi(sb)->ll_stats_track_type == type) {
-		return seq_printf(m, "%d\n",
-				ll_s2sbi(sb)->ll_stats_track_id);
+	if (ll_s2sbi(sb)->ll_stats_track_type == type)
+		seq_printf(m, "%d\n", ll_s2sbi(sb)->ll_stats_track_id);
+	else if (ll_s2sbi(sb)->ll_stats_track_type == STATS_TRACK_ALL)
+		seq_puts(m, "0 (all)\n");
+	else
+		seq_puts(m, "untracked\n");
 
-	} else if (ll_s2sbi(sb)->ll_stats_track_type == STATS_TRACK_ALL) {
-		return seq_printf(m, "0 (all)\n");
-	} else {
-		return seq_printf(m, "untracked\n");
-	}
+	return 0;
 }
 
 static int ll_wr_track_id(const char __user *buffer, unsigned long count,
@@ -601,7 +609,8 @@ static int ll_statahead_max_seq_show(struct seq_file *m, void *v)
 	struct super_block *sb = m->private;
 	struct ll_sb_info *sbi = ll_s2sbi(sb);
 
-	return seq_printf(m, "%u\n", sbi->ll_sa_max);
+	seq_printf(m, "%u\n", sbi->ll_sa_max);
+	return 0;
 }
 
 static ssize_t ll_statahead_max_seq_write(struct file *file,
@@ -631,8 +640,8 @@ static int ll_statahead_agl_seq_show(struct seq_file *m, void *v)
 	struct super_block *sb = m->private;
 	struct ll_sb_info *sbi = ll_s2sbi(sb);
 
-	return seq_printf(m, "%u\n",
-			sbi->ll_flags & LL_SBI_AGL_ENABLED ? 1 : 0);
+	seq_printf(m, "%u\n", sbi->ll_flags & LL_SBI_AGL_ENABLED ? 1 : 0);
+	return 0;
 }
 
 static ssize_t ll_statahead_agl_seq_write(struct file *file,
@@ -661,13 +670,14 @@ static int ll_statahead_stats_seq_show(struct seq_file *m, void *v)
 	struct super_block *sb = m->private;
 	struct ll_sb_info *sbi = ll_s2sbi(sb);
 
-	return seq_printf(m,
-			"statahead total: %u\n"
-			"statahead wrong: %u\n"
-			"agl total: %u\n",
-			atomic_read(&sbi->ll_sa_total),
-			atomic_read(&sbi->ll_sa_wrong),
-			atomic_read(&sbi->ll_agl_total));
+	seq_printf(m,
+		   "statahead total: %u\n"
+		   "statahead wrong: %u\n"
+		   "agl total: %u\n",
+		   atomic_read(&sbi->ll_sa_total),
+		   atomic_read(&sbi->ll_sa_wrong),
+		   atomic_read(&sbi->ll_agl_total));
+	return 0;
 }
 LPROC_SEQ_FOPS_RO(ll_statahead_stats);
 
@@ -676,8 +686,8 @@ static int ll_lazystatfs_seq_show(struct seq_file *m, void *v)
 	struct super_block *sb = m->private;
 	struct ll_sb_info *sbi = ll_s2sbi(sb);
 
-	return seq_printf(m, "%u\n",
-			(sbi->ll_flags & LL_SBI_LAZYSTATFS) ? 1 : 0);
+	seq_printf(m, "%u\n", sbi->ll_flags & LL_SBI_LAZYSTATFS ? 1 : 0);
+	return 0;
 }
 
 static ssize_t ll_lazystatfs_seq_write(struct file *file,
@@ -712,11 +722,12 @@ static int ll_max_easize_seq_show(struct seq_file *m, void *v)
 	if (rc)
 		return rc;
 
-	return seq_printf(m, "%u\n", ealen);
+	seq_printf(m, "%u\n", ealen);
+	return 0;
 }
 LPROC_SEQ_FOPS_RO(ll_max_easize);
 
-static int ll_defult_easize_seq_show(struct seq_file *m, void *v)
+static int ll_default_easize_seq_show(struct seq_file *m, void *v)
 {
 	struct super_block *sb = m->private;
 	struct ll_sb_info *sbi = ll_s2sbi(sb);
@@ -727,9 +738,10 @@ static int ll_defult_easize_seq_show(struct seq_file *m, void *v)
 	if (rc)
 		return rc;
 
-	return seq_printf(m, "%u\n", ealen);
+	seq_printf(m, "%u\n", ealen);
+	return 0;
 }
-LPROC_SEQ_FOPS_RO(ll_defult_easize);
+LPROC_SEQ_FOPS_RO(ll_default_easize);
 
 static int ll_max_cookiesize_seq_show(struct seq_file *m, void *v)
 {
@@ -742,11 +754,12 @@ static int ll_max_cookiesize_seq_show(struct seq_file *m, void *v)
 	if (rc)
 		return rc;
 
-	return seq_printf(m, "%u\n", cookielen);
+	seq_printf(m, "%u\n", cookielen);
+	return 0;
 }
 LPROC_SEQ_FOPS_RO(ll_max_cookiesize);
 
-static int ll_defult_cookiesize_seq_show(struct seq_file *m, void *v)
+static int ll_default_cookiesize_seq_show(struct seq_file *m, void *v)
 {
 	struct super_block *sb = m->private;
 	struct ll_sb_info *sbi = ll_s2sbi(sb);
@@ -757,9 +770,10 @@ static int ll_defult_cookiesize_seq_show(struct seq_file *m, void *v)
 	if (rc)
 		return rc;
 
-	return seq_printf(m, "%u\n", cookielen);
+	seq_printf(m, "%u\n", cookielen);
+	return 0;
 }
-LPROC_SEQ_FOPS_RO(ll_defult_cookiesize);
+LPROC_SEQ_FOPS_RO(ll_default_cookiesize);
 
 static int ll_sbi_flags_seq_show(struct seq_file *m, void *v)
 {
@@ -789,11 +803,10 @@ static int ll_xattr_cache_seq_show(struct seq_file *m, void *v)
 {
 	struct super_block *sb = m->private;
 	struct ll_sb_info *sbi = ll_s2sbi(sb);
-	int rc;
 
-	rc = seq_printf(m, "%u\n", sbi->ll_xattr_cache_enabled);
+	seq_printf(m, "%u\n", sbi->ll_xattr_cache_enabled);
 
-	return rc;
+	return 0;
 }
 
 static ssize_t ll_xattr_cache_seq_write(struct file *file,
@@ -849,9 +862,9 @@ static struct lprocfs_vars lprocfs_llite_obd_vars[] = {
 	{ "statahead_stats",  &ll_statahead_stats_fops, NULL, 0 },
 	{ "lazystatfs",       &ll_lazystatfs_fops, NULL },
 	{ "max_easize",       &ll_max_easize_fops, NULL, 0 },
-	{ "default_easize",   &ll_defult_easize_fops, NULL, 0 },
+	{ "default_easize",   &ll_default_easize_fops, NULL, 0 },
 	{ "max_cookiesize",   &ll_max_cookiesize_fops, NULL, 0 },
-	{ "default_cookiesize", &ll_defult_cookiesize_fops, NULL, 0 },
+	{ "default_cookiesize", &ll_default_cookiesize_fops, NULL, 0 },
 	{ "sbi_flags",	      &ll_sbi_flags_fops, NULL, 0 },
 	{ "xattr_cache",      &ll_xattr_cache_fops, NULL, 0 },
 	{ NULL }
diff --git a/drivers/staging/lustre/lustre/llite/namei.c b/drivers/staging/lustre/lustre/llite/namei.c
index 890ac190f5fa..49f1cb067ea2 100644
--- a/drivers/staging/lustre/lustre/llite/namei.c
+++ b/drivers/staging/lustre/lustre/llite/namei.c
@@ -83,7 +83,8 @@ static int ll_set_inode(struct inode *inode, void *opaque)
 
 	lli->lli_fid = body->fid1;
 	if (unlikely(!(body->valid & OBD_MD_FLTYPE))) {
-		CERROR("Can not initialize inode " DFID " without object type: valid = %#llx\n",
+		CERROR("Can not initialize inode " DFID
+		       " without object type: valid = %#llx\n",
 		       PFID(&lli->lli_fid), body->valid);
 		return -EINVAL;
 	}
@@ -600,7 +601,8 @@ static int ll_atomic_open(struct inode *dir, struct dentry *dentry,
 	long long lookup_flags = LOOKUP_OPEN;
 	int rc = 0;
 
-	CDEBUG(D_VFSTRACE, "VFS Op:name=%pd,dir=%lu/%u(%p),file %p,open_flags %x,mode %x opened %d\n",
+	CDEBUG(D_VFSTRACE,
+	       "VFS Op:name=%pd,dir=%lu/%u(%p),file %p,open_flags %x,mode %x opened %d\n",
 	       dentry, dir->i_ino,
 	       dir->i_generation, dir, file, open_flags, mode, *opened);
 
@@ -974,11 +976,12 @@ out:
  * Instead, ll_ddelete() and ll_d_iput() will update it based upon if there
  * is any lock existing. They will recycle dentries and inodes based upon locks
  * too. b=20433 */
-static int ll_unlink(struct inode * dir, struct dentry *dentry)
+static int ll_unlink(struct inode *dir, struct dentry *dentry)
 {
 	struct ptlrpc_request *request = NULL;
 	struct md_op_data *op_data;
 	int rc;
+
 	CDEBUG(D_VFSTRACE, "VFS Op:name=%pd,dir=%lu/%u(%p)\n",
 	       dentry, dir->i_ino, dir->i_generation, dir);
 
@@ -1033,7 +1036,7 @@ static int ll_rmdir(struct inode *dir, struct dentry *dentry)
 	       dentry, dir->i_ino, dir->i_generation, dir);
 
 	op_data = ll_prep_md_op_data(NULL, dir, NULL,
-				     dentry->d_name.name, 
+				     dentry->d_name.name,
 				     dentry->d_name.len,
 				     S_IFDIR, LUSTRE_OPC_ANY, NULL);
 	if (IS_ERR(op_data))
@@ -1111,10 +1114,9 @@ static int ll_rename(struct inode *old_dir, struct dentry *old_dentry,
 	int err;
 
 	CDEBUG(D_VFSTRACE,
-	       "VFS Op:oldname=%pd,src_dir=%lu/%u(%p),newname=%pd,"
-	       "tgt_dir=%lu/%u(%p)\n", old_dentry,
-	       old_dir->i_ino, old_dir->i_generation, old_dir, new_dentry,
-	       new_dir->i_ino, new_dir->i_generation, new_dir);
+	       "VFS Op:oldname=%pd,src_dir=%lu/%u(%p),newname=%pd,tgt_dir=%lu/%u(%p)\n",
+	       old_dentry, old_dir->i_ino, old_dir->i_generation, old_dir,
+	       new_dentry, new_dir->i_ino, new_dir->i_generation, new_dir);
 
 	op_data = ll_prep_md_op_data(NULL, old_dir, new_dir, NULL, 0, 0,
 				     LUSTRE_OPC_ANY, NULL);
diff --git a/drivers/staging/lustre/lustre/llite/rw.c b/drivers/staging/lustre/lustre/llite/rw.c
index 10a0421366d0..991d20c5065d 100644
--- a/drivers/staging/lustre/lustre/llite/rw.c
+++ b/drivers/staging/lustre/lustre/llite/rw.c
@@ -45,7 +45,7 @@
 #include <linux/errno.h>
 #include <linux/unistd.h>
 #include <linux/writeback.h>
-#include <asm/uaccess.h>
+#include <linux/uaccess.h>
 
 #include <linux/fs.h>
 #include <linux/pagemap.h>
@@ -750,7 +750,7 @@ int ll_readahead(const struct lu_env *env, struct cl_io *io,
 		/* Note: we only trim the RPC, instead of extending the RPC
 		 * to the boundary, so to avoid reading too much pages during
 		 * random reading. */
-		rpc_boundary = ((end + 1) & (~(PTLRPC_MAX_BRW_PAGES - 1)));
+		rpc_boundary = (end + 1) & (~(PTLRPC_MAX_BRW_PAGES - 1));
 		if (rpc_boundary > 0)
 			rpc_boundary--;
 
diff --git a/drivers/staging/lustre/lustre/llite/rw26.c b/drivers/staging/lustre/lustre/llite/rw26.c
index 2f21304046aa..91442fab5725 100644
--- a/drivers/staging/lustre/lustre/llite/rw26.c
+++ b/drivers/staging/lustre/lustre/llite/rw26.c
@@ -44,7 +44,7 @@
 #include <linux/stat.h>
 #include <linux/errno.h>
 #include <linux/unistd.h>
-#include <asm/uaccess.h>
+#include <linux/uaccess.h>
 
 #include <linux/migrate.h>
 #include <linux/fs.h>
diff --git a/drivers/staging/lustre/lustre/llite/statahead.c b/drivers/staging/lustre/lustre/llite/statahead.c
index 6ad9dd0fe2b3..b75562c6b5de 100644
--- a/drivers/staging/lustre/lustre/llite/statahead.c
+++ b/drivers/staging/lustre/lustre/llite/statahead.c
@@ -84,7 +84,7 @@ struct ll_sa_entry {
 	struct qstr	     se_qstr;
 };
 
-static unsigned int sai_generation = 0;
+static unsigned int sai_generation;
 static DEFINE_SPINLOCK(sai_generation_lock);
 
 static inline int ll_sa_entry_unhashed(struct ll_sa_entry *entry)
@@ -706,11 +706,21 @@ static int ll_statahead_interpret(struct ptlrpc_request *req,
 	struct ll_inode_info     *lli = ll_i2info(dir);
 	struct ll_statahead_info *sai = NULL;
 	struct ll_sa_entry       *entry;
+	__u64			  handle = 0;
 	int		       wakeup;
 
 	if (it_disposition(it, DISP_LOOKUP_NEG))
 		rc = -ENOENT;
 
+	if (rc == 0) {
+		/* release ibits lock ASAP to avoid deadlock when statahead
+		 * thread enqueues lock on parent in readdir and another
+		 * process enqueues lock on child with parent lock held, eg.
+		 * unlink. */
+		handle = it->d.lustre.it_lock_handle;
+		ll_intent_drop_lock(it);
+	}
+
 	spin_lock(&lli->lli_sa_lock);
 	/* stale entry */
 	if (unlikely(lli->lli_sai == NULL ||
@@ -745,8 +755,7 @@ static int ll_statahead_interpret(struct ptlrpc_request *req,
 			 * when statahead thread tries to enqueue lock on parent
 			 * for readpage and other tries to enqueue lock on child
 			 * with parent's lock held, for example: unlink. */
-			entry->se_handle = it->d.lustre.it_lock_handle;
-			ll_intent_drop_lock(it);
+			entry->se_handle = handle;
 			wakeup = sa_received_empty(sai);
 			list_add_tail(&entry->se_list,
 					  &sai->sai_entries_received);
diff --git a/drivers/staging/lustre/lustre/llite/super25.c b/drivers/staging/lustre/lustre/llite/super25.c
index 7c1e02a031ba..a494f6271fa0 100644
--- a/drivers/staging/lustre/lustre/llite/super25.c
+++ b/drivers/staging/lustre/lustre/llite/super25.c
@@ -152,9 +152,7 @@ static int __init init_lustre_lite(void)
 
 	do_gettimeofday(&tv);
 	cfs_srand(tv.tv_sec ^ seed[0], tv.tv_usec ^ seed[1]);
-
-	init_timer(&ll_capa_timer);
-	ll_capa_timer.function = ll_capa_timer_callback;
+	setup_timer(&ll_capa_timer, ll_capa_timer_callback, 0);
 	rc = ll_capa_thread_start();
 	if (rc != 0)
 		goto out_proc;
diff --git a/drivers/staging/lustre/lustre/llite/vvp_dev.c b/drivers/staging/lustre/lustre/llite/vvp_dev.c
index 5a1078a4198d..fde41d7c5e3d 100644
--- a/drivers/staging/lustre/lustre/llite/vvp_dev.c
+++ b/drivers/staging/lustre/lustre/llite/vvp_dev.c
@@ -63,12 +63,12 @@ static struct lu_kmem_descr vvp_caches[] = {
 	{
 		.ckd_cache = &vvp_thread_kmem,
 		.ckd_name  = "vvp_thread_kmem",
-		.ckd_size  = sizeof (struct vvp_thread_info),
+		.ckd_size  = sizeof(struct vvp_thread_info),
 	},
 	{
 		.ckd_cache = &vvp_session_kmem,
 		.ckd_name  = "vvp_session_kmem",
-		.ckd_size  = sizeof (struct vvp_session)
+		.ckd_size  = sizeof(struct vvp_session)
 	},
 	{
 		.ckd_cache = NULL
@@ -90,6 +90,7 @@ static void vvp_key_fini(const struct lu_context *ctx,
 			 struct lu_context_key *key, void *data)
 {
 	struct vvp_thread_info *info = data;
+
 	OBD_SLAB_FREE_PTR(info, vvp_thread_kmem);
 }
 
@@ -108,6 +109,7 @@ static void vvp_session_key_fini(const struct lu_context *ctx,
 				 struct lu_context_key *key, void *data)
 {
 	struct vvp_session *session = data;
+
 	OBD_SLAB_FREE_PTR(session, vvp_session_kmem);
 }
 
@@ -286,7 +288,7 @@ static void vvp_pgcache_id_unpack(loff_t pos, struct vvp_pgcache_id *id)
 
 	id->vpi_index  = pos & 0xffffffff;
 	id->vpi_depth  = (pos >> PGC_DEPTH_SHIFT) & 0xf;
-	id->vpi_bucket = ((unsigned long long)pos >> PGC_OBJ_SHIFT);
+	id->vpi_bucket = (unsigned long long)pos >> PGC_OBJ_SHIFT;
 }
 
 static loff_t vvp_pgcache_id_pack(struct vvp_pgcache_id *id)
diff --git a/drivers/staging/lustre/lustre/llite/xattr_cache.c b/drivers/staging/lustre/lustre/llite/xattr_cache.c
index e2badf17d95e..69ea92adf4f1 100644
--- a/drivers/staging/lustre/lustre/llite/xattr_cache.c
+++ b/drivers/staging/lustre/lustre/llite/xattr_cache.c
@@ -132,14 +132,10 @@ static int ll_xattr_cache_add(struct list_head *cache,
 		       xattr->xe_namelen);
 		goto err_name;
 	}
-	xattr->xe_value = kzalloc(xattr_val_len, GFP_NOFS);
-	if (!xattr->xe_value) {
-		CDEBUG(D_CACHE, "failed to alloc xattr value %d\n",
-		       xattr_val_len);
+	xattr->xe_value = kmemdup(xattr_val, xattr_val_len, GFP_NOFS);
+	if (!xattr->xe_value)
 		goto err_value;
-	}
 
-	memcpy(xattr->xe_value, xattr_val, xattr_val_len);
 	xattr->xe_vallen = xattr_val_len;
 	list_add(&xattr->xe_list, cache);
 
@@ -299,13 +295,18 @@ static int ll_xattr_find_get_lock(struct inode *inode,
 
 
 	mutex_lock(&lli->lli_xattrs_enq_lock);
-	/* Try matching first. */
-	mode = ll_take_md_lock(inode, MDS_INODELOCK_XATTR, &lockh, 0, LCK_PR);
-	if (mode != 0) {
-		/* fake oit in mdc_revalidate_lock() manner */
-		oit->d.lustre.it_lock_handle = lockh.cookie;
-		oit->d.lustre.it_lock_mode = mode;
-		goto out;
+	/* inode may have been shrunk and recreated, so data is gone, match lock
+	 * only when data exists. */
+	if (ll_xattr_cache_valid(lli)) {
+		/* Try matching first. */
+		mode = ll_take_md_lock(inode, MDS_INODELOCK_XATTR, &lockh, 0,
+				       LCK_PR);
+		if (mode != 0) {
+			/* fake oit in mdc_revalidate_lock() manner */
+			oit->d.lustre.it_lock_handle = lockh.cookie;
+			oit->d.lustre.it_lock_mode = mode;
+			goto out;
+		}
 	}
 
 	/* Enqueue if the lock isn't cached locally. */
diff --git a/drivers/staging/lustre/lustre/lmv/lmv_obd.c b/drivers/staging/lustre/lustre/lmv/lmv_obd.c
index b779f47384c5..b9459faf8645 100644
--- a/drivers/staging/lustre/lustre/lmv/lmv_obd.c
+++ b/drivers/staging/lustre/lustre/lmv/lmv_obd.c
@@ -43,7 +43,7 @@
 #include <asm/div64.h>
 #include <linux/seq_file.h>
 #include <linux/namei.h>
-#include <asm/uaccess.h>
+#include <linux/uaccess.h>
 
 #include "../include/lustre/lustre_idl.h"
 #include "../include/obd_support.h"
@@ -487,7 +487,7 @@ static int lmv_add_target(struct obd_device *obd, struct obd_uuid *uuidp,
 		__u32 oldsize = 0;
 
 		while (newsize < index + 1)
-			newsize = newsize << 1;
+			newsize <<= 1;
 		OBD_ALLOC(newtgts, sizeof(*newtgts) * newsize);
 		if (newtgts == NULL) {
 			lmv_init_unlock(lmv);
diff --git a/drivers/staging/lustre/lustre/lmv/lproc_lmv.c b/drivers/staging/lustre/lustre/lmv/lproc_lmv.c
index 5be4176829d3..22e5c315faa4 100644
--- a/drivers/staging/lustre/lustre/lmv/lproc_lmv.c
+++ b/drivers/staging/lustre/lustre/lmv/lproc_lmv.c
@@ -40,6 +40,7 @@
 #include <linux/statfs.h>
 #include "../include/lprocfs_status.h"
 #include "../include/obd_class.h"
+#include "lmv_internal.h"
 
 static int lmv_numobd_seq_show(struct seq_file *m, void *v)
 {
@@ -48,7 +49,8 @@ static int lmv_numobd_seq_show(struct seq_file *m, void *v)
 
 	LASSERT(dev != NULL);
 	desc = &dev->u.lmv.desc;
-	return seq_printf(m, "%u\n", desc->ld_tgt_count);
+	seq_printf(m, "%u\n", desc->ld_tgt_count);
+	return 0;
 }
 LPROC_SEQ_FOPS_RO(lmv_numobd);
 
@@ -82,7 +84,8 @@ static int lmv_placement_seq_show(struct seq_file *m, void *v)
 
 	LASSERT(dev != NULL);
 	lmv = &dev->u.lmv;
-	return seq_printf(m, "%s\n", placement_policy2name(lmv->lmv_placement));
+	seq_printf(m, "%s\n", placement_policy2name(lmv->lmv_placement));
+	return 0;
 }
 
 #define MAX_POLICY_STRING_SIZE 64
@@ -130,7 +133,8 @@ static int lmv_activeobd_seq_show(struct seq_file *m, void *v)
 
 	LASSERT(dev != NULL);
 	desc = &dev->u.lmv.desc;
-	return seq_printf(m, "%u\n", desc->ld_active_tgt_count);
+	seq_printf(m, "%u\n", desc->ld_active_tgt_count);
+	return 0;
 }
 LPROC_SEQ_FOPS_RO(lmv_activeobd);
 
@@ -141,7 +145,8 @@ static int lmv_desc_uuid_seq_show(struct seq_file *m, void *v)
 
 	LASSERT(dev != NULL);
 	lmv = &dev->u.lmv;
-	return seq_printf(m, "%s\n", lmv->desc.ld_uuid.uuid);
+	seq_printf(m, "%s\n", lmv->desc.ld_uuid.uuid);
+	return 0;
 }
 LPROC_SEQ_FOPS_RO(lmv_desc_uuid);
 
@@ -171,8 +176,10 @@ static int lmv_tgt_seq_show(struct seq_file *p, void *v)
 
 	if (tgt == NULL)
 		return 0;
-	return seq_printf(p, "%d: %s %sACTIVE\n", tgt->ltd_idx,
-			  tgt->ltd_uuid.uuid, tgt->ltd_active ? "" : "IN");
+	seq_printf(p, "%d: %s %sACTIVE\n",
+		   tgt->ltd_idx, tgt->ltd_uuid.uuid,
+		   tgt->ltd_active ? "" : "IN");
+	return 0;
 }
 
 static struct seq_operations lmv_tgt_sops = {
diff --git a/drivers/staging/lustre/lustre/lov/lov_dev.c b/drivers/staging/lustre/lustre/lov/lov_dev.c
index 796a015d070c..711b837ddba2 100644
--- a/drivers/staging/lustre/lustre/lov/lov_dev.c
+++ b/drivers/staging/lustre/lustre/lov/lov_dev.c
@@ -60,7 +60,7 @@ struct kmem_cache *lovsub_req_kmem;
 struct kmem_cache *lov_lock_link_kmem;
 
 /** Lock class of lov_device::ld_mutex. */
-struct lock_class_key cl_lov_device_mutex_class;
+static struct lock_class_key cl_lov_device_mutex_class;
 
 struct lu_kmem_descr lov_caches[] = {
 	{
diff --git a/drivers/staging/lustre/lustre/lov/lov_ea.c b/drivers/staging/lustre/lustre/lov/lov_ea.c
index e9ec39c5a6c2..2bcfaeaff6fa 100644
--- a/drivers/staging/lustre/lustre/lov/lov_ea.c
+++ b/drivers/staging/lustre/lustre/lov/lov_ea.c
@@ -209,8 +209,8 @@ static int lsm_lmm_verify_v1(struct lov_mds_md_v1 *lmm, int lmm_bytes,
 	return lsm_lmm_verify_common(lmm, lmm_bytes, *stripe_count);
 }
 
-int lsm_unpackmd_v1(struct lov_obd *lov, struct lov_stripe_md *lsm,
-		    struct lov_mds_md_v1 *lmm)
+static int lsm_unpackmd_v1(struct lov_obd *lov, struct lov_stripe_md *lsm,
+			   struct lov_mds_md_v1 *lmm)
 {
 	struct lov_oinfo *loi;
 	int i;
@@ -227,6 +227,9 @@ int lsm_unpackmd_v1(struct lov_obd *lov, struct lov_stripe_md *lsm,
 		ostid_le_to_cpu(&lmm->lmm_objects[i].l_ost_oi, &loi->loi_oi);
 		loi->loi_ost_idx = le32_to_cpu(lmm->lmm_objects[i].l_ost_idx);
 		loi->loi_ost_gen = le32_to_cpu(lmm->lmm_objects[i].l_ost_gen);
+		if (lov_oinfo_is_dummy(loi))
+			continue;
+
 		if (loi->loi_ost_idx >= lov->desc.ld_tgt_count) {
 			CERROR("OST index %d more than OST count %d\n",
 			       loi->loi_ost_idx, lov->desc.ld_tgt_count);
@@ -287,8 +290,8 @@ static int lsm_lmm_verify_v3(struct lov_mds_md *lmmv1, int lmm_bytes,
 				     *stripe_count);
 }
 
-int lsm_unpackmd_v3(struct lov_obd *lov, struct lov_stripe_md *lsm,
-		    struct lov_mds_md *lmmv1)
+static int lsm_unpackmd_v3(struct lov_obd *lov, struct lov_stripe_md *lsm,
+			   struct lov_mds_md *lmmv1)
 {
 	struct lov_mds_md_v3 *lmm;
 	struct lov_oinfo *loi;
@@ -314,6 +317,9 @@ int lsm_unpackmd_v3(struct lov_obd *lov, struct lov_stripe_md *lsm,
 		ostid_le_to_cpu(&lmm->lmm_objects[i].l_ost_oi, &loi->loi_oi);
 		loi->loi_ost_idx = le32_to_cpu(lmm->lmm_objects[i].l_ost_idx);
 		loi->loi_ost_gen = le32_to_cpu(lmm->lmm_objects[i].l_ost_gen);
+		if (lov_oinfo_is_dummy(loi))
+			continue;
+
 		if (loi->loi_ost_idx >= lov->desc.ld_tgt_count) {
 			CERROR("OST index %d more than OST count %d\n",
 			       loi->loi_ost_idx, lov->desc.ld_tgt_count);
diff --git a/drivers/staging/lustre/lustre/lov/lov_internal.h b/drivers/staging/lustre/lustre/lov/lov_internal.h
index 8c8508bbbb80..b644acc9b034 100644
--- a/drivers/staging/lustre/lustre/lov/lov_internal.h
+++ b/drivers/staging/lustre/lustre/lov/lov_internal.h
@@ -304,4 +304,16 @@ static inline struct lov_stripe_md *lsm_addref(struct lov_stripe_md *lsm)
 	return lsm;
 }
 
+static inline bool lov_oinfo_is_dummy(const struct lov_oinfo *loi)
+{
+	if (unlikely(loi->loi_oi.oi.oi_id == 0 &&
+		     loi->loi_oi.oi.oi_seq == 0 &&
+		     loi->loi_ost_idx == 0 &&
+		     loi->loi_ost_gen == 0))
+		return true;
+
+	return false;
+}
+
+
 #endif
diff --git a/drivers/staging/lustre/lustre/lov/lov_io.c b/drivers/staging/lustre/lustre/lov/lov_io.c
index f1f6db3f664a..cf96e0d01e22 100644
--- a/drivers/staging/lustre/lustre/lov/lov_io.c
+++ b/drivers/staging/lustre/lustre/lov/lov_io.c
@@ -148,6 +148,9 @@ static int lov_io_sub_init(const struct lu_env *env, struct lov_io *lio,
 	LASSERT(sub->sub_env == NULL);
 	LASSERT(sub->sub_stripe < lio->lis_stripe_count);
 
+	if (unlikely(lov_r0(lov)->lo_sub[stripe] == NULL))
+		return -EIO;
+
 	result = 0;
 	sub->sub_io_initialized = 0;
 	sub->sub_borrowed = 0;
@@ -391,7 +394,16 @@ static int lov_io_iter_init(const struct lu_env *env,
 					   endpos, &start, &end))
 			continue;
 
-		end = lov_offset_mod(end, +1);
+		if (unlikely(lov_r0(lio->lis_object)->lo_sub[stripe] == NULL)) {
+			if (ios->cis_io->ci_type == CIT_READ ||
+			    ios->cis_io->ci_type == CIT_WRITE ||
+			    ios->cis_io->ci_type == CIT_FAULT)
+				return -EIO;
+
+			continue;
+		}
+
+		end = lov_offset_mod(end, 1);
 		sub = lov_sub_get(env, lio, stripe);
 		if (!IS_ERR(sub)) {
 			lov_io_sub_inherit(sub->sub_io, lio, stripe,
@@ -913,7 +925,7 @@ int lov_io_init_empty(const struct lu_env *env, struct cl_object *obj,
 		break;
 	case CIT_FSYNC:
 	case CIT_SETATTR:
-		result = +1;
+		result = 1;
 		break;
 	case CIT_WRITE:
 		result = -EBADF;
diff --git a/drivers/staging/lustre/lustre/lov/lov_lock.c b/drivers/staging/lustre/lustre/lov/lov_lock.c
index 49e694222ac8..f2eca565bdda 100644
--- a/drivers/staging/lustre/lustre/lov/lov_lock.c
+++ b/drivers/staging/lustre/lustre/lov/lov_lock.c
@@ -308,7 +308,8 @@ static int lov_lock_sub_init(const struct lu_env *env,
 		 * XXX for wide striping smarter algorithm is desirable,
 		 * breaking out of the loop, early.
 		 */
-		if (lov_stripe_intersects(loo->lo_lsm, i,
+		if (likely(r0->lo_sub[i] != NULL) &&
+		    lov_stripe_intersects(loo->lo_lsm, i,
 					  file_start, file_end, &start, &end))
 			nr++;
 	}
@@ -326,7 +327,8 @@ static int lov_lock_sub_init(const struct lu_env *env,
 	 * top-lock.
 	 */
 	for (i = 0, nr = 0; i < r0->lo_nr; ++i) {
-		if (lov_stripe_intersects(loo->lo_lsm, i,
+		if (likely(r0->lo_sub[i] != NULL) &&
+		    lov_stripe_intersects(loo->lo_lsm, i,
 					  file_start, file_end, &start, &end)) {
 			struct cl_lock_descr *descr;
 
@@ -914,10 +916,22 @@ static int lov_lock_stripe_is_matching(const struct lu_env *env,
 	 */
 	start = cl_offset(&lov->lo_cl, descr->cld_start);
 	end   = cl_offset(&lov->lo_cl, descr->cld_end + 1) - 1;
-	result = end - start <= lsm->lsm_stripe_size &&
-		 stripe == lov_stripe_number(lsm, start) &&
-		 stripe == lov_stripe_number(lsm, end);
-	if (result) {
+	result = 0;
+	/* glimpse should work on the object with LOV EA hole. */
+	if (end - start <= lsm->lsm_stripe_size) {
+		int idx;
+
+		idx = lov_stripe_number(lsm, start);
+		if (idx == stripe ||
+		    unlikely(lov_r0(lov)->lo_sub[idx] == NULL)) {
+			idx = lov_stripe_number(lsm, end);
+			if (idx == stripe ||
+			    unlikely(lov_r0(lov)->lo_sub[idx] == NULL))
+				result = 1;
+		}
+	}
+
+	if (result != 0) {
 		struct cl_lock_descr *subd = &lov_env_info(env)->lti_ldescr;
 		u64 sub_start;
 		u64 sub_end;
diff --git a/drivers/staging/lustre/lustre/lov/lov_obd.c b/drivers/staging/lustre/lustre/lov/lov_obd.c
index ea503d2a19f8..02781576637e 100644
--- a/drivers/staging/lustre/lustre/lov/lov_obd.c
+++ b/drivers/staging/lustre/lustre/lov/lov_obd.c
@@ -553,7 +553,7 @@ static int lov_add_target(struct obd_device *obd, struct obd_uuid *uuidp,
 
 		newsize = max_t(__u32, lov->lov_tgt_size, 2);
 		while (newsize < index + 1)
-			newsize = newsize << 1;
+			newsize <<= 1;
 		OBD_ALLOC(newtgts, sizeof(*newtgts) * newsize);
 		if (newtgts == NULL) {
 			mutex_unlock(&lov->lov_lock);
@@ -1011,9 +1011,13 @@ static int lov_recreate(struct obd_export *exp, struct obdo *src_oa,
 	}
 
 	for (i = 0; i < lsm->lsm_stripe_count; i++) {
-		if (lsm->lsm_oinfo[i]->loi_ost_idx == ost_idx) {
-			if (ostid_id(&lsm->lsm_oinfo[i]->loi_oi) !=
-					ostid_id(&src_oa->o_oi)) {
+		struct lov_oinfo *loi = lsm->lsm_oinfo[i];
+
+		if (lov_oinfo_is_dummy(loi))
+			continue;
+
+		if (loi->loi_ost_idx == ost_idx) {
+			if (ostid_id(&loi->loi_oi) != ostid_id(&src_oa->o_oi)) {
 				rc = -EINVAL;
 				goto out;
 			}
@@ -1305,10 +1309,14 @@ static int lov_find_cbdata(struct obd_export *exp,
 		struct lov_stripe_md submd;
 		struct lov_oinfo *loi = lsm->lsm_oinfo[i];
 
+		if (lov_oinfo_is_dummy(loi))
+			continue;
+
 		if (!lov->lov_tgts[loi->loi_ost_idx]) {
-			CDEBUG(D_HA, "lov idx %d NULL \n", loi->loi_ost_idx);
+			CDEBUG(D_HA, "lov idx %d NULL\n", loi->loi_ost_idx);
 			continue;
 		}
+
 		submd.lsm_oi = loi->loi_oi;
 		submd.lsm_stripe_count = 0;
 		rc = obd_find_cbdata(lov->lov_tgts[loi->loi_ost_idx]->ltd_exp,
@@ -1601,9 +1609,9 @@ static int lov_iocontrol(unsigned int cmd, struct obd_export *exp, int len,
  * \param fm_end logical end of mapping
  * \param start_stripe starting stripe will be returned in this
  */
-u64 fiemap_calc_fm_end_offset(struct ll_user_fiemap *fiemap,
-				   struct lov_stripe_md *lsm, u64 fm_start,
-				   u64 fm_end, int *start_stripe)
+static u64 fiemap_calc_fm_end_offset(struct ll_user_fiemap *fiemap,
+				     struct lov_stripe_md *lsm, u64 fm_start,
+				     u64 fm_end, int *start_stripe)
 {
 	u64 local_end = fiemap->fm_extents[0].fe_logical;
 	u64 lun_start, lun_end;
@@ -1616,8 +1624,12 @@ u64 fiemap_calc_fm_end_offset(struct ll_user_fiemap *fiemap,
 
 	/* Find out stripe_no from ost_index saved in the fe_device */
 	for (i = 0; i < lsm->lsm_stripe_count; i++) {
-		if (lsm->lsm_oinfo[i]->loi_ost_idx ==
-					fiemap->fm_extents[0].fe_device) {
+		struct lov_oinfo *oinfo = lsm->lsm_oinfo[i];
+
+		if (lov_oinfo_is_dummy(oinfo))
+			continue;
+
+		if (oinfo->loi_ost_idx == fiemap->fm_extents[0].fe_device) {
 			stripe_no = i;
 			break;
 		}
@@ -1658,17 +1670,17 @@ u64 fiemap_calc_fm_end_offset(struct ll_user_fiemap *fiemap,
  *
  * \retval last_stripe return the last stripe of the mapping
  */
-int fiemap_calc_last_stripe(struct lov_stripe_md *lsm, u64 fm_start,
-			    u64 fm_end, int start_stripe,
-			    int *stripe_count)
+static int fiemap_calc_last_stripe(struct lov_stripe_md *lsm, u64 fm_start,
+				   u64 fm_end, int start_stripe,
+				   int *stripe_count)
 {
 	int last_stripe;
 	u64 obd_start, obd_end;
 	int i, j;
 
 	if (fm_end - fm_start > lsm->lsm_stripe_size * lsm->lsm_stripe_count) {
-		last_stripe = (start_stripe < 1 ? lsm->lsm_stripe_count - 1 :
-							      start_stripe - 1);
+		last_stripe = start_stripe < 1 ? lsm->lsm_stripe_count - 1 :
+							      start_stripe - 1;
 		*stripe_count = lsm->lsm_stripe_count;
 	} else {
 		for (j = 0, i = start_stripe; j < lsm->lsm_stripe_count;
@@ -1694,10 +1706,10 @@ int fiemap_calc_last_stripe(struct lov_stripe_md *lsm, u64 fm_start,
  * \param ext_count number of extents to be copied
  * \param current_extent where to start copying in main extent array
  */
-void fiemap_prepare_and_copy_exts(struct ll_user_fiemap *fiemap,
-				  struct ll_fiemap_extent *lcl_fm_ext,
-				  int ost_index, unsigned int ext_count,
-				  int current_extent)
+static void fiemap_prepare_and_copy_exts(struct ll_user_fiemap *fiemap,
+					 struct ll_fiemap_extent *lcl_fm_ext,
+					 int ost_index, unsigned int ext_count,
+					 int current_extent)
 {
 	char *to;
 	int ext;
@@ -1795,6 +1807,11 @@ static int lov_fiemap(struct lov_obd *lov, __u32 keylen, void *key,
 					   &lun_start, &obd_object_end)) == 0)
 			continue;
 
+		if (lov_oinfo_is_dummy(lsm->lsm_oinfo[cur_stripe])) {
+			rc = -EIO;
+			goto out;
+		}
+
 		/* If this is a continuation FIEMAP call and we are on
 		 * starting stripe then lun_start needs to be set to
 		 * fm_end_offset */
@@ -1985,6 +2002,9 @@ static int lov_get_info(const struct lu_env *env, struct obd_export *exp,
 		 * be NULL and won't match the lock's export. */
 		for (i = 0; i < lsm->lsm_stripe_count; i++) {
 			loi = lsm->lsm_oinfo[i];
+			if (lov_oinfo_is_dummy(loi))
+				continue;
+
 			if (!lov->lov_tgts[loi->loi_ost_idx])
 				continue;
 			if (lov->lov_tgts[loi->loi_ost_idx]->ltd_exp ==
@@ -2290,7 +2310,7 @@ out:
 	return rc;
 }
 
-struct obd_ops lov_obd_ops = {
+static struct obd_ops lov_obd_ops = {
 	.o_owner	       = THIS_MODULE,
 	.o_setup	       = lov_setup,
 	.o_precleanup	  = lov_precleanup,
@@ -2324,7 +2344,7 @@ struct obd_ops lov_obd_ops = {
 
 struct kmem_cache *lov_oinfo_slab;
 
-int __init lov_init(void)
+static int __init lov_init(void)
 {
 	struct lprocfs_static_vars lvars = { NULL };
 	int rc;
diff --git a/drivers/staging/lustre/lustre/lov/lov_object.c b/drivers/staging/lustre/lustre/lov/lov_object.c
index 4cab730ab429..a22342fa792d 100644
--- a/drivers/staging/lustre/lustre/lov/lov_object.c
+++ b/drivers/staging/lustre/lustre/lov/lov_object.c
@@ -42,6 +42,7 @@
 #define DEBUG_SUBSYSTEM S_LOV
 
 #include "lov_cl_internal.h"
+#include "../include/lclient.h"
 
 /** \addtogroup lov
  *  @{
@@ -230,6 +231,9 @@ static int lov_init_raid0(const struct lu_env *env,
 			struct lov_oinfo *oinfo = lsm->lsm_oinfo[i];
 			int ost_idx = oinfo->loi_ost_idx;
 
+			if (lov_oinfo_is_dummy(oinfo))
+				continue;
+
 			result = ostid_to_fid(ofid, &oinfo->loi_oi,
 					      oinfo->loi_ost_idx);
 			if (result != 0)
@@ -563,7 +567,7 @@ static const struct lov_layout_operations lov_dispatch[] = {
 /**
  * Return lov_layout_type associated with a given lsm
  */
-enum lov_layout_type lov_type(struct lov_stripe_md *lsm)
+static enum lov_layout_type lov_type(struct lov_stripe_md *lsm)
 {
 	if (lsm == NULL)
 		return LLT_EMPTY;
@@ -973,6 +977,10 @@ int lov_read_and_clear_async_rc(struct cl_object *clob)
 			LASSERT(lsm != NULL);
 			for (i = 0; i < lsm->lsm_stripe_count; i++) {
 				struct lov_oinfo *loi = lsm->lsm_oinfo[i];
+
+				if (lov_oinfo_is_dummy(loi))
+					continue;
+
 				if (loi->loi_ar.ar_rc && !rc)
 					rc = loi->loi_ar.ar_rc;
 				loi->loi_ar.ar_rc = 0;
diff --git a/drivers/staging/lustre/lustre/lov/lov_pool.c b/drivers/staging/lustre/lustre/lov/lov_pool.c
index 0e0ea60de4c8..d96163de773f 100644
--- a/drivers/staging/lustre/lustre/lov/lov_pool.c
+++ b/drivers/staging/lustre/lustre/lov/lov_pool.c
@@ -71,7 +71,7 @@ void lov_pool_putref(struct pool_desc *pool)
 	}
 }
 
-void lov_pool_putref_locked(struct pool_desc *pool)
+static void lov_pool_putref_locked(struct pool_desc *pool)
 {
 	CDEBUG(D_INFO, "pool %p\n", pool);
 	LASSERT(atomic_read(&pool->pool_refcount) > 1);
diff --git a/drivers/staging/lustre/lustre/lov/lov_request.c b/drivers/staging/lustre/lustre/lov/lov_request.c
index 7358b9d29103..933e2d1f8127 100644
--- a/drivers/staging/lustre/lustre/lov/lov_request.c
+++ b/drivers/staging/lustre/lustre/lov/lov_request.c
@@ -299,6 +299,9 @@ int lov_prep_getattr_set(struct obd_export *exp, struct obd_info *oinfo,
 		struct lov_request *req;
 
 		loi = oinfo->oi_md->lsm_oinfo[i];
+		if (lov_oinfo_is_dummy(loi))
+			continue;
+
 		if (!lov_check_and_wait_active(lov, loi->loi_ost_idx)) {
 			CDEBUG(D_HA, "lov idx %d inactive\n", loi->loi_ost_idx);
 			if (oinfo->oi_oa->o_valid & OBD_MD_FLEPOCH) {
@@ -384,6 +387,9 @@ int lov_prep_destroy_set(struct obd_export *exp, struct obd_info *oinfo,
 		struct lov_request *req;
 
 		loi = lsm->lsm_oinfo[i];
+		if (lov_oinfo_is_dummy(loi))
+			continue;
+
 		if (!lov_check_and_wait_active(lov, loi->loi_ost_idx)) {
 			CDEBUG(D_HA, "lov idx %d inactive\n", loi->loi_ost_idx);
 			continue;
@@ -497,6 +503,9 @@ int lov_prep_setattr_set(struct obd_export *exp, struct obd_info *oinfo,
 		struct lov_oinfo *loi = oinfo->oi_md->lsm_oinfo[i];
 		struct lov_request *req;
 
+		if (lov_oinfo_is_dummy(loi))
+			continue;
+
 		if (!lov_check_and_wait_active(lov, loi->loi_ost_idx)) {
 			CDEBUG(D_HA, "lov idx %d inactive\n", loi->loi_ost_idx);
 			continue;
diff --git a/drivers/staging/lustre/lustre/lov/lproc_lov.c b/drivers/staging/lustre/lustre/lov/lproc_lov.c
index c99f2f44ec62..174cbf5c138f 100644
--- a/drivers/staging/lustre/lustre/lov/lproc_lov.c
+++ b/drivers/staging/lustre/lustre/lov/lproc_lov.c
@@ -35,7 +35,7 @@
  */
 #define DEBUG_SUBSYSTEM S_CLASS
 
-#include <asm/statfs.h>
+#include <linux/statfs.h>
 #include "../include/lprocfs_status.h"
 #include "../include/obd_class.h"
 #include <linux/seq_file.h>
@@ -48,7 +48,8 @@ static int lov_stripesize_seq_show(struct seq_file *m, void *v)
 
 	LASSERT(dev != NULL);
 	desc = &dev->u.lov.desc;
-	return seq_printf(m, "%llu\n", desc->ld_default_stripe_size);
+	seq_printf(m, "%llu\n", desc->ld_default_stripe_size);
+	return 0;
 }
 
 static ssize_t lov_stripesize_seq_write(struct file *file,
@@ -79,7 +80,8 @@ static int lov_stripeoffset_seq_show(struct seq_file *m, void *v)
 
 	LASSERT(dev != NULL);
 	desc = &dev->u.lov.desc;
-	return seq_printf(m, "%llu\n", desc->ld_default_stripe_offset);
+	seq_printf(m, "%llu\n", desc->ld_default_stripe_offset);
+	return 0;
 }
 
 static ssize_t lov_stripeoffset_seq_write(struct file *file,
@@ -109,7 +111,8 @@ static int lov_stripetype_seq_show(struct seq_file *m, void *v)
 
 	LASSERT(dev != NULL);
 	desc = &dev->u.lov.desc;
-	return seq_printf(m, "%u\n", desc->ld_pattern);
+	seq_printf(m, "%u\n", desc->ld_pattern);
+	return 0;
 }
 
 static ssize_t lov_stripetype_seq_write(struct file *file,
@@ -139,8 +142,8 @@ static int lov_stripecount_seq_show(struct seq_file *m, void *v)
 
 	LASSERT(dev != NULL);
 	desc = &dev->u.lov.desc;
-	return seq_printf(m, "%d\n",
-			(__s16)(desc->ld_default_stripe_count + 1) - 1);
+	seq_printf(m, "%d\n", (__s16)(desc->ld_default_stripe_count + 1) - 1);
+	return 0;
 }
 
 static ssize_t lov_stripecount_seq_write(struct file *file,
@@ -170,7 +173,8 @@ static int lov_numobd_seq_show(struct seq_file *m, void *v)
 
 	LASSERT(dev != NULL);
 	desc = &dev->u.lov.desc;
-	return seq_printf(m, "%u\n", desc->ld_tgt_count);
+	seq_printf(m, "%u\n", desc->ld_tgt_count);
+	return 0;
 }
 LPROC_SEQ_FOPS_RO(lov_numobd);
 
@@ -181,7 +185,8 @@ static int lov_activeobd_seq_show(struct seq_file *m, void *v)
 
 	LASSERT(dev != NULL);
 	desc = &dev->u.lov.desc;
-	return seq_printf(m, "%u\n", desc->ld_active_tgt_count);
+	seq_printf(m, "%u\n", desc->ld_active_tgt_count);
+	return 0;
 }
 LPROC_SEQ_FOPS_RO(lov_activeobd);
 
@@ -192,7 +197,8 @@ static int lov_desc_uuid_seq_show(struct seq_file *m, void *v)
 
 	LASSERT(dev != NULL);
 	lov = &dev->u.lov;
-	return seq_printf(m, "%s\n", lov->desc.ld_uuid.uuid);
+	seq_printf(m, "%s\n", lov->desc.ld_uuid.uuid);
+	return 0;
 }
 LPROC_SEQ_FOPS_RO(lov_desc_uuid);
 
@@ -228,12 +234,14 @@ static void *lov_tgt_seq_next(struct seq_file *p, void *v, loff_t *pos)
 static int lov_tgt_seq_show(struct seq_file *p, void *v)
 {
 	struct lov_tgt_desc *tgt = v;
-	return seq_printf(p, "%d: %s %sACTIVE\n", tgt->ltd_index,
-			  obd_uuid2str(&tgt->ltd_uuid),
-			  tgt->ltd_active ? "" : "IN");
+
+	seq_printf(p, "%d: %s %sACTIVE\n",
+		   tgt->ltd_index, obd_uuid2str(&tgt->ltd_uuid),
+		   tgt->ltd_active ? "" : "IN");
+	return 0;
 }
 
-struct seq_operations lov_tgt_sops = {
+static const struct seq_operations lov_tgt_sops = {
 	.start = lov_tgt_seq_start,
 	.stop = lov_tgt_seq_stop,
 	.next = lov_tgt_seq_next,
@@ -262,7 +270,7 @@ LPROC_SEQ_FOPS_RO_TYPE(lov, kbytestotal);
 LPROC_SEQ_FOPS_RO_TYPE(lov, kbytesfree);
 LPROC_SEQ_FOPS_RO_TYPE(lov, kbytesavail);
 
-struct lprocfs_vars lprocfs_lov_obd_vars[] = {
+static struct lprocfs_vars lprocfs_lov_obd_vars[] = {
 	{ "uuid",	  &lov_uuid_fops,	  NULL, 0 },
 	{ "stripesize",   &lov_stripesize_fops,   NULL },
 	{ "stripeoffset", &lov_stripeoffset_fops, NULL },
diff --git a/drivers/staging/lustre/lustre/mdc/lproc_mdc.c b/drivers/staging/lustre/lustre/mdc/lproc_mdc.c
index c791941bd810..acfe08e459c0 100644
--- a/drivers/staging/lustre/lustre/mdc/lproc_mdc.c
+++ b/drivers/staging/lustre/lustre/mdc/lproc_mdc.c
@@ -38,17 +38,18 @@
 #include <linux/vfs.h>
 #include "../include/obd_class.h"
 #include "../include/lprocfs_status.h"
+#include "mdc_internal.h"
 
 static int mdc_max_rpcs_in_flight_seq_show(struct seq_file *m, void *v)
 {
 	struct obd_device *dev = m->private;
 	struct client_obd *cli = &dev->u.cli;
-	int rc;
 
 	client_obd_list_lock(&cli->cl_loi_list_lock);
-	rc = seq_printf(m, "%u\n", cli->cl_max_rpcs_in_flight);
+	seq_printf(m, "%u\n", cli->cl_max_rpcs_in_flight);
 	client_obd_list_unlock(&cli->cl_loi_list_lock);
-	return rc;
+
+	return 0;
 }
 
 static ssize_t mdc_max_rpcs_in_flight_seq_write(struct file *file,
diff --git a/drivers/staging/lustre/lustre/mdc/mdc_request.c b/drivers/staging/lustre/lustre/mdc/mdc_request.c
index ef2744700d8b..f8ef5fe5e771 100644
--- a/drivers/staging/lustre/lustre/mdc/mdc_request.c
+++ b/drivers/staging/lustre/lustre/mdc/mdc_request.c
@@ -481,6 +481,9 @@ static int mdc_unpack_acl(struct ptlrpc_request *req, struct lustre_md *md)
 		return -EPROTO;
 
 	acl = posix_acl_from_xattr(&init_user_ns, buf, body->aclsize);
+	if (acl == NULL)
+		return 0;
+
 	if (IS_ERR(acl)) {
 		rc = PTR_ERR(acl);
 		CERROR("convert xattr to acl: %d\n", rc);
@@ -2707,14 +2710,12 @@ static struct md_ops mdc_md_ops = {
 
 static int __init mdc_init(void)
 {
-	int rc;
 	struct lprocfs_static_vars lvars = { NULL };
 
 	lprocfs_mdc_init_vars(&lvars);
 
-	rc = class_register_type(&mdc_obd_ops, &mdc_md_ops, lvars.module_vars,
+	return class_register_type(&mdc_obd_ops, &mdc_md_ops, lvars.module_vars,
 				 LUSTRE_MDC_NAME, NULL);
-	return rc;
 }
 
 static void /*__exit*/ mdc_exit(void)
diff --git a/drivers/staging/lustre/lustre/mgc/mgc_request.c b/drivers/staging/lustre/lustre/mgc/mgc_request.c
index 60d2b0f12693..7947aec5c847 100644
--- a/drivers/staging/lustre/lustre/mgc/mgc_request.c
+++ b/drivers/staging/lustre/lustre/mgc/mgc_request.c
@@ -95,7 +95,7 @@ int mgc_fsname2resid(char *fsname, struct ldlm_res_id *res_id, int type)
 }
 EXPORT_SYMBOL(mgc_fsname2resid);
 
-int mgc_logname2resid(char *logname, struct ldlm_res_id *res_id, int type)
+static int mgc_logname2resid(char *logname, struct ldlm_res_id *res_id, int type)
 {
 	char *name_end;
 	int len;
@@ -160,7 +160,7 @@ struct config_llog_data *config_log_find(char *logname,
 {
 	struct config_llog_data *cld;
 	struct config_llog_data *found = NULL;
-	void *		   instance;
+	void *instance;
 
 	LASSERT(logname != NULL);
 
@@ -452,10 +452,14 @@ static int config_log_end(char *logname, struct config_llog_instance *cfg)
 int lprocfs_mgc_rd_ir_state(struct seq_file *m, void *data)
 {
 	struct obd_device       *obd = data;
-	struct obd_import       *imp = obd->u.cli.cl_import;
-	struct obd_connect_data *ocd = &imp->imp_connect_data;
+	struct obd_import       *imp;
+	struct obd_connect_data *ocd;
 	struct config_llog_data *cld;
 
+	LPROCFS_CLIMP_CHECK(obd);
+	imp = obd->u.cli.cl_import;
+	ocd = &imp->imp_connect_data;
+
 	seq_printf(m, "imperative_recovery: %s\n",
 		      OCD_HAS_FLAG(ocd, IMP_RECOV) ? "ENABLED" : "DISABLED");
 	seq_printf(m, "client_state:\n");
@@ -470,6 +474,7 @@ int lprocfs_mgc_rd_ir_state(struct seq_file *m, void *data)
 	}
 	spin_unlock(&config_list_lock);
 
+	LPROCFS_CLIMP_EXIT(obd);
 	return 0;
 }
 #endif
@@ -479,9 +484,11 @@ int lprocfs_mgc_rd_ir_state(struct seq_file *m, void *data)
 #define RQ_NOW     0x2
 #define RQ_LATER   0x4
 #define RQ_STOP    0x8
-static int		    rq_state = 0;
+#define RQ_PRECLEANUP  0x10
+static int rq_state;
 static wait_queue_head_t	    rq_waitq;
 static DECLARE_COMPLETION(rq_exit);
+static DECLARE_COMPLETION(rq_start);
 
 static void do_requeue(struct config_llog_data *cld)
 {
@@ -510,6 +517,8 @@ static void do_requeue(struct config_llog_data *cld)
 
 static int mgc_requeue_thread(void *data)
 {
+	bool first = true;
+
 	CDEBUG(D_MGC, "Starting requeue thread\n");
 
 	/* Keep trying failed locks periodically */
@@ -526,13 +535,19 @@ static int mgc_requeue_thread(void *data)
 		rq_state &= ~(RQ_NOW | RQ_LATER);
 		spin_unlock(&config_list_lock);
 
+		if (first) {
+			first = false;
+			complete(&rq_start);
+		}
+
 		/* Always wait a few seconds to allow the server who
 		   caused the lock revocation to finish its setup, plus some
 		   random so everyone doesn't try to reconnect at once. */
 		to = MGC_TIMEOUT_MIN_SECONDS * HZ;
 		to += rand * HZ / 100; /* rand is centi-seconds */
 		lwi = LWI_TIMEOUT(to, NULL, NULL);
-		l_wait_event(rq_waitq, rq_state & RQ_STOP, &lwi);
+		l_wait_event(rq_waitq, rq_state & (RQ_STOP | RQ_PRECLEANUP),
+			     &lwi);
 
 		/*
 		 * iterate & processing through the list. for each cld, process
@@ -545,6 +560,7 @@ static int mgc_requeue_thread(void *data)
 		cld_prev = NULL;
 
 		spin_lock(&config_list_lock);
+		rq_state &= ~RQ_PRECLEANUP;
 		list_for_each_entry(cld, &config_llog_list,
 					cld_list_chain) {
 			if (!cld->cld_lostlock)
@@ -661,24 +677,26 @@ static atomic_t mgc_count = ATOMIC_INIT(0);
 static int mgc_precleanup(struct obd_device *obd, enum obd_cleanup_stage stage)
 {
 	int rc = 0;
+	int temp;
 
 	switch (stage) {
 	case OBD_CLEANUP_EARLY:
 		break;
 	case OBD_CLEANUP_EXPORTS:
 		if (atomic_dec_and_test(&mgc_count)) {
-			int running;
+			LASSERT(rq_state & RQ_RUNNING);
 			/* stop requeue thread */
-			spin_lock(&config_list_lock);
-			running = rq_state & RQ_RUNNING;
-			if (running)
-				rq_state |= RQ_STOP;
-			spin_unlock(&config_list_lock);
-			if (running) {
-				wake_up(&rq_waitq);
-				wait_for_completion(&rq_exit);
-			}
+			temp = RQ_STOP;
+		} else {
+			/* wakeup requeue thread to clean our cld */
+			temp = RQ_NOW | RQ_PRECLEANUP;
 		}
+		spin_lock(&config_list_lock);
+		rq_state |= temp;
+		spin_unlock(&config_list_lock);
+		wake_up(&rq_waitq);
+		if (temp & RQ_STOP)
+			wait_for_completion(&rq_exit);
 		obd_cleanup_client_import(obd);
 		rc = mgc_llog_fini(NULL, obd);
 		if (rc != 0)
@@ -690,8 +708,6 @@ static int mgc_precleanup(struct obd_device *obd, enum obd_cleanup_stage stage)
 
 static int mgc_cleanup(struct obd_device *obd)
 {
-	int rc;
-
 	/* COMPAT_146 - old config logs may have added profiles we don't
 	   know about */
 	if (obd->obd_type->typ_refcnt <= 1)
@@ -701,8 +717,7 @@ static int mgc_cleanup(struct obd_device *obd)
 	lprocfs_obd_cleanup(obd);
 	ptlrpcd_decref();
 
-	rc = client_obd_cleanup(obd);
-	return rc;
+	return client_obd_cleanup(obd);
 }
 
 static int mgc_setup(struct obd_device *obd, struct lustre_cfg *lcfg)
@@ -740,6 +755,7 @@ static int mgc_setup(struct obd_device *obd, struct lustre_cfg *lcfg)
 		}
 		/* rc is the task_struct pointer of mgc_requeue_thread. */
 		rc = 0;
+		wait_for_completion(&rq_start);
 	}
 
 	return rc;
@@ -951,7 +967,7 @@ static int mgc_target_register(struct obd_export *exp,
 	return rc;
 }
 
-int mgc_set_info_async(const struct lu_env *env, struct obd_export *exp,
+static int mgc_set_info_async(const struct lu_env *env, struct obd_export *exp,
 		       u32 keylen, void *key, u32 vallen,
 		       void *val, struct ptlrpc_request_set *set)
 {
@@ -1727,7 +1743,7 @@ struct obd_ops mgc_obd_ops = {
 	.o_process_config = mgc_process_config,
 };
 
-int __init mgc_init(void)
+static int __init mgc_init(void)
 {
 	return class_register_type(&mgc_obd_ops, NULL, NULL,
 				   LUSTRE_MGC_NAME, NULL);
diff --git a/drivers/staging/lustre/lustre/obdclass/class_obd.c b/drivers/staging/lustre/lustre/obdclass/class_obd.c
index 29456e1ad225..d4b74b670c43 100644
--- a/drivers/staging/lustre/lustre/obdclass/class_obd.c
+++ b/drivers/staging/lustre/lustre/obdclass/class_obd.c
@@ -437,8 +437,6 @@ int class_handle_ioctl(unsigned int cmd, unsigned long arg)
 	return err;
 } /* class_handle_ioctl */
 
-extern struct miscdevice obd_psdev;
-
 #define OBD_INIT_CHECK
 int obd_init_checks(void)
 {
@@ -508,7 +506,6 @@ int obd_init_checks(void)
 	return ret;
 }
 
-extern spinlock_t obd_types_lock;
 #if defined (CONFIG_PROC_FS)
 extern int class_procfs_init(void);
 extern int class_procfs_clean(void);
diff --git a/drivers/staging/lustre/lustre/obdclass/dt_object.c b/drivers/staging/lustre/lustre/obdclass/dt_object.c
index e7be26ec7521..b1eee0a6dc9a 100644
--- a/drivers/staging/lustre/lustre/obdclass/dt_object.c
+++ b/drivers/staging/lustre/lustre/obdclass/dt_object.c
@@ -424,11 +424,8 @@ EXPORT_SYMBOL(dt_find_or_create);
 /* dt class init function. */
 int dt_global_init(void)
 {
-	int result;
-
 	LU_CONTEXT_KEY_INIT(&dt_key);
-	result = lu_context_key_register(&dt_key);
-	return result;
+	return lu_context_key_register(&dt_key);
 }
 
 void dt_global_fini(void)
diff --git a/drivers/staging/lustre/lustre/obdclass/genops.c b/drivers/staging/lustre/lustre/obdclass/genops.c
index 82508210465e..66b56784f674 100644
--- a/drivers/staging/lustre/lustre/obdclass/genops.c
+++ b/drivers/staging/lustre/lustre/obdclass/genops.c
@@ -43,17 +43,16 @@
 #include "../include/obd_class.h"
 #include "../include/lprocfs_status.h"
 
-extern struct list_head obd_types;
 spinlock_t obd_types_lock;
 
 struct kmem_cache *obd_device_cachep;
 struct kmem_cache *obdo_cachep;
 EXPORT_SYMBOL(obdo_cachep);
-struct kmem_cache *import_cachep;
+static struct kmem_cache *import_cachep;
 
-struct list_head      obd_zombie_imports;
-struct list_head      obd_zombie_exports;
-spinlock_t  obd_zombie_impexp_lock;
+static struct list_head      obd_zombie_imports;
+static struct list_head      obd_zombie_exports;
+static spinlock_t  obd_zombie_impexp_lock;
 static void obd_zombie_impexp_notify(void);
 static void obd_zombie_export_add(struct obd_export *exp);
 static void obd_zombie_import_add(struct obd_import *imp);
@@ -930,7 +929,7 @@ void class_unlink_export(struct obd_export *exp)
 EXPORT_SYMBOL(class_unlink_export);
 
 /* Import management functions */
-void class_import_destroy(struct obd_import *imp)
+static void class_import_destroy(struct obd_import *imp)
 {
 	CDEBUG(D_IOCTL, "destroying import %p for %s\n", imp,
 		imp->imp_obd->obd_name);
@@ -1127,7 +1126,7 @@ int class_connect(struct lustre_handle *conn, struct obd_device *obd,
 EXPORT_SYMBOL(class_connect);
 
 /* if export is involved in recovery then clean up related things */
-void class_export_recovery_cleanup(struct obd_export *exp)
+static void class_export_recovery_cleanup(struct obd_export *exp)
 {
 	struct obd_device *obd = exp->exp_obd;
 
@@ -1221,7 +1220,7 @@ int class_connected_export(struct obd_export *exp)
 	if (exp) {
 		int connected;
 		spin_lock(&exp->exp_lock);
-		connected = (exp->exp_conn_cnt > 0);
+		connected = exp->exp_conn_cnt > 0;
 		spin_unlock(&exp->exp_lock);
 		return connected;
 	}
@@ -1559,7 +1558,7 @@ void obd_exports_barrier(struct obd_device *obd)
 EXPORT_SYMBOL(obd_exports_barrier);
 
 /* Total amount of zombies to be destroyed */
-static int zombies_count = 0;
+static int zombies_count;
 
 /**
  * kill zombie imports and exports
diff --git a/drivers/staging/lustre/lustre/obdclass/linux/linux-module.c b/drivers/staging/lustre/lustre/obdclass/linux/linux-module.c
index b94aeac18a37..06944b863d16 100644
--- a/drivers/staging/lustre/lustre/obdclass/linux/linux-module.c
+++ b/drivers/staging/lustre/lustre/obdclass/linux/linux-module.c
@@ -217,23 +217,25 @@ struct miscdevice obd_psdev = {
 
 
 #if defined (CONFIG_PROC_FS)
-int obd_proc_version_seq_show(struct seq_file *m, void *v)
+static int obd_proc_version_seq_show(struct seq_file *m, void *v)
 {
-	return seq_printf(m, "lustre: %s\nkernel: %s\nbuild:  %s\n",
-			LUSTRE_VERSION_STRING, "patchless_client",
-			BUILD_VERSION);
+	seq_printf(m, "lustre: %s\nkernel: %s\nbuild:  %s\n",
+		   LUSTRE_VERSION_STRING, "patchless_client", BUILD_VERSION);
+	return 0;
 }
 LPROC_SEQ_FOPS_RO(obd_proc_version);
 
 int obd_proc_pinger_seq_show(struct seq_file *m, void *v)
 {
-	return seq_printf(m, "%s\n", "on");
+	seq_printf(m, "%s\n", "on");
+	return 0;
 }
 LPROC_SEQ_FOPS_RO(obd_proc_pinger);
 
 static int obd_proc_health_seq_show(struct seq_file *m, void *v)
 {
-	int rc = 0, i;
+	bool healthy = true;
+	int i;
 
 	if (libcfs_catastrophe)
 		seq_printf(m, "LBUG\n");
@@ -255,25 +257,27 @@ static int obd_proc_health_seq_show(struct seq_file *m, void *v)
 
 		if (obd_health_check(NULL, obd)) {
 			seq_printf(m, "device %s reported unhealthy\n",
-				      obd->obd_name);
-			rc++;
+				   obd->obd_name);
+			healthy = false;
 		}
 		class_decref(obd, __func__, current);
 		read_lock(&obd_dev_lock);
 	}
 	read_unlock(&obd_dev_lock);
 
-	if (rc == 0)
-		return seq_printf(m, "healthy\n");
+	if (healthy)
+		seq_puts(m, "healthy\n");
+	else
+		seq_puts(m, "NOT HEALTHY\n");
 
-	seq_printf(m, "NOT HEALTHY\n");
 	return 0;
 }
 LPROC_SEQ_FOPS_RO(obd_proc_health);
 
 static int obd_proc_jobid_var_seq_show(struct seq_file *m, void *v)
 {
-	return seq_printf(m, "%s\n", obd_jobid_var);
+	seq_printf(m, "%s\n", obd_jobid_var);
+	return 0;
 }
 
 static ssize_t obd_proc_jobid_var_seq_write(struct file *file,
@@ -299,7 +303,8 @@ LPROC_SEQ_FOPS(obd_proc_jobid_var);
 
 static int obd_proc_jobid_name_seq_show(struct seq_file *m, void *v)
 {
-	return seq_printf(m, "%s\n", obd_jobid_var);
+	seq_printf(m, "%s\n", obd_jobid_var);
+	return 0;
 }
 
 static ssize_t obd_proc_jobid_name_seq_write(struct file *file,
@@ -378,10 +383,11 @@ static int obd_device_list_seq_show(struct seq_file *p, void *v)
 	else
 		status = "--";
 
-	return seq_printf(p, "%3d %s %s %s %s %d\n",
-			  (int)index, status, obd->obd_type->typ_name,
-			  obd->obd_name, obd->obd_uuid.uuid,
-			  atomic_read(&obd->obd_refcount));
+	seq_printf(p, "%3d %s %s %s %s %d\n",
+		   (int)index, status, obd->obd_type->typ_name,
+		   obd->obd_name, obd->obd_uuid.uuid,
+		   atomic_read(&obd->obd_refcount));
+	return 0;
 }
 
 struct seq_operations obd_device_list_sops = {
diff --git a/drivers/staging/lustre/lustre/obdclass/linux/linux-sysctl.c b/drivers/staging/lustre/lustre/obdclass/linux/linux-sysctl.c
index dd46e7358160..4b62d25764ad 100644
--- a/drivers/staging/lustre/lustre/obdclass/linux/linux-sysctl.c
+++ b/drivers/staging/lustre/lustre/obdclass/linux/linux-sysctl.c
@@ -79,6 +79,7 @@ enum {
 };
 
 
+#ifdef CONFIG_SYSCTL
 static int proc_set_timeout(struct ctl_table *table, int write,
 			void __user *buffer, size_t *lenp, loff_t *ppos)
 {
@@ -205,7 +206,7 @@ static int proc_max_dirty_pages_in_mb(struct ctl_table *table, int write,
 			CERROR("Refusing to set max dirty pages to %u, which is more than 90%% of available RAM; setting to %lu\n",
 			       obd_max_dirty_pages,
 			       ((totalram_pages / 10) * 9));
-			obd_max_dirty_pages = ((totalram_pages / 10) * 9);
+			obd_max_dirty_pages = (totalram_pages / 10) * 9;
 		} else if (obd_max_dirty_pages < 4 << (20 - PAGE_CACHE_SHIFT)) {
 			obd_max_dirty_pages = 4 << (20 - PAGE_CACHE_SHIFT);
 		}
@@ -258,7 +259,6 @@ static int proc_alloc_fail_rate(struct ctl_table *table, int write,
 	return rc;
 }
 
-#ifdef CONFIG_SYSCTL
 static struct ctl_table obd_table[] = {
 	{
 		.procname = "timeout",
diff --git a/drivers/staging/lustre/lustre/obdclass/llog_cat.c b/drivers/staging/lustre/lustre/obdclass/llog_cat.c
index 4b850fc5f5d9..c8f6ab006124 100644
--- a/drivers/staging/lustre/lustre/obdclass/llog_cat.c
+++ b/drivers/staging/lustre/lustre/obdclass/llog_cat.c
@@ -526,8 +526,9 @@ int llog_cat_cancel_records(const struct lu_env *env,
 }
 EXPORT_SYMBOL(llog_cat_cancel_records);
 
-int llog_cat_process_cb(const struct lu_env *env, struct llog_handle *cat_llh,
-			struct llog_rec_hdr *rec, void *data)
+static int llog_cat_process_cb(const struct lu_env *env,
+			       struct llog_handle *cat_llh,
+			       struct llog_rec_hdr *rec, void *data)
 {
 	struct llog_process_data *d = data;
 	struct llog_logid_rec *lir = (struct llog_logid_rec *)rec;
@@ -691,7 +692,7 @@ int llog_cat_reverse_process(const struct lu_env *env,
 }
 EXPORT_SYMBOL(llog_cat_reverse_process);
 
-int llog_cat_set_first_idx(struct llog_handle *cathandle, int index)
+static int llog_cat_set_first_idx(struct llog_handle *cathandle, int index)
 {
 	struct llog_log_hdr *llh = cathandle->lgh_hdr;
 	int i, bitmap_size, idx;
@@ -750,7 +751,7 @@ int llog_cat_cleanup(const struct lu_env *env, struct llog_handle *cathandle,
 	return rc;
 }
 
-int cat_cancel_cb(const struct lu_env *env, struct llog_handle *cathandle,
+static int cat_cancel_cb(const struct lu_env *env, struct llog_handle *cathandle,
 		  struct llog_rec_hdr *rec, void *data)
 {
 	struct llog_logid_rec	*lir = (struct llog_logid_rec *)rec;
@@ -794,7 +795,6 @@ int cat_cancel_cb(const struct lu_env *env, struct llog_handle *cathandle,
 
 	return rc;
 }
-EXPORT_SYMBOL(cat_cancel_cb);
 
 /* helper to initialize catalog llog and process it to cancel */
 int llog_cat_init_and_process(const struct lu_env *env,
diff --git a/drivers/staging/lustre/lustre/obdclass/lprocfs_status.c b/drivers/staging/lustre/lustre/obdclass/lprocfs_status.c
index ddab94d7ee82..c171c6c6c457 100644
--- a/drivers/staging/lustre/lustre/obdclass/lprocfs_status.c
+++ b/drivers/staging/lustre/lustre/obdclass/lprocfs_status.c
@@ -223,7 +223,7 @@ EXPORT_SYMBOL(lprocfs_write_frac_helper);
 
 #if defined (CONFIG_PROC_FS)
 
-static int lprocfs_no_percpu_stats = 0;
+static int lprocfs_no_percpu_stats;
 module_param(lprocfs_no_percpu_stats, int, 0644);
 MODULE_PARM_DESC(lprocfs_no_percpu_stats, "Do not alloc percpu data for lprocfs stats");
 
@@ -377,7 +377,8 @@ EXPORT_SYMBOL(lprocfs_register);
 /* Generic callbacks */
 int lprocfs_rd_uint(struct seq_file *m, void *data)
 {
-	return seq_printf(m, "%u\n", *(unsigned int *)data);
+	seq_printf(m, "%u\n", *(unsigned int *)data);
+	return 0;
 }
 EXPORT_SYMBOL(lprocfs_rd_uint);
 
@@ -403,7 +404,8 @@ EXPORT_SYMBOL(lprocfs_wr_uint);
 
 int lprocfs_rd_u64(struct seq_file *m, void *data)
 {
-	return seq_printf(m, "%llu\n", *(__u64 *)data);
+	seq_printf(m, "%llu\n", *(__u64 *)data);
+	return 0;
 }
 EXPORT_SYMBOL(lprocfs_rd_u64);
 
@@ -411,7 +413,8 @@ int lprocfs_rd_atomic(struct seq_file *m, void *data)
 {
 	atomic_t *atom = data;
 	LASSERT(atom != NULL);
-	return seq_printf(m, "%d\n", atomic_read(atom));
+	seq_printf(m, "%d\n", atomic_read(atom));
+	return 0;
 }
 EXPORT_SYMBOL(lprocfs_rd_atomic);
 
@@ -439,7 +442,8 @@ int lprocfs_rd_uuid(struct seq_file *m, void *data)
 	struct obd_device *obd = data;
 
 	LASSERT(obd != NULL);
-	return seq_printf(m, "%s\n", obd->obd_uuid.uuid);
+	seq_printf(m, "%s\n", obd->obd_uuid.uuid);
+	return 0;
 }
 EXPORT_SYMBOL(lprocfs_rd_uuid);
 
@@ -448,7 +452,8 @@ int lprocfs_rd_name(struct seq_file *m, void *data)
 	struct obd_device *dev = data;
 
 	LASSERT(dev != NULL);
-	return seq_printf(m, "%s\n", dev->obd_name);
+	seq_printf(m, "%s\n", dev->obd_name);
+	return 0;
 }
 EXPORT_SYMBOL(lprocfs_rd_name);
 
@@ -460,7 +465,8 @@ int lprocfs_rd_blksize(struct seq_file *m, void *data)
 			    cfs_time_shift_64(-OBD_STATFS_CACHE_SECONDS),
 			    OBD_STATFS_NODELAY);
 	if (!rc)
-		rc = seq_printf(m, "%u\n", osfs.os_bsize);
+		seq_printf(m, "%u\n", osfs.os_bsize);
+
 	return rc;
 }
 EXPORT_SYMBOL(lprocfs_rd_blksize);
@@ -479,8 +485,9 @@ int lprocfs_rd_kbytestotal(struct seq_file *m, void *data)
 		while (blk_size >>= 1)
 			result <<= 1;
 
-		rc = seq_printf(m, "%llu\n", result);
+		seq_printf(m, "%llu\n", result);
 	}
+
 	return rc;
 }
 EXPORT_SYMBOL(lprocfs_rd_kbytestotal);
@@ -499,8 +506,9 @@ int lprocfs_rd_kbytesfree(struct seq_file *m, void *data)
 		while (blk_size >>= 1)
 			result <<= 1;
 
-		rc = seq_printf(m, "%llu\n", result);
+		seq_printf(m, "%llu\n", result);
 	}
+
 	return rc;
 }
 EXPORT_SYMBOL(lprocfs_rd_kbytesfree);
@@ -519,8 +527,9 @@ int lprocfs_rd_kbytesavail(struct seq_file *m, void *data)
 		while (blk_size >>= 1)
 			result <<= 1;
 
-		rc = seq_printf(m, "%llu\n", result);
+		seq_printf(m, "%llu\n", result);
 	}
+
 	return rc;
 }
 EXPORT_SYMBOL(lprocfs_rd_kbytesavail);
@@ -533,7 +542,7 @@ int lprocfs_rd_filestotal(struct seq_file *m, void *data)
 			    cfs_time_shift_64(-OBD_STATFS_CACHE_SECONDS),
 			    OBD_STATFS_NODELAY);
 	if (!rc)
-		rc = seq_printf(m, "%llu\n", osfs.os_files);
+		seq_printf(m, "%llu\n", osfs.os_files);
 
 	return rc;
 }
@@ -547,7 +556,8 @@ int lprocfs_rd_filesfree(struct seq_file *m, void *data)
 			    cfs_time_shift_64(-OBD_STATFS_CACHE_SECONDS),
 			    OBD_STATFS_NODELAY);
 	if (!rc)
-		rc = seq_printf(m, "%llu\n", osfs.os_ffree);
+		seq_printf(m, "%llu\n", osfs.os_ffree);
+
 	return rc;
 }
 EXPORT_SYMBOL(lprocfs_rd_filesfree);
@@ -557,17 +567,18 @@ int lprocfs_rd_server_uuid(struct seq_file *m, void *data)
 	struct obd_device *obd = data;
 	struct obd_import *imp;
 	char *imp_state_name = NULL;
-	int rc = 0;
 
 	LASSERT(obd != NULL);
 	LPROCFS_CLIMP_CHECK(obd);
 	imp = obd->u.cli.cl_import;
 	imp_state_name = ptlrpc_import_state_name(imp->imp_state);
-	rc = seq_printf(m, "%s\t%s%s\n", obd2cli_tgt(obd), imp_state_name,
-			imp->imp_deactive ? "\tDEACTIVATED" : "");
+	seq_printf(m, "%s\t%s%s\n",
+		   obd2cli_tgt(obd), imp_state_name,
+		   imp->imp_deactive ? "\tDEACTIVATED" : "");
 
 	LPROCFS_CLIMP_EXIT(obd);
-	return rc;
+
+	return 0;
 }
 EXPORT_SYMBOL(lprocfs_rd_server_uuid);
 
@@ -575,19 +586,19 @@ int lprocfs_rd_conn_uuid(struct seq_file *m, void *data)
 {
 	struct obd_device *obd = data;
 	struct ptlrpc_connection *conn;
-	int rc = 0;
 
 	LASSERT(obd != NULL);
 
 	LPROCFS_CLIMP_CHECK(obd);
 	conn = obd->u.cli.cl_import->imp_connection;
 	if (conn && obd->u.cli.cl_import)
-		rc = seq_printf(m, "%s\n", conn->c_remote_uuid.uuid);
+		seq_printf(m, "%s\n", conn->c_remote_uuid.uuid);
 	else
-		rc = seq_printf(m, "%s\n", "<none>");
+		seq_puts(m, "<none>\n");
 
 	LPROCFS_CLIMP_EXIT(obd);
-	return rc;
+
+	return 0;
 }
 EXPORT_SYMBOL(lprocfs_rd_conn_uuid);
 
@@ -924,7 +935,8 @@ int lprocfs_rd_num_exports(struct seq_file *m, void *data)
 	struct obd_device *obd = data;
 
 	LASSERT(obd != NULL);
-	return seq_printf(m, "%u\n", obd->obd_num_exports);
+	seq_printf(m, "%u\n", obd->obd_num_exports);
+	return 0;
 }
 EXPORT_SYMBOL(lprocfs_rd_num_exports);
 
@@ -933,7 +945,8 @@ int lprocfs_rd_numrefs(struct seq_file *m, void *data)
 	struct obd_type *class = (struct obd_type *) data;
 
 	LASSERT(class != NULL);
-	return seq_printf(m, "%d\n", class->typ_refcnt);
+	seq_printf(m, "%d\n", class->typ_refcnt);
+	return 0;
 }
 EXPORT_SYMBOL(lprocfs_rd_numrefs);
 
@@ -1200,41 +1213,33 @@ static int lprocfs_stats_seq_show(struct seq_file *p, void *v)
 	struct lprocfs_counter_header   *hdr;
 	struct lprocfs_counter           ctr;
 	int                              idx    = *(loff_t *)v;
-	int                              rc     = 0;
 
 	if (idx == 0) {
 		struct timeval now;
 		do_gettimeofday(&now);
-		rc = seq_printf(p, "%-25s %lu.%lu secs.usecs\n",
-				"snapshot_time", now.tv_sec, (unsigned long)now.tv_usec);
-		if (rc < 0)
-			return rc;
+		seq_printf(p, "%-25s %lu.%lu secs.usecs\n",
+			   "snapshot_time",
+			   now.tv_sec, (unsigned long)now.tv_usec);
 	}
+
 	hdr = &stats->ls_cnt_header[idx];
 	lprocfs_stats_collect(stats, idx, &ctr);
 
-	if (ctr.lc_count == 0)
-		goto out;
-
-	rc = seq_printf(p, "%-25s %lld samples [%s]", hdr->lc_name,
-			ctr.lc_count, hdr->lc_units);
+	if (ctr.lc_count != 0) {
+		seq_printf(p, "%-25s %lld samples [%s]",
+			   hdr->lc_name, ctr.lc_count, hdr->lc_units);
 
-	if (rc < 0)
-		goto out;
-
-	if ((hdr->lc_config & LPROCFS_CNTR_AVGMINMAX) && (ctr.lc_count > 0)) {
-		rc = seq_printf(p, " %lld %lld %lld",
-				ctr.lc_min, ctr.lc_max, ctr.lc_sum);
-		if (rc < 0)
-			goto out;
-		if (hdr->lc_config & LPROCFS_CNTR_STDDEV)
-			rc = seq_printf(p, " %lld", ctr.lc_sumsquare);
-		if (rc < 0)
-			goto out;
+		if ((hdr->lc_config & LPROCFS_CNTR_AVGMINMAX) &&
+		    (ctr.lc_count > 0)) {
+			seq_printf(p, " %lld %lld %lld",
+				   ctr.lc_min, ctr.lc_max, ctr.lc_sum);
+			if (hdr->lc_config & LPROCFS_CNTR_STDDEV)
+				seq_printf(p, " %lld", ctr.lc_sumsquare);
+		}
+		seq_putc(p, '\n');
 	}
-	rc = seq_printf(p, "\n");
-out:
-	return (rc < 0) ? rc : 0;
+
+	return 0;
 }
 
 static const struct seq_operations lprocfs_stats_seq_sops = {
@@ -1606,8 +1611,9 @@ LPROC_SEQ_FOPS_RO(lproc_exp_hash);
 
 int lprocfs_nid_stats_clear_read(struct seq_file *m, void *data)
 {
-	return seq_printf(m, "%s\n",
-			  "Write into this file to clear all nid stats and stale nid entries");
+	seq_printf(m, "%s\n",
+		   "Write into this file to clear all nid stats and stale nid entries");
+	return 0;
 }
 EXPORT_SYMBOL(lprocfs_nid_stats_clear_read);
 
@@ -2041,12 +2047,12 @@ int lprocfs_obd_rd_max_pages_per_rpc(struct seq_file *m, void *data)
 {
 	struct obd_device *dev = data;
 	struct client_obd *cli = &dev->u.cli;
-	int rc;
 
 	client_obd_list_lock(&cli->cl_loi_list_lock);
-	rc = seq_printf(m, "%d\n", cli->cl_max_pages_per_rpc);
+	seq_printf(m, "%d\n", cli->cl_max_pages_per_rpc);
 	client_obd_list_unlock(&cli->cl_loi_list_lock);
-	return rc;
+
+	return 0;
 }
 EXPORT_SYMBOL(lprocfs_obd_rd_max_pages_per_rpc);
 
diff --git a/drivers/staging/lustre/lustre/obdclass/lu_object.c b/drivers/staging/lustre/lustre/obdclass/lu_object.c
index 83bf168c2939..20c0779951fd 100644
--- a/drivers/staging/lustre/lustre/obdclass/lu_object.c
+++ b/drivers/staging/lustre/lustre/obdclass/lu_object.c
@@ -1308,7 +1308,7 @@ static DEFINE_SPINLOCK(lu_keys_guard);
  * lu_context_refill(). No locking is provided, as initialization and shutdown
  * are supposed to be externally serialized.
  */
-static unsigned key_set_version = 0;
+static unsigned key_set_version;
 
 /**
  * Register new key.
@@ -1769,8 +1769,6 @@ EXPORT_SYMBOL(lu_env_refill);
 int lu_env_refill_by_tags(struct lu_env *env, __u32 ctags,
 			  __u32 stags)
 {
-	int    result;
-
 	if ((env->le_ctx.lc_tags & ctags) != ctags) {
 		env->le_ctx.lc_version = 0;
 		env->le_ctx.lc_tags |= ctags;
@@ -1781,9 +1779,7 @@ int lu_env_refill_by_tags(struct lu_env *env, __u32 ctags,
 		env->le_ses->lc_tags |= stags;
 	}
 
-	result = lu_env_refill(env);
-
-	return result;
+	return lu_env_refill(env);
 }
 EXPORT_SYMBOL(lu_env_refill_by_tags);
 
@@ -2014,18 +2010,19 @@ int lu_site_stats_print(const struct lu_site *s, struct seq_file *m)
 	memset(&stats, 0, sizeof(stats));
 	lu_site_stats_get(s->ls_obj_hash, &stats, 1);
 
-	return seq_printf(m, "%d/%d %d/%d %d %d %d %d %d %d %d\n",
-			stats.lss_busy,
-			stats.lss_total,
-			stats.lss_populated,
-			CFS_HASH_NHLIST(s->ls_obj_hash),
-			stats.lss_max_search,
-			ls_stats_read(s->ls_stats, LU_SS_CREATED),
-			ls_stats_read(s->ls_stats, LU_SS_CACHE_HIT),
-			ls_stats_read(s->ls_stats, LU_SS_CACHE_MISS),
-			ls_stats_read(s->ls_stats, LU_SS_CACHE_RACE),
-			ls_stats_read(s->ls_stats, LU_SS_CACHE_DEATH_RACE),
-			ls_stats_read(s->ls_stats, LU_SS_LRU_PURGED));
+	seq_printf(m, "%d/%d %d/%d %d %d %d %d %d %d %d\n",
+		   stats.lss_busy,
+		   stats.lss_total,
+		   stats.lss_populated,
+		   CFS_HASH_NHLIST(s->ls_obj_hash),
+		   stats.lss_max_search,
+		   ls_stats_read(s->ls_stats, LU_SS_CREATED),
+		   ls_stats_read(s->ls_stats, LU_SS_CACHE_HIT),
+		   ls_stats_read(s->ls_stats, LU_SS_CACHE_MISS),
+		   ls_stats_read(s->ls_stats, LU_SS_CACHE_RACE),
+		   ls_stats_read(s->ls_stats, LU_SS_CACHE_DEATH_RACE),
+		   ls_stats_read(s->ls_stats, LU_SS_LRU_PURGED));
+	return 0;
 }
 EXPORT_SYMBOL(lu_site_stats_print);
 
diff --git a/drivers/staging/lustre/lustre/obdclass/obd_mount.c b/drivers/staging/lustre/lustre/obdclass/obd_mount.c
index 3c0c9109cefd..3437b2ecfc02 100644
--- a/drivers/staging/lustre/lustre/obdclass/obd_mount.c
+++ b/drivers/staging/lustre/lustre/obdclass/obd_mount.c
@@ -645,10 +645,10 @@ int lustre_put_lsi(struct super_block *sb)
 }
 
 /*** SERVER NAME ***
- * <FSNAME><SEPERATOR><TYPE><INDEX>
+ * <FSNAME><SEPARATOR><TYPE><INDEX>
  * FSNAME is between 1 and 8 characters (inclusive).
  *	Excluded characters are '/' and ':'
- * SEPERATOR is either ':' or '-'
+ * SEPARATOR is either ':' or '-'
  * TYPE: "OST", "MDT", etc.
  * INDEX: Hex representation of the index
  */
@@ -1286,7 +1286,7 @@ struct dentry *lustre_mount(struct file_system_type *fs_type, int flags,
 	return mount_nodev(fs_type, flags, &lmd2, lustre_fill_super);
 }
 
-void lustre_kill_super(struct super_block *sb)
+static void lustre_kill_super(struct super_block *sb)
 {
 	struct lustre_sb_info *lsi = s2lsi(sb);
 
diff --git a/drivers/staging/lustre/lustre/obdecho/echo_client.c b/drivers/staging/lustre/lustre/obdecho/echo_client.c
index 5f6d9441bc44..d542e06d6cd3 100644
--- a/drivers/staging/lustre/lustre/obdecho/echo_client.c
+++ b/drivers/staging/lustre/lustre/obdecho/echo_client.c
@@ -146,6 +146,7 @@ static struct lu_context_key echo_thread_key;
 static inline struct echo_thread_info *echo_env_info(const struct lu_env *env)
 {
 	struct echo_thread_info *info;
+
 	info = lu_context_key_get(&env->le_ctx, &echo_thread_key);
 	LASSERT(info != NULL);
 	return info;
@@ -195,22 +196,22 @@ static struct lu_kmem_descr echo_caches[] = {
 	{
 		.ckd_cache = &echo_lock_kmem,
 		.ckd_name  = "echo_lock_kmem",
-		.ckd_size  = sizeof (struct echo_lock)
+		.ckd_size  = sizeof(struct echo_lock)
 	},
 	{
 		.ckd_cache = &echo_object_kmem,
 		.ckd_name  = "echo_object_kmem",
-		.ckd_size  = sizeof (struct echo_object)
+		.ckd_size  = sizeof(struct echo_object)
 	},
 	{
 		.ckd_cache = &echo_thread_kmem,
 		.ckd_name  = "echo_thread_kmem",
-		.ckd_size  = sizeof (struct echo_thread_info)
+		.ckd_size  = sizeof(struct echo_thread_info)
 	},
 	{
 		.ckd_cache = &echo_session_kmem,
 		.ckd_name  = "echo_session_kmem",
-		.ckd_size  = sizeof (struct echo_session_info)
+		.ckd_size  = sizeof(struct echo_session_info)
 	},
 	{
 		.ckd_cache = NULL
@@ -637,6 +638,7 @@ static void echo_thread_key_fini(const struct lu_context *ctx,
 			 struct lu_context_key *key, void *data)
 {
 	struct echo_thread_info *info = data;
+
 	OBD_SLAB_FREE_PTR(info, echo_thread_kmem);
 }
 
@@ -667,6 +669,7 @@ static void echo_session_key_fini(const struct lu_context *ctx,
 				 struct lu_context_key *key, void *data)
 {
 	struct echo_session_info *session = data;
+
 	OBD_SLAB_FREE_PTR(session, echo_session_kmem);
 }
 
@@ -783,6 +786,7 @@ out:
 	switch (cleanup) {
 	case 4: {
 		int rc2;
+
 		rc2 = echo_client_cleanup(obd);
 		if (rc2)
 			CERROR("Cleanup obd device %s error(%d)\n",
@@ -958,11 +962,13 @@ static struct echo_object *cl_echo_object_find(struct echo_device *d,
 	if (d->ed_next) {
 		if (!d->ed_next_islov) {
 			struct lov_oinfo *oinfo = lsm->lsm_oinfo[0];
+
 			LASSERT(oinfo != NULL);
 			oinfo->loi_oi = lsm->lsm_oi;
 			conf->eoc_cl.u.coc_oinfo = oinfo;
 		} else {
 			struct lustre_md *md;
+
 			md = &info->eti_md;
 			memset(md, 0, sizeof(*md));
 			md->lsm = lsm;
@@ -1011,6 +1017,7 @@ static int cl_echo_object_put(struct echo_object *eco)
 	/* an external function to kill an object? */
 	if (eco->eo_deleted) {
 		struct lu_object_header *loh = obj->co_lu.lo_header;
+
 		LASSERT(&eco->eo_hdr == luh2coh(loh));
 		set_bit(LU_OBJECT_HEARD_BANSHEE, &loh->loh_flags);
 	}
@@ -1022,7 +1029,7 @@ static int cl_echo_object_put(struct echo_object *eco)
 
 static int cl_echo_enqueue0(struct lu_env *env, struct echo_object *eco,
 			    u64 start, u64 end, int mode,
-			    __u64 *cookie , __u32 enqflags)
+			    __u64 *cookie, __u32 enqflags)
 {
 	struct cl_io *io;
 	struct cl_lock *lck;
@@ -1106,8 +1113,8 @@ static int cl_echo_cancel0(struct lu_env *env, struct echo_device *ed,
 
 	LASSERT(ec != NULL);
 	spin_lock(&ec->ec_lock);
-	list_for_each (el, &ec->ec_locks) {
-		ecl = list_entry (el, struct echo_lock, el_chain);
+	list_for_each(el, &ec->ec_locks) {
+		ecl = list_entry(el, struct echo_lock, el_chain);
 		CDEBUG(D_INFO, "ecl: %p, cookie: %#llx\n", ecl, ecl->el_cookie);
 		found = (ecl->el_cookie == cookie);
 		if (found) {
@@ -1152,6 +1159,7 @@ static int cl_echo_async_brw(const struct lu_env *env, struct cl_io *io,
 
 	cl_page_list_for_each_safe(clp, temp, &queue->c2_qin) {
 		int rc;
+
 		rc = cl_page_cache_add(env, io, clp, CRT_WRITE);
 		if (rc == 0)
 			continue;
@@ -1257,20 +1265,20 @@ out:
 static u64 last_object_id;
 
 static int
-echo_copyout_lsm (struct lov_stripe_md *lsm, void *_ulsm, int ulsm_nob)
+echo_copyout_lsm(struct lov_stripe_md *lsm, void *_ulsm, int ulsm_nob)
 {
 	struct lov_stripe_md *ulsm = _ulsm;
 	int nob, i;
 
-	nob = offsetof (struct lov_stripe_md, lsm_oinfo[lsm->lsm_stripe_count]);
+	nob = offsetof(struct lov_stripe_md, lsm_oinfo[lsm->lsm_stripe_count]);
 	if (nob > ulsm_nob)
 		return -EINVAL;
 
-	if (copy_to_user (ulsm, lsm, sizeof(*ulsm)))
+	if (copy_to_user(ulsm, lsm, sizeof(*ulsm)))
 		return -EFAULT;
 
 	for (i = 0; i < lsm->lsm_stripe_count; i++) {
-		if (copy_to_user (ulsm->lsm_oinfo[i], lsm->lsm_oinfo[i],
+		if (copy_to_user(ulsm->lsm_oinfo[i], lsm->lsm_oinfo[i],
 				      sizeof(lsm->lsm_oinfo[0])))
 			return -EFAULT;
 	}
@@ -1278,16 +1286,16 @@ echo_copyout_lsm (struct lov_stripe_md *lsm, void *_ulsm, int ulsm_nob)
 }
 
 static int
-echo_copyin_lsm (struct echo_device *ed, struct lov_stripe_md *lsm,
+echo_copyin_lsm(struct echo_device *ed, struct lov_stripe_md *lsm,
 		 void *ulsm, int ulsm_nob)
 {
 	struct echo_client_obd *ec = ed->ed_ec;
 	int		     i;
 
-	if (ulsm_nob < sizeof (*lsm))
+	if (ulsm_nob < sizeof(*lsm))
 		return -EINVAL;
 
-	if (copy_from_user (lsm, ulsm, sizeof (*lsm)))
+	if (copy_from_user(lsm, ulsm, sizeof(*lsm)))
 		return -EFAULT;
 
 	if (lsm->lsm_stripe_count > ec->ec_nstripes ||
@@ -1320,7 +1328,7 @@ static int echo_create_object(const struct lu_env *env, struct echo_device *ed,
 	if ((oa->o_valid & OBD_MD_FLID) == 0 && /* no obj id */
 	    (on_target ||		       /* set_stripe */
 	     ec->ec_nstripes != 0)) {	   /* LOV */
-		CERROR ("No valid oid\n");
+		CERROR("No valid oid\n");
 		return -EINVAL;
 	}
 
@@ -1333,7 +1341,7 @@ static int echo_create_object(const struct lu_env *env, struct echo_device *ed,
 	if (ulsm != NULL) {
 		int i, idx;
 
-		rc = echo_copyin_lsm (ed, lsm, ulsm, ulsm_nob);
+		rc = echo_copyin_lsm(ed, lsm, ulsm, ulsm_nob);
 		if (rc != 0)
 			goto failed;
 
@@ -1409,7 +1417,7 @@ static int echo_get_object(struct echo_object **ecop, struct echo_device *ed,
 
 	if ((oa->o_valid & OBD_MD_FLID) == 0 || ostid_id(&oa->o_oi) == 0) {
 		/* disallow use of object id 0 */
-		CERROR ("No valid oid\n");
+		CERROR("No valid oid\n");
 		return -EINVAL;
 	}
 
@@ -1459,7 +1467,7 @@ echo_get_stripe_off_id(struct lov_stripe_md *lsm, u64 *offp, u64 *idp)
 	width = stripe_size * stripe_count;
 
 	/* woffset = offset within a width; offset = whole number of widths */
-	woffset = do_div (offset, width);
+	woffset = do_div(offset, width);
 
 	stripe_index = woffset / stripe_size;
 
@@ -1517,13 +1525,13 @@ static int echo_client_page_debug_check(struct lov_stripe_md *lsm,
 	for (rc = delta = 0; delta < PAGE_CACHE_SIZE; delta += OBD_ECHO_BLOCK_SIZE) {
 		stripe_off = offset + delta;
 		stripe_id = id;
-		echo_get_stripe_off_id (lsm, &stripe_off, &stripe_id);
+		echo_get_stripe_off_id(lsm, &stripe_off, &stripe_id);
 
 		rc2 = block_debug_check("test_brw",
 					addr + delta, OBD_ECHO_BLOCK_SIZE,
 					stripe_off, stripe_id);
 		if (rc2 != 0) {
-			CERROR ("Error in echo object %#llx\n", id);
+			CERROR("Error in echo object %#llx\n", id);
 			rc = rc2;
 		}
 	}
@@ -1583,7 +1591,7 @@ static int echo_client_kbrw(struct echo_device *ed, int rw, struct obdo *oa,
 	     i < npages;
 	     i++, pgp++, off += PAGE_CACHE_SIZE) {
 
-		LASSERT (pgp->pg == NULL);      /* for cleanup */
+		LASSERT(pgp->pg == NULL);      /* for cleanup */
 
 		rc = -ENOMEM;
 		OBD_PAGE_ALLOC(pgp->pg, gfp_mask);
@@ -1615,6 +1623,7 @@ static int echo_client_kbrw(struct echo_device *ed, int rw, struct obdo *oa,
 
 		if (verify) {
 			int vrc;
+
 			vrc = echo_client_page_debug_check(lsm, pgp->pg,
 							   ostid_id(&oa->o_oi),
 							   pgp->off, pgp->count);
@@ -1812,7 +1821,7 @@ echo_client_enqueue(struct obd_export *exp, struct obdo *oa,
 	    (nob & (~CFS_PAGE_MASK)) != 0)
 		return -EINVAL;
 
-	rc = echo_get_object (&eco, ed, oa);
+	rc = echo_get_object(&eco, ed, oa);
 	if (rc != 0)
 		return rc;
 
@@ -1911,6 +1920,7 @@ echo_client_iocontrol(unsigned int cmd, struct obd_export *exp, int len,
 		rc = echo_get_object(&eco, ed, oa);
 		if (rc == 0) {
 			struct obd_info oinfo = { { { 0 } } };
+
 			oinfo.oi_md = eco->eo_lsm;
 			oinfo.oi_oa = oa;
 			rc = obd_getattr(env, ec->ec_exp, &oinfo);
@@ -1927,6 +1937,7 @@ echo_client_iocontrol(unsigned int cmd, struct obd_export *exp, int len,
 		rc = echo_get_object(&eco, ed, oa);
 		if (rc == 0) {
 			struct obd_info oinfo = { { { 0 } } };
+
 			oinfo.oi_oa = oa;
 			oinfo.oi_md = eco->eo_lsm;
 
@@ -1992,7 +2003,7 @@ echo_client_iocontrol(unsigned int cmd, struct obd_export *exp, int len,
 		goto out;
 
 	default:
-		CERROR ("echo_ioctl(): unrecognised ioctl %#x\n", cmd);
+		CERROR("echo_ioctl(): unrecognised ioctl %#x\n", cmd);
 		rc = -ENOTTY;
 		goto out;
 	}
@@ -2034,8 +2045,8 @@ static int echo_client_setup(const struct lu_env *env,
 	}
 
 	spin_lock_init(&ec->ec_lock);
-	INIT_LIST_HEAD (&ec->ec_objects);
-	INIT_LIST_HEAD (&ec->ec_locks);
+	INIT_LIST_HEAD(&ec->ec_objects);
+	INIT_LIST_HEAD(&ec->ec_locks);
 	ec->ec_unique = 0;
 	ec->ec_nstripes = 0;
 
@@ -2158,7 +2169,6 @@ void echo_client_exit(void)
 static int __init obdecho_init(void)
 {
 	struct lprocfs_static_vars lvars;
-	int rc;
 
 	LCONSOLE_INFO("Echo OBD driver; http://www.lustre.org/\n");
 
@@ -2167,9 +2177,7 @@ static int __init obdecho_init(void)
 	lprocfs_echo_init_vars(&lvars);
 
 
-	rc = echo_client_init();
-
-	return rc;
+	return echo_client_init();
 }
 
 static void /*__exit*/ obdecho_exit(void)
diff --git a/drivers/staging/lustre/lustre/obdecho/lproc_echo.c b/drivers/staging/lustre/lustre/obdecho/lproc_echo.c
index 1d3bf6c93129..0beb97db7c7d 100644
--- a/drivers/staging/lustre/lustre/obdecho/lproc_echo.c
+++ b/drivers/staging/lustre/lustre/obdecho/lproc_echo.c
@@ -36,7 +36,7 @@
 #include "../include/lprocfs_status.h"
 #include "../include/obd_class.h"
 
-#if defined (CONFIG_PROC_FS)
+#if defined(CONFIG_PROC_FS)
 LPROC_SEQ_FOPS_RO_TYPE(echo, uuid);
 static struct lprocfs_vars lprocfs_echo_obd_vars[] = {
 	{ "uuid",	 &echo_uuid_fops,	NULL, 0 },
diff --git a/drivers/staging/lustre/lustre/osc/lproc_osc.c b/drivers/staging/lustre/lustre/osc/lproc_osc.c
index 1795d3a7a029..15a66209831c 100644
--- a/drivers/staging/lustre/lustre/osc/lproc_osc.c
+++ b/drivers/staging/lustre/lustre/osc/lproc_osc.c
@@ -35,7 +35,7 @@
  */
 #define DEBUG_SUBSYSTEM S_CLASS
 
-#include <asm/statfs.h>
+#include <linux/statfs.h>
 #include "../include/obd_cksum.h"
 #include "../include/obd_class.h"
 #include "../include/lprocfs_status.h"
@@ -45,12 +45,12 @@
 static int osc_active_seq_show(struct seq_file *m, void *v)
 {
 	struct obd_device *dev = m->private;
-	int rc;
 
 	LPROCFS_CLIMP_CHECK(dev);
-	rc = seq_printf(m, "%d\n", !dev->u.cli.cl_import->imp_deactive);
+	seq_printf(m, "%d\n", !dev->u.cli.cl_import->imp_deactive);
 	LPROCFS_CLIMP_EXIT(dev);
-	return rc;
+
+	return 0;
 }
 
 static ssize_t osc_active_seq_write(struct file *file,
@@ -80,12 +80,12 @@ static int osc_max_rpcs_in_flight_seq_show(struct seq_file *m, void *v)
 {
 	struct obd_device *dev = m->private;
 	struct client_obd *cli = &dev->u.cli;
-	int rc;
 
 	client_obd_list_lock(&cli->cl_loi_list_lock);
-	rc = seq_printf(m, "%u\n", cli->cl_max_rpcs_in_flight);
+	seq_printf(m, "%u\n", cli->cl_max_rpcs_in_flight);
 	client_obd_list_unlock(&cli->cl_loi_list_lock);
-	return rc;
+
+	return 0;
 }
 
 static ssize_t osc_max_rpcs_in_flight_seq_write(struct file *file,
@@ -164,16 +164,15 @@ static int osc_cached_mb_seq_show(struct seq_file *m, void *v)
 	struct obd_device *dev = m->private;
 	struct client_obd *cli = &dev->u.cli;
 	int shift = 20 - PAGE_CACHE_SHIFT;
-	int rc;
 
-	rc = seq_printf(m,
-		      "used_mb: %d\n"
-		      "busy_cnt: %d\n",
-		      (atomic_read(&cli->cl_lru_in_list) +
-			atomic_read(&cli->cl_lru_busy)) >> shift,
-		      atomic_read(&cli->cl_lru_busy));
+	seq_printf(m,
+		   "used_mb: %d\n"
+		   "busy_cnt: %d\n",
+		   (atomic_read(&cli->cl_lru_in_list) +
+		    atomic_read(&cli->cl_lru_busy)) >> shift,
+		   atomic_read(&cli->cl_lru_busy));
 
-	return rc;
+	return 0;
 }
 
 /* shrink the number of caching pages to a specific number */
@@ -215,12 +214,12 @@ static int osc_cur_dirty_bytes_seq_show(struct seq_file *m, void *v)
 {
 	struct obd_device *dev = m->private;
 	struct client_obd *cli = &dev->u.cli;
-	int rc;
 
 	client_obd_list_lock(&cli->cl_loi_list_lock);
-	rc = seq_printf(m, "%lu\n", cli->cl_dirty);
+	seq_printf(m, "%lu\n", cli->cl_dirty);
 	client_obd_list_unlock(&cli->cl_loi_list_lock);
-	return rc;
+
+	return 0;
 }
 LPROC_SEQ_FOPS_RO(osc_cur_dirty_bytes);
 
@@ -228,12 +227,12 @@ static int osc_cur_grant_bytes_seq_show(struct seq_file *m, void *v)
 {
 	struct obd_device *dev = m->private;
 	struct client_obd *cli = &dev->u.cli;
-	int rc;
 
 	client_obd_list_lock(&cli->cl_loi_list_lock);
-	rc = seq_printf(m, "%lu\n", cli->cl_avail_grant);
+	seq_printf(m, "%lu\n", cli->cl_avail_grant);
 	client_obd_list_unlock(&cli->cl_loi_list_lock);
-	return rc;
+
+	return 0;
 }
 
 static ssize_t osc_cur_grant_bytes_seq_write(struct file *file,
@@ -274,12 +273,12 @@ static int osc_cur_lost_grant_bytes_seq_show(struct seq_file *m, void *v)
 {
 	struct obd_device *dev = m->private;
 	struct client_obd *cli = &dev->u.cli;
-	int rc;
 
 	client_obd_list_lock(&cli->cl_loi_list_lock);
-	rc = seq_printf(m, "%lu\n", cli->cl_lost_grant);
+	seq_printf(m, "%lu\n", cli->cl_lost_grant);
 	client_obd_list_unlock(&cli->cl_loi_list_lock);
-	return rc;
+
+	return 0;
 }
 LPROC_SEQ_FOPS_RO(osc_cur_lost_grant_bytes);
 
@@ -289,8 +288,8 @@ static int osc_grant_shrink_interval_seq_show(struct seq_file *m, void *v)
 
 	if (obd == NULL)
 		return 0;
-	return seq_printf(m, "%d\n",
-			obd->u.cli.cl_grant_shrink_interval);
+	seq_printf(m, "%d\n", obd->u.cli.cl_grant_shrink_interval);
+	return 0;
 }
 
 static ssize_t osc_grant_shrink_interval_seq_write(struct file *file,
@@ -323,8 +322,8 @@ static int osc_checksum_seq_show(struct seq_file *m, void *v)
 	if (obd == NULL)
 		return 0;
 
-	return seq_printf(m, "%d\n",
-			obd->u.cli.cl_checksum ? 1 : 0);
+	seq_printf(m, "%d\n", obd->u.cli.cl_checksum ? 1 : 0);
+	return 0;
 }
 
 static ssize_t osc_checksum_seq_write(struct file *file,
@@ -405,7 +404,8 @@ static int osc_resend_count_seq_show(struct seq_file *m, void *v)
 {
 	struct obd_device *obd = m->private;
 
-	return seq_printf(m, "%u\n", atomic_read(&obd->u.cli.cl_resends));
+	seq_printf(m, "%u\n", atomic_read(&obd->u.cli.cl_resends));
+	return 0;
 }
 
 static ssize_t osc_resend_count_seq_write(struct file *file,
@@ -433,7 +433,8 @@ static int osc_contention_seconds_seq_show(struct seq_file *m, void *v)
 	struct obd_device *obd = m->private;
 	struct osc_device *od  = obd2osc_dev(obd);
 
-	return seq_printf(m, "%u\n", od->od_contention_time);
+	seq_printf(m, "%u\n", od->od_contention_time);
+	return 0;
 }
 
 static ssize_t osc_contention_seconds_seq_write(struct file *file,
@@ -453,7 +454,8 @@ static int osc_lockless_truncate_seq_show(struct seq_file *m, void *v)
 	struct obd_device *obd = m->private;
 	struct osc_device *od  = obd2osc_dev(obd);
 
-	return seq_printf(m, "%u\n", od->od_lockless_truncate);
+	seq_printf(m, "%u\n", od->od_lockless_truncate);
+	return 0;
 }
 
 static ssize_t osc_lockless_truncate_seq_write(struct file *file,
@@ -471,8 +473,9 @@ LPROC_SEQ_FOPS(osc_lockless_truncate);
 static int osc_destroys_in_flight_seq_show(struct seq_file *m, void *v)
 {
 	struct obd_device *obd = m->private;
-	return seq_printf(m, "%u\n",
-			atomic_read(&obd->u.cli.cl_destroy_in_flight));
+
+	seq_printf(m, "%u\n", atomic_read(&obd->u.cli.cl_destroy_in_flight));
+	return 0;
 }
 LPROC_SEQ_FOPS_RO(osc_destroys_in_flight);
 
diff --git a/drivers/staging/lustre/lustre/osc/osc_cache.c b/drivers/staging/lustre/lustre/osc/osc_cache.c
index 7022ed42d2d1..d44b3d4ffe4d 100644
--- a/drivers/staging/lustre/lustre/osc/osc_cache.c
+++ b/drivers/staging/lustre/lustre/osc/osc_cache.c
@@ -2613,7 +2613,7 @@ int osc_queue_sync_pages(const struct lu_env *env, struct osc_object *obj,
 	}
 	osc_object_unlock(obj);
 
-	osc_io_unplug(env, cli, obj, PDL_POLICY_ROUND);
+	osc_io_unplug_async(env, cli, obj);
 	return 0;
 }
 
diff --git a/drivers/staging/lustre/lustre/osc/osc_io.c b/drivers/staging/lustre/lustre/osc/osc_io.c
index fad4c135f928..3c7300b0651d 100644
--- a/drivers/staging/lustre/lustre/osc/osc_io.c
+++ b/drivers/staging/lustre/lustre/osc/osc_io.c
@@ -417,7 +417,7 @@ static int osc_io_setattr_start(const struct lu_env *env,
 
 			if (ia_valid & ATTR_SIZE) {
 				attr->cat_size = attr->cat_kms = size;
-				cl_valid = (CAT_SIZE | CAT_KMS);
+				cl_valid = CAT_SIZE | CAT_KMS;
 			}
 			if (ia_valid & ATTR_MTIME_SET) {
 				attr->cat_mtime = lvb->lvb_mtime;
diff --git a/drivers/staging/lustre/lustre/osc/osc_lock.c b/drivers/staging/lustre/lustre/osc/osc_lock.c
index 445655724904..350ad49550ab 100644
--- a/drivers/staging/lustre/lustre/osc/osc_lock.c
+++ b/drivers/staging/lustre/lustre/osc/osc_lock.c
@@ -1010,7 +1010,7 @@ static int osc_lock_enqueue_wait(const struct lu_env *env,
 	struct cl_lock_descr    *descr   = &lock->cll_descr;
 	struct cl_object_header *hdr     = cl_object_header(descr->cld_obj);
 	struct cl_lock	  *scan;
-	struct cl_lock	  *conflict= NULL;
+	struct cl_lock	  *conflict = NULL;
 	int lockless		     = osc_lock_is_lockless(olck);
 	int rc			   = 0;
 
diff --git a/drivers/staging/lustre/lustre/osc/osc_request.c b/drivers/staging/lustre/lustre/osc/osc_request.c
index 0adfa707a763..d7a9b650df09 100644
--- a/drivers/staging/lustre/lustre/osc/osc_request.c
+++ b/drivers/staging/lustre/lustre/osc/osc_request.c
@@ -1165,8 +1165,8 @@ static int check_write_rcs(struct ptlrpc_request *req,
 static inline int can_merge_pages(struct brw_page *p1, struct brw_page *p2)
 {
 	if (p1->flag != p2->flag) {
-		unsigned mask = ~(OBD_BRW_FROM_GRANT| OBD_BRW_NOCACHE|
-				  OBD_BRW_SYNC|OBD_BRW_ASYNC|OBD_BRW_NOQUOTA);
+		unsigned mask = ~(OBD_BRW_FROM_GRANT | OBD_BRW_NOCACHE |
+				  OBD_BRW_SYNC | OBD_BRW_ASYNC|OBD_BRW_NOQUOTA);
 
 		/* warn if we try to combine flags that we don't know to be
 		 * safe to combine */
@@ -1880,6 +1880,7 @@ int osc_build_rpc(const struct lu_env *env, struct client_obd *cli,
 	int				page_count = 0;
 	int				i;
 	int				rc;
+	struct ost_body			*body;
 	LIST_HEAD(rpc_list);
 
 	LASSERT(!list_empty(ext_list));
@@ -1981,6 +1982,8 @@ int osc_build_rpc(const struct lu_env *env, struct client_obd *cli,
 	 * later setattr before earlier BRW (as determined by the request xid),
 	 * the OST will not use BRW timestamps.  Sadly, there is no obvious
 	 * way to do this in a single call.  bug 10150 */
+	body = req_capsule_client_get(&req->rq_pill, &RMF_OST_BODY);
+	crattr->cra_oa = &body->oa;
 	cl_req_attr_set(env, clerq, crattr,
 			OBD_MD_FLMTIME|OBD_MD_FLCTIME|OBD_MD_FLATIME);
 
diff --git a/drivers/staging/lustre/lustre/ptlrpc/client.c b/drivers/staging/lustre/lustre/ptlrpc/client.c
index 4882dd0a4483..0357f1d4532f 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/client.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/client.c
@@ -285,14 +285,27 @@ static void ptlrpc_at_adj_net_latency(struct ptlrpc_request *req,
 	time_t now = get_seconds();
 
 	LASSERT(req->rq_import);
-	at = &req->rq_import->imp_at;
+
+	if (service_time > now - req->rq_sent + 3) {
+		/* bz16408, however, this can also happen if early reply
+		 * is lost and client RPC is expired and resent, early reply
+		 * or reply of original RPC can still be fit in reply buffer
+		 * of resent RPC, now client is measuring time from the
+		 * resent time, but server sent back service time of original
+		 * RPC.
+		 */
+		CDEBUG((lustre_msg_get_flags(req->rq_reqmsg) & MSG_RESENT) ?
+		       D_ADAPTTO : D_WARNING,
+		       "Reported service time %u > total measured time "
+		       CFS_DURATION_T"\n", service_time,
+		       cfs_time_sub(now, req->rq_sent));
+		return;
+	}
 
 	/* Network latency is total time less server processing time */
-	nl = max_t(int, now - req->rq_sent - service_time, 0) + 1/*st rounding*/;
-	if (service_time > now - req->rq_sent + 3 /* bz16408 */)
-		CWARN("Reported service time %u > total measured time "
-		      CFS_DURATION_T"\n", service_time,
-		      cfs_time_sub(now, req->rq_sent));
+	nl = max_t(int, now - req->rq_sent -
+			service_time, 0) + 1; /* st rounding */
+	at = &req->rq_import->imp_at;
 
 	oldnl = at_measured(&at->iat_net_latency, nl);
 	if (oldnl != 0)
@@ -468,7 +481,6 @@ void ptlrpc_add_rqs_to_pool(struct ptlrpc_request_pool *pool, int num_rq)
 		list_add_tail(&req->rq_list, &pool->prp_req_list);
 	}
 	spin_unlock(&pool->prp_lock);
-	return;
 }
 EXPORT_SYMBOL(ptlrpc_add_rqs_to_pool);
 
@@ -1439,12 +1451,11 @@ static int ptlrpc_send_new_req(struct ptlrpc_request *req)
 		if (req->rq_err) {
 			req->rq_status = rc;
 			return 1;
-		} else {
-			spin_lock(&req->rq_lock);
-			req->rq_wait_ctx = 1;
-			spin_unlock(&req->rq_lock);
-			return 0;
 		}
+		spin_lock(&req->rq_lock);
+		req->rq_wait_ctx = 1;
+		spin_unlock(&req->rq_lock);
+		return 0;
 	}
 
 	CDEBUG(D_RPCTRACE, "Sending RPC pname:cluuid:pid:xid:nid:opc %s:%s:%d:%llu:%s:%d\n",
@@ -2192,7 +2203,7 @@ int ptlrpc_set_wait(struct ptlrpc_request_set *set)
 	if (set->set_interpret != NULL) {
 		int (*interpreter)(struct ptlrpc_request_set *set, void *, int) =
 			set->set_interpret;
-		rc = interpreter (set, set->set_arg, rc);
+		rc = interpreter(set, set->set_arg, rc);
 	} else {
 		struct ptlrpc_set_cbdata *cbdata, *n;
 		int err;
diff --git a/drivers/staging/lustre/lustre/ptlrpc/connection.c b/drivers/staging/lustre/lustre/ptlrpc/connection.c
index 2a875ab57911..7e27397ce384 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/connection.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/connection.c
@@ -184,6 +184,7 @@ static void *
 conn_key(struct hlist_node *hnode)
 {
 	struct ptlrpc_connection *conn;
+
 	conn = hlist_entry(hnode, struct ptlrpc_connection, c_hash);
 	return &conn->c_peer;
 }
diff --git a/drivers/staging/lustre/lustre/ptlrpc/import.c b/drivers/staging/lustre/lustre/ptlrpc/import.c
index 4ceb90db02a3..d5fc689c008b 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/import.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/import.c
@@ -63,6 +63,19 @@ struct ptlrpc_connect_async_args {
 static void __import_set_state(struct obd_import *imp,
 			       enum lustre_imp_state state)
 {
+	switch (state) {
+	case LUSTRE_IMP_CLOSED:
+	case LUSTRE_IMP_NEW:
+	case LUSTRE_IMP_DISCON:
+	case LUSTRE_IMP_CONNECTING:
+		break;
+	case LUSTRE_IMP_REPLAY_WAIT:
+		imp->imp_replay_state = LUSTRE_IMP_REPLAY_LOCKS;
+		break;
+	default:
+		imp->imp_replay_state = LUSTRE_IMP_REPLAY;
+	}
+
 	imp->imp_state = state;
 	imp->imp_state_hist[imp->imp_state_hist_idx].ish_state = state;
 	imp->imp_state_hist[imp->imp_state_hist_idx].ish_time =
@@ -966,7 +979,7 @@ static int ptlrpc_connect_interpret(const struct lu_env *env,
 			imp->imp_resend_replay = 1;
 			spin_unlock(&imp->imp_lock);
 
-			IMPORT_SET_STATE(imp, LUSTRE_IMP_REPLAY);
+			IMPORT_SET_STATE(imp, imp->imp_replay_state);
 		} else {
 			IMPORT_SET_STATE(imp, LUSTRE_IMP_RECOVER);
 		}
diff --git a/drivers/staging/lustre/lustre/ptlrpc/layout.c b/drivers/staging/lustre/lustre/ptlrpc/layout.c
index bbef666b1d16..a42335e26de9 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/layout.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/layout.c
@@ -811,8 +811,8 @@ struct req_capsule;
 	.rmf_name    = (name),				  \
 	.rmf_flags   = (flags),				 \
 	.rmf_size    = (size),				  \
-	.rmf_swabber = (void (*)(void*))(swabber),	      \
-	.rmf_dumper  = (void (*)(void*))(dumper)		\
+	.rmf_swabber = (void (*)(void *))(swabber),	      \
+	.rmf_dumper  = (void (*)(void *))(dumper)		\
 }
 
 struct req_msg_field RMF_GENERIC_DATA =
@@ -1839,7 +1839,7 @@ static int __req_capsule_offset(const struct req_capsule *pill,
 	LASSERTF(offset > 0, "%s:%s, off=%d, loc=%d\n",
 			    pill->rc_fmt->rf_name,
 			    field->rmf_name, offset, loc);
-	offset --;
+	offset--;
 
 	LASSERT(0 <= offset && offset < REQ_MAX_FIELD_NR);
 	return offset;
diff --git a/drivers/staging/lustre/lustre/ptlrpc/lproc_ptlrpc.c b/drivers/staging/lustre/lustre/ptlrpc/lproc_ptlrpc.c
index 0e2071b8a36e..9533ab976a33 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/lproc_ptlrpc.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/lproc_ptlrpc.c
@@ -181,7 +181,7 @@ static const char *ll_eopcode2str(__u32 opcode)
 	return ll_eopcode_table[opcode].opname;
 }
 
-#if defined (CONFIG_PROC_FS)
+#if defined(CONFIG_PROC_FS)
 static void ptlrpc_lprocfs_register(struct proc_dir_entry *root, char *dir,
 				    char *name,
 				    struct proc_dir_entry **procroot_ret,
@@ -267,7 +267,8 @@ ptlrpc_lprocfs_req_history_len_seq_show(struct seq_file *m, void *v)
 	ptlrpc_service_for_each_part(svcpt, i, svc)
 		total += svcpt->scp_hist_nrqbds;
 
-	return seq_printf(m, "%d\n", total);
+	seq_printf(m, "%d\n", total);
+	return 0;
 }
 LPROC_SEQ_FOPS_RO(ptlrpc_lprocfs_req_history_len);
 
@@ -282,7 +283,8 @@ ptlrpc_lprocfs_req_history_max_seq_show(struct seq_file *m, void *n)
 	ptlrpc_service_for_each_part(svcpt, i, svc)
 		total += svc->srv_hist_nrqbds_cpt_max;
 
-	return seq_printf(m, "%d\n", total);
+	seq_printf(m, "%d\n", total);
+	return 0;
 }
 
 static ssize_t
@@ -327,8 +329,8 @@ ptlrpc_lprocfs_threads_min_seq_show(struct seq_file *m, void *n)
 {
 	struct ptlrpc_service *svc = m->private;
 
-	return seq_printf(m, "%d\n",
-			svc->srv_nthrs_cpt_init * svc->srv_ncpts);
+	seq_printf(m, "%d\n", svc->srv_nthrs_cpt_init * svc->srv_ncpts);
+	return 0;
 }
 
 static ssize_t
@@ -371,7 +373,8 @@ ptlrpc_lprocfs_threads_started_seq_show(struct seq_file *m, void *n)
 	ptlrpc_service_for_each_part(svcpt, i, svc)
 		total += svcpt->scp_nthrs_running;
 
-	return seq_printf(m, "%d\n", total);
+	seq_printf(m, "%d\n", total);
+	return 0;
 }
 LPROC_SEQ_FOPS_RO(ptlrpc_lprocfs_threads_started);
 
@@ -380,8 +383,8 @@ ptlrpc_lprocfs_threads_max_seq_show(struct seq_file *m, void *n)
 {
 	struct ptlrpc_service *svc = m->private;
 
-	return seq_printf(m, "%d\n",
-			svc->srv_nthrs_cpt_limit * svc->srv_ncpts);
+	seq_printf(m, "%d\n", svc->srv_nthrs_cpt_limit * svc->srv_ncpts);
+	return 0;
 }
 
 static ssize_t
@@ -1026,7 +1029,8 @@ LPROC_SEQ_FOPS_RO(ptlrpc_lprocfs_timeouts);
 static int ptlrpc_lprocfs_hp_ratio_seq_show(struct seq_file *m, void *v)
 {
 	struct ptlrpc_service *svc = m->private;
-	return seq_printf(m, "%d", svc->srv_hpreq_ratio);
+	seq_printf(m, "%d", svc->srv_hpreq_ratio);
+	return 0;
 }
 
 static ssize_t ptlrpc_lprocfs_hp_ratio_seq_write(struct file *file,
@@ -1083,7 +1087,7 @@ void ptlrpc_lprocfs_register_service(struct proc_dir_entry *entry,
 		 .data	     = svc},
 		{NULL}
 	};
-	static struct file_operations req_history_fops = {
+	static const struct file_operations req_history_fops = {
 		.owner       = THIS_MODULE,
 		.open	= ptlrpc_lprocfs_svc_req_history_open,
 		.read	= seq_read,
@@ -1324,13 +1328,12 @@ int lprocfs_rd_pinger_recov(struct seq_file *m, void *n)
 {
 	struct obd_device *obd = m->private;
 	struct obd_import *imp = obd->u.cli.cl_import;
-	int rc;
 
 	LPROCFS_CLIMP_CHECK(obd);
-	rc = seq_printf(m, "%d\n", !imp->imp_no_pinger_recover);
+	seq_printf(m, "%d\n", !imp->imp_no_pinger_recover);
 	LPROCFS_CLIMP_EXIT(obd);
 
-	return rc;
+	return 0;
 }
 EXPORT_SYMBOL(lprocfs_rd_pinger_recov);
 
diff --git a/drivers/staging/lustre/lustre/ptlrpc/niobuf.c b/drivers/staging/lustre/lustre/ptlrpc/niobuf.c
index f715e9a8b996..2fa2585584a3 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/niobuf.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/niobuf.c
@@ -383,12 +383,13 @@ int ptlrpc_send_reply(struct ptlrpc_request *req, int flags)
 		       req->rq_export->exp_obd->obd_minor);
 	}
 
-	/* In order to keep interoprability with the client (< 2.3) which
+	/* In order to keep interoperability with the client (< 2.3) which
 	 * doesn't have pb_jobid in ptlrpc_body, We have to shrink the
 	 * ptlrpc_body in reply buffer to ptlrpc_body_v2, otherwise, the
 	 * reply buffer on client will be overflow.
 	 *
-	 * XXX Remove this whenever we drop the interoprability with such client.
+	 * XXX Remove this whenever we drop the interoperability with
+	 * such client.
 	 */
 	req->rq_replen = lustre_shrink_msg(req->rq_repmsg, 0,
 					   sizeof(struct ptlrpc_body_v2), 1);
diff --git a/drivers/staging/lustre/lustre/ptlrpc/nrs.c b/drivers/staging/lustre/lustre/ptlrpc/nrs.c
index d5fd7215c72f..81ad7473242e 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/nrs.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/nrs.c
@@ -155,9 +155,8 @@ static void nrs_policy_stop_primary(struct ptlrpc_nrs *nrs)
 {
 	struct ptlrpc_nrs_policy *tmp = nrs->nrs_policy_primary;
 
-	if (tmp == NULL) {
+	if (tmp == NULL)
 		return;
-	}
 
 	nrs->nrs_policy_primary = NULL;
 
@@ -912,7 +911,6 @@ static int nrs_register_policies_locked(struct ptlrpc_nrs *nrs)
 static int nrs_svcpt_setup_locked0(struct ptlrpc_nrs *nrs,
 				   struct ptlrpc_service_part *svcpt)
 {
-	int				rc;
 	enum ptlrpc_nrs_queue_type	queue;
 
 	LASSERT(mutex_is_locked(&nrs_core.nrs_mutex));
@@ -930,9 +928,7 @@ static int nrs_svcpt_setup_locked0(struct ptlrpc_nrs *nrs,
 	INIT_LIST_HEAD(&nrs->nrs_policy_list);
 	INIT_LIST_HEAD(&nrs->nrs_policy_queued);
 
-	rc = nrs_register_policies_locked(nrs);
-
-	return rc;
+	return nrs_register_policies_locked(nrs);
 }
 
 /**
diff --git a/drivers/staging/lustre/lustre/ptlrpc/pack_generic.c b/drivers/staging/lustre/lustre/ptlrpc/pack_generic.c
index 2f45f7657830..b51af9bf37b7 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/pack_generic.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/pack_generic.c
@@ -117,13 +117,13 @@ EXPORT_SYMBOL(lustre_msg_check_version);
 /* early reply size */
 int lustre_msg_early_size(void)
 {
-	static int size = 0;
+	static int size;
 	if (!size) {
-		/* Always reply old ptlrpc_body_v2 to keep interoprability
+		/* Always reply old ptlrpc_body_v2 to keep interoperability
 		 * with the old client (< 2.3) which doesn't have pb_jobid
 		 * in the ptlrpc_body.
 		 *
-		 * XXX Remove this whenever we drop interoprability with such
+		 * XXX Remove this whenever we drop interoperability with such
 		 *     client.
 		 */
 		__u32 pblen = sizeof(struct ptlrpc_body_v2);
diff --git a/drivers/staging/lustre/lustre/ptlrpc/pinger.c b/drivers/staging/lustre/lustre/ptlrpc/pinger.c
index 340d98a64137..9dbda9332dd8 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/pinger.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/pinger.c
@@ -545,7 +545,7 @@ void ptlrpc_pinger_wake_up(void)
 #define PET_READY     1
 #define PET_TERMINATE 2
 
-static int	       pet_refcount = 0;
+static int pet_refcount;
 static int	       pet_state;
 static wait_queue_head_t       pet_waitq;
 LIST_HEAD(pet_list);
diff --git a/drivers/staging/lustre/lustre/ptlrpc/ptlrpc_internal.h b/drivers/staging/lustre/lustre/ptlrpc/ptlrpc_internal.h
index 7fe2e584ffd7..a66dc3c6da41 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/ptlrpc_internal.h
+++ b/drivers/staging/lustre/lustre/ptlrpc/ptlrpc_internal.h
@@ -76,7 +76,7 @@ void ptlrpc_initiate_recovery(struct obd_import *imp);
 int lustre_unpack_req_ptlrpc_body(struct ptlrpc_request *req, int offset);
 int lustre_unpack_rep_ptlrpc_body(struct ptlrpc_request *req, int offset);
 
-#if defined (CONFIG_PROC_FS)
+#if defined(CONFIG_PROC_FS)
 void ptlrpc_lprocfs_register_service(struct proc_dir_entry *proc_entry,
 				     struct ptlrpc_service *svc);
 void ptlrpc_lprocfs_unregister_service(struct ptlrpc_service *svc);
@@ -263,7 +263,7 @@ void sptlrpc_enc_pool_fini(void);
 int sptlrpc_proc_enc_pool_seq_show(struct seq_file *m, void *v);
 
 /* sec_lproc.c */
-#if defined (CONFIG_PROC_FS)
+#if defined(CONFIG_PROC_FS)
 int  sptlrpc_lproc_init(void);
 void sptlrpc_lproc_fini(void);
 #else
diff --git a/drivers/staging/lustre/lustre/ptlrpc/ptlrpcd.c b/drivers/staging/lustre/lustre/ptlrpc/ptlrpcd.c
index 4621b71fe0b6..0c178ec0e487 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/ptlrpcd.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/ptlrpcd.c
@@ -85,7 +85,7 @@ MODULE_PARM_DESC(ptlrpcd_bind_policy, "Ptlrpcd threads binding mode.");
 static struct ptlrpcd *ptlrpcds;
 
 struct mutex ptlrpcd_mutex;
-static int ptlrpcd_users = 0;
+static int ptlrpcd_users;
 
 void ptlrpcd_wake(struct ptlrpc_request *req)
 {
@@ -511,10 +511,10 @@ static int ptlrpcd_bind(int index, int max)
 #if defined(CONFIG_NUMA)
 	{
 		int i;
-		mask = *cpumask_of_node(cpu_to_node(index));
+		cpumask_copy(&mask, cpumask_of_node(cpu_to_node(index)));
 		for (i = max; i < num_online_cpus(); i++)
-			cpu_clear(i, mask);
-		pc->pc_npartners = cpus_weight(mask) - 1;
+			cpumask_clear_cpu(i, &mask);
+		pc->pc_npartners = cpumask_weight(&mask) - 1;
 		set_bit(LIOD_BIND, &pc->pc_flags);
 	}
 #else
@@ -554,7 +554,7 @@ static int ptlrpcd_bind(int index, int max)
 				 * that are already initialized
 				 */
 				for (pidx = 0, i = 0; i < index; i++) {
-					if (cpu_isset(i, mask)) {
+					if (cpumask_test_cpu(i, &mask)) {
 						ppc = &ptlrpcds->pd_threads[i];
 						pc->pc_partners[pidx++] = ppc;
 						ppc->pc_partners[ppc->
diff --git a/drivers/staging/lustre/lustre/ptlrpc/sec_bulk.c b/drivers/staging/lustre/lustre/ptlrpc/sec_bulk.c
index 0dabd83fd46f..c05a8554d737 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/sec_bulk.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/sec_bulk.c
@@ -125,52 +125,50 @@ static struct ptlrpc_enc_page_pool {
  */
 int sptlrpc_proc_enc_pool_seq_show(struct seq_file *m, void *v)
 {
-	int     rc;
-
 	spin_lock(&page_pools.epp_lock);
 
-	rc = seq_printf(m,
-		      "physical pages:	  %lu\n"
-		      "pages per pool:	  %lu\n"
-		      "max pages:	       %lu\n"
-		      "max pools:	       %u\n"
-		      "total pages:	     %lu\n"
-		      "total free:	      %lu\n"
-		      "idle index:	      %lu/100\n"
-		      "last shrink:	     %lds\n"
-		      "last access:	     %lds\n"
-		      "max pages reached:       %lu\n"
-		      "grows:		   %u\n"
-		      "grows failure:	   %u\n"
-		      "shrinks:		 %u\n"
-		      "cache access:	    %lu\n"
-		      "cache missing:	   %lu\n"
-		      "low free mark:	   %lu\n"
-		      "max waitqueue depth:     %u\n"
-		      "max wait time:	   "CFS_TIME_T"/%u\n"
-		      ,
-		      totalram_pages,
-		      PAGES_PER_POOL,
-		      page_pools.epp_max_pages,
-		      page_pools.epp_max_pools,
-		      page_pools.epp_total_pages,
-		      page_pools.epp_free_pages,
-		      page_pools.epp_idle_idx,
-		      get_seconds() - page_pools.epp_last_shrink,
-		      get_seconds() - page_pools.epp_last_access,
-		      page_pools.epp_st_max_pages,
-		      page_pools.epp_st_grows,
-		      page_pools.epp_st_grow_fails,
-		      page_pools.epp_st_shrinks,
-		      page_pools.epp_st_access,
-		      page_pools.epp_st_missings,
-		      page_pools.epp_st_lowfree,
-		      page_pools.epp_st_max_wqlen,
-		      page_pools.epp_st_max_wait, HZ
-		     );
+	seq_printf(m,
+		   "physical pages:	  %lu\n"
+		   "pages per pool:	  %lu\n"
+		   "max pages:	       %lu\n"
+		   "max pools:	       %u\n"
+		   "total pages:	     %lu\n"
+		   "total free:	      %lu\n"
+		   "idle index:	      %lu/100\n"
+		   "last shrink:	     %lds\n"
+		   "last access:	     %lds\n"
+		   "max pages reached:       %lu\n"
+		   "grows:		   %u\n"
+		   "grows failure:	   %u\n"
+		   "shrinks:		 %u\n"
+		   "cache access:	    %lu\n"
+		   "cache missing:	   %lu\n"
+		   "low free mark:	   %lu\n"
+		   "max waitqueue depth:     %u\n"
+		   "max wait time:	   " CFS_TIME_T "/%u\n",
+		   totalram_pages,
+		   PAGES_PER_POOL,
+		   page_pools.epp_max_pages,
+		   page_pools.epp_max_pools,
+		   page_pools.epp_total_pages,
+		   page_pools.epp_free_pages,
+		   page_pools.epp_idle_idx,
+		   get_seconds() - page_pools.epp_last_shrink,
+		   get_seconds() - page_pools.epp_last_access,
+		   page_pools.epp_st_max_pages,
+		   page_pools.epp_st_grows,
+		   page_pools.epp_st_grow_fails,
+		   page_pools.epp_st_shrinks,
+		   page_pools.epp_st_access,
+		   page_pools.epp_st_missings,
+		   page_pools.epp_st_lowfree,
+		   page_pools.epp_st_max_wqlen,
+		   page_pools.epp_st_max_wait,
+		   HZ);
 
 	spin_unlock(&page_pools.epp_lock);
-	return rc;
+
+	return 0;
 }
 
 static void enc_pools_release_free_pages(long npages)
@@ -365,8 +363,8 @@ static void enc_pools_insert(struct page ***pools, int npools, int npages)
 	 */
 	cur_npools = (page_pools.epp_total_pages + PAGES_PER_POOL - 1) /
 		     PAGES_PER_POOL;
-	end_npools = (page_pools.epp_total_pages + npages + PAGES_PER_POOL - 1) /
-		     PAGES_PER_POOL;
+	end_npools = (page_pools.epp_total_pages + npages + PAGES_PER_POOL - 1)
+		     / PAGES_PER_POOL;
 	LASSERT(end_npools <= page_pools.epp_max_pools);
 
 	np_idx = 0;
@@ -816,9 +814,8 @@ int bulk_sec_desc_unpack(struct lustre_msg *msg, int offset, int swabbed)
 		return -EINVAL;
 	}
 
-	if (swabbed) {
+	if (swabbed)
 		__swab32s(&bsd->bsd_nob);
-	}
 
 	if (unlikely(bsd->bsd_version != 0)) {
 		CERROR("Unexpected version %u\n", bsd->bsd_version);
diff --git a/drivers/staging/lustre/lustre/ptlrpc/service.c b/drivers/staging/lustre/lustre/ptlrpc/service.c
index 635b12b22cef..8e61421515cb 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/service.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/service.c
@@ -543,7 +543,6 @@ ptlrpc_server_nthreads_check(struct ptlrpc_service *svc,
 	if (tc->tc_thr_factor != 0) {
 		int	  factor = tc->tc_thr_factor;
 		const int fade = 4;
-		cpumask_t mask;
 
 		/*
 		 * User wants to increase number of threads with for
@@ -557,8 +556,8 @@ ptlrpc_server_nthreads_check(struct ptlrpc_service *svc,
 		 * have too many threads no matter how many cores/HTs
 		 * there are.
 		 */
-		cpumask_copy(&mask, topology_thread_cpumask(0));
-		if (cpus_weight(mask) > 1) { /* weight is # of HTs */
+		/* weight is # of HTs */
+		if (cpumask_weight(topology_thread_cpumask(0)) > 1) {
 			/* depress thread factor for hyper-thread */
 			factor = factor - (factor >> 1) + (factor >> 3);
 		}
@@ -2752,7 +2751,6 @@ int ptlrpc_start_thread(struct ptlrpc_service_part *svcpt, int wait)
 
 int ptlrpc_hr_init(void)
 {
-	cpumask_t			mask;
 	struct ptlrpc_hr_partition	*hrp;
 	struct ptlrpc_hr_thread		*hrt;
 	int				rc;
@@ -2770,8 +2768,7 @@ int ptlrpc_hr_init(void)
 
 	init_waitqueue_head(&ptlrpc_hr.hr_waitq);
 
-	cpumask_copy(&mask, topology_thread_cpumask(0));
-	weight = cpus_weight(mask);
+	weight = cpumask_weight(topology_thread_cpumask(0));
 
 	cfs_percpt_for_each(hrp, i, ptlrpc_hr.hr_partitions) {
 		hrp->hrp_cpt = i;
diff --git a/drivers/staging/media/bcm2048/radio-bcm2048.c b/drivers/staging/media/bcm2048/radio-bcm2048.c
index 538250667918..f28ffef0d1f0 100644
--- a/drivers/staging/media/bcm2048/radio-bcm2048.c
+++ b/drivers/staging/media/bcm2048/radio-bcm2048.c
@@ -1448,8 +1448,8 @@ static void bcm2048_parse_rds_pi(struct bcm2048_device *bdev)
 		/* 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]);
+			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;
@@ -1503,8 +1503,8 @@ static int bcm2048_parse_rt_match_b(struct bcm2048_device *bdev, int i)
 	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_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] &
@@ -1792,7 +1792,7 @@ static int bcm2048_get_rds_data(struct bcm2048_device *bdev, char *data)
 		goto unlock;
 	}
 
-	data_buffer = kzalloc(BCM2048_MAX_RDS_RADIO_TEXT*5, GFP_KERNEL);
+	data_buffer = kcalloc(BCM2048_MAX_RDS_RADIO_TEXT, 5, GFP_KERNEL);
 	if (!data_buffer) {
 		err = -ENOMEM;
 		goto unlock;
@@ -2245,8 +2245,7 @@ static ssize_t bcm2048_fops_read(struct file *file, char __user *buf,
 
 		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);
+		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;
@@ -2718,22 +2717,7 @@ static struct i2c_driver bcm2048_i2c_driver = {
 	.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_i2c_driver(bcm2048_i2c_driver);
 
 MODULE_LICENSE("GPL");
 MODULE_AUTHOR(BCM2048_DRIVER_AUTHOR);
diff --git a/drivers/staging/media/cxd2099/cxd2099.c b/drivers/staging/media/cxd2099/cxd2099.c
index 657ea480c6e7..692ba3e63e14 100644
--- a/drivers/staging/media/cxd2099/cxd2099.c
+++ b/drivers/staging/media/cxd2099/cxd2099.c
@@ -25,7 +25,6 @@
 #include <linux/slab.h>
 #include <linux/kernel.h>
 #include <linux/module.h>
-#include <linux/moduleparam.h>
 #include <linux/i2c.h>
 #include <linux/wait.h>
 #include <linux/delay.h>
diff --git a/drivers/staging/media/davinci_vpfe/dm365_ipipeif.c b/drivers/staging/media/davinci_vpfe/dm365_ipipeif.c
index 87d42e18377d..17e105e7d892 100644
--- a/drivers/staging/media/davinci_vpfe/dm365_ipipeif.c
+++ b/drivers/staging/media/davinci_vpfe/dm365_ipipeif.c
@@ -209,10 +209,10 @@ static int ipipeif_hw_setup(struct v4l2_subdev *sd)
 	/* Combine all the fields to make CFG1 register of IPIPEIF */
 	tmp = val = get_oneshot_mode(ipipeif->input);
 	if (tmp < 0) {
-		pr_err("ipipeif: links setup required");
+		dev_err(&sd->devnode->dev, "ipipeif: links setup required");
 		return -EINVAL;
 	}
-	val = val << ONESHOT_SHIFT;
+	val <<= ONESHOT_SHIFT;
 
 	ipipeif_source = ipipeif_get_source(ipipeif);
 	val |= ipipeif_source << INPSRC_SHIFT;
@@ -747,7 +747,6 @@ static void ipipeif_set_default_config(struct vpfe_ipipeif_device *ipipeif)
 			.clip = 4095,
 		},
 	};
-	memset(&ipipeif->config, 0, sizeof(struct ipipeif_params));
 	memcpy(&ipipeif->config, &ipipeif_defaults,
 	       sizeof(struct ipipeif_params));
 }
diff --git a/drivers/staging/media/davinci_vpfe/dm365_resizer.c b/drivers/staging/media/davinci_vpfe/dm365_resizer.c
index 75e70e14b724..7cc8d1b4d737 100644
--- a/drivers/staging/media/davinci_vpfe/dm365_resizer.c
+++ b/drivers/staging/media/davinci_vpfe/dm365_resizer.c
@@ -907,7 +907,6 @@ resizer_set_defualt_configuration(struct vpfe_resizer_device *resizer)
 			.out_chr_pos = VPFE_IPIPE_YUV422_CHR_POS_COSITE,
 		},
 	};
-	memset(&resizer->config, 0, sizeof(struct resizer_params));
 	memcpy(&resizer->config, &rsz_default_config,
 	       sizeof(struct resizer_params));
 }
diff --git a/drivers/staging/media/davinci_vpfe/vpfe_mc_capture.c b/drivers/staging/media/davinci_vpfe/vpfe_mc_capture.c
index a350a20955f1..57426199ad7a 100644
--- a/drivers/staging/media/davinci_vpfe/vpfe_mc_capture.c
+++ b/drivers/staging/media/davinci_vpfe/vpfe_mc_capture.c
@@ -226,8 +226,8 @@ static int vpfe_enable_clock(struct vpfe_device *vpfe_dev)
 	if (!vpfe_cfg->num_clocks)
 		return 0;
 
-	vpfe_dev->clks = kzalloc(vpfe_cfg->num_clocks *
-				   sizeof(struct clock *), GFP_KERNEL);
+	vpfe_dev->clks = kcalloc(vpfe_cfg->num_clocks,
+				 sizeof(struct clock *), GFP_KERNEL);
 	if (vpfe_dev->clks == NULL)
 		return -ENOMEM;
 
@@ -346,7 +346,8 @@ static int register_i2c_devices(struct vpfe_device *vpfe_dev)
 	i2c_adap = i2c_get_adapter(1);
 	num_subdevs = vpfe_cfg->num_subdevs;
 	vpfe_dev->sd =
-		  kzalloc(sizeof(struct v4l2_subdev *)*num_subdevs, GFP_KERNEL);
+		  kcalloc(num_subdevs, sizeof(struct v4l2_subdev *),
+			  GFP_KERNEL);
 	if (vpfe_dev->sd == NULL)
 		return -ENOMEM;
 
diff --git a/drivers/staging/media/lirc/lirc_imon.c b/drivers/staging/media/lirc/lirc_imon.c
index 9ce7d9990e3e..335b98a54237 100644
--- a/drivers/staging/media/lirc/lirc_imon.c
+++ b/drivers/staging/media/lirc/lirc_imon.c
@@ -208,8 +208,7 @@ static void deregister_from_lirc(struct imon_context *context)
 	retval = lirc_unregister_driver(minor);
 	if (retval)
 		dev_err(&context->usbdev->dev,
-			": %s: unable to deregister from lirc(%d)",
-			__func__, retval);
+			"unable to deregister from lirc(%d)", retval);
 	else
 		dev_info(&context->usbdev->dev,
 			 "Deregistered iMON driver (minor:%d)\n", minor);
@@ -241,9 +240,8 @@ static int display_open(struct inode *inode, struct file *file)
 	context = usb_get_intfdata(interface);
 
 	if (!context) {
-		dev_err(&interface->dev,
-			"%s: no context found for minor %d\n",
-			__func__, subminor);
+		dev_err(&interface->dev, "no context found for minor %d\n",
+			subminor);
 		retval = -ENODEV;
 		goto exit;
 	}
@@ -339,13 +337,13 @@ static int send_packet(struct imon_context *context)
 	context->tx_urb->actual_length = 0;
 
 	init_completion(&context->tx.finished);
-	atomic_set(&(context->tx.busy), 1);
+	atomic_set(&context->tx.busy, 1);
 
 	retval = usb_submit_urb(context->tx_urb, GFP_KERNEL);
 	if (retval) {
-		atomic_set(&(context->tx.busy), 0);
-		dev_err(&context->usbdev->dev,
-			"%s: error submitting urb(%d)\n", __func__, retval);
+		atomic_set(&context->tx.busy, 0);
+		dev_err(&context->usbdev->dev, "error submitting urb(%d)\n",
+			retval);
 	} else {
 		/* Wait for transmission to complete (or abort) */
 		mutex_unlock(&context->ctx_lock);
@@ -359,8 +357,7 @@ static int send_packet(struct imon_context *context)
 		retval = context->tx.status;
 		if (retval)
 			dev_err(&context->usbdev->dev,
-				"%s: packet tx failed (%d)\n",
-				__func__, retval);
+				"packet tx failed (%d)\n", retval);
 	}
 
 	return retval;
@@ -437,8 +434,8 @@ static ssize_t vfd_write(struct file *file, const char __user *buf,
 		retval = send_packet(context);
 		if (retval) {
 			dev_err(&context->usbdev->dev,
-				"%s: send packet failed for packet #%d\n",
-				__func__, seq/2);
+				"send packet failed for packet #%d\n",
+				seq / 2);
 			goto exit;
 		} else {
 			seq += 2;
@@ -454,8 +451,8 @@ static ssize_t vfd_write(struct file *file, const char __user *buf,
 		retval = send_packet(context);
 		if (retval)
 			dev_err(&context->usbdev->dev,
-				"%s: send packet failed for packet #%d\n",
-					__func__, seq/2);
+				"send packet failed for packet #%d\n",
+				seq / 2);
 	}
 
 exit:
@@ -877,8 +874,7 @@ static int imon_probe(struct usb_interface *interface,
 	retval = usb_submit_urb(context->rx_urb, GFP_KERNEL);
 
 	if (retval) {
-		dev_err(dev, "%s: usb_submit_urb failed for intf0 (%d)\n",
-			__func__, retval);
+		dev_err(dev, "usb_submit_urb failed for intf0 (%d)\n", retval);
 		alloc_status = 8;
 		goto unlock;
 	}
diff --git a/drivers/staging/media/lirc/lirc_parallel.c b/drivers/staging/media/lirc/lirc_parallel.c
index 19c5c21babf5..c1408342b1d0 100644
--- a/drivers/staging/media/lirc/lirc_parallel.c
+++ b/drivers/staging/media/lirc/lirc_parallel.c
@@ -161,8 +161,8 @@ static unsigned int init_lirc_timer(void)
 			     || (now.tv_sec == tv.tv_sec
 				 && now.tv_usec < tv.tv_usec)));
 
-	timeelapsed = ((now.tv_sec + 1 - tv.tv_sec)*1000000
-		     + (now.tv_usec - tv.tv_usec));
+	timeelapsed = (now.tv_sec + 1 - tv.tv_sec)*1000000
+		     + (now.tv_usec - tv.tv_usec);
 	if (count >= 1000 && timeelapsed > 0) {
 		if (default_timer == 0) {
 			/* autodetect timer */
@@ -336,7 +336,7 @@ static ssize_t lirc_read(struct file *filep, char __user *buf, size_t n,
 	set_current_state(TASK_INTERRUPTIBLE);
 	while (count < n) {
 		if (rptr != wptr) {
-			if (copy_to_user(buf+count, (char *) &rbuf[rptr],
+			if (copy_to_user(buf+count, &rbuf[rptr],
 					 sizeof(int))) {
 				result = -EFAULT;
 				break;
diff --git a/drivers/staging/media/lirc/lirc_sasem.c b/drivers/staging/media/lirc/lirc_sasem.c
index 4a268200cbf5..9e5674341abe 100644
--- a/drivers/staging/media/lirc/lirc_sasem.c
+++ b/drivers/staging/media/lirc/lirc_sasem.c
@@ -170,9 +170,6 @@ static void delete_context(struct sasem_context *context)
 	kfree(context->driver->rbuf);
 	kfree(context->driver);
 	kfree(context);
-
-	if (debug)
-		pr_info("%s: context deleted\n", __func__);
 }
 
 static void deregister_from_lirc(struct sasem_context *context)
@@ -182,10 +179,12 @@ static void deregister_from_lirc(struct sasem_context *context)
 
 	retval = lirc_unregister_driver(minor);
 	if (retval)
-		pr_err("%s: unable to deregister from lirc (%d)\n",
+		dev_err(&context->dev->dev,
+			"%s: unable to deregister from lirc (%d)\n",
 		       __func__, retval);
 	else
-		pr_info("Deregistered Sasem driver (minor:%d)\n", minor);
+		dev_info(&context->dev->dev,
+		         "Deregistered Sasem driver (minor:%d)\n", minor);
 
 }
 
@@ -214,9 +213,8 @@ static int vfd_open(struct inode *inode, struct file *file)
 	context = usb_get_intfdata(interface);
 
 	if (!context) {
-		dev_err(&interface->dev,
-			"%s: no context found for minor %d\n",
-			__func__, subminor);
+		dev_err(&interface->dev, "no context found for minor %d\n",
+			subminor);
 		retval = -ENODEV;
 		goto exit;
 	}
@@ -332,13 +330,13 @@ static int send_packet(struct sasem_context *context)
 	context->tx_urb->actual_length = 0;
 
 	init_completion(&context->tx.finished);
-	atomic_set(&(context->tx.busy), 1);
+	atomic_set(&context->tx.busy, 1);
 
 	retval =  usb_submit_urb(context->tx_urb, GFP_KERNEL);
 	if (retval) {
-		atomic_set(&(context->tx.busy), 0);
-		dev_err(&context->dev->dev, "%s: error submitting urb (%d)\n",
-			__func__, retval);
+		atomic_set(&context->tx.busy, 0);
+		dev_err(&context->dev->dev, "error submitting urb (%d)\n",
+			retval);
 	} else {
 		/* Wait for transmission to complete (or abort) */
 		mutex_unlock(&context->ctx_lock);
@@ -348,8 +346,7 @@ static int send_packet(struct sasem_context *context)
 		retval = context->tx.status;
 		if (retval)
 			dev_err(&context->dev->dev,
-				"%s: packet tx failed (%d)\n",
-				__func__, retval);
+				"packet tx failed (%d)\n", retval);
 	}
 
 	return retval;
@@ -389,7 +386,7 @@ static ssize_t vfd_write(struct file *file, const char __user *buf,
 		goto exit;
 	}
 
-	data_buf = memdup_user((void const __user *)buf, n_bytes);
+	data_buf = memdup_user(buf, n_bytes);
 	if (IS_ERR(data_buf)) {
 		retval = PTR_ERR(data_buf);
 		data_buf = NULL;
@@ -444,8 +441,7 @@ static ssize_t vfd_write(struct file *file, const char __user *buf,
 		retval = send_packet(context);
 		if (retval) {
 			dev_err(&context->dev->dev,
-				"%s: send packet failed for packet #%d\n",
-				__func__, i);
+				"send packet failed for packet #%d\n", i);
 			goto exit;
 		}
 	}
@@ -509,8 +505,7 @@ static int ir_open(void *data)
 
 	if (retval)
 		dev_err(&context->dev->dev,
-			"%s: usb_submit_urb failed for ir_open (%d)\n",
-			__func__, retval);
+			"usb_submit_urb failed for ir_open (%d)\n", retval);
 	else {
 		context->ir_isopen = 1;
 		dev_info(&context->dev->dev, "IR port opened\n");
diff --git a/drivers/staging/media/lirc/lirc_serial.c b/drivers/staging/media/lirc/lirc_serial.c
index 19628d0104ab..dc7984455c3a 100644
--- a/drivers/staging/media/lirc/lirc_serial.c
+++ b/drivers/staging/media/lirc/lirc_serial.c
@@ -344,7 +344,7 @@ static int init_timing_params(unsigned int new_duty_cycle,
 	/* How many clocks in a microsecond?, avoiding long long divide */
 	work = loops_per_sec;
 	work *= 4295;  /* 4295 = 2^32 / 1e6 */
-	conv_us_to_clocks = (work >> 32);
+	conv_us_to_clocks = work >> 32;
 
 	/*
 	 * Carrier period in clocks, approach good up to 32GHz clock,
@@ -784,7 +784,7 @@ static int lirc_serial_probe(struct platform_device *dev)
 
 	result = devm_request_irq(&dev->dev, irq, lirc_irq_handler,
 			     (share_irq ? IRQF_SHARED : 0),
-			     LIRC_DRIVER_NAME, (void *)&hardware);
+			     LIRC_DRIVER_NAME, &hardware);
 	if (result < 0) {
 		if (result == -EBUSY)
 			dev_err(&dev->dev, "IRQ %d busy\n", irq);
@@ -838,7 +838,7 @@ static int lirc_serial_probe(struct platform_device *dev)
 				nhigh++;
 			msleep(40);
 		}
-		sense = (nlow >= nhigh ? 1 : 0);
+		sense = nlow >= nhigh ? 1 : 0;
 		dev_info(&dev->dev, "auto-detected active %s receiver\n",
 			 sense ? "low" : "high");
 	} else
diff --git a/drivers/staging/media/lirc/lirc_sir.c b/drivers/staging/media/lirc/lirc_sir.c
index 39f4733fb1ee..29087f66e2f4 100644
--- a/drivers/staging/media/lirc/lirc_sir.c
+++ b/drivers/staging/media/lirc/lirc_sir.c
@@ -683,9 +683,7 @@ static int init_port(void)
 	}
 	pr_info("I/O port 0x%.4x, IRQ %d.\n", io, irq);
 
-	init_timer(&timerlist);
-	timerlist.function = sir_timeout;
-	timerlist.data = 0xabadcafe;
+	setup_timer(&timerlist, sir_timeout, 0);
 
 	return 0;
 }
diff --git a/drivers/staging/media/lirc/lirc_zilog.c b/drivers/staging/media/lirc/lirc_zilog.c
index e16627ca488e..261e27d6b054 100644
--- a/drivers/staging/media/lirc/lirc_zilog.c
+++ b/drivers/staging/media/lirc/lirc_zilog.c
@@ -1341,8 +1341,7 @@ static int close(struct inode *node, struct file *filep)
 	struct IR *ir = filep->private_data;
 
 	if (ir == NULL) {
-		dev_err(ir->l.dev,
-			"close: no private_data attached to the file!\n");
+		pr_err("ir: close: no private_data attached to the file!\n");
 		return -ENODEV;
 	}
 
diff --git a/drivers/staging/media/mn88472/mn88472.c b/drivers/staging/media/mn88472/mn88472.c
index 6eebe564e557..2a68582e7f71 100644
--- a/drivers/staging/media/mn88472/mn88472.c
+++ b/drivers/staging/media/mn88472/mn88472.c
@@ -276,7 +276,7 @@ static int mn88472_init(struct dvb_frontend *fe)
 			remaining -= (dev->i2c_wr_max - 1)) {
 		len = remaining;
 		if (len > (dev->i2c_wr_max - 1))
-			len = (dev->i2c_wr_max - 1);
+			len = dev->i2c_wr_max - 1;
 
 		ret = regmap_bulk_write(dev->regmap[0], 0xf6,
 				&fw->data[fw->size - remaining], len);
diff --git a/drivers/staging/media/mn88473/mn88473.c b/drivers/staging/media/mn88473/mn88473.c
index a333744b76b9..5baeb03ab3d1 100644
--- a/drivers/staging/media/mn88473/mn88473.c
+++ b/drivers/staging/media/mn88473/mn88473.c
@@ -17,7 +17,7 @@
 #include "mn88473_priv.h"
 
 static int mn88473_get_tune_settings(struct dvb_frontend *fe,
-	struct dvb_frontend_tune_settings *s)
+				     struct dvb_frontend_tune_settings *s)
 {
 	s->min_delay_ms = 1000;
 	return 0;
@@ -33,10 +33,14 @@ static int mn88473_set_frontend(struct dvb_frontend *fe)
 	u8 delivery_system_val, if_val[3], bw_val[7];
 
 	dev_dbg(&client->dev,
-			"delivery_system=%u modulation=%u frequency=%u bandwidth_hz=%u symbol_rate=%u inversion=%d stream_id=%d\n",
-			c->delivery_system, c->modulation,
-			c->frequency, c->bandwidth_hz, c->symbol_rate,
-			c->inversion, c->stream_id);
+		"delivery_system=%u modulation=%u frequency=%u bandwidth_hz=%u symbol_rate=%u inversion=%d stream_id=%d\n",
+		c->delivery_system,
+		c->modulation,
+		c->frequency,
+		c->bandwidth_hz,
+		c->symbol_rate,
+		c->inversion,
+		c->stream_id);
 
 	if (!dev->warm) {
 		ret = -EAGAIN;
@@ -112,7 +116,7 @@ static int mn88473_set_frontend(struct dvb_frontend *fe)
 		break;
 	default:
 		dev_err(&client->dev, "IF frequency %d not supported\n",
-				if_frequency);
+			if_frequency);
 		ret = -EINVAL;
 		goto err;
 	}
@@ -229,7 +233,7 @@ static int mn88473_init(struct dvb_frontend *fe)
 	}
 
 	dev_info(&client->dev, "downloading firmware from file '%s'\n",
-			fw_file);
+		 fw_file);
 
 	ret = regmap_write(dev->regmap[0], 0xf5, 0x03);
 	if (ret)
@@ -239,13 +243,13 @@ static int mn88473_init(struct dvb_frontend *fe)
 			remaining -= (dev->i2c_wr_max - 1)) {
 		len = remaining;
 		if (len > (dev->i2c_wr_max - 1))
-			len = (dev->i2c_wr_max - 1);
+			len = dev->i2c_wr_max - 1;
 
 		ret = regmap_bulk_write(dev->regmap[0], 0xf6,
-				&fw->data[fw->size - remaining], len);
+					&fw->data[fw->size - remaining], len);
 		if (ret) {
 			dev_err(&client->dev, "firmware download failed=%d\n",
-					ret);
+				ret);
 			goto err;
 		}
 	}
@@ -325,7 +329,7 @@ static struct dvb_frontend_ops mn88473_ops = {
 };
 
 static int mn88473_probe(struct i2c_client *client,
-		const struct i2c_device_id *id)
+			 const struct i2c_device_id *id)
 {
 	struct mn88473_config *config = client->dev.platform_data;
 	struct mn88473_dev *dev;
diff --git a/drivers/staging/media/omap4iss/iss.c b/drivers/staging/media/omap4iss/iss.c
index 44b81a2c8b6f..e0ad5e520e2d 100644
--- a/drivers/staging/media/omap4iss/iss.c
+++ b/drivers/staging/media/omap4iss/iss.c
@@ -1160,8 +1160,8 @@ iss_register_subdev_group(struct iss_device *iss,
 		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);
+			dev_err(iss->dev, "Unable to register subdev %s\n",
+				board_info->board_info->type);
 			continue;
 		}
 
@@ -1185,16 +1185,16 @@ static int iss_register_entities(struct iss_device *iss)
 	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);
+		dev_err(iss->dev, "Media device registration failed (%d)\n",
+			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);
+		dev_err(iss->dev, "V4L2 device registration failed (%d)\n",
+			ret);
 		goto done;
 	}
 
@@ -1252,8 +1252,8 @@ static int iss_register_entities(struct iss_device *iss)
 			break;
 
 		default:
-			dev_err(iss->dev, "%s: invalid interface type %u\n",
-				__func__, subdevs->interface);
+			dev_err(iss->dev, "invalid interface type %u\n",
+				subdevs->interface);
 			ret = -EINVAL;
 			goto done;
 		}
diff --git a/drivers/staging/media/omap4iss/iss_video.c b/drivers/staging/media/omap4iss/iss_video.c
index 69550445a341..55938cccde7f 100644
--- a/drivers/staging/media/omap4iss/iss_video.c
+++ b/drivers/staging/media/omap4iss/iss_video.c
@@ -1221,8 +1221,7 @@ int omap4iss_video_register(struct iss_video *video, struct v4l2_device *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);
+			"could not register video device (%d)\n", ret);
 
 	return ret;
 }
diff --git a/drivers/staging/mt29f_spinand/mt29f_spinand.c b/drivers/staging/mt29f_spinand/mt29f_spinand.c
index 3b191fce45ec..7285c64bac24 100644
--- a/drivers/staging/mt29f_spinand/mt29f_spinand.c
+++ b/drivers/staging/mt29f_spinand/mt29f_spinand.c
@@ -770,7 +770,7 @@ static void spinand_cmdfunc(struct mtd_info *mtd, unsigned int command,
 		break;
 	case NAND_CMD_READID:
 		state->buf_ptr = 0;
-		spinand_read_id(info->spi, (u8 *)state->buf);
+		spinand_read_id(info->spi, state->buf);
 		break;
 	case NAND_CMD_PARAM:
 		state->buf_ptr = 0;
diff --git a/drivers/staging/netlogic/xlr_net.c b/drivers/staging/netlogic/xlr_net.c
index e8aae09d1624..8ae01753b011 100644
--- a/drivers/staging/netlogic/xlr_net.c
+++ b/drivers/staging/netlogic/xlr_net.c
@@ -1012,7 +1012,7 @@ static int xlr_net_probe(struct platform_device *pdev)
 	 * Allocate our adapter data structure and attach it to the device.
 	 */
 	adapter = (struct xlr_adapter *)
-		devm_kzalloc(&pdev->dev, sizeof(adapter), GFP_KERNEL);
+		devm_kzalloc(&pdev->dev, sizeof(*adapter), GFP_KERNEL);
 	if (!adapter) {
 		err = -ENOMEM;
 		return err;
diff --git a/drivers/staging/nvec/Kconfig b/drivers/staging/nvec/Kconfig
index 9475e20c3d64..e3a89fb1a4a2 100644
--- a/drivers/staging/nvec/Kconfig
+++ b/drivers/staging/nvec/Kconfig
@@ -6,6 +6,9 @@ config MFD_NVEC
 	    Say Y here to enable support for a nVidia compliant embedded
 	    controller.
 
+	    To compile this driver as a module, say M here: the module will be
+            called mfd-nvec
+
 config KEYBOARD_NVEC
 	tristate "Keyboard on nVidia compliant EC"
 	depends on MFD_NVEC && INPUT
@@ -13,6 +16,9 @@ config KEYBOARD_NVEC
 	  Say Y here to enable support for a keyboard connected to
 	  a nVidia compliant embedded controller.
 
+	  To compile this driver as a module, say M here: the module will be
+          called keyboard-nvec
+
 config SERIO_NVEC_PS2
 	tristate "PS2 on nVidia EC"
 	depends on MFD_NVEC && SERIO
@@ -20,6 +26,10 @@ config SERIO_NVEC_PS2
 	  Say Y here to enable support for a Touchpad / Mouse connected
 	  to a nVidia compliant embedded controller.
 
+	  To compile this driver as a module, say M here: the module will be
+          called serio-nvec-ps2
+
+
 config NVEC_POWER
 	tristate "NVEC charger and battery"
 	depends on MFD_NVEC && POWER_SUPPLY
@@ -27,9 +37,17 @@ config NVEC_POWER
 	  Say Y to enable support for battery and charger interface for
 	  nVidia compliant embedded controllers.
 
+	  To compile this driver as a module, say M here: the module will be
+          called nvec-power
+
+
 config NVEC_PAZ00
 	tristate "Support for OEM specific functions on Compal PAZ00 based devices"
 	depends on MFD_NVEC && LEDS_CLASS
 	help
 	  Say Y to enable control of the yellow side leds on Compal PAZ00 based
 	  devices, e.g. Toshbia AC100 and Dynabooks AZ netbooks.
+
+	  To compile this driver as a module, say M here: the module will be
+          called nvec-paz00
+
diff --git a/drivers/staging/nvec/nvec.c b/drivers/staging/nvec/nvec.c
index 5868ebb8389e..1bdc8d001e65 100644
--- a/drivers/staging/nvec/nvec.c
+++ b/drivers/staging/nvec/nvec.c
@@ -803,7 +803,7 @@ static int tegra_nvec_probe(struct platform_device *pdev)
 	}
 
 	nvec = devm_kzalloc(&pdev->dev, sizeof(struct nvec_chip), GFP_KERNEL);
-	if (nvec == NULL)
+	if (!nvec)
 		return -ENOMEM;
 
 	platform_set_drvdata(pdev, nvec);
diff --git a/drivers/staging/nvec/nvec_paz00.c b/drivers/staging/nvec/nvec_paz00.c
index f0cea0e43c96..68146bfee2b3 100644
--- a/drivers/staging/nvec/nvec_paz00.c
+++ b/drivers/staging/nvec/nvec_paz00.c
@@ -51,7 +51,7 @@ static int nvec_paz00_probe(struct platform_device *pdev)
 	int ret = 0;
 
 	led = devm_kzalloc(&pdev->dev, sizeof(*led), GFP_KERNEL);
-	if (led == NULL)
+	if (!led)
 		return -ENOMEM;
 
 	led->cdev.max_brightness = NVEC_LED_MAX;
diff --git a/drivers/staging/nvec/nvec_power.c b/drivers/staging/nvec/nvec_power.c
index 6a1459d4f8fb..04a7402ae2df 100644
--- a/drivers/staging/nvec/nvec_power.c
+++ b/drivers/staging/nvec/nvec_power.c
@@ -82,8 +82,8 @@ struct bat_response {
 	};
 };
 
-static struct power_supply nvec_bat_psy;
-static struct power_supply nvec_psy;
+static struct power_supply *nvec_bat_psy;
+static struct power_supply *nvec_psy;
 
 static int nvec_power_notifier(struct notifier_block *nb,
 			       unsigned long event_type, void *data)
@@ -98,7 +98,7 @@ static int nvec_power_notifier(struct notifier_block *nb,
 	if (res->sub_type == 0) {
 		if (power->on != res->plu) {
 			power->on = res->plu;
-			power_supply_changed(&nvec_psy);
+			power_supply_changed(nvec_psy);
 		}
 		return NOTIFY_STOP;
 	}
@@ -167,7 +167,7 @@ static int nvec_power_bat_notifier(struct notifier_block *nb,
 		}
 		power->bat_cap = res->plc[1];
 		if (status_changed)
-			power_supply_changed(&nvec_bat_psy);
+			power_supply_changed(nvec_bat_psy);
 		break;
 	case VOLTAGE:
 		power->bat_voltage_now = res->plu * 1000;
@@ -225,7 +225,7 @@ static int nvec_power_get_property(struct power_supply *psy,
 				   enum power_supply_property psp,
 				   union power_supply_propval *val)
 {
-	struct nvec_power *power = dev_get_drvdata(psy->dev->parent);
+	struct nvec_power *power = dev_get_drvdata(psy->dev.parent);
 
 	switch (psp) {
 	case POWER_SUPPLY_PROP_ONLINE:
@@ -241,7 +241,7 @@ static int nvec_battery_get_property(struct power_supply *psy,
 				     enum power_supply_property psp,
 				     union power_supply_propval *val)
 {
-	struct nvec_power *power = dev_get_drvdata(psy->dev->parent);
+	struct nvec_power *power = dev_get_drvdata(psy->dev.parent);
 
 	switch (psp) {
 	case POWER_SUPPLY_PROP_STATUS:
@@ -323,7 +323,7 @@ static char *nvec_power_supplied_to[] = {
 	"battery",
 };
 
-static struct power_supply nvec_bat_psy = {
+static const struct power_supply_desc nvec_bat_psy_desc = {
 	.name = "battery",
 	.type = POWER_SUPPLY_TYPE_BATTERY,
 	.properties = nvec_battery_props,
@@ -331,11 +331,9 @@ static struct power_supply nvec_bat_psy = {
 	.get_property = nvec_battery_get_property,
 };
 
-static struct power_supply nvec_psy = {
+static const struct power_supply_desc nvec_psy_desc = {
 	.name = "ac",
 	.type = POWER_SUPPLY_TYPE_MAINS,
-	.supplied_to = nvec_power_supplied_to,
-	.num_supplicants = ARRAY_SIZE(nvec_power_supplied_to),
 	.properties = nvec_power_props,
 	.num_properties = ARRAY_SIZE(nvec_power_props),
 	.get_property = nvec_power_get_property,
@@ -373,12 +371,14 @@ static void nvec_power_poll(struct work_struct *work)
 
 static int nvec_power_probe(struct platform_device *pdev)
 {
-	struct power_supply *psy;
+	struct power_supply **psy;
+	const struct power_supply_desc *psy_desc;
 	struct nvec_power *power;
 	struct nvec_chip *nvec = dev_get_drvdata(pdev->dev.parent);
+	struct power_supply_config psy_cfg = {};
 
 	power = devm_kzalloc(&pdev->dev, sizeof(struct nvec_power), GFP_NOWAIT);
-	if (power == NULL)
+	if (!power)
 		return -ENOMEM;
 
 	dev_set_drvdata(&pdev->dev, power);
@@ -387,6 +387,9 @@ static int nvec_power_probe(struct platform_device *pdev)
 	switch (pdev->id) {
 	case AC:
 		psy = &nvec_psy;
+		psy_desc = &nvec_psy_desc;
+		psy_cfg.supplied_to = nvec_power_supplied_to;
+		psy_cfg.num_supplicants = ARRAY_SIZE(nvec_power_supplied_to);
 
 		power->notifier.notifier_call = nvec_power_notifier;
 
@@ -395,6 +398,7 @@ static int nvec_power_probe(struct platform_device *pdev)
 		break;
 	case BAT:
 		psy = &nvec_bat_psy;
+		psy_desc = &nvec_bat_psy_desc;
 
 		power->notifier.notifier_call = nvec_power_bat_notifier;
 		break;
@@ -407,7 +411,9 @@ static int nvec_power_probe(struct platform_device *pdev)
 	if (pdev->id == BAT)
 		get_bat_mfg_data(power);
 
-	return power_supply_register(&pdev->dev, psy);
+	*psy = power_supply_register(&pdev->dev, psy_desc, &psy_cfg);
+
+	return PTR_ERR_OR_ZERO(*psy);
 }
 
 static int nvec_power_remove(struct platform_device *pdev)
@@ -418,10 +424,10 @@ static int nvec_power_remove(struct platform_device *pdev)
 	nvec_unregister_notifier(power->nvec, &power->notifier);
 	switch (pdev->id) {
 	case AC:
-		power_supply_unregister(&nvec_psy);
+		power_supply_unregister(nvec_psy);
 		break;
 	case BAT:
-		power_supply_unregister(&nvec_bat_psy);
+		power_supply_unregister(nvec_bat_psy);
 	}
 
 	return 0;
diff --git a/drivers/staging/nvec/nvec_ps2.c b/drivers/staging/nvec/nvec_ps2.c
index 4fd63c239454..6ebbc82323c3 100644
--- a/drivers/staging/nvec/nvec_ps2.c
+++ b/drivers/staging/nvec/nvec_ps2.c
@@ -109,7 +109,7 @@ static int nvec_mouse_probe(struct platform_device *pdev)
 	char mouse_reset[] = { NVEC_PS2, SEND_COMMAND, PSMOUSE_RST, 3 };
 
 	ser_dev = devm_kzalloc(&pdev->dev, sizeof(struct serio), GFP_KERNEL);
-	if (ser_dev == NULL)
+	if (!ser_dev)
 		return -ENOMEM;
 
 	ser_dev->id.type = SERIO_PS_PSTHRU;
diff --git a/drivers/staging/octeon-usb/octeon-hcd.c b/drivers/staging/octeon-usb/octeon-hcd.c
index 1daeb3125a1f..9e5476e352b4 100644
--- a/drivers/staging/octeon-usb/octeon-hcd.c
+++ b/drivers/staging/octeon-usb/octeon-hcd.c
@@ -214,15 +214,14 @@ enum cvmx_usb_initialize_flags {
 /**
  * enum cvmx_usb_pipe_flags - internal flags for a pipe.
  *
- * @__CVMX_USB_PIPE_FLAGS_SCHEDULED: Used internally to determine if a pipe is
- *				     actively using hardware. Do not use.
- * @__CVMX_USB_PIPE_FLAGS_NEED_PING: Used internally to determine if a high
- *				     speed pipe is in the ping state. Do not
- *				     use.
+ * @CVMX_USB_PIPE_FLAGS_SCHEDULED: Used internally to determine if a pipe is
+ *				   actively using hardware.
+ * @CVMX_USB_PIPE_FLAGS_NEED_PING: Used internally to determine if a high speed
+ *				   pipe is in the ping state.
  */
 enum cvmx_usb_pipe_flags {
-	__CVMX_USB_PIPE_FLAGS_SCHEDULED	= 1 << 17,
-	__CVMX_USB_PIPE_FLAGS_NEED_PING	= 1 << 18,
+	CVMX_USB_PIPE_FLAGS_SCHEDULED	= 1 << 17,
+	CVMX_USB_PIPE_FLAGS_NEED_PING	= 1 << 18,
 };
 
 /* Maximum number of times to retry failed transactions */
@@ -231,15 +230,6 @@ enum cvmx_usb_pipe_flags {
 /* Maximum number of hardware channels supported by the USB block */
 #define MAX_CHANNELS		8
 
-/* The highest valid USB device address */
-#define MAX_USB_ADDRESS		127
-
-/* The highest valid USB endpoint number */
-#define MAX_USB_ENDPOINT	15
-
-/* The highest valid port number on a hub */
-#define MAX_USB_HUB_PORT	15
-
 /*
  * The low level hardware can transfer a maximum of this number of bytes in each
  * transfer. The field is 19 bits wide
@@ -403,16 +393,18 @@ struct octeon_hcd {
 	struct cvmx_usb_state usb;
 };
 
-/* This macro spins on a field waiting for it to reach a value */
-#define CVMX_WAIT_FOR_FIELD32(address, type, field, op, value, timeout_usec)\
+/* This macro spins on a register waiting for it to reach a condition. */
+#define CVMX_WAIT_FOR_FIELD32(address, _union, cond, timeout_usec)	    \
 	({int result;							    \
 	do {								    \
 		uint64_t done = cvmx_get_cycle() + (uint64_t)timeout_usec * \
 			octeon_get_clock_rate() / 1000000;		    \
-		type c;							    \
+		union _union c;						    \
+									    \
 		while (1) {						    \
-			c.u32 = __cvmx_usb_read_csr32(usb, address);	    \
-			if (c.s.field op (value)) {			    \
+			c.u32 = cvmx_usb_read_csr32(usb, address);	    \
+									    \
+			if (cond) {					    \
 				result = 0;				    \
 				break;					    \
 			} else if (cvmx_get_cycle() > done) {		    \
@@ -428,12 +420,13 @@ struct octeon_hcd {
  * This macro logically sets a single field in a CSR. It does the sequence
  * read, modify, and write
  */
-#define USB_SET_FIELD32(address, type, field, value)		\
+#define USB_SET_FIELD32(address, _union, field, value)		\
 	do {							\
-		type c;						\
-		c.u32 = __cvmx_usb_read_csr32(usb, address);	\
+		union _union c;					\
+								\
+		c.u32 = cvmx_usb_read_csr32(usb, address);	\
 		c.s.field = value;				\
-		__cvmx_usb_write_csr32(usb, address, c.u32);	\
+		cvmx_usb_write_csr32(usb, address, c.u32);	\
 	} while (0)
 
 /* Returns the IO address to push/pop stuff data from the FIFOs */
@@ -442,7 +435,6 @@ struct octeon_hcd {
 
 /**
  * struct octeon_temp_buffer - a bounce buffer for USB transfers
- * @temp_buffer: the newly allocated temporary buffer (including meta-data)
  * @orig_buffer: the original buffer passed by the USB stack
  * @data:	 the newly allocated temporary buffer (excluding meta-data)
  *
@@ -451,7 +443,6 @@ struct octeon_hcd {
  * represents it.
  */
 struct octeon_temp_buffer {
-	void *temp_buffer;
 	void *orig_buffer;
 	u8 data[0];
 };
@@ -489,7 +480,6 @@ static int octeon_alloc_temp_buffer(struct urb *urb, gfp_t mem_flags)
 	if (!temp)
 		return -ENOMEM;
 
-	temp->temp_buffer = temp;
 	temp->orig_buffer = urb->transfer_buffer;
 	if (usb_urb_dir_out(urb))
 		memcpy(temp->data, urb->transfer_buffer,
@@ -520,7 +510,7 @@ static void octeon_free_temp_buffer(struct urb *urb)
 		       urb->actual_length);
 	urb->transfer_buffer = temp->orig_buffer;
 	urb->transfer_flags &= ~URB_ALIGNED_TEMP_BUFFER;
-	kfree(temp->temp_buffer);
+	kfree(temp);
 }
 
 /**
@@ -566,8 +556,8 @@ static void octeon_unmap_urb_for_dma(struct usb_hcd *hcd, struct urb *urb)
  *
  * Returns: Result of the read
  */
-static inline uint32_t __cvmx_usb_read_csr32(struct cvmx_usb_state *usb,
-					     uint64_t address)
+static inline uint32_t cvmx_usb_read_csr32(struct cvmx_usb_state *usb,
+					   uint64_t address)
 {
 	uint32_t result = cvmx_read64_uint32(address ^ 4);
 	return result;
@@ -583,45 +573,13 @@ static inline uint32_t __cvmx_usb_read_csr32(struct cvmx_usb_state *usb,
  * @address: 64bit address to write
  * @value:   Value to write
  */
-static inline void __cvmx_usb_write_csr32(struct cvmx_usb_state *usb,
-					  uint64_t address, uint32_t value)
+static inline void cvmx_usb_write_csr32(struct cvmx_usb_state *usb,
+					uint64_t address, uint32_t value)
 {
 	cvmx_write64_uint32(address ^ 4, value);
 	cvmx_read64_uint64(CVMX_USBNX_DMA0_INB_CHN0(usb->index));
 }
 
-
-/**
- * Read a USB 64bit CSR. It logs the value in a readable format if
- * debugging is on.
- *
- * @usb:     USB block this access is for
- * @address: 64bit address to read
- *
- * Returns: Result of the read
- */
-static inline uint64_t __cvmx_usb_read_csr64(struct cvmx_usb_state *usb,
-					     uint64_t address)
-{
-	uint64_t result = cvmx_read64_uint64(address);
-	return result;
-}
-
-
-/**
- * Write a USB 64bit CSR. It logs the value in a readable format
- * if debugging is on.
- *
- * @usb:     USB block this access is for
- * @address: 64bit address to write
- * @value:   Value to write
- */
-static inline void __cvmx_usb_write_csr64(struct cvmx_usb_state *usb,
-					  uint64_t address, uint64_t value)
-{
-	cvmx_write64_uint64(address, value);
-}
-
 /**
  * Return non zero if this pipe connects to a non HIGH speed
  * device through a high speed hub.
@@ -631,8 +589,8 @@ static inline void __cvmx_usb_write_csr64(struct cvmx_usb_state *usb,
  *
  * Returns: Non zero if we need to do split transactions
  */
-static inline int __cvmx_usb_pipe_needs_split(struct cvmx_usb_state *usb,
-					      struct cvmx_usb_pipe *pipe)
+static inline int cvmx_usb_pipe_needs_split(struct cvmx_usb_state *usb,
+					    struct cvmx_usb_pipe *pipe)
 {
 	return pipe->device_speed != CVMX_USB_SPEED_HIGH &&
 	       usb->usbcx_hprt.s.prtspd == CVMX_USB_SPEED_HIGH;
@@ -646,48 +604,119 @@ static inline int __cvmx_usb_pipe_needs_split(struct cvmx_usb_state *usb,
  *
  * Returns: PID for pipe
  */
-static inline int __cvmx_usb_get_data_pid(struct cvmx_usb_pipe *pipe)
+static inline int cvmx_usb_get_data_pid(struct cvmx_usb_pipe *pipe)
 {
 	if (pipe->pid_toggle)
 		return 2; /* Data1 */
 	return 0; /* Data0 */
 }
 
+static void cvmx_fifo_setup(struct cvmx_usb_state *usb)
+{
+	union cvmx_usbcx_ghwcfg3 usbcx_ghwcfg3;
+	union cvmx_usbcx_gnptxfsiz npsiz;
+	union cvmx_usbcx_hptxfsiz psiz;
+
+	usbcx_ghwcfg3.u32 = cvmx_usb_read_csr32(usb,
+						CVMX_USBCX_GHWCFG3(usb->index));
+
+	/*
+	 * Program the USBC_GRXFSIZ register to select the size of the receive
+	 * FIFO (25%).
+	 */
+	USB_SET_FIELD32(CVMX_USBCX_GRXFSIZ(usb->index), cvmx_usbcx_grxfsiz,
+			rxfdep, usbcx_ghwcfg3.s.dfifodepth / 4);
+
+	/*
+	 * Program the USBC_GNPTXFSIZ register to select the size and the start
+	 * address of the non-periodic transmit FIFO for nonperiodic
+	 * transactions (50%).
+	 */
+	npsiz.u32 = cvmx_usb_read_csr32(usb, CVMX_USBCX_GNPTXFSIZ(usb->index));
+	npsiz.s.nptxfdep = usbcx_ghwcfg3.s.dfifodepth / 2;
+	npsiz.s.nptxfstaddr = usbcx_ghwcfg3.s.dfifodepth / 4;
+	cvmx_usb_write_csr32(usb, CVMX_USBCX_GNPTXFSIZ(usb->index), npsiz.u32);
+
+	/*
+	 * Program the USBC_HPTXFSIZ register to select the size and start
+	 * address of the periodic transmit FIFO for periodic transactions
+	 * (25%).
+	 */
+	psiz.u32 = cvmx_usb_read_csr32(usb, CVMX_USBCX_HPTXFSIZ(usb->index));
+	psiz.s.ptxfsize = usbcx_ghwcfg3.s.dfifodepth / 4;
+	psiz.s.ptxfstaddr = 3 * usbcx_ghwcfg3.s.dfifodepth / 4;
+	cvmx_usb_write_csr32(usb, CVMX_USBCX_HPTXFSIZ(usb->index), psiz.u32);
+
+	/* Flush all FIFOs */
+	USB_SET_FIELD32(CVMX_USBCX_GRSTCTL(usb->index),
+			cvmx_usbcx_grstctl, txfnum, 0x10);
+	USB_SET_FIELD32(CVMX_USBCX_GRSTCTL(usb->index),
+			cvmx_usbcx_grstctl, txfflsh, 1);
+	CVMX_WAIT_FOR_FIELD32(CVMX_USBCX_GRSTCTL(usb->index),
+			      cvmx_usbcx_grstctl, c.s.txfflsh == 0, 100);
+	USB_SET_FIELD32(CVMX_USBCX_GRSTCTL(usb->index),
+			cvmx_usbcx_grstctl, rxfflsh, 1);
+	CVMX_WAIT_FOR_FIELD32(CVMX_USBCX_GRSTCTL(usb->index),
+			      cvmx_usbcx_grstctl, c.s.rxfflsh == 0, 100);
+}
+
+/**
+ * Shutdown a USB port after a call to cvmx_usb_initialize().
+ * The port should be disabled with all pipes closed when this
+ * function is called.
+ *
+ * @usb: USB device state populated by cvmx_usb_initialize().
+ *
+ * Returns: 0 or a negative error code.
+ */
+static int cvmx_usb_shutdown(struct cvmx_usb_state *usb)
+{
+	union cvmx_usbnx_clk_ctl usbn_clk_ctl;
+
+	/* Make sure all pipes are closed */
+	if (!list_empty(&usb->idle_pipes) ||
+	    !list_empty(&usb->active_pipes[CVMX_USB_TRANSFER_ISOCHRONOUS]) ||
+	    !list_empty(&usb->active_pipes[CVMX_USB_TRANSFER_INTERRUPT]) ||
+	    !list_empty(&usb->active_pipes[CVMX_USB_TRANSFER_CONTROL]) ||
+	    !list_empty(&usb->active_pipes[CVMX_USB_TRANSFER_BULK]))
+		return -EBUSY;
+
+	/* Disable the clocks and put them in power on reset */
+	usbn_clk_ctl.u64 = cvmx_read64_uint64(CVMX_USBNX_CLK_CTL(usb->index));
+	usbn_clk_ctl.s.enable = 1;
+	usbn_clk_ctl.s.por = 1;
+	usbn_clk_ctl.s.hclk_rst = 1;
+	usbn_clk_ctl.s.prst = 0;
+	usbn_clk_ctl.s.hrst = 0;
+	cvmx_write64_uint64(CVMX_USBNX_CLK_CTL(usb->index), usbn_clk_ctl.u64);
+	return 0;
+}
+
 /**
  * Initialize a USB port for use. This must be called before any
  * other access to the Octeon USB port is made. The port starts
  * off in the disabled state.
  *
- * @usb:	 Pointer to an empty struct cvmx_usb_state
- *		 that will be populated by the initialize call.
- *		 This structure is then passed to all other USB
- *		 functions.
- * @usb_port_number:
- *		 Which Octeon USB port to initialize.
+ * @dev:	 Pointer to struct device for logging purposes.
+ * @usb:	 Pointer to struct cvmx_usb_state.
  *
  * Returns: 0 or a negative error code.
  */
-static int cvmx_usb_initialize(struct cvmx_usb_state *usb,
-			       int usb_port_number,
-			       enum cvmx_usb_initialize_flags flags)
+static int cvmx_usb_initialize(struct device *dev,
+			       struct cvmx_usb_state *usb)
 {
+	int channel;
+	int divisor;
+	int retries = 0;
+	union cvmx_usbcx_hcfg usbcx_hcfg;
 	union cvmx_usbnx_clk_ctl usbn_clk_ctl;
+	union cvmx_usbcx_gintsts usbc_gintsts;
+	union cvmx_usbcx_gahbcfg usbcx_gahbcfg;
+	union cvmx_usbcx_gintmsk usbcx_gintmsk;
+	union cvmx_usbcx_gusbcfg usbcx_gusbcfg;
 	union cvmx_usbnx_usbp_ctl_status usbn_usbp_ctl_status;
-	int i;
-
-	/* At first allow 0-1 for the usb port number */
-	if ((usb_port_number < 0) || (usb_port_number > 1))
-		return -EINVAL;
-
-	memset(usb, 0, sizeof(*usb));
-	usb->init_flags = flags;
-
-	/* Initialize the USB state structure */
-	usb->index = usb_port_number;
-	INIT_LIST_HEAD(&usb->idle_pipes);
-	for (i = 0; i < ARRAY_SIZE(usb->active_pipes); i++)
-		INIT_LIST_HEAD(&usb->active_pipes[i]);
 
+retry:
 	/*
 	 * Power On Reset and PHY Initialization
 	 *
@@ -696,8 +725,7 @@ static int cvmx_usb_initialize(struct cvmx_usb_state *usb,
 	 * 2a. Write USBN0/1_CLK_CTL[POR] = 1 and
 	 *     USBN0/1_CLK_CTL[HRST,PRST,HCLK_RST] = 0
 	 */
-	usbn_clk_ctl.u64 =
-		__cvmx_usb_read_csr64(usb, CVMX_USBNX_CLK_CTL(usb->index));
+	usbn_clk_ctl.u64 = cvmx_read64_uint64(CVMX_USBNX_CLK_CTL(usb->index));
 	usbn_clk_ctl.s.por = 1;
 	usbn_clk_ctl.s.hrst = 0;
 	usbn_clk_ctl.s.prst = 0;
@@ -722,7 +750,8 @@ static int cvmx_usb_initialize(struct cvmx_usb_state *usb,
 			/* From CN52XX manual */
 			usbn_clk_ctl.s.p_rtype = 1;
 
-		switch (flags & CVMX_USB_INITIALIZE_FLAGS_CLOCK_MHZ_MASK) {
+		switch (usb->init_flags &
+			CVMX_USB_INITIALIZE_FLAGS_CLOCK_MHZ_MASK) {
 		case CVMX_USB_INITIALIZE_FLAGS_CLOCK_12MHZ:
 			usbn_clk_ctl.s.p_c_sel = 0;
 			break;
@@ -752,20 +781,17 @@ static int cvmx_usb_initialize(struct cvmx_usb_state *usb,
 	 *     setting USBN0/1_CLK_CTL[ENABLE] = 1. Divide the core clock down
 	 *     such that USB is as close as possible to 125Mhz
 	 */
-	{
-		int divisor = DIV_ROUND_UP(octeon_get_clock_rate(), 125000000);
-		/* Lower than 4 doesn't seem to work properly */
-		if (divisor < 4)
-			divisor = 4;
-		usbn_clk_ctl.s.divide = divisor;
-		usbn_clk_ctl.s.divide2 = 0;
-	}
-	__cvmx_usb_write_csr64(usb, CVMX_USBNX_CLK_CTL(usb->index),
-			       usbn_clk_ctl.u64);
+	divisor = DIV_ROUND_UP(octeon_get_clock_rate(), 125000000);
+	/* Lower than 4 doesn't seem to work properly */
+	if (divisor < 4)
+		divisor = 4;
+	usbn_clk_ctl.s.divide = divisor;
+	usbn_clk_ctl.s.divide2 = 0;
+	cvmx_write64_uint64(CVMX_USBNX_CLK_CTL(usb->index), usbn_clk_ctl.u64);
+
 	/* 2d. Write USBN0/1_CLK_CTL[HCLK_RST] = 1 */
 	usbn_clk_ctl.s.hclk_rst = 1;
-	__cvmx_usb_write_csr64(usb, CVMX_USBNX_CLK_CTL(usb->index),
-			       usbn_clk_ctl.u64);
+	cvmx_write64_uint64(CVMX_USBNX_CLK_CTL(usb->index), usbn_clk_ctl.u64);
 	/* 2e.  Wait 64 core-clock cycles for HCLK to stabilize */
 	cvmx_wait(64);
 	/*
@@ -774,8 +800,7 @@ static int cvmx_usb_initialize(struct cvmx_usb_state *usb,
 	 *    USBN_CLK_CTL[POR] = 0
 	 */
 	usbn_clk_ctl.s.por = 0;
-	__cvmx_usb_write_csr64(usb, CVMX_USBNX_CLK_CTL(usb->index),
-			       usbn_clk_ctl.u64);
+	cvmx_write64_uint64(CVMX_USBNX_CLK_CTL(usb->index), usbn_clk_ctl.u64);
 	/* 4. Wait 1 ms for PHY clock to start */
 	mdelay(1);
 	/*
@@ -783,11 +808,11 @@ static int cvmx_usb_initialize(struct cvmx_usb_state *usb,
 	 *    USBP control and status register:
 	 *    USBN_USBP_CTL_STATUS[ATE_RESET] = 1
 	 */
-	usbn_usbp_ctl_status.u64 = __cvmx_usb_read_csr64(usb,
-			CVMX_USBNX_USBP_CTL_STATUS(usb->index));
+	usbn_usbp_ctl_status.u64 =
+		cvmx_read64_uint64(CVMX_USBNX_USBP_CTL_STATUS(usb->index));
 	usbn_usbp_ctl_status.s.ate_reset = 1;
-	__cvmx_usb_write_csr64(usb, CVMX_USBNX_USBP_CTL_STATUS(usb->index),
-			       usbn_usbp_ctl_status.u64);
+	cvmx_write64_uint64(CVMX_USBNX_USBP_CTL_STATUS(usb->index),
+			    usbn_usbp_ctl_status.u64);
 	/* 6. Wait 10 cycles */
 	cvmx_wait(10);
 	/*
@@ -795,23 +820,22 @@ static int cvmx_usb_initialize(struct cvmx_usb_state *usb,
 	 *    USBN_USBP_CTL_STATUS[ATE_RESET] = 0
 	 */
 	usbn_usbp_ctl_status.s.ate_reset = 0;
-	__cvmx_usb_write_csr64(usb, CVMX_USBNX_USBP_CTL_STATUS(usb->index),
-			       usbn_usbp_ctl_status.u64);
+	cvmx_write64_uint64(CVMX_USBNX_USBP_CTL_STATUS(usb->index),
+			    usbn_usbp_ctl_status.u64);
 	/*
 	 * 8. Program the PHY reset field in the USBN clock-control register:
 	 *    USBN_CLK_CTL[PRST] = 1
 	 */
 	usbn_clk_ctl.s.prst = 1;
-	__cvmx_usb_write_csr64(usb, CVMX_USBNX_CLK_CTL(usb->index),
-			       usbn_clk_ctl.u64);
+	cvmx_write64_uint64(CVMX_USBNX_CLK_CTL(usb->index), usbn_clk_ctl.u64);
 	/*
 	 * 9. Program the USBP control and status register to select host or
 	 *    device mode. USBN_USBP_CTL_STATUS[HST_MODE] = 0 for host, = 1 for
 	 *    device
 	 */
 	usbn_usbp_ctl_status.s.hst_mode = 0;
-	__cvmx_usb_write_csr64(usb, CVMX_USBNX_USBP_CTL_STATUS(usb->index),
-			       usbn_usbp_ctl_status.u64);
+	cvmx_write64_uint64(CVMX_USBNX_USBP_CTL_STATUS(usb->index),
+			    usbn_usbp_ctl_status.u64);
 	/* 10. Wait 1 us */
 	udelay(1);
 	/*
@@ -819,12 +843,10 @@ static int cvmx_usb_initialize(struct cvmx_usb_state *usb,
 	 *     USBN_CLK_CTL[HRST] = 1
 	 */
 	usbn_clk_ctl.s.hrst = 1;
-	__cvmx_usb_write_csr64(usb, CVMX_USBNX_CLK_CTL(usb->index),
-			       usbn_clk_ctl.u64);
+	cvmx_write64_uint64(CVMX_USBNX_CLK_CTL(usb->index), usbn_clk_ctl.u64);
 	/* 12. Proceed to USB core initialization */
 	usbn_clk_ctl.s.enable = 1;
-	__cvmx_usb_write_csr64(usb, CVMX_USBNX_CLK_CTL(usb->index),
-			       usbn_clk_ctl.u64);
+	cvmx_write64_uint64(CVMX_USBNX_CLK_CTL(usb->index), usbn_clk_ctl.u64);
 	udelay(1);
 
 	/*
@@ -845,29 +867,16 @@ static int cvmx_usb_initialize(struct cvmx_usb_state *usb,
 	 *    USBC_GAHBCFG[PTXFEMPLVL]
 	 *    Global interrupt mask, USBC_GAHBCFG[GLBLINTRMSK] = 1
 	 */
-	{
-		union cvmx_usbcx_gahbcfg usbcx_gahbcfg;
-		/* Due to an errata, CN31XX doesn't support DMA */
-		if (OCTEON_IS_MODEL(OCTEON_CN31XX))
-			usb->init_flags |= CVMX_USB_INITIALIZE_FLAGS_NO_DMA;
-		usbcx_gahbcfg.u32 = 0;
-		usbcx_gahbcfg.s.dmaen = !(usb->init_flags &
-					  CVMX_USB_INITIALIZE_FLAGS_NO_DMA);
-		if (usb->init_flags & CVMX_USB_INITIALIZE_FLAGS_NO_DMA)
-			/* Only use one channel with non DMA */
-			usb->idle_hardware_channels = 0x1;
-		else if (OCTEON_IS_MODEL(OCTEON_CN5XXX))
-			/* CN5XXX have an errata with channel 3 */
-			usb->idle_hardware_channels = 0xf7;
-		else
-			usb->idle_hardware_channels = 0xff;
-		usbcx_gahbcfg.s.hbstlen = 0;
-		usbcx_gahbcfg.s.nptxfemplvl = 1;
-		usbcx_gahbcfg.s.ptxfemplvl = 1;
-		usbcx_gahbcfg.s.glblintrmsk = 1;
-		__cvmx_usb_write_csr32(usb, CVMX_USBCX_GAHBCFG(usb->index),
-				       usbcx_gahbcfg.u32);
-	}
+	usbcx_gahbcfg.u32 = 0;
+	usbcx_gahbcfg.s.dmaen = !(usb->init_flags &
+				  CVMX_USB_INITIALIZE_FLAGS_NO_DMA);
+	usbcx_gahbcfg.s.hbstlen = 0;
+	usbcx_gahbcfg.s.nptxfemplvl = 1;
+	usbcx_gahbcfg.s.ptxfemplvl = 1;
+	usbcx_gahbcfg.s.glblintrmsk = 1;
+	cvmx_usb_write_csr32(usb, CVMX_USBCX_GAHBCFG(usb->index),
+			     usbcx_gahbcfg.u32);
+
 	/*
 	 * 3. Program the following fields in USBC_GUSBCFG register.
 	 *    HS/FS timeout calibration, USBC_GUSBCFG[TOUTCAL] = 0
@@ -875,154 +884,98 @@ static int cvmx_usb_initialize(struct cvmx_usb_state *usb,
 	 *    USB turnaround time, USBC_GUSBCFG[USBTRDTIM] = 0x5
 	 *    PHY low-power clock select, USBC_GUSBCFG[PHYLPWRCLKSEL] = 0
 	 */
-	{
-		union cvmx_usbcx_gusbcfg usbcx_gusbcfg;
-
-		usbcx_gusbcfg.u32 = __cvmx_usb_read_csr32(usb,
-				CVMX_USBCX_GUSBCFG(usb->index));
-		usbcx_gusbcfg.s.toutcal = 0;
-		usbcx_gusbcfg.s.ddrsel = 0;
-		usbcx_gusbcfg.s.usbtrdtim = 0x5;
-		usbcx_gusbcfg.s.phylpwrclksel = 0;
-		__cvmx_usb_write_csr32(usb, CVMX_USBCX_GUSBCFG(usb->index),
-				       usbcx_gusbcfg.u32);
-	}
+	usbcx_gusbcfg.u32 = cvmx_usb_read_csr32(usb,
+						CVMX_USBCX_GUSBCFG(usb->index));
+	usbcx_gusbcfg.s.toutcal = 0;
+	usbcx_gusbcfg.s.ddrsel = 0;
+	usbcx_gusbcfg.s.usbtrdtim = 0x5;
+	usbcx_gusbcfg.s.phylpwrclksel = 0;
+	cvmx_usb_write_csr32(usb, CVMX_USBCX_GUSBCFG(usb->index),
+			     usbcx_gusbcfg.u32);
+
 	/*
 	 * 4. The software must unmask the following bits in the USBC_GINTMSK
 	 *    register.
 	 *    OTG interrupt mask, USBC_GINTMSK[OTGINTMSK] = 1
 	 *    Mode mismatch interrupt mask, USBC_GINTMSK[MODEMISMSK] = 1
 	 */
-	{
-		union cvmx_usbcx_gintmsk usbcx_gintmsk;
-		int channel;
-
-		usbcx_gintmsk.u32 = __cvmx_usb_read_csr32(usb,
-				CVMX_USBCX_GINTMSK(usb->index));
-		usbcx_gintmsk.s.otgintmsk = 1;
-		usbcx_gintmsk.s.modemismsk = 1;
-		usbcx_gintmsk.s.hchintmsk = 1;
-		usbcx_gintmsk.s.sofmsk = 0;
-		/* We need RX FIFO interrupts if we don't have DMA */
-		if (usb->init_flags & CVMX_USB_INITIALIZE_FLAGS_NO_DMA)
-			usbcx_gintmsk.s.rxflvlmsk = 1;
-		__cvmx_usb_write_csr32(usb, CVMX_USBCX_GINTMSK(usb->index),
-				       usbcx_gintmsk.u32);
-
-		/*
-		 * Disable all channel interrupts. We'll enable them per channel
-		 * later.
-		 */
-		for (channel = 0; channel < 8; channel++)
-			__cvmx_usb_write_csr32(usb,
-				CVMX_USBCX_HCINTMSKX(channel, usb->index), 0);
-	}
-
-	{
-		/*
-		 * Host Port Initialization
-		 *
-		 * 1. Program the host-port interrupt-mask field to unmask,
-		 *    USBC_GINTMSK[PRTINT] = 1
-		 */
-		USB_SET_FIELD32(CVMX_USBCX_GINTMSK(usb->index),
-				union cvmx_usbcx_gintmsk, prtintmsk, 1);
-		USB_SET_FIELD32(CVMX_USBCX_GINTMSK(usb->index),
-				union cvmx_usbcx_gintmsk, disconnintmsk, 1);
-		/*
-		 * 2. Program the USBC_HCFG register to select full-speed host
-		 *    or high-speed host.
-		 */
-		{
-			union cvmx_usbcx_hcfg usbcx_hcfg;
-
-			usbcx_hcfg.u32 = __cvmx_usb_read_csr32(usb,
-					CVMX_USBCX_HCFG(usb->index));
-			usbcx_hcfg.s.fslssupp = 0;
-			usbcx_hcfg.s.fslspclksel = 0;
-			__cvmx_usb_write_csr32(usb,
-					CVMX_USBCX_HCFG(usb->index),
-					usbcx_hcfg.u32);
-		}
-		/*
-		 * 3. Program the port power bit to drive VBUS on the USB,
-		 *    USBC_HPRT[PRTPWR] = 1
-		 */
-		USB_SET_FIELD32(CVMX_USBCX_HPRT(usb->index),
-				union cvmx_usbcx_hprt, prtpwr, 1);
-
-		/*
-		 * Steps 4-15 from the manual are done later in the port enable
-		 */
-	}
+	usbcx_gintmsk.u32 = cvmx_usb_read_csr32(usb,
+						CVMX_USBCX_GINTMSK(usb->index));
+	usbcx_gintmsk.s.otgintmsk = 1;
+	usbcx_gintmsk.s.modemismsk = 1;
+	usbcx_gintmsk.s.hchintmsk = 1;
+	usbcx_gintmsk.s.sofmsk = 0;
+	/* We need RX FIFO interrupts if we don't have DMA */
+	if (usb->init_flags & CVMX_USB_INITIALIZE_FLAGS_NO_DMA)
+		usbcx_gintmsk.s.rxflvlmsk = 1;
+	cvmx_usb_write_csr32(usb, CVMX_USBCX_GINTMSK(usb->index),
+			     usbcx_gintmsk.u32);
 
-	return 0;
-}
+	/*
+	 * Disable all channel interrupts. We'll enable them per channel later.
+	 */
+	for (channel = 0; channel < 8; channel++)
+		cvmx_usb_write_csr32(usb,
+				     CVMX_USBCX_HCINTMSKX(channel, usb->index),
+				     0);
 
+	/*
+	 * Host Port Initialization
+	 *
+	 * 1. Program the host-port interrupt-mask field to unmask,
+	 *    USBC_GINTMSK[PRTINT] = 1
+	 */
+	USB_SET_FIELD32(CVMX_USBCX_GINTMSK(usb->index),
+			cvmx_usbcx_gintmsk, prtintmsk, 1);
+	USB_SET_FIELD32(CVMX_USBCX_GINTMSK(usb->index),
+			cvmx_usbcx_gintmsk, disconnintmsk, 1);
 
-/**
- * Shutdown a USB port after a call to cvmx_usb_initialize().
- * The port should be disabled with all pipes closed when this
- * function is called.
- *
- * @usb: USB device state populated by cvmx_usb_initialize().
- *
- * Returns: 0 or a negative error code.
- */
-static int cvmx_usb_shutdown(struct cvmx_usb_state *usb)
-{
-	union cvmx_usbnx_clk_ctl usbn_clk_ctl;
+	/*
+	 * 2. Program the USBC_HCFG register to select full-speed host
+	 *    or high-speed host.
+	 */
+	usbcx_hcfg.u32 = cvmx_usb_read_csr32(usb, CVMX_USBCX_HCFG(usb->index));
+	usbcx_hcfg.s.fslssupp = 0;
+	usbcx_hcfg.s.fslspclksel = 0;
+	cvmx_usb_write_csr32(usb, CVMX_USBCX_HCFG(usb->index), usbcx_hcfg.u32);
 
-	/* Make sure all pipes are closed */
-	if (!list_empty(&usb->idle_pipes) ||
-	    !list_empty(&usb->active_pipes[CVMX_USB_TRANSFER_ISOCHRONOUS]) ||
-	    !list_empty(&usb->active_pipes[CVMX_USB_TRANSFER_INTERRUPT]) ||
-	    !list_empty(&usb->active_pipes[CVMX_USB_TRANSFER_CONTROL]) ||
-	    !list_empty(&usb->active_pipes[CVMX_USB_TRANSFER_BULK]))
-		return -EBUSY;
+	cvmx_fifo_setup(usb);
 
-	/* Disable the clocks and put them in power on reset */
-	usbn_clk_ctl.u64 = __cvmx_usb_read_csr64(usb,
-			CVMX_USBNX_CLK_CTL(usb->index));
-	usbn_clk_ctl.s.enable = 1;
-	usbn_clk_ctl.s.por = 1;
-	usbn_clk_ctl.s.hclk_rst = 1;
-	usbn_clk_ctl.s.prst = 0;
-	usbn_clk_ctl.s.hrst = 0;
-	__cvmx_usb_write_csr64(usb, CVMX_USBNX_CLK_CTL(usb->index),
-			       usbn_clk_ctl.u64);
-	return 0;
+	/*
+	 * If the controller is getting port events right after the reset, it
+	 * means the initialization failed. Try resetting the controller again
+	 * in such case. This is seen to happen after cold boot on DSR-1000N.
+	 */
+	usbc_gintsts.u32 = cvmx_usb_read_csr32(usb,
+					       CVMX_USBCX_GINTSTS(usb->index));
+	cvmx_usb_write_csr32(usb, CVMX_USBCX_GINTSTS(usb->index),
+			     usbc_gintsts.u32);
+	dev_dbg(dev, "gintsts after reset: 0x%x\n", (int)usbc_gintsts.u32);
+	if (!usbc_gintsts.s.disconnint && !usbc_gintsts.s.prtint)
+		return 0;
+	if (retries++ >= 5)
+		return -EAGAIN;
+	dev_info(dev, "controller reset failed (gintsts=0x%x) - retrying\n",
+		 (int)usbc_gintsts.u32);
+	msleep(50);
+	cvmx_usb_shutdown(usb);
+	msleep(50);
+	goto retry;
 }
 
-
 /**
- * Enable a USB port. After this call succeeds, the USB port is
+ * Reset a USB port. After this call succeeds, the USB port is
  * online and servicing requests.
  *
  * @usb: USB device state populated by cvmx_usb_initialize().
- *
- * Returns: 0 or a negative error code.
  */
-static int cvmx_usb_enable(struct cvmx_usb_state *usb)
+static void cvmx_usb_reset_port(struct cvmx_usb_state *usb)
 {
-	union cvmx_usbcx_ghwcfg3 usbcx_ghwcfg3;
-
-	usb->usbcx_hprt.u32 = __cvmx_usb_read_csr32(usb,
-			CVMX_USBCX_HPRT(usb->index));
-
-	/*
-	 * If the port is already enabled the just return. We don't need to do
-	 * anything
-	 */
-	if (usb->usbcx_hprt.s.prtena)
-		return 0;
-
-	/* If there is nothing plugged into the port then fail immediately */
-	if (!usb->usbcx_hprt.s.prtconnsts)
-		return -ETIMEDOUT;
+	usb->usbcx_hprt.u32 = cvmx_usb_read_csr32(usb,
+						  CVMX_USBCX_HPRT(usb->index));
 
 	/* Program the port reset bit to start the reset process */
-	USB_SET_FIELD32(CVMX_USBCX_HPRT(usb->index), union cvmx_usbcx_hprt,
+	USB_SET_FIELD32(CVMX_USBCX_HPRT(usb->index), cvmx_usbcx_hprt,
 			prtrst, 1);
 
 	/*
@@ -1032,75 +985,15 @@ static int cvmx_usb_enable(struct cvmx_usb_state *usb)
 	mdelay(50);
 
 	/* Program the port reset bit to 0, USBC_HPRT[PRTRST] = 0 */
-	USB_SET_FIELD32(CVMX_USBCX_HPRT(usb->index), union cvmx_usbcx_hprt,
+	USB_SET_FIELD32(CVMX_USBCX_HPRT(usb->index), cvmx_usbcx_hprt,
 			prtrst, 0);
 
-	/* Wait for the USBC_HPRT[PRTENA]. */
-	if (CVMX_WAIT_FOR_FIELD32(CVMX_USBCX_HPRT(usb->index),
-				union cvmx_usbcx_hprt, prtena, ==, 1, 100000))
-		return -ETIMEDOUT;
-
 	/*
 	 * Read the port speed field to get the enumerated speed,
 	 * USBC_HPRT[PRTSPD].
 	 */
-	usb->usbcx_hprt.u32 = __cvmx_usb_read_csr32(usb,
-			CVMX_USBCX_HPRT(usb->index));
-	usbcx_ghwcfg3.u32 = __cvmx_usb_read_csr32(usb,
-			CVMX_USBCX_GHWCFG3(usb->index));
-
-	/*
-	 * 13. Program the USBC_GRXFSIZ register to select the size of the
-	 *     receive FIFO (25%).
-	 */
-	USB_SET_FIELD32(CVMX_USBCX_GRXFSIZ(usb->index),
-			union cvmx_usbcx_grxfsiz, rxfdep,
-			usbcx_ghwcfg3.s.dfifodepth / 4);
-	/*
-	 * 14. Program the USBC_GNPTXFSIZ register to select the size and the
-	 *     start address of the non- periodic transmit FIFO for nonperiodic
-	 *     transactions (50%).
-	 */
-	{
-		union cvmx_usbcx_gnptxfsiz siz;
-
-		siz.u32 = __cvmx_usb_read_csr32(usb,
-				CVMX_USBCX_GNPTXFSIZ(usb->index));
-		siz.s.nptxfdep = usbcx_ghwcfg3.s.dfifodepth / 2;
-		siz.s.nptxfstaddr = usbcx_ghwcfg3.s.dfifodepth / 4;
-		__cvmx_usb_write_csr32(usb, CVMX_USBCX_GNPTXFSIZ(usb->index),
-				       siz.u32);
-	}
-	/*
-	 * 15. Program the USBC_HPTXFSIZ register to select the size and start
-	 *     address of the periodic transmit FIFO for periodic transactions
-	 *     (25%).
-	 */
-	{
-		union cvmx_usbcx_hptxfsiz siz;
-
-		siz.u32 = __cvmx_usb_read_csr32(usb,
-				CVMX_USBCX_HPTXFSIZ(usb->index));
-		siz.s.ptxfsize = usbcx_ghwcfg3.s.dfifodepth / 4;
-		siz.s.ptxfstaddr = 3 * usbcx_ghwcfg3.s.dfifodepth / 4;
-		__cvmx_usb_write_csr32(usb, CVMX_USBCX_HPTXFSIZ(usb->index),
-				       siz.u32);
-	}
-	/* Flush all FIFOs */
-	USB_SET_FIELD32(CVMX_USBCX_GRSTCTL(usb->index),
-			union cvmx_usbcx_grstctl, txfnum, 0x10);
-	USB_SET_FIELD32(CVMX_USBCX_GRSTCTL(usb->index),
-			union cvmx_usbcx_grstctl, txfflsh, 1);
-	CVMX_WAIT_FOR_FIELD32(CVMX_USBCX_GRSTCTL(usb->index),
-			      union cvmx_usbcx_grstctl,
-			      txfflsh, ==, 0, 100);
-	USB_SET_FIELD32(CVMX_USBCX_GRSTCTL(usb->index),
-			union cvmx_usbcx_grstctl, rxfflsh, 1);
-	CVMX_WAIT_FOR_FIELD32(CVMX_USBCX_GRSTCTL(usb->index),
-			      union cvmx_usbcx_grstctl,
-			      rxfflsh, ==, 0, 100);
-
-	return 0;
+	usb->usbcx_hprt.u32 = cvmx_usb_read_csr32(usb,
+						  CVMX_USBCX_HPRT(usb->index));
 }
 
 
@@ -1117,7 +1010,7 @@ static int cvmx_usb_enable(struct cvmx_usb_state *usb)
 static int cvmx_usb_disable(struct cvmx_usb_state *usb)
 {
 	/* Disable the port */
-	USB_SET_FIELD32(CVMX_USBCX_HPRT(usb->index), union cvmx_usbcx_hprt,
+	USB_SET_FIELD32(CVMX_USBCX_HPRT(usb->index), cvmx_usbcx_hprt,
 			prtena, 1);
 	return 0;
 }
@@ -1142,8 +1035,7 @@ static struct cvmx_usb_port_status cvmx_usb_get_status(
 
 	memset(&result, 0, sizeof(result));
 
-	usbc_hprt.u32 = __cvmx_usb_read_csr32(usb,
-			CVMX_USBCX_HPRT(usb->index));
+	usbc_hprt.u32 = cvmx_usb_read_csr32(usb, CVMX_USBCX_HPRT(usb->index));
 	result.port_enabled = usbc_hprt.s.prtena;
 	result.port_over_current = usbc_hprt.s.prtovrcurract;
 	result.port_powered = usbc_hprt.s.prtpwr;
@@ -1223,41 +1115,13 @@ static struct cvmx_usb_pipe *cvmx_usb_open_pipe(struct cvmx_usb_state *usb,
 {
 	struct cvmx_usb_pipe *pipe;
 
-	if (unlikely((device_addr < 0) || (device_addr > MAX_USB_ADDRESS)))
-		return NULL;
-	if (unlikely((endpoint_num < 0) || (endpoint_num > MAX_USB_ENDPOINT)))
-		return NULL;
-	if (unlikely(device_speed > CVMX_USB_SPEED_LOW))
-		return NULL;
-	if (unlikely((max_packet <= 0) || (max_packet > 1024)))
-		return NULL;
-	if (unlikely(transfer_type > CVMX_USB_TRANSFER_INTERRUPT))
-		return NULL;
-	if (unlikely((transfer_dir != CVMX_USB_DIRECTION_OUT) &&
-		(transfer_dir != CVMX_USB_DIRECTION_IN)))
-		return NULL;
-	if (unlikely(interval < 0))
-		return NULL;
-	if (unlikely((transfer_type == CVMX_USB_TRANSFER_CONTROL) && interval))
-		return NULL;
-	if (unlikely(multi_count < 0))
-		return NULL;
-	if (unlikely((device_speed != CVMX_USB_SPEED_HIGH) &&
-		(multi_count != 0)))
-		return NULL;
-	if (unlikely((hub_device_addr < 0) ||
-		(hub_device_addr > MAX_USB_ADDRESS)))
-		return NULL;
-	if (unlikely((hub_port < 0) || (hub_port > MAX_USB_HUB_PORT)))
-		return NULL;
-
 	pipe = kzalloc(sizeof(*pipe), GFP_ATOMIC);
 	if (!pipe)
 		return NULL;
 	if ((device_speed == CVMX_USB_SPEED_HIGH) &&
 		(transfer_dir == CVMX_USB_DIRECTION_OUT) &&
 		(transfer_type == CVMX_USB_TRANSFER_BULK))
-		pipe->flags |= __CVMX_USB_PIPE_FLAGS_NEED_PING;
+		pipe->flags |= CVMX_USB_PIPE_FLAGS_NEED_PING;
 	pipe->device_addr = device_addr;
 	pipe->endpoint_num = endpoint_num;
 	pipe->device_speed = device_speed;
@@ -1272,7 +1136,7 @@ static struct cvmx_usb_pipe *cvmx_usb_open_pipe(struct cvmx_usb_state *usb,
 	 */
 	if (!interval)
 		interval = 1;
-	if (__cvmx_usb_pipe_needs_split(usb, pipe)) {
+	if (cvmx_usb_pipe_needs_split(usb, pipe)) {
 		pipe->interval = interval*8;
 		/* Force start splits to be schedule on uFrame 0 */
 		pipe->next_tx_frame = ((usb->frame_number+7)&~7) +
@@ -1304,7 +1168,7 @@ static struct cvmx_usb_pipe *cvmx_usb_open_pipe(struct cvmx_usb_state *usb,
  *
  * @usb:	USB device state populated by cvmx_usb_initialize().
  */
-static void __cvmx_usb_poll_rx_fifo(struct cvmx_usb_state *usb)
+static void cvmx_usb_poll_rx_fifo(struct cvmx_usb_state *usb)
 {
 	union cvmx_usbcx_grxstsph rx_status;
 	int channel;
@@ -1312,8 +1176,8 @@ static void __cvmx_usb_poll_rx_fifo(struct cvmx_usb_state *usb)
 	uint64_t address;
 	uint32_t *ptr;
 
-	rx_status.u32 = __cvmx_usb_read_csr32(usb,
-			CVMX_USBCX_GRXSTSPH(usb->index));
+	rx_status.u32 = cvmx_usb_read_csr32(usb,
+					    CVMX_USBCX_GRXSTSPH(usb->index));
 	/* Only read data if IN data is there */
 	if (rx_status.s.pktsts != 2)
 		return;
@@ -1327,18 +1191,17 @@ static void __cvmx_usb_poll_rx_fifo(struct cvmx_usb_state *usb)
 		return;
 
 	/* Get where the DMA engine would have written this data */
-	address = __cvmx_usb_read_csr64(usb,
-			CVMX_USBNX_DMA0_INB_CHN0(usb->index) + channel*8);
+	address = cvmx_read64_uint64(CVMX_USBNX_DMA0_INB_CHN0(usb->index) +
+				     channel * 8);
 
 	ptr = cvmx_phys_to_ptr(address);
-	__cvmx_usb_write_csr64(usb,
-			       CVMX_USBNX_DMA0_INB_CHN0(usb->index) + channel*8,
-			       address + bytes);
+	cvmx_write64_uint64(CVMX_USBNX_DMA0_INB_CHN0(usb->index) + channel * 8,
+			    address + bytes);
 
 	/* Loop writing the FIFO data for this packet into memory */
 	while (bytes > 0) {
-		*ptr++ = __cvmx_usb_read_csr32(usb,
-				USB_FIFO_ADDRESS(channel, usb->index));
+		*ptr++ = cvmx_usb_read_csr32(usb,
+					USB_FIFO_ADDRESS(channel, usb->index));
 		bytes -= 4;
 	}
 	CVMX_SYNCW;
@@ -1356,8 +1219,8 @@ static void __cvmx_usb_poll_rx_fifo(struct cvmx_usb_state *usb)
  * Returns: Non zero if the hardware fifo was too small and needs
  *	    to be serviced again.
  */
-static int __cvmx_usb_fill_tx_hw(struct cvmx_usb_state *usb,
-				 struct cvmx_usb_tx_fifo *fifo, int available)
+static int cvmx_usb_fill_tx_hw(struct cvmx_usb_state *usb,
+			       struct cvmx_usb_tx_fifo *fifo, int available)
 {
 	/*
 	 * We're done either when there isn't anymore space or the software FIFO
@@ -1412,38 +1275,34 @@ static int __cvmx_usb_fill_tx_hw(struct cvmx_usb_state *usb,
  *
  * @usb:	USB device state populated by cvmx_usb_initialize().
  */
-static void __cvmx_usb_poll_tx_fifo(struct cvmx_usb_state *usb)
+static void cvmx_usb_poll_tx_fifo(struct cvmx_usb_state *usb)
 {
 	if (usb->periodic.head != usb->periodic.tail) {
 		union cvmx_usbcx_hptxsts tx_status;
 
-		tx_status.u32 = __cvmx_usb_read_csr32(usb,
-				CVMX_USBCX_HPTXSTS(usb->index));
-		if (__cvmx_usb_fill_tx_hw(usb, &usb->periodic,
-					  tx_status.s.ptxfspcavail))
+		tx_status.u32 = cvmx_usb_read_csr32(usb,
+					CVMX_USBCX_HPTXSTS(usb->index));
+		if (cvmx_usb_fill_tx_hw(usb, &usb->periodic,
+					tx_status.s.ptxfspcavail))
 			USB_SET_FIELD32(CVMX_USBCX_GINTMSK(usb->index),
-					union cvmx_usbcx_gintmsk,
-					ptxfempmsk, 1);
+					cvmx_usbcx_gintmsk, ptxfempmsk, 1);
 		else
 			USB_SET_FIELD32(CVMX_USBCX_GINTMSK(usb->index),
-					union cvmx_usbcx_gintmsk,
-					ptxfempmsk, 0);
+					cvmx_usbcx_gintmsk, ptxfempmsk, 0);
 	}
 
 	if (usb->nonperiodic.head != usb->nonperiodic.tail) {
 		union cvmx_usbcx_gnptxsts tx_status;
 
-		tx_status.u32 = __cvmx_usb_read_csr32(usb,
-				CVMX_USBCX_GNPTXSTS(usb->index));
-		if (__cvmx_usb_fill_tx_hw(usb, &usb->nonperiodic,
-					  tx_status.s.nptxfspcavail))
+		tx_status.u32 = cvmx_usb_read_csr32(usb,
+					CVMX_USBCX_GNPTXSTS(usb->index));
+		if (cvmx_usb_fill_tx_hw(usb, &usb->nonperiodic,
+					tx_status.s.nptxfspcavail))
 			USB_SET_FIELD32(CVMX_USBCX_GINTMSK(usb->index),
-					union cvmx_usbcx_gintmsk,
-					nptxfempmsk, 1);
+					cvmx_usbcx_gintmsk, nptxfempmsk, 1);
 		else
 			USB_SET_FIELD32(CVMX_USBCX_GINTMSK(usb->index),
-					union cvmx_usbcx_gintmsk,
-					nptxfempmsk, 0);
+					cvmx_usbcx_gintmsk, nptxfempmsk, 0);
 	}
 }
 
@@ -1454,7 +1313,7 @@ static void __cvmx_usb_poll_tx_fifo(struct cvmx_usb_state *usb)
  * @usb:	  USB device state populated by cvmx_usb_initialize().
  * @channel:	  Channel number to get packet from
  */
-static void __cvmx_usb_fill_tx_fifo(struct cvmx_usb_state *usb, int channel)
+static void cvmx_usb_fill_tx_fifo(struct cvmx_usb_state *usb, int channel)
 {
 	union cvmx_usbcx_hccharx hcchar;
 	union cvmx_usbcx_hcspltx usbc_hcsplt;
@@ -1462,14 +1321,14 @@ static void __cvmx_usb_fill_tx_fifo(struct cvmx_usb_state *usb, int channel)
 	struct cvmx_usb_tx_fifo *fifo;
 
 	/* We only need to fill data on outbound channels */
-	hcchar.u32 = __cvmx_usb_read_csr32(usb,
+	hcchar.u32 = cvmx_usb_read_csr32(usb,
 			CVMX_USBCX_HCCHARX(channel, usb->index));
 	if (hcchar.s.epdir != CVMX_USB_DIRECTION_OUT)
 		return;
 
 	/* OUT Splits only have data on the start and not the complete */
-	usbc_hcsplt.u32 = __cvmx_usb_read_csr32(usb,
-			CVMX_USBCX_HCSPLTX(channel, usb->index));
+	usbc_hcsplt.u32 = cvmx_usb_read_csr32(usb,
+				CVMX_USBCX_HCSPLTX(channel, usb->index));
 	if (usbc_hcsplt.s.spltena && usbc_hcsplt.s.compsplt)
 		return;
 
@@ -1477,8 +1336,8 @@ static void __cvmx_usb_fill_tx_fifo(struct cvmx_usb_state *usb, int channel)
 	 * Find out how many bytes we need to fill and convert it into 32bit
 	 * words.
 	 */
-	usbc_hctsiz.u32 = __cvmx_usb_read_csr32(usb,
-			CVMX_USBCX_HCTSIZX(channel, usb->index));
+	usbc_hctsiz.u32 = cvmx_usb_read_csr32(usb,
+				CVMX_USBCX_HCTSIZX(channel, usb->index));
 	if (!usbc_hctsiz.s.xfersize)
 		return;
 
@@ -1489,28 +1348,28 @@ static void __cvmx_usb_fill_tx_fifo(struct cvmx_usb_state *usb, int channel)
 		fifo = &usb->nonperiodic;
 
 	fifo->entry[fifo->head].channel = channel;
-	fifo->entry[fifo->head].address = __cvmx_usb_read_csr64(usb,
-			CVMX_USBNX_DMA0_OUTB_CHN0(usb->index) + channel*8);
+	fifo->entry[fifo->head].address =
+		cvmx_read64_uint64(CVMX_USBNX_DMA0_OUTB_CHN0(usb->index) +
+				   channel * 8);
 	fifo->entry[fifo->head].size = (usbc_hctsiz.s.xfersize+3)>>2;
 	fifo->head++;
 	if (fifo->head > MAX_CHANNELS)
 		fifo->head = 0;
 
-	__cvmx_usb_poll_tx_fifo(usb);
+	cvmx_usb_poll_tx_fifo(usb);
 }
 
 /**
  * Perform channel specific setup for Control transactions. All
- * the generic stuff will already have been done in
- * __cvmx_usb_start_channel()
+ * the generic stuff will already have been done in cvmx_usb_start_channel().
  *
  * @usb:	  USB device state populated by cvmx_usb_initialize().
  * @channel:	  Channel to setup
  * @pipe:	  Pipe for control transaction
  */
-static void __cvmx_usb_start_channel_control(struct cvmx_usb_state *usb,
-					     int channel,
-					     struct cvmx_usb_pipe *pipe)
+static void cvmx_usb_start_channel_control(struct cvmx_usb_state *usb,
+					   int channel,
+					   struct cvmx_usb_pipe *pipe)
 {
 	struct octeon_hcd *priv = cvmx_usb_to_octeon(usb);
 	struct usb_hcd *hcd = octeon_to_hcd(priv);
@@ -1525,8 +1384,8 @@ static void __cvmx_usb_start_channel_control(struct cvmx_usb_state *usb,
 	int packets_to_transfer;
 	union cvmx_usbcx_hctsizx usbc_hctsiz;
 
-	usbc_hctsiz.u32 = __cvmx_usb_read_csr32(usb,
-			CVMX_USBCX_HCTSIZX(channel, usb->index));
+	usbc_hctsiz.u32 = cvmx_usb_read_csr32(usb,
+				CVMX_USBCX_HCTSIZX(channel, usb->index));
 
 	switch (transaction->stage) {
 	case CVMX_USB_STAGE_NON_CONTROL:
@@ -1538,72 +1397,72 @@ static void __cvmx_usb_start_channel_control(struct cvmx_usb_state *usb,
 		bytes_to_transfer = sizeof(*header);
 		/* All Control operations start with a setup going OUT */
 		USB_SET_FIELD32(CVMX_USBCX_HCCHARX(channel, usb->index),
-				union cvmx_usbcx_hccharx, epdir,
+				cvmx_usbcx_hccharx, epdir,
 				CVMX_USB_DIRECTION_OUT);
 		/*
 		 * Setup send the control header instead of the buffer data. The
 		 * buffer data will be used in the next stage
 		 */
-		__cvmx_usb_write_csr64(usb,
-			CVMX_USBNX_DMA0_OUTB_CHN0(usb->index) + channel*8,
-			transaction->control_header);
+		cvmx_write64_uint64(CVMX_USBNX_DMA0_OUTB_CHN0(usb->index) +
+					channel * 8,
+				    transaction->control_header);
 		break;
 	case CVMX_USB_STAGE_SETUP_SPLIT_COMPLETE:
 		usbc_hctsiz.s.pid = 3; /* Setup */
 		bytes_to_transfer = 0;
 		/* All Control operations start with a setup going OUT */
 		USB_SET_FIELD32(CVMX_USBCX_HCCHARX(channel, usb->index),
-				union cvmx_usbcx_hccharx, epdir,
+				cvmx_usbcx_hccharx, epdir,
 				CVMX_USB_DIRECTION_OUT);
 
 		USB_SET_FIELD32(CVMX_USBCX_HCSPLTX(channel, usb->index),
-				union cvmx_usbcx_hcspltx, compsplt, 1);
+				cvmx_usbcx_hcspltx, compsplt, 1);
 		break;
 	case CVMX_USB_STAGE_DATA:
-		usbc_hctsiz.s.pid = __cvmx_usb_get_data_pid(pipe);
-		if (__cvmx_usb_pipe_needs_split(usb, pipe)) {
+		usbc_hctsiz.s.pid = cvmx_usb_get_data_pid(pipe);
+		if (cvmx_usb_pipe_needs_split(usb, pipe)) {
 			if (header->bRequestType & USB_DIR_IN)
 				bytes_to_transfer = 0;
 			else if (bytes_to_transfer > pipe->max_packet)
 				bytes_to_transfer = pipe->max_packet;
 		}
 		USB_SET_FIELD32(CVMX_USBCX_HCCHARX(channel, usb->index),
-				union cvmx_usbcx_hccharx, epdir,
+				cvmx_usbcx_hccharx, epdir,
 				((header->bRequestType & USB_DIR_IN) ?
 					CVMX_USB_DIRECTION_IN :
 					CVMX_USB_DIRECTION_OUT));
 		break;
 	case CVMX_USB_STAGE_DATA_SPLIT_COMPLETE:
-		usbc_hctsiz.s.pid = __cvmx_usb_get_data_pid(pipe);
+		usbc_hctsiz.s.pid = cvmx_usb_get_data_pid(pipe);
 		if (!(header->bRequestType & USB_DIR_IN))
 			bytes_to_transfer = 0;
 		USB_SET_FIELD32(CVMX_USBCX_HCCHARX(channel, usb->index),
-				union cvmx_usbcx_hccharx, epdir,
+				cvmx_usbcx_hccharx, epdir,
 				((header->bRequestType & USB_DIR_IN) ?
 					CVMX_USB_DIRECTION_IN :
 					CVMX_USB_DIRECTION_OUT));
 		USB_SET_FIELD32(CVMX_USBCX_HCSPLTX(channel, usb->index),
-				union cvmx_usbcx_hcspltx, compsplt, 1);
+				cvmx_usbcx_hcspltx, compsplt, 1);
 		break;
 	case CVMX_USB_STAGE_STATUS:
-		usbc_hctsiz.s.pid = __cvmx_usb_get_data_pid(pipe);
+		usbc_hctsiz.s.pid = cvmx_usb_get_data_pid(pipe);
 		bytes_to_transfer = 0;
 		USB_SET_FIELD32(CVMX_USBCX_HCCHARX(channel, usb->index),
-				union cvmx_usbcx_hccharx, epdir,
+				cvmx_usbcx_hccharx, epdir,
 				((header->bRequestType & USB_DIR_IN) ?
 					CVMX_USB_DIRECTION_OUT :
 					CVMX_USB_DIRECTION_IN));
 		break;
 	case CVMX_USB_STAGE_STATUS_SPLIT_COMPLETE:
-		usbc_hctsiz.s.pid = __cvmx_usb_get_data_pid(pipe);
+		usbc_hctsiz.s.pid = cvmx_usb_get_data_pid(pipe);
 		bytes_to_transfer = 0;
 		USB_SET_FIELD32(CVMX_USBCX_HCCHARX(channel, usb->index),
-				union cvmx_usbcx_hccharx, epdir,
+				cvmx_usbcx_hccharx, epdir,
 				((header->bRequestType & USB_DIR_IN) ?
 					CVMX_USB_DIRECTION_OUT :
 					CVMX_USB_DIRECTION_IN));
 		USB_SET_FIELD32(CVMX_USBCX_HCSPLTX(channel, usb->index),
-				union cvmx_usbcx_hcspltx, compsplt, 1);
+				cvmx_usbcx_hcspltx, compsplt, 1);
 		break;
 	}
 
@@ -1646,8 +1505,8 @@ static void __cvmx_usb_start_channel_control(struct cvmx_usb_state *usb,
 	usbc_hctsiz.s.xfersize = bytes_to_transfer;
 	usbc_hctsiz.s.pktcnt = packets_to_transfer;
 
-	__cvmx_usb_write_csr32(usb, CVMX_USBCX_HCTSIZX(channel, usb->index),
-			       usbc_hctsiz.u32);
+	cvmx_usb_write_csr32(usb, CVMX_USBCX_HCTSIZX(channel, usb->index),
+			     usbc_hctsiz.u32);
 }
 
 
@@ -1658,9 +1517,8 @@ static void __cvmx_usb_start_channel_control(struct cvmx_usb_state *usb,
  * @channel:	  Channel to setup
  * @pipe:	  Pipe to start
  */
-static void __cvmx_usb_start_channel(struct cvmx_usb_state *usb,
-				     int channel,
-				     struct cvmx_usb_pipe *pipe)
+static void cvmx_usb_start_channel(struct cvmx_usb_state *usb, int channel,
+				   struct cvmx_usb_pipe *pipe)
 {
 	struct cvmx_usb_transaction *transaction =
 		list_first_entry(&pipe->transactions, typeof(*transaction),
@@ -1672,7 +1530,7 @@ static void __cvmx_usb_start_channel(struct cvmx_usb_state *usb,
 	/* Attach the channel to the pipe */
 	usb->pipe_for_channel[channel] = pipe;
 	pipe->channel = channel;
-	pipe->flags |= __CVMX_USB_PIPE_FLAGS_SCHEDULED;
+	pipe->flags |= CVMX_USB_PIPE_FLAGS_SCHEDULED;
 
 	/* Mark this channel as in use */
 	usb->idle_hardware_channels &= ~(1<<channel);
@@ -1684,12 +1542,12 @@ static void __cvmx_usb_start_channel(struct cvmx_usb_state *usb,
 		union cvmx_usbcx_haintmsk usbc_haintmsk;
 
 		/* Clear all channel status bits */
-		usbc_hcint.u32 = __cvmx_usb_read_csr32(usb,
-				CVMX_USBCX_HCINTX(channel, usb->index));
+		usbc_hcint.u32 = cvmx_usb_read_csr32(usb,
+					CVMX_USBCX_HCINTX(channel, usb->index));
 
-		__cvmx_usb_write_csr32(usb,
-				       CVMX_USBCX_HCINTX(channel, usb->index),
-				       usbc_hcint.u32);
+		cvmx_usb_write_csr32(usb,
+				     CVMX_USBCX_HCINTX(channel, usb->index),
+				     usbc_hcint.u32);
 
 		usbc_hcintmsk.u32 = 0;
 		usbc_hcintmsk.s.chhltdmsk = 1;
@@ -1702,7 +1560,7 @@ static void __cvmx_usb_start_channel(struct cvmx_usb_state *usb,
 			usbc_hcintmsk.s.frmovrunmsk = 1;
 			usbc_hcintmsk.s.bblerrmsk = 1;
 			usbc_hcintmsk.s.xacterrmsk = 1;
-			if (__cvmx_usb_pipe_needs_split(usb, pipe)) {
+			if (cvmx_usb_pipe_needs_split(usb, pipe)) {
 				/*
 				 * Splits don't generate xfercompl, so we need
 				 * ACK and NYET.
@@ -1714,21 +1572,21 @@ static void __cvmx_usb_start_channel(struct cvmx_usb_state *usb,
 			usbc_hcintmsk.s.stallmsk = 1;
 			usbc_hcintmsk.s.xfercomplmsk = 1;
 		}
-		__cvmx_usb_write_csr32(usb,
+		cvmx_usb_write_csr32(usb,
 				CVMX_USBCX_HCINTMSKX(channel, usb->index),
 				usbc_hcintmsk.u32);
 
 		/* Enable the channel interrupt to propagate */
-		usbc_haintmsk.u32 = __cvmx_usb_read_csr32(usb,
+		usbc_haintmsk.u32 = cvmx_usb_read_csr32(usb,
 					CVMX_USBCX_HAINTMSK(usb->index));
 		usbc_haintmsk.s.haintmsk |= 1<<channel;
-		__cvmx_usb_write_csr32(usb,
-					CVMX_USBCX_HAINTMSK(usb->index),
-					usbc_haintmsk.u32);
+		cvmx_usb_write_csr32(usb, CVMX_USBCX_HAINTMSK(usb->index),
+				     usbc_haintmsk.u32);
 	}
 
-	/* Setup the locations the DMA engines use  */
+	/* Setup the location the DMA engine uses. */
 	{
+		uint64_t reg;
 		uint64_t dma_address = transaction->buffer +
 					transaction->actual_bytes;
 
@@ -1737,13 +1595,11 @@ static void __cvmx_usb_start_channel(struct cvmx_usb_state *usb,
 					transaction->iso_packets[0].offset +
 					transaction->actual_bytes;
 
-		__cvmx_usb_write_csr64(usb,
-			CVMX_USBNX_DMA0_OUTB_CHN0(usb->index) + channel*8,
-			dma_address);
-
-		__cvmx_usb_write_csr64(usb,
-			CVMX_USBNX_DMA0_INB_CHN0(usb->index) + channel*8,
-			dma_address);
+		if (pipe->transfer_dir == CVMX_USB_DIRECTION_OUT)
+			reg = CVMX_USBNX_DMA0_OUTB_CHN0(usb->index);
+		else
+			reg = CVMX_USBNX_DMA0_INB_CHN0(usb->index);
+		cvmx_write64_uint64(reg + channel * 8, dma_address);
 	}
 
 	/* Setup both the size of the transfer and the SPLIT characteristics */
@@ -1767,7 +1623,7 @@ static void __cvmx_usb_start_channel(struct cvmx_usb_state *usb,
 		 * We need to do split transactions when we are talking to non
 		 * high speed devices that are behind a high speed hub
 		 */
-		if (__cvmx_usb_pipe_needs_split(usb, pipe)) {
+		if (cvmx_usb_pipe_needs_split(usb, pipe)) {
 			/*
 			 * On the start split phase (stage is even) record the
 			 * frame number we will need to send the split complete.
@@ -1898,18 +1754,19 @@ static void __cvmx_usb_start_channel(struct cvmx_usb_state *usb,
 		usbc_hctsiz.s.pktcnt = packets_to_transfer;
 
 		/* Update the DATA0/DATA1 toggle */
-		usbc_hctsiz.s.pid = __cvmx_usb_get_data_pid(pipe);
+		usbc_hctsiz.s.pid = cvmx_usb_get_data_pid(pipe);
 		/*
 		 * High speed pipes may need a hardware ping before they start
 		 */
-		if (pipe->flags & __CVMX_USB_PIPE_FLAGS_NEED_PING)
+		if (pipe->flags & CVMX_USB_PIPE_FLAGS_NEED_PING)
 			usbc_hctsiz.s.dopng = 1;
 
-		__cvmx_usb_write_csr32(usb,
-				       CVMX_USBCX_HCSPLTX(channel, usb->index),
-				       usbc_hcsplt.u32);
-		__cvmx_usb_write_csr32(usb, CVMX_USBCX_HCTSIZX(channel,
-					usb->index), usbc_hctsiz.u32);
+		cvmx_usb_write_csr32(usb,
+				     CVMX_USBCX_HCSPLTX(channel, usb->index),
+				     usbc_hcsplt.u32);
+		cvmx_usb_write_csr32(usb,
+				     CVMX_USBCX_HCTSIZX(channel, usb->index),
+				     usbc_hctsiz.u32);
 	}
 
 	/* Setup the Host Channel Characteristics Register */
@@ -1928,7 +1785,7 @@ static void __cvmx_usb_start_channel(struct cvmx_usb_state *usb,
 		 * immediate retries of failure. These retries happen too
 		 * quickly, so we disable these entirely for splits
 		 */
-		if (__cvmx_usb_pipe_needs_split(usb, pipe))
+		if (cvmx_usb_pipe_needs_split(usb, pipe))
 			usbc_hcchar.s.ec = 1;
 		else if (pipe->multi_count < 1)
 			usbc_hcchar.s.ec = 1;
@@ -1945,21 +1802,21 @@ static void __cvmx_usb_start_channel(struct cvmx_usb_state *usb,
 		usbc_hcchar.s.epdir = pipe->transfer_dir;
 		usbc_hcchar.s.epnum = pipe->endpoint_num;
 		usbc_hcchar.s.mps = pipe->max_packet;
-		__cvmx_usb_write_csr32(usb,
-				       CVMX_USBCX_HCCHARX(channel, usb->index),
-				       usbc_hcchar.u32);
+		cvmx_usb_write_csr32(usb,
+				     CVMX_USBCX_HCCHARX(channel, usb->index),
+				     usbc_hcchar.u32);
 	}
 
 	/* Do transaction type specific fixups as needed */
 	switch (transaction->type) {
 	case CVMX_USB_TRANSFER_CONTROL:
-		__cvmx_usb_start_channel_control(usb, channel, pipe);
+		cvmx_usb_start_channel_control(usb, channel, pipe);
 		break;
 	case CVMX_USB_TRANSFER_BULK:
 	case CVMX_USB_TRANSFER_INTERRUPT:
 		break;
 	case CVMX_USB_TRANSFER_ISOCHRONOUS:
-		if (!__cvmx_usb_pipe_needs_split(usb, pipe)) {
+		if (!cvmx_usb_pipe_needs_split(usb, pipe)) {
 			/*
 			 * ISO transactions require different PIDs depending on
 			 * direction and how many packets are needed
@@ -1969,32 +1826,30 @@ static void __cvmx_usb_start_channel(struct cvmx_usb_state *usb,
 					USB_SET_FIELD32(
 						CVMX_USBCX_HCTSIZX(channel,
 								   usb->index),
-						union cvmx_usbcx_hctsizx,
-						pid, 0);
+						cvmx_usbcx_hctsizx, pid, 0);
 				else /* Need MDATA */
 					USB_SET_FIELD32(
 						CVMX_USBCX_HCTSIZX(channel,
 								   usb->index),
-						union cvmx_usbcx_hctsizx,
-						pid, 3);
+						cvmx_usbcx_hctsizx, pid, 3);
 			}
 		}
 		break;
 	}
 	{
 		union cvmx_usbcx_hctsizx usbc_hctsiz = {.u32 =
-			__cvmx_usb_read_csr32(usb,
+			cvmx_usb_read_csr32(usb,
 				CVMX_USBCX_HCTSIZX(channel, usb->index))};
 		transaction->xfersize = usbc_hctsiz.s.xfersize;
 		transaction->pktcnt = usbc_hctsiz.s.pktcnt;
 	}
 	/* Remeber when we start a split transaction */
-	if (__cvmx_usb_pipe_needs_split(usb, pipe))
+	if (cvmx_usb_pipe_needs_split(usb, pipe))
 		usb->active_split = transaction;
 	USB_SET_FIELD32(CVMX_USBCX_HCCHARX(channel, usb->index),
-			union cvmx_usbcx_hccharx, chena, 1);
+			cvmx_usbcx_hccharx, chena, 1);
 	if (usb->init_flags & CVMX_USB_INITIALIZE_FLAGS_NO_DMA)
-		__cvmx_usb_fill_tx_fifo(usb, channel);
+		cvmx_usb_fill_tx_fifo(usb, channel);
 }
 
 
@@ -2007,7 +1862,7 @@ static void __cvmx_usb_start_channel(struct cvmx_usb_state *usb,
  *
  * Returns: Pipe or NULL if none are ready
  */
-static struct cvmx_usb_pipe *__cvmx_usb_find_ready_pipe(
+static struct cvmx_usb_pipe *cvmx_usb_find_ready_pipe(
 		struct cvmx_usb_state *usb,
 		struct list_head *list,
 		uint64_t current_frame)
@@ -2018,7 +1873,7 @@ static struct cvmx_usb_pipe *__cvmx_usb_find_ready_pipe(
 		struct cvmx_usb_transaction *t =
 			list_first_entry(&pipe->transactions, typeof(*t),
 					 node);
-		if (!(pipe->flags & __CVMX_USB_PIPE_FLAGS_SCHEDULED) && t &&
+		if (!(pipe->flags & CVMX_USB_PIPE_FLAGS_SCHEDULED) && t &&
 			(pipe->next_tx_frame <= current_frame) &&
 			((pipe->split_sc_frame == -1) ||
 			 ((((int)current_frame - (int)pipe->split_sc_frame)
@@ -2039,7 +1894,7 @@ static struct cvmx_usb_pipe *__cvmx_usb_find_ready_pipe(
  * @usb:	 USB device state populated by cvmx_usb_initialize().
  * @is_sof:	 True if this schedule was called on a SOF interrupt.
  */
-static void __cvmx_usb_schedule(struct cvmx_usb_state *usb, int is_sof)
+static void cvmx_usb_schedule(struct cvmx_usb_state *usb, int is_sof)
 {
 	int channel;
 	struct cvmx_usb_pipe *pipe;
@@ -2052,12 +1907,12 @@ static void __cvmx_usb_schedule(struct cvmx_usb_state *usb, int is_sof)
 		 * at the end of a frame and cause an overrun.
 		 */
 		union cvmx_usbcx_hfnum hfnum = {
-			.u32 = __cvmx_usb_read_csr32(usb,
+			.u32 = cvmx_usb_read_csr32(usb,
 						CVMX_USBCX_HFNUM(usb->index))
 		};
 
 		union cvmx_usbcx_hfir hfir = {
-			.u32 = __cvmx_usb_read_csr32(usb,
+			.u32 = cvmx_usb_read_csr32(usb,
 						CVMX_USBCX_HFIR(usb->index))
 		};
 
@@ -2079,23 +1934,23 @@ static void __cvmx_usb_schedule(struct cvmx_usb_state *usb, int is_sof)
 			 * way we are sure that the periodic data is sent in the
 			 * beginning of the frame
 			 */
-			pipe = __cvmx_usb_find_ready_pipe(usb,
+			pipe = cvmx_usb_find_ready_pipe(usb,
 					usb->active_pipes +
 					CVMX_USB_TRANSFER_ISOCHRONOUS,
 					usb->frame_number);
 			if (likely(!pipe))
-				pipe = __cvmx_usb_find_ready_pipe(usb,
+				pipe = cvmx_usb_find_ready_pipe(usb,
 						usb->active_pipes +
 						CVMX_USB_TRANSFER_INTERRUPT,
 						usb->frame_number);
 		}
 		if (likely(!pipe)) {
-			pipe = __cvmx_usb_find_ready_pipe(usb,
+			pipe = cvmx_usb_find_ready_pipe(usb,
 					usb->active_pipes +
 					CVMX_USB_TRANSFER_CONTROL,
 					usb->frame_number);
 			if (likely(!pipe))
-				pipe = __cvmx_usb_find_ready_pipe(usb,
+				pipe = cvmx_usb_find_ready_pipe(usb,
 						usb->active_pipes +
 						CVMX_USB_TRANSFER_BULK,
 						usb->frame_number);
@@ -2103,7 +1958,7 @@ static void __cvmx_usb_schedule(struct cvmx_usb_state *usb, int is_sof)
 		if (!pipe)
 			break;
 
-		__cvmx_usb_start_channel(usb, channel, pipe);
+		cvmx_usb_start_channel(usb, channel, pipe);
 	}
 
 done:
@@ -2122,7 +1977,7 @@ done:
 		}
 	}
 	USB_SET_FIELD32(CVMX_USBCX_GINTMSK(usb->index),
-			union cvmx_usbcx_gintmsk, sofmsk, need_sof);
+			cvmx_usbcx_gintmsk, sofmsk, need_sof);
 }
 
 static void octeon_usb_urb_complete_callback(struct cvmx_usb_state *usb,
@@ -2226,11 +2081,10 @@ static void octeon_usb_urb_complete_callback(struct cvmx_usb_state *usb,
  * @complete_code:
  *		 Completion code
  */
-static void __cvmx_usb_perform_complete(
-				struct cvmx_usb_state *usb,
-				struct cvmx_usb_pipe *pipe,
-				struct cvmx_usb_transaction *transaction,
-				enum cvmx_usb_complete complete_code)
+static void cvmx_usb_perform_complete(struct cvmx_usb_state *usb,
+				      struct cvmx_usb_pipe *pipe,
+				      struct cvmx_usb_transaction *transaction,
+				      enum cvmx_usb_complete complete_code)
 {
 	/* If this was a split then clear our split in progress marker */
 	if (usb->active_split == transaction)
@@ -2296,7 +2150,7 @@ static void __cvmx_usb_perform_complete(
  *
  * Returns: Transaction or NULL on failure.
  */
-static struct cvmx_usb_transaction *__cvmx_usb_submit_transaction(
+static struct cvmx_usb_transaction *cvmx_usb_submit_transaction(
 				struct cvmx_usb_state *usb,
 				struct cvmx_usb_pipe *pipe,
 				enum cvmx_usb_transfer type,
@@ -2342,7 +2196,7 @@ static struct cvmx_usb_transaction *__cvmx_usb_submit_transaction(
 		 * We may need to schedule the pipe if this was the head of the
 		 * pipe.
 		 */
-		__cvmx_usb_schedule(usb, 0);
+		cvmx_usb_schedule(usb, 0);
 	}
 
 	return transaction;
@@ -2363,14 +2217,14 @@ static struct cvmx_usb_transaction *cvmx_usb_submit_bulk(
 						struct cvmx_usb_pipe *pipe,
 						struct urb *urb)
 {
-	return __cvmx_usb_submit_transaction(usb, pipe, CVMX_USB_TRANSFER_BULK,
-					     urb->transfer_dma,
-					     urb->transfer_buffer_length,
-					     0, /* control_header */
-					     0, /* iso_start_frame */
-					     0, /* iso_number_packets */
-					     NULL, /* iso_packets */
-					     urb);
+	return cvmx_usb_submit_transaction(usb, pipe, CVMX_USB_TRANSFER_BULK,
+					   urb->transfer_dma,
+					   urb->transfer_buffer_length,
+					   0, /* control_header */
+					   0, /* iso_start_frame */
+					   0, /* iso_number_packets */
+					   NULL, /* iso_packets */
+					   urb);
 }
 
 
@@ -2388,15 +2242,15 @@ static struct cvmx_usb_transaction *cvmx_usb_submit_interrupt(
 						struct cvmx_usb_pipe *pipe,
 						struct urb *urb)
 {
-	return __cvmx_usb_submit_transaction(usb, pipe,
-					     CVMX_USB_TRANSFER_INTERRUPT,
-					     urb->transfer_dma,
-					     urb->transfer_buffer_length,
-					     0, /* control_header */
-					     0, /* iso_start_frame */
-					     0, /* iso_number_packets */
-					     NULL, /* iso_packets */
-					     urb);
+	return cvmx_usb_submit_transaction(usb, pipe,
+					   CVMX_USB_TRANSFER_INTERRUPT,
+					   urb->transfer_dma,
+					   urb->transfer_buffer_length,
+					   0, /* control_header */
+					   0, /* iso_start_frame */
+					   0, /* iso_number_packets */
+					   NULL, /* iso_packets */
+					   urb);
 }
 
 
@@ -2421,14 +2275,14 @@ static struct cvmx_usb_transaction *cvmx_usb_submit_control(
 	if ((header->bRequestType & USB_DIR_IN) == 0)
 		buffer_length = le16_to_cpu(header->wLength);
 
-	return __cvmx_usb_submit_transaction(usb, pipe,
-					     CVMX_USB_TRANSFER_CONTROL,
-					     urb->transfer_dma, buffer_length,
-					     control_header,
-					     0, /* iso_start_frame */
-					     0, /* iso_number_packets */
-					     NULL, /* iso_packets */
-					     urb);
+	return cvmx_usb_submit_transaction(usb, pipe,
+					   CVMX_USB_TRANSFER_CONTROL,
+					   urb->transfer_dma, buffer_length,
+					   control_header,
+					   0, /* iso_start_frame */
+					   0, /* iso_number_packets */
+					   NULL, /* iso_packets */
+					   urb);
 }
 
 
@@ -2449,14 +2303,14 @@ static struct cvmx_usb_transaction *cvmx_usb_submit_isochronous(
 	struct cvmx_usb_iso_packet *packets;
 
 	packets = (struct cvmx_usb_iso_packet *) urb->setup_packet;
-	return __cvmx_usb_submit_transaction(usb, pipe,
-					     CVMX_USB_TRANSFER_ISOCHRONOUS,
-					     urb->transfer_dma,
-					     urb->transfer_buffer_length,
-					     0, /* control_header */
-					     urb->start_frame,
-					     urb->number_of_packets,
-					     packets, urb);
+	return cvmx_usb_submit_transaction(usb, pipe,
+					   CVMX_USB_TRANSFER_ISOCHRONOUS,
+					   urb->transfer_dma,
+					   urb->transfer_buffer_length,
+					   0, /* control_header */
+					   urb->start_frame,
+					   urb->number_of_packets,
+					   packets, urb);
 }
 
 
@@ -2482,15 +2336,15 @@ static int cvmx_usb_cancel(struct cvmx_usb_state *usb,
 	 * treat it special
 	 */
 	if (list_first_entry(&pipe->transactions, typeof(*transaction), node) ==
-	    transaction && (pipe->flags & __CVMX_USB_PIPE_FLAGS_SCHEDULED)) {
+	    transaction && (pipe->flags & CVMX_USB_PIPE_FLAGS_SCHEDULED)) {
 		union cvmx_usbcx_hccharx usbc_hcchar;
 
 		usb->pipe_for_channel[pipe->channel] = NULL;
-		pipe->flags &= ~__CVMX_USB_PIPE_FLAGS_SCHEDULED;
+		pipe->flags &= ~CVMX_USB_PIPE_FLAGS_SCHEDULED;
 
 		CVMX_SYNCW;
 
-		usbc_hcchar.u32 = __cvmx_usb_read_csr32(usb,
+		usbc_hcchar.u32 = cvmx_usb_read_csr32(usb,
 				CVMX_USBCX_HCCHARX(pipe->channel, usb->index));
 		/*
 		 * If the channel isn't enabled then the transaction already
@@ -2498,14 +2352,14 @@ static int cvmx_usb_cancel(struct cvmx_usb_state *usb,
 		 */
 		if (usbc_hcchar.s.chena) {
 			usbc_hcchar.s.chdis = 1;
-			__cvmx_usb_write_csr32(usb,
+			cvmx_usb_write_csr32(usb,
 					CVMX_USBCX_HCCHARX(pipe->channel,
 						usb->index),
 					usbc_hcchar.u32);
 		}
 	}
-	__cvmx_usb_perform_complete(usb, pipe, transaction,
-				    CVMX_USB_COMPLETE_CANCEL);
+	cvmx_usb_perform_complete(usb, pipe, transaction,
+				  CVMX_USB_COMPLETE_CANCEL);
 	return 0;
 }
 
@@ -2570,8 +2424,7 @@ static int cvmx_usb_get_frame_number(struct cvmx_usb_state *usb)
 	int frame_number;
 	union cvmx_usbcx_hfnum usbc_hfnum;
 
-	usbc_hfnum.u32 = __cvmx_usb_read_csr32(usb,
-			CVMX_USBCX_HFNUM(usb->index));
+	usbc_hfnum.u32 = cvmx_usb_read_csr32(usb, CVMX_USBCX_HFNUM(usb->index));
 	frame_number = usbc_hfnum.s.frnum;
 
 	return frame_number;
@@ -2586,7 +2439,7 @@ static int cvmx_usb_get_frame_number(struct cvmx_usb_state *usb)
  *
  * Returns: Zero on success
  */
-static int __cvmx_usb_poll_channel(struct cvmx_usb_state *usb, int channel)
+static int cvmx_usb_poll_channel(struct cvmx_usb_state *usb, int channel)
 {
 	struct octeon_hcd *priv = cvmx_usb_to_octeon(usb);
 	struct usb_hcd *hcd = octeon_to_hcd(priv);
@@ -2602,11 +2455,11 @@ static int __cvmx_usb_poll_channel(struct cvmx_usb_state *usb, int channel)
 	int buffer_space_left;
 
 	/* Read the interrupt status bits for the channel */
-	usbc_hcint.u32 = __cvmx_usb_read_csr32(usb,
-			CVMX_USBCX_HCINTX(channel, usb->index));
+	usbc_hcint.u32 = cvmx_usb_read_csr32(usb,
+				CVMX_USBCX_HCINTX(channel, usb->index));
 
 	if (usb->init_flags & CVMX_USB_INITIALIZE_FLAGS_NO_DMA) {
-		usbc_hcchar.u32 = __cvmx_usb_read_csr32(usb,
+		usbc_hcchar.u32 = cvmx_usb_read_csr32(usb,
 				CVMX_USBCX_HCCHARX(channel, usb->index));
 
 		if (usbc_hcchar.s.chena && usbc_hcchar.s.chdis) {
@@ -2615,7 +2468,7 @@ static int __cvmx_usb_poll_channel(struct cvmx_usb_state *usb, int channel)
 			 * interrupt IN transfers to get stuck until we do a
 			 * write of HCCHARX without changing things
 			 */
-			__cvmx_usb_write_csr32(usb,
+			cvmx_usb_write_csr32(usb,
 					CVMX_USBCX_HCCHARX(channel,
 							   usb->index),
 					usbc_hcchar.u32);
@@ -2632,12 +2485,12 @@ static int __cvmx_usb_poll_channel(struct cvmx_usb_state *usb, int channel)
 				/* Disable all interrupts except CHHLTD */
 				hcintmsk.u32 = 0;
 				hcintmsk.s.chhltdmsk = 1;
-				__cvmx_usb_write_csr32(usb,
+				cvmx_usb_write_csr32(usb,
 						CVMX_USBCX_HCINTMSKX(channel,
 							usb->index),
 						hcintmsk.u32);
 				usbc_hcchar.s.chdis = 1;
-				__cvmx_usb_write_csr32(usb,
+				cvmx_usb_write_csr32(usb,
 						CVMX_USBCX_HCCHARX(channel,
 							usb->index),
 						usbc_hcchar.u32);
@@ -2663,9 +2516,7 @@ static int __cvmx_usb_poll_channel(struct cvmx_usb_state *usb, int channel)
 	}
 
 	/* Disable the channel interrupts now that it is done */
-	__cvmx_usb_write_csr32(usb,
-				CVMX_USBCX_HCINTMSKX(channel, usb->index),
-				0);
+	cvmx_usb_write_csr32(usb, CVMX_USBCX_HCINTMSKX(channel, usb->index), 0);
 	usb->idle_hardware_channels |= (1<<channel);
 
 	/* Make sure this channel is tied to a valid pipe */
@@ -2683,15 +2534,15 @@ static int __cvmx_usb_poll_channel(struct cvmx_usb_state *usb, int channel)
 	 * function will figure out which pipe needs to go
 	 */
 	usb->pipe_for_channel[channel] = NULL;
-	pipe->flags &= ~__CVMX_USB_PIPE_FLAGS_SCHEDULED;
+	pipe->flags &= ~CVMX_USB_PIPE_FLAGS_SCHEDULED;
 
 	/*
 	 * Read the channel config info so we can figure out how much data
 	 * transferred
 	 */
-	usbc_hcchar.u32 = __cvmx_usb_read_csr32(usb,
+	usbc_hcchar.u32 = cvmx_usb_read_csr32(usb,
 			CVMX_USBCX_HCCHARX(channel, usb->index));
-	usbc_hctsiz.u32 = __cvmx_usb_read_csr32(usb,
+	usbc_hctsiz.u32 = cvmx_usb_read_csr32(usb,
 			CVMX_USBCX_HCTSIZX(channel, usb->index));
 
 	/*
@@ -2766,7 +2617,18 @@ static int __cvmx_usb_poll_channel(struct cvmx_usb_state *usb, int channel)
 	if ((pipe->device_speed == CVMX_USB_SPEED_HIGH) &&
 		(pipe->transfer_type == CVMX_USB_TRANSFER_BULK) &&
 		(pipe->transfer_dir == CVMX_USB_DIRECTION_OUT))
-		pipe->flags |= __CVMX_USB_PIPE_FLAGS_NEED_PING;
+		pipe->flags |= CVMX_USB_PIPE_FLAGS_NEED_PING;
+
+	if (unlikely(WARN_ON_ONCE(bytes_this_transfer < 0))) {
+		/*
+		 * In some rare cases the DMA engine seems to get stuck and
+		 * keeps substracting same byte count over and over again. In
+		 * such case we just need to fail every transaction.
+		 */
+		cvmx_usb_perform_complete(usb, pipe, transaction,
+					  CVMX_USB_COMPLETE_ERROR);
+		return 0;
+	}
 
 	if (usbc_hcint.s.stall) {
 		/*
@@ -2776,52 +2638,24 @@ static int __cvmx_usb_poll_channel(struct cvmx_usb_state *usb, int channel)
 		 * the actual bytes transferred
 		 */
 		pipe->pid_toggle = 0;
-		__cvmx_usb_perform_complete(usb, pipe, transaction,
-					    CVMX_USB_COMPLETE_STALL);
+		cvmx_usb_perform_complete(usb, pipe, transaction,
+					  CVMX_USB_COMPLETE_STALL);
 	} else if (usbc_hcint.s.xacterr) {
 		/*
-		 * We know at least one packet worked if we get a ACK or NAK.
-		 * Reset the retry counter
+		 * XactErr as a response means the device signaled
+		 * something wrong with the transfer. For example, PID
+		 * toggle errors cause these.
 		 */
-		if (usbc_hcint.s.nak || usbc_hcint.s.ack)
-			transaction->retries = 0;
-		transaction->retries++;
-		if (transaction->retries > MAX_RETRIES) {
-			/*
-			 * XactErr as a response means the device signaled
-			 * something wrong with the transfer. For example, PID
-			 * toggle errors cause these
-			 */
-			__cvmx_usb_perform_complete(usb, pipe, transaction,
-						    CVMX_USB_COMPLETE_XACTERR);
-		} else {
-			/*
-			 * If this was a split then clear our split in progress
-			 * marker
-			 */
-			if (usb->active_split == transaction)
-				usb->active_split = NULL;
-			/*
-			 * Rewind to the beginning of the transaction by anding
-			 * off the split complete bit
-			 */
-			transaction->stage &= ~1;
-			pipe->split_sc_frame = -1;
-			pipe->next_tx_frame += pipe->interval;
-			if (pipe->next_tx_frame < usb->frame_number)
-				pipe->next_tx_frame =
-					usb->frame_number + pipe->interval -
-					(usb->frame_number -
-					 pipe->next_tx_frame) % pipe->interval;
-		}
+		cvmx_usb_perform_complete(usb, pipe, transaction,
+					  CVMX_USB_COMPLETE_XACTERR);
 	} else if (usbc_hcint.s.bblerr) {
 		/* Babble Error (BblErr) */
-		__cvmx_usb_perform_complete(usb, pipe, transaction,
-					    CVMX_USB_COMPLETE_BABBLEERR);
+		cvmx_usb_perform_complete(usb, pipe, transaction,
+					  CVMX_USB_COMPLETE_BABBLEERR);
 	} else if (usbc_hcint.s.datatglerr) {
 		/* Data toggle error */
-		__cvmx_usb_perform_complete(usb, pipe, transaction,
-					    CVMX_USB_COMPLETE_DATATGLERR);
+		cvmx_usb_perform_complete(usb, pipe, transaction,
+					  CVMX_USB_COMPLETE_DATATGLERR);
 	} else if (usbc_hcint.s.nyet) {
 		/*
 		 * NYET as a response is only allowed in three cases: as a
@@ -2829,7 +2663,7 @@ static int __cvmx_usb_poll_channel(struct cvmx_usb_state *usb, int channel)
 		 * as a response to a bulk out. The ping case is handled by
 		 * hardware, so we only have splits and bulk out
 		 */
-		if (!__cvmx_usb_pipe_needs_split(usb, pipe)) {
+		if (!cvmx_usb_pipe_needs_split(usb, pipe)) {
 			transaction->retries = 0;
 			/*
 			 * If there is more data to go then we need to try
@@ -2837,7 +2671,7 @@ static int __cvmx_usb_poll_channel(struct cvmx_usb_state *usb, int channel)
 			 */
 			if ((buffer_space_left == 0) ||
 				(bytes_in_last_packet < pipe->max_packet))
-				__cvmx_usb_perform_complete(usb, pipe,
+				cvmx_usb_perform_complete(usb, pipe,
 						transaction,
 						CVMX_USB_COMPLETE_SUCCESS);
 		} else {
@@ -2869,7 +2703,7 @@ static int __cvmx_usb_poll_channel(struct cvmx_usb_state *usb, int channel)
 		 * Since we got an ACK, we know we don't need to do a ping on
 		 * this pipe
 		 */
-		pipe->flags &= ~__CVMX_USB_PIPE_FLAGS_NEED_PING;
+		pipe->flags &= ~CVMX_USB_PIPE_FLAGS_NEED_PING;
 
 		switch (transaction->type) {
 		case CVMX_USB_TRANSFER_CONTROL:
@@ -2877,12 +2711,12 @@ static int __cvmx_usb_poll_channel(struct cvmx_usb_state *usb, int channel)
 			case CVMX_USB_STAGE_NON_CONTROL:
 			case CVMX_USB_STAGE_NON_CONTROL_SPLIT_COMPLETE:
 				/* This should be impossible */
-				__cvmx_usb_perform_complete(usb, pipe,
+				cvmx_usb_perform_complete(usb, pipe,
 					transaction, CVMX_USB_COMPLETE_ERROR);
 				break;
 			case CVMX_USB_STAGE_SETUP:
 				pipe->pid_toggle = 1;
-				if (__cvmx_usb_pipe_needs_split(usb, pipe))
+				if (cvmx_usb_pipe_needs_split(usb, pipe))
 					transaction->stage =
 						CVMX_USB_STAGE_SETUP_SPLIT_COMPLETE;
 				else {
@@ -2909,7 +2743,7 @@ static int __cvmx_usb_poll_channel(struct cvmx_usb_state *usb, int channel)
 				}
 				break;
 			case CVMX_USB_STAGE_DATA:
-				if (__cvmx_usb_pipe_needs_split(usb, pipe)) {
+				if (cvmx_usb_pipe_needs_split(usb, pipe)) {
 					transaction->stage =
 						CVMX_USB_STAGE_DATA_SPLIT_COMPLETE;
 					/*
@@ -2947,16 +2781,16 @@ static int __cvmx_usb_poll_channel(struct cvmx_usb_state *usb, int channel)
 				}
 				break;
 			case CVMX_USB_STAGE_STATUS:
-				if (__cvmx_usb_pipe_needs_split(usb, pipe))
+				if (cvmx_usb_pipe_needs_split(usb, pipe))
 					transaction->stage =
 						CVMX_USB_STAGE_STATUS_SPLIT_COMPLETE;
 				else
-					__cvmx_usb_perform_complete(usb, pipe,
+					cvmx_usb_perform_complete(usb, pipe,
 						transaction,
 						CVMX_USB_COMPLETE_SUCCESS);
 				break;
 			case CVMX_USB_STAGE_STATUS_SPLIT_COMPLETE:
-				__cvmx_usb_perform_complete(usb, pipe,
+				cvmx_usb_perform_complete(usb, pipe,
 						transaction,
 						CVMX_USB_COMPLETE_SUCCESS);
 				break;
@@ -2970,7 +2804,7 @@ static int __cvmx_usb_poll_channel(struct cvmx_usb_state *usb, int channel)
 			 * For splits we need to continue the transfer if more
 			 * data is needed
 			 */
-			if (__cvmx_usb_pipe_needs_split(usb, pipe)) {
+			if (cvmx_usb_pipe_needs_split(usb, pipe)) {
 				if (transaction->stage ==
 						CVMX_USB_STAGE_NON_CONTROL)
 					transaction->stage =
@@ -2986,7 +2820,7 @@ static int __cvmx_usb_poll_channel(struct cvmx_usb_state *usb, int channel)
 							CVMX_USB_TRANSFER_INTERRUPT)
 							pipe->next_tx_frame +=
 								pipe->interval;
-							__cvmx_usb_perform_complete(
+							cvmx_usb_perform_complete(
 								usb,
 								pipe,
 								transaction,
@@ -3002,7 +2836,7 @@ static int __cvmx_usb_poll_channel(struct cvmx_usb_state *usb, int channel)
 				     CVMX_USB_DIRECTION_OUT) &&
 				    (usbc_hcint.s.nak))
 					pipe->flags |=
-						__CVMX_USB_PIPE_FLAGS_NEED_PING;
+						CVMX_USB_PIPE_FLAGS_NEED_PING;
 				if (!buffer_space_left ||
 					(bytes_in_last_packet <
 					 pipe->max_packet)) {
@@ -3010,15 +2844,14 @@ static int __cvmx_usb_poll_channel(struct cvmx_usb_state *usb, int channel)
 						CVMX_USB_TRANSFER_INTERRUPT)
 						pipe->next_tx_frame +=
 							pipe->interval;
-					__cvmx_usb_perform_complete(usb,
-						pipe,
+					cvmx_usb_perform_complete(usb, pipe,
 						transaction,
 						CVMX_USB_COMPLETE_SUCCESS);
 				}
 			}
 			break;
 		case CVMX_USB_TRANSFER_ISOCHRONOUS:
-			if (__cvmx_usb_pipe_needs_split(usb, pipe)) {
+			if (cvmx_usb_pipe_needs_split(usb, pipe)) {
 				/*
 				 * ISOCHRONOUS OUT splits don't require a
 				 * complete split stage. Instead they use a
@@ -3039,10 +2872,8 @@ static int __cvmx_usb_poll_channel(struct cvmx_usb_state *usb, int channel)
 						(bytes_this_transfer < 188)) {
 						pipe->next_tx_frame +=
 							pipe->interval;
-						__cvmx_usb_perform_complete(
-							usb,
-							pipe,
-							transaction,
+						cvmx_usb_perform_complete(usb,
+							pipe, transaction,
 							CVMX_USB_COMPLETE_SUCCESS);
 					}
 				} else {
@@ -3059,7 +2890,7 @@ static int __cvmx_usb_poll_channel(struct cvmx_usb_state *usb, int channel)
 							 pipe->max_packet)) {
 							pipe->next_tx_frame +=
 								pipe->interval;
-							__cvmx_usb_perform_complete(
+							cvmx_usb_perform_complete(
 								usb,
 								pipe,
 								transaction,
@@ -3071,8 +2902,7 @@ static int __cvmx_usb_poll_channel(struct cvmx_usb_state *usb, int channel)
 				}
 			} else {
 				pipe->next_tx_frame += pipe->interval;
-				__cvmx_usb_perform_complete(usb,
-						pipe,
+				cvmx_usb_perform_complete(usb, pipe,
 						transaction,
 						CVMX_USB_COMPLETE_SUCCESS);
 			}
@@ -3110,7 +2940,7 @@ static int __cvmx_usb_poll_channel(struct cvmx_usb_state *usb, int channel)
 			 * We get channel halted interrupts with no result bits
 			 * sets when the cable is unplugged
 			 */
-			__cvmx_usb_perform_complete(usb, pipe, transaction,
+			cvmx_usb_perform_complete(usb, pipe, transaction,
 					CVMX_USB_COMPLETE_ERROR);
 		}
 	}
@@ -3144,21 +2974,19 @@ static int cvmx_usb_poll(struct cvmx_usb_state *usb)
 	prefetch_range(usb, sizeof(*usb));
 
 	/* Update the frame counter */
-	usbc_hfnum.u32 = __cvmx_usb_read_csr32(usb,
-						CVMX_USBCX_HFNUM(usb->index));
+	usbc_hfnum.u32 = cvmx_usb_read_csr32(usb, CVMX_USBCX_HFNUM(usb->index));
 	if ((usb->frame_number&0x3fff) > usbc_hfnum.s.frnum)
 		usb->frame_number += 0x4000;
 	usb->frame_number &= ~0x3fffull;
 	usb->frame_number |= usbc_hfnum.s.frnum;
 
 	/* Read the pending interrupts */
-	usbc_gintsts.u32 = __cvmx_usb_read_csr32(usb,
-						CVMX_USBCX_GINTSTS(usb->index));
+	usbc_gintsts.u32 = cvmx_usb_read_csr32(usb,
+					       CVMX_USBCX_GINTSTS(usb->index));
 
 	/* Clear the interrupts now that we know about them */
-	__cvmx_usb_write_csr32(usb,
-				CVMX_USBCX_GINTSTS(usb->index),
-				usbc_gintsts.u32);
+	cvmx_usb_write_csr32(usb, CVMX_USBCX_GINTSTS(usb->index),
+			     usbc_gintsts.u32);
 
 	if (usbc_gintsts.s.rxflvl) {
 		/*
@@ -3169,12 +2997,12 @@ static int cvmx_usb_poll(struct cvmx_usb_state *usb)
 		 * In DMA mode this is handled by hardware
 		 */
 		if (usb->init_flags & CVMX_USB_INITIALIZE_FLAGS_NO_DMA)
-			__cvmx_usb_poll_rx_fifo(usb);
+			cvmx_usb_poll_rx_fifo(usb);
 	}
 	if (usbc_gintsts.s.ptxfemp || usbc_gintsts.s.nptxfemp) {
 		/* Fill the Tx FIFOs when not in DMA mode */
 		if (usb->init_flags & CVMX_USB_INITIALIZE_FLAGS_NO_DMA)
-			__cvmx_usb_poll_tx_fifo(usb);
+			cvmx_usb_poll_tx_fifo(usb);
 	}
 	if (usbc_gintsts.s.disconnint || usbc_gintsts.s.prtint) {
 		union cvmx_usbcx_hprt usbc_hprt;
@@ -3194,11 +3022,11 @@ static int cvmx_usb_poll(struct cvmx_usb_state *usb)
 		 */
 		octeon_usb_port_callback(usb);
 		/* Clear the port change bits */
-		usbc_hprt.u32 = __cvmx_usb_read_csr32(usb,
-				CVMX_USBCX_HPRT(usb->index));
+		usbc_hprt.u32 = cvmx_usb_read_csr32(usb,
+					CVMX_USBCX_HPRT(usb->index));
 		usbc_hprt.s.prtena = 0;
-		__cvmx_usb_write_csr32(usb, CVMX_USBCX_HPRT(usb->index),
-				       usbc_hprt.u32);
+		cvmx_usb_write_csr32(usb, CVMX_USBCX_HPRT(usb->index),
+				     usbc_hprt.u32);
 	}
 	if (usbc_gintsts.s.hchint) {
 		/*
@@ -3215,18 +3043,18 @@ static int cvmx_usb_poll(struct cvmx_usb_state *usb)
 		 */
 		union cvmx_usbcx_haint usbc_haint;
 
-		usbc_haint.u32 = __cvmx_usb_read_csr32(usb,
+		usbc_haint.u32 = cvmx_usb_read_csr32(usb,
 					CVMX_USBCX_HAINT(usb->index));
 		while (usbc_haint.u32) {
 			int channel;
 
 			channel = __fls(usbc_haint.u32);
-			__cvmx_usb_poll_channel(usb, channel);
+			cvmx_usb_poll_channel(usb, channel);
 			usbc_haint.u32 ^= 1<<channel;
 		}
 	}
 
-	__cvmx_usb_schedule(usb, usbc_gintsts.s.sof);
+	cvmx_usb_schedule(usb, usbc_gintsts.s.sof);
 
 	return 0;
 }
@@ -3510,6 +3338,7 @@ static int octeon_usb_hub_control(struct usb_hcd *hcd, u16 typeReq, u16 wValue,
 	struct octeon_hcd *priv = hcd_to_octeon(hcd);
 	struct device *dev = hcd->self.controller;
 	struct cvmx_usb_port_status usb_port_status;
+	struct cvmx_usb_state *usb = &priv->usb;
 	int port_status;
 	struct usb_hub_descriptor *desc;
 	unsigned long flags;
@@ -3687,13 +3516,18 @@ static int octeon_usb_hub_control(struct usb_hcd *hcd, u16 typeReq, u16 wValue,
 			return -EINVAL;
 		case USB_PORT_FEAT_POWER:
 			dev_dbg(dev, " POWER\n");
-			return -EINVAL;
+			/*
+			 * Program the port power bit to drive VBUS on the USB.
+			 */
+			spin_lock_irqsave(&priv->lock, flags);
+			USB_SET_FIELD32(CVMX_USBCX_HPRT(usb->index),
+					cvmx_usbcx_hprt, prtpwr, 1);
+			spin_unlock_irqrestore(&priv->lock, flags);
+			return 0;
 		case USB_PORT_FEAT_RESET:
 			dev_dbg(dev, " RESET\n");
 			spin_lock_irqsave(&priv->lock, flags);
-			cvmx_usb_disable(&priv->usb);
-			if (cvmx_usb_enable(&priv->usb))
-				dev_dbg(dev, "Failed to enable the port\n");
+			cvmx_usb_reset_port(&priv->usb);
 			spin_unlock_irqrestore(&priv->lock, flags);
 			return 0;
 		case USB_PORT_FEAT_INDICATOR:
@@ -3741,7 +3575,6 @@ static int octeon_usb_probe(struct platform_device *pdev)
 	struct device *dev = &pdev->dev;
 	struct octeon_hcd *priv;
 	struct usb_hcd *hcd;
-	unsigned long flags;
 	u32 clock_rate = 48000000;
 	bool is_crystal_clock = false;
 	const char *clock_type;
@@ -3838,20 +3671,33 @@ static int octeon_usb_probe(struct platform_device *pdev)
 
 	spin_lock_init(&priv->lock);
 
-	status = cvmx_usb_initialize(&priv->usb, usb_num, initialize_flags);
+	priv->usb.init_flags = initialize_flags;
+
+	/* Initialize the USB state structure */
+	priv->usb.index = usb_num;
+	INIT_LIST_HEAD(&priv->usb.idle_pipes);
+	for (i = 0; i < ARRAY_SIZE(priv->usb.active_pipes); i++)
+		INIT_LIST_HEAD(&priv->usb.active_pipes[i]);
+
+	/* Due to an errata, CN31XX doesn't support DMA */
+	if (OCTEON_IS_MODEL(OCTEON_CN31XX)) {
+		priv->usb.init_flags |= CVMX_USB_INITIALIZE_FLAGS_NO_DMA;
+		/* Only use one channel with non DMA */
+		priv->usb.idle_hardware_channels = 0x1;
+	} else if (OCTEON_IS_MODEL(OCTEON_CN5XXX)) {
+		/* CN5XXX have an errata with channel 3 */
+		priv->usb.idle_hardware_channels = 0xf7;
+	} else {
+		priv->usb.idle_hardware_channels = 0xff;
+	}
+
+	status = cvmx_usb_initialize(dev, &priv->usb);
 	if (status) {
 		dev_dbg(dev, "USB initialization failed with %d\n", status);
 		kfree(hcd);
 		return -1;
 	}
 
-	/* This delay is needed for CN3010, but I don't know why... */
-	mdelay(10);
-
-	spin_lock_irqsave(&priv->lock, flags);
-	cvmx_usb_poll(&priv->usb);
-	spin_unlock_irqrestore(&priv->lock, flags);
-
 	status = usb_add_hcd(hcd, irq, 0);
 	if (status) {
 		dev_dbg(dev, "USB add HCD failed with %d\n", status);
@@ -3885,7 +3731,7 @@ static int octeon_usb_remove(struct platform_device *pdev)
 	return 0;
 }
 
-static struct of_device_id octeon_usb_match[] = {
+static const struct of_device_id octeon_usb_match[] = {
 	{
 		.compatible = "cavium,octeon-5750-usbc",
 	},
diff --git a/drivers/staging/octeon-usb/octeon-hcd.h b/drivers/staging/octeon-usb/octeon-hcd.h
index 42fe4fec7d5d..3e351ab7465a 100644
--- a/drivers/staging/octeon-usb/octeon-hcd.h
+++ b/drivers/staging/octeon-usb/octeon-hcd.h
@@ -47,6 +47,8 @@
 #ifndef __OCTEON_HCD_H__
 #define __OCTEON_HCD_H__
 
+#include <asm/bitfield.h>
+
 #define CVMX_USBCXBASE 0x00016F0010000000ull
 #define CVMX_USBCXREG1(reg, bid) \
 	(CVMX_ADD_IO_SEG(CVMX_USBCXBASE | reg) + \
@@ -143,13 +145,14 @@ union cvmx_usbcx_gahbcfg {
 	 *	* 1'b1: Unmask the interrupt assertion to the application.
 	 */
 	struct cvmx_usbcx_gahbcfg_s {
-		uint32_t reserved_9_31	: 23;
-		uint32_t ptxfemplvl	: 1;
-		uint32_t nptxfemplvl	: 1;
-		uint32_t reserved_6_6	: 1;
-		uint32_t dmaen		: 1;
-		uint32_t hbstlen	: 4;
-		uint32_t glblintrmsk	: 1;
+		__BITFIELD_FIELD(uint32_t reserved_9_31	: 23,
+		__BITFIELD_FIELD(uint32_t ptxfemplvl	: 1,
+		__BITFIELD_FIELD(uint32_t nptxfemplvl	: 1,
+		__BITFIELD_FIELD(uint32_t reserved_6_6	: 1,
+		__BITFIELD_FIELD(uint32_t dmaen		: 1,
+		__BITFIELD_FIELD(uint32_t hbstlen	: 4,
+		__BITFIELD_FIELD(uint32_t glblintrmsk	: 1,
+		;)))))))
 	} s;
 };
 
@@ -209,16 +212,17 @@ union cvmx_usbcx_ghwcfg3 {
 	 *	* Others: Reserved
 	 */
 	struct cvmx_usbcx_ghwcfg3_s {
-		uint32_t dfifodepth				: 16;
-		uint32_t reserved_13_15				: 3;
-		uint32_t ahbphysync				: 1;
-		uint32_t rsttype				: 1;
-		uint32_t optfeature				: 1;
-		uint32_t vendor_control_interface_support	: 1;
-		uint32_t i2c_selection				: 1;
-		uint32_t otgen					: 1;
-		uint32_t pktsizewidth				: 3;
-		uint32_t xfersizewidth				: 4;
+		__BITFIELD_FIELD(uint32_t dfifodepth			: 16,
+		__BITFIELD_FIELD(uint32_t reserved_13_15		: 3,
+		__BITFIELD_FIELD(uint32_t ahbphysync			: 1,
+		__BITFIELD_FIELD(uint32_t rsttype			: 1,
+		__BITFIELD_FIELD(uint32_t optfeature			: 1,
+		__BITFIELD_FIELD(uint32_t vendor_control_interface_support : 1,
+		__BITFIELD_FIELD(uint32_t i2c_selection			: 1,
+		__BITFIELD_FIELD(uint32_t otgen				: 1,
+		__BITFIELD_FIELD(uint32_t pktsizewidth			: 3,
+		__BITFIELD_FIELD(uint32_t xfersizewidth			: 4,
+		;))))))))))
 	} s;
 };
 
@@ -275,38 +279,39 @@ union cvmx_usbcx_gintmsk {
 	 * @modemismsk: Mode Mismatch Interrupt Mask (ModeMisMsk)
 	 */
 	struct cvmx_usbcx_gintmsk_s {
-		uint32_t wkupintmsk		: 1;
-		uint32_t sessreqintmsk		: 1;
-		uint32_t disconnintmsk		: 1;
-		uint32_t conidstschngmsk	: 1;
-		uint32_t reserved_27_27		: 1;
-		uint32_t ptxfempmsk		: 1;
-		uint32_t hchintmsk		: 1;
-		uint32_t prtintmsk		: 1;
-		uint32_t reserved_23_23		: 1;
-		uint32_t fetsuspmsk		: 1;
-		uint32_t incomplpmsk		: 1;
-		uint32_t incompisoinmsk		: 1;
-		uint32_t oepintmsk		: 1;
-		uint32_t inepintmsk		: 1;
-		uint32_t epmismsk		: 1;
-		uint32_t reserved_16_16		: 1;
-		uint32_t eopfmsk		: 1;
-		uint32_t isooutdropmsk		: 1;
-		uint32_t enumdonemsk		: 1;
-		uint32_t usbrstmsk		: 1;
-		uint32_t usbsuspmsk		: 1;
-		uint32_t erlysuspmsk		: 1;
-		uint32_t i2cint			: 1;
-		uint32_t ulpickintmsk		: 1;
-		uint32_t goutnakeffmsk		: 1;
-		uint32_t ginnakeffmsk		: 1;
-		uint32_t nptxfempmsk		: 1;
-		uint32_t rxflvlmsk		: 1;
-		uint32_t sofmsk			: 1;
-		uint32_t otgintmsk		: 1;
-		uint32_t modemismsk		: 1;
-		uint32_t reserved_0_0		: 1;
+		__BITFIELD_FIELD(uint32_t wkupintmsk		: 1,
+		__BITFIELD_FIELD(uint32_t sessreqintmsk		: 1,
+		__BITFIELD_FIELD(uint32_t disconnintmsk		: 1,
+		__BITFIELD_FIELD(uint32_t conidstschngmsk	: 1,
+		__BITFIELD_FIELD(uint32_t reserved_27_27	: 1,
+		__BITFIELD_FIELD(uint32_t ptxfempmsk		: 1,
+		__BITFIELD_FIELD(uint32_t hchintmsk		: 1,
+		__BITFIELD_FIELD(uint32_t prtintmsk		: 1,
+		__BITFIELD_FIELD(uint32_t reserved_23_23	: 1,
+		__BITFIELD_FIELD(uint32_t fetsuspmsk		: 1,
+		__BITFIELD_FIELD(uint32_t incomplpmsk		: 1,
+		__BITFIELD_FIELD(uint32_t incompisoinmsk	: 1,
+		__BITFIELD_FIELD(uint32_t oepintmsk		: 1,
+		__BITFIELD_FIELD(uint32_t inepintmsk		: 1,
+		__BITFIELD_FIELD(uint32_t epmismsk		: 1,
+		__BITFIELD_FIELD(uint32_t reserved_16_16	: 1,
+		__BITFIELD_FIELD(uint32_t eopfmsk		: 1,
+		__BITFIELD_FIELD(uint32_t isooutdropmsk		: 1,
+		__BITFIELD_FIELD(uint32_t enumdonemsk		: 1,
+		__BITFIELD_FIELD(uint32_t usbrstmsk		: 1,
+		__BITFIELD_FIELD(uint32_t usbsuspmsk		: 1,
+		__BITFIELD_FIELD(uint32_t erlysuspmsk		: 1,
+		__BITFIELD_FIELD(uint32_t i2cint		: 1,
+		__BITFIELD_FIELD(uint32_t ulpickintmsk		: 1,
+		__BITFIELD_FIELD(uint32_t goutnakeffmsk		: 1,
+		__BITFIELD_FIELD(uint32_t ginnakeffmsk		: 1,
+		__BITFIELD_FIELD(uint32_t nptxfempmsk		: 1,
+		__BITFIELD_FIELD(uint32_t rxflvlmsk		: 1,
+		__BITFIELD_FIELD(uint32_t sofmsk		: 1,
+		__BITFIELD_FIELD(uint32_t otgintmsk		: 1,
+		__BITFIELD_FIELD(uint32_t modemismsk		: 1,
+		__BITFIELD_FIELD(uint32_t reserved_0_0		: 1,
+		;))))))))))))))))))))))))))))))))
 	} s;
 };
 
@@ -504,38 +509,39 @@ union cvmx_usbcx_gintsts {
 	 *	* 1'b1: Host mode
 	 */
 	struct cvmx_usbcx_gintsts_s {
-		uint32_t wkupint	: 1;
-		uint32_t sessreqint	: 1;
-		uint32_t disconnint	: 1;
-		uint32_t conidstschng	: 1;
-		uint32_t reserved_27_27	: 1;
-		uint32_t ptxfemp	: 1;
-		uint32_t hchint		: 1;
-		uint32_t prtint		: 1;
-		uint32_t reserved_23_23	: 1;
-		uint32_t fetsusp	: 1;
-		uint32_t incomplp	: 1;
-		uint32_t incompisoin	: 1;
-		uint32_t oepint		: 1;
-		uint32_t iepint		: 1;
-		uint32_t epmis		: 1;
-		uint32_t reserved_16_16	: 1;
-		uint32_t eopf		: 1;
-		uint32_t isooutdrop	: 1;
-		uint32_t enumdone	: 1;
-		uint32_t usbrst		: 1;
-		uint32_t usbsusp	: 1;
-		uint32_t erlysusp	: 1;
-		uint32_t i2cint		: 1;
-		uint32_t ulpickint	: 1;
-		uint32_t goutnakeff	: 1;
-		uint32_t ginnakeff	: 1;
-		uint32_t nptxfemp	: 1;
-		uint32_t rxflvl		: 1;
-		uint32_t sof		: 1;
-		uint32_t otgint		: 1;
-		uint32_t modemis	: 1;
-		uint32_t curmod		: 1;
+		__BITFIELD_FIELD(uint32_t wkupint		: 1,
+		__BITFIELD_FIELD(uint32_t sessreqint		: 1,
+		__BITFIELD_FIELD(uint32_t disconnint		: 1,
+		__BITFIELD_FIELD(uint32_t conidstschng		: 1,
+		__BITFIELD_FIELD(uint32_t reserved_27_27	: 1,
+		__BITFIELD_FIELD(uint32_t ptxfemp		: 1,
+		__BITFIELD_FIELD(uint32_t hchint		: 1,
+		__BITFIELD_FIELD(uint32_t prtint		: 1,
+		__BITFIELD_FIELD(uint32_t reserved_23_23	: 1,
+		__BITFIELD_FIELD(uint32_t fetsusp		: 1,
+		__BITFIELD_FIELD(uint32_t incomplp		: 1,
+		__BITFIELD_FIELD(uint32_t incompisoin		: 1,
+		__BITFIELD_FIELD(uint32_t oepint		: 1,
+		__BITFIELD_FIELD(uint32_t iepint		: 1,
+		__BITFIELD_FIELD(uint32_t epmis			: 1,
+		__BITFIELD_FIELD(uint32_t reserved_16_16	: 1,
+		__BITFIELD_FIELD(uint32_t eopf			: 1,
+		__BITFIELD_FIELD(uint32_t isooutdrop		: 1,
+		__BITFIELD_FIELD(uint32_t enumdone		: 1,
+		__BITFIELD_FIELD(uint32_t usbrst		: 1,
+		__BITFIELD_FIELD(uint32_t usbsusp		: 1,
+		__BITFIELD_FIELD(uint32_t erlysusp		: 1,
+		__BITFIELD_FIELD(uint32_t i2cint		: 1,
+		__BITFIELD_FIELD(uint32_t ulpickint		: 1,
+		__BITFIELD_FIELD(uint32_t goutnakeff		: 1,
+		__BITFIELD_FIELD(uint32_t ginnakeff		: 1,
+		__BITFIELD_FIELD(uint32_t nptxfemp		: 1,
+		__BITFIELD_FIELD(uint32_t rxflvl		: 1,
+		__BITFIELD_FIELD(uint32_t sof			: 1,
+		__BITFIELD_FIELD(uint32_t otgint		: 1,
+		__BITFIELD_FIELD(uint32_t modemis		: 1,
+		__BITFIELD_FIELD(uint32_t curmod		: 1,
+		;))))))))))))))))))))))))))))))))
 	} s;
 };
 
@@ -560,8 +566,9 @@ union cvmx_usbcx_gnptxfsiz {
 	 *	Transmit FIFO RAM.
 	 */
 	struct cvmx_usbcx_gnptxfsiz_s {
-		uint32_t nptxfdep	: 16;
-		uint32_t nptxfstaddr	: 16;
+		__BITFIELD_FIELD(uint32_t nptxfdep	: 16,
+		__BITFIELD_FIELD(uint32_t nptxfstaddr	: 16,
+		;))
 	} s;
 };
 
@@ -610,10 +617,11 @@ union cvmx_usbcx_gnptxsts {
 	 *	* Others: Reserved
 	 */
 	struct cvmx_usbcx_gnptxsts_s {
-		uint32_t reserved_31_31	: 1;
-		uint32_t nptxqtop	: 7;
-		uint32_t nptxqspcavail	: 8;
-		uint32_t nptxfspcavail	: 16;
+		__BITFIELD_FIELD(uint32_t reserved_31_31	: 1,
+		__BITFIELD_FIELD(uint32_t nptxqtop		: 7,
+		__BITFIELD_FIELD(uint32_t nptxqspcavail		: 8,
+		__BITFIELD_FIELD(uint32_t nptxfspcavail		: 16,
+		;))))
 	} s;
 };
 
@@ -731,16 +739,17 @@ union cvmx_usbcx_grstctl {
 	 *	selected, the PHY domain has to be reset for proper operation.
 	 */
 	struct cvmx_usbcx_grstctl_s {
-		uint32_t ahbidle	: 1;
-		uint32_t dmareq		: 1;
-		uint32_t reserved_11_29	: 19;
-		uint32_t txfnum		: 5;
-		uint32_t txfflsh	: 1;
-		uint32_t rxfflsh	: 1;
-		uint32_t intknqflsh	: 1;
-		uint32_t frmcntrrst	: 1;
-		uint32_t hsftrst	: 1;
-		uint32_t csftrst	: 1;
+		__BITFIELD_FIELD(uint32_t ahbidle		: 1,
+		__BITFIELD_FIELD(uint32_t dmareq		: 1,
+		__BITFIELD_FIELD(uint32_t reserved_11_29	: 19,
+		__BITFIELD_FIELD(uint32_t txfnum		: 5,
+		__BITFIELD_FIELD(uint32_t txfflsh		: 1,
+		__BITFIELD_FIELD(uint32_t rxfflsh		: 1,
+		__BITFIELD_FIELD(uint32_t intknqflsh		: 1,
+		__BITFIELD_FIELD(uint32_t frmcntrrst		: 1,
+		__BITFIELD_FIELD(uint32_t hsftrst		: 1,
+		__BITFIELD_FIELD(uint32_t csftrst		: 1,
+		;))))))))))
 	} s;
 };
 
@@ -762,8 +771,9 @@ union cvmx_usbcx_grxfsiz {
 	 *	* Maximum value is 32768
 	 */
 	struct cvmx_usbcx_grxfsiz_s {
-		uint32_t reserved_16_31	: 16;
-		uint32_t rxfdep		: 16;
+		__BITFIELD_FIELD(uint32_t reserved_16_31	: 16,
+		__BITFIELD_FIELD(uint32_t rxfdep		: 16,
+		;))
 	} s;
 };
 
@@ -804,11 +814,12 @@ union cvmx_usbcx_grxstsph {
 	 *	packet belongs.
 	 */
 	struct cvmx_usbcx_grxstsph_s {
-		uint32_t reserved_21_31	: 11;
-		uint32_t pktsts		: 4;
-		uint32_t dpid		: 2;
-		uint32_t bcnt		: 11;
-		uint32_t chnum		: 4;
+		__BITFIELD_FIELD(uint32_t reserved_21_31	: 11,
+		__BITFIELD_FIELD(uint32_t pktsts		: 4,
+		__BITFIELD_FIELD(uint32_t dpid			: 2,
+		__BITFIELD_FIELD(uint32_t bcnt			: 11,
+		__BITFIELD_FIELD(uint32_t chnum			: 4,
+		;)))))
 	} s;
 };
 
@@ -884,19 +895,20 @@ union cvmx_usbcx_gusbcfg {
 	 *	* One 48-MHz PHY clock = 0.25 bit times
 	 */
 	struct cvmx_usbcx_gusbcfg_s {
-		uint32_t reserved_17_31	: 15;
-		uint32_t otgi2csel	: 1;
-		uint32_t phylpwrclksel	: 1;
-		uint32_t reserved_14_14	: 1;
-		uint32_t usbtrdtim	: 4;
-		uint32_t hnpcap		: 1;
-		uint32_t srpcap		: 1;
-		uint32_t ddrsel		: 1;
-		uint32_t physel		: 1;
-		uint32_t fsintf		: 1;
-		uint32_t ulpi_utmi_sel	: 1;
-		uint32_t phyif		: 1;
-		uint32_t toutcal	: 3;
+		__BITFIELD_FIELD(uint32_t reserved_17_31	: 15,
+		__BITFIELD_FIELD(uint32_t otgi2csel		: 1,
+		__BITFIELD_FIELD(uint32_t phylpwrclksel		: 1,
+		__BITFIELD_FIELD(uint32_t reserved_14_14	: 1,
+		__BITFIELD_FIELD(uint32_t usbtrdtim		: 4,
+		__BITFIELD_FIELD(uint32_t hnpcap		: 1,
+		__BITFIELD_FIELD(uint32_t srpcap		: 1,
+		__BITFIELD_FIELD(uint32_t ddrsel		: 1,
+		__BITFIELD_FIELD(uint32_t physel		: 1,
+		__BITFIELD_FIELD(uint32_t fsintf		: 1,
+		__BITFIELD_FIELD(uint32_t ulpi_utmi_sel		: 1,
+		__BITFIELD_FIELD(uint32_t phyif			: 1,
+		__BITFIELD_FIELD(uint32_t toutcal		: 3,
+		;)))))))))))))
 	} s;
 };
 
@@ -920,8 +932,9 @@ union cvmx_usbcx_haint {
 	 *	One bit per channel: Bit 0 for Channel 0, bit 15 for Channel 15
 	 */
 	struct cvmx_usbcx_haint_s {
-		uint32_t reserved_16_31	: 16;
-		uint32_t haint		: 16;
+		__BITFIELD_FIELD(uint32_t reserved_16_31	: 16,
+		__BITFIELD_FIELD(uint32_t haint			: 16,
+		;))
 	} s;
 };
 
@@ -944,8 +957,9 @@ union cvmx_usbcx_haintmsk {
 	 *	One bit per channel: Bit 0 for channel 0, bit 15 for channel 15
 	 */
 	struct cvmx_usbcx_haintmsk_s {
-		uint32_t reserved_16_31	: 16;
-		uint32_t haintmsk	: 16;
+		__BITFIELD_FIELD(uint32_t reserved_16_31	: 16,
+		__BITFIELD_FIELD(uint32_t haintmsk		: 16,
+		;))
 	} s;
 };
 
@@ -1014,17 +1028,18 @@ union cvmx_usbcx_hccharx {
 	 *	Indicates the maximum packet size of the associated endpoint.
 	 */
 	struct cvmx_usbcx_hccharx_s {
-		uint32_t chena		: 1;
-		uint32_t chdis		: 1;
-		uint32_t oddfrm		: 1;
-		uint32_t devaddr	: 7;
-		uint32_t ec		: 2;
-		uint32_t eptype		: 2;
-		uint32_t lspddev	: 1;
-		uint32_t reserved_16_16	: 1;
-		uint32_t epdir		: 1;
-		uint32_t epnum		: 4;
-		uint32_t mps		: 11;
+		__BITFIELD_FIELD(uint32_t chena			: 1,
+		__BITFIELD_FIELD(uint32_t chdis			: 1,
+		__BITFIELD_FIELD(uint32_t oddfrm		: 1,
+		__BITFIELD_FIELD(uint32_t devaddr		: 7,
+		__BITFIELD_FIELD(uint32_t ec			: 2,
+		__BITFIELD_FIELD(uint32_t eptype		: 2,
+		__BITFIELD_FIELD(uint32_t lspddev		: 1,
+		__BITFIELD_FIELD(uint32_t reserved_16_16	: 1,
+		__BITFIELD_FIELD(uint32_t epdir			: 1,
+		__BITFIELD_FIELD(uint32_t epnum			: 4,
+		__BITFIELD_FIELD(uint32_t mps			: 11,
+		;)))))))))))
 	} s;
 };
 
@@ -1069,9 +1084,10 @@ union cvmx_usbcx_hcfg {
 	 *	* 2'b11: Reserved
 	 */
 	struct cvmx_usbcx_hcfg_s {
-		uint32_t reserved_3_31	: 29;
-		uint32_t fslssupp	: 1;
-		uint32_t fslspclksel	: 2;
+		__BITFIELD_FIELD(uint32_t reserved_3_31	: 29,
+		__BITFIELD_FIELD(uint32_t fslssupp	: 1,
+		__BITFIELD_FIELD(uint32_t fslspclksel	: 2,
+		;)))
 	} s;
 };
 
@@ -1110,18 +1126,19 @@ union cvmx_usbcx_hcintx {
 	 *	Transfer completed normally without any errors.
 	 */
 	struct cvmx_usbcx_hcintx_s {
-		uint32_t reserved_11_31	: 21;
-		uint32_t datatglerr	: 1;
-		uint32_t frmovrun	: 1;
-		uint32_t bblerr		: 1;
-		uint32_t xacterr	: 1;
-		uint32_t nyet		: 1;
-		uint32_t ack		: 1;
-		uint32_t nak		: 1;
-		uint32_t stall		: 1;
-		uint32_t ahberr		: 1;
-		uint32_t chhltd		: 1;
-		uint32_t xfercompl	: 1;
+		__BITFIELD_FIELD(uint32_t reserved_11_31	: 21,
+		__BITFIELD_FIELD(uint32_t datatglerr		: 1,
+		__BITFIELD_FIELD(uint32_t frmovrun		: 1,
+		__BITFIELD_FIELD(uint32_t bblerr		: 1,
+		__BITFIELD_FIELD(uint32_t xacterr		: 1,
+		__BITFIELD_FIELD(uint32_t nyet			: 1,
+		__BITFIELD_FIELD(uint32_t ack			: 1,
+		__BITFIELD_FIELD(uint32_t nak			: 1,
+		__BITFIELD_FIELD(uint32_t stall			: 1,
+		__BITFIELD_FIELD(uint32_t ahberr		: 1,
+		__BITFIELD_FIELD(uint32_t chhltd		: 1,
+		__BITFIELD_FIELD(uint32_t xfercompl		: 1,
+		;))))))))))))
 	} s;
 };
 
@@ -1151,18 +1168,19 @@ union cvmx_usbcx_hcintmskx {
 	 * @xfercomplmsk: Transfer Completed Mask (XferComplMsk)
 	 */
 	struct cvmx_usbcx_hcintmskx_s {
-		uint32_t reserved_11_31	: 21;
-		uint32_t datatglerrmsk	: 1;
-		uint32_t frmovrunmsk	: 1;
-		uint32_t bblerrmsk	: 1;
-		uint32_t xacterrmsk	: 1;
-		uint32_t nyetmsk	: 1;
-		uint32_t ackmsk		: 1;
-		uint32_t nakmsk		: 1;
-		uint32_t stallmsk	: 1;
-		uint32_t ahberrmsk	: 1;
-		uint32_t chhltdmsk	: 1;
-		uint32_t xfercomplmsk	: 1;
+		__BITFIELD_FIELD(uint32_t reserved_11_31		: 21,
+		__BITFIELD_FIELD(uint32_t datatglerrmsk			: 1,
+		__BITFIELD_FIELD(uint32_t frmovrunmsk			: 1,
+		__BITFIELD_FIELD(uint32_t bblerrmsk			: 1,
+		__BITFIELD_FIELD(uint32_t xacterrmsk			: 1,
+		__BITFIELD_FIELD(uint32_t nyetmsk			: 1,
+		__BITFIELD_FIELD(uint32_t ackmsk			: 1,
+		__BITFIELD_FIELD(uint32_t nakmsk			: 1,
+		__BITFIELD_FIELD(uint32_t stallmsk			: 1,
+		__BITFIELD_FIELD(uint32_t ahberrmsk			: 1,
+		__BITFIELD_FIELD(uint32_t chhltdmsk			: 1,
+		__BITFIELD_FIELD(uint32_t xfercomplmsk			: 1,
+		;))))))))))))
 	} s;
 };
 
@@ -1201,12 +1219,13 @@ union cvmx_usbcx_hcspltx {
 	 *	translator.
 	 */
 	struct cvmx_usbcx_hcspltx_s {
-		uint32_t spltena	: 1;
-		uint32_t reserved_17_30	: 14;
-		uint32_t compsplt	: 1;
-		uint32_t xactpos	: 2;
-		uint32_t hubaddr	: 7;
-		uint32_t prtaddr	: 7;
+		__BITFIELD_FIELD(uint32_t spltena			: 1,
+		__BITFIELD_FIELD(uint32_t reserved_17_30		: 14,
+		__BITFIELD_FIELD(uint32_t compsplt			: 1,
+		__BITFIELD_FIELD(uint32_t xactpos			: 2,
+		__BITFIELD_FIELD(uint32_t hubaddr			: 7,
+		__BITFIELD_FIELD(uint32_t prtaddr			: 7,
+		;))))))
 	} s;
 };
 
@@ -1246,10 +1265,11 @@ union cvmx_usbcx_hctsizx {
 	 *	size for IN transactions (periodic and non-periodic).
 	 */
 	struct cvmx_usbcx_hctsizx_s {
-		uint32_t dopng		: 1;
-		uint32_t pid		: 2;
-		uint32_t pktcnt		: 10;
-		uint32_t xfersize	: 19;
+		__BITFIELD_FIELD(uint32_t dopng			: 1,
+		__BITFIELD_FIELD(uint32_t pid			: 2,
+		__BITFIELD_FIELD(uint32_t pktcnt		: 10,
+		__BITFIELD_FIELD(uint32_t xfersize		: 19,
+		;))))
 	} s;
 };
 
@@ -1283,8 +1303,9 @@ union cvmx_usbcx_hfir {
 	 *	* 1 ms (PHY clock frequency for FS/LS)
 	 */
 	struct cvmx_usbcx_hfir_s {
-		uint32_t reserved_16_31	: 16;
-		uint32_t frint		: 16;
+		__BITFIELD_FIELD(uint32_t reserved_16_31		: 16,
+		__BITFIELD_FIELD(uint32_t frint				: 16,
+		;))
 	} s;
 };
 
@@ -1312,8 +1333,9 @@ union cvmx_usbcx_hfnum {
 	 *	USB, and is reset to 0 when it reaches 16'h3FFF.
 	 */
 	struct cvmx_usbcx_hfnum_s {
-		uint32_t frrem	: 16;
-		uint32_t frnum	: 16;
+		__BITFIELD_FIELD(uint32_t frrem		: 16,
+		__BITFIELD_FIELD(uint32_t frnum		: 16,
+		;))
 	} s;
 };
 
@@ -1439,21 +1461,22 @@ union cvmx_usbcx_hprt {
 	 *	* 1: A device is attached to the port.
 	 */
 	struct cvmx_usbcx_hprt_s {
-		uint32_t reserved_19_31	: 13;
-		uint32_t prtspd		: 2;
-		uint32_t prttstctl	: 4;
-		uint32_t prtpwr		: 1;
-		uint32_t prtlnsts	: 2;
-		uint32_t reserved_9_9	: 1;
-		uint32_t prtrst		: 1;
-		uint32_t prtsusp	: 1;
-		uint32_t prtres		: 1;
-		uint32_t prtovrcurrchng	: 1;
-		uint32_t prtovrcurract	: 1;
-		uint32_t prtenchng	: 1;
-		uint32_t prtena		: 1;
-		uint32_t prtconndet	: 1;
-		uint32_t prtconnsts	: 1;
+		__BITFIELD_FIELD(uint32_t reserved_19_31	: 13,
+		__BITFIELD_FIELD(uint32_t prtspd		: 2,
+		__BITFIELD_FIELD(uint32_t prttstctl		: 4,
+		__BITFIELD_FIELD(uint32_t prtpwr		: 1,
+		__BITFIELD_FIELD(uint32_t prtlnsts		: 2,
+		__BITFIELD_FIELD(uint32_t reserved_9_9		: 1,
+		__BITFIELD_FIELD(uint32_t prtrst		: 1,
+		__BITFIELD_FIELD(uint32_t prtsusp		: 1,
+		__BITFIELD_FIELD(uint32_t prtres		: 1,
+		__BITFIELD_FIELD(uint32_t prtovrcurrchng	: 1,
+		__BITFIELD_FIELD(uint32_t prtovrcurract		: 1,
+		__BITFIELD_FIELD(uint32_t prtenchng		: 1,
+		__BITFIELD_FIELD(uint32_t prtena		: 1,
+		__BITFIELD_FIELD(uint32_t prtconndet		: 1,
+		__BITFIELD_FIELD(uint32_t prtconnsts		: 1,
+		;)))))))))))))))
 	} s;
 };
 
@@ -1476,8 +1499,9 @@ union cvmx_usbcx_hptxfsiz {
 	 * @ptxfstaddr: Host Periodic TxFIFO Start Address (PTxFStAddr)
 	 */
 	struct cvmx_usbcx_hptxfsiz_s {
-		uint32_t ptxfsize	: 16;
-		uint32_t ptxfstaddr	: 16;
+		__BITFIELD_FIELD(uint32_t ptxfsize	: 16,
+		__BITFIELD_FIELD(uint32_t ptxfstaddr	: 16,
+		;))
 	} s;
 };
 
@@ -1531,9 +1555,10 @@ union cvmx_usbcx_hptxsts {
 	 *	* Others: Reserved
 	 */
 	struct cvmx_usbcx_hptxsts_s {
-		uint32_t ptxqtop	: 8;
-		uint32_t ptxqspcavail	: 8;
-		uint32_t ptxfspcavail	: 16;
+		__BITFIELD_FIELD(uint32_t ptxqtop	: 8,
+		__BITFIELD_FIELD(uint32_t ptxqspcavail	: 8,
+		__BITFIELD_FIELD(uint32_t ptxfspcavail	: 16,
+		;)))
 	} s;
 };
 
@@ -1636,21 +1661,22 @@ union cvmx_usbnx_clk_ctl {
 	 *	until AFTER this field is set and then read.
 	 */
 	struct cvmx_usbnx_clk_ctl_s {
-		uint64_t reserved_20_63	: 44;
-		uint64_t divide2	: 2;
-		uint64_t hclk_rst	: 1;
-		uint64_t p_x_on		: 1;
-		uint64_t p_rtype	: 2;
-		uint64_t p_com_on	: 1;
-		uint64_t p_c_sel	: 2;
-		uint64_t cdiv_byp	: 1;
-		uint64_t sd_mode	: 2;
-		uint64_t s_bist		: 1;
-		uint64_t por		: 1;
-		uint64_t enable		: 1;
-		uint64_t prst		: 1;
-		uint64_t hrst		: 1;
-		uint64_t divide		: 3;
+		__BITFIELD_FIELD(uint64_t reserved_20_63	: 44,
+		__BITFIELD_FIELD(uint64_t divide2		: 2,
+		__BITFIELD_FIELD(uint64_t hclk_rst		: 1,
+		__BITFIELD_FIELD(uint64_t p_x_on		: 1,
+		__BITFIELD_FIELD(uint64_t p_rtype		: 2,
+		__BITFIELD_FIELD(uint64_t p_com_on		: 1,
+		__BITFIELD_FIELD(uint64_t p_c_sel		: 2,
+		__BITFIELD_FIELD(uint64_t cdiv_byp		: 1,
+		__BITFIELD_FIELD(uint64_t sd_mode		: 2,
+		__BITFIELD_FIELD(uint64_t s_bist		: 1,
+		__BITFIELD_FIELD(uint64_t por			: 1,
+		__BITFIELD_FIELD(uint64_t enable		: 1,
+		__BITFIELD_FIELD(uint64_t prst			: 1,
+		__BITFIELD_FIELD(uint64_t hrst			: 1,
+		__BITFIELD_FIELD(uint64_t divide		: 3,
+		;)))))))))))))))
 	} s;
 };
 
@@ -1778,41 +1804,42 @@ union cvmx_usbnx_usbp_ctl_status {
 	 *	de-assertion.
 	 */
 	struct cvmx_usbnx_usbp_ctl_status_s {
-		uint64_t txrisetune		: 1;
-		uint64_t txvreftune		: 4;
-		uint64_t txfslstune		: 4;
-		uint64_t txhsxvtune		: 2;
-		uint64_t sqrxtune		: 3;
-		uint64_t compdistune		: 3;
-		uint64_t otgtune		: 3;
-		uint64_t otgdisable		: 1;
-		uint64_t portreset		: 1;
-		uint64_t drvvbus		: 1;
-		uint64_t lsbist			: 1;
-		uint64_t fsbist			: 1;
-		uint64_t hsbist			: 1;
-		uint64_t bist_done		: 1;
-		uint64_t bist_err		: 1;
-		uint64_t tdata_out		: 4;
-		uint64_t siddq			: 1;
-		uint64_t txpreemphasistune	: 1;
-		uint64_t dma_bmode		: 1;
-		uint64_t usbc_end		: 1;
-		uint64_t usbp_bist		: 1;
-		uint64_t tclk			: 1;
-		uint64_t dp_pulld		: 1;
-		uint64_t dm_pulld		: 1;
-		uint64_t hst_mode		: 1;
-		uint64_t tuning			: 4;
-		uint64_t tx_bs_enh		: 1;
-		uint64_t tx_bs_en		: 1;
-		uint64_t loop_enb		: 1;
-		uint64_t vtest_enb		: 1;
-		uint64_t bist_enb		: 1;
-		uint64_t tdata_sel		: 1;
-		uint64_t taddr_in		: 4;
-		uint64_t tdata_in		: 8;
-		uint64_t ate_reset		: 1;
+		__BITFIELD_FIELD(uint64_t txrisetune		: 1,
+		__BITFIELD_FIELD(uint64_t txvreftune		: 4,
+		__BITFIELD_FIELD(uint64_t txfslstune		: 4,
+		__BITFIELD_FIELD(uint64_t txhsxvtune		: 2,
+		__BITFIELD_FIELD(uint64_t sqrxtune		: 3,
+		__BITFIELD_FIELD(uint64_t compdistune		: 3,
+		__BITFIELD_FIELD(uint64_t otgtune		: 3,
+		__BITFIELD_FIELD(uint64_t otgdisable		: 1,
+		__BITFIELD_FIELD(uint64_t portreset		: 1,
+		__BITFIELD_FIELD(uint64_t drvvbus		: 1,
+		__BITFIELD_FIELD(uint64_t lsbist		: 1,
+		__BITFIELD_FIELD(uint64_t fsbist		: 1,
+		__BITFIELD_FIELD(uint64_t hsbist		: 1,
+		__BITFIELD_FIELD(uint64_t bist_done		: 1,
+		__BITFIELD_FIELD(uint64_t bist_err		: 1,
+		__BITFIELD_FIELD(uint64_t tdata_out		: 4,
+		__BITFIELD_FIELD(uint64_t siddq			: 1,
+		__BITFIELD_FIELD(uint64_t txpreemphasistune	: 1,
+		__BITFIELD_FIELD(uint64_t dma_bmode		: 1,
+		__BITFIELD_FIELD(uint64_t usbc_end		: 1,
+		__BITFIELD_FIELD(uint64_t usbp_bist		: 1,
+		__BITFIELD_FIELD(uint64_t tclk			: 1,
+		__BITFIELD_FIELD(uint64_t dp_pulld		: 1,
+		__BITFIELD_FIELD(uint64_t dm_pulld		: 1,
+		__BITFIELD_FIELD(uint64_t hst_mode		: 1,
+		__BITFIELD_FIELD(uint64_t tuning		: 4,
+		__BITFIELD_FIELD(uint64_t tx_bs_enh		: 1,
+		__BITFIELD_FIELD(uint64_t tx_bs_en		: 1,
+		__BITFIELD_FIELD(uint64_t loop_enb		: 1,
+		__BITFIELD_FIELD(uint64_t vtest_enb		: 1,
+		__BITFIELD_FIELD(uint64_t bist_enb		: 1,
+		__BITFIELD_FIELD(uint64_t tdata_sel		: 1,
+		__BITFIELD_FIELD(uint64_t taddr_in		: 4,
+		__BITFIELD_FIELD(uint64_t tdata_in		: 8,
+		__BITFIELD_FIELD(uint64_t ate_reset		: 1,
+		;)))))))))))))))))))))))))))))))))))
 	} s;
 };
 
diff --git a/drivers/staging/octeon/ethernet-mdio.c b/drivers/staging/octeon/ethernet-mdio.c
index ebfa9c9e71b1..40dab11e5333 100644
--- a/drivers/staging/octeon/ethernet-mdio.c
+++ b/drivers/staging/octeon/ethernet-mdio.c
@@ -130,19 +130,6 @@ static void cvm_oct_note_carrier(struct octeon_ethernet *priv,
 	}
 }
 
-void cvm_oct_set_carrier(struct octeon_ethernet *priv,
-			 cvmx_helper_link_info_t link_info)
-{
-	cvm_oct_note_carrier(priv, link_info);
-	if (link_info.s.link_up) {
-		if (!netif_carrier_ok(priv->netdev))
-			netif_carrier_on(priv->netdev);
-	} else {
-		if (netif_carrier_ok(priv->netdev))
-			netif_carrier_off(priv->netdev);
-	}
-}
-
 void cvm_oct_adjust_link(struct net_device *dev)
 {
 	struct octeon_ethernet *priv = netdev_priv(dev);
diff --git a/drivers/staging/octeon/ethernet-mdio.h b/drivers/staging/octeon/ethernet-mdio.h
index eccfcc54cea8..6191b0850646 100644
--- a/drivers/staging/octeon/ethernet-mdio.h
+++ b/drivers/staging/octeon/ethernet-mdio.h
@@ -40,5 +40,8 @@
 #endif /* CONFIG_XFRM */
 
 extern const struct ethtool_ops cvm_oct_ethtool_ops;
+
+extern void octeon_mdiobus_force_mod_depencency(void);
+
 int cvm_oct_ioctl(struct net_device *dev, struct ifreq *rq, int cmd);
 int cvm_oct_phy_setup_device(struct net_device *dev);
diff --git a/drivers/staging/octeon/ethernet.c b/drivers/staging/octeon/ethernet.c
index 460e8545904f..f539d82f2f11 100644
--- a/drivers/staging/octeon/ethernet.c
+++ b/drivers/staging/octeon/ethernet.c
@@ -573,8 +573,6 @@ static const struct net_device_ops cvm_oct_pow_netdev_ops = {
 #endif
 };
 
-extern void octeon_mdiobus_force_mod_depencency(void);
-
 static struct device_node *cvm_oct_of_get_child(
 				const struct device_node *parent, int reg_val)
 {
@@ -859,7 +857,7 @@ static int cvm_oct_remove(struct platform_device *pdev)
 	return 0;
 }
 
-static struct of_device_id cvm_oct_match[] = {
+static const struct of_device_id cvm_oct_match[] = {
 	{
 		.compatible = "cavium,octeon-3860-pip",
 	},
diff --git a/drivers/staging/olpc_dcon/olpc_dcon.c b/drivers/staging/olpc_dcon/olpc_dcon.c
index bc7e664cc8a7..d115f5c0e341 100644
--- a/drivers/staging/olpc_dcon/olpc_dcon.c
+++ b/drivers/staging/olpc_dcon/olpc_dcon.c
@@ -18,8 +18,6 @@
 #include <linux/console.h>
 #include <linux/i2c.h>
 #include <linux/platform_device.h>
-#include <linux/pci.h>
-#include <linux/pci_ids.h>
 #include <linux/interrupt.h>
 #include <linux/delay.h>
 #include <linux/module.h>
diff --git a/drivers/staging/ozwpan/ozhcd.c b/drivers/staging/ozwpan/ozhcd.c
index 8543bb29a138..5ff4716b72c3 100644
--- a/drivers/staging/ozwpan/ozhcd.c
+++ b/drivers/staging/ozwpan/ozhcd.c
@@ -280,17 +280,20 @@ static void oz_free_urb_link(struct oz_urb_link *urbl)
  */
 static struct oz_endpoint *oz_ep_alloc(int buffer_size, gfp_t mem_flags)
 {
-	struct oz_endpoint *ep =
-		kzalloc(sizeof(struct oz_endpoint)+buffer_size, mem_flags);
-	if (ep) {
-		INIT_LIST_HEAD(&ep->urb_list);
-		INIT_LIST_HEAD(&ep->link);
-		ep->credit = -1;
-		if (buffer_size) {
-			ep->buffer_size = buffer_size;
-			ep->buffer = (u8 *)(ep+1);
-		}
+	struct oz_endpoint *ep;
+
+	ep = kzalloc(sizeof(struct oz_endpoint)+buffer_size, mem_flags);
+	if (!ep)
+		return NULL;
+
+	INIT_LIST_HEAD(&ep->urb_list);
+	INIT_LIST_HEAD(&ep->link);
+	ep->credit = -1;
+	if (buffer_size) {
+		ep->buffer_size = buffer_size;
+		ep->buffer = (u8 *)(ep+1);
 	}
+
 	return ep;
 }
 
diff --git a/drivers/staging/ozwpan/ozmain.c b/drivers/staging/ozwpan/ozmain.c
index 7d6ef4cadf1a..74ef34815b98 100644
--- a/drivers/staging/ozwpan/ozmain.c
+++ b/drivers/staging/ozwpan/ozmain.c
@@ -34,11 +34,21 @@ MODULE_PARM_DESC(g_net_dev, "The device(s) to bind to; "
  */
 static int __init ozwpan_init(void)
 {
-	oz_cdev_register();
-	oz_protocol_init(g_net_dev);
+	int err;
+
+	err = oz_cdev_register();
+	if (err)
+		return err;
+	err = oz_protocol_init(g_net_dev);
+	if (err)
+		goto err_protocol;
 	oz_app_enable(OZ_APPID_USB, 1);
 	oz_apps_init();
 	return 0;
+
+err_protocol:
+	oz_cdev_deregister();
+	return err;
 }
 
 /*
diff --git a/drivers/staging/ozwpan/ozpd.c b/drivers/staging/ozwpan/ozpd.c
index 852c288aaf13..021d74a132dd 100644
--- a/drivers/staging/ozwpan/ozpd.c
+++ b/drivers/staging/ozwpan/ozpd.c
@@ -102,34 +102,36 @@ void oz_pd_put(struct oz_pd *pd)
  */
 struct oz_pd *oz_pd_alloc(const u8 *mac_addr)
 {
-	struct oz_pd *pd = kzalloc(sizeof(struct oz_pd), GFP_ATOMIC);
-
-	if (pd) {
-		int i;
-
-		atomic_set(&pd->ref_count, 2);
-		for (i = 0; i < OZ_NB_APPS; i++)
-			spin_lock_init(&pd->app_lock[i]);
-		pd->last_rx_pkt_num = 0xffffffff;
-		oz_pd_set_state(pd, OZ_PD_S_IDLE);
-		pd->max_tx_size = OZ_MAX_TX_SIZE;
-		ether_addr_copy(pd->mac_addr, mac_addr);
-		oz_elt_buf_init(&pd->elt_buff);
-		spin_lock_init(&pd->tx_frame_lock);
-		INIT_LIST_HEAD(&pd->tx_queue);
-		INIT_LIST_HEAD(&pd->farewell_list);
-		pd->last_sent_frame = &pd->tx_queue;
-		spin_lock_init(&pd->stream_lock);
-		INIT_LIST_HEAD(&pd->stream_list);
-		tasklet_init(&pd->heartbeat_tasklet, oz_pd_heartbeat_handler,
-							(unsigned long)pd);
-		tasklet_init(&pd->timeout_tasklet, oz_pd_timeout_handler,
-							(unsigned long)pd);
-		hrtimer_init(&pd->heartbeat, CLOCK_MONOTONIC, HRTIMER_MODE_REL);
-		hrtimer_init(&pd->timeout, CLOCK_MONOTONIC, HRTIMER_MODE_REL);
-		pd->heartbeat.function = oz_pd_heartbeat_event;
-		pd->timeout.function = oz_pd_timeout_event;
-	}
+	struct oz_pd *pd;
+	int i;
+
+	pd = kzalloc(sizeof(struct oz_pd), GFP_ATOMIC);
+	if (!pd)
+		return NULL;
+
+	atomic_set(&pd->ref_count, 2);
+	for (i = 0; i < OZ_NB_APPS; i++)
+		spin_lock_init(&pd->app_lock[i]);
+	pd->last_rx_pkt_num = 0xffffffff;
+	oz_pd_set_state(pd, OZ_PD_S_IDLE);
+	pd->max_tx_size = OZ_MAX_TX_SIZE;
+	ether_addr_copy(pd->mac_addr, mac_addr);
+	oz_elt_buf_init(&pd->elt_buff);
+	spin_lock_init(&pd->tx_frame_lock);
+	INIT_LIST_HEAD(&pd->tx_queue);
+	INIT_LIST_HEAD(&pd->farewell_list);
+	pd->last_sent_frame = &pd->tx_queue;
+	spin_lock_init(&pd->stream_lock);
+	INIT_LIST_HEAD(&pd->stream_list);
+	tasklet_init(&pd->heartbeat_tasklet, oz_pd_heartbeat_handler,
+						(unsigned long)pd);
+	tasklet_init(&pd->timeout_tasklet, oz_pd_timeout_handler,
+						(unsigned long)pd);
+	hrtimer_init(&pd->heartbeat, CLOCK_MONOTONIC, HRTIMER_MODE_REL);
+	hrtimer_init(&pd->timeout, CLOCK_MONOTONIC, HRTIMER_MODE_REL);
+	pd->heartbeat.function = oz_pd_heartbeat_event;
+	pd->timeout.function = oz_pd_timeout_event;
+
 	return pd;
 }
 
@@ -652,8 +654,9 @@ static struct oz_isoc_stream *pd_stream_find(struct oz_pd *pd, u8 ep_num)
  */
 int oz_isoc_stream_create(struct oz_pd *pd, u8 ep_num)
 {
-	struct oz_isoc_stream *st =
-		kzalloc(sizeof(struct oz_isoc_stream), GFP_ATOMIC);
+	struct oz_isoc_stream *st;
+
+	st = kzalloc(sizeof(struct oz_isoc_stream), GFP_ATOMIC);
 	if (!st)
 		return -ENOMEM;
 	st->ep_num = ep_num;
diff --git a/drivers/staging/ozwpan/ozproto.c b/drivers/staging/ozwpan/ozproto.c
index 3d3a3a890f73..1ba24a2aef83 100644
--- a/drivers/staging/ozwpan/ozproto.c
+++ b/drivers/staging/ozwpan/ozproto.c
@@ -98,7 +98,7 @@ static void oz_send_conn_rsp(struct oz_pd *pd, u8 status)
 		kfree_skb(skb);
 		return;
 	}
-	oz_hdr->control = (OZ_PROTOCOL_VERSION<<OZ_VERSION_SHIFT);
+	oz_hdr->control = OZ_PROTOCOL_VERSION<<OZ_VERSION_SHIFT;
 	oz_hdr->last_pkt_num = 0;
 	put_unaligned(0, &oz_hdr->pkt_num);
 	elt->type = OZ_ELT_CONNECT_RSP;
diff --git a/drivers/staging/panel/panel.c b/drivers/staging/panel/panel.c
index 6ed35b6ecf0d..ea54fb4ec837 100644
--- a/drivers/staging/panel/panel.c
+++ b/drivers/staging/panel/panel.c
@@ -335,11 +335,11 @@ static unsigned char lcd_bits[LCD_PORTS][LCD_BITS][BIT_STATES];
  * LCD types
  */
 #define LCD_TYPE_NONE		0
-#define LCD_TYPE_OLD		1
-#define LCD_TYPE_KS0074		2
-#define LCD_TYPE_HANTRONIX	3
-#define LCD_TYPE_NEXCOM		4
-#define LCD_TYPE_CUSTOM		5
+#define LCD_TYPE_CUSTOM		1
+#define LCD_TYPE_OLD		2
+#define LCD_TYPE_KS0074		3
+#define LCD_TYPE_HANTRONIX	4
+#define LCD_TYPE_NEXCOM		5
 
 /*
  * keypad types
@@ -473,8 +473,6 @@ static struct pardevice *pprt;
 
 static int keypad_initialized;
 
-static char init_in_progress;
-
 static void (*lcd_write_cmd)(int);
 static void (*lcd_write_data)(int);
 static void (*lcd_clear_fast)(void);
@@ -502,7 +500,7 @@ MODULE_PARM_DESC(keypad_type,
 static int lcd_type = NOT_SET;
 module_param(lcd_type, int, 0000);
 MODULE_PARM_DESC(lcd_type,
-		 "LCD type: 0=none, 1=old //, 2=serial ks0074, 3=hantronix //, 4=nexcom //, 5=compiled-in");
+		 "LCD type: 0=none, 1=compiled-in, 2=old, 3=serial ks0074, 4=hantronix, 5=nexcom");
 
 static int lcd_height = NOT_SET;
 module_param(lcd_height, int, 0000);
@@ -1718,9 +1716,6 @@ static struct miscdevice keypad_dev = {
 
 static void keypad_send_key(const char *string, int max_len)
 {
-	if (init_in_progress)
-		return;
-
 	/* send the key to the device only if a process is attached to it. */
 	if (!atomic_read(&keypad_available)) {
 		while (max_len-- && keypad_buflen < KEYPAD_BUFFER && *string) {
@@ -2010,10 +2005,8 @@ static void init_scan_timer(void)
 	if (scan_timer.function != NULL)
 		return;		/* already started */
 
-	init_timer(&scan_timer);
+	setup_timer(&scan_timer, (void *)&panel_scan_timer, 0);
 	scan_timer.expires = jiffies + INPUT_POLL_TIME;
-	scan_timer.data = 0;
-	scan_timer.function = (void *)&panel_scan_timer;
 	add_timer(&scan_timer);
 }
 
@@ -2236,6 +2229,7 @@ static void panel_attach(struct parport *port)
 		if (misc_register(&keypad_dev))
 			goto err_lcd_unreg;
 	}
+	register_reboot_notifier(&panel_notifier);
 	return;
 
 err_lcd_unreg:
@@ -2257,6 +2251,8 @@ static void panel_detach(struct parport *port)
 		return;
 	}
 
+	unregister_reboot_notifier(&panel_notifier);
+
 	if (keypad.enabled && keypad_initialized) {
 		misc_deregister(&keypad_dev);
 		keypad_initialized = 0;
@@ -2281,7 +2277,7 @@ static struct parport_driver panel_driver = {
 /* init function */
 static int __init panel_init_module(void)
 {
-	int selected_keypad_type = NOT_SET;
+	int selected_keypad_type = NOT_SET, err;
 
 	/* take care of an eventual profile */
 	switch (profile) {
@@ -2324,26 +2320,6 @@ static int __init panel_init_module(void)
 	}
 
 	/*
-	 * Init lcd struct with load-time values to preserve exact current
-	 * functionality (at least for now).
-	 */
-	lcd.height = lcd_height;
-	lcd.width = lcd_width;
-	lcd.bwidth = lcd_bwidth;
-	lcd.hwidth = lcd_hwidth;
-	lcd.charset = lcd_charset;
-	lcd.proto = lcd_proto;
-	lcd.pins.e = lcd_e_pin;
-	lcd.pins.rs = lcd_rs_pin;
-	lcd.pins.rw = lcd_rw_pin;
-	lcd.pins.cl = lcd_cl_pin;
-	lcd.pins.da = lcd_da_pin;
-	lcd.pins.bl = lcd_bl_pin;
-
-	/* Leave it for now, just in case */
-	lcd.esc_seq.len = -1;
-
-	/*
 	 * Overwrite selection with module param values (both keypad and lcd),
 	 * where the deprecated params have lower prio.
 	 */
@@ -2361,6 +2337,28 @@ static int __init panel_init_module(void)
 
 	lcd.enabled = (selected_lcd_type > 0);
 
+	if (lcd.enabled) {
+		/*
+		 * Init lcd struct with load-time values to preserve exact
+		 * current functionality (at least for now).
+		 */
+		lcd.height = lcd_height;
+		lcd.width = lcd_width;
+		lcd.bwidth = lcd_bwidth;
+		lcd.hwidth = lcd_hwidth;
+		lcd.charset = lcd_charset;
+		lcd.proto = lcd_proto;
+		lcd.pins.e = lcd_e_pin;
+		lcd.pins.rs = lcd_rs_pin;
+		lcd.pins.rw = lcd_rw_pin;
+		lcd.pins.cl = lcd_cl_pin;
+		lcd.pins.da = lcd_da_pin;
+		lcd.pins.bl = lcd_bl_pin;
+
+		/* Leave it for now, just in case */
+		lcd.esc_seq.len = -1;
+	}
+
 	switch (selected_keypad_type) {
 	case KEYPAD_TYPE_OLD:
 		keypad_profile = old_keypad_profile;
@@ -2376,27 +2374,17 @@ static int __init panel_init_module(void)
 		break;
 	}
 
-	/* tells various subsystems about the fact that we are initializing */
-	init_in_progress = 1;
-
-	if (parport_register_driver(&panel_driver)) {
-		pr_err("could not register with parport. Aborting.\n");
-		return -EIO;
-	}
-
 	if (!lcd.enabled && !keypad.enabled) {
-		/* no device enabled, let's release the parport */
-		if (pprt) {
-			parport_release(pprt);
-			parport_unregister_device(pprt);
-			pprt = NULL;
-		}
-		parport_unregister_driver(&panel_driver);
+		/* no device enabled, let's exit */
 		pr_err("driver version " PANEL_VERSION " disabled.\n");
 		return -ENODEV;
 	}
 
-	register_reboot_notifier(&panel_notifier);
+	err = parport_register_driver(&panel_driver);
+	if (err) {
+		pr_err("could not register with parport. Aborting.\n");
+		return err;
+	}
 
 	if (pprt)
 		pr_info("driver version " PANEL_VERSION
@@ -2405,15 +2393,11 @@ static int __init panel_init_module(void)
 	else
 		pr_info("driver version " PANEL_VERSION
 			" not yet registered\n");
-	/* tells various subsystems about the fact that initialization
-	   is finished */
-	init_in_progress = 0;
 	return 0;
 }
 
 static void __exit panel_cleanup_module(void)
 {
-	unregister_reboot_notifier(&panel_notifier);
 
 	if (scan_timer.function != NULL)
 		del_timer_sync(&scan_timer);
diff --git a/drivers/staging/rtl8188eu/core/rtw_ap.c b/drivers/staging/rtl8188eu/core/rtw_ap.c
index da19145c49c5..e65ee6e858a8 100644
--- a/drivers/staging/rtl8188eu/core/rtw_ap.c
+++ b/drivers/staging/rtl8188eu/core/rtw_ap.c
@@ -23,6 +23,7 @@
 #include <drv_types.h>
 #include <wifi.h>
 #include <ieee80211.h>
+#include <asm/unaligned.h>
 
 #ifdef CONFIG_88EU_AP_MODE
 
@@ -78,11 +79,8 @@ static void update_BCNTIM(struct adapter *padapter)
 	if (true) {
 		u8 *p, *dst_ie, *premainder_ie = NULL;
 		u8 *pbackup_remainder_ie = NULL;
-		__le16 tim_bitmap_le;
 		uint offset, tmp_len, tim_ielen, tim_ie_offset, remainder_ielen;
 
-		tim_bitmap_le = cpu_to_le16(pstapriv->tim_bitmap);
-
 		p = rtw_get_ie(pie + _FIXED_IE_LENGTH_, _TIM_IE_, &tim_ielen, pnetwork_mlmeext->IELength - _FIXED_IE_LENGTH_);
 		if (p != NULL && tim_ielen > 0) {
 			tim_ielen += 2;
@@ -137,9 +135,9 @@ static void update_BCNTIM(struct adapter *padapter)
 			*dst_ie++ = 0;
 
 		if (tim_ielen == 4) {
-			*dst_ie++ = *(u8 *)&tim_bitmap_le;
+			*dst_ie++ = pstapriv->tim_bitmap & 0xff;
 		} else if (tim_ielen == 5) {
-			memcpy(dst_ie, &tim_bitmap_le, 2);
+			put_unaligned_le16(pstapriv->tim_bitmap, dst_ie);
 			dst_ie += 2;
 		}
 
diff --git a/drivers/staging/rtl8188eu/core/rtw_cmd.c b/drivers/staging/rtl8188eu/core/rtw_cmd.c
index 4b4346244953..89b5e48ed68a 100644
--- a/drivers/staging/rtl8188eu/core/rtw_cmd.c
+++ b/drivers/staging/rtl8188eu/core/rtw_cmd.c
@@ -325,7 +325,8 @@ u8 rtw_sitesurvey_cmd(struct adapter  *padapter, struct ndis_802_11_ssid *ssid,
 	if (res == _SUCCESS) {
 		pmlmepriv->scan_start_time = jiffies;
 
-		_set_timer(&pmlmepriv->scan_to_timer, SCANNING_TIMEOUT);
+		mod_timer(&pmlmepriv->scan_to_timer,
+			  jiffies + msecs_to_jiffies(SCANNING_TIMEOUT));
 
 		rtw_led_control(padapter, LED_CTL_SITE_SURVEY);
 
@@ -1234,9 +1235,11 @@ void rtw_survey_cmd_callback(struct adapter *padapter,  struct cmd_obj *pcmd)
 	if (pcmd->res == H2C_DROPPED) {
 		/* TODO: cancel timer and do timeout handler directly... */
 		/* need to make timeout handlerOS independent */
-		_set_timer(&pmlmepriv->scan_to_timer, 1);
+		mod_timer(&pmlmepriv->scan_to_timer,
+			  jiffies + msecs_to_jiffies(1));
 	} else if (pcmd->res != H2C_SUCCESS) {
-		_set_timer(&pmlmepriv->scan_to_timer, 1);
+		mod_timer(&pmlmepriv->scan_to_timer,
+			  jiffies + msecs_to_jiffies(1));
 		RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_, ("\n ********Error: MgntActrtw_set_802_11_bssid_LIST_SCAN Fail ************\n\n."));
 	}
 
@@ -1270,10 +1273,12 @@ void rtw_joinbss_cmd_callback(struct adapter *padapter,  struct cmd_obj *pcmd)
 	if (pcmd->res == H2C_DROPPED) {
 		/* TODO: cancel timer and do timeout handler directly... */
 		/* need to make timeout handlerOS independent */
-		_set_timer(&pmlmepriv->assoc_timer, 1);
+		mod_timer(&pmlmepriv->assoc_timer,
+			  jiffies + msecs_to_jiffies(1));
 	} else if (pcmd->res != H2C_SUCCESS) {
 		RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_, ("********Error:rtw_select_and_join_from_scanned_queue Wait Sema  Fail ************\n"));
-		_set_timer(&pmlmepriv->assoc_timer, 1);
+		mod_timer(&pmlmepriv->assoc_timer,
+			  jiffies + msecs_to_jiffies(1));
 	}
 
 	rtw_free_cmd_obj(pcmd);
@@ -1291,7 +1296,8 @@ void rtw_createbss_cmd_callback(struct adapter *padapter, struct cmd_obj *pcmd)
 
 	if (pcmd->res != H2C_SUCCESS) {
 		RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_, ("\n ********Error: rtw_createbss_cmd_callback  Fail ************\n\n."));
-		_set_timer(&pmlmepriv->assoc_timer, 1);
+		mod_timer(&pmlmepriv->assoc_timer,
+			  jiffies + msecs_to_jiffies(1));
 	}
 
 	del_timer_sync(&pmlmepriv->assoc_timer);
diff --git a/drivers/staging/rtl8188eu/core/rtw_efuse.c b/drivers/staging/rtl8188eu/core/rtw_efuse.c
index 8816d116a8b8..b66746160223 100644
--- a/drivers/staging/rtl8188eu/core/rtw_efuse.c
+++ b/drivers/staging/rtl8188eu/core/rtw_efuse.c
@@ -139,7 +139,7 @@ efuse_phymap_to_logical(u8 *phymap, u16 _offset, u16 _size_byte, u8  *pbuf)
 	while ((rtemp8 != 0xFF) && (eFuse_Addr < EFUSE_REAL_CONTENT_LEN_88E)) {
 		/*  Check PG header for section num. */
 		if ((rtemp8 & 0x1F) == 0x0F) {		/* extended header */
-			u1temp = ((rtemp8 & 0xE0) >> 5);
+			u1temp = (rtemp8 & 0xE0) >> 5;
 			rtemp8 = *(phymap+eFuse_Addr);
 			if ((rtemp8 & 0x0F) == 0x0F) {
 				eFuse_Addr++;
@@ -150,12 +150,12 @@ efuse_phymap_to_logical(u8 *phymap, u16 _offset, u16 _size_byte, u8  *pbuf)
 				continue;
 			} else {
 				offset = ((rtemp8 & 0xF0) >> 1) | u1temp;
-				wren = (rtemp8 & 0x0F);
+				wren = rtemp8 & 0x0F;
 				eFuse_Addr++;
 			}
 		} else {
-			offset = ((rtemp8 >> 4) & 0x0f);
-			wren = (rtemp8 & 0x0f);
+			offset = (rtemp8 >> 4) & 0x0f;
+			wren = rtemp8 & 0x0f;
 		}
 
 		if (offset < EFUSE_MAX_SECTION_88E) {
diff --git a/drivers/staging/rtl8188eu/core/rtw_ieee80211.c b/drivers/staging/rtl8188eu/core/rtw_ieee80211.c
index f2c3ca79c0c9..11b780d6c4ab 100644
--- a/drivers/staging/rtl8188eu/core/rtw_ieee80211.c
+++ b/drivers/staging/rtl8188eu/core/rtw_ieee80211.c
@@ -27,7 +27,6 @@
 #include <wlan_bssdef.h>
 
 u8 RTW_WPA_OUI_TYPE[] = { 0x00, 0x50, 0xf2, 1 };
-u16 RTW_WPA_VERSION = 1;
 u8 WPA_AUTH_KEY_MGMT_NONE[] = { 0x00, 0x50, 0xf2, 0 };
 u8 WPA_AUTH_KEY_MGMT_UNSPEC_802_1X[] = { 0x00, 0x50, 0xf2, 1 };
 u8 WPA_AUTH_KEY_MGMT_PSK_OVER_802_1X[] = { 0x00, 0x50, 0xf2, 2 };
@@ -663,7 +662,7 @@ int rtw_get_sec_ie(u8 *in_ie, uint in_len, u8 *rsn_ie, u16 *rsn_len, u8 *wpa_ie,
 
 	/* Search required WPA or WPA2 IE and copy to sec_ie[] */
 
-	cnt = (_TIMESTAMP_ + _BEACON_ITERVAL_ + _CAPABILITY_);
+	cnt = _TIMESTAMP_ + _BEACON_ITERVAL_ + _CAPABILITY_;
 
 	sec_idx = 0;
 
@@ -789,7 +788,7 @@ u8 *rtw_get_wps_ie(u8 *in_ie, uint in_len, u8 *wps_ie, uint *wps_ielen)
  *
  * Returns: the address of the specific WPS attribute found, or NULL
  */
-u8 *rtw_get_wps_attr(u8 *wps_ie, uint wps_ielen, u16 target_attr_id , u8 *buf_attr, u32 *len_attr)
+u8 *rtw_get_wps_attr(u8 *wps_ie, uint wps_ielen, u16 target_attr_id, u8 *buf_attr, u32 *len_attr)
 {
 	u8 *attr_ptr = NULL;
 	u8 *target_attr_ptr = NULL;
@@ -799,7 +798,7 @@ u8 *rtw_get_wps_attr(u8 *wps_ie, uint wps_ielen, u16 target_attr_id , u8 *buf_at
 		*len_attr = 0;
 
 	if ((wps_ie[0] != _VENDOR_SPECIFIC_IE_) ||
-	    (memcmp(wps_ie + 2, wps_oui , 4)))
+	    (memcmp(wps_ie + 2, wps_oui, 4)))
 		return attr_ptr;
 
 	/*  6 = 1(Element ID) + 1(Length) + 4(WPS OUI) */
@@ -835,7 +834,7 @@ u8 *rtw_get_wps_attr(u8 *wps_ie, uint wps_ielen, u16 target_attr_id , u8 *buf_at
  *
  * Returns: the address of the specific WPS attribute content found, or NULL
  */
-u8 *rtw_get_wps_attr_content(u8 *wps_ie, uint wps_ielen, u16 target_attr_id , u8 *buf_content, uint *len_content)
+u8 *rtw_get_wps_attr_content(u8 *wps_ie, uint wps_ielen, u16 target_attr_id, u8 *buf_content, uint *len_content)
 {
 	u8 *attr_ptr;
 	u32 attr_len;
@@ -1239,7 +1238,7 @@ void rtw_get_bcn_info(struct wlan_network *pnetwork)
 	} else {
 		pnetwork->BcnInfo.encryp_protocol = ENCRYP_PROTOCOL_OPENSYS;
 	}
-	rtw_get_sec_ie(pnetwork->network.IEs , pnetwork->network.IELength, NULL, &rsn_len, NULL, &wpa_len);
+	rtw_get_sec_ie(pnetwork->network.IEs, pnetwork->network.IELength, NULL, &rsn_len, NULL, &wpa_len);
 	RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("rtw_get_bcn_info: ssid =%s\n", pnetwork->network.Ssid.Ssid));
 	RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("rtw_get_bcn_info: wpa_len =%d rsn_len =%d\n", wpa_len, rsn_len));
 	RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("rtw_get_bcn_info: ssid =%s\n", pnetwork->network.Ssid.Ssid));
diff --git a/drivers/staging/rtl8188eu/core/rtw_ioctl_set.c b/drivers/staging/rtl8188eu/core/rtw_ioctl_set.c
index 2faf6b2e8129..969150a48661 100644
--- a/drivers/staging/rtl8188eu/core/rtw_ioctl_set.c
+++ b/drivers/staging/rtl8188eu/core/rtw_ioctl_set.c
@@ -86,7 +86,8 @@ u8 rtw_do_join(struct adapter *padapter)
 		select_ret = rtw_select_and_join_from_scanned_queue(pmlmepriv);
 		if (select_ret == _SUCCESS) {
 			pmlmepriv->to_join = false;
-			_set_timer(&pmlmepriv->assoc_timer, MAX_JOIN_TIMEOUT);
+			mod_timer(&pmlmepriv->assoc_timer,
+				  jiffies + msecs_to_jiffies(MAX_JOIN_TIMEOUT));
 		} else {
 			if (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE) == true) {
 				/*  submit createbss_cmd to change to a ADHOC_MASTER */
diff --git a/drivers/staging/rtl8188eu/core/rtw_led.c b/drivers/staging/rtl8188eu/core/rtw_led.c
index 1b8264b978da..94405dc44220 100644
--- a/drivers/staging/rtl8188eu/core/rtw_led.c
+++ b/drivers/staging/rtl8188eu/core/rtw_led.c
@@ -22,9 +22,9 @@
 /*		Callback function of LED BlinkTimer, */
 /*		it just schedules to corresponding BlinkWorkItem/led_blink_hdl */
 /*  */
-void BlinkTimerCallback(void *data)
+void BlinkTimerCallback(unsigned long data)
 {
-	struct LED_871x *pLed = data;
+	struct LED_871x *pLed = (struct LED_871x *)data;
 	struct adapter *padapter = pLed->padapter;
 
 	if ((padapter->bSurpriseRemoved) || (padapter->bDriverStopped))
@@ -72,7 +72,8 @@ void InitLed871x(struct adapter *padapter, struct LED_871x *pLed)
 
 	ResetLedStatus(pLed);
 
-	_init_timer(&(pLed->BlinkTimer), padapter->pnetdev, BlinkTimerCallback, pLed);
+	setup_timer(&(pLed->BlinkTimer), BlinkTimerCallback,
+		    (unsigned long)pLed);
 
 	INIT_WORK(&(pLed->BlinkWorkItem), BlinkWorkItemCallback);
 }
@@ -122,14 +123,16 @@ static void SwLedBlink1(struct LED_871x *pLed)
 			pLed->BlinkingLedState = RTW_LED_OFF;
 		else
 			pLed->BlinkingLedState = RTW_LED_ON;
-		_set_timer(&(pLed->BlinkTimer), LED_BLINK_NO_LINK_INTERVAL_ALPHA);
+		mod_timer(&pLed->BlinkTimer, jiffies +
+			  msecs_to_jiffies(LED_BLINK_NO_LINK_INTERVAL_ALPHA));
 		break;
 	case LED_BLINK_NORMAL:
 		if (pLed->bLedOn)
 			pLed->BlinkingLedState = RTW_LED_OFF;
 		else
 			pLed->BlinkingLedState = RTW_LED_ON;
-		_set_timer(&(pLed->BlinkTimer), LED_BLINK_LINK_INTERVAL_ALPHA);
+		mod_timer(&pLed->BlinkTimer, jiffies +
+			  msecs_to_jiffies(LED_BLINK_LINK_INTERVAL_ALPHA));
 		break;
 	case LED_BLINK_SCAN:
 		pLed->BlinkTimes--;
@@ -143,7 +146,8 @@ static void SwLedBlink1(struct LED_871x *pLed)
 					pLed->BlinkingLedState = RTW_LED_OFF;
 				else
 					pLed->BlinkingLedState = RTW_LED_ON;
-				_set_timer(&(pLed->BlinkTimer), LED_BLINK_LINK_INTERVAL_ALPHA);
+				mod_timer(&pLed->BlinkTimer, jiffies +
+					  msecs_to_jiffies(LED_BLINK_LINK_INTERVAL_ALPHA));
 				RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("CurrLedState %d\n", pLed->CurrLedState));
 			} else if (!check_fwstate(pmlmepriv, _FW_LINKED)) {
 				pLed->bLedNoLinkBlinkInProgress = true;
@@ -152,7 +156,8 @@ static void SwLedBlink1(struct LED_871x *pLed)
 					pLed->BlinkingLedState = RTW_LED_OFF;
 				else
 					pLed->BlinkingLedState = RTW_LED_ON;
-				_set_timer(&(pLed->BlinkTimer), LED_BLINK_NO_LINK_INTERVAL_ALPHA);
+				mod_timer(&pLed->BlinkTimer, jiffies +
+					  msecs_to_jiffies(LED_BLINK_NO_LINK_INTERVAL_ALPHA));
 				RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("CurrLedState %d\n", pLed->CurrLedState));
 			}
 			pLed->bLedScanBlinkInProgress = false;
@@ -161,7 +166,8 @@ static void SwLedBlink1(struct LED_871x *pLed)
 				pLed->BlinkingLedState = RTW_LED_OFF;
 			else
 				pLed->BlinkingLedState = RTW_LED_ON;
-			_set_timer(&(pLed->BlinkTimer), LED_BLINK_SCAN_INTERVAL_ALPHA);
+			mod_timer(&pLed->BlinkTimer, jiffies +
+				  msecs_to_jiffies(LED_BLINK_SCAN_INTERVAL_ALPHA));
 		}
 		break;
 	case LED_BLINK_TXRX:
@@ -176,7 +182,8 @@ static void SwLedBlink1(struct LED_871x *pLed)
 					pLed->BlinkingLedState = RTW_LED_OFF;
 				else
 					pLed->BlinkingLedState = RTW_LED_ON;
-				_set_timer(&(pLed->BlinkTimer), LED_BLINK_LINK_INTERVAL_ALPHA);
+				mod_timer(&pLed->BlinkTimer, jiffies +
+					  msecs_to_jiffies(LED_BLINK_LINK_INTERVAL_ALPHA));
 				RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("CurrLedState %d\n", pLed->CurrLedState));
 			} else if (!check_fwstate(pmlmepriv, _FW_LINKED)) {
 				pLed->bLedNoLinkBlinkInProgress = true;
@@ -185,7 +192,8 @@ static void SwLedBlink1(struct LED_871x *pLed)
 					pLed->BlinkingLedState = RTW_LED_OFF;
 				else
 					pLed->BlinkingLedState = RTW_LED_ON;
-				_set_timer(&(pLed->BlinkTimer), LED_BLINK_NO_LINK_INTERVAL_ALPHA);
+				mod_timer(&pLed->BlinkTimer, jiffies +
+					  msecs_to_jiffies(LED_BLINK_NO_LINK_INTERVAL_ALPHA));
 				RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("CurrLedState %d\n", pLed->CurrLedState));
 			}
 			pLed->BlinkTimes = 0;
@@ -195,7 +203,8 @@ static void SwLedBlink1(struct LED_871x *pLed)
 				pLed->BlinkingLedState = RTW_LED_OFF;
 			else
 				pLed->BlinkingLedState = RTW_LED_ON;
-			_set_timer(&(pLed->BlinkTimer), LED_BLINK_FASTER_INTERVAL_ALPHA);
+			mod_timer(&pLed->BlinkTimer, jiffies +
+				  msecs_to_jiffies(LED_BLINK_FASTER_INTERVAL_ALPHA));
 		}
 		break;
 	case LED_BLINK_WPS:
@@ -203,7 +212,8 @@ static void SwLedBlink1(struct LED_871x *pLed)
 			pLed->BlinkingLedState = RTW_LED_OFF;
 		else
 			pLed->BlinkingLedState = RTW_LED_ON;
-		_set_timer(&(pLed->BlinkTimer), LED_BLINK_SCAN_INTERVAL_ALPHA);
+		mod_timer(&pLed->BlinkTimer, jiffies +
+			  msecs_to_jiffies(LED_BLINK_SCAN_INTERVAL_ALPHA));
 		break;
 	case LED_BLINK_WPS_STOP:	/* WPS success */
 		if (pLed->BlinkingLedState == RTW_LED_ON)
@@ -218,14 +228,15 @@ static void SwLedBlink1(struct LED_871x *pLed)
 				pLed->BlinkingLedState = RTW_LED_OFF;
 			else
 				pLed->BlinkingLedState = RTW_LED_ON;
-			_set_timer(&(pLed->BlinkTimer), LED_BLINK_LINK_INTERVAL_ALPHA);
+			mod_timer(&pLed->BlinkTimer, jiffies +
+				  msecs_to_jiffies(LED_BLINK_LINK_INTERVAL_ALPHA));
 			RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("CurrLedState %d\n", pLed->CurrLedState));
 
 			pLed->bLedWPSBlinkInProgress = false;
 		} else {
 			pLed->BlinkingLedState = RTW_LED_OFF;
-			_set_timer(&(pLed->BlinkTimer),
-				   LED_BLINK_WPS_SUCCESS_INTERVAL_ALPHA);
+			mod_timer(&pLed->BlinkTimer, jiffies +
+				  msecs_to_jiffies(LED_BLINK_WPS_SUCCESS_INTERVAL_ALPHA));
 		}
 		break;
 	default:
@@ -262,7 +273,8 @@ static void SwLedControlMode1(struct adapter *padapter, enum LED_CTL_MODE LedAct
 				pLed->BlinkingLedState = RTW_LED_OFF;
 			else
 				pLed->BlinkingLedState = RTW_LED_ON;
-			_set_timer(&(pLed->BlinkTimer), LED_BLINK_NO_LINK_INTERVAL_ALPHA);
+			mod_timer(&pLed->BlinkTimer, jiffies +
+				  msecs_to_jiffies(LED_BLINK_NO_LINK_INTERVAL_ALPHA));
 		}
 		break;
 	case LED_CTL_LINK:
@@ -283,7 +295,8 @@ static void SwLedControlMode1(struct adapter *padapter, enum LED_CTL_MODE LedAct
 				pLed->BlinkingLedState = RTW_LED_OFF;
 			else
 				pLed->BlinkingLedState = RTW_LED_ON;
-			_set_timer(&(pLed->BlinkTimer), LED_BLINK_LINK_INTERVAL_ALPHA);
+			mod_timer(&pLed->BlinkTimer, jiffies +
+				  msecs_to_jiffies(LED_BLINK_LINK_INTERVAL_ALPHA));
 		}
 		break;
 	case LED_CTL_SITE_SURVEY:
@@ -311,7 +324,8 @@ static void SwLedControlMode1(struct adapter *padapter, enum LED_CTL_MODE LedAct
 				pLed->BlinkingLedState = RTW_LED_OFF;
 			else
 				pLed->BlinkingLedState = RTW_LED_ON;
-			_set_timer(&(pLed->BlinkTimer), LED_BLINK_SCAN_INTERVAL_ALPHA);
+			mod_timer(&pLed->BlinkTimer, jiffies +
+				  msecs_to_jiffies(LED_BLINK_SCAN_INTERVAL_ALPHA));
 		 }
 		break;
 	case LED_CTL_TX:
@@ -334,7 +348,8 @@ static void SwLedControlMode1(struct adapter *padapter, enum LED_CTL_MODE LedAct
 				pLed->BlinkingLedState = RTW_LED_OFF;
 			else
 				pLed->BlinkingLedState = RTW_LED_ON;
-			_set_timer(&(pLed->BlinkTimer), LED_BLINK_FASTER_INTERVAL_ALPHA);
+			mod_timer(&pLed->BlinkTimer, jiffies +
+				  msecs_to_jiffies(LED_BLINK_FASTER_INTERVAL_ALPHA));
 		}
 		break;
 	case LED_CTL_START_WPS: /* wait until xinpin finish */
@@ -362,7 +377,8 @@ static void SwLedControlMode1(struct adapter *padapter, enum LED_CTL_MODE LedAct
 				pLed->BlinkingLedState = RTW_LED_OFF;
 			else
 				pLed->BlinkingLedState = RTW_LED_ON;
-			_set_timer(&(pLed->BlinkTimer), LED_BLINK_SCAN_INTERVAL_ALPHA);
+			mod_timer(&pLed->BlinkTimer, jiffies +
+				  msecs_to_jiffies(LED_BLINK_SCAN_INTERVAL_ALPHA));
 		 }
 		break;
 	case LED_CTL_STOP_WPS:
@@ -389,11 +405,12 @@ static void SwLedControlMode1(struct adapter *padapter, enum LED_CTL_MODE LedAct
 		pLed->CurrLedState = LED_BLINK_WPS_STOP;
 		if (pLed->bLedOn) {
 			pLed->BlinkingLedState = RTW_LED_OFF;
-			_set_timer(&(pLed->BlinkTimer),
-				   LED_BLINK_WPS_SUCCESS_INTERVAL_ALPHA);
+			mod_timer(&pLed->BlinkTimer, jiffies +
+				  msecs_to_jiffies(LED_BLINK_WPS_SUCCESS_INTERVAL_ALPHA));
 		} else {
 			pLed->BlinkingLedState = RTW_LED_ON;
-			_set_timer(&(pLed->BlinkTimer), 0);
+			mod_timer(&pLed->BlinkTimer,
+				  jiffies + msecs_to_jiffies(0));
 		}
 		break;
 	case LED_CTL_STOP_WPS_FAIL:
@@ -407,7 +424,8 @@ static void SwLedControlMode1(struct adapter *padapter, enum LED_CTL_MODE LedAct
 			pLed->BlinkingLedState = RTW_LED_OFF;
 		else
 			pLed->BlinkingLedState = RTW_LED_ON;
-		_set_timer(&(pLed->BlinkTimer), LED_BLINK_NO_LINK_INTERVAL_ALPHA);
+		mod_timer(&pLed->BlinkTimer, jiffies +
+			  msecs_to_jiffies(LED_BLINK_NO_LINK_INTERVAL_ALPHA));
 		break;
 	case LED_CTL_POWER_OFF:
 		pLed->CurrLedState = RTW_LED_OFF;
diff --git a/drivers/staging/rtl8188eu/core/rtw_mlme.c b/drivers/staging/rtl8188eu/core/rtw_mlme.c
index d4632da50c1d..6c91aa58d924 100644
--- a/drivers/staging/rtl8188eu/core/rtw_mlme.c
+++ b/drivers/staging/rtl8188eu/core/rtw_mlme.c
@@ -665,7 +665,8 @@ void rtw_surveydone_event_callback(struct adapter	*adapter, u8 *pbuf)
 				set_fwstate(pmlmepriv, _FW_UNDER_LINKING);
 
 				if (rtw_select_and_join_from_scanned_queue(pmlmepriv) == _SUCCESS) {
-					_set_timer(&pmlmepriv->assoc_timer, MAX_JOIN_TIMEOUT);
+					mod_timer(&pmlmepriv->assoc_timer,
+						  jiffies + msecs_to_jiffies(MAX_JOIN_TIMEOUT));
 				} else {
 					struct wlan_bssid_ex    *pdev_network = &(adapter->registrypriv.dev_network);
 					u8 *pibss = adapter->registrypriv.dev_network.MacAddress;
@@ -692,7 +693,8 @@ void rtw_surveydone_event_callback(struct adapter	*adapter, u8 *pbuf)
 			pmlmepriv->to_join = false;
 			s_ret = rtw_select_and_join_from_scanned_queue(pmlmepriv);
 			if (_SUCCESS == s_ret) {
-			     _set_timer(&pmlmepriv->assoc_timer, MAX_JOIN_TIMEOUT);
+			     mod_timer(&pmlmepriv->assoc_timer,
+				       jiffies + msecs_to_jiffies(MAX_JOIN_TIMEOUT));
 			} else if (s_ret == 2) { /* there is no need to wait for join */
 				_clr_fwstate_(pmlmepriv, _FW_UNDER_LINKING);
 				rtw_indicate_connect(adapter);
@@ -1127,14 +1129,16 @@ void rtw_joinbss_event_prehandle(struct adapter *adapter, u8 *pbuf)
 
 	} else if (pnetwork->join_res == -4) {
 		rtw_reset_securitypriv(adapter);
-		_set_timer(&pmlmepriv->assoc_timer, 1);
+		mod_timer(&pmlmepriv->assoc_timer,
+			  jiffies + msecs_to_jiffies(1));
 
 		if ((check_fwstate(pmlmepriv, _FW_UNDER_LINKING)) == true) {
 			RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("fail! clear _FW_UNDER_LINKING ^^^fw_state=%x\n", get_fwstate(pmlmepriv)));
 			_clr_fwstate_(pmlmepriv, _FW_UNDER_LINKING);
 		}
 	} else { /* if join_res < 0 (join fails), then try again */
-		_set_timer(&pmlmepriv->assoc_timer, 1);
+		mod_timer(&pmlmepriv->assoc_timer,
+			  jiffies + msecs_to_jiffies(1));
 		_clr_fwstate_(pmlmepriv, _FW_UNDER_LINKING);
 	}
 
@@ -1360,9 +1364,9 @@ void rtw_cpwm_event_callback(struct adapter *padapter, u8 *pbuf)
 * _rtw_join_timeout_handler - Timeout/faliure handler for CMD JoinBss
 * @adapter: pointer to struct adapter structure
 */
-void _rtw_join_timeout_handler (void *function_context)
+void _rtw_join_timeout_handler (unsigned long data)
 {
-	struct adapter *adapter = function_context;
+	struct adapter *adapter = (struct adapter *)data;
 	struct	mlme_priv *pmlmepriv = &adapter->mlmepriv;
 	int do_join_r;
 
@@ -1402,9 +1406,9 @@ void _rtw_join_timeout_handler (void *function_context)
 * rtw_scan_timeout_handler - Timeout/Faliure handler for CMD SiteSurvey
 * @adapter: pointer to struct adapter structure
 */
-void rtw_scan_timeout_handler (void *function_context)
+void rtw_scan_timeout_handler (unsigned long data)
 {
-	struct adapter *adapter = function_context;
+	struct adapter *adapter = (struct adapter *)data;
 	struct	mlme_priv *pmlmepriv = &adapter->mlmepriv;
 
 	DBG_88E(FUNC_ADPT_FMT" fw_state=%x\n", FUNC_ADPT_ARG(adapter), get_fwstate(pmlmepriv));
@@ -1429,9 +1433,9 @@ static void rtw_auto_scan_handler(struct adapter *padapter)
 	}
 }
 
-void rtw_dynamic_check_timer_handlder(void *function_context)
+void rtw_dynamic_check_timer_handlder(unsigned long data)
 {
-	struct adapter *adapter = (struct adapter *)function_context;
+	struct adapter *adapter = (struct adapter *)data;
 	struct registry_priv *pregistrypriv = &adapter->registrypriv;
 
 	if (!adapter)
@@ -1449,7 +1453,8 @@ void rtw_dynamic_check_timer_handlder(void *function_context)
 		rtw_auto_scan_handler(adapter);
 	}
 exit:
-	_set_timer(&adapter->mlmepriv.dynamic_chk_timer, 2000);
+	mod_timer(&adapter->mlmepriv.dynamic_chk_timer,
+		  jiffies + msecs_to_jiffies(2000));
 }
 
 #define RTW_SCAN_RESULT_EXPIRE 2000
@@ -2037,7 +2042,7 @@ void rtw_update_ht_cap(struct adapter *padapter, u8 *pie, uint ie_len)
 	p = rtw_get_ie(pie+sizeof(struct ndis_802_11_fixed_ie), _HT_CAPABILITY_IE_, &len, ie_len-sizeof(struct ndis_802_11_fixed_ie));
 	if (p && len > 0) {
 		pht_capie = (struct rtw_ieee80211_ht_cap *)(p+2);
-		max_ampdu_sz = (pht_capie->ampdu_params_info & IEEE80211_HT_CAP_AMPDU_FACTOR);
+		max_ampdu_sz = pht_capie->ampdu_params_info & IEEE80211_HT_CAP_AMPDU_FACTOR;
 		max_ampdu_sz = 1 << (max_ampdu_sz+3); /*  max_ampdu_sz (kbytes); */
 		phtpriv->rx_ampdu_maxlen = max_ampdu_sz;
 	}
diff --git a/drivers/staging/rtl8188eu/core/rtw_mlme_ext.c b/drivers/staging/rtl8188eu/core/rtw_mlme_ext.c
index cd12dd70dd88..be9e34a0daef 100644
--- a/drivers/staging/rtl8188eu/core/rtw_mlme_ext.c
+++ b/drivers/staging/rtl8188eu/core/rtw_mlme_ext.c
@@ -1639,7 +1639,7 @@ unsigned int OnAction_back(struct adapter *padapter, struct recv_frame *precv_fr
 			break;
 		case RTW_WLAN_ACTION_ADDBA_RESP: /* ADDBA response */
 			status = get_unaligned_le16(&frame_body[3]);
-			tid = ((frame_body[5] >> 2) & 0x7);
+			tid = (frame_body[5] >> 2) & 0x7;
 			if (status == 0) {	/* successful */
 				DBG_88E("agg_enable for TID=%d\n", tid);
 				psta->htpriv.agg_enable_bitmap |= 1 << tid;
@@ -2491,7 +2491,7 @@ void issue_auth(struct adapter *padapter, struct sta_info *psta, unsigned short
 
 		/* setting IV for auth seq #3 */
 		if ((pmlmeinfo->auth_seq == 3) && (pmlmeinfo->state & WIFI_FW_AUTH_STATE) && (use_shared_key == 1)) {
-			val32 = ((pmlmeinfo->iv++) | (pmlmeinfo->key_index << 30));
+			val32 = (pmlmeinfo->iv++) | (pmlmeinfo->key_index << 30);
 			le_tmp32 = cpu_to_le32(val32);
 			pframe = rtw_set_fixed_ie(pframe, 4, (unsigned char *)&le_tmp32, &(pattrib->pktlen));
 
@@ -3331,7 +3331,7 @@ void issue_action_BA(struct adapter *padapter, unsigned char *raddr, unsigned ch
 			} while (pmlmeinfo->dialogToken == 0);
 			pframe = rtw_set_fixed_ie(pframe, 1, &(pmlmeinfo->dialogToken), &(pattrib->pktlen));
 
-			BA_para_set = (0x1002 | ((status & 0xf) << 2)); /* immediate ack & 64 buffer size */
+			BA_para_set = 0x1002 | ((status & 0xf) << 2); /* immediate ack & 64 buffer size */
 			le_tmp = cpu_to_le16(BA_para_set);
 			pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)(&(le_tmp)), &(pattrib->pktlen));
 
@@ -3972,8 +3972,8 @@ void start_clnt_join(struct adapter *padapter)
 		/* and enable a timer */
 		beacon_timeout = decide_wait_for_beacon_timeout(pmlmeinfo->bcn_interval);
 		set_link_timer(pmlmeext, beacon_timeout);
-		_set_timer(&padapter->mlmepriv.assoc_timer,
-			   (REAUTH_TO * REAUTH_LIMIT) + (REASSOC_TO*REASSOC_LIMIT) + beacon_timeout);
+		mod_timer(&padapter->mlmepriv.assoc_timer, jiffies +
+			  msecs_to_jiffies((REAUTH_TO * REAUTH_LIMIT) + (REASSOC_TO * REASSOC_LIMIT) + beacon_timeout));
 
 		pmlmeinfo->state = WIFI_FW_AUTH_NULL | WIFI_FW_STATION_STATE;
 	} else if (caps&cap_IBSS) { /* adhoc client */
@@ -4247,7 +4247,7 @@ void report_survey_event(struct adapter *padapter,
 	if (pcmd_obj == NULL)
 		return;
 
-	cmdsz = (sizeof(struct survey_event) + sizeof(struct C2HEvent_Header));
+	cmdsz = sizeof(struct survey_event) + sizeof(struct C2HEvent_Header);
 	pevtcmd = kzalloc(cmdsz, GFP_ATOMIC);
 	if (pevtcmd == NULL) {
 		kfree(pcmd_obj);
@@ -4299,7 +4299,7 @@ void report_surveydone_event(struct adapter *padapter)
 	if (pcmd_obj == NULL)
 		return;
 
-	cmdsz = (sizeof(struct surveydone_event) + sizeof(struct C2HEvent_Header));
+	cmdsz = sizeof(struct surveydone_event) + sizeof(struct C2HEvent_Header);
 	pevtcmd = kzalloc(cmdsz, GFP_KERNEL);
 	if (pevtcmd == NULL) {
 		kfree(pcmd_obj);
@@ -4345,7 +4345,7 @@ void report_join_res(struct adapter *padapter, int res)
 	if (pcmd_obj == NULL)
 		return;
 
-	cmdsz = (sizeof(struct joinbss_event) + sizeof(struct C2HEvent_Header));
+	cmdsz = sizeof(struct joinbss_event) + sizeof(struct C2HEvent_Header);
 	pevtcmd = kzalloc(cmdsz, GFP_ATOMIC);
 	if (pevtcmd == NULL) {
 		kfree(pcmd_obj);
@@ -4398,7 +4398,7 @@ void report_del_sta_event(struct adapter *padapter, unsigned char *MacAddr, unsi
 	if (pcmd_obj == NULL)
 		return;
 
-	cmdsz = (sizeof(struct stadel_event) + sizeof(struct C2HEvent_Header));
+	cmdsz = sizeof(struct stadel_event) + sizeof(struct C2HEvent_Header);
 	pevtcmd = kzalloc(cmdsz, GFP_KERNEL);
 	if (pevtcmd == NULL) {
 		kfree(pcmd_obj);
@@ -4428,7 +4428,7 @@ void report_del_sta_event(struct adapter *padapter, unsigned char *MacAddr, unsi
 	if (psta)
 		mac_id = (int)psta->mac_id;
 	else
-		mac_id = (-1);
+		mac_id = -1;
 
 	pdel_sta_evt->mac_id = mac_id;
 
@@ -4453,7 +4453,7 @@ void report_add_sta_event(struct adapter *padapter, unsigned char *MacAddr, int
 	if (pcmd_obj == NULL)
 		return;
 
-	cmdsz = (sizeof(struct stassoc_event) + sizeof(struct C2HEvent_Header));
+	cmdsz = sizeof(struct stassoc_event) + sizeof(struct C2HEvent_Header);
 	pevtcmd = kzalloc(cmdsz, GFP_KERNEL);
 	if (pevtcmd == NULL) {
 		kfree(pcmd_obj);
@@ -4835,9 +4835,9 @@ void linked_status_chk(struct adapter *padapter)
 	}
 }
 
-void survey_timer_hdl(void *function_context)
+void survey_timer_hdl(unsigned long data)
 {
-	struct adapter *padapter = function_context;
+	struct adapter *padapter = (struct adapter *)data;
 	struct cmd_obj	*ph2c;
 	struct sitesurvey_parm	*psurveyPara;
 	struct cmd_priv					*pcmdpriv = &padapter->cmdpriv;
@@ -4875,9 +4875,9 @@ exit_survey_timer_hdl:
 	return;
 }
 
-void link_timer_hdl(void *function_context)
+void link_timer_hdl(unsigned long data)
 {
-	struct adapter *padapter = (struct adapter *)function_context;
+	struct adapter *padapter = (struct adapter *)data;
 	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
 
@@ -4912,9 +4912,9 @@ void link_timer_hdl(void *function_context)
 	return;
 }
 
-void addba_timer_hdl(void *function_context)
+void addba_timer_hdl(unsigned long data)
 {
-	struct sta_info *psta = function_context;
+	struct sta_info *psta = (struct sta_info *)data;
 	struct ht_priv	*phtpriv;
 
 	if (!psta)
@@ -5356,7 +5356,7 @@ u8 set_stakey_hdl(struct adapter *padapter, u8 *pbuf)
 
 		psta = rtw_get_stainfo(pstapriv, pparm->addr);
 		if (psta) {
-			ctrl = (BIT(15) | ((pparm->algorithm) << 2));
+			ctrl = BIT(15) | ((pparm->algorithm) << 2);
 
 			DBG_88E("r871x_set_stakey_hdl(): enc_algorithm=%d\n", pparm->algorithm);
 
@@ -5365,7 +5365,7 @@ u8 set_stakey_hdl(struct adapter *padapter, u8 *pbuf)
 				return H2C_REJECTED;
 			}
 
-			cam_id = (psta->mac_id + 3);/* 0~3 for default key, cmd_id = macid + 3, macid = aid+1; */
+			cam_id = psta->mac_id + 3;/* 0~3 for default key, cmd_id = macid + 3, macid = aid+1; */
 
 			DBG_88E("Write CAM, mac_addr =%x:%x:%x:%x:%x:%x, cam_entry=%d\n", pparm->addr[0],
 				pparm->addr[1], pparm->addr[2], pparm->addr[3], pparm->addr[4],
@@ -5406,7 +5406,8 @@ u8 add_ba_hdl(struct adapter *padapter, unsigned char *pbuf)
 	if (((pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS) && (pmlmeinfo->HT_enable)) ||
 	    ((pmlmeinfo->state&0x03) == WIFI_FW_AP_STATE)) {
 		issue_action_BA(padapter, pparm->addr, RTW_WLAN_ACTION_ADDBA_REQ, (u16)pparm->tid);
-		_set_timer(&psta->addba_retry_timer, ADDBA_TO);
+		mod_timer(&psta->addba_retry_timer,
+			  jiffies + msecs_to_jiffies(ADDBA_TO));
 	} else {
 		psta->htpriv.candidate_tid_bitmap &= ~BIT(pparm->tid);
 	}
@@ -5430,15 +5431,14 @@ u8 set_tx_beacon_cmd(struct adapter *padapter)
 		goto exit;
 	}
 
-	ptxBeacon_parm = kzalloc(sizeof(struct wlan_bssid_ex), GFP_KERNEL);
+	ptxBeacon_parm = kmemdup(&(pmlmeinfo->network),
+				sizeof(struct wlan_bssid_ex), GFP_KERNEL);
 	if (ptxBeacon_parm == NULL) {
 		kfree(ph2c);
 		res = _FAIL;
 		goto exit;
 	}
 
-	memcpy(ptxBeacon_parm, &(pmlmeinfo->network), sizeof(struct wlan_bssid_ex));
-
 	len_diff = update_hidden_ssid(ptxBeacon_parm->IEs+_BEACON_IE_OFFSET_,
 				      ptxBeacon_parm->IELength-_BEACON_IE_OFFSET_,
 				      pmlmeinfo->hidden_ssid_mode);
diff --git a/drivers/staging/rtl8188eu/core/rtw_pwrctrl.c b/drivers/staging/rtl8188eu/core/rtw_pwrctrl.c
index df463a29b641..ec0a8a4cdc6e 100644
--- a/drivers/staging/rtl8188eu/core/rtw_pwrctrl.c
+++ b/drivers/staging/rtl8188eu/core/rtw_pwrctrl.c
@@ -281,9 +281,9 @@ exit:
 	pwrpriv->ps_processing = false;
 }
 
-static void pwr_state_check_handler(void *FunctionContext)
+static void pwr_state_check_handler(unsigned long data)
 {
-	struct adapter *padapter = FunctionContext;
+	struct adapter *padapter = (struct adapter *)data;
 	rtw_ps_cmd(padapter);
 }
 
@@ -544,7 +544,9 @@ void rtw_init_pwrctrl_priv(struct adapter *padapter)
 
 	pwrctrlpriv->btcoex_rfon = false;
 
-	_init_timer(&(pwrctrlpriv->pwr_state_check_timer), padapter->pnetdev, pwr_state_check_handler, (u8 *)padapter);
+	setup_timer(&pwrctrlpriv->pwr_state_check_timer,
+		    pwr_state_check_handler,
+		    (unsigned long)padapter);
 }
 
 inline void rtw_set_ips_deny(struct adapter *padapter, u32 ms)
diff --git a/drivers/staging/rtl8188eu/core/rtw_recv.c b/drivers/staging/rtl8188eu/core/rtw_recv.c
index bd79e9e7105a..cda725a8f9cd 100644
--- a/drivers/staging/rtl8188eu/core/rtw_recv.c
+++ b/drivers/staging/rtl8188eu/core/rtw_recv.c
@@ -41,7 +41,7 @@ static u8 rtw_rfc1042_header[] = {
        0xaa, 0xaa, 0x03, 0x00, 0x00, 0x00
 };
 
-void rtw_signal_stat_timer_hdl(RTW_TIMER_HDL_ARGS);
+void rtw_signal_stat_timer_hdl(unsigned long data);
 
 void _rtw_init_sta_recv_priv(struct sta_recv_priv *psta_recvpriv)
 {
@@ -98,7 +98,9 @@ int _rtw_init_recv_priv(struct recv_priv *precvpriv, struct adapter *padapter)
 
 	res = rtw_hal_init_recv_priv(padapter);
 
-	_init_timer(&precvpriv->signal_stat_timer, padapter->pnetdev, RTW_TIMER_HDL_NAME(signal_stat), padapter);
+	setup_timer(&precvpriv->signal_stat_timer,
+		    rtw_signal_stat_timer_hdl,
+		    (unsigned long)padapter);
 
 	precvpriv->signal_stat_sampling_interval = 1000; /* ms */
 
@@ -1927,7 +1929,8 @@ static int recv_indicatepkt_reorder(struct adapter *padapter,
 
 	/* recv_indicatepkts_in_order(padapter, preorder_ctrl, true); */
 	if (recv_indicatepkts_in_order(padapter, preorder_ctrl, false)) {
-		_set_timer(&preorder_ctrl->reordering_ctrl_timer, REORDER_WAIT_TIME);
+		mod_timer(&preorder_ctrl->reordering_ctrl_timer,
+			  jiffies + msecs_to_jiffies(REORDER_WAIT_TIME));
 		spin_unlock_bh(&ppending_recvframe_queue->lock);
 	} else {
 		spin_unlock_bh(&ppending_recvframe_queue->lock);
@@ -1945,9 +1948,9 @@ _err_exit:
 	return _FAIL;
 }
 
-void rtw_reordering_ctrl_timeout_handler(void *pcontext)
+void rtw_reordering_ctrl_timeout_handler(unsigned long data)
 {
-	struct recv_reorder_ctrl *preorder_ctrl = pcontext;
+	struct recv_reorder_ctrl *preorder_ctrl = (struct recv_reorder_ctrl *)data;
 	struct adapter *padapter = preorder_ctrl->padapter;
 	struct __queue *ppending_recvframe_queue = &preorder_ctrl->pending_recvframe_queue;
 
@@ -1957,7 +1960,8 @@ void rtw_reordering_ctrl_timeout_handler(void *pcontext)
 	spin_lock_bh(&ppending_recvframe_queue->lock);
 
 	if (recv_indicatepkts_in_order(padapter, preorder_ctrl, true) == true)
-		_set_timer(&preorder_ctrl->reordering_ctrl_timer, REORDER_WAIT_TIME);
+		mod_timer(&preorder_ctrl->reordering_ctrl_timer,
+			  jiffies + msecs_to_jiffies(REORDER_WAIT_TIME));
 
 	spin_unlock_bh(&ppending_recvframe_queue->lock);
 }
@@ -2130,9 +2134,9 @@ _recv_entry_drop:
 	return ret;
 }
 
-void rtw_signal_stat_timer_hdl(RTW_TIMER_HDL_ARGS)
+void rtw_signal_stat_timer_hdl(unsigned long data)
 {
-	struct adapter *adapter = (struct adapter *)FunctionContext;
+	struct adapter *adapter = (struct adapter *)data;
 	struct recv_priv *recvpriv = &adapter->recvpriv;
 
 	u32 tmp_s, tmp_q;
@@ -2159,7 +2163,7 @@ void rtw_signal_stat_timer_hdl(RTW_TIMER_HDL_ARGS)
 
 		/* update value of signal_strength, rssi, signal_qual */
 		if (check_fwstate(&adapter->mlmepriv, _FW_UNDER_SURVEY) == false) {
-			tmp_s = (avg_signal_strength+(_alpha-1)*recvpriv->signal_strength);
+			tmp_s = avg_signal_strength+(_alpha-1)*recvpriv->signal_strength;
 			if (tmp_s % _alpha)
 				tmp_s = tmp_s/_alpha + 1;
 			else
@@ -2167,7 +2171,7 @@ void rtw_signal_stat_timer_hdl(RTW_TIMER_HDL_ARGS)
 			if (tmp_s > 100)
 				tmp_s = 100;
 
-			tmp_q = (avg_signal_qual+(_alpha-1)*recvpriv->signal_qual);
+			tmp_q = avg_signal_qual+(_alpha-1)*recvpriv->signal_qual;
 			if (tmp_q % _alpha)
 				tmp_q = tmp_q/_alpha + 1;
 			else
diff --git a/drivers/staging/rtl8188eu/core/rtw_security.c b/drivers/staging/rtl8188eu/core/rtw_security.c
index bd8d60a230e9..d870a5ce8585 100644
--- a/drivers/staging/rtl8188eu/core/rtw_security.c
+++ b/drivers/staging/rtl8188eu/core/rtw_security.c
@@ -854,7 +854,7 @@ static void mix_column(u8 *in, u8 *out)
 	u8 add1b[4];
 	u8 add1bf7[4];
 	u8 rotl[4];
-	u8 swap_halfs[4];
+	u8 swap_halves[4];
 	u8 andf7[4];
 	u8 rotr[4];
 	u8 temp[4];
@@ -866,10 +866,10 @@ static void mix_column(u8 *in, u8 *out)
 			add1b[i] = 0x00;
 	}
 
-	swap_halfs[0] = in[2];    /* Swap halves */
-	swap_halfs[1] = in[3];
-	swap_halfs[2] = in[0];
-	swap_halfs[3] = in[1];
+	swap_halves[0] = in[2];    /* Swap halves */
+	swap_halves[1] = in[3];
+	swap_halves[2] = in[0];
+	swap_halves[3] = in[1];
 
 	rotl[0] = in[3];	/* Rotate left 8 bits */
 	rotl[1] = in[0];
@@ -900,7 +900,7 @@ static void mix_column(u8 *in, u8 *out)
 	rotr[3] = temp[0];
 
 	xor_32(add1bf7, rotr, temp);
-	xor_32(swap_halfs, rotl, tempb);
+	xor_32(swap_halves, rotl, tempb);
 	xor_32(temp, tempb, out);
 }
 
@@ -1079,7 +1079,7 @@ static int aes_cipher(u8 *key, uint hdrlen, u8 *pframe, uint plen)
 	uint	frtype  = GetFrameType(pframe);
 	uint	frsubtype  = GetFrameSubType(pframe);
 
-	frsubtype = frsubtype>>4;
+	frsubtype >>= 4;
 
 	memset((void *)mic_iv, 0, 16);
 	memset((void *)mic_header1, 0, 16);
@@ -1122,7 +1122,7 @@ static int aes_cipher(u8 *key, uint hdrlen, u8 *pframe, uint plen)
 	num_blocks = plen / 16;
 
 	/* Find start of payload */
-	payload_index = (hdrlen + 8);
+	payload_index = hdrlen + 8;
 
 	/* Calculate MIC */
 	aes128k128d(key, mic_iv, aes_out);
@@ -1277,7 +1277,7 @@ static int aes_decipher(u8 *key, uint	hdrlen,
 /*	uint	offset = 0; */
 	uint	frtype  = GetFrameType(pframe);
 	uint	frsubtype  = GetFrameSubType(pframe);
-	frsubtype = frsubtype>>4;
+	frsubtype >>= 4;
 
 	memset((void *)mic_iv, 0, 16);
 	memset((void *)mic_header1, 0, 16);
@@ -1366,7 +1366,7 @@ static int aes_decipher(u8 *key, uint	hdrlen,
 	num_blocks = (plen-8) / 16;
 
 	/* Find start of payload */
-	payload_index = (hdrlen + 8);
+	payload_index = hdrlen + 8;
 
 	/* Calculate MIC */
 	aes128k128d(key, mic_iv, aes_out);
diff --git a/drivers/staging/rtl8188eu/core/rtw_sreset.c b/drivers/staging/rtl8188eu/core/rtw_sreset.c
index cd4e344e6ffd..e725a4708775 100644
--- a/drivers/staging/rtl8188eu/core/rtw_sreset.c
+++ b/drivers/staging/rtl8188eu/core/rtw_sreset.c
@@ -47,7 +47,7 @@ u8 sreset_get_wifi_status(struct adapter *padapter)
 
 	if (WIFI_STATUS_SUCCESS != psrtpriv->Wifi_Error_Status) {
 		DBG_88E("==>%s error_status(0x%x)\n", __func__, psrtpriv->Wifi_Error_Status);
-		status = (psrtpriv->Wifi_Error_Status & (~(USB_READ_PORT_FAIL|USB_WRITE_PORT_FAIL)));
+		status = psrtpriv->Wifi_Error_Status & (~(USB_READ_PORT_FAIL|USB_WRITE_PORT_FAIL));
 	}
 	DBG_88E("==> %s wifi_status(0x%x)\n", __func__, status);
 
diff --git a/drivers/staging/rtl8188eu/core/rtw_wlan_util.c b/drivers/staging/rtl8188eu/core/rtw_wlan_util.c
index a3ffc691be9a..2b371757cbfe 100644
--- a/drivers/staging/rtl8188eu/core/rtw_wlan_util.c
+++ b/drivers/staging/rtl8188eu/core/rtw_wlan_util.c
@@ -88,35 +88,25 @@ int cckratesonly_included(unsigned char *rate, int ratelen)
 
 unsigned char networktype_to_raid(unsigned char network_type)
 {
-	unsigned char raid;
-
 	switch (network_type) {
 	case WIRELESS_11B:
-		raid = RATR_INX_WIRELESS_B;
-		break;
+		return RATR_INX_WIRELESS_B;
 	case WIRELESS_11A:
 	case WIRELESS_11G:
-		raid = RATR_INX_WIRELESS_G;
-		break;
+		return RATR_INX_WIRELESS_G;
 	case WIRELESS_11BG:
-		raid = RATR_INX_WIRELESS_GB;
-		break;
+		return RATR_INX_WIRELESS_GB;
 	case WIRELESS_11_24N:
 	case WIRELESS_11_5N:
-		raid = RATR_INX_WIRELESS_N;
-		break;
+		return RATR_INX_WIRELESS_N;
 	case WIRELESS_11A_5N:
 	case WIRELESS_11G_24N:
-		raid = RATR_INX_WIRELESS_NG;
-		break;
+		return  RATR_INX_WIRELESS_NG;
 	case WIRELESS_11BG_24N:
-		raid = RATR_INX_WIRELESS_NGB;
-		break;
+		return RATR_INX_WIRELESS_NGB;
 	default:
-		raid = RATR_INX_WIRELESS_GB;
-		break;
+		return RATR_INX_WIRELESS_GB;
 	}
-	return raid;
 }
 
 u8 judge_network_type(struct adapter *padapter, unsigned char *rate, int ratelen)
@@ -146,47 +136,34 @@ u8 judge_network_type(struct adapter *padapter, unsigned char *rate, int ratelen
 
 static unsigned char ratetbl_val_2wifirate(unsigned char rate)
 {
-	unsigned char val = 0;
-
 	switch (rate & 0x7f) {
 	case 0:
-		val = IEEE80211_CCK_RATE_1MB;
-		break;
+		return IEEE80211_CCK_RATE_1MB;
 	case 1:
-		val = IEEE80211_CCK_RATE_2MB;
-		break;
+		return IEEE80211_CCK_RATE_2MB;
 	case 2:
-		val = IEEE80211_CCK_RATE_5MB;
-		break;
+		return IEEE80211_CCK_RATE_5MB;
 	case 3:
-		val = IEEE80211_CCK_RATE_11MB;
-		break;
+		return IEEE80211_CCK_RATE_11MB;
 	case 4:
-		val = IEEE80211_OFDM_RATE_6MB;
-		break;
+		return IEEE80211_OFDM_RATE_6MB;
 	case 5:
-		val = IEEE80211_OFDM_RATE_9MB;
-		break;
+		return IEEE80211_OFDM_RATE_9MB;
 	case 6:
-		val = IEEE80211_OFDM_RATE_12MB;
-		break;
+		return IEEE80211_OFDM_RATE_12MB;
 	case 7:
-		val = IEEE80211_OFDM_RATE_18MB;
-		break;
+		return IEEE80211_OFDM_RATE_18MB;
 	case 8:
-		val = IEEE80211_OFDM_RATE_24MB;
-		break;
+		return IEEE80211_OFDM_RATE_24MB;
 	case 9:
-		val = IEEE80211_OFDM_RATE_36MB;
-		break;
+		return IEEE80211_OFDM_RATE_36MB;
 	case 10:
-		val = IEEE80211_OFDM_RATE_48MB;
-		break;
+		return IEEE80211_OFDM_RATE_48MB;
 	case 11:
-		val = IEEE80211_OFDM_RATE_54MB;
-		break;
+		return IEEE80211_OFDM_RATE_54MB;
+	default:
+		return 0;
 	}
-	return val;
 }
 
 static int is_basicrate(struct adapter *padapter, unsigned char rate)
@@ -485,14 +462,14 @@ void write_cam(struct adapter *padapter, u8 entry, u16 ctrl, u8 *mac, u8 *key)
 	for (j = 5; j >= 0; j--) {
 		switch (j) {
 		case 0:
-			val = (ctrl | (mac[0] << 16) | (mac[1] << 24));
+			val = ctrl | (mac[0] << 16) | (mac[1] << 24);
 			break;
 		case 1:
-			val = (mac[2] | (mac[3] << 8) | (mac[4] << 16) | (mac[5] << 24));
+			val = mac[2] | (mac[3] << 8) | (mac[4] << 16) | (mac[5] << 24);
 			break;
 		default:
 			i = (j - 2) << 2;
-			val = (key[i] | (key[i+1] << 8) | (key[i+2] << 16) | (key[i+3] << 24));
+			val = key[i] | (key[i+1] << 8) | (key[i+2] << 16) | (key[i+3] << 24);
 			break;
 		}
 
@@ -587,7 +564,7 @@ void WMMOnAssocRsp(struct adapter *padapter)
 		/* AIFS = AIFSN * slot time + SIFS - r2t phy delay */
 		AIFS = (pmlmeinfo->WMM_param.ac_param[i].ACI_AIFSN & 0x0f) * pmlmeinfo->slotTime + aSifsTime;
 
-		ECWMin = (pmlmeinfo->WMM_param.ac_param[i].CW & 0x0f);
+		ECWMin = pmlmeinfo->WMM_param.ac_param[i].CW & 0x0f;
 		ECWMax = (pmlmeinfo->WMM_param.ac_param[i].CW & 0xf0) >> 4;
 		TXOP = le16_to_cpu(pmlmeinfo->WMM_param.ac_param[i].TXOP_limit);
 
@@ -659,8 +636,6 @@ void WMMOnAssocRsp(struct adapter *padapter)
 		pxmitpriv->wmm_para_seq[i] = inx[i];
 		DBG_88E("wmm_para_seq(%d): %d\n", i, pxmitpriv->wmm_para_seq[i]);
 	}
-
-	return;
 }
 
 static void bwmode_update_check(struct adapter *padapter, struct ndis_802_11_var_ie *pIE)
@@ -766,14 +741,14 @@ void HT_caps_handler(struct adapter *padapter, struct ndis_802_11_var_ie *pIE)
 		} else {
 			/* modify from  fw by Thomas 2010/11/17 */
 			if ((pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para & 0x3) > (pIE->data[i] & 0x3))
-				max_AMPDU_len = (pIE->data[i] & 0x3);
+				max_AMPDU_len = pIE->data[i] & 0x3;
 			else
-				max_AMPDU_len = (pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para & 0x3);
+				max_AMPDU_len = pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para & 0x3;
 
 			if ((pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para & 0x1c) > (pIE->data[i] & 0x1c))
-				min_MPDU_spacing = (pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para & 0x1c);
+				min_MPDU_spacing = pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para & 0x1c;
 			else
-				min_MPDU_spacing = (pIE->data[i] & 0x1c);
+				min_MPDU_spacing = pIE->data[i] & 0x1c;
 
 			pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para = max_AMPDU_len | min_MPDU_spacing;
 		}
@@ -788,7 +763,6 @@ void HT_caps_handler(struct adapter *padapter, struct ndis_802_11_var_ie *pIE)
 		else
 			pmlmeinfo->HT_caps.u.HT_cap_element.MCS_rate[i] &= MCS_rate_2R[i];
 	}
-	return;
 }
 
 void HT_info_handler(struct adapter *padapter, struct ndis_802_11_var_ie *pIE)
@@ -809,7 +783,6 @@ void HT_info_handler(struct adapter *padapter, struct ndis_802_11_var_ie *pIE)
 
 	pmlmeinfo->HT_info_enable = 1;
 	memcpy(&(pmlmeinfo->HT_info), pIE->data, pIE->Length);
-	return;
 }
 
 void HTOnAssocRsp(struct adapter *padapter)
@@ -1221,48 +1194,36 @@ unsigned int is_ap_in_wep(struct adapter *padapter)
 
 static int wifirate2_ratetbl_inx(unsigned char rate)
 {
-	int	inx = 0;
 	rate = rate & 0x7f;
 
 	switch (rate) {
 	case 54*2:
-		inx = 11;
-		break;
+		return 11;
 	case 48*2:
-		inx = 10;
-		break;
+		return 10;
 	case 36*2:
-		inx = 9;
-		break;
+		return 9;
 	case 24*2:
-		inx = 8;
-		break;
+		return 8;
 	case 18*2:
-		inx = 7;
-		break;
+		return 7;
 	case 12*2:
-		inx = 6;
-		break;
+		return 6;
 	case 9*2:
-		inx = 5;
-		break;
+		return 5;
 	case 6*2:
-		inx = 4;
-		break;
+		return 4;
 	case 11*2:
-		inx = 3;
-		break;
+		return 3;
 	case 11:
-		inx = 2;
-		break;
+		return 2;
 	case 2*2:
-		inx = 1;
-		break;
+		return 1;
 	case 1*2:
-		inx = 0;
-		break;
+		return 0;
+	default:
+		return 0;
 	}
-	return inx;
 }
 
 unsigned int update_basic_rate(unsigned char *ptn, unsigned int ptn_sz)
@@ -1295,7 +1256,7 @@ unsigned int update_MSC_rate(struct HT_caps_element *pHT_caps)
 {
 	unsigned int mask = 0;
 
-	mask = ((pHT_caps->u.HT_cap_element.MCS_rate[0] << 12) | (pHT_caps->u.HT_cap_element.MCS_rate[1] << 20));
+	mask = (pHT_caps->u.HT_cap_element.MCS_rate[0] << 12) | (pHT_caps->u.HT_cap_element.MCS_rate[1] << 20);
 
 	return mask;
 }
@@ -1309,7 +1270,7 @@ int support_short_GI(struct adapter *padapter, struct HT_caps_element *pHT_caps)
 	if (!(pmlmeinfo->HT_enable))
 		return _FAIL;
 
-	if ((pmlmeinfo->assoc_AP_vendor == HT_IOT_PEER_RALINK))
+	if (pmlmeinfo->assoc_AP_vendor == HT_IOT_PEER_RALINK)
 		return _FAIL;
 
 	bit_offset = (pmlmeext->cur_bwmode & HT_CHANNEL_WIDTH_40) ? 6 : 5;
diff --git a/drivers/staging/rtl8188eu/core/rtw_xmit.c b/drivers/staging/rtl8188eu/core/rtw_xmit.c
index 7a71df167464..fda169d37771 100644
--- a/drivers/staging/rtl8188eu/core/rtw_xmit.c
+++ b/drivers/staging/rtl8188eu/core/rtw_xmit.c
@@ -1634,23 +1634,15 @@ void rtw_alloc_hwxmits(struct adapter *padapter)
 
 	pxmitpriv->hwxmit_entry = HWXMIT_ENTRY;
 
-	pxmitpriv->hwxmits = kzalloc(sizeof(struct hw_xmit) * pxmitpriv->hwxmit_entry, GFP_KERNEL);
+	pxmitpriv->hwxmits = kcalloc(pxmitpriv->hwxmit_entry,
+				     sizeof(struct hw_xmit), GFP_KERNEL);
 
 	hwxmits = pxmitpriv->hwxmits;
 
-	if (pxmitpriv->hwxmit_entry == 5) {
-		hwxmits[0] .sta_queue = &pxmitpriv->bm_pending;
-		hwxmits[1] .sta_queue = &pxmitpriv->vo_pending;
-		hwxmits[2] .sta_queue = &pxmitpriv->vi_pending;
-		hwxmits[3] .sta_queue = &pxmitpriv->bk_pending;
-		hwxmits[4] .sta_queue = &pxmitpriv->be_pending;
-	} else if (pxmitpriv->hwxmit_entry == 4) {
-		hwxmits[0] .sta_queue = &pxmitpriv->vo_pending;
-		hwxmits[1] .sta_queue = &pxmitpriv->vi_pending;
-		hwxmits[2] .sta_queue = &pxmitpriv->be_pending;
-		hwxmits[3] .sta_queue = &pxmitpriv->bk_pending;
-	} else {
-	}
+	hwxmits[0] .sta_queue = &pxmitpriv->vo_pending;
+	hwxmits[1] .sta_queue = &pxmitpriv->vi_pending;
+	hwxmits[2] .sta_queue = &pxmitpriv->be_pending;
+	hwxmits[3] .sta_queue = &pxmitpriv->bk_pending;
 }
 
 void rtw_free_hwxmits(struct adapter *padapter)
diff --git a/drivers/staging/rtl8188eu/hal/Hal8188ERateAdaptive.c b/drivers/staging/rtl8188eu/hal/Hal8188ERateAdaptive.c
index 3c651d5c6824..082f0ca198ef 100644
--- a/drivers/staging/rtl8188eu/hal/Hal8188ERateAdaptive.c
+++ b/drivers/staging/rtl8188eu/hal/Hal8188ERateAdaptive.c
@@ -115,17 +115,21 @@ static void odm_SetTxRPTTiming_8188E(
 	}
 	pRaInfo->RptTime = DynamicTxRPTTiming[idx];
 
-	ODM_RT_TRACE(dm_odm, ODM_COMP_RATE_ADAPTIVE, ODM_DBG_LOUD, ("pRaInfo->RptTime = 0x%x\n", pRaInfo->RptTime));
+	ODM_RT_TRACE(dm_odm, ODM_COMP_RATE_ADAPTIVE, ODM_DBG_LOUD,
+			("pRaInfo->RptTime = 0x%x\n", pRaInfo->RptTime));
 }
 
-static int odm_RateDown_8188E(struct odm_dm_struct *dm_odm, struct odm_ra_info *pRaInfo)
+static int odm_RateDown_8188E(struct odm_dm_struct *dm_odm,
+				struct odm_ra_info *pRaInfo)
 {
 	u8 RateID, LowestRate, HighestRate;
 	u8 i;
 
-	ODM_RT_TRACE(dm_odm, ODM_COMP_RATE_ADAPTIVE, ODM_DBG_TRACE, ("=====>odm_RateDown_8188E()\n"));
+	ODM_RT_TRACE(dm_odm, ODM_COMP_RATE_ADAPTIVE,
+			ODM_DBG_TRACE, ("=====>odm_RateDown_8188E()\n"));
 	if (NULL == pRaInfo) {
-		ODM_RT_TRACE(dm_odm, ODM_COMP_RATE_ADAPTIVE, ODM_DBG_LOUD, ("odm_RateDown_8188E(): pRaInfo is NULL\n"));
+		ODM_RT_TRACE(dm_odm, ODM_COMP_RATE_ADAPTIVE, ODM_DBG_LOUD,
+				("odm_RateDown_8188E(): pRaInfo is NULL\n"));
 		return -1;
 	}
 	RateID = pRaInfo->PreRate;
@@ -167,10 +171,15 @@ RateDownFinish:
 
 	pRaInfo->DecisionRate = RateID;
 	odm_SetTxRPTTiming_8188E(dm_odm, pRaInfo, 2);
-	ODM_RT_TRACE(dm_odm, ODM_COMP_RATE_ADAPTIVE, ODM_DBG_LOUD, ("Rate down, RPT Timing default\n"));
-	ODM_RT_TRACE(dm_odm, ODM_COMP_RATE_ADAPTIVE, ODM_DBG_TRACE, ("RAWaitingCounter %d, RAPendingCounter %d", pRaInfo->RAWaitingCounter, pRaInfo->RAPendingCounter));
-	ODM_RT_TRACE(dm_odm, ODM_COMP_RATE_ADAPTIVE, ODM_DBG_LOUD, ("Rate down to RateID %d RateSGI %d\n", RateID, pRaInfo->RateSGI));
-	ODM_RT_TRACE(dm_odm, ODM_COMP_RATE_ADAPTIVE, ODM_DBG_TRACE, ("<===== odm_RateDown_8188E()\n"));
+	ODM_RT_TRACE(dm_odm, ODM_COMP_RATE_ADAPTIVE,
+			ODM_DBG_LOUD, ("Rate down, RPT Timing default\n"));
+	ODM_RT_TRACE(dm_odm, ODM_COMP_RATE_ADAPTIVE, ODM_DBG_TRACE,
+			("RAWaitingCounter %d, RAPendingCounter %d",
+			 pRaInfo->RAWaitingCounter, pRaInfo->RAPendingCounter));
+	ODM_RT_TRACE(dm_odm, ODM_COMP_RATE_ADAPTIVE, ODM_DBG_LOUD,
+			("Rate down to RateID %d RateSGI %d\n", RateID, pRaInfo->RateSGI));
+	ODM_RT_TRACE(dm_odm, ODM_COMP_RATE_ADAPTIVE, ODM_DBG_TRACE,
+			("<===== odm_RateDown_8188E()\n"));
 	return 0;
 }
 
@@ -182,9 +191,11 @@ static int odm_RateUp_8188E(
 	u8 RateID, HighestRate;
 	u8 i;
 
-	ODM_RT_TRACE(dm_odm, ODM_COMP_RATE_ADAPTIVE, ODM_DBG_TRACE, ("=====>odm_RateUp_8188E()\n"));
+	ODM_RT_TRACE(dm_odm, ODM_COMP_RATE_ADAPTIVE,
+			ODM_DBG_TRACE, ("=====>odm_RateUp_8188E()\n"));
 	if (NULL == pRaInfo) {
-		ODM_RT_TRACE(dm_odm, ODM_COMP_RATE_ADAPTIVE, ODM_DBG_LOUD, ("odm_RateUp_8188E(): pRaInfo is NULL\n"));
+		ODM_RT_TRACE(dm_odm, ODM_COMP_RATE_ADAPTIVE, ODM_DBG_LOUD,
+				("odm_RateUp_8188E(): pRaInfo is NULL\n"));
 		return -1;
 	}
 	RateID = pRaInfo->PreRate;
@@ -200,7 +211,8 @@ static int odm_RateUp_8188E(
 		goto RateUpfinish;
 	}
 	odm_SetTxRPTTiming_8188E(dm_odm, pRaInfo, 0);
-	ODM_RT_TRACE(dm_odm, ODM_COMP_RATE_ADAPTIVE, ODM_DBG_LOUD, ("odm_RateUp_8188E():Decrease RPT Timing\n"));
+	ODM_RT_TRACE(dm_odm, ODM_COMP_RATE_ADAPTIVE, ODM_DBG_LOUD,
+			("odm_RateUp_8188E():Decrease RPT Timing\n"));
 
 	if (RateID < HighestRate) {
 		for (i = RateID+1; i <= HighestRate; i++) {
@@ -218,15 +230,20 @@ static int odm_RateUp_8188E(
 		RateID = HighestRate;
 	}
 RateUpfinish:
-	if (pRaInfo->RAWaitingCounter == (4+PendingForRateUpFail[pRaInfo->RAPendingCounter]))
+	if (pRaInfo->RAWaitingCounter ==
+		(4+PendingForRateUpFail[pRaInfo->RAPendingCounter]))
 		pRaInfo->RAWaitingCounter = 0;
 	else
 		pRaInfo->RAWaitingCounter++;
 
 	pRaInfo->DecisionRate = RateID;
-	ODM_RT_TRACE(dm_odm, ODM_COMP_RATE_ADAPTIVE, ODM_DBG_LOUD, ("Rate up to RateID %d\n", RateID));
-	ODM_RT_TRACE(dm_odm, ODM_COMP_RATE_ADAPTIVE, ODM_DBG_TRACE, ("RAWaitingCounter %d, RAPendingCounter %d", pRaInfo->RAWaitingCounter, pRaInfo->RAPendingCounter));
-	ODM_RT_TRACE(dm_odm, ODM_COMP_RATE_ADAPTIVE, ODM_DBG_TRACE, ("<===== odm_RateUp_8188E()\n"));
+	ODM_RT_TRACE(dm_odm, ODM_COMP_RATE_ADAPTIVE, ODM_DBG_LOUD,
+			("Rate up to RateID %d\n", RateID));
+	ODM_RT_TRACE(dm_odm, ODM_COMP_RATE_ADAPTIVE, ODM_DBG_TRACE,
+			("RAWaitingCounter %d, RAPendingCounter %d",
+			 pRaInfo->RAWaitingCounter, pRaInfo->RAPendingCounter));
+	ODM_RT_TRACE(dm_odm, ODM_COMP_RATE_ADAPTIVE,
+			ODM_DBG_TRACE, ("<===== odm_RateUp_8188E()\n"));
 	return 0;
 }
 
@@ -243,11 +260,12 @@ static void odm_RateDecision_8188E(struct odm_dm_struct *dm_odm,
 		struct odm_ra_info *pRaInfo
 	)
 {
-	u8 RateID = 0, RtyPtID = 0, PenaltyID1 = 0, PenaltyID2 = 0;
+	u8 RateID = 0, RtyPtID = 0, PenaltyID1 = 0, PenaltyID2 = 0, i = 0;
 	/* u32 pool_retry; */
 	static u8 DynamicTxRPTTimingCounter;
 
-	ODM_RT_TRACE(dm_odm, ODM_COMP_RATE_ADAPTIVE, ODM_DBG_TRACE, ("=====>odm_RateDecision_8188E()\n"));
+	ODM_RT_TRACE(dm_odm, ODM_COMP_RATE_ADAPTIVE, ODM_DBG_TRACE,
+			("=====>odm_RateDecision_8188E()\n"));
 
 	if (pRaInfo->Active && (pRaInfo->TOTAL > 0)) { /*  STA used and data packet exits */
 		if ((pRaInfo->RssiStaRA < (pRaInfo->PreRssiStaRA - 3)) ||
@@ -268,14 +286,14 @@ static void odm_RateDecision_8188E(struct odm_dm_struct *dm_odm,
 
 		ODM_RT_TRACE(dm_odm, ODM_COMP_RATE_ADAPTIVE, ODM_DBG_TRACE,
 			     (" NscDown init is %d\n", pRaInfo->NscDown));
-		pRaInfo->NscDown += pRaInfo->RTY[0] * RETRY_PENALTY[PenaltyID1][0];
-		pRaInfo->NscDown += pRaInfo->RTY[1] * RETRY_PENALTY[PenaltyID1][1];
-		pRaInfo->NscDown += pRaInfo->RTY[2] * RETRY_PENALTY[PenaltyID1][2];
-		pRaInfo->NscDown += pRaInfo->RTY[3] * RETRY_PENALTY[PenaltyID1][3];
-		pRaInfo->NscDown += pRaInfo->RTY[4] * RETRY_PENALTY[PenaltyID1][4];
+
+		for (i = 0 ; i <= 4 ; i++)
+			pRaInfo->NscDown += pRaInfo->RTY[i] * RETRY_PENALTY[PenaltyID1][i];
+
 		ODM_RT_TRACE(dm_odm, ODM_COMP_RATE_ADAPTIVE, ODM_DBG_TRACE,
-			     (" NscDown is %d, total*penalty[5] is %d\n",
-			     pRaInfo->NscDown, (pRaInfo->TOTAL * RETRY_PENALTY[PenaltyID1][5])));
+			     (" NscDown is %d, total*penalty[5] is %d\n", pRaInfo->NscDown,
+			      (pRaInfo->TOTAL * RETRY_PENALTY[PenaltyID1][5])));
+
 		if (pRaInfo->NscDown > (pRaInfo->TOTAL * RETRY_PENALTY[PenaltyID1][5]))
 			pRaInfo->NscDown -= pRaInfo->TOTAL * RETRY_PENALTY[PenaltyID1][5];
 		else
@@ -285,14 +303,14 @@ static void odm_RateDecision_8188E(struct odm_dm_struct *dm_odm,
 		PenaltyID2 = RETRY_PENALTY_UP_IDX[RateID];
 		ODM_RT_TRACE(dm_odm, ODM_COMP_RATE_ADAPTIVE, ODM_DBG_TRACE,
 			     (" NscUp init is %d\n", pRaInfo->NscUp));
-		pRaInfo->NscUp += pRaInfo->RTY[0] * RETRY_PENALTY[PenaltyID2][0];
-		pRaInfo->NscUp += pRaInfo->RTY[1] * RETRY_PENALTY[PenaltyID2][1];
-		pRaInfo->NscUp += pRaInfo->RTY[2] * RETRY_PENALTY[PenaltyID2][2];
-		pRaInfo->NscUp += pRaInfo->RTY[3] * RETRY_PENALTY[PenaltyID2][3];
-		pRaInfo->NscUp += pRaInfo->RTY[4] * RETRY_PENALTY[PenaltyID2][4];
+
+		for (i = 0 ; i <= 4 ; i++)
+			pRaInfo->NscUp += pRaInfo->RTY[i] * RETRY_PENALTY[PenaltyID2][i];
+
 		ODM_RT_TRACE(dm_odm, ODM_COMP_RATE_ADAPTIVE, ODM_DBG_TRACE,
 			     ("NscUp is %d, total*up[5] is %d\n",
 			     pRaInfo->NscUp, (pRaInfo->TOTAL * RETRY_PENALTY[PenaltyID2][5])));
+
 		if (pRaInfo->NscUp > (pRaInfo->TOTAL * RETRY_PENALTY[PenaltyID2][5]))
 			pRaInfo->NscUp -= pRaInfo->TOTAL * RETRY_PENALTY[PenaltyID2][5];
 		else
@@ -487,7 +505,7 @@ static void odm_PTDecision_8188E(struct odm_ra_info *pRaInfo)
 			break;
 	}
 
-	j = j >> 1;
+	j >>= 1;
 	temp_stage = (pRaInfo->PTStage + 1) >> 1;
 	if (temp_stage > j)
 		stage_id = temp_stage-j;
@@ -538,6 +556,7 @@ int ODM_RAInfo_Init(struct odm_dm_struct *dm_odm, u8 macid)
 	struct odm_ra_info *pRaInfo = &dm_odm->RAInfo[macid];
 	u8 WirelessMode = 0xFF; /* invalid value */
 	u8 max_rate_idx = 0x13; /* MCS7 */
+
 	if (dm_odm->pWirelessMode != NULL)
 		WirelessMode = *(dm_odm->pWirelessMode);
 
@@ -618,7 +637,7 @@ u8 ODM_RA_GetDecisionRate_8188E(struct odm_dm_struct *dm_odm, u8 macid)
 
 	if ((NULL == dm_odm) || (macid >= ASSOCIATE_ENTRY_NUM))
 		return 0;
-	DecisionRate = (dm_odm->RAInfo[macid].DecisionRate);
+	DecisionRate = dm_odm->RAInfo[macid].DecisionRate;
 	ODM_RT_TRACE(dm_odm, ODM_COMP_RATE_ADAPTIVE, ODM_DBG_TRACE,
 		(" macid =%d DecisionRate = 0x%x\n", macid, DecisionRate));
 	return DecisionRate;
@@ -630,7 +649,7 @@ u8 ODM_RA_GetHwPwrStatus_8188E(struct odm_dm_struct *dm_odm, u8 macid)
 
 	if ((NULL == dm_odm) || (macid >= ASSOCIATE_ENTRY_NUM))
 		return 0;
-	PTStage = (dm_odm->RAInfo[macid].PTStage);
+	PTStage = dm_odm->RAInfo[macid].PTStage;
 	ODM_RT_TRACE(dm_odm, ODM_COMP_RATE_ADAPTIVE, ODM_DBG_TRACE,
 		     ("macid =%d PTStage = 0x%x\n", macid, PTStage));
 	return PTStage;
diff --git a/drivers/staging/rtl8188eu/hal/bb_cfg.c b/drivers/staging/rtl8188eu/hal/bb_cfg.c
index 1e963bf9e48b..8eb2b39a0b67 100644
--- a/drivers/staging/rtl8188eu/hal/bb_cfg.c
+++ b/drivers/staging/rtl8188eu/hal/bb_cfg.c
@@ -511,62 +511,71 @@ static u32 array_phy_reg_pg_8188e[] = {
 static void store_pwrindex_offset(struct adapter *Adapter, u32 regaddr, u32 bitmask, u32 data)
 {
 	struct hal_data_8188e *hal_data = GET_HAL_DATA(Adapter);
+	 u8 pwrGrpCnt = hal_data->pwrGroupCnt;
 
 	if (regaddr == rTxAGC_A_Rate18_06)
-		hal_data->MCSTxPowerLevelOriginalOffset[hal_data->pwrGroupCnt][0] = data;
+		hal_data->MCSTxPowerLevelOriginalOffset[pwrGrpCnt][0] = data;
 	if (regaddr == rTxAGC_A_Rate54_24)
-		hal_data->MCSTxPowerLevelOriginalOffset[hal_data->pwrGroupCnt][1] = data;
+		hal_data->MCSTxPowerLevelOriginalOffset[pwrGrpCnt][1] = data;
 	if (regaddr == rTxAGC_A_CCK1_Mcs32)
-		hal_data->MCSTxPowerLevelOriginalOffset[hal_data->pwrGroupCnt][6] = data;
+		hal_data->MCSTxPowerLevelOriginalOffset[pwrGrpCnt][6] = data;
 	if (regaddr == rTxAGC_B_CCK11_A_CCK2_11 && bitmask == 0xffffff00)
-		hal_data->MCSTxPowerLevelOriginalOffset[hal_data->pwrGroupCnt][7] = data;
+		hal_data->MCSTxPowerLevelOriginalOffset[pwrGrpCnt][7] = data;
 	if (regaddr == rTxAGC_A_Mcs03_Mcs00)
-		hal_data->MCSTxPowerLevelOriginalOffset[hal_data->pwrGroupCnt][2] = data;
+		hal_data->MCSTxPowerLevelOriginalOffset[pwrGrpCnt][2] = data;
 	if (regaddr == rTxAGC_A_Mcs07_Mcs04)
-		hal_data->MCSTxPowerLevelOriginalOffset[hal_data->pwrGroupCnt][3] = data;
+		hal_data->MCSTxPowerLevelOriginalOffset[pwrGrpCnt][3] = data;
 	if (regaddr == rTxAGC_A_Mcs11_Mcs08)
-		hal_data->MCSTxPowerLevelOriginalOffset[hal_data->pwrGroupCnt][4] = data;
+		hal_data->MCSTxPowerLevelOriginalOffset[pwrGrpCnt][4] = data;
 	if (regaddr == rTxAGC_A_Mcs15_Mcs12) {
-		hal_data->MCSTxPowerLevelOriginalOffset[hal_data->pwrGroupCnt][5] = data;
+		hal_data->MCSTxPowerLevelOriginalOffset[pwrGrpCnt][5] = data;
 		if (hal_data->rf_type == RF_1T1R)
 			hal_data->pwrGroupCnt++;
 	}
 	if (regaddr == rTxAGC_B_Rate18_06)
-		hal_data->MCSTxPowerLevelOriginalOffset[hal_data->pwrGroupCnt][8] = data;
+		hal_data->MCSTxPowerLevelOriginalOffset[pwrGrpCnt][8] = data;
 	if (regaddr == rTxAGC_B_Rate54_24)
-		hal_data->MCSTxPowerLevelOriginalOffset[hal_data->pwrGroupCnt][9] = data;
+		hal_data->MCSTxPowerLevelOriginalOffset[pwrGrpCnt][9] = data;
 	if (regaddr == rTxAGC_B_CCK1_55_Mcs32)
-		hal_data->MCSTxPowerLevelOriginalOffset[hal_data->pwrGroupCnt][14] = data;
+		hal_data->MCSTxPowerLevelOriginalOffset[pwrGrpCnt][14] = data;
 	if (regaddr == rTxAGC_B_CCK11_A_CCK2_11 && bitmask == 0x000000ff)
-		hal_data->MCSTxPowerLevelOriginalOffset[hal_data->pwrGroupCnt][15] = data;
+		hal_data->MCSTxPowerLevelOriginalOffset[pwrGrpCnt][15] = data;
 	if (regaddr == rTxAGC_B_Mcs03_Mcs00)
-		hal_data->MCSTxPowerLevelOriginalOffset[hal_data->pwrGroupCnt][10] = data;
+		hal_data->MCSTxPowerLevelOriginalOffset[pwrGrpCnt][10] = data;
 	if (regaddr == rTxAGC_B_Mcs07_Mcs04)
-		hal_data->MCSTxPowerLevelOriginalOffset[hal_data->pwrGroupCnt][11] = data;
+		hal_data->MCSTxPowerLevelOriginalOffset[pwrGrpCnt][11] = data;
 	if (regaddr == rTxAGC_B_Mcs11_Mcs08)
-		hal_data->MCSTxPowerLevelOriginalOffset[hal_data->pwrGroupCnt][12] = data;
+		hal_data->MCSTxPowerLevelOriginalOffset[pwrGrpCnt][12] = data;
 	if (regaddr == rTxAGC_B_Mcs15_Mcs12) {
-		hal_data->MCSTxPowerLevelOriginalOffset[hal_data->pwrGroupCnt][13] = data;
+		hal_data->MCSTxPowerLevelOriginalOffset[pwrGrpCnt][13] = data;
 		if (hal_data->rf_type != RF_1T1R)
 			hal_data->pwrGroupCnt++;
 	}
 }
 
-static void rtl_addr_delay(struct adapter *adapt, u32 addr, u32 bit_mask, u32 data)
+static void rtl_addr_delay(struct adapter *adapt,
+			u32 addr, u32 bit_mask, u32 data)
 {
-	if (addr == 0xfe) {
+	switch (addr) {
+	case 0xfe:
 		msleep(50);
-	} else if (addr == 0xfd) {
+		break;
+	case 0xfd:
 		mdelay(5);
-	} else if (addr == 0xfc) {
+		break;
+	case 0xfc:
 		mdelay(1);
-	} else if (addr == 0xfb) {
+		break;
+	case 0xfb:
 		udelay(50);
-	} else if (addr == 0xfa) {
+		break;
+	case 0xfa:
 		udelay(5);
-	} else if (addr == 0xf9) {
+		break;
+	case 0xf9:
 		udelay(1);
-	} else{
+		break;
+	default:
 		store_pwrindex_offset(adapt, addr, bit_mask, data);
 	}
 }
@@ -591,84 +600,90 @@ static bool config_bb_with_pgheader(struct adapter *adapt)
 static void rtl88e_phy_init_bb_rf_register_definition(struct adapter *Adapter)
 {
 	struct hal_data_8188e		*hal_data = GET_HAL_DATA(Adapter);
-
-	hal_data->PHYRegDef[RF_PATH_A].rfintfs = rFPGA0_XAB_RFInterfaceSW;
-	hal_data->PHYRegDef[RF_PATH_B].rfintfs = rFPGA0_XAB_RFInterfaceSW;
-	hal_data->PHYRegDef[RF_PATH_C].rfintfs = rFPGA0_XCD_RFInterfaceSW;
-	hal_data->PHYRegDef[RF_PATH_D].rfintfs = rFPGA0_XCD_RFInterfaceSW;
-
-	hal_data->PHYRegDef[RF_PATH_A].rfintfi = rFPGA0_XAB_RFInterfaceRB;
-	hal_data->PHYRegDef[RF_PATH_B].rfintfi = rFPGA0_XAB_RFInterfaceRB;
-	hal_data->PHYRegDef[RF_PATH_C].rfintfi = rFPGA0_XCD_RFInterfaceRB;
-	hal_data->PHYRegDef[RF_PATH_D].rfintfi = rFPGA0_XCD_RFInterfaceRB;
-
-	hal_data->PHYRegDef[RF_PATH_A].rfintfo = rFPGA0_XA_RFInterfaceOE;
-	hal_data->PHYRegDef[RF_PATH_B].rfintfo = rFPGA0_XB_RFInterfaceOE;
-
-	hal_data->PHYRegDef[RF_PATH_A].rfintfe = rFPGA0_XA_RFInterfaceOE;
-	hal_data->PHYRegDef[RF_PATH_B].rfintfe = rFPGA0_XB_RFInterfaceOE;
-
-	hal_data->PHYRegDef[RF_PATH_A].rf3wireOffset = rFPGA0_XA_LSSIParameter;
-	hal_data->PHYRegDef[RF_PATH_B].rf3wireOffset = rFPGA0_XB_LSSIParameter;
-
-	hal_data->PHYRegDef[RF_PATH_A].rfLSSI_Select = rFPGA0_XAB_RFParameter;
-	hal_data->PHYRegDef[RF_PATH_B].rfLSSI_Select = rFPGA0_XAB_RFParameter;
-	hal_data->PHYRegDef[RF_PATH_C].rfLSSI_Select = rFPGA0_XCD_RFParameter;
-	hal_data->PHYRegDef[RF_PATH_D].rfLSSI_Select = rFPGA0_XCD_RFParameter;
-
-	hal_data->PHYRegDef[RF_PATH_A].rfTxGainStage = rFPGA0_TxGainStage;
-	hal_data->PHYRegDef[RF_PATH_B].rfTxGainStage = rFPGA0_TxGainStage;
-	hal_data->PHYRegDef[RF_PATH_C].rfTxGainStage = rFPGA0_TxGainStage;
-	hal_data->PHYRegDef[RF_PATH_D].rfTxGainStage = rFPGA0_TxGainStage;
-
-	hal_data->PHYRegDef[RF_PATH_A].rfHSSIPara1 = rFPGA0_XA_HSSIParameter1;
-	hal_data->PHYRegDef[RF_PATH_B].rfHSSIPara1 = rFPGA0_XB_HSSIParameter1;
-
-	hal_data->PHYRegDef[RF_PATH_A].rfHSSIPara2 = rFPGA0_XA_HSSIParameter2;
-	hal_data->PHYRegDef[RF_PATH_B].rfHSSIPara2 = rFPGA0_XB_HSSIParameter2;
-
-	hal_data->PHYRegDef[RF_PATH_A].rfSwitchControl = rFPGA0_XAB_SwitchControl;
-	hal_data->PHYRegDef[RF_PATH_B].rfSwitchControl = rFPGA0_XAB_SwitchControl;
-	hal_data->PHYRegDef[RF_PATH_C].rfSwitchControl = rFPGA0_XCD_SwitchControl;
-	hal_data->PHYRegDef[RF_PATH_D].rfSwitchControl = rFPGA0_XCD_SwitchControl;
-
-	hal_data->PHYRegDef[RF_PATH_A].rfAGCControl1 = rOFDM0_XAAGCCore1;
-	hal_data->PHYRegDef[RF_PATH_B].rfAGCControl1 = rOFDM0_XBAGCCore1;
-	hal_data->PHYRegDef[RF_PATH_C].rfAGCControl1 = rOFDM0_XCAGCCore1;
-	hal_data->PHYRegDef[RF_PATH_D].rfAGCControl1 = rOFDM0_XDAGCCore1;
-
-	hal_data->PHYRegDef[RF_PATH_A].rfAGCControl2 = rOFDM0_XAAGCCore2;
-	hal_data->PHYRegDef[RF_PATH_B].rfAGCControl2 = rOFDM0_XBAGCCore2;
-	hal_data->PHYRegDef[RF_PATH_C].rfAGCControl2 = rOFDM0_XCAGCCore2;
-	hal_data->PHYRegDef[RF_PATH_D].rfAGCControl2 = rOFDM0_XDAGCCore2;
-
-	hal_data->PHYRegDef[RF_PATH_A].rfRxIQImbalance = rOFDM0_XARxIQImbalance;
-	hal_data->PHYRegDef[RF_PATH_B].rfRxIQImbalance = rOFDM0_XBRxIQImbalance;
-	hal_data->PHYRegDef[RF_PATH_C].rfRxIQImbalance = rOFDM0_XCRxIQImbalance;
-	hal_data->PHYRegDef[RF_PATH_D].rfRxIQImbalance = rOFDM0_XDRxIQImbalance;
-
-	hal_data->PHYRegDef[RF_PATH_A].rfRxAFE = rOFDM0_XARxAFE;
-	hal_data->PHYRegDef[RF_PATH_B].rfRxAFE = rOFDM0_XBRxAFE;
-	hal_data->PHYRegDef[RF_PATH_C].rfRxAFE = rOFDM0_XCRxAFE;
-	hal_data->PHYRegDef[RF_PATH_D].rfRxAFE = rOFDM0_XDRxAFE;
-
-	hal_data->PHYRegDef[RF_PATH_A].rfTxIQImbalance = rOFDM0_XATxIQImbalance;
-	hal_data->PHYRegDef[RF_PATH_B].rfTxIQImbalance = rOFDM0_XBTxIQImbalance;
-	hal_data->PHYRegDef[RF_PATH_C].rfTxIQImbalance = rOFDM0_XCTxIQImbalance;
-	hal_data->PHYRegDef[RF_PATH_D].rfTxIQImbalance = rOFDM0_XDTxIQImbalance;
-
-	hal_data->PHYRegDef[RF_PATH_A].rfTxAFE = rOFDM0_XATxAFE;
-	hal_data->PHYRegDef[RF_PATH_B].rfTxAFE = rOFDM0_XBTxAFE;
-	hal_data->PHYRegDef[RF_PATH_C].rfTxAFE = rOFDM0_XCTxAFE;
-	hal_data->PHYRegDef[RF_PATH_D].rfTxAFE = rOFDM0_XDTxAFE;
-
-	hal_data->PHYRegDef[RF_PATH_A].rfLSSIReadBack = rFPGA0_XA_LSSIReadBack;
-	hal_data->PHYRegDef[RF_PATH_B].rfLSSIReadBack = rFPGA0_XB_LSSIReadBack;
-	hal_data->PHYRegDef[RF_PATH_C].rfLSSIReadBack = rFPGA0_XC_LSSIReadBack;
-	hal_data->PHYRegDef[RF_PATH_D].rfLSSIReadBack = rFPGA0_XD_LSSIReadBack;
-
-	hal_data->PHYRegDef[RF_PATH_A].rfLSSIReadBackPi = TransceiverA_HSPI_Readback;
-	hal_data->PHYRegDef[RF_PATH_B].rfLSSIReadBackPi = TransceiverB_HSPI_Readback;
+	struct bb_reg_def               *reg[4];
+
+	reg[RF_PATH_A] = &(hal_data->PHYRegDef[RF_PATH_A]);
+	reg[RF_PATH_B] = &(hal_data->PHYRegDef[RF_PATH_B]);
+	reg[RF_PATH_C] = &(hal_data->PHYRegDef[RF_PATH_C]);
+	reg[RF_PATH_D] = &(hal_data->PHYRegDef[RF_PATH_D]);
+
+	reg[RF_PATH_A]->rfintfs = rFPGA0_XAB_RFInterfaceSW;
+	reg[RF_PATH_B]->rfintfs = rFPGA0_XAB_RFInterfaceSW;
+	reg[RF_PATH_C]->rfintfs = rFPGA0_XCD_RFInterfaceSW;
+	reg[RF_PATH_D]->rfintfs = rFPGA0_XCD_RFInterfaceSW;
+
+	reg[RF_PATH_A]->rfintfi = rFPGA0_XAB_RFInterfaceRB;
+	reg[RF_PATH_B]->rfintfi = rFPGA0_XAB_RFInterfaceRB;
+	reg[RF_PATH_C]->rfintfi = rFPGA0_XCD_RFInterfaceRB;
+	reg[RF_PATH_D]->rfintfi = rFPGA0_XCD_RFInterfaceRB;
+
+	reg[RF_PATH_A]->rfintfo = rFPGA0_XA_RFInterfaceOE;
+	reg[RF_PATH_B]->rfintfo = rFPGA0_XB_RFInterfaceOE;
+
+	reg[RF_PATH_A]->rfintfe = rFPGA0_XA_RFInterfaceOE;
+	reg[RF_PATH_B]->rfintfe = rFPGA0_XB_RFInterfaceOE;
+
+	reg[RF_PATH_A]->rf3wireOffset = rFPGA0_XA_LSSIParameter;
+	reg[RF_PATH_B]->rf3wireOffset = rFPGA0_XB_LSSIParameter;
+
+	reg[RF_PATH_A]->rfLSSI_Select = rFPGA0_XAB_RFParameter;
+	reg[RF_PATH_B]->rfLSSI_Select = rFPGA0_XAB_RFParameter;
+	reg[RF_PATH_C]->rfLSSI_Select = rFPGA0_XCD_RFParameter;
+	reg[RF_PATH_D]->rfLSSI_Select = rFPGA0_XCD_RFParameter;
+
+	reg[RF_PATH_A]->rfTxGainStage = rFPGA0_TxGainStage;
+	reg[RF_PATH_B]->rfTxGainStage = rFPGA0_TxGainStage;
+	reg[RF_PATH_C]->rfTxGainStage = rFPGA0_TxGainStage;
+	reg[RF_PATH_D]->rfTxGainStage = rFPGA0_TxGainStage;
+
+	reg[RF_PATH_A]->rfHSSIPara1 = rFPGA0_XA_HSSIParameter1;
+	reg[RF_PATH_B]->rfHSSIPara1 = rFPGA0_XB_HSSIParameter1;
+
+	reg[RF_PATH_A]->rfHSSIPara2 = rFPGA0_XA_HSSIParameter2;
+	reg[RF_PATH_B]->rfHSSIPara2 = rFPGA0_XB_HSSIParameter2;
+
+	reg[RF_PATH_A]->rfSwitchControl = rFPGA0_XAB_SwitchControl;
+	reg[RF_PATH_B]->rfSwitchControl = rFPGA0_XAB_SwitchControl;
+	reg[RF_PATH_C]->rfSwitchControl = rFPGA0_XCD_SwitchControl;
+	reg[RF_PATH_D]->rfSwitchControl = rFPGA0_XCD_SwitchControl;
+
+	reg[RF_PATH_A]->rfAGCControl1 = rOFDM0_XAAGCCore1;
+	reg[RF_PATH_B]->rfAGCControl1 = rOFDM0_XBAGCCore1;
+	reg[RF_PATH_C]->rfAGCControl1 = rOFDM0_XCAGCCore1;
+	reg[RF_PATH_D]->rfAGCControl1 = rOFDM0_XDAGCCore1;
+
+	reg[RF_PATH_A]->rfAGCControl2 = rOFDM0_XAAGCCore2;
+	reg[RF_PATH_B]->rfAGCControl2 = rOFDM0_XBAGCCore2;
+	reg[RF_PATH_C]->rfAGCControl2 = rOFDM0_XCAGCCore2;
+	reg[RF_PATH_D]->rfAGCControl2 = rOFDM0_XDAGCCore2;
+
+	reg[RF_PATH_A]->rfRxIQImbalance = rOFDM0_XARxIQImbalance;
+	reg[RF_PATH_B]->rfRxIQImbalance = rOFDM0_XBRxIQImbalance;
+	reg[RF_PATH_C]->rfRxIQImbalance = rOFDM0_XCRxIQImbalance;
+	reg[RF_PATH_D]->rfRxIQImbalance = rOFDM0_XDRxIQImbalance;
+
+	reg[RF_PATH_A]->rfRxAFE = rOFDM0_XARxAFE;
+	reg[RF_PATH_B]->rfRxAFE = rOFDM0_XBRxAFE;
+	reg[RF_PATH_C]->rfRxAFE = rOFDM0_XCRxAFE;
+	reg[RF_PATH_D]->rfRxAFE = rOFDM0_XDRxAFE;
+
+	reg[RF_PATH_A]->rfTxIQImbalance = rOFDM0_XATxIQImbalance;
+	reg[RF_PATH_B]->rfTxIQImbalance = rOFDM0_XBTxIQImbalance;
+	reg[RF_PATH_C]->rfTxIQImbalance = rOFDM0_XCTxIQImbalance;
+	reg[RF_PATH_D]->rfTxIQImbalance = rOFDM0_XDTxIQImbalance;
+
+	reg[RF_PATH_A]->rfTxAFE = rOFDM0_XATxAFE;
+	reg[RF_PATH_B]->rfTxAFE = rOFDM0_XBTxAFE;
+	reg[RF_PATH_C]->rfTxAFE = rOFDM0_XCTxAFE;
+	reg[RF_PATH_D]->rfTxAFE = rOFDM0_XDTxAFE;
+
+	reg[RF_PATH_A]->rfLSSIReadBack = rFPGA0_XA_LSSIReadBack;
+	reg[RF_PATH_B]->rfLSSIReadBack = rFPGA0_XB_LSSIReadBack;
+	reg[RF_PATH_C]->rfLSSIReadBack = rFPGA0_XC_LSSIReadBack;
+	reg[RF_PATH_D]->rfLSSIReadBack = rFPGA0_XD_LSSIReadBack;
+
+	reg[RF_PATH_A]->rfLSSIReadBackPi = TransceiverA_HSPI_Readback;
+	reg[RF_PATH_B]->rfLSSIReadBackPi = TransceiverB_HSPI_Readback;
 }
 
 static bool config_parafile(struct adapter *adapt)
diff --git a/drivers/staging/rtl8188eu/hal/fw.c b/drivers/staging/rtl8188eu/hal/fw.c
index 3b2875481fc5..a71c54295508 100644
--- a/drivers/staging/rtl8188eu/hal/fw.c
+++ b/drivers/staging/rtl8188eu/hal/fw.c
@@ -154,9 +154,8 @@ static int _rtl88e_fw_free_to_go(struct adapter *adapt)
 			break;
 	} while (counter++ < POLLING_READY_TIMEOUT_COUNT);
 
-	if (counter >= POLLING_READY_TIMEOUT_COUNT) {
+	if (counter >= POLLING_READY_TIMEOUT_COUNT)
 		goto exit;
-	}
 
 	value32 = usb_read32(adapt, REG_MCUFWDL);
 	value32 |= MCUFWDL_RDY;
diff --git a/drivers/staging/rtl8188eu/hal/odm.c b/drivers/staging/rtl8188eu/hal/odm.c
index 06477e834653..28b5e7bd4fc0 100644
--- a/drivers/staging/rtl8188eu/hal/odm.c
+++ b/drivers/staging/rtl8188eu/hal/odm.c
@@ -741,13 +741,13 @@ void odm_FalseAlarmCounterStatistics(struct odm_dm_struct *pDM_Odm)
 
 	ret_value = phy_query_bb_reg(adapter, ODM_REG_OFDM_FA_TYPE1_11N, bMaskDWord);
 	FalseAlmCnt->Cnt_Fast_Fsync = (ret_value&0xffff);
-	FalseAlmCnt->Cnt_SB_Search_fail = ((ret_value&0xffff0000)>>16);
+	FalseAlmCnt->Cnt_SB_Search_fail = (ret_value & 0xffff0000)>>16;
 	ret_value = phy_query_bb_reg(adapter, ODM_REG_OFDM_FA_TYPE2_11N, bMaskDWord);
 	FalseAlmCnt->Cnt_OFDM_CCA = (ret_value&0xffff);
-	FalseAlmCnt->Cnt_Parity_Fail = ((ret_value&0xffff0000)>>16);
+	FalseAlmCnt->Cnt_Parity_Fail = (ret_value & 0xffff0000)>>16;
 	ret_value = phy_query_bb_reg(adapter, ODM_REG_OFDM_FA_TYPE3_11N, bMaskDWord);
 	FalseAlmCnt->Cnt_Rate_Illegal = (ret_value&0xffff);
-	FalseAlmCnt->Cnt_Crc8_fail = ((ret_value&0xffff0000)>>16);
+	FalseAlmCnt->Cnt_Crc8_fail = (ret_value & 0xffff0000)>>16;
 	ret_value = phy_query_bb_reg(adapter, ODM_REG_OFDM_FA_TYPE4_11N, bMaskDWord);
 	FalseAlmCnt->Cnt_Mcs_fail = (ret_value&0xffff);
 
@@ -757,7 +757,7 @@ void odm_FalseAlarmCounterStatistics(struct odm_dm_struct *pDM_Odm)
 
 	ret_value = phy_query_bb_reg(adapter, ODM_REG_SC_CNT_11N, bMaskDWord);
 	FalseAlmCnt->Cnt_BW_LSC = (ret_value&0xffff);
-	FalseAlmCnt->Cnt_BW_USC = ((ret_value&0xffff0000)>>16);
+	FalseAlmCnt->Cnt_BW_USC = (ret_value & 0xffff0000)>>16;
 
 	/* hold cck counter */
 	phy_set_bb_reg(adapter, ODM_REG_CCK_FA_RST_11N, BIT12, 1);
diff --git a/drivers/staging/rtl8188eu/hal/odm_HWConfig.c b/drivers/staging/rtl8188eu/hal/odm_HWConfig.c
index 29f87dffbad3..36afe45d1c9a 100644
--- a/drivers/staging/rtl8188eu/hal/odm_HWConfig.c
+++ b/drivers/staging/rtl8188eu/hal/odm_HWConfig.c
@@ -123,8 +123,8 @@ static void odm_RxPhyStatus92CSeries_Parsing(struct odm_dm_struct *dm_odm,
 		/* 2011.11.28 LukeLee: 88E use different LNA & VGA gain table */
 		/* The RSSI formula should be modified according to the gain table */
 		/* In 88E, cck_highpwr is always set to 1 */
-		LNA_idx = ((cck_agc_rpt & 0xE0) >> 5);
-		VGA_idx = (cck_agc_rpt & 0x1F);
+		LNA_idx = (cck_agc_rpt & 0xE0) >> 5;
+		VGA_idx = cck_agc_rpt & 0x1F;
 		switch (LNA_idx) {
 		case 7:
 			if (VGA_idx <= 27)
diff --git a/drivers/staging/rtl8188eu/hal/phy.c b/drivers/staging/rtl8188eu/hal/phy.c
index 3f663fe151ba..6e4c3ee0399a 100644
--- a/drivers/staging/rtl8188eu/hal/phy.c
+++ b/drivers/staging/rtl8188eu/hal/phy.c
@@ -60,7 +60,7 @@ void phy_set_bb_reg(struct adapter *adapt, u32 regaddr, u32 bitmask, u32 data)
 	if (bitmask != bMaskDWord) { /* if not "double word" write */
 		original_value = usb_read32(adapt, regaddr);
 		bit_shift = cal_bit_shift(bitmask);
-		data = ((original_value & (~bitmask)) | (data << bit_shift));
+		data = (original_value & (~bitmask)) | (data << bit_shift);
 	}
 
 	usb_write32(adapt, regaddr, data);
@@ -72,12 +72,10 @@ static u32 rf_serial_read(struct adapter *adapt,
 	u32 ret = 0;
 	struct hal_data_8188e *hal_data = GET_HAL_DATA(adapt);
 	struct bb_reg_def *phyreg = &hal_data->PHYRegDef[rfpath];
-	u32 newoffset;
 	u32 tmplong, tmplong2;
 	u8 rfpi_enable = 0;
 
 	offset &= 0xff;
-	newoffset = offset;
 
 	tmplong = phy_query_bb_reg(adapt, rFPGA0_XA_HSSIParameter2, bMaskDWord);
 	if (rfpath == RF_PATH_A)
@@ -87,7 +85,7 @@ static u32 rf_serial_read(struct adapter *adapt,
 					    bMaskDWord);
 
 	tmplong2 = (tmplong2 & (~bLSSIReadAddress)) |
-		   (newoffset<<23) | bLSSIReadEdge;
+		   (offset<<23) | bLSSIReadEdge;
 
 	phy_set_bb_reg(adapt, rFPGA0_XA_HSSIParameter2, bMaskDWord,
 		       tmplong&(~bLSSIReadEdge));
@@ -119,10 +117,9 @@ static void rf_serial_write(struct adapter *adapt,
 	u32 data_and_addr = 0;
 	struct hal_data_8188e *hal_data = GET_HAL_DATA(adapt);
 	struct bb_reg_def *phyreg = &hal_data->PHYRegDef[rfpath];
-	u32 newoffset;
 
-	newoffset = offset & 0xff;
-	data_and_addr = ((newoffset<<20) | (data&0x000fffff)) & 0x0fffffff;
+	offset &= 0xff;
+	data_and_addr = ((offset<<20) | (data&0x000fffff)) & 0x0fffffff;
 	phy_set_bb_reg(adapt, phyreg->rf3wireOffset, bMaskDWord, data_and_addr);
 }
 
@@ -146,7 +143,7 @@ void phy_set_rf_reg(struct adapter *adapt, enum rf_radio_path rf_path,
 	if (bit_mask != bRFRegOffsetMask) {
 		original_value = rf_serial_read(adapt, rf_path, reg_addr);
 		bit_shift =  cal_bit_shift(bit_mask);
-		data = ((original_value & (~bit_mask)) | (data << bit_shift));
+		data = (original_value & (~bit_mask)) | (data << bit_shift);
 	}
 
 	rf_serial_write(adapt, rf_path, reg_addr, data);
@@ -304,21 +301,8 @@ static void phy_set_bw_mode_callback(struct adapter *adapt)
 	}
 
 	/* Set RF related register */
-	switch (hal_data->rf_chip) {
-	case RF_8225:
-		break;
-	case RF_8256:
-		break;
-	case RF_8258:
-		break;
-	case RF_PSEUDO_11N:
-		break;
-	case RF_6052:
+	if (hal_data->rf_chip == RF_6052)
 		rtl88eu_phy_rf6052_set_bandwidth(adapt, hal_data->CurrentChannelBW);
-		break;
-	default:
-		break;
-	}
 }
 
 void phy_set_bw_mode(struct adapter *adapt, enum ht_channel_width bandwidth,
@@ -361,7 +345,6 @@ void phy_sw_chnl(struct adapter *adapt, u8 channel)
 {
 	struct hal_data_8188e *hal_data = GET_HAL_DATA(adapt);
 	u8 tmpchannel = hal_data->CurrentChannel;
-	bool  result = true;
 
 	if (hal_data->rf_chip == RF_PSEUDO_11N)
 		return;
@@ -371,34 +354,28 @@ void phy_sw_chnl(struct adapter *adapt, u8 channel)
 
 	hal_data->CurrentChannel = channel;
 
-	if ((!adapt->bDriverStopped) && (!adapt->bSurpriseRemoved)) {
+	if ((!adapt->bDriverStopped) && (!adapt->bSurpriseRemoved))
 		phy_sw_chnl_callback(adapt, channel);
-
-		if (!result)
-			hal_data->CurrentChannel = tmpchannel;
-
-	} else {
+	else
 		hal_data->CurrentChannel = tmpchannel;
-	}
 }
 
 #define ODM_TXPWRTRACK_MAX_IDX_88E  6
 
 static u8 get_right_chnl_for_iqk(u8 chnl)
 {
+	u8 place;
 	u8 channel_all[ODM_TARGET_CHNL_NUM_2G_5G] = {
-		1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14,
 		36, 38, 40, 42, 44, 46, 48, 50, 52, 54, 56, 58, 60, 62, 64,
 		100, 102, 104, 106, 108, 110, 112, 114, 116, 118, 120, 122,
 		124, 126, 128, 130, 132, 134, 136, 138, 140, 149, 151, 153,
 		155, 157, 159, 161, 163, 165
 	};
-	u8 place = chnl;
 
 	if (chnl > 14) {
-		for (place = 14; place < sizeof(channel_all); place++) {
+		for (place = 0; place < sizeof(channel_all); place++) {
 			if (channel_all[place] == chnl)
-				return place-13;
+				return ++place;
 		}
 	}
 	return 0;
@@ -416,12 +393,12 @@ void rtl88eu_dm_txpower_track_adjust(struct odm_dm_struct *dm_odm, u8 type,
 
 		if (dm_odm->BbSwingIdxOfdm <= dm_odm->BbSwingIdxOfdmBase) {
 			*direction = 1;
-			pwr_value = (dm_odm->BbSwingIdxOfdmBase -
-				     dm_odm->BbSwingIdxOfdm);
+			pwr_value = dm_odm->BbSwingIdxOfdmBase -
+				     dm_odm->BbSwingIdxOfdm;
 		} else {
 			*direction = 2;
-			pwr_value = (dm_odm->BbSwingIdxOfdm -
-				     dm_odm->BbSwingIdxOfdmBase);
+			pwr_value = dm_odm->BbSwingIdxOfdm -
+				     dm_odm->BbSwingIdxOfdmBase;
 		}
 
 	} else if (type == 1) { /* For CCK adjust. */
@@ -431,12 +408,12 @@ void rtl88eu_dm_txpower_track_adjust(struct odm_dm_struct *dm_odm, u8 type,
 
 		if (dm_odm->BbSwingIdxCck <= dm_odm->BbSwingIdxCckBase) {
 			*direction = 1;
-			pwr_value = (dm_odm->BbSwingIdxCckBase -
-				     dm_odm->BbSwingIdxCck);
+			pwr_value = dm_odm->BbSwingIdxCckBase -
+				     dm_odm->BbSwingIdxCck;
 		} else {
 			*direction = 2;
-			pwr_value = (dm_odm->BbSwingIdxCck -
-				     dm_odm->BbSwingIdxCckBase);
+			pwr_value = dm_odm->BbSwingIdxCck -
+				     dm_odm->BbSwingIdxCckBase;
 		}
 
 	}
@@ -465,15 +442,13 @@ void rtl88eu_dm_txpower_tracking_callback_thermalmeter(struct adapter *adapt)
 	u8 thermal_val = 0, delta, delta_lck, delta_iqk, offset;
 	u8 thermal_avg_count = 0;
 	u32 thermal_avg = 0;
-	s32 ele_a = 0, ele_d, temp_cck, x, value32;
-	s32 y, ele_c = 0;
+	s32 ele_d, temp_cck;
 	s8 ofdm_index[2], cck_index = 0;
 	s8 ofdm_index_old[2] = {0, 0}, cck_index_old = 0;
 	u32 i = 0, j = 0;
 	bool is2t = false;
 
 	u8 ofdm_min_index = 6, rf; /* OFDM BB Swing should be less than +3.0dB */
-	u8 indexforchannel = 0;
 	s8 ofdm_index_mapping[2][index_mapping_NUM_88E] = {
 		/* 2.4G, decrease power */
 		{0, 0, 2, 3, 4, 4, 5, 6, 7, 7, 8, 9, 10, 10, 11},
@@ -529,18 +504,12 @@ void rtl88eu_dm_txpower_tracking_callback_thermalmeter(struct adapter *adapt)
 		temp_cck = dm_odm->RFCalibrateInfo.RegA24;
 
 		for (i = 0; i < CCK_TABLE_SIZE; i++) {
-			if (dm_odm->RFCalibrateInfo.bCCKinCH14) {
-				if (memcmp(&temp_cck, &CCKSwingTable_Ch14[i][2], 4)) {
-					cck_index_old = (u8)i;
-					dm_odm->BbSwingIdxCckBase = (u8)i;
-					break;
-				}
-			} else {
-				if (memcmp(&temp_cck, &CCKSwingTable_Ch1_Ch13[i][2], 4)) {
+			if ((dm_odm->RFCalibrateInfo.bCCKinCH14 &&
+				memcmp(&temp_cck, &CCKSwingTable_Ch14[i][2], 4)) ||
+				memcmp(&temp_cck, &CCKSwingTable_Ch1_Ch13[i][2], 4)) {
 					cck_index_old = (u8)i;
 					dm_odm->BbSwingIdxCckBase = (u8)i;
 					break;
-				}
 			}
 		}
 
@@ -570,27 +539,19 @@ void rtl88eu_dm_txpower_tracking_callback_thermalmeter(struct adapter *adapt)
 		if (thermal_avg_count)
 			thermal_val = (u8)(thermal_avg / thermal_avg_count);
 
-		if (dm_odm->RFCalibrateInfo.bReloadtxpowerindex) {
-			delta = thermal_val > hal_data->EEPROMThermalMeter ?
-				(thermal_val - hal_data->EEPROMThermalMeter) :
-				(hal_data->EEPROMThermalMeter - thermal_val);
-			dm_odm->RFCalibrateInfo.bReloadtxpowerindex = false;
-			dm_odm->RFCalibrateInfo.bDoneTxpower = false;
-		} else if (dm_odm->RFCalibrateInfo.bDoneTxpower) {
-			delta = (thermal_val > dm_odm->RFCalibrateInfo.ThermalValue) ?
-				(thermal_val - dm_odm->RFCalibrateInfo.ThermalValue) :
-				(dm_odm->RFCalibrateInfo.ThermalValue - thermal_val);
-		} else {
-			delta = thermal_val > hal_data->EEPROMThermalMeter ?
-				(thermal_val - hal_data->EEPROMThermalMeter) :
-				(hal_data->EEPROMThermalMeter - thermal_val);
+		if (dm_odm->RFCalibrateInfo.bDoneTxpower &&
+			!dm_odm->RFCalibrateInfo.bReloadtxpowerindex)
+			delta = abs(thermal_val - dm_odm->RFCalibrateInfo.ThermalValue);
+		else {
+			delta = abs(thermal_val - hal_data->EEPROMThermalMeter);
+			if (dm_odm->RFCalibrateInfo.bReloadtxpowerindex) {
+				dm_odm->RFCalibrateInfo.bReloadtxpowerindex = false;
+				dm_odm->RFCalibrateInfo.bDoneTxpower = false;
+			}
 		}
-		delta_lck = (thermal_val > dm_odm->RFCalibrateInfo.ThermalValue_LCK) ?
-			    (thermal_val - dm_odm->RFCalibrateInfo.ThermalValue_LCK) :
-			    (dm_odm->RFCalibrateInfo.ThermalValue_LCK - thermal_val);
-		delta_iqk = (thermal_val > dm_odm->RFCalibrateInfo.ThermalValue_IQK) ?
-			    (thermal_val - dm_odm->RFCalibrateInfo.ThermalValue_IQK) :
-			    (dm_odm->RFCalibrateInfo.ThermalValue_IQK - thermal_val);
+
+		delta_lck = abs(dm_odm->RFCalibrateInfo.ThermalValue_LCK - thermal_val);
+		delta_iqk = abs(dm_odm->RFCalibrateInfo.ThermalValue_IQK - thermal_val);
 
 		/* Delta temperature is equal to or larger than 20 centigrade.*/
 		if ((delta_lck >= 8)) {
@@ -599,9 +560,8 @@ void rtl88eu_dm_txpower_tracking_callback_thermalmeter(struct adapter *adapt)
 		}
 
 		if (delta > 0 && dm_odm->RFCalibrateInfo.TxPowerTrackControl) {
-			delta = thermal_val > hal_data->EEPROMThermalMeter ?
-				(thermal_val - hal_data->EEPROMThermalMeter) :
-				(hal_data->EEPROMThermalMeter - thermal_val);
+			delta = abs(hal_data->EEPROMThermalMeter - thermal_val);
+
 			/* calculate new OFDM / CCK offset */
 			if (thermal_val > hal_data->EEPROMThermalMeter)
 				j = 1;
@@ -616,17 +576,17 @@ void rtl88eu_dm_txpower_tracking_callback_thermalmeter(struct adapter *adapt)
 			}
 			if (offset >= index_mapping_NUM_88E)
 				offset = index_mapping_NUM_88E-1;
-			for (i = 0; i < rf; i++)
-				ofdm_index[i] = dm_odm->RFCalibrateInfo.OFDM_index[i] + ofdm_index_mapping[j][offset];
-			cck_index = dm_odm->RFCalibrateInfo.CCK_index + ofdm_index_mapping[j][offset];
 
+			/* Updating ofdm_index values with new OFDM / CCK offset */
 			for (i = 0; i < rf; i++) {
+				ofdm_index[i] = dm_odm->RFCalibrateInfo.OFDM_index[i] + ofdm_index_mapping[j][offset];
 				if (ofdm_index[i] > OFDM_TABLE_SIZE_92D-1)
 					ofdm_index[i] = OFDM_TABLE_SIZE_92D-1;
 				else if (ofdm_index[i] < ofdm_min_index)
 					ofdm_index[i] = ofdm_min_index;
 			}
 
+			cck_index = dm_odm->RFCalibrateInfo.CCK_index + ofdm_index_mapping[j][offset];
 			if (cck_index > CCK_TABLE_SIZE-1)
 				cck_index = CCK_TABLE_SIZE-1;
 			else if (cck_index < 0)
@@ -637,11 +597,6 @@ void rtl88eu_dm_txpower_tracking_callback_thermalmeter(struct adapter *adapt)
 			if (dm_odm->RFCalibrateInfo.TxPowerTrackControl) {
 				dm_odm->RFCalibrateInfo.bDoneTxpower = true;
 
-				/* Adujst OFDM Ant_A according to IQK result */
-				ele_d = (OFDMSwingTable[(u8)ofdm_index[0]] & 0xFFC00000)>>22;
-				x = dm_odm->RFCalibrateInfo.IQKMatrixRegSetting[indexforchannel].Value[0][0];
-				y = dm_odm->RFCalibrateInfo.IQKMatrixRegSetting[indexforchannel].Value[0][1];
-
 				/*  Revse TX power table. */
 				dm_odm->BbSwingIdxOfdm = (u8)ofdm_index[0];
 				dm_odm->BbSwingIdxCck = (u8)cck_index;
@@ -655,53 +610,6 @@ void rtl88eu_dm_txpower_tracking_callback_thermalmeter(struct adapter *adapt)
 					dm_odm->BbSwingIdxCckCurrent = dm_odm->BbSwingIdxCck;
 					dm_odm->BbSwingFlagCck = true;
 				}
-
-				if (x != 0) {
-					if ((x & 0x00000200) != 0)
-						x = x | 0xFFFFFC00;
-					ele_a = ((x * ele_d)>>8)&0x000003FF;
-
-					/* new element C = element D x Y */
-					if ((y & 0x00000200) != 0)
-						y = y | 0xFFFFFC00;
-					ele_c = ((y * ele_d)>>8)&0x000003FF;
-
-				}
-
-				if (is2t) {
-					ele_d = (OFDMSwingTable[(u8)ofdm_index[1]] & 0xFFC00000)>>22;
-
-					/* new element A = element D x X */
-					x = dm_odm->RFCalibrateInfo.IQKMatrixRegSetting[indexforchannel].Value[0][4];
-					y = dm_odm->RFCalibrateInfo.IQKMatrixRegSetting[indexforchannel].Value[0][5];
-
-					if ((x != 0) && (*(dm_odm->pBandType) == ODM_BAND_2_4G)) {
-						if ((x & 0x00000200) != 0)	/* consider minus */
-							x = x | 0xFFFFFC00;
-						ele_a = ((x * ele_d)>>8)&0x000003FF;
-
-						/* new element C = element D x Y */
-						if ((y & 0x00000200) != 0)
-							y = y | 0xFFFFFC00;
-						ele_c = ((y * ele_d)>>8)&0x00003FF;
-
-						/* wtite new elements A, C, D to regC88 and regC9C, element B is always 0 */
-						value32 = (ele_d<<22) | ((ele_c&0x3F)<<16) | ele_a;
-						phy_set_bb_reg(adapt, rOFDM0_XBTxIQImbalance, bMaskDWord, value32);
-
-						value32 = (ele_c&0x000003C0)>>6;
-						phy_set_bb_reg(adapt, rOFDM0_XDTxAFE, bMaskH4Bits, value32);
-
-						value32 = ((x * ele_d)>>7)&0x01;
-						phy_set_bb_reg(adapt, rOFDM0_ECCAThreshold, BIT28, value32);
-					} else {
-						phy_set_bb_reg(adapt, rOFDM0_XBTxIQImbalance, bMaskDWord, OFDMSwingTable[(u8)ofdm_index[1]]);
-						phy_set_bb_reg(adapt, rOFDM0_XDTxAFE, bMaskH4Bits, 0x00);
-						phy_set_bb_reg(adapt, rOFDM0_ECCAThreshold, BIT28, 0x00);
-					}
-
-				}
-
 			}
 		}
 
@@ -1033,11 +941,11 @@ static void path_adda_on(struct adapter *adapt, u32 *adda_reg,
 	u32 path_on;
 	u32 i;
 
-	path_on = is_path_a_on ? 0x04db25a4 : 0x0b1b25a4;
 	if (!is2t) {
 		path_on = 0x0bdb25a0;
 		phy_set_bb_reg(adapt, adda_reg[0], bMaskDWord, 0x0b1b25a0);
 	} else {
+		path_on = is_path_a_on ? 0x04db25a4 : 0x0b1b25a4;
 		phy_set_bb_reg(adapt, adda_reg[0], bMaskDWord, path_on);
 	}
 
@@ -1077,27 +985,19 @@ static void pi_mode_switch(struct adapter *adapt, bool pi_mode)
 static bool simularity_compare(struct adapter *adapt, s32 resulta[][8],
 			       u8 c1, u8 c2)
 {
-	u32 i, j, diff, sim_bitmap, bound = 0;
+	u32 i, j, diff, sim_bitmap = 0, bound;
 	struct hal_data_8188e *hal_data = GET_HAL_DATA(adapt);
 	struct odm_dm_struct *dm_odm = &hal_data->odmpriv;
 	u8 final_candidate[2] = {0xFF, 0xFF};	/* for path A and path B */
 	bool result = true;
-	bool is2t;
 	s32 tmp1 = 0, tmp2 = 0;
 
 	if ((dm_odm->RFType == ODM_2T2R) || (dm_odm->RFType == ODM_2T3R) ||
 	    (dm_odm->RFType == ODM_2T4R))
-		is2t = true;
-	else
-		is2t = false;
-
-	if (is2t)
 		bound = 8;
 	else
 		bound = 4;
 
-	sim_bitmap = 0;
-
 	for (i = 0; i < bound; i++) {
 		if ((i == 1) || (i == 3) || (i == 5) || (i == 7)) {
 			if ((resulta[c1][i] & 0x00000200) != 0)
diff --git a/drivers/staging/rtl8188eu/hal/pwrseqcmd.c b/drivers/staging/rtl8188eu/hal/pwrseqcmd.c
index be0663e93f61..73e1f8b36b37 100644
--- a/drivers/staging/rtl8188eu/hal/pwrseqcmd.c
+++ b/drivers/staging/rtl8188eu/hal/pwrseqcmd.c
@@ -109,7 +109,6 @@ u8 rtl88eu_pwrseqcmdparsing(struct adapter *padapter, u8 cut_vers, u8 fab_vers,
 				RT_TRACE(_module_hal_init_c_, _drv_info_,
 					 ("rtl88eu_pwrseqcmdparsing: PWR_CMD_END\n"));
 				return true;
-				break;
 			default:
 				RT_TRACE(_module_hal_init_c_, _drv_err_,
 					 ("rtl88eu_pwrseqcmdparsing: Unknown CMD!!\n"));
diff --git a/drivers/staging/rtl8188eu/hal/rf.c b/drivers/staging/rtl8188eu/hal/rf.c
index eea4c8a6022b..097092772a86 100644
--- a/drivers/staging/rtl8188eu/hal/rf.c
+++ b/drivers/staging/rtl8188eu/hal/rf.c
@@ -201,7 +201,7 @@ static void get_rx_power_val_by_reg(struct adapter *adapt, u8 channel,
 			break;
 		case 2:	/*  Better regulatory */
 				/*  don't increase any power diff */
-			write_val = ((index < 2) ? powerbase0[rf] : powerbase1[rf]);
+			write_val = (index < 2) ? powerbase0[rf] : powerbase1[rf];
 			break;
 		case 3:	/*  Customer defined power diff. */
 				/*  increase power diff defined by customer. */
diff --git a/drivers/staging/rtl8188eu/hal/rf_cfg.c b/drivers/staging/rtl8188eu/hal/rf_cfg.c
index 5dc11cae2ef9..455ecdc8d9fa 100644
--- a/drivers/staging/rtl8188eu/hal/rf_cfg.c
+++ b/drivers/staging/rtl8188eu/hal/rf_cfg.c
@@ -38,12 +38,12 @@ static bool check_condition(struct adapter *adapt, const u32  condition)
 		return false;
 
 	cond = condition & 0x0000FF00;
-	cond = cond >> 8;
+	cond >>= 8;
 	if ((_interface & cond) == 0 && cond != 0x07)
 		return false;
 
 	cond = condition & 0x00FF0000;
-	cond = cond >> 16;
+	cond >>= 16;
 	if ((_platform & cond) == 0 && cond != 0x0F)
 		return false;
 	return true;
diff --git a/drivers/staging/rtl8188eu/hal/rtl8188e_hal_init.c b/drivers/staging/rtl8188eu/hal/rtl8188e_hal_init.c
index 3222d8d08b5b..7904d2260f2c 100644
--- a/drivers/staging/rtl8188eu/hal/rtl8188e_hal_init.c
+++ b/drivers/staging/rtl8188eu/hal/rtl8188e_hal_init.c
@@ -596,7 +596,8 @@ void Hal_EfuseParseBoardType88E(struct adapter *pAdapter, u8 *hwinfo, bool AutoL
 	struct hal_data_8188e *pHalData = GET_HAL_DATA(pAdapter);
 
 	if (!AutoLoadFail)
-		pHalData->BoardType = ((hwinfo[EEPROM_RF_BOARD_OPTION_88E]&0xE0)>>5);
+		pHalData->BoardType = (hwinfo[EEPROM_RF_BOARD_OPTION_88E]
+					& 0xE0) >> 5;
 	else
 		pHalData->BoardType = 0;
 	DBG_88E("Board Type: 0x%2x\n", pHalData->BoardType);
diff --git a/drivers/staging/rtl8188eu/hal/rtl8188eu_recv.c b/drivers/staging/rtl8188eu/hal/rtl8188eu_recv.c
index bc275b2a7d37..06d1e654483e 100644
--- a/drivers/staging/rtl8188eu/hal/rtl8188eu_recv.c
+++ b/drivers/staging/rtl8188eu/hal/rtl8188eu_recv.c
@@ -42,7 +42,7 @@ int	rtl8188eu_init_recv_priv(struct adapter *padapter)
 	_rtw_init_queue(&precvpriv->free_recv_buf_queue);
 
 	precvpriv->pallocated_recv_buf =
-		kzalloc(NR_RECVBUFF * sizeof(struct recv_buf), GFP_KERNEL);
+		kcalloc(NR_RECVBUFF, sizeof(struct recv_buf), GFP_KERNEL);
 	if (precvpriv->pallocated_recv_buf == NULL) {
 		res = _FAIL;
 		RT_TRACE(_module_rtl871x_recv_c_, _drv_err_,
diff --git a/drivers/staging/rtl8188eu/hal/usb_halinit.c b/drivers/staging/rtl8188eu/hal/usb_halinit.c
index 14650e91c78a..7b01d5aa6b23 100644
--- a/drivers/staging/rtl8188eu/hal/usb_halinit.c
+++ b/drivers/staging/rtl8188eu/hal/usb_halinit.c
@@ -1096,10 +1096,8 @@ static void Hal_EfuseParseMACAddr_8188EU(struct adapter *adapt, u8 *hwinfo, bool
 		memcpy(eeprom->mac_addr, &hwinfo[EEPROM_MAC_ADDR_88EU], ETH_ALEN);
 	}
 	RT_TRACE(_module_hci_hal_init_c_, _drv_notice_,
-		 ("Hal_EfuseParseMACAddr_8188EU: Permanent Address = %02x-%02x-%02x-%02x-%02x-%02x\n",
-		 eeprom->mac_addr[0], eeprom->mac_addr[1],
-		 eeprom->mac_addr[2], eeprom->mac_addr[3],
-		 eeprom->mac_addr[4], eeprom->mac_addr[5]));
+		 ("Hal_EfuseParseMACAddr_8188EU: Permanent Address = %pM\n",
+		 eeprom->mac_addr));
 }
 
 static void
@@ -1352,7 +1350,7 @@ static void SetHwReg8188EU(struct adapter *Adapter, u8 variable, u8 *val)
 
 			/*  Set RTS initial rate */
 			while (BrateCfg > 0x1) {
-				BrateCfg = (BrateCfg >> 1);
+				BrateCfg >>= 1;
 				RateIndex++;
 			}
 			/*  Ziv - Check */
diff --git a/drivers/staging/rtl8188eu/include/ieee80211.h b/drivers/staging/rtl8188eu/include/ieee80211.h
index 3299571e339f..8fd35dcdbb94 100644
--- a/drivers/staging/rtl8188eu/include/ieee80211.h
+++ b/drivers/staging/rtl8188eu/include/ieee80211.h
@@ -122,7 +122,6 @@ enum {
 
 #define WPA_SELECTOR_LEN 4
 extern u8 RTW_WPA_OUI_TYPE[];
-extern u16 RTW_WPA_VERSION;
 extern u8 WPA_AUTH_KEY_MGMT_NONE[];
 extern u8 WPA_AUTH_KEY_MGMT_UNSPEC_802_1X[];
 extern u8 WPA_AUTH_KEY_MGMT_PSK_OVER_802_1X[];
diff --git a/drivers/staging/rtl8188eu/include/osdep_service.h b/drivers/staging/rtl8188eu/include/osdep_service.h
index 3a274770364b..515e949629e2 100644
--- a/drivers/staging/rtl8188eu/include/osdep_service.h
+++ b/drivers/staging/rtl8188eu/include/osdep_service.h
@@ -76,25 +76,6 @@ static inline int _enter_critical_mutex(struct mutex *pmutex,
 	return ret;
 }
 
-static inline void _init_timer(struct timer_list *ptimer,
-			       struct  net_device *nic_hdl,
-			       void *pfunc, void *cntx)
-{
-	ptimer->function = pfunc;
-	ptimer->data = (unsigned long)cntx;
-	init_timer(ptimer);
-}
-
-static inline void _set_timer(struct timer_list *ptimer, u32 delay_time)
-{
-	mod_timer(ptimer , (jiffies+msecs_to_jiffies(delay_time)));
-}
-
-#define RTW_TIMER_HDL_ARGS void *FunctionContext
-#define RTW_TIMER_HDL_NAME(name) rtw_##name##_timer_hdl
-#define RTW_DECLARE_TIMER_HDL(name) \
-	void RTW_TIMER_HDL_NAME(name)(RTW_TIMER_HDL_ARGS)
-
 static inline int rtw_netif_queue_stopped(struct net_device *pnetdev)
 {
 	return  netif_tx_queue_stopped(netdev_get_tx_queue(pnetdev, 0)) &&
diff --git a/drivers/staging/rtl8188eu/include/rtw_led.h b/drivers/staging/rtl8188eu/include/rtw_led.h
index 23f0cfe312f3..7a5303d50d49 100644
--- a/drivers/staging/rtl8188eu/include/rtw_led.h
+++ b/drivers/staging/rtl8188eu/include/rtw_led.h
@@ -103,7 +103,7 @@ struct led_priv {
 			(adapt)->ledpriv.LedControlHandler((adapt), (action)); \
 	} while (0)
 
-void BlinkTimerCallback(void *data);
+void BlinkTimerCallback(unsigned long data);
 void BlinkWorkItemCallback(struct work_struct *work);
 
 void ResetLedStatus(struct LED_871x *pLed);
diff --git a/drivers/staging/rtl8188eu/include/rtw_mlme.h b/drivers/staging/rtl8188eu/include/rtw_mlme.h
index 8d83f7ceda76..3f7d1e631ef9 100644
--- a/drivers/staging/rtl8188eu/include/rtw_mlme.h
+++ b/drivers/staging/rtl8188eu/include/rtw_mlme.h
@@ -551,10 +551,10 @@ void rtw_update_registrypriv_dev_network(struct adapter *adapter);
 
 void rtw_get_encrypt_decrypt_from_registrypriv(struct adapter *adapter);
 
-void _rtw_join_timeout_handler(void *function_context);
-void rtw_scan_timeout_handler(void *function_context);
+void _rtw_join_timeout_handler(unsigned long data);
+void rtw_scan_timeout_handler(unsigned long data);
 
-void rtw_dynamic_check_timer_handlder(void *function_context);
+void rtw_dynamic_check_timer_handlder(unsigned long data);
 #define rtw_is_scan_deny(adapter) false
 #define rtw_clear_scan_deny(adapter) do {} while (0)
 #define rtw_set_scan_deny_timer_hdl(adapter) do {} while (0)
diff --git a/drivers/staging/rtl8188eu/include/rtw_mlme_ext.h b/drivers/staging/rtl8188eu/include/rtw_mlme_ext.h
index 4f05aee93c9c..2bebf46b053a 100644
--- a/drivers/staging/rtl8188eu/include/rtw_mlme_ext.h
+++ b/drivers/staging/rtl8188eu/include/rtw_mlme_ext.h
@@ -645,19 +645,17 @@ void mlmeext_sta_add_event_callback(struct adapter *padapter,
 
 void linked_status_chk(struct adapter *padapter);
 
-void survey_timer_hdl(void *function_context);
-void link_timer_hdl(void *funtion_context);
-void addba_timer_hdl(void *function_context);
+void survey_timer_hdl(unsigned long data);
+void link_timer_hdl(unsigned long data);
+void addba_timer_hdl(unsigned long data);
 
 #define set_survey_timer(mlmeext, ms) \
-	do { \
-		_set_timer(&(mlmeext)->survey_timer, (ms)); \
-	} while (0)
+	mod_timer(&mlmeext->survey_timer, jiffies +	\
+		  msecs_to_jiffies(ms))
 
 #define set_link_timer(mlmeext, ms) \
-	do { \
-		_set_timer(&(mlmeext)->link_timer, (ms)); \
-	} while (0)
+	mod_timer(&mlmeext->link_timer, jiffies +	\
+		  msecs_to_jiffies(ms))
 
 int cckrates_included(unsigned char *rate, int ratelen);
 int cckratesonly_included(unsigned char *rate, int ratelen);
diff --git a/drivers/staging/rtl8188eu/include/rtw_pwrctrl.h b/drivers/staging/rtl8188eu/include/rtw_pwrctrl.h
index 54dfbf07564b..aa1fd87c47fb 100644
--- a/drivers/staging/rtl8188eu/include/rtw_pwrctrl.h
+++ b/drivers/staging/rtl8188eu/include/rtw_pwrctrl.h
@@ -233,9 +233,8 @@ struct pwrctrl_priv {
 #define RTW_PWR_STATE_CHK_INTERVAL 2000
 
 #define _rtw_set_pwr_state_check_timer(pwrctrlpriv, ms) \
-	do { \
-		_set_timer(&(pwrctrlpriv)->pwr_state_check_timer, (ms)); \
-	} while (0)
+	mod_timer(&pwrctrlpriv->pwr_state_check_timer,	\
+		  jiffies + msecs_to_jiffies(ms))
 
 #define rtw_set_pwr_state_check_timer(pwrctrl)			\
 	_rtw_set_pwr_state_check_timer((pwrctrl),		\
diff --git a/drivers/staging/rtl8188eu/include/rtw_recv.h b/drivers/staging/rtl8188eu/include/rtw_recv.h
index f0c26ef8f66a..eb1ac3d03123 100644
--- a/drivers/staging/rtl8188eu/include/rtw_recv.h
+++ b/drivers/staging/rtl8188eu/include/rtw_recv.h
@@ -216,8 +216,8 @@ struct recv_priv {
 };
 
 #define rtw_set_signal_stat_timer(recvpriv)			\
-	_set_timer(&(recvpriv)->signal_stat_timer,		\
-		   (recvpriv)->signal_stat_sampling_interval)
+	mod_timer(&(recvpriv)->signal_stat_timer, jiffies +	\
+		  msecs_to_jiffies((recvpriv)->signal_stat_sampling_interval))
 
 struct sta_recv_priv {
 	spinlock_t lock;
@@ -278,7 +278,7 @@ void rtw_free_recvframe_queue(struct __queue *pframequeue,
 			      struct __queue *pfree_recv_queue);
 u32 rtw_free_uc_swdec_pending_queue(struct adapter *adapter);
 
-void rtw_reordering_ctrl_timeout_handler(void *pcontext);
+void rtw_reordering_ctrl_timeout_handler(unsigned long data);
 
 static inline u8 *get_rxmem(struct recv_frame *precvframe)
 {
diff --git a/drivers/staging/rtl8188eu/include/rtw_security.h b/drivers/staging/rtl8188eu/include/rtw_security.h
index 66d60aaf4ae8..e9723a72af5e 100644
--- a/drivers/staging/rtl8188eu/include/rtw_security.h
+++ b/drivers/staging/rtl8188eu/include/rtw_security.h
@@ -245,10 +245,6 @@ struct mic_data {
 };
 
 extern const u32 Te0[256];
-extern const u32 Te1[256];
-extern const u32 Te2[256];
-extern const u32 Te3[256];
-extern const u32 Te4[256];
 extern const u32 Td0[256];
 extern const u32 Td1[256];
 extern const u32 Td2[256];
@@ -269,28 +265,6 @@ static inline u32 rotr(u32 val, int bits)
 #define TE1(i) rotr(Te0[((i) >> 16) & 0xff], 8)
 #define TE2(i) rotr(Te0[((i) >> 8) & 0xff], 16)
 #define TE3(i) rotr(Te0[(i) & 0xff], 24)
-#define TE41(i) ((Te0[((i) >> 24) & 0xff] << 8) & 0xff000000)
-#define TE42(i) (Te0[((i) >> 16) & 0xff] & 0x00ff0000)
-#define TE43(i) (Te0[((i) >> 8) & 0xff] & 0x0000ff00)
-#define TE44(i) ((Te0[(i) & 0xff] >> 8) & 0x000000ff)
-#define TE421(i) ((Te0[((i) >> 16) & 0xff] << 8) & 0xff000000)
-#define TE432(i) (Te0[((i) >> 8) & 0xff] & 0x00ff0000)
-#define TE443(i) (Te0[(i) & 0xff] & 0x0000ff00)
-#define TE414(i) ((Te0[((i) >> 24) & 0xff] >> 8) & 0x000000ff)
-#define TE4(i) ((Te0[(i)] >> 8) & 0x000000ff)
-
-#define TD0(i) Td0[((i) >> 24) & 0xff]
-#define TD1(i) rotr(Td0[((i) >> 16) & 0xff], 8)
-#define TD2(i) rotr(Td0[((i) >> 8) & 0xff], 16)
-#define TD3(i) rotr(Td0[(i) & 0xff], 24)
-#define TD41(i) (Td4s[((i) >> 24) & 0xff] << 24)
-#define TD42(i) (Td4s[((i) >> 16) & 0xff] << 16)
-#define TD43(i) (Td4s[((i) >> 8) & 0xff] << 8)
-#define TD44(i) (Td4s[(i) & 0xff])
-#define TD0_(i) Td0[(i) & 0xff]
-#define TD1_(i) rotr(Td0[(i) & 0xff], 8)
-#define TD2_(i) rotr(Td0[(i) & 0xff], 16)
-#define TD3_(i) rotr(Td0[(i) & 0xff], 24)
 
 #define GETU32(pt) (((u32)(pt)[0] << 24) ^ ((u32)(pt)[1] << 16) ^ \
 			((u32)(pt)[2] <<  8) ^ ((u32)(pt)[3]))
diff --git a/drivers/staging/rtl8188eu/include/wifi.h b/drivers/staging/rtl8188eu/include/wifi.h
index 8dbdfafd52b5..a89275e0e0e0 100644
--- a/drivers/staging/rtl8188eu/include/wifi.h
+++ b/drivers/staging/rtl8188eu/include/wifi.h
@@ -301,22 +301,12 @@ enum WIFI_REG_DOMAIN {
 #define GetPrivacy(pbuf)					\
 	(((*(__le16 *)(pbuf)) & cpu_to_le16(_PRIVACY_)) != 0)
 
-#define ClearPrivacy(pbuf)	\
-	*(__le16 *)(pbuf) &= (~cpu_to_le16(_PRIVACY_))
-
-
 #define GetOrder(pbuf)					\
 	(((*(__le16 *)(pbuf)) & cpu_to_le16(_ORDER_)) != 0)
 
 #define GetFrameType(pbuf)				\
 	(le16_to_cpu(*(__le16 *)(pbuf)) & (BIT(3) | BIT(2)))
 
-#define SetFrameType(pbuf, type)	\
-	do {	\
-		*(unsigned short *)(pbuf) &= __constant_cpu_to_le16(~(BIT(3) | BIT(2))); \
-		*(unsigned short *)(pbuf) |= __constant_cpu_to_le16(type); \
-	} while (0)
-
 #define GetFrameSubType(pbuf)	(le16_to_cpu(*(__le16 *)(pbuf)) & (BIT(7) |\
 	 BIT(6) | BIT(5) | BIT(4) | BIT(3) | BIT(2)))
 
@@ -333,17 +323,6 @@ enum WIFI_REG_DOMAIN {
 #define GetFragNum(pbuf)			\
 	(le16_to_cpu(*(__le16 *)((size_t)(pbuf) + 22)) & 0x0f)
 
-#define GetTupleCache(pbuf)			\
-	(cpu_to_le16(*(unsigned short *)((size_t)(pbuf) + 22)))
-
-#define SetFragNum(pbuf, num) \
-	do {    \
-		*(unsigned short *)((size_t)(pbuf) + 22) = \
-			((*(unsigned short *)((size_t)(pbuf) + 22)) &	\
-			le16_to_cpu(~(0x000f))) | \
-			cpu_to_le16(0x0f & (num));     \
-	} while (0)
-
 #define SetSeqNum(pbuf, num) \
 	do {    \
 		*(__le16 *)((size_t)(pbuf) + 22) = \
@@ -370,15 +349,8 @@ enum WIFI_REG_DOMAIN {
 
 #define GetAMsdu(pbuf) (((le16_to_cpu(*(__le16 *)pbuf)) >> 7) & 0x1)
 
-#define SetAMsdu(pbuf, amsdu)	\
-	*(__le16 *)(pbuf) |= cpu_to_le16((amsdu & 1) << 7)
-
 #define GetAid(pbuf)	(le16_to_cpu(*(__le16 *)((size_t)(pbuf) + 2)) & 0x3fff)
 
-#define GetTid(pbuf)	(le16_to_cpu(*(__le16 *)((size_t)(pbuf) +	\
-			(((GetToDs(pbuf)<<1) | GetFrDs(pbuf)) == 3 ?	\
-			30 : 24))) & 0x000f)
-
 #define GetAddr1Ptr(pbuf)	((unsigned char *)((size_t)(pbuf) + 4))
 
 #define GetAddr2Ptr(pbuf)	((unsigned char *)((size_t)(pbuf) + 10))
@@ -916,7 +888,7 @@ enum ht_cap_ampdu_factor {
 #define	P2P_STATUS_FAIL_INCOMPATIBLE_PROVSION		0x0A
 #define	P2P_STATUS_FAIL_USER_REJECT			0x0B
 
-/*	Value of Inviation Flags Attribute */
+/*	Value of Invitation Flags Attribute */
 #define	P2P_INVITATION_FLAGS_PERSISTENT			BIT(0)
 
 #define	DMP_P2P_DEVCAP_SUPPORT	(P2P_DEVCAP_SERVICE_DISCOVERY | \
@@ -970,7 +942,7 @@ enum ht_cap_ampdu_factor {
 
 #define	P2P_WILDCARD_SSID_LEN			7
 
-/* default value, used when: (1)p2p disabed or (2)p2p enabled
+/* default value, used when: (1)p2p disabled or (2)p2p enabled
  * but only do 1 scan phase */
 #define	P2P_FINDPHASE_EX_NONE		0
 /*  used when p2p enabled and want to do 1 scan phase and
@@ -1035,13 +1007,13 @@ enum P2P_STATE {
 	P2P_STATE_TX_PROVISION_DIS_REQ = 6,
 	P2P_STATE_RX_PROVISION_DIS_RSP = 7,
 	P2P_STATE_RX_PROVISION_DIS_REQ = 8,
-	/* Doing the group owner negoitation handshake */
+	/* Doing the group owner negotiation handshake */
 	P2P_STATE_GONEGO_ING = 9,
-	/* finish the group negoitation handshake with success */
+	/* finish the group negotiation handshake with success */
 	P2P_STATE_GONEGO_OK = 10,
-	/* finish the group negoitation handshake with failure */
+	/* finish the group negotiation handshake with failure */
 	P2P_STATE_GONEGO_FAIL = 11,
-	/* receiving the P2P Inviation request and match with the profile. */
+	/* receiving the P2P Invitation request and match with the profile. */
 	P2P_STATE_RECV_INVITE_REQ_MATCH = 12,
 	/* Doing the P2P WPS */
 	P2P_STATE_PROVISIONING_ING = 13,
@@ -1051,17 +1023,17 @@ enum P2P_STATE {
 	P2P_STATE_TX_INVITE_REQ = 15,
 	/* Receiving the P2P Invitation response */
 	P2P_STATE_RX_INVITE_RESP_OK = 16,
-	/* receiving the P2P Inviation request and dismatch with the profile. */
+	/* receiving the P2P Invitation request and dismatch with the profile. */
 	P2P_STATE_RECV_INVITE_REQ_DISMATCH = 17,
-	/* receiving the P2P Inviation request and this wifi is GO. */
+	/* receiving the P2P Invitation request and this wifi is GO. */
 	P2P_STATE_RECV_INVITE_REQ_GO = 18,
-	/* receiving the P2P Inviation request to join an existing P2P Group. */
+	/* receiving the P2P Invitation request to join an existing P2P Group. */
 	P2P_STATE_RECV_INVITE_REQ_JOIN = 19,
-	/* recveing the P2P Inviation response with failure */
+	/* receiving the P2P Invitation response with failure */
 	P2P_STATE_RX_INVITE_RESP_FAIL = 20,
-	/* receiving p2p negoitation response with information is not available */
+	/* receiving p2p negotiation response with information is not available */
 	P2P_STATE_RX_INFOR_NOREADY = 21,
-	/* sending p2p negoitation response with information is not available */
+	/* sending p2p negotiation response with information is not available */
 	P2P_STATE_TX_INFOR_NOREADY = 22,
 };
 
diff --git a/drivers/staging/rtl8188eu/include/wlan_bssdef.h b/drivers/staging/rtl8188eu/include/wlan_bssdef.h
index 53b1bd8e9946..85b99da49a2d 100644
--- a/drivers/staging/rtl8188eu/include/wlan_bssdef.h
+++ b/drivers/staging/rtl8188eu/include/wlan_bssdef.h
@@ -156,16 +156,6 @@ enum ndis_802_11_reload_def {
 	Ndis802_11ReloadWEPKeys
 };
 
-/*  Key mapping keys require a BSSID */
-struct ndis_802_11_key {
-	u32           Length;             /*  Length of this structure */
-	u32           KeyIndex;
-	u32           KeyLength;          /*  length of key in bytes */
-	unsigned char BSSID[ETH_ALEN];
-	unsigned long long KeyRSC;
-	u8           KeyMaterial[32];  /* var len depending on above field */
-};
-
 struct ndis_802_11_remove_key {
 	u32                   Length;        /*  Length */
 	u32                   KeyIndex;
diff --git a/drivers/staging/rtl8188eu/os_dep/ioctl_linux.c b/drivers/staging/rtl8188eu/os_dep/ioctl_linux.c
index 24a8f5ac96e5..96c1c2d4a112 100644
--- a/drivers/staging/rtl8188eu/os_dep/ioctl_linux.c
+++ b/drivers/staging/rtl8188eu/os_dep/ioctl_linux.c
@@ -32,6 +32,8 @@
 
 #include <rtw_iol.h>
 #include <linux/vmalloc.h>
+#include <linux/etherdevice.h>
+
 #include "osdep_intf.h"
 
 #define RTL_IOCTL_WPA_SUPPLICANT	(SIOCIWFIRSTPRIV + 30)
@@ -92,7 +94,7 @@ void rtw_indicate_wx_disassoc_event(struct adapter *padapter)
 	memset(&wrqu, 0, sizeof(union iwreq_data));
 
 	wrqu.ap_addr.sa_family = ARPHRD_ETHER;
-	memset(wrqu.ap_addr.sa_data, 0, ETH_ALEN);
+	eth_zero_addr(wrqu.ap_addr.sa_data);
 
 	DBG_88E_LEVEL(_drv_always_, "indicate disassoc\n");
 	wireless_send_event(padapter->pnetdev, SIOCGIWAP, &wrqu, NULL);
@@ -827,7 +829,7 @@ static int rtw_wx_set_pmkid(struct net_device *dev,
 		for (j = 0; j < NUM_PMKID_CACHE; j++) {
 			if (!memcmp(psecuritypriv->PMKIDList[j].Bssid, strIssueBssid, ETH_ALEN)) {
 				/*  BSSID is matched, the same AP => Remove this PMKID information and reset it. */
-				memset(psecuritypriv->PMKIDList[j].Bssid, 0x00, ETH_ALEN);
+				eth_zero_addr(psecuritypriv->PMKIDList[j].Bssid);
 				psecuritypriv->PMKIDList[j].bUsed = false;
 				break;
 			}
@@ -1028,7 +1030,7 @@ static int rtw_wx_get_wap(struct net_device *dev,
 
 	wrqu->ap_addr.sa_family = ARPHRD_ETHER;
 
-	memset(wrqu->ap_addr.sa_data, 0, ETH_ALEN);
+	eth_zero_addr(wrqu->ap_addr.sa_data);
 
 	RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("rtw_wx_get_wap\n"));
 
@@ -1037,7 +1039,7 @@ static int rtw_wx_get_wap(struct net_device *dev,
 	    ((check_fwstate(pmlmepriv, WIFI_AP_STATE)) == true))
 		memcpy(wrqu->ap_addr.sa_data, pcur_bss->MacAddress, ETH_ALEN);
 	else
-		memset(wrqu->ap_addr.sa_data, 0, ETH_ALEN);
+		eth_zero_addr(wrqu->ap_addr.sa_data);
 	return 0;
 }
 
@@ -1796,11 +1798,9 @@ static int rtw_wx_set_gen_ie(struct net_device *dev,
 			     struct iw_request_info *info,
 			     union iwreq_data *wrqu, char *extra)
 {
-	int ret;
 	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
 
-	ret = rtw_set_wpa_ie(padapter, extra, wrqu->data.length);
-	return ret;
+	return rtw_set_wpa_ie(padapter, extra, wrqu->data.length);
 }
 
 static int rtw_wx_set_auth(struct net_device *dev,
diff --git a/drivers/staging/rtl8188eu/os_dep/mlme_linux.c b/drivers/staging/rtl8188eu/os_dep/mlme_linux.c
index 1b892c424cb8..baff1e2661d5 100644
--- a/drivers/staging/rtl8188eu/os_dep/mlme_linux.c
+++ b/drivers/staging/rtl8188eu/os_dep/mlme_linux.c
@@ -29,9 +29,12 @@ void rtw_init_mlme_timer(struct adapter *padapter)
 {
 	struct	mlme_priv *pmlmepriv = &padapter->mlmepriv;
 
-	_init_timer(&(pmlmepriv->assoc_timer), padapter->pnetdev, _rtw_join_timeout_handler, padapter);
-	_init_timer(&(pmlmepriv->scan_to_timer), padapter->pnetdev, rtw_scan_timeout_handler, padapter);
-	_init_timer(&(pmlmepriv->dynamic_chk_timer), padapter->pnetdev, rtw_dynamic_check_timer_handlder, padapter);
+	setup_timer(&pmlmepriv->assoc_timer, _rtw_join_timeout_handler,
+		    (unsigned long)padapter);
+	setup_timer(&pmlmepriv->scan_to_timer, rtw_scan_timeout_handler,
+		    (unsigned long)padapter);
+	setup_timer(&pmlmepriv->dynamic_chk_timer,
+		    rtw_dynamic_check_timer_handlder, (unsigned long)padapter);
 }
 
 void rtw_os_indicate_connect(struct adapter *adapter)
@@ -60,7 +63,6 @@ void rtw_reset_securitypriv(struct adapter *adapter)
 		/*  We have to backup the PMK information for WiFi PMK Caching test item. */
 		/*  Backup the btkip_countermeasure information. */
 		/*  When the countermeasure is trigger, the driver have to disconnect with AP for 60 seconds. */
-		memset(&backup_pmkid[0], 0x00, sizeof(struct rt_pmkid_list) * NUM_PMKID_CACHE);
 		memcpy(&backup_pmkid[0], &adapter->securitypriv.PMKIDList[0], sizeof(struct rt_pmkid_list) * NUM_PMKID_CACHE);
 		backup_index = adapter->securitypriv.PMKIDIndex;
 		backup_counter = adapter->securitypriv.btkip_countermeasure;
@@ -131,15 +133,18 @@ void rtw_report_sec_ie(struct adapter *adapter, u8 authmode, u8 *sec_ie)
 
 void init_addba_retry_timer(struct adapter *padapter, struct sta_info *psta)
 {
-	_init_timer(&psta->addba_retry_timer, padapter->pnetdev, addba_timer_hdl, psta);
+	setup_timer(&psta->addba_retry_timer, addba_timer_hdl,
+		    (unsigned long)psta);
 }
 
 void init_mlme_ext_timer(struct adapter *padapter)
 {
 	struct	mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 
-	_init_timer(&pmlmeext->survey_timer, padapter->pnetdev, survey_timer_hdl, padapter);
-	_init_timer(&pmlmeext->link_timer, padapter->pnetdev, link_timer_hdl, padapter);
+	setup_timer(&pmlmeext->survey_timer, survey_timer_hdl,
+		    (unsigned long)padapter);
+	setup_timer(&pmlmeext->link_timer, link_timer_hdl,
+		    (unsigned long)padapter);
 }
 
 #ifdef CONFIG_88EU_AP_MODE
diff --git a/drivers/staging/rtl8188eu/os_dep/os_intfs.c b/drivers/staging/rtl8188eu/os_dep/os_intfs.c
index 88a909c9e457..750c87b46365 100644
--- a/drivers/staging/rtl8188eu/os_dep/os_intfs.c
+++ b/drivers/staging/rtl8188eu/os_dep/os_intfs.c
@@ -38,7 +38,7 @@ MODULE_VERSION(DRIVERVERSION);
 #define RTW_NOTCH_FILTER 0 /* 0:Disable, 1:Enable, */
 
 /* module param defaults */
-static int rtw_chip_version = 0x00;
+static int rtw_chip_version;
 static int rtw_rfintfs = HWPI;
 static int rtw_lbkmode;/* RTL8712_AIR_TRX; */
 static int rtw_network_mode = Ndis802_11IBSS;/* Ndis802_11Infrastructure; infra, ad-hoc, auto */
@@ -998,7 +998,8 @@ int _netdev_open(struct net_device *pnetdev)
 	}
 	padapter->net_closed = false;
 
-	_set_timer(&padapter->mlmepriv.dynamic_chk_timer, 2000);
+	mod_timer(&padapter->mlmepriv.dynamic_chk_timer,
+		  jiffies + msecs_to_jiffies(2000));
 
 	padapter->pwrctrlpriv.bips_processing = false;
 	rtw_set_pwr_state_check_timer(&padapter->pwrctrlpriv);
@@ -1052,7 +1053,8 @@ static int  ips_netdrv_open(struct adapter *padapter)
 		padapter->intf_start(padapter);
 
 	rtw_set_pwr_state_check_timer(&padapter->pwrctrlpriv);
-	_set_timer(&padapter->mlmepriv.dynamic_chk_timer, 5000);
+	mod_timer(&padapter->mlmepriv.dynamic_chk_timer,
+		  jiffies + msecs_to_jiffies(5000));
 
 	 return _SUCCESS;
 
diff --git a/drivers/staging/rtl8188eu/os_dep/recv_linux.c b/drivers/staging/rtl8188eu/os_dep/recv_linux.c
index 05427c489b3f..05701328dce4 100644
--- a/drivers/staging/rtl8188eu/os_dep/recv_linux.c
+++ b/drivers/staging/rtl8188eu/os_dep/recv_linux.c
@@ -193,7 +193,8 @@ _recv_indicatepkt_drop:
 
 void rtw_init_recv_timer(struct recv_reorder_ctrl *preorder_ctrl)
 {
-	struct adapter *padapter = preorder_ctrl->padapter;
 
-	_init_timer(&(preorder_ctrl->reordering_ctrl_timer), padapter->pnetdev, rtw_reordering_ctrl_timeout_handler, preorder_ctrl);
+	setup_timer(&preorder_ctrl->reordering_ctrl_timer,
+		    rtw_reordering_ctrl_timeout_handler,
+		    (unsigned long)preorder_ctrl);
 }
diff --git a/drivers/staging/rtl8188eu/os_dep/usb_intf.c b/drivers/staging/rtl8188eu/os_dep/usb_intf.c
index bee39c2278f1..ef3c73e38172 100644
--- a/drivers/staging/rtl8188eu/os_dep/usb_intf.c
+++ b/drivers/staging/rtl8188eu/os_dep/usb_intf.c
@@ -179,7 +179,7 @@ static void usb_intf_stop(struct adapter *padapter)
 {
 	RT_TRACE(_module_hci_intfs_c_, _drv_err_, ("+usb_intf_stop\n"));
 
-	/* disabel_hw_interrupt */
+	/* disable_hw_interrupt */
 	if (!padapter->bSurpriseRemoved) {
 		/* device still exists, so driver can do i/o operation */
 		/* TODO: */
diff --git a/drivers/staging/rtl8188eu/os_dep/usb_ops_linux.c b/drivers/staging/rtl8188eu/os_dep/usb_ops_linux.c
index 80e7ef96d807..7e599bc5b2d3 100644
--- a/drivers/staging/rtl8188eu/os_dep/usb_ops_linux.c
+++ b/drivers/staging/rtl8188eu/os_dep/usb_ops_linux.c
@@ -530,6 +530,7 @@ void usb_read_port_cancel(struct adapter *padapter)
 {
 	int i;
 	struct recv_buf *precvbuf;
+
 	precvbuf = (struct recv_buf *)padapter->recvpriv.precv_buf;
 
 	DBG_88E("%s\n", __func__);
@@ -552,7 +553,6 @@ int usb_write8(struct adapter *adapter, u32 addr, u8 val)
 	u16 index;
 	u16 len;
 	u8 data;
-	int ret;
 
 	request = 0x05;
 	requesttype = 0x00;/* write_out */
@@ -560,8 +560,8 @@ int usb_write8(struct adapter *adapter, u32 addr, u8 val)
 	wvalue = (u16)(addr&0x0000ffff);
 	len = 1;
 	data = val;
-	ret = usbctrl_vendorreq(adapter, request, wvalue, index, &data, len, requesttype);
-	return ret;
+	return usbctrl_vendorreq(adapter, request, wvalue,
+				 index, &data, len, requesttype);
 }
 
 int usb_write16(struct adapter *adapter, u32 addr, u16 val)
@@ -572,7 +572,6 @@ int usb_write16(struct adapter *adapter, u32 addr, u16 val)
 	u16 index;
 	u16 len;
 	__le32 data;
-	int ret;
 
 
 	request = 0x05;
@@ -584,10 +583,10 @@ int usb_write16(struct adapter *adapter, u32 addr, u16 val)
 
 	data = cpu_to_le32(val & 0x0000ffff);
 
-	ret = usbctrl_vendorreq(adapter, request, wvalue, index, &data, len, requesttype);
+	return usbctrl_vendorreq(adapter, request, wvalue,
+				 index, &data, len, requesttype);
 
 
-	return ret;
 }
 
 int usb_write32(struct adapter *adapter, u32 addr, u32 val)
@@ -598,7 +597,6 @@ int usb_write32(struct adapter *adapter, u32 addr, u32 val)
 	u16 index;
 	u16 len;
 	__le32 data;
-	int ret;
 
 
 	request = 0x05;
@@ -609,10 +607,10 @@ int usb_write32(struct adapter *adapter, u32 addr, u32 val)
 	len = 4;
 	data = cpu_to_le32(val);
 
-	ret = usbctrl_vendorreq(adapter, request, wvalue, index, &data, len, requesttype);
+	return usbctrl_vendorreq(adapter, request, wvalue,
+				 index, &data, len, requesttype);
 
 
-	return ret;
 }
 
 static void usb_write_port_complete(struct urb *purb, struct pt_regs *regs)
diff --git a/drivers/staging/rtl8192e/dot11d.h b/drivers/staging/rtl8192e/dot11d.h
index eeea50260f1d..aad3394392fe 100644
--- a/drivers/staging/rtl8192e/dot11d.h
+++ b/drivers/staging/rtl8192e/dot11d.h
@@ -74,16 +74,11 @@ static inline void cpMacAddr(unsigned char *des, unsigned char *src)
 	(GET_DOT11D_INFO(__pIeeeDev)->CountryIeLen > 0)
 
 #define IS_EQUAL_CIE_SRC(__pIeeeDev, __pTa)		\
-	 eqMacAddr(GET_DOT11D_INFO(__pIeeeDev)->CountryIeSrcAddr, __pTa)
+	 ether_addr_equal_unaligned(GET_DOT11D_INFO(__pIeeeDev)->CountryIeSrcAddr, \
+	__pTa)
 #define UPDATE_CIE_SRC(__pIeeeDev, __pTa)		\
 	cpMacAddr(GET_DOT11D_INFO(__pIeeeDev)->CountryIeSrcAddr, __pTa)
 
-#define IS_COUNTRY_IE_CHANGED(__pIeeeDev, __Ie) \
-	(((__Ie).Length == 0 || (__Ie).Length !=	\
-	GET_DOT11D_INFO(__pIeeeDev)->CountryIeLen) ?	\
-	false : (!memcmp(GET_DOT11D_INFO(__pIeeeDev)->CountryIeBuf,	\
-	(__Ie).Octet, (__Ie).Length)))
-
 #define CIE_WATCHDOG_TH 1
 #define GET_CIE_WATCHDOG(__pIeeeDev)				\
 	 (GET_DOT11D_INFO(__pIeeeDev)->CountryIeWatchdog)
diff --git a/drivers/staging/rtl8192e/rtl8192e/r8190P_rtl8256.c b/drivers/staging/rtl8192e/rtl8192e/r8190P_rtl8256.c
index 0e6bdd222e75..01d2201afc94 100644
--- a/drivers/staging/rtl8192e/rtl8192e/r8190P_rtl8256.c
+++ b/drivers/staging/rtl8192e/rtl8192e/r8190P_rtl8256.c
@@ -47,8 +47,8 @@ void PHY_SetRF8256Bandwidth(struct net_device *dev,
 						0x0e, bMask12Bits, 0x021);
 
 			} else {
-				RT_TRACE(COMP_ERR, "PHY_SetRF8256Bandwidth(): "
-					 "unknown hardware version\n");
+				RT_TRACE(COMP_ERR,
+					 "PHY_SetRF8256Bandwidth(): unknown hardware version\n");
 			}
 
 			break;
@@ -66,15 +66,16 @@ void PHY_SetRF8256Bandwidth(struct net_device *dev,
 						 0x0e, bMask12Bits, 0x0e1);
 
 			} else {
-				RT_TRACE(COMP_ERR, "PHY_SetRF8256Bandwidth(): "
-					 "unknown hardware version\n");
+				RT_TRACE(COMP_ERR,
+					 "PHY_SetRF8256Bandwidth(): unknown hardware version\n");
 			}
 
 
 			break;
 		default:
-			RT_TRACE(COMP_ERR, "PHY_SetRF8256Bandwidth(): unknown "
-				 "Bandwidth: %#X\n", Bandwidth);
+			RT_TRACE(COMP_ERR,
+				 "PHY_SetRF8256Bandwidth(): unknown Bandwidth: %#X\n",
+				 Bandwidth);
 			break;
 
 		}
@@ -138,8 +139,9 @@ bool phy_RF8256_Config_ParaFile(struct net_device *dev)
 		rtStatus = rtl8192_phy_checkBBAndRF(dev, HW90_BLOCK_RF,
 						(enum rf90_radio_path)eRFPath);
 		if (!rtStatus) {
-			RT_TRACE(COMP_ERR, "PHY_RF8256_Config():Check "
-				 "Radio[%d] Fail!!\n", eRFPath);
+			RT_TRACE(COMP_ERR,
+				 "PHY_RF8256_Config():Check Radio[%d] Fail!!\n",
+				 eRFPath);
 			goto phy_RF8256_Config_ParaFile_Fail;
 		}
 
@@ -155,9 +157,10 @@ bool phy_RF8256_Config_ParaFile(struct net_device *dev)
 						 (enum rf90_radio_path)eRFPath,
 						 RegOffSetToBeCheck,
 						 bMask12Bits);
-				RT_TRACE(COMP_RF, "RF %d %d register final "
-					 "value: %x\n", eRFPath,
-					 RegOffSetToBeCheck, RF3_Final_Value);
+				RT_TRACE(COMP_RF,
+					 "RF %d %d register final value: %x\n",
+					 eRFPath, RegOffSetToBeCheck,
+					 RF3_Final_Value);
 				RetryTimes--;
 			}
 			break;
@@ -170,9 +173,10 @@ bool phy_RF8256_Config_ParaFile(struct net_device *dev)
 						 (enum rf90_radio_path)eRFPath,
 						 RegOffSetToBeCheck,
 						 bMask12Bits);
-				RT_TRACE(COMP_RF, "RF %d %d register final "
-					 "value: %x\n", eRFPath,
-					  RegOffSetToBeCheck, RF3_Final_Value);
+				RT_TRACE(COMP_RF,
+					 "RF %d %d register final value: %x\n",
+					 eRFPath, RegOffSetToBeCheck,
+					 RF3_Final_Value);
 				RetryTimes--;
 			}
 			break;
@@ -185,9 +189,10 @@ bool phy_RF8256_Config_ParaFile(struct net_device *dev)
 						(enum rf90_radio_path)eRFPath,
 						RegOffSetToBeCheck,
 						bMask12Bits);
-				RT_TRACE(COMP_RF, "RF %d %d register final "
-					 "value: %x\n", eRFPath,
-					 RegOffSetToBeCheck, RF3_Final_Value);
+				RT_TRACE(COMP_RF,
+					 "RF %d %d register final value: %x\n",
+					 eRFPath, RegOffSetToBeCheck,
+					 RF3_Final_Value);
 				RetryTimes--;
 			}
 			break;
@@ -199,9 +204,10 @@ bool phy_RF8256_Config_ParaFile(struct net_device *dev)
 				RF3_Final_Value = rtl8192_phy_QueryRFReg(dev,
 					       (enum rf90_radio_path)eRFPath,
 					       RegOffSetToBeCheck, bMask12Bits);
-				RT_TRACE(COMP_RF, "RF %d %d register final "
-					 "value: %x\n", eRFPath,
-					  RegOffSetToBeCheck, RF3_Final_Value);
+				RT_TRACE(COMP_RF,
+					 "RF %d %d register final value: %x\n",
+					 eRFPath, RegOffSetToBeCheck,
+					 RF3_Final_Value);
 				RetryTimes--;
 			}
 			break;
@@ -221,8 +227,9 @@ bool phy_RF8256_Config_ParaFile(struct net_device *dev)
 		}
 
 		if (ret) {
-			RT_TRACE(COMP_ERR, "phy_RF8256_Config_ParaFile():"
-				 "Radio[%d] Fail!!", eRFPath);
+			RT_TRACE(COMP_ERR,
+				 "phy_RF8256_Config_ParaFile():Radio[%d] Fail!!",
+				 eRFPath);
 			goto phy_RF8256_Config_ParaFile_Fail;
 		}
 
diff --git a/drivers/staging/rtl8192e/rtl8192e/r8192E_cmdpkt.c b/drivers/staging/rtl8192e/rtl8192e/r8192E_cmdpkt.c
index 2b1430d66adb..ecdd2e5c6bac 100644
--- a/drivers/staging/rtl8192e/rtl8192e/r8192E_cmdpkt.c
+++ b/drivers/staging/rtl8192e/rtl8192e/r8192E_cmdpkt.c
@@ -46,7 +46,7 @@ bool cmpk_message_handle_tx(
 
 	do {
 		if ((buffer_len - frag_offset) > frag_threshold) {
-			frag_length = frag_threshold ;
+			frag_length = frag_threshold;
 			bLastIniPkt = 0;
 
 		} else {
@@ -165,7 +165,7 @@ static void cmdpkt_beacontimerinterrupt_819xusb(struct net_device *dev)
 
 static void cmpk_handle_interrupt_status(struct net_device *dev, u8 *pmsg)
 {
-	struct cmpk_intr_sta rx_intr_status;	/* */
+	struct cmpk_intr_sta rx_intr_status;
 	struct r8192_priv *priv = rtllib_priv(dev);
 
 	DMESG("---> cmpk_Handle_Interrupt_Status()\n");
@@ -332,44 +332,44 @@ u32 cmpk_message_handle_rx(struct net_device *dev,
 
 		switch (element_id) {
 		case RX_TX_FEEDBACK:
-			RT_TRACE(COMP_CMDPKT, "---->cmpk_message_handle_rx():"
-				 "RX_TX_FEEDBACK\n");
+			RT_TRACE(COMP_CMDPKT,
+				 "---->cmpk_message_handle_rx():RX_TX_FEEDBACK\n");
 			cmpk_handle_tx_feedback(dev, pcmd_buff);
 			cmd_length = CMPK_RX_TX_FB_SIZE;
 			break;
 		case RX_INTERRUPT_STATUS:
-			RT_TRACE(COMP_CMDPKT, "---->cmpk_message_handle_rx():"
-				 "RX_INTERRUPT_STATUS\n");
+			RT_TRACE(COMP_CMDPKT,
+				 "---->cmpk_message_handle_rx():RX_INTERRUPT_STATUS\n");
 			cmpk_handle_interrupt_status(dev, pcmd_buff);
 			cmd_length = sizeof(struct cmpk_intr_sta);
 			break;
 		case BOTH_QUERY_CONFIG:
-			RT_TRACE(COMP_CMDPKT, "---->cmpk_message_handle_rx():"
-				 "BOTH_QUERY_CONFIG\n");
+			RT_TRACE(COMP_CMDPKT,
+				 "---->cmpk_message_handle_rx():BOTH_QUERY_CONFIG\n");
 			cmpk_handle_query_config_rx(dev, pcmd_buff);
 			cmd_length = CMPK_BOTH_QUERY_CONFIG_SIZE;
 			break;
 		case RX_TX_STATUS:
-			RT_TRACE(COMP_CMDPKT, "---->cmpk_message_handle_rx():"
-				 "RX_TX_STATUS\n");
+			RT_TRACE(COMP_CMDPKT,
+				 "---->cmpk_message_handle_rx():RX_TX_STATUS\n");
 			cmpk_handle_tx_status(dev, pcmd_buff);
 			cmd_length = CMPK_RX_TX_STS_SIZE;
 			break;
 		case RX_TX_PER_PKT_FEEDBACK:
-			RT_TRACE(COMP_CMDPKT, "---->cmpk_message_handle_rx():"
-				 "RX_TX_PER_PKT_FEEDBACK\n");
+			RT_TRACE(COMP_CMDPKT,
+				 "---->cmpk_message_handle_rx():RX_TX_PER_PKT_FEEDBACK\n");
 			cmd_length = CMPK_RX_TX_FB_SIZE;
 			break;
 		case RX_TX_RATE_HISTORY:
-			RT_TRACE(COMP_CMDPKT, "---->cmpk_message_handle_rx():"
-				 "RX_TX_HISTORY\n");
+			RT_TRACE(COMP_CMDPKT,
+				 "---->cmpk_message_handle_rx():RX_TX_HISTORY\n");
 			cmpk_handle_tx_rate_history(dev, pcmd_buff);
 			cmd_length = CMPK_TX_RAHIS_SIZE;
 			break;
 		default:
 
-			RT_TRACE(COMP_CMDPKT, "---->cmpk_message_handle_rx():"
-				 "unknown CMD Element\n");
+			RT_TRACE(COMP_CMDPKT,
+				 "---->cmpk_message_handle_rx():unknown CMD Element\n");
 			return 1;
 		}
 
diff --git a/drivers/staging/rtl8192e/rtl8192e/r8192E_cmdpkt.h b/drivers/staging/rtl8192e/rtl8192e/r8192E_cmdpkt.h
index 23219e17e5a1..2693682644df 100644
--- a/drivers/staging/rtl8192e/rtl8192e/r8192E_cmdpkt.h
+++ b/drivers/staging/rtl8192e/rtl8192e/r8192E_cmdpkt.h
@@ -33,28 +33,28 @@
 struct cmpk_txfb {
 	u8	element_id;
 	u8	length;
-	u8	TID:4;				/* */
-	u8	fail_reason:3;		/* */
+	u8	TID:4;
+	u8	fail_reason:3;
 	u8	tok:1;
-	u8	reserve1:4;			/* */
-	u8	pkt_type:2;		/* */
-	u8	bandwidth:1;		/* */
-	u8	qos_pkt:1;			/* */
+	u8	reserve1:4;
+	u8	pkt_type:2;
+	u8	bandwidth:1;
+	u8	qos_pkt:1;
 
-	u8	reserve2;			/* */
-	u8	retry_cnt;			/* */
-	u16	pkt_id;				/* */
+	u8	reserve2;
+	u8	retry_cnt;
+	u16	pkt_id;
 
-	u16	seq_num;			/* */
+	u16	seq_num;
 	u8	s_rate;
 	u8	f_rate;
 
-	u8	s_rts_rate;			/* */
-	u8	f_rts_rate;			/* */
-	u16	pkt_length;			/* */
+	u8	s_rts_rate;
+	u8	f_rts_rate;
+	u16	pkt_length;
 
-	u16	reserve3;			/* */
-	u16	duration;			/* */
+	u16	reserve3;
+	u16	duration;
 };
 
 struct cmpk_intr_sta {
diff --git a/drivers/staging/rtl8192e/rtl8192e/r8192E_dev.c b/drivers/staging/rtl8192e/rtl8192e/r8192E_dev.c
index 552d943b1761..2869602436ef 100644
--- a/drivers/staging/rtl8192e/rtl8192e/r8192E_dev.c
+++ b/drivers/staging/rtl8192e/rtl8192e/r8192E_dev.c
@@ -224,8 +224,8 @@ void rtl8192e_SetHwReg(struct net_device *dev, u8 variable, u8 *val)
 			break;
 
 		default:
-			printk(KERN_INFO "SetHwReg8185(): invalid ACI: %d !\n",
-			       eACI);
+			netdev_info(dev, "SetHwReg8185(): invalid ACI: %d !\n",
+				    eACI);
 			break;
 		}
 		priv->rtllib->SetHwRegHandler(dev, HW_VAR_ACM_CTRL,
@@ -263,9 +263,9 @@ void rtl8192e_SetHwReg(struct net_device *dev, u8 variable, u8 *val)
 				break;
 
 			default:
-				RT_TRACE(COMP_QOS, "SetHwReg8185(): [HW_VAR_"
-					 "ACM_CTRL] acm set failed: eACI is "
-					 "%d\n", eACI);
+				RT_TRACE(COMP_QOS,
+					 "SetHwReg8185(): [HW_VAR_ACM_CTRL] acm set failed: eACI is %d\n",
+					 eACI);
 				break;
 			}
 		} else {
@@ -287,8 +287,9 @@ void rtl8192e_SetHwReg(struct net_device *dev, u8 variable, u8 *val)
 			}
 		}
 
-		RT_TRACE(COMP_QOS, "SetHwReg8190pci(): [HW_VAR_ACM_CTRL] Write"
-			 " 0x%X\n", AcmCtrl);
+		RT_TRACE(COMP_QOS,
+			 "SetHwReg8190pci(): [HW_VAR_ACM_CTRL] Write 0x%X\n",
+			 AcmCtrl);
 		write_nic_byte(dev, AcmHwCtrl, AcmCtrl);
 		break;
 	}
@@ -334,17 +335,17 @@ static void rtl8192_read_eeprom_info(struct net_device *dev)
 	}
 
 	if (!priv->AutoloadFailFlag) {
-		priv->eeprom_vid = eprom_read(dev, (EEPROM_VID >> 1));
-		priv->eeprom_did = eprom_read(dev, (EEPROM_DID >> 1));
+		priv->eeprom_vid = eprom_read(dev, EEPROM_VID >> 1);
+		priv->eeprom_did = eprom_read(dev, EEPROM_DID >> 1);
 
 		usValue = eprom_read(dev, (u16)(EEPROM_Customer_ID>>1)) >> 8;
 		priv->eeprom_CustomerID = (u8)(usValue & 0xff);
-		usValue = eprom_read(dev, (EEPROM_ICVersion_ChannelPlan>>1));
+		usValue = eprom_read(dev, EEPROM_ICVersion_ChannelPlan>>1);
 		priv->eeprom_ChannelPlan = usValue&0xff;
-		IC_Version = ((usValue&0xff00)>>8);
+		IC_Version = (usValue & 0xff00)>>8;
 
 		ICVer8192 = (IC_Version&0xf);
-		ICVer8256 = ((IC_Version&0xf0)>>4);
+		ICVer8256 = (IC_Version & 0xf0)>>4;
 		RT_TRACE(COMP_INIT, "\nICVer8192 = 0x%x\n", ICVer8192);
 		RT_TRACE(COMP_INIT, "\nICVer8256 = 0x%x\n", ICVer8256);
 		if (ICVer8192 == 0x2) {
@@ -424,7 +425,7 @@ static void rtl8192_read_eeprom_info(struct net_device *dev)
 		if (priv->epromtype == EEPROM_93C46) {
 			if (!priv->AutoloadFailFlag) {
 				usValue = eprom_read(dev,
-					  (EEPROM_TxPwDiff_CrystalCap >> 1));
+					  EEPROM_TxPwDiff_CrystalCap >> 1);
 				priv->EEPROMAntPwDiff = (usValue&0x0fff);
 				priv->EEPROMCrystalCap = (u8)((usValue & 0xf000)
 							 >> 12);
@@ -448,12 +449,12 @@ static void rtl8192_read_eeprom_info(struct net_device *dev)
 					usValue = EEPROM_Default_TxPower;
 				*((u16 *)(&priv->EEPROMTxPowerLevelCCK[i])) =
 								 usValue;
-				RT_TRACE(COMP_INIT, "CCK Tx Power Level, Index"
-					 " %d = 0x%02x\n", i,
-					 priv->EEPROMTxPowerLevelCCK[i]);
-				RT_TRACE(COMP_INIT, "CCK Tx Power Level, Index"
-					 " %d = 0x%02x\n", i+1,
-					 priv->EEPROMTxPowerLevelCCK[i+1]);
+				RT_TRACE(COMP_INIT,
+					 "CCK Tx Power Level, Index %d = 0x%02x\n",
+					 i, priv->EEPROMTxPowerLevelCCK[i]);
+				RT_TRACE(COMP_INIT,
+					 "CCK Tx Power Level, Index %d = 0x%02x\n",
+					 i+1, priv->EEPROMTxPowerLevelCCK[i+1]);
 			}
 			for (i = 0; i < 14; i += 2) {
 				if (!priv->AutoloadFailFlag)
@@ -464,11 +465,12 @@ static void rtl8192_read_eeprom_info(struct net_device *dev)
 					usValue = EEPROM_Default_TxPower;
 				*((u16 *)(&priv->EEPROMTxPowerLevelOFDM24G[i]))
 							 = usValue;
-				RT_TRACE(COMP_INIT, "OFDM 2.4G Tx Power Level,"
-					 " Index %d = 0x%02x\n", i,
-					 priv->EEPROMTxPowerLevelOFDM24G[i]);
-				RT_TRACE(COMP_INIT, "OFDM 2.4G Tx Power Level,"
-					 " Index %d = 0x%02x\n", i + 1,
+				RT_TRACE(COMP_INIT,
+					 "OFDM 2.4G Tx Power Level, Index %d = 0x%02x\n",
+					 i, priv->EEPROMTxPowerLevelOFDM24G[i]);
+				RT_TRACE(COMP_INIT,
+					 "OFDM 2.4G Tx Power Level, Index %d = 0x%02x\n",
+					 i + 1,
 					 priv->EEPROMTxPowerLevelOFDM24G[i+1]);
 			}
 		}
@@ -483,15 +485,15 @@ static void rtl8192_read_eeprom_info(struct net_device *dev)
 					 priv->EEPROMLegacyHTTxPowerDiff;
 			priv->AntennaTxPwDiff[0] = (priv->EEPROMAntPwDiff &
 						    0xf);
-			priv->AntennaTxPwDiff[1] = ((priv->EEPROMAntPwDiff &
-						    0xf0)>>4);
-			priv->AntennaTxPwDiff[2] = ((priv->EEPROMAntPwDiff &
-						    0xf00)>>8);
+			priv->AntennaTxPwDiff[1] = (priv->EEPROMAntPwDiff &
+							0xf0) >> 4;
+			priv->AntennaTxPwDiff[2] = (priv->EEPROMAntPwDiff &
+							0xf00) >> 8;
 			priv->CrystalCap = priv->EEPROMCrystalCap;
 			priv->ThermalMeter[0] = (priv->EEPROMThermalMeter &
 						 0xf);
-			priv->ThermalMeter[1] = ((priv->EEPROMThermalMeter &
-						 0xf0)>>4);
+			priv->ThermalMeter[1] = (priv->EEPROMThermalMeter &
+						     0xf0) >> 4;
 		} else if (priv->epromtype == EEPROM_93C56) {
 
 			for (i = 0; i < 3; i++) {
@@ -525,21 +527,21 @@ static void rtl8192_read_eeprom_info(struct net_device *dev)
 					 priv->EEPROMRfCOfdmChnlTxPwLevel[2];
 			}
 			for (i = 0; i < 14; i++)
-				RT_TRACE(COMP_INIT, "priv->TxPowerLevelCCK_A"
-					 "[%d] = 0x%x\n", i,
-					 priv->TxPowerLevelCCK_A[i]);
+				RT_TRACE(COMP_INIT,
+					 "priv->TxPowerLevelCCK_A[%d] = 0x%x\n",
+					 i, priv->TxPowerLevelCCK_A[i]);
 			for (i = 0; i < 14; i++)
-				RT_TRACE(COMP_INIT, "priv->TxPowerLevelOFDM"
-					 "24G_A[%d] = 0x%x\n", i,
-					 priv->TxPowerLevelOFDM24G_A[i]);
+				RT_TRACE(COMP_INIT,
+					 "priv->TxPowerLevelOFDM24G_A[%d] = 0x%x\n",
+					 i, priv->TxPowerLevelOFDM24G_A[i]);
 			for (i = 0; i < 14; i++)
-				RT_TRACE(COMP_INIT, "priv->TxPowerLevelCCK_C"
-					 "[%d] = 0x%x\n", i,
-					 priv->TxPowerLevelCCK_C[i]);
+				RT_TRACE(COMP_INIT,
+					 "priv->TxPowerLevelCCK_C[%d] = 0x%x\n",
+					 i, priv->TxPowerLevelCCK_C[i]);
 			for (i = 0; i < 14; i++)
-				RT_TRACE(COMP_INIT, "priv->TxPowerLevelOFDM"
-					 "24G_C[%d] = 0x%x\n", i,
-					 priv->TxPowerLevelOFDM24G_C[i]);
+				RT_TRACE(COMP_INIT,
+					 "priv->TxPowerLevelOFDM24G_C[%d] = 0x%x\n",
+					 i, priv->TxPowerLevelOFDM24G_C[i]);
 			priv->LegacyHTTxPowerDiff =
 				 priv->EEPROMLegacyHTTxPowerDiff;
 			priv->AntennaTxPwDiff[0] = 0;
@@ -548,8 +550,8 @@ static void rtl8192_read_eeprom_info(struct net_device *dev)
 			priv->CrystalCap = priv->EEPROMCrystalCap;
 			priv->ThermalMeter[0] = (priv->EEPROMThermalMeter &
 						 0xf);
-			priv->ThermalMeter[1] = ((priv->EEPROMThermalMeter &
-						 0xf0)>>4);
+			priv->ThermalMeter[1] = (priv->EEPROMThermalMeter &
+						     0xf0) >> 4;
 		}
 	}
 
@@ -735,8 +737,9 @@ start:
 	else if (priv->pFirmware->firmware_status == FW_STATUS_5_READY)
 		ulRegRead |= CPU_GEN_FIRMWARE_RESET;
 	else
-		RT_TRACE(COMP_ERR, "ERROR in %s(): undefined firmware state(%d)"
-			 "\n", __func__,   priv->pFirmware->firmware_status);
+		RT_TRACE(COMP_ERR,
+			 "ERROR in %s(): undefined firmware state(%d)\n",
+			 __func__,   priv->pFirmware->firmware_status);
 
 	write_nic_dword(dev, CPU_GEN, ulRegRead);
 
@@ -766,8 +769,8 @@ start:
 		else if (priv->LoopbackMode == RTL819X_MAC_LOOPBACK)
 			ulRegRead |= CPU_CCK_LOOPBACK;
 		else
-			RT_TRACE(COMP_ERR, "Serious error: wrong loopback"
-				 " mode setting\n");
+			RT_TRACE(COMP_ERR,
+				 "Serious error: wrong loopback mode setting\n");
 
 		write_nic_dword(dev, CPU_GEN, ulRegRead);
 
@@ -883,15 +886,15 @@ start:
 			  __func__);
 		MgntActSet_RF_State(dev, eRfOff, RF_CHANGE_BY_SW, true);
 	} else if (priv->rtllib->RfOffReason > RF_CHANGE_BY_PS) {
-		RT_TRACE((COMP_INIT|COMP_RF|COMP_POWER), "%s(): Turn off RF for"
-			 " RfOffReason(%d) ----------\n", __func__,
-			 priv->rtllib->RfOffReason);
+		RT_TRACE((COMP_INIT|COMP_RF|COMP_POWER),
+			 "%s(): Turn off RF for RfOffReason(%d) ----------\n",
+			 __func__, priv->rtllib->RfOffReason);
 		MgntActSet_RF_State(dev, eRfOff, priv->rtllib->RfOffReason,
 				    true);
 	} else if (priv->rtllib->RfOffReason >= RF_CHANGE_BY_IPS) {
-		RT_TRACE((COMP_INIT|COMP_RF|COMP_POWER), "%s(): Turn off RF for"
-			 " RfOffReason(%d) ----------\n", __func__,
-			 priv->rtllib->RfOffReason);
+		RT_TRACE((COMP_INIT|COMP_RF|COMP_POWER),
+			 "%s(): Turn off RF for RfOffReason(%d) ----------\n",
+			 __func__, priv->rtllib->RfOffReason);
 		MgntActSet_RF_State(dev, eRfOff, priv->rtllib->RfOffReason,
 				    true);
 	} else {
@@ -939,17 +942,17 @@ start:
 			priv->CCKPresentAttentuation_difference = 0;
 			priv->CCKPresentAttentuation =
 				  priv->CCKPresentAttentuation_20Mdefault;
-			RT_TRACE(COMP_POWER_TRACKING, "priv->rfa_txpower"
-				 "trackingindex_initial = %d\n",
+			RT_TRACE(COMP_POWER_TRACKING,
+				 "priv->rfa_txpowertrackingindex_initial = %d\n",
 				 priv->rfa_txpowertrackingindex);
-			RT_TRACE(COMP_POWER_TRACKING, "priv->rfa_txpower"
-				 "trackingindex_real__initial = %d\n",
+			RT_TRACE(COMP_POWER_TRACKING,
+				 "priv->rfa_txpowertrackingindex_real__initial = %d\n",
 				 priv->rfa_txpowertrackingindex_real);
-			RT_TRACE(COMP_POWER_TRACKING, "priv->CCKPresent"
-				 "Attentuation_difference_initial = %d\n",
+			RT_TRACE(COMP_POWER_TRACKING,
+				 "priv->CCKPresentAttentuation_difference_initial = %d\n",
 				  priv->CCKPresentAttentuation_difference);
-			RT_TRACE(COMP_POWER_TRACKING, "priv->CCKPresent"
-				 "Attentuation_initial = %d\n",
+			RT_TRACE(COMP_POWER_TRACKING,
+				 "priv->CCKPresentAttentuation_initial = %d\n",
 				 priv->CCKPresentAttentuation);
 			priv->btxpower_tracking = false;
 		}
@@ -1169,8 +1172,9 @@ static u8 rtl8192_MapHwQueueToFirmwareQueue(u8 QueueID, u8 priority)
 		QueueSelect = QSLT_HIGH;
 		break;
 	default:
-		RT_TRACE(COMP_ERR, "TransmitTCB(): Impossible Queue Selection:"
-			 " %d\n", QueueID);
+		RT_TRACE(COMP_ERR,
+			 "TransmitTCB(): Impossible Queue Selection: %d\n",
+			 QueueID);
 		break;
 	}
 	return QueueSelect;
@@ -1361,9 +1365,10 @@ static u8 HwRateToMRate90(bool bIsHT, u8 rate)
 			break;
 
 		default:
-			RT_TRACE(COMP_RECV, "HwRateToMRate90(): Non supported"
-				 "Rate [%x], bIsHT = %d!!!\n", rate, bIsHT);
-						  break;
+			RT_TRACE(COMP_RECV,
+				 "HwRateToMRate90(): Non supportedRate [%x], bIsHT = %d!!!\n",
+				 rate, bIsHT);
+			break;
 		}
 
 	} else {
@@ -1421,8 +1426,9 @@ static u8 HwRateToMRate90(bool bIsHT, u8 rate)
 			break;
 
 		default:
-			RT_TRACE(COMP_RECV, "HwRateToMRate90(): Non supported "
-				 "Rate [%x], bIsHT = %d!!!\n", rate, bIsHT);
+			RT_TRACE(COMP_RECV,
+				 "HwRateToMRate90(): Non supported Rate [%x], bIsHT = %d!!!\n",
+				 rate, bIsHT);
 			break;
 		}
 	}
@@ -1527,7 +1533,7 @@ static void rtl8192_query_rxphystatus(
 		priv->stats.numqry_phystatusCCK++;
 		if (!reg824_bit9) {
 			report = pcck_buf->cck_agc_rpt & 0xc0;
-			report = report>>6;
+			report >>= 6;
 			switch (report) {
 			case 0x3:
 				rx_pwr_all = -35 - (pcck_buf->cck_agc_rpt &
@@ -1547,7 +1553,7 @@ static void rtl8192_query_rxphystatus(
 			}
 		} else {
 			report = pcck_buf->cck_agc_rpt & 0x60;
-			report = report>>5;
+			report >>= 5;
 			switch (report) {
 			case 0x3:
 				rx_pwr_all = -35 -
@@ -1737,8 +1743,8 @@ static void rtl8192_process_phyinfo(struct r8192_priv *priv, u8 *buffer,
 			if (!rtl8192_phy_CheckIsLegalRFPath(priv->rtllib->dev,
 			    rfpath))
 				continue;
-			RT_TRACE(COMP_DBG, "Jacken -> pPreviousstats->RxMIMO"
-				 "SignalStrength[rfpath]  = %d\n",
+			RT_TRACE(COMP_DBG,
+				 "Jacken -> pPreviousstats->RxMIMOSignalStrength[rfpath]  = %d\n",
 				 prev_st->RxMIMOSignalStrength[rfpath]);
 			if (priv->stats.rx_rssi_percentage[rfpath] == 0) {
 				priv->stats.rx_rssi_percentage[rfpath] =
@@ -1761,8 +1767,8 @@ static void rtl8192_process_phyinfo(struct r8192_priv *priv, u8 *buffer,
 				   (prev_st->RxMIMOSignalStrength[rfpath])) /
 				   (RX_SMOOTH);
 			}
-			RT_TRACE(COMP_DBG, "Jacken -> priv->RxStats.RxRSSI"
-				 "Percentage[rfPath]  = %d\n",
+			RT_TRACE(COMP_DBG,
+				 "Jacken -> priv->RxStats.RxRSSIPercentage[rfPath]  = %d\n",
 				 priv->stats.rx_rssi_percentage[rfpath]);
 		}
 	}
@@ -2019,6 +2025,7 @@ bool rtl8192_rx_query_status_desc(struct net_device *dev,
 				  struct sk_buff *skb)
 {
 	struct r8192_priv *priv = rtllib_priv(dev);
+	struct rx_fwinfo *pDrvInfo = NULL;
 
 	stats->bICV = pdesc->ICV;
 	stats->bCRC = pdesc->CRC32;
@@ -2040,51 +2047,49 @@ bool rtl8192_rx_query_status_desc(struct net_device *dev,
 				priv->stats.rxcrcerrmid++;
 		}
 		return false;
-	} else {
-		struct rx_fwinfo *pDrvInfo = NULL;
+	}
 
-		stats->RxDrvInfoSize = pdesc->RxDrvInfoSize;
-		stats->RxBufShift = ((pdesc->Shift)&0x03);
-		stats->Decrypted = !pdesc->SWDec;
+	stats->RxDrvInfoSize = pdesc->RxDrvInfoSize;
+	stats->RxBufShift = ((pdesc->Shift)&0x03);
+	stats->Decrypted = !pdesc->SWDec;
 
-		pDrvInfo = (struct rx_fwinfo *)(skb->data + stats->RxBufShift);
+	pDrvInfo = (struct rx_fwinfo *)(skb->data + stats->RxBufShift);
 
-		stats->rate = HwRateToMRate90((bool)pDrvInfo->RxHT,
-					     (u8)pDrvInfo->RxRate);
-		stats->bShortPreamble = pDrvInfo->SPLCP;
+	stats->rate = HwRateToMRate90((bool)pDrvInfo->RxHT,
+				     (u8)pDrvInfo->RxRate);
+	stats->bShortPreamble = pDrvInfo->SPLCP;
 
-		rtl8192_UpdateReceivedRateHistogramStatistics(dev, stats);
+	rtl8192_UpdateReceivedRateHistogramStatistics(dev, stats);
 
-		stats->bIsAMPDU = (pDrvInfo->PartAggr == 1);
-		stats->bFirstMPDU = (pDrvInfo->PartAggr == 1) &&
-				    (pDrvInfo->FirstAGGR == 1);
+	stats->bIsAMPDU = (pDrvInfo->PartAggr == 1);
+	stats->bFirstMPDU = (pDrvInfo->PartAggr == 1) &&
+			    (pDrvInfo->FirstAGGR == 1);
 
-		stats->TimeStampLow = pDrvInfo->TSFL;
-		stats->TimeStampHigh = read_nic_dword(dev, TSFR+4);
+	stats->TimeStampLow = pDrvInfo->TSFL;
+	stats->TimeStampHigh = read_nic_dword(dev, TSFR+4);
 
-		rtl819x_UpdateRxPktTimeStamp(dev, stats);
+	rtl819x_UpdateRxPktTimeStamp(dev, stats);
 
-		if ((stats->RxBufShift + stats->RxDrvInfoSize) > 0)
-			stats->bShift = 1;
+	if ((stats->RxBufShift + stats->RxDrvInfoSize) > 0)
+		stats->bShift = 1;
 
-		stats->RxIs40MHzPacket = pDrvInfo->BW;
+	stats->RxIs40MHzPacket = pDrvInfo->BW;
 
-		rtl8192_TranslateRxSignalStuff(dev, skb, stats, pdesc,
-					       pDrvInfo);
+	rtl8192_TranslateRxSignalStuff(dev, skb, stats, pdesc,
+				       pDrvInfo);
 
-		if (pDrvInfo->FirstAGGR == 1 || pDrvInfo->PartAggr == 1)
-			RT_TRACE(COMP_RXDESC, "pDrvInfo->FirstAGGR = %d,"
-				 " pDrvInfo->PartAggr = %d\n",
-				 pDrvInfo->FirstAGGR, pDrvInfo->PartAggr);
-		skb_trim(skb, skb->len - 4/*sCrcLng*/);
+	if (pDrvInfo->FirstAGGR == 1 || pDrvInfo->PartAggr == 1)
+		RT_TRACE(COMP_RXDESC,
+			 "pDrvInfo->FirstAGGR = %d, pDrvInfo->PartAggr = %d\n",
+			 pDrvInfo->FirstAGGR, pDrvInfo->PartAggr);
+	skb_trim(skb, skb->len - 4/*sCrcLng*/);
 
 
-		stats->packetlength = stats->Length-4;
-		stats->fraglength = stats->packetlength;
-		stats->fragoffset = 0;
-		stats->ntotalfrag = 1;
-		return true;
-	}
+	stats->packetlength = stats->Length-4;
+	stats->fraglength = stats->packetlength;
+	stats->fragoffset = 0;
+	stats->ntotalfrag = 1;
+	return true;
 }
 
 void rtl8192_halt_adapter(struct net_device *dev, bool reset)
@@ -2132,7 +2137,6 @@ void rtl8192_halt_adapter(struct net_device *dev, bool reset)
 		skb_queue_purge(&priv->rtllib->skb_aggQ[i]);
 
 	skb_queue_purge(&priv->skb_queue);
-	return;
 }
 
 void rtl8192_update_ratr_table(struct net_device *dev)
@@ -2191,8 +2195,7 @@ rtl8192_InitializeVariables(struct net_device  *dev)
 
 	priv->rtllib->softmac_features  = IEEE_SOFTMAC_SCAN |
 		IEEE_SOFTMAC_ASSOCIATE | IEEE_SOFTMAC_PROBERQ |
-		IEEE_SOFTMAC_PROBERS | IEEE_SOFTMAC_TX_QUEUE /* |
-		IEEE_SOFTMAC_BEACONS*/;
+		IEEE_SOFTMAC_PROBERS | IEEE_SOFTMAC_TX_QUEUE;
 
 	priv->rtllib->tx_headroom = sizeof(struct tx_fwinfo_8190pci);
 
@@ -2308,8 +2311,7 @@ bool rtl8192_HalRxCheckStuck(struct net_device *dev)
 	  (priv->undecorated_smoothed_pwdb >= RateAdaptiveTH_Low_20M)))) {
 		if (rx_chk_cnt < 2)
 			return bStuck;
-		else
-			rx_chk_cnt = 0;
+		rx_chk_cnt = 0;
 	} else if ((((priv->CurrentChannelBW != HT_CHANNEL_WIDTH_20) &&
 		  (priv->undecorated_smoothed_pwdb < RateAdaptiveTH_Low_40M)) ||
 		((priv->CurrentChannelBW == HT_CHANNEL_WIDTH_20) &&
@@ -2317,13 +2319,11 @@ bool rtl8192_HalRxCheckStuck(struct net_device *dev)
 		priv->undecorated_smoothed_pwdb >= VeryLowRSSI) {
 		if (rx_chk_cnt < 4)
 			return bStuck;
-		else
-			rx_chk_cnt = 0;
+		rx_chk_cnt = 0;
 	} else {
 		if (rx_chk_cnt < 8)
 			return bStuck;
-		else
-			rx_chk_cnt = 0;
+		rx_chk_cnt = 0;
 	}
 
 
@@ -2413,5 +2413,4 @@ void ActUpdateChannelAccessSetting(struct net_device *dev,
 	enum wireless_mode WirelessMode,
 	struct channel_access_setting *ChnlAccessSetting)
 {
-	return;
 }
diff --git a/drivers/staging/rtl8192e/rtl8192e/r8192E_firmware.c b/drivers/staging/rtl8192e/rtl8192e/r8192E_firmware.c
index 2e28744b9aab..c465f8749acd 100644
--- a/drivers/staging/rtl8192e/rtl8192e/r8192E_firmware.c
+++ b/drivers/staging/rtl8192e/rtl8192e/r8192E_firmware.c
@@ -36,7 +36,6 @@ static bool fw_download_code(struct net_device *dev, u8 *code_virtual_address,
 			     u32 buffer_len)
 {
 	struct r8192_priv *priv = rtllib_priv(dev);
-	bool		    rt_status = true;
 	u16		    frag_threshold;
 	u16		    frag_length, frag_offset = 0;
 	int		    i;
@@ -51,7 +50,7 @@ static bool fw_download_code(struct net_device *dev, u8 *code_virtual_address,
 	frag_threshold = pfirmware->cmdpacket_frag_thresold;
 	do {
 		if ((buffer_len - frag_offset) > frag_threshold) {
-			frag_length = frag_threshold ;
+			frag_length = frag_threshold;
 			bLastIniPkt = 0;
 
 		} else {
@@ -84,8 +83,8 @@ static bool fw_download_code(struct net_device *dev, u8 *code_virtual_address,
 		if (!priv->rtllib->check_nic_enough_desc(dev, tcb_desc->queue_index) ||
 		    (!skb_queue_empty(&priv->rtllib->skb_waitQ[tcb_desc->queue_index])) ||
 		    (priv->rtllib->queue_stop)) {
-			RT_TRACE(COMP_FIRMWARE, "===================> tx "
-				 "full!\n");
+			RT_TRACE(COMP_FIRMWARE,
+				 "===================> tx full!\n");
 			skb_queue_tail(&priv->rtllib->skb_waitQ
 					[tcb_desc->queue_index], skb);
 		} else {
@@ -99,7 +98,7 @@ static bool fw_download_code(struct net_device *dev, u8 *code_virtual_address,
 
 	write_nic_byte(dev, TPPoll, TPPoll_CQ);
 
-	return rt_status;
+	return true;
 }
 
 static bool CPUcheck_maincodeok_turnonCPU(struct net_device *dev)
@@ -108,7 +107,7 @@ static bool CPUcheck_maincodeok_turnonCPU(struct net_device *dev)
 	u32		CPU_status = 0;
 	unsigned long   timeout;
 
-	timeout = jiffies + MSECS(200);
+	timeout = jiffies + msecs_to_jiffies(200);
 	while (time_before(jiffies, timeout)) {
 		CPU_status = read_nic_dword(dev, CPU_GEN);
 		if (CPU_status & CPU_GEN_PUT_CODE_OK)
@@ -128,7 +127,7 @@ static bool CPUcheck_maincodeok_turnonCPU(struct net_device *dev)
 		       (u8)((CPU_status|CPU_GEN_PWR_STB_CPU)&0xff));
 	mdelay(1);
 
-	timeout = jiffies + MSECS(200);
+	timeout = jiffies + msecs_to_jiffies(200);
 	while (time_before(jiffies, timeout)) {
 		CPU_status = read_nic_dword(dev, CPU_GEN);
 		if (CPU_status&CPU_GEN_BOOT_RDY)
@@ -156,7 +155,7 @@ static bool CPUcheck_firmware_ready(struct net_device *dev)
 	u32	CPU_status = 0;
 	unsigned long timeout;
 
-	timeout = jiffies + MSECS(20);
+	timeout = jiffies + msecs_to_jiffies(20);
 	while (time_before(jiffies, timeout)) {
 		CPU_status = read_nic_dword(dev, CPU_GEN);
 		if (CPU_status&CPU_GEN_FIRM_RDY)
@@ -197,8 +196,8 @@ static bool firmware_check_ready(struct net_device *dev,
 		if (rt_status)
 			pfirmware->firmware_status = FW_STATUS_3_TURNON_CPU;
 		else
-			RT_TRACE(COMP_FIRMWARE, "CPUcheck_maincodeok_turnon"
-				 "CPU fail!\n");
+			RT_TRACE(COMP_FIRMWARE,
+				 "CPUcheck_maincodeok_turnonCPU fail!\n");
 
 		break;
 
@@ -210,8 +209,9 @@ static bool firmware_check_ready(struct net_device *dev,
 		if (rt_status)
 			pfirmware->firmware_status = FW_STATUS_5_READY;
 		else
-			RT_TRACE(COMP_FIRMWARE, "CPUcheck_firmware_ready fail"
-				 "(%d)!\n", rt_status);
+			RT_TRACE(COMP_FIRMWARE,
+				 "CPUcheck_firmware_ready fail(%d)!\n",
+				 rt_status);
 
 		break;
 	default:
@@ -246,8 +246,8 @@ bool init_firmware(struct net_device *dev)
 		rst_opt = OPT_FIRMWARE_RESET;
 		starting_state = FW_INIT_STEP2_DATA;
 	} else {
-		RT_TRACE(COMP_FIRMWARE, "PlatformInitFirmware: undefined"
-			 " firmware state\n");
+		RT_TRACE(COMP_FIRMWARE,
+			 "PlatformInitFirmware: undefined firmware state\n");
 	}
 
 	for (init_step = starting_state; init_step <= FW_INIT_STEP2_DATA;
@@ -266,14 +266,14 @@ bool init_firmware(struct net_device *dev)
 						      fw_name[init_step],
 						      &priv->pdev->dev);
 				if (rc < 0) {
-					RT_TRACE(COMP_FIRMWARE, "request firmware fail!\n");
+					RT_TRACE(COMP_FIRMWARE,
+						 "request firmware fail!\n");
 					goto download_firmware_fail;
 				}
 				if (fw_entry->size >
 				    sizeof(pfirmware->firmware_buf[init_step])) {
-					RT_TRACE(COMP_FIRMWARE, "img file size "
-						 "exceed the container struct "
-						 "buffer fail!\n");
+					RT_TRACE(COMP_FIRMWARE,
+						 "img file size exceed the container struct buffer fail!\n");
 					goto download_firmware_fail;
 				}
 
@@ -301,13 +301,11 @@ bool init_firmware(struct net_device *dev)
 		file_length = pfirmware->firmware_buf_size[init_step];
 
 		rt_status = fw_download_code(dev, mapped_file, file_length);
-		if (!rt_status) {
+		if (!rt_status)
 			goto download_firmware_fail;
-		}
 
-		if (!firmware_check_ready(dev, init_step)) {
+		if (!firmware_check_ready(dev, init_step))
 			goto download_firmware_fail;
-		}
 	}
 
 	RT_TRACE(COMP_FIRMWARE, "Firmware Download Success\n");
diff --git a/drivers/staging/rtl8192e/rtl8192e/r8192E_hwimg.h b/drivers/staging/rtl8192e/rtl8192e/r8192E_hwimg.h
index 019836bb36c2..d804876dd92f 100644
--- a/drivers/staging/rtl8192e/rtl8192e/r8192E_hwimg.h
+++ b/drivers/staging/rtl8192e/rtl8192e/r8192E_hwimg.h
@@ -34,18 +34,18 @@ extern u32 Rtl8192PciEPHY_REGArray[PHY_REGArrayLengthPciE];
 #define PHY_REG_1T2RArrayLengthPciE 296
 extern u32 Rtl8192PciEPHY_REG_1T2RArray[PHY_REG_1T2RArrayLengthPciE];
 #define RadioA_ArrayLengthPciE 246
-extern u32 Rtl8192PciERadioA_Array[RadioA_ArrayLengthPciE] ;
+extern u32 Rtl8192PciERadioA_Array[RadioA_ArrayLengthPciE];
 #define RadioB_ArrayLengthPciE 78
-extern u32 Rtl8192PciERadioB_Array[RadioB_ArrayLengthPciE] ;
+extern u32 Rtl8192PciERadioB_Array[RadioB_ArrayLengthPciE];
 #define RadioC_ArrayLengthPciE 2
-extern u32 Rtl8192PciERadioC_Array[RadioC_ArrayLengthPciE] ;
+extern u32 Rtl8192PciERadioC_Array[RadioC_ArrayLengthPciE];
 #define RadioD_ArrayLengthPciE 2
-extern u32 Rtl8192PciERadioD_Array[RadioD_ArrayLengthPciE] ;
+extern u32 Rtl8192PciERadioD_Array[RadioD_ArrayLengthPciE];
 #define MACPHY_ArrayLengthPciE 18
-extern u32 Rtl8192PciEMACPHY_Array[MACPHY_ArrayLengthPciE] ;
+extern u32 Rtl8192PciEMACPHY_Array[MACPHY_ArrayLengthPciE];
 #define MACPHY_Array_PGLengthPciE 30
-extern u32 Rtl8192PciEMACPHY_Array_PG[MACPHY_Array_PGLengthPciE] ;
+extern u32 Rtl8192PciEMACPHY_Array_PG[MACPHY_Array_PGLengthPciE];
 #define AGCTAB_ArrayLengthPciE 384
-extern u32 Rtl8192PciEAGCTAB_Array[AGCTAB_ArrayLengthPciE] ;
+extern u32 Rtl8192PciEAGCTAB_Array[AGCTAB_ArrayLengthPciE];
 
 #endif
diff --git a/drivers/staging/rtl8192e/rtl8192e/r8192E_phy.c b/drivers/staging/rtl8192e/rtl8192e/r8192E_phy.c
index 8c08ef6a40a4..4664a4fd1e48 100644
--- a/drivers/staging/rtl8192e/rtl8192e/r8192E_phy.c
+++ b/drivers/staging/rtl8192e/rtl8192e/r8192E_phy.c
@@ -94,7 +94,6 @@ void rtl8192_setBBreg(struct net_device *dev, u32 dwRegAddr, u32 dwBitMask,
 		write_nic_dword(dev, dwRegAddr, NewValue);
 	} else
 		write_nic_dword(dev, dwRegAddr, dwData);
-	return;
 }
 
 u32 rtl8192_QueryBBReg(struct net_device *dev, u32 dwRegAddr, u32 dwBitMask)
@@ -136,8 +135,8 @@ static u32 rtl8192_phy_RFSerialRead(struct net_device *dev,
 		} else
 			NewOffset = Offset;
 	} else {
-		RT_TRACE((COMP_PHY|COMP_ERR), "check RF type here, need"
-			 " to be 8256\n");
+		RT_TRACE((COMP_PHY|COMP_ERR),
+			 "check RF type here, need to be 8256\n");
 		NewOffset = Offset;
 	}
 	rtl8192_setBBreg(dev, pPhyReg->rfHSSIPara2, bLSSIReadAddress,
@@ -192,8 +191,8 @@ static void rtl8192_phy_RFSerialWrite(struct net_device *dev,
 		} else
 			NewOffset = Offset;
 	} else {
-		RT_TRACE((COMP_PHY|COMP_ERR), "check RF type here, need to be"
-			 " 8256\n");
+		RT_TRACE((COMP_PHY|COMP_ERR),
+			 "check RF type here, need to be 8256\n");
 		NewOffset = Offset;
 	}
 
@@ -215,7 +214,6 @@ static void rtl8192_phy_RFSerialWrite(struct net_device *dev,
 		}
 		rtl8192_setBBreg(dev, rFPGA0_AnalogParameter4, 0x300, 0x3);
 	}
-	return;
 }
 
 void rtl8192_phy_SetRFReg(struct net_device *dev, enum rf90_radio_path eRFPath,
@@ -256,7 +254,6 @@ void rtl8192_phy_SetRFReg(struct net_device *dev, enum rf90_radio_path eRFPath,
 		} else
 			rtl8192_phy_RFSerialWrite(dev, eRFPath, RegAddr, Data);
 	}
-	return;
 }
 
 u32 rtl8192_phy_QueryRFReg(struct net_device *dev, enum rf90_radio_path eRFPath,
@@ -307,7 +304,7 @@ static u32 phy_FwRFSerialRead(struct net_device *dev,
 	}
 	return read_nic_dword(dev, RF_DATA);
 
-}	/* phy_FwRFSerialRead */
+}
 
 static void phy_FwRFSerialWrite(struct net_device *dev,
 				enum rf90_radio_path eRFPath,
@@ -328,7 +325,7 @@ static void phy_FwRFSerialWrite(struct net_device *dev,
 	}
 	write_nic_dword(dev, QPNR, Data);
 
-}	/* phy_FwRFSerialWrite */
+}
 
 
 void rtl8192_phy_configmac(struct net_device *dev)
@@ -348,8 +345,8 @@ void rtl8192_phy_configmac(struct net_device *dev)
 		pdwArray = Rtl819XMACPHY_Array;
 	}
 	for (i = 0; i < dwArrayLen; i += 3) {
-		RT_TRACE(COMP_DBG, "The Rtl8190MACPHY_Array[0] is %x Rtl8190MAC"
-			 "PHY_Array[1] is %x Rtl8190MACPHY_Array[2] is %x\n",
+		RT_TRACE(COMP_DBG,
+			 "The Rtl8190MACPHY_Array[0] is %x Rtl8190MACPHY_Array[1] is %x Rtl8190MACPHY_Array[2] is %x\n",
 			 pdwArray[i], pdwArray[i+1], pdwArray[i+2]);
 		if (pdwArray[i] == 0x318)
 			pdwArray[i+2] = 0x00000800;
@@ -383,8 +380,8 @@ void rtl8192_phyConfigBB(struct net_device *dev, u8 ConfigType)
 			rtl8192_setBBreg(dev, Rtl819XPHY_REGArray_Table[i],
 					 bMaskDWord,
 					 Rtl819XPHY_REGArray_Table[i+1]);
-			RT_TRACE(COMP_DBG, "i: %x, The Rtl819xUsbPHY_REGArray"
-				 "[0] is %x Rtl819xUsbPHY_REGArray[1] is %x\n",
+			RT_TRACE(COMP_DBG,
+				 "i: %x, The Rtl819xUsbPHY_REGArray[0] is %x Rtl819xUsbPHY_REGArray[1] is %x\n",
 				 i, Rtl819XPHY_REGArray_Table[i],
 				 Rtl819XPHY_REGArray_Table[i+1]);
 		}
@@ -393,13 +390,12 @@ void rtl8192_phyConfigBB(struct net_device *dev, u8 ConfigType)
 			rtl8192_setBBreg(dev, Rtl819XAGCTAB_Array_Table[i],
 					 bMaskDWord,
 					 Rtl819XAGCTAB_Array_Table[i+1]);
-			RT_TRACE(COMP_DBG, "i:%x, The rtl819XAGCTAB_Array[0] "
-				 "is %x rtl819XAGCTAB_Array[1] is %x\n", i,
-				 Rtl819XAGCTAB_Array_Table[i],
+			RT_TRACE(COMP_DBG,
+				 "i:%x, The rtl819XAGCTAB_Array[0] is %x rtl819XAGCTAB_Array[1] is %x\n",
+				 i, Rtl819XAGCTAB_Array_Table[i],
 				 Rtl819XAGCTAB_Array_Table[i+1]);
 		}
 	}
-	return;
 }
 
 static void rtl8192_InitBBRFRegDef(struct net_device *dev)
@@ -511,8 +507,8 @@ bool rtl8192_phy_checkBBAndRF(struct net_device *dev,
 	for (i = 0; i < CheckTimes; i++) {
 		switch (CheckBlock) {
 		case HW90_BLOCK_MAC:
-			RT_TRACE(COMP_ERR, "PHY_CheckBBRFOK(): Never Write "
-				 "0x100 here!");
+			RT_TRACE(COMP_ERR,
+				 "PHY_CheckBBRFOK(): Never Write 0x100 here!");
 			break;
 
 		case HW90_BLOCK_PHY0:
@@ -541,8 +537,9 @@ bool rtl8192_phy_checkBBAndRF(struct net_device *dev,
 
 
 		if (dwRegRead != WriteData[i]) {
-			RT_TRACE(COMP_ERR, "====>error=====dwRegRead: %x, "
-				 "WriteData: %x\n", dwRegRead, WriteData[i]);
+			RT_TRACE(COMP_ERR,
+				 "====>error=====dwRegRead: %x, WriteData: %x\n",
+				 dwRegRead, WriteData[i]);
 			ret = false;
 			break;
 		}
@@ -570,8 +567,9 @@ static bool rtl8192_BB_Config_ParaFile(struct net_device *dev)
 					 (enum hw90_block)eCheckItem,
 					 (enum rf90_radio_path)0);
 		if (!rtStatus) {
-			RT_TRACE((COMP_ERR | COMP_PHY), "PHY_RF8256_Config():"
-				 "Check PHY%d Fail!!\n", eCheckItem-1);
+			RT_TRACE((COMP_ERR | COMP_PHY),
+				 "PHY_RF8256_Config():Check PHY%d Fail!!\n",
+				 eCheckItem-1);
 			return rtStatus;
 		}
 	}
@@ -628,8 +626,8 @@ void rtl8192_phy_getTxPower(struct net_device *dev)
 	priv->DefaultInitialGain[1] = read_nic_byte(dev, rOFDM0_XBAGCCore1);
 	priv->DefaultInitialGain[2] = read_nic_byte(dev, rOFDM0_XCAGCCore1);
 	priv->DefaultInitialGain[3] = read_nic_byte(dev, rOFDM0_XDAGCCore1);
-	RT_TRACE(COMP_INIT, "Default initial gain (c50=0x%x, c58=0x%x, "
-		"c60=0x%x, c68=0x%x)\n",
+	RT_TRACE(COMP_INIT,
+		 "Default initial gain (c50=0x%x, c58=0x%x, c60=0x%x, c68=0x%x)\n",
 		priv->DefaultInitialGain[0], priv->DefaultInitialGain[1],
 		priv->DefaultInitialGain[2], priv->DefaultInitialGain[3]);
 
@@ -638,7 +636,6 @@ void rtl8192_phy_getTxPower(struct net_device *dev)
 	RT_TRACE(COMP_INIT, "Default framesync (0x%x) = 0x%x\n",
 		rOFDM0_RxDetector3, priv->framesync);
 	priv->SifsTime = read_nic_word(dev, SIFS);
-	return;
 }
 
 void rtl8192_phy_setTxPower(struct net_device *dev, u8 channel)
@@ -692,7 +689,6 @@ void rtl8192_phy_setTxPower(struct net_device *dev, u8 channel)
 			 __func__);
 		break;
 	}
-	return;
 }
 
 bool rtl8192_phy_RFConfig(struct net_device *dev)
@@ -721,7 +717,6 @@ bool rtl8192_phy_RFConfig(struct net_device *dev)
 
 void rtl8192_phy_updateInitGain(struct net_device *dev)
 {
-	return;
 }
 
 u8 rtl8192_phy_ConfigRFWithHeaderFile(struct net_device *dev,
@@ -807,11 +802,10 @@ static void rtl8192_SetTxPowerLevel(struct net_device *dev, u8 channel)
 	case RF_8258:
 		break;
 	default:
-		RT_TRACE(COMP_ERR, "unknown rf chip ID in rtl8192_SetTxPower"
-			 "Level()\n");
+		RT_TRACE(COMP_ERR,
+			 "unknown rf chip ID in rtl8192_SetTxPowerLevel()\n");
 		break;
 	}
-	return;
 }
 
 static u8 rtl8192_phy_SetSwChnlCmdArray(struct sw_chnl_cmd *CmdTable,
@@ -822,15 +816,14 @@ static u8 rtl8192_phy_SetSwChnlCmdArray(struct sw_chnl_cmd *CmdTable,
 	struct sw_chnl_cmd *pCmd;
 
 	if (CmdTable == NULL) {
-		RT_TRACE(COMP_ERR, "phy_SetSwChnlCmdArray(): CmdTable cannot "
-			 "be NULL.\n");
+		RT_TRACE(COMP_ERR,
+			 "phy_SetSwChnlCmdArray(): CmdTable cannot be NULL.\n");
 		return false;
 	}
 	if (CmdTableIdx >= CmdTableSz) {
-		RT_TRACE(COMP_ERR, "phy_SetSwChnlCmdArray(): Access invalid"
-			 " index, please check size of the table, CmdTableIdx:"
-			 "%d, CmdTableSz:%d\n",
-				CmdTableIdx, CmdTableSz);
+		RT_TRACE(COMP_ERR,
+			 "phy_SetSwChnlCmdArray(): Access invalid index, please check size of the table, CmdTableIdx:%d, CmdTableSz:%d\n",
+			 CmdTableIdx, CmdTableSz);
 		return false;
 	}
 
@@ -883,8 +876,9 @@ static u8 rtl8192_phy_SwChnlStepByStep(struct net_device *dev, u8 channel,
 		switch (priv->rf_chip) {
 		case RF_8225:
 			if (!(channel >= 1 && channel <= 14)) {
-				RT_TRACE(COMP_ERR, "illegal channel for Zebra "
-					 "8225: %d\n", channel);
+				RT_TRACE(COMP_ERR,
+					 "illegal channel for Zebra 8225: %d\n",
+					 channel);
 				return false;
 			}
 			rtl8192_phy_SetSwChnlCmdArray(ieee->RfDependCmd,
@@ -898,8 +892,9 @@ static u8 rtl8192_phy_SwChnlStepByStep(struct net_device *dev, u8 channel,
 
 		case RF_8256:
 			if (!(channel >= 1 && channel <= 14)) {
-				RT_TRACE(COMP_ERR, "illegal channel for Zebra"
-					 " 8256: %d\n", channel);
+				RT_TRACE(COMP_ERR,
+					 "illegal channel for Zebra 8256: %d\n",
+					 channel);
 				return false;
 			}
 			rtl8192_phy_SetSwChnlCmdArray(ieee->RfDependCmd,
@@ -920,7 +915,6 @@ static u8 rtl8192_phy_SwChnlStepByStep(struct net_device *dev, u8 channel,
 			RT_TRACE(COMP_ERR, "Unknown RFChipID: %d\n",
 				 priv->rf_chip);
 			return false;
-			break;
 		}
 
 
@@ -938,13 +932,11 @@ static u8 rtl8192_phy_SwChnlStepByStep(struct net_device *dev, u8 channel,
 			}
 
 			if (CurrentCmd && CurrentCmd->CmdID == CmdID_End) {
-				if ((*stage) == 2) {
+				if ((*stage) == 2)
 					return true;
-				} else {
-					(*stage)++;
-					(*step) = 0;
-					continue;
-				}
+				(*stage)++;
+				(*step) = 0;
+				continue;
 			}
 
 			if (!CurrentCmd)
@@ -996,7 +988,7 @@ static void rtl8192_phy_FinishSwChnlNow(struct net_device *dev, u8 channel)
 	      &priv->SwChnlStep, &delay)) {
 		if (delay > 0)
 			msleep(delay);
-		if (IS_NIC_DOWN(priv))
+		if (!priv->up)
 			break;
 	}
 }
@@ -1020,7 +1012,7 @@ u8 rtl8192_phy_SwChnl(struct net_device *dev, u8 channel)
 	struct r8192_priv *priv = rtllib_priv(dev);
 
 	RT_TRACE(COMP_PHY, "=====>%s()\n", __func__);
-	if (IS_NIC_DOWN(priv)) {
+	if (!priv->up) {
 		RT_TRACE(COMP_ERR, "%s(): ERR !! driver is not up\n", __func__);
 		return false;
 	}
@@ -1060,7 +1052,7 @@ u8 rtl8192_phy_SwChnl(struct net_device *dev, u8 channel)
 	priv->SwChnlStage = 0;
 	priv->SwChnlStep = 0;
 
-	if (!IS_NIC_DOWN(priv))
+	if (priv->up)
 		rtl8192_SwChnl_WorkItem(dev);
 	priv->SwChnlInProgress = false;
 	return true;
@@ -1083,8 +1075,8 @@ static void CCK_Tx_Power_Track_BW_Switch_TSSI(struct net_device *dev)
 		if (priv->CCKPresentAttentuation < 0)
 			priv->CCKPresentAttentuation = 0;
 
-		RT_TRACE(COMP_POWER_TRACKING, "20M, priv->CCKPresent"
-			 "Attentuation = %d\n",
+		RT_TRACE(COMP_POWER_TRACKING,
+			 "20M, priv->CCKPresentAttentuation = %d\n",
 			 priv->CCKPresentAttentuation);
 
 		if (priv->rtllib->current_network.channel == 14 &&
@@ -1105,8 +1097,8 @@ static void CCK_Tx_Power_Track_BW_Switch_TSSI(struct net_device *dev)
 			priv->CCKPresentAttentuation_40Mdefault +
 			priv->CCKPresentAttentuation_difference;
 
-		RT_TRACE(COMP_POWER_TRACKING, "40M, priv->CCKPresent"
-			 "Attentuation = %d\n",
+		RT_TRACE(COMP_POWER_TRACKING,
+			 "40M, priv->CCKPresentAttentuation = %d\n",
 			 priv->CCKPresentAttentuation);
 		if (priv->CCKPresentAttentuation >
 		    (CCKTxBBGainTableLength - 1))
@@ -1146,15 +1138,15 @@ static void CCK_Tx_Power_Track_BW_Switch_ThermalMeter(struct net_device *dev)
 		if (priv->Record_CCK_20Mindex == 0)
 			priv->Record_CCK_20Mindex = 6;
 		priv->CCK_index = priv->Record_CCK_20Mindex;
-		RT_TRACE(COMP_POWER_TRACKING, "20MHz, CCK_Tx_Power_Track_BW_"
-			 "Switch_ThermalMeter(),CCK_index = %d\n",
+		RT_TRACE(COMP_POWER_TRACKING,
+			 "20MHz, CCK_Tx_Power_Track_BW_Switch_ThermalMeter(),CCK_index = %d\n",
 			 priv->CCK_index);
 	break;
 
 	case HT_CHANNEL_WIDTH_20_40:
 		priv->CCK_index = priv->Record_CCK_40Mindex;
-		RT_TRACE(COMP_POWER_TRACKING, "40MHz, CCK_Tx_Power_Track_BW_"
-			 "Switch_ThermalMeter(), CCK_index = %d\n",
+		RT_TRACE(COMP_POWER_TRACKING,
+			 "40MHz, CCK_Tx_Power_Track_BW_Switch_ThermalMeter(), CCK_index = %d\n",
 			 priv->CCK_index);
 	break;
 	}
@@ -1177,16 +1169,17 @@ void rtl8192_SetBWModeWorkItem(struct net_device *dev)
 	struct r8192_priv *priv = rtllib_priv(dev);
 	u8 regBwOpMode;
 
-	RT_TRACE(COMP_SWBW, "==>rtl8192_SetBWModeWorkItem()  Switch to %s "
-		 "bandwidth\n", priv->CurrentChannelBW == HT_CHANNEL_WIDTH_20 ?
-		 "20MHz" : "40MHz");
+	RT_TRACE(COMP_SWBW,
+		 "==>rtl8192_SetBWModeWorkItem()  Switch to %s bandwidth\n",
+		 priv->CurrentChannelBW == HT_CHANNEL_WIDTH_20 ?
+			 "20MHz" : "40MHz");
 
 
 	if (priv->rf_chip == RF_PSEUDO_11N) {
 		priv->SetBWModeInProgress = false;
 		return;
 	}
-	if (IS_NIC_DOWN(priv)) {
+	if (!priv->up) {
 		RT_TRACE(COMP_ERR, "%s(): ERR!! driver is not up\n", __func__);
 		return;
 	}
@@ -1204,8 +1197,9 @@ void rtl8192_SetBWModeWorkItem(struct net_device *dev)
 		break;
 
 	default:
-		RT_TRACE(COMP_ERR, "SetChannelBandwidth819xUsb(): unknown "
-			 "Bandwidth: %#X\n", priv->CurrentChannelBW);
+		RT_TRACE(COMP_ERR,
+			 "SetChannelBandwidth819xUsb(): unknown Bandwidth: %#X\n",
+			 priv->CurrentChannelBW);
 		break;
 	}
 
@@ -1245,8 +1239,9 @@ void rtl8192_SetBWModeWorkItem(struct net_device *dev)
 		rtl8192_setBBreg(dev, rFPGA0_AnalogParameter1, 0x00100000, 0);
 		break;
 	default:
-		RT_TRACE(COMP_ERR, "SetChannelBandwidth819xUsb(): unknown "
-			 "Bandwidth: %#X\n", priv->CurrentChannelBW);
+		RT_TRACE(COMP_ERR,
+			 "SetChannelBandwidth819xUsb(): unknown Bandwidth: %#X\n",
+			 priv->CurrentChannelBW);
 		break;
 
 	}
@@ -1309,11 +1304,11 @@ void InitialGain819xPci(struct net_device *dev, u8 Operation)
 	u32 BitMask;
 	u8 initial_gain;
 
-	if (!IS_NIC_DOWN(priv)) {
+	if (priv->up) {
 		switch (Operation) {
 		case IG_Backup:
-			RT_TRACE(COMP_SCAN, "IG_Backup, backup the initial"
-				 " gain.\n");
+			RT_TRACE(COMP_SCAN,
+				 "IG_Backup, backup the initial gain.\n");
 			initial_gain = SCAN_RX_INITIAL_GAIN;
 			BitMask = bMaskByte0;
 			if (dm_digtable.dig_algorithm ==
@@ -1335,16 +1330,21 @@ void InitialGain819xPci(struct net_device *dev, u8 Operation)
 			priv->initgain_backup.cca = (u8)rtl8192_QueryBBReg(dev,
 						    rCCK0_CCA, BitMask);
 
-			RT_TRACE(COMP_SCAN, "Scan InitialGainBackup 0xc50 is"
-				 " %x\n", priv->initgain_backup.xaagccore1);
-			RT_TRACE(COMP_SCAN, "Scan InitialGainBackup 0xc58 is"
-				 " %x\n", priv->initgain_backup.xbagccore1);
-			RT_TRACE(COMP_SCAN, "Scan InitialGainBackup 0xc60 is"
-				 " %x\n", priv->initgain_backup.xcagccore1);
-			RT_TRACE(COMP_SCAN, "Scan InitialGainBackup 0xc68 is"
-				 " %x\n", priv->initgain_backup.xdagccore1);
-			RT_TRACE(COMP_SCAN, "Scan InitialGainBackup 0xa0a is"
-				 " %x\n", priv->initgain_backup.cca);
+			RT_TRACE(COMP_SCAN,
+				 "Scan InitialGainBackup 0xc50 is %x\n",
+				 priv->initgain_backup.xaagccore1);
+			RT_TRACE(COMP_SCAN,
+				 "Scan InitialGainBackup 0xc58 is %x\n",
+				 priv->initgain_backup.xbagccore1);
+			RT_TRACE(COMP_SCAN,
+				 "Scan InitialGainBackup 0xc60 is %x\n",
+				 priv->initgain_backup.xcagccore1);
+			RT_TRACE(COMP_SCAN,
+				 "Scan InitialGainBackup 0xc68 is %x\n",
+				 priv->initgain_backup.xdagccore1);
+			RT_TRACE(COMP_SCAN,
+				 "Scan InitialGainBackup 0xa0a is %x\n",
+				 priv->initgain_backup.cca);
 
 			RT_TRACE(COMP_SCAN, "Write scan initial gain = 0x%x\n",
 				 initial_gain);
@@ -1357,8 +1357,8 @@ void InitialGain819xPci(struct net_device *dev, u8 Operation)
 			write_nic_byte(dev, 0xa0a, POWER_DETECTION_TH);
 			break;
 		case IG_Restore:
-			RT_TRACE(COMP_SCAN, "IG_Restore, restore the initial "
-				 "gain.\n");
+			RT_TRACE(COMP_SCAN,
+				 "IG_Restore, restore the initial gain.\n");
 			BitMask = 0x7f;
 			if (dm_digtable.dig_algorithm ==
 			    DIG_ALGO_BY_FALSE_ALARM)
@@ -1376,16 +1376,21 @@ void InitialGain819xPci(struct net_device *dev, u8 Operation)
 			rtl8192_setBBreg(dev, rCCK0_CCA, BitMask,
 					 (u32)priv->initgain_backup.cca);
 
-			RT_TRACE(COMP_SCAN, "Scan BBInitialGainRestore 0xc50"
-				 " is %x\n", priv->initgain_backup.xaagccore1);
-			RT_TRACE(COMP_SCAN, "Scan BBInitialGainRestore 0xc58"
-				 " is %x\n", priv->initgain_backup.xbagccore1);
-			RT_TRACE(COMP_SCAN, "Scan BBInitialGainRestore 0xc60"
-				 " is %x\n", priv->initgain_backup.xcagccore1);
-			RT_TRACE(COMP_SCAN, "Scan BBInitialGainRestore 0xc68"
-				 " is %x\n", priv->initgain_backup.xdagccore1);
-			RT_TRACE(COMP_SCAN, "Scan BBInitialGainRestore 0xa0a"
-				 " is %x\n", priv->initgain_backup.cca);
+			RT_TRACE(COMP_SCAN,
+				 "Scan BBInitialGainRestore 0xc50 is %x\n",
+				 priv->initgain_backup.xaagccore1);
+			RT_TRACE(COMP_SCAN,
+				 "Scan BBInitialGainRestore 0xc58 is %x\n",
+				 priv->initgain_backup.xbagccore1);
+			RT_TRACE(COMP_SCAN,
+				 "Scan BBInitialGainRestore 0xc60 is %x\n",
+				 priv->initgain_backup.xcagccore1);
+			RT_TRACE(COMP_SCAN,
+				 "Scan BBInitialGainRestore 0xc68 is %x\n",
+				 priv->initgain_backup.xdagccore1);
+			RT_TRACE(COMP_SCAN,
+				 "Scan BBInitialGainRestore 0xa0a is %x\n",
+				 priv->initgain_backup.cca);
 
 			rtl8192_phy_setTxPower(dev,
 					 priv->rtllib->current_network.channel);
@@ -1447,8 +1452,8 @@ static bool SetRFPowerState8190(struct net_device *dev,
 				} while (!rtstatus && (InitilizeCount > 0));
 
 				if (!rtstatus) {
-					RT_TRACE(COMP_ERR, "%s():Initialize Ada"
-						 "pter fail,return\n",
+					RT_TRACE(COMP_ERR,
+						 "%s():Initialize Adapter fail,return\n",
 						 __func__);
 					priv->SetRFPowerStateInProgress = false;
 					return false;
@@ -1492,19 +1497,16 @@ static bool SetRFPowerState8190(struct net_device *dev,
 					QueueID++;
 					continue;
 				} else {
-					RT_TRACE((COMP_POWER|COMP_RF), "eRf Off"
-						 "/Sleep: %d times TcbBusyQueue"
-						 "[%d] !=0 before doze!\n",
+					RT_TRACE((COMP_POWER|COMP_RF),
+						 "eRf Off/Sleep: %d times TcbBusyQueue[%d] !=0 before doze!\n",
 						 (i+1), QueueID);
 					udelay(10);
 					i++;
 				}
 
 				if (i >= MAX_DOZE_WAITING_TIMES_9x) {
-					RT_TRACE(COMP_POWER, "\n\n\n TimeOut!! "
-						 "SetRFPowerState8190(): eRfOff"
-						 ": %d times TcbBusyQueue[%d] "
-						 "!= 0 !!!\n",
+					RT_TRACE(COMP_POWER,
+						 "\n\n\n TimeOut!! SetRFPowerState8190(): eRfOff: %d times TcbBusyQueue[%d] != 0 !!!\n",
 						 MAX_DOZE_WAITING_TIMES_9x,
 						 QueueID);
 					break;
@@ -1514,8 +1516,8 @@ static bool SetRFPowerState8190(struct net_device *dev,
 			break;
 
 		case eRfOff:
-			RT_TRACE(COMP_PS, "SetRFPowerState8190() eRfOff/"
-				 "Sleep !\n");
+			RT_TRACE(COMP_PS,
+				 "SetRFPowerState8190() eRfOff/Sleep !\n");
 
 			for (QueueID = 0, i = 0; QueueID < MAX_TX_QUEUE; ) {
 				ring = &priv->tx_ring[QueueID];
@@ -1524,19 +1526,16 @@ static bool SetRFPowerState8190(struct net_device *dev,
 					QueueID++;
 					continue;
 				} else {
-					RT_TRACE(COMP_POWER, "eRf Off/Sleep: %d"
-						 " times TcbBusyQueue[%d] !=0 b"
-						 "efore doze!\n", (i+1),
-						 QueueID);
+					RT_TRACE(COMP_POWER,
+						 "eRf Off/Sleep: %d times TcbBusyQueue[%d] !=0 before doze!\n",
+						 (i+1), QueueID);
 					udelay(10);
 					i++;
 				}
 
 				if (i >= MAX_DOZE_WAITING_TIMES_9x) {
-					RT_TRACE(COMP_POWER, "\n\n\n SetZebra: "
-						 "RFPowerState8185B(): eRfOff:"
-						 " %d times TcbBusyQueue[%d] "
-						 "!= 0 !!!\n",
+					RT_TRACE(COMP_POWER,
+						 "\n\n\n SetZebra: RFPowerState8185B(): eRfOff: %d times TcbBusyQueue[%d] != 0 !!!\n",
 						 MAX_DOZE_WAITING_TIMES_9x,
 						 QueueID);
 					break;
@@ -1556,8 +1555,9 @@ static bool SetRFPowerState8190(struct net_device *dev,
 
 		default:
 			bResult = false;
-			RT_TRACE(COMP_ERR, "SetRFPowerState8190(): unknown state"
-				 " to set: 0x%X!!!\n", eRFPowerState);
+			RT_TRACE(COMP_ERR,
+				 "SetRFPowerState8190(): unknown state to set: 0x%X!!!\n",
+				 eRFPowerState);
 			break;
 		}
 
@@ -1576,8 +1576,8 @@ static bool SetRFPowerState8190(struct net_device *dev,
 			break;
 
 		default:
-			RT_TRACE(COMP_ERR, "SetRFPowerState8190(): Unknown "
-				 "RF type\n");
+			RT_TRACE(COMP_ERR,
+				 "SetRFPowerState8190(): Unknown RF type\n");
 			break;
 		}
 	}
@@ -1599,8 +1599,8 @@ bool SetRFPowerState(struct net_device *dev,
 		 eRFPowerState);
 	if (eRFPowerState == priv->rtllib->eRFPowerState &&
 	    priv->bHwRfOffAction == 0) {
-		RT_TRACE(COMP_PS, "<--------- SetRFPowerState(): discard the "
-			 "request for eRFPowerState(%d) is the same.\n",
+		RT_TRACE(COMP_PS,
+			 "<--------- SetRFPowerState(): discard the request for eRFPowerState(%d) is the same.\n",
 			 eRFPowerState);
 		return bResult;
 	}
diff --git a/drivers/staging/rtl8192e/rtl8192e/r819xE_phyreg.h b/drivers/staging/rtl8192e/rtl8192e/r819xE_phyreg.h
index 970298b07af7..03eee3d059c6 100644
--- a/drivers/staging/rtl8192e/rtl8192e/r819xE_phyreg.h
+++ b/drivers/staging/rtl8192e/rtl8192e/r819xE_phyreg.h
@@ -888,13 +888,6 @@
 #define bOFDMPHY0_End             0xcff
 #define bOFDMPHY1_End             0xdff
 
-/*#define max debug item in each debug page
-#define bMaxItem_FPGA_PHY0        0x9
-#define bMaxItem_FPGA_PHY1        0x3
-#define bMaxItem_PHY_11B          0x16
-#define bMaxItem_OFDM_PHY0        0x29
-#define bMaxItem_OFDM_PHY1        0x0 */
-
 #define bPMACControl              0x0
 #define bWMACControl              0x1
 #define bWNICControl              0x2
diff --git a/drivers/staging/rtl8192e/rtl8192e/rtl_cam.c b/drivers/staging/rtl8192e/rtl8192e/rtl_cam.c
index 89ea70b0d3aa..41b025e250fe 100644
--- a/drivers/staging/rtl8192e/rtl8192e/rtl_cam.c
+++ b/drivers/staging/rtl8192e/rtl8192e/rtl_cam.c
@@ -86,9 +86,9 @@ void set_swcam(struct net_device *dev, u8 EntryNo, u8 KeyIndex, u16 KeyType,
 	struct r8192_priv *priv = rtllib_priv(dev);
 	struct rtllib_device *ieee = priv->rtllib;
 
-	RT_TRACE(COMP_DBG, "===========>%s():EntryNo is %d,KeyIndex is "
-		 "%d,KeyType is %d,is_mesh is %d\n", __func__, EntryNo,
-		 KeyIndex, KeyType, is_mesh);
+	RT_TRACE(COMP_DBG,
+		 "===========>%s():EntryNo is %d,KeyIndex is %d,KeyType is %d,is_mesh is %d\n",
+		 __func__, EntryNo, KeyIndex, KeyType, is_mesh);
 	if (!is_mesh) {
 		ieee->swcamtable[EntryNo].bused = true;
 		ieee->swcamtable[EntryNo].key_index = KeyIndex;
@@ -115,21 +115,20 @@ void setKey(struct net_device *dev, u8 EntryNo, u8 KeyIndex, u16 KeyType,
 			if (priv->rtllib->RfOffReason > RF_CHANGE_BY_IPS) {
 				RT_TRACE(COMP_ERR, "%s(): RF is OFF.\n",
 					__func__);
-				return ;
-			} else {
-				down(&priv->rtllib->ips_sem);
-				IPSLeave(dev);
-				up(&priv->rtllib->ips_sem);
+				return;
 			}
+			down(&priv->rtllib->ips_sem);
+			IPSLeave(dev);
+			up(&priv->rtllib->ips_sem);
 		}
 	}
 	priv->rtllib->is_set_key = true;
 	if (EntryNo >= TOTAL_CAM_ENTRY)
 		RT_TRACE(COMP_ERR, "cam entry exceeds in setKey()\n");
 
-	RT_TRACE(COMP_SEC, "====>to setKey(), dev:%p, EntryNo:%d, KeyIndex:%d,"
-		 "KeyType:%d, MacAddr %pM\n", dev, EntryNo, KeyIndex,
-		 KeyType, MacAddr);
+	RT_TRACE(COMP_SEC,
+		 "====>to setKey(), dev:%p, EntryNo:%d, KeyIndex:%d,KeyType:%d, MacAddr %pM\n",
+		 dev, EntryNo, KeyIndex, KeyType, MacAddr);
 
 	if (DefaultKey)
 		usConfig |= BIT15 | (KeyType<<2);
@@ -192,7 +191,7 @@ void CamRestoreAllEntry(struct net_device *dev)
 		for (EntryId = 0; EntryId < 4; EntryId++) {
 			MacAddr = CAM_CONST_ADDR[EntryId];
 			if (priv->rtllib->swcamtable[EntryId].bused) {
-				setKey(dev, EntryId , EntryId,
+				setKey(dev, EntryId, EntryId,
 				       priv->rtllib->pairwise_key_type, MacAddr,
 				       0, (u32 *)(&priv->rtllib->swcamtable
 				      [EntryId].key_buf[0]));
@@ -244,8 +243,8 @@ void CamRestoreAllEntry(struct net_device *dev)
 				       (u32 *)(&priv->rtllib->swcamtable[0].key_buf[0])
 				     );
 			} else {
-				RT_TRACE(COMP_ERR, "===>%s():ERR!! ADHOC TKIP "
-					 ",but 0 entry is have no data\n",
+				RT_TRACE(COMP_ERR,
+					 "===>%s():ERR!! ADHOC TKIP ,but 0 entry is have no data\n",
 					 __func__);
 				return;
 			}
@@ -254,7 +253,7 @@ void CamRestoreAllEntry(struct net_device *dev)
 		MacAddr = CAM_CONST_BROAD;
 		for (EntryId = 1; EntryId < 4; EntryId++) {
 			if (priv->rtllib->swcamtable[EntryId].bused) {
-				setKey(dev, EntryId , EntryId,
+				setKey(dev, EntryId, EntryId,
 				       priv->rtllib->group_key_type,
 				       MacAddr, 0,
 				       (u32 *)(&priv->rtllib->swcamtable[EntryId].key_buf[0]));
@@ -263,13 +262,13 @@ void CamRestoreAllEntry(struct net_device *dev)
 
 		if (priv->rtllib->iw_mode == IW_MODE_ADHOC) {
 			if (priv->rtllib->swcamtable[0].bused) {
-				setKey(dev, 0 , 0,
+				setKey(dev, 0, 0,
 					priv->rtllib->group_key_type,
 					CAM_CONST_ADDR[0], 0,
 					(u32 *)(&priv->rtllib->swcamtable[0].key_buf[0]));
 			} else {
-				RT_TRACE(COMP_ERR, "===>%s():ERR!! ADHOC CCMP ,"
-					 "but 0 entry is have no data\n",
+				RT_TRACE(COMP_ERR,
+					 "===>%s():ERR!! ADHOC CCMP ,but 0 entry is have no data\n",
 					 __func__);
 				return;
 			}
diff --git a/drivers/staging/rtl8192e/rtl8192e/rtl_core.c b/drivers/staging/rtl8192e/rtl8192e/rtl_core.c
index 5615c80f614c..352d381b7c4a 100644
--- a/drivers/staging/rtl8192e/rtl8192e/rtl_core.c
+++ b/drivers/staging/rtl8192e/rtl8192e/rtl_core.c
@@ -131,7 +131,7 @@ static bool PlatformIOCheckPageLegalAndGetRegMask(u32 u4bPage, u8 *pu1bPageMask)
 
 void write_nic_io_byte(struct net_device *dev, int x, u8 y)
 {
-	u32 u4bPage = (x >> 8);
+	u32 u4bPage = x >> 8;
 	u8 u1PageMask = 0;
 	bool	bIsLegalPage = false;
 
@@ -154,7 +154,7 @@ void write_nic_io_byte(struct net_device *dev, int x, u8 y)
 
 void write_nic_io_word(struct net_device *dev, int x, u16 y)
 {
-	u32 u4bPage = (x >> 8);
+	u32 u4bPage = x >> 8;
 	u8 u1PageMask = 0;
 	bool	bIsLegalPage = false;
 
@@ -177,7 +177,7 @@ void write_nic_io_word(struct net_device *dev, int x, u16 y)
 
 void write_nic_io_dword(struct net_device *dev, int x, u32 y)
 {
-	u32 u4bPage = (x >> 8);
+	u32 u4bPage = x >> 8;
 	u8 u1PageMask = 0;
 	bool	bIsLegalPage = false;
 
@@ -199,24 +199,23 @@ void write_nic_io_dword(struct net_device *dev, int x, u32 y)
 
 u8 read_nic_io_byte(struct net_device *dev, int x)
 {
-	u32 u4bPage = (x >> 8);
+	u32 u4bPage = x >> 8;
 	u8 u1PageMask = 0;
 	bool	bIsLegalPage = false;
 	u8	Data = 0;
 
-	if (u4bPage == 0) {
+	if (u4bPage == 0)
 		return 0xff&inb(dev->base_addr + x);
-	} else {
-		bIsLegalPage = PlatformIOCheckPageLegalAndGetRegMask(u4bPage,
-							&u1PageMask);
-		if (bIsLegalPage) {
-			u8 u1bPsr = read_nic_io_byte(dev, PSR);
 
-			write_nic_io_byte(dev, PSR, ((u1bPsr & u1PageMask) |
-					  (u8)u4bPage));
-			Data = read_nic_io_byte(dev, (x & 0xff));
-			write_nic_io_byte(dev, PSR, (u1bPsr & u1PageMask));
-		}
+	bIsLegalPage = PlatformIOCheckPageLegalAndGetRegMask(u4bPage,
+							     &u1PageMask);
+	if (bIsLegalPage) {
+		u8 u1bPsr = read_nic_io_byte(dev, PSR);
+
+		write_nic_io_byte(dev, PSR, ((u1bPsr & u1PageMask) |
+				  (u8)u4bPage));
+		Data = read_nic_io_byte(dev, (x & 0xff));
+		write_nic_io_byte(dev, PSR, (u1bPsr & u1PageMask));
 	}
 
 	return Data;
@@ -224,25 +223,22 @@ u8 read_nic_io_byte(struct net_device *dev, int x)
 
 u16 read_nic_io_word(struct net_device *dev, int x)
 {
-	u32 u4bPage = (x >> 8);
+	u32 u4bPage = x >> 8;
 	u8 u1PageMask = 0;
 	bool	bIsLegalPage = false;
 	u16	Data = 0;
 
-	if (u4bPage == 0) {
+	if (u4bPage == 0)
 		return inw(dev->base_addr + x);
-	} else {
-		bIsLegalPage = PlatformIOCheckPageLegalAndGetRegMask(u4bPage,
-			       &u1PageMask);
-		if (bIsLegalPage) {
-			u8 u1bPsr = read_nic_io_byte(dev, PSR);
+	bIsLegalPage = PlatformIOCheckPageLegalAndGetRegMask(u4bPage,
+							     &u1PageMask);
+	if (bIsLegalPage) {
+		u8 u1bPsr = read_nic_io_byte(dev, PSR);
 
-			write_nic_io_byte(dev, PSR, ((u1bPsr & u1PageMask) |
-					  (u8)u4bPage));
-			Data = read_nic_io_word(dev, (x & 0xff));
-			write_nic_io_byte(dev, PSR, (u1bPsr & u1PageMask));
-
-		}
+		write_nic_io_byte(dev, PSR, ((u1bPsr & u1PageMask) |
+				  (u8)u4bPage));
+		Data = read_nic_io_word(dev, (x & 0xff));
+		write_nic_io_byte(dev, PSR, (u1bPsr & u1PageMask));
 	}
 
 	return Data;
@@ -250,25 +246,22 @@ u16 read_nic_io_word(struct net_device *dev, int x)
 
 u32 read_nic_io_dword(struct net_device *dev, int x)
 {
-	u32 u4bPage = (x >> 8);
+	u32 u4bPage = x >> 8;
 	u8 u1PageMask = 0;
 	bool	bIsLegalPage = false;
 	u32	Data = 0;
 
-	if (u4bPage == 0) {
+	if (u4bPage == 0)
 		return inl(dev->base_addr + x);
-	} else {
-		bIsLegalPage = PlatformIOCheckPageLegalAndGetRegMask(u4bPage,
-			       &u1PageMask);
-		if (bIsLegalPage) {
-			u8 u1bPsr = read_nic_io_byte(dev, PSR);
-
-			write_nic_io_byte(dev, PSR, ((u1bPsr & u1PageMask) |
-					  (u8)u4bPage));
-			Data = read_nic_io_dword(dev, (x & 0xff));
-			write_nic_io_byte(dev, PSR, (u1bPsr & u1PageMask));
+	bIsLegalPage = PlatformIOCheckPageLegalAndGetRegMask(u4bPage,
+		       &u1PageMask);
+	if (bIsLegalPage) {
+		u8 u1bPsr = read_nic_io_byte(dev, PSR);
 
-		}
+		write_nic_io_byte(dev, PSR, ((u1bPsr & u1PageMask) |
+				  (u8)u4bPage));
+		Data = read_nic_io_dword(dev, (x & 0xff));
+		write_nic_io_byte(dev, PSR, (u1bPsr & u1PageMask));
 	}
 
 	return Data;
@@ -326,8 +319,8 @@ bool MgntActSet_RF_State(struct net_device *dev,
 	u16			RFWaitCounter = 0;
 	unsigned long flag;
 
-	RT_TRACE((COMP_PS | COMP_RF), "===>MgntActSet_RF_State(): "
-		 "StateToSet(%d)\n", StateToSet);
+	RT_TRACE((COMP_PS | COMP_RF),
+		 "===>MgntActSet_RF_State(): StateToSet(%d)\n", StateToSet);
 
 	ProtectOrNot = false;
 
@@ -338,22 +331,19 @@ bool MgntActSet_RF_State(struct net_device *dev,
 			if (priv->RFChangeInProgress) {
 				spin_unlock_irqrestore(&priv->rf_ps_lock, flag);
 				RT_TRACE((COMP_PS | COMP_RF),
-					 "MgntActSet_RF_State(): RF Change in "
-					 "progress! Wait to set..StateToSet"
-					 "(%d).\n", StateToSet);
+					 "MgntActSet_RF_State(): RF Change in progress! Wait to set..StateToSet(%d).\n",
+					 StateToSet);
 
 				while (priv->RFChangeInProgress) {
 					RFWaitCounter++;
 					RT_TRACE((COMP_PS | COMP_RF),
-						 "MgntActSet_RF_State(): Wait 1"
-						 " ms (%d times)...\n",
+						 "MgntActSet_RF_State(): Wait 1 ms (%d times)...\n",
 						 RFWaitCounter);
 					mdelay(1);
 
 					if (RFWaitCounter > 100) {
-						RT_TRACE(COMP_ERR, "MgntActSet_"
-							 "RF_State(): Wait too "
-							 "logn to set RF\n");
+						RT_TRACE(COMP_ERR,
+							 "MgntActSet_RF_State(): Wait too logn to set RF\n");
 						return false;
 					}
 				}
@@ -383,9 +373,8 @@ bool MgntActSet_RF_State(struct net_device *dev,
 			    ChangeSource >= RF_CHANGE_BY_HW)
 				bConnectBySSID = true;
 		} else {
-			RT_TRACE((COMP_PS | COMP_RF), "MgntActSet_RF_State - "
-				 "eRfon reject pMgntInfo->RfOffReason= 0x%x,"
-				 " ChangeSource=0x%X\n",
+			RT_TRACE((COMP_PS | COMP_RF),
+				 "MgntActSet_RF_State - eRfon reject pMgntInfo->RfOffReason= 0x%x, ChangeSource=0x%X\n",
 				  priv->rtllib->RfOffReason, ChangeSource);
 	}
 
@@ -421,8 +410,8 @@ bool MgntActSet_RF_State(struct net_device *dev,
 	}
 
 	if (bActionAllowed) {
-		RT_TRACE((COMP_PS | COMP_RF), "MgntActSet_RF_State(): Action is"
-			 " allowed.... StateToSet(%d), RfOffReason(%#X)\n",
+		RT_TRACE((COMP_PS | COMP_RF),
+			 "MgntActSet_RF_State(): Action is allowed.... StateToSet(%d), RfOffReason(%#X)\n",
 			 StateToSet, priv->rtllib->RfOffReason);
 		PHY_SetRFPowerState(dev, StateToSet);
 		if (StateToSet == eRfOn) {
@@ -434,10 +423,9 @@ bool MgntActSet_RF_State(struct net_device *dev,
 			}
 		}
 	} else {
-		RT_TRACE((COMP_PS | COMP_RF), "MgntActSet_RF_State(): "
-			 "Action is rejected.... StateToSet(%d), ChangeSource"
-			 "(%#X), RfOffReason(%#X)\n", StateToSet, ChangeSource,
-			 priv->rtllib->RfOffReason);
+		RT_TRACE((COMP_PS | COMP_RF),
+			 "MgntActSet_RF_State(): Action is rejected.... StateToSet(%d), ChangeSource(%#X), RfOffReason(%#X)\n",
+			 StateToSet, ChangeSource, priv->rtllib->RfOffReason);
 	}
 
 	if (!ProtectOrNot) {
@@ -460,9 +448,9 @@ static short rtl8192_get_nic_desc_num(struct net_device *dev, int prio)
 	* between the tail and the head
 	*/
 	if ((prio == MGNT_QUEUE) && (skb_queue_len(&ring->queue) > 10))
-		RT_TRACE(COMP_DBG, "-----[%d]---------ring->idx=%d "
-			 "queue_len=%d---------\n", prio, ring->idx,
-			 skb_queue_len(&ring->queue));
+		RT_TRACE(COMP_DBG,
+			 "-----[%d]---------ring->idx=%d queue_len=%d---------\n",
+			 prio, ring->idx, skb_queue_len(&ring->queue));
 	return skb_queue_len(&ring->queue);
 }
 
@@ -481,7 +469,7 @@ void rtl8192_tx_timeout(struct net_device *dev)
 	struct r8192_priv *priv = rtllib_priv(dev);
 
 	schedule_work(&priv->reset_wq);
-	printk(KERN_INFO "TXTIMEOUT");
+	netdev_info(dev, "TXTIMEOUT");
 }
 
 void rtl8192_irq_enable(struct net_device *dev)
@@ -526,8 +514,9 @@ void rtl8192_update_cap(struct net_device *dev, u16 cap)
 		if (priv->dot11CurrentPreambleMode != PREAMBLE_SHORT) {
 			ShortPreamble = true;
 			priv->dot11CurrentPreambleMode = PREAMBLE_SHORT;
-			RT_TRACE(COMP_DBG, "%s(): WLAN_CAPABILITY_SHORT_"
-				 "PREAMBLE\n", __func__);
+			RT_TRACE(COMP_DBG,
+				 "%s(): WLAN_CAPABILITY_SHORT_PREAMBLE\n",
+				 __func__);
 			priv->rtllib->SetHwRegHandler(dev, HW_VAR_ACK_PREAMBLE,
 					(unsigned char *)&ShortPreamble);
 		}
@@ -535,8 +524,9 @@ void rtl8192_update_cap(struct net_device *dev, u16 cap)
 		if (priv->dot11CurrentPreambleMode != PREAMBLE_LONG) {
 			ShortPreamble = false;
 			priv->dot11CurrentPreambleMode = PREAMBLE_LONG;
-			RT_TRACE(COMP_DBG, "%s(): WLAN_CAPABILITY_LONG_"
-				 "PREAMBLE\n", __func__);
+			RT_TRACE(COMP_DBG,
+				 "%s(): WLAN_CAPABILITY_LONG_PREAMBLE\n",
+				 __func__);
 			priv->rtllib->SetHwRegHandler(dev, HW_VAR_ACK_PREAMBLE,
 					      (unsigned char *)&ShortPreamble);
 		}
@@ -597,8 +587,8 @@ static void rtl8192_qos_activate(void *data)
 	mutex_lock(&priv->mutex);
 	if (priv->rtllib->state != RTLLIB_LINKED)
 		goto success;
-	RT_TRACE(COMP_QOS, "qos active process with associate response "
-		 "received\n");
+	RT_TRACE(COMP_QOS,
+		 "qos active process with associate response received\n");
 
 	for (i = 0; i <  QOS_QUEUE_NUM; i++)
 		priv->rtllib->SetHwRegHandler(dev, HW_VAR_AC_PARAM, (u8 *)(&i));
@@ -618,7 +608,7 @@ static int rtl8192_qos_handle_probe_response(struct r8192_priv *priv,
 	if (priv->rtllib->state != RTLLIB_LINKED)
 		return ret;
 
-	if ((priv->rtllib->iw_mode != IW_MODE_INFRA))
+	if (priv->rtllib->iw_mode != IW_MODE_INFRA)
 		return ret;
 
 	if (network->flags & NETWORK_HAS_QOS_MASK) {
@@ -634,8 +624,8 @@ static int rtl8192_qos_handle_probe_response(struct r8192_priv *priv,
 				network->qos_data.param_count;
 	priv->rtllib->wmm_acm = network->qos_data.wmm_acm;
 			queue_work_rsl(priv->priv_wq, &priv->qos_activate);
-			RT_TRACE(COMP_QOS, "QoS parameters change call "
-					"qos_activate\n");
+			RT_TRACE(COMP_QOS,
+				 "QoS parameters change call qos_activate\n");
 		}
 	} else {
 		memcpy(&priv->rtllib->current_network.qos_data.parameters,
@@ -643,8 +633,8 @@ static int rtl8192_qos_handle_probe_response(struct r8192_priv *priv,
 
 		if ((network->qos_data.active == 1) && (active_network == 1)) {
 			queue_work_rsl(priv->priv_wq, &priv->qos_activate);
-			RT_TRACE(COMP_QOS, "QoS was disabled call qos_"
-				 "activate\n");
+			RT_TRACE(COMP_QOS,
+				 "QoS was disabled call qos_activate\n");
 		}
 		network->qos_data.active = 0;
 		network->qos_data.supported = 0;
@@ -679,7 +669,7 @@ static int rtl8192_qos_association_resp(struct r8192_priv *priv,
 	if (priv->rtllib->state != RTLLIB_LINKED)
 		return 0;
 
-	if ((priv->rtllib->iw_mode != IW_MODE_INFRA))
+	if (priv->rtllib->iw_mode != IW_MODE_INFRA)
 		return 0;
 
 	spin_lock_irqsave(&priv->rtllib->lock, flags);
@@ -751,8 +741,6 @@ static void rtl8192_prepare_beacon(struct r8192_priv *priv)
 	priv->ops->tx_fill_descriptor(dev, pdesc, tcb_desc, pnewskb);
 	__skb_queue_tail(&ring->queue, pnewskb);
 	pdesc->OWN = 1;
-
-	return;
 }
 
 static void rtl8192_stop_beacon(struct net_device *dev)
@@ -866,7 +854,6 @@ static void rtl8192_refresh_supportrate(struct r8192_priv *priv)
 	} else {
 		memset(ieee->Regdot11HTOperationalRateSet, 0, 16);
 	}
-	return;
 }
 
 static u8 rtl8192_getSupportedWireleeMode(struct net_device *dev)
@@ -909,8 +896,9 @@ void rtl8192_SetWirelessMode(struct net_device *dev, u8 wireless_mode)
 		} else if ((bSupportMode & WIRELESS_MODE_B)) {
 			wireless_mode = WIRELESS_MODE_B;
 		} else {
-			RT_TRACE(COMP_ERR, "%s(), No valid wireless mode "
-				 "supported (%x)!!!\n", __func__, bSupportMode);
+			RT_TRACE(COMP_ERR,
+				 "%s(), No valid wireless mode supported (%x)!!!\n",
+				 __func__, bSupportMode);
 			wireless_mode = WIRELESS_MODE_B;
 		}
 	}
@@ -1025,8 +1013,9 @@ static int rtl8192_sta_down(struct net_device *dev, bool shutdownrf)
 			spin_lock_irqsave(&priv->rf_ps_lock, flags);
 			break;
 		}
-		RT_TRACE(COMP_DBG, "===>%s():RF is in progress, need to wait "
-			 "until rf change is done.\n", __func__);
+		RT_TRACE(COMP_DBG,
+			 "===>%s():RF is in progress, need to wait until rf change is done.\n",
+			 __func__);
 		mdelay(1);
 		RFInProgressTimeOut++;
 		spin_lock_irqsave(&priv->rf_ps_lock, flags);
@@ -1212,8 +1201,8 @@ static void rtl8192_init_priv_variable(struct net_device *dev)
 	priv->AcmControl = 0;
 	priv->pFirmware = vzalloc(sizeof(struct rt_firmware));
 	if (!priv->pFirmware)
-		printk(KERN_ERR "rtl8192e: Unable to allocate space "
-		       "for firmware\n");
+		netdev_err(dev,
+			   "rtl8192e: Unable to allocate space for firmware\n");
 
 	skb_queue_head_init(&priv->rx_queue);
 	skb_queue_head_init(&priv->skb_queue);
@@ -1278,14 +1267,15 @@ static short rtl8192_get_channel_map(struct net_device *dev)
 
 	if ((priv->rf_chip != RF_8225) && (priv->rf_chip != RF_8256)
 			&& (priv->rf_chip != RF_6052)) {
-		RT_TRACE(COMP_ERR, "%s: unknown rf chip, can't set channel "
-			 "map\n", __func__);
+		RT_TRACE(COMP_ERR,
+			 "%s: unknown rf chip, can't set channel map\n",
+			 __func__);
 		return -1;
 	}
 
 	if (priv->ChannelPlan >= COUNTRY_CODE_MAX) {
-		printk(KERN_INFO "rtl819x_init:Error channel plan! Set to "
-		       "default.\n");
+		netdev_info(dev,
+			    "rtl819x_init:Error channel plan! Set to default.\n");
 		priv->ChannelPlan = COUNTRY_CODE_FCC;
 	}
 	RT_TRACE(COMP_INIT, "Channel plan is %d\n", priv->ChannelPlan);
@@ -1329,15 +1319,15 @@ static short rtl8192_init(struct net_device *dev)
 	rtl8192_irq_disable(dev);
 	if (request_irq(dev->irq, rtl8192_interrupt, IRQF_SHARED,
 	    dev->name, dev)) {
-		printk(KERN_ERR "Error allocating IRQ %d", dev->irq);
+		netdev_err(dev, "Error allocating IRQ %d", dev->irq);
 		return -1;
-	} else {
-		priv->irq = dev->irq;
-		RT_TRACE(COMP_INIT, "IRQ %d\n", dev->irq);
 	}
 
+	priv->irq = dev->irq;
+	RT_TRACE(COMP_INIT, "IRQ %d\n", dev->irq);
+
 	if (rtl8192_pci_initdescring(dev) != 0) {
-		printk(KERN_ERR "Endopoints initialization failed");
+		netdev_err(dev, "Endopoints initialization failed");
 		free_irq(dev->irq, dev);
 		return -1;
 	}
@@ -1357,7 +1347,7 @@ short rtl8192_is_tx_queue_empty(struct net_device *dev)
 		if ((i == TXCMD_QUEUE) || (i == HCCA_QUEUE))
 			continue;
 		if (skb_queue_len(&(&priv->tx_ring[i])->queue) > 0) {
-			printk(KERN_INFO "===>tx queue is not empty:%d, %d\n",
+			netdev_info(dev, "===>tx queue is not empty:%d, %d\n",
 			       i, skb_queue_len(&(&priv->tx_ring[i])->queue));
 			return 0;
 		}
@@ -1406,17 +1396,18 @@ static enum reset_type rtl819x_TxCheckStuck(struct net_device *dev)
 			tcb_desc->nStuckCount++;
 			bCheckFwTxCnt = true;
 			if (tcb_desc->nStuckCount > 1)
-				printk(KERN_INFO "%s: QueueID=%d tcb_desc->n"
-				       "StuckCount=%d\n", __func__, QueueID,
-				       tcb_desc->nStuckCount);
+				netdev_info(dev,
+					    "%s: QueueID=%d tcb_desc->nStuckCount=%d\n",
+					    __func__, QueueID,
+					    tcb_desc->nStuckCount);
 		}
 	}
 	spin_unlock_irqrestore(&priv->irq_th_lock, flags);
 
 	if (bCheckFwTxCnt) {
 		if (priv->ops->TxCheckStuckHandler(dev)) {
-			RT_TRACE(COMP_RESET, "TxCheckStuck(): Fw indicates no"
-				 " Tx condition!\n");
+			RT_TRACE(COMP_RESET,
+				 "TxCheckStuck(): Fw indicates no Tx condition!\n");
 			return RESET_TYPE_SILENT;
 		}
 	}
@@ -1455,13 +1446,13 @@ static enum reset_type rtl819x_ifcheck_resetornot(struct net_device *dev)
 
 	if (TxResetType == RESET_TYPE_NORMAL ||
 	    RxResetType == RESET_TYPE_NORMAL) {
-		printk(KERN_INFO "%s(): TxResetType is %d, RxResetType is %d\n",
-		       __func__, TxResetType, RxResetType);
+		netdev_info(dev, "%s(): TxResetType is %d, RxResetType is %d\n",
+			    __func__, TxResetType, RxResetType);
 		return RESET_TYPE_NORMAL;
 	} else if (TxResetType == RESET_TYPE_SILENT ||
 		   RxResetType == RESET_TYPE_SILENT) {
-		printk(KERN_INFO "%s(): TxResetType is %d, RxResetType is %d\n",
-		       __func__, TxResetType, RxResetType);
+		netdev_info(dev, "%s(): TxResetType is %d, RxResetType is %d\n",
+			    __func__, TxResetType, RxResetType);
 		return RESET_TYPE_SILENT;
 	} else {
 		return RESET_TYPE_NORESET;
@@ -1506,9 +1497,10 @@ RESET_START:
 		if (priv->rtllib->state == RTLLIB_LINKED)
 			LeisurePSLeave(dev);
 
-		if (IS_NIC_DOWN(priv)) {
-			RT_TRACE(COMP_ERR, "%s():the driver is not up! "
-				 "return\n", __func__);
+		if (priv->up) {
+			RT_TRACE(COMP_ERR,
+				 "%s():the driver is not up! return\n",
+				 __func__);
 			up(&priv->wx_sem);
 			return;
 		}
@@ -1517,8 +1509,9 @@ RESET_START:
 		RT_TRACE(COMP_RESET, "%s():======>start to down the driver\n",
 			  __func__);
 		mdelay(1000);
-		RT_TRACE(COMP_RESET, "%s():111111111111111111111111======>start"
-			 " to down the driver\n", __func__);
+		RT_TRACE(COMP_RESET,
+			 "%s():111111111111111111111111======>start to down the driver\n",
+			 __func__);
 
 		if (!netif_queue_stopped(dev))
 			netif_stop_queue(dev);
@@ -1531,7 +1524,7 @@ RESET_START:
 
 		if (ieee->state == RTLLIB_LINKED) {
 			SEM_DOWN_IEEE_WX(&ieee->wx_sem);
-			printk(KERN_INFO "ieee->state is RTLLIB_LINKED\n");
+			netdev_info(dev, "ieee->state is RTLLIB_LINKED\n");
 			rtllib_stop_send_beacons(priv->rtllib);
 			del_timer_sync(&ieee->associate_timer);
 			cancel_delayed_work(&ieee->associate_retry_wq);
@@ -1539,29 +1532,31 @@ RESET_START:
 			netif_carrier_off(dev);
 			SEM_UP_IEEE_WX(&ieee->wx_sem);
 		} else {
-			printk(KERN_INFO "ieee->state is NOT LINKED\n");
+			netdev_info(dev, "ieee->state is NOT LINKED\n");
 			rtllib_softmac_stop_protocol(priv->rtllib, 0 , true);
 		}
 
 		dm_backup_dynamic_mechanism_state(dev);
 
 		up(&priv->wx_sem);
-		RT_TRACE(COMP_RESET, "%s():<==========down process is "
-			 "finished\n", __func__);
+		RT_TRACE(COMP_RESET,
+			 "%s():<==========down process is finished\n",
+			 __func__);
 
 		RT_TRACE(COMP_RESET, "%s():<===========up process start\n",
 			 __func__);
 		reset_status = _rtl8192_up(dev, true);
 
-		RT_TRACE(COMP_RESET, "%s():<===========up process is "
-			 "finished\n", __func__);
+		RT_TRACE(COMP_RESET,
+			 "%s():<===========up process is finished\n", __func__);
 		if (reset_status == -1) {
 			if (reset_times < 3) {
 				reset_times++;
 				goto RESET_START;
 			} else {
-				RT_TRACE(COMP_ERR, " ERR!!! %s():  Reset "
-					 "Failed!!\n", __func__);
+				RT_TRACE(COMP_ERR,
+					 " ERR!!! %s():  Reset Failed!!\n",
+					 __func__);
 			}
 		}
 
@@ -1650,7 +1645,7 @@ void	rtl819x_watchdog_wqcallback(void *data)
 	bool	bHigherBusyRxTraffic = false;
 	bool bEnterPS = false;
 
-	if (IS_NIC_DOWN(priv) || priv->bHwRadioOff)
+	if (!priv->up || priv->bHwRadioOff)
 		return;
 
 	if (priv->rtllib->state >= RTLLIB_LINKED) {
@@ -1670,8 +1665,8 @@ void	rtl819x_watchdog_wqcallback(void *data)
 			if ((ieee->PowerSaveControl.ReturnPoint ==
 			     IPS_CALLBACK_NONE) &&
 			     (!ieee->bNetPromiscuousMode)) {
-				RT_TRACE(COMP_PS, "====================>haha: "
-					 "IPSEnter()\n");
+				RT_TRACE(COMP_PS,
+					 "====================>haha: IPSEnter()\n");
 				IPSEnter(dev);
 			}
 		}
@@ -1736,8 +1731,9 @@ void	rtl819x_watchdog_wqcallback(void *data)
 			if (ieee->eRFPowerState == eRfOff)
 				RT_TRACE(COMP_ERR, "========>%s()\n", __func__);
 
-			printk(KERN_INFO "===>%s(): AP is power off, chan:%d,"
-			       " connect another one\n", __func__, priv->chan);
+			netdev_info(dev,
+				    "===>%s(): AP is power off, chan:%d, connect another one\n",
+				    __func__, priv->chan);
 
 			ieee->state = RTLLIB_ASSOCIATING;
 
@@ -1793,7 +1789,7 @@ void watch_dog_timer_callback(unsigned long data)
 
 	queue_delayed_work_rsl(priv->priv_wq, &priv->watch_dog_wq, 0);
 	mod_timer(&priv->watch_dog_timer, jiffies +
-		  MSECS(RTLLIB_WATCH_DOG_TIME));
+		  msecs_to_jiffies(RTLLIB_WATCH_DOG_TIME));
 }
 
 /****************************************************************************
@@ -1882,7 +1878,7 @@ void rtl8192_hard_data_xmit(struct sk_buff *skb, struct net_device *dev,
 				    MAX_DEV_ADDR_SIZE);
 	u8 queue_index = tcb_desc->queue_index;
 
-	if ((priv->rtllib->eRFPowerState == eRfOff) || IS_NIC_DOWN(priv) ||
+	if ((priv->rtllib->eRFPowerState == eRfOff) || !priv->up ||
 	     priv->bResetInProgress) {
 		kfree_skb(skb);
 		return;
@@ -1902,8 +1898,6 @@ void rtl8192_hard_data_xmit(struct sk_buff *skb, struct net_device *dev,
 						 priv->rtllib->tx_headroom);
 		priv->rtllib->stats.tx_packets++;
 	}
-
-	return;
 }
 
 int rtl8192_hard_start_xmit(struct sk_buff *skb, struct net_device *dev)
@@ -1916,7 +1910,7 @@ int rtl8192_hard_start_xmit(struct sk_buff *skb, struct net_device *dev)
 
 	if (queue_index != TXCMD_QUEUE) {
 		if ((priv->rtllib->eRFPowerState == eRfOff) ||
-		     IS_NIC_DOWN(priv) || priv->bResetInProgress) {
+		     !priv->up || priv->bResetInProgress) {
 			kfree_skb(skb);
 			return 0;
 		}
@@ -1926,17 +1920,16 @@ int rtl8192_hard_start_xmit(struct sk_buff *skb, struct net_device *dev)
 	if (queue_index == TXCMD_QUEUE) {
 		rtl8192_tx_cmd(dev, skb);
 		return 0;
-	} else {
-		tcb_desc->RATRIndex = 7;
-		tcb_desc->bTxDisableRateFallBack = 1;
-		tcb_desc->bTxUseDriverAssingedRate = 1;
-		tcb_desc->bTxEnableFwCalcDur = 1;
-		skb_push(skb, priv->rtllib->tx_headroom);
-		ret = rtl8192_tx(dev, skb);
-		if (ret != 0)
-			kfree_skb(skb);
 	}
 
+	tcb_desc->RATRIndex = 7;
+	tcb_desc->bTxDisableRateFallBack = 1;
+	tcb_desc->bTxUseDriverAssingedRate = 1;
+	tcb_desc->bTxEnableFwCalcDur = 1;
+	skb_push(skb, priv->rtllib->tx_headroom);
+	ret = rtl8192_tx(dev, skb);
+	if (ret != 0)
+		kfree_skb(skb);
 	return ret;
 }
 
@@ -1987,8 +1980,6 @@ void rtl8192_tx_cmd(struct net_device *dev, struct sk_buff *skb)
 
 	__skb_queue_tail(&ring->queue, skb);
 	spin_unlock_irqrestore(&priv->irq_th_lock, flags);
-
-	return;
 }
 
 short rtl8192_tx(struct net_device *dev, struct sk_buff *skb)
@@ -2007,9 +1998,9 @@ short rtl8192_tx(struct net_device *dev, struct sk_buff *skb)
 	u32 fwinfo_size = 0;
 
 	if (priv->bdisable_nic) {
-		RT_TRACE(COMP_ERR, "%s: ERR!! Nic is disabled! Can't tx packet"
-			 " len=%d qidx=%d!!!\n", __func__, skb->len,
-			 tcb_desc->queue_index);
+		RT_TRACE(COMP_ERR,
+			 "%s: ERR!! Nic is disabled! Can't tx packet len=%d qidx=%d!!!\n",
+			 __func__, skb->len, tcb_desc->queue_index);
 		return skb->len;
 	}
 
@@ -2046,8 +2037,8 @@ short rtl8192_tx(struct net_device *dev, struct sk_buff *skb)
 
 	pdesc = &ring->desc[idx];
 	if ((pdesc->OWN == 1) && (tcb_desc->queue_index != BEACON_QUEUE)) {
-		RT_TRACE(COMP_ERR, "No more TX desc@%d, ring->idx = %d, idx = "
-			 "%d, skblen = 0x%x queuelen=%d",
+		RT_TRACE(COMP_ERR,
+			 "No more TX desc@%d, ring->idx = %d, idx = %d, skblen = 0x%x queuelen=%d",
 			 tcb_desc->queue_index, ring->idx, idx, skb->len,
 			 skb_queue_len(&ring->queue));
 		spin_unlock_irqrestore(&priv->irq_th_lock, flags);
@@ -2332,76 +2323,74 @@ static void rtl8192_rx_normal(struct net_device *dev)
 					[priv->rx_idx[rx_queue_idx]];
 		struct sk_buff *skb = priv->rx_buf[rx_queue_idx]
 				      [priv->rx_idx[rx_queue_idx]];
+		struct sk_buff *new_skb;
 
-		if (pdesc->OWN) {
+		if (pdesc->OWN)
 			return;
-		} else {
-			struct sk_buff *new_skb;
-
-			if (!priv->ops->rx_query_status_descriptor(dev, &stats,
-			pdesc, skb))
-				goto done;
-			new_skb = dev_alloc_skb(priv->rxbuffersize);
-			/* if allocation of new skb failed - drop current packet
-			* and reuse skb */
-			if (unlikely(!new_skb))
-				goto done;
-
-			pci_unmap_single(priv->pdev,
-					*((dma_addr_t *)skb->cb),
-					priv->rxbuffersize,
-					PCI_DMA_FROMDEVICE);
-
-			skb_put(skb, pdesc->Length);
-			skb_reserve(skb, stats.RxDrvInfoSize +
-				stats.RxBufShift);
-			skb_trim(skb, skb->len - 4/*sCrcLng*/);
-			rtllib_hdr = (struct rtllib_hdr_1addr *)skb->data;
-			if (!is_multicast_ether_addr(rtllib_hdr->addr1)) {
-				/* unicast packet */
-				unicast_packet = true;
-			}
-			fc = le16_to_cpu(rtllib_hdr->frame_ctl);
-			type = WLAN_FC_GET_TYPE(fc);
-			if (type == RTLLIB_FTYPE_MGMT)
-				bLedBlinking = false;
-
-			if (bLedBlinking)
-				if (priv->rtllib->LedControlHandler)
-					priv->rtllib->LedControlHandler(dev,
-								LED_CTL_RX);
-
-			if (stats.bCRC) {
-				if (type != RTLLIB_FTYPE_MGMT)
-					priv->stats.rxdatacrcerr++;
-				else
-					priv->stats.rxmgmtcrcerr++;
-			}
-
-			skb_len = skb->len;
+		if (!priv->ops->rx_query_status_descriptor(dev, &stats,
+		pdesc, skb))
+			goto done;
+		new_skb = dev_alloc_skb(priv->rxbuffersize);
+		/* if allocation of new skb failed - drop current packet
+		 * and reuse skb
+		 */
+		if (unlikely(!new_skb))
+			goto done;
+
+		pci_unmap_single(priv->pdev,
+				*((dma_addr_t *)skb->cb),
+				priv->rxbuffersize,
+				PCI_DMA_FROMDEVICE);
+
+		skb_put(skb, pdesc->Length);
+		skb_reserve(skb, stats.RxDrvInfoSize +
+			stats.RxBufShift);
+		skb_trim(skb, skb->len - 4/*sCrcLng*/);
+		rtllib_hdr = (struct rtllib_hdr_1addr *)skb->data;
+		if (!is_multicast_ether_addr(rtllib_hdr->addr1)) {
+			/* unicast packet */
+			unicast_packet = true;
+		}
+		fc = le16_to_cpu(rtllib_hdr->frame_ctl);
+		type = WLAN_FC_GET_TYPE(fc);
+		if (type == RTLLIB_FTYPE_MGMT)
+			bLedBlinking = false;
+
+		if (bLedBlinking)
+			if (priv->rtllib->LedControlHandler)
+				priv->rtllib->LedControlHandler(dev,
+							LED_CTL_RX);
+
+		if (stats.bCRC) {
+			if (type != RTLLIB_FTYPE_MGMT)
+				priv->stats.rxdatacrcerr++;
+			else
+				priv->stats.rxmgmtcrcerr++;
+		}
 
-			if (!rtllib_rx(priv->rtllib, skb, &stats)) {
-				dev_kfree_skb_any(skb);
-			} else {
-				priv->stats.rxok++;
-				if (unicast_packet)
-					priv->stats.rxbytesunicast += skb_len;
-			}
+		skb_len = skb->len;
 
-			skb = new_skb;
-			skb->dev = dev;
+		if (!rtllib_rx(priv->rtllib, skb, &stats)) {
+			dev_kfree_skb_any(skb);
+		} else {
+			priv->stats.rxok++;
+			if (unicast_packet)
+				priv->stats.rxbytesunicast += skb_len;
+		}
 
-			priv->rx_buf[rx_queue_idx][priv->rx_idx[rx_queue_idx]] =
-									 skb;
-			*((dma_addr_t *) skb->cb) = pci_map_single(priv->pdev,
-						    skb_tail_pointer_rsl(skb),
-						    priv->rxbuffersize,
-						    PCI_DMA_FROMDEVICE);
-			if (pci_dma_mapping_error(priv->pdev,
-						  *((dma_addr_t *)skb->cb))) {
-				dev_kfree_skb_any(skb);
-				return;
-			}
+		skb = new_skb;
+		skb->dev = dev;
+
+		priv->rx_buf[rx_queue_idx][priv->rx_idx[rx_queue_idx]] =
+								 skb;
+		*((dma_addr_t *) skb->cb) = pci_map_single(priv->pdev,
+					    skb_tail_pointer_rsl(skb),
+					    priv->rxbuffersize,
+					    PCI_DMA_FROMDEVICE);
+		if (pci_dma_mapping_error(priv->pdev,
+					  *((dma_addr_t *)skb->cb))) {
+			dev_kfree_skb_any(skb);
+			return;
 		}
 done:
 		pdesc->BufferAddress = *((dma_addr_t *)skb->cb);
@@ -2567,7 +2556,7 @@ static int r8192_set_mac_adr(struct net_device *dev, void *mac)
 
 	down(&priv->wx_sem);
 
-	memcpy(dev->dev_addr, addr->sa_data, ETH_ALEN);
+	ether_addr_copy(dev->dev_addr, addr->sa_data);
 
 	schedule_work(&priv->reset_wq);
 	up(&priv->wx_sem);
@@ -2869,7 +2858,8 @@ static int rtl8192_pci_probe(struct pci_dev *pdev,
 
 	if (!pci_set_dma_mask(pdev, DMA_BIT_MASK(32))) {
 		if (pci_set_consistent_dma_mask(pdev, DMA_BIT_MASK(32))) {
-			printk(KERN_INFO "Unable to obtain 32bit DMA for consistent allocations\n");
+			dev_info(&pdev->dev,
+				 "Unable to obtain 32bit DMA for consistent allocations\n");
 			goto err_pci_disable;
 		}
 	}
@@ -2902,7 +2892,8 @@ static int rtl8192_pci_probe(struct pci_dev *pdev,
 		goto err_rel_rtllib;
 	}
 
-	printk(KERN_INFO "Memory mapped space start: 0x%08lx\n", pmem_start);
+	dev_info(&pdev->dev, "Memory mapped space start: 0x%08lx\n",
+		 pmem_start);
 	if (!request_mem_region(pmem_start, pmem_len, DRV_NAME)) {
 		RT_TRACE(COMP_ERR, "request_mem_region failed!");
 		goto err_rel_rtllib;
@@ -2940,8 +2931,8 @@ static int rtl8192_pci_probe(struct pci_dev *pdev,
 	dev->watchdog_timeo = HZ * 3;
 
 	if (dev_alloc_name(dev, ifname) < 0) {
-		RT_TRACE(COMP_INIT, "Oops: devname already taken! Trying "
-			 "wlan%%d...\n");
+		RT_TRACE(COMP_INIT,
+			 "Oops: devname already taken! Trying wlan%%d...\n");
 			dev_alloc_name(dev, ifname);
 	}
 
@@ -2981,7 +2972,7 @@ err_pci_disable:
 static void rtl8192_pci_disconnect(struct pci_dev *pdev)
 {
 	struct net_device *dev = pci_get_drvdata(pdev);
-	struct r8192_priv *priv ;
+	struct r8192_priv *priv;
 	u32 i;
 
 	if (dev) {
@@ -3004,7 +2995,7 @@ static void rtl8192_pci_disconnect(struct pci_dev *pdev)
 			rtl8192_free_tx_ring(dev, i);
 
 		if (priv->irq) {
-			printk(KERN_INFO "Freeing irq %d\n", dev->irq);
+			dev_info(&pdev->dev, "Freeing irq %d\n", dev->irq);
 			free_irq(dev->irq, dev);
 			priv->irq = 0;
 		}
@@ -3032,7 +3023,7 @@ bool NicIFEnableNIC(struct net_device *dev)
 	struct rt_pwr_save_ctrl *pPSC = (struct rt_pwr_save_ctrl *)
 					(&(priv->rtllib->PowerSaveControl));
 
-	if (IS_NIC_DOWN(priv)) {
+	if (!priv->up) {
 		RT_TRACE(COMP_ERR, "ERR!!! %s(): Driver is already down!\n",
 			 __func__);
 		priv->bdisable_nic = false;
@@ -3059,7 +3050,6 @@ bool NicIFEnableNIC(struct net_device *dev)
 }
 bool NicIFDisableNIC(struct net_device *dev)
 {
-	bool	status = true;
 	struct r8192_priv *priv = rtllib_priv(dev);
 	u8 tmp_state = 0;
 
@@ -3074,13 +3064,13 @@ bool NicIFDisableNIC(struct net_device *dev)
 	priv->ops->stop_adapter(dev, false);
 	RT_TRACE(COMP_PS, "<=========%s()\n", __func__);
 
-	return status;
+	return true;
 }
 
 static int __init rtl8192_pci_module_init(void)
 {
-	printk(KERN_INFO "\nLinux kernel driver for RTL8192E WLAN cards\n");
-	printk(KERN_INFO "Copyright (c) 2007-2008, Realsil Wlan Driver\n");
+	pr_info("\nLinux kernel driver for RTL8192E WLAN cards\n");
+	pr_info("Copyright (c) 2007-2008, Realsil Wlan Driver\n");
 
 	if (0 != pci_register_driver(&rtl8192_pci_driver)) {
 		DMESG("No device found");
@@ -3106,7 +3096,7 @@ void check_rfctrl_gpio_timer(unsigned long data)
 	queue_delayed_work_rsl(priv->priv_wq, &priv->gpio_change_rf_wq, 0);
 
 	mod_timer(&priv->gpio_polling_timer, jiffies +
-		  MSECS(RTLLIB_WATCH_DOG_TIME));
+		  msecs_to_jiffies(RTLLIB_WATCH_DOG_TIME));
 }
 
 /***************************************************************************
diff --git a/drivers/staging/rtl8192e/rtl8192e/rtl_core.h b/drivers/staging/rtl8192e/rtl8192e/rtl_core.h
index d1438c2f8980..d365af6ebdc7 100644
--- a/drivers/staging/rtl8192e/rtl8192e/rtl_core.h
+++ b/drivers/staging/rtl8192e/rtl8192e/rtl_core.h
@@ -98,14 +98,8 @@
 #define BIT(_i)				(1<<(_i))
 #endif
 
-#define IS_NIC_DOWN(priv)	(!(priv)->up)
-
 #define IS_ADAPTER_SENDS_BEACON(dev) 0
 
-#define IS_UNDER_11N_AES_MODE(_rtllib)		\
-	((_rtllib->pHTInfo->bCurrentHTSupport == true) && \
-	(_rtllib->pairwise_key_type == KEY_TYPE_CCMP))
-
 #define HAL_MEMORY_MAPPED_IO_RANGE_8190PCI	0x1000
 #define HAL_HW_PCI_REVISION_ID_8190PCI			0x00
 #define HAL_MEMORY_MAPPED_IO_RANGE_8192PCIE	0x4000
@@ -1004,14 +998,14 @@ short rtl8192_tx(struct net_device *dev, struct sk_buff *skb);
 
 u8 read_nic_io_byte(struct net_device *dev, int x);
 u32 read_nic_io_dword(struct net_device *dev, int x);
-u16 read_nic_io_word(struct net_device *dev, int x) ;
+u16 read_nic_io_word(struct net_device *dev, int x);
 void write_nic_io_byte(struct net_device *dev, int x, u8 y);
 void write_nic_io_word(struct net_device *dev, int x, u16 y);
 void write_nic_io_dword(struct net_device *dev, int x, u32 y);
 
 u8 read_nic_byte(struct net_device *dev, int x);
 u32 read_nic_dword(struct net_device *dev, int x);
-u16 read_nic_word(struct net_device *dev, int x) ;
+u16 read_nic_word(struct net_device *dev, int x);
 void write_nic_byte(struct net_device *dev, int x, u8 y);
 void write_nic_word(struct net_device *dev, int x, u16 y);
 void write_nic_dword(struct net_device *dev, int x, u32 y);
diff --git a/drivers/staging/rtl8192e/rtl8192e/rtl_dm.c b/drivers/staging/rtl8192e/rtl8192e/rtl_dm.c
index b8891c62af3e..df4bbcf38bae 100644
--- a/drivers/staging/rtl8192e/rtl8192e/rtl_dm.c
+++ b/drivers/staging/rtl8192e/rtl8192e/rtl_dm.c
@@ -267,7 +267,7 @@ static void dm_check_rate_adaptive(struct net_device *dev)
 	bool bshort_gi_enabled = false;
 	static u8 ping_rssi_state;
 
-	if (IS_NIC_DOWN(priv)) {
+	if (!priv->up) {
 		RT_TRACE(COMP_RATE, "<---- dm_check_rate_adaptive(): driver is going to unload\n");
 		return;
 	}
@@ -378,19 +378,16 @@ static void dm_bandwidth_autoswitch(struct net_device *dev)
 	struct r8192_priv *priv = rtllib_priv(dev);
 
 	if (priv->CurrentChannelBW == HT_CHANNEL_WIDTH_20 ||
-	   !priv->rtllib->bandwidth_auto_switch.bautoswitch_enable) {
+	   !priv->rtllib->bandwidth_auto_switch.bautoswitch_enable)
 		return;
+	if (priv->rtllib->bandwidth_auto_switch.bforced_tx20Mhz == false) {
+		if (priv->undecorated_smoothed_pwdb <=
+		    priv->rtllib->bandwidth_auto_switch.threshold_40Mhzto20Mhz)
+			priv->rtllib->bandwidth_auto_switch.bforced_tx20Mhz = true;
 	} else {
-		if (priv->rtllib->bandwidth_auto_switch.bforced_tx20Mhz == false) {
-			if (priv->undecorated_smoothed_pwdb <=
-			    priv->rtllib->bandwidth_auto_switch.threshold_40Mhzto20Mhz)
-				priv->rtllib->bandwidth_auto_switch.bforced_tx20Mhz = true;
-		} else {
-			if (priv->undecorated_smoothed_pwdb >=
-			    priv->rtllib->bandwidth_auto_switch.threshold_20Mhzto40Mhz)
-				priv->rtllib->bandwidth_auto_switch.bforced_tx20Mhz = false;
-
-		}
+		if (priv->undecorated_smoothed_pwdb >=
+		    priv->rtllib->bandwidth_auto_switch.threshold_20Mhzto40Mhz)
+			priv->rtllib->bandwidth_auto_switch.bforced_tx20Mhz = false;
 	}
 }
 
@@ -501,7 +498,7 @@ static void dm_TXPowerTrackingCallback_TSSI(struct net_device *dev)
 					write_nic_byte(dev, FW_Busy_Flag, 0);
 					return;
 				}
-				if ((priv->rtllib->eRFPowerState != eRfOn)) {
+				if (priv->rtllib->eRFPowerState != eRfOn) {
 					RT_TRACE(COMP_POWER_TRACKING,
 						 "we are in power save, so return\n");
 					write_nic_byte(dev, Pw_Track_Flag, 0);
@@ -581,148 +578,148 @@ static void dm_TXPowerTrackingCallback_TSSI(struct net_device *dev)
 					 "priv->CCKPresentAttentuation = %d\n",
 					 priv->CCKPresentAttentuation);
 				return;
-			} else {
-				if (Avg_TSSI_Meas_from_driver < TSSI_13dBm - E_FOR_TX_POWER_TRACK) {
-					if (RF_Type == RF_2T4R) {
-
-						if ((priv->rfa_txpowertrackingindex > 0) &&
-						    (priv->rfc_txpowertrackingindex > 0)) {
-							priv->rfa_txpowertrackingindex--;
-							if (priv->rfa_txpowertrackingindex_real > 4) {
-								priv->rfa_txpowertrackingindex_real--;
-								rtl8192_setBBreg(dev,
-									 rOFDM0_XATxIQImbalance,
-									 bMaskDWord,
-									 priv->txbbgain_table[priv->rfa_txpowertrackingindex_real].txbbgain_value);
-							}
-
-							priv->rfc_txpowertrackingindex--;
-							if (priv->rfc_txpowertrackingindex_real > 4) {
-								priv->rfc_txpowertrackingindex_real--;
-								rtl8192_setBBreg(dev,
-									 rOFDM0_XCTxIQImbalance,
-									 bMaskDWord,
-									 priv->txbbgain_table[priv->rfc_txpowertrackingindex_real].txbbgain_value);
-							}
-						} else {
-							rtl8192_setBBreg(dev, rOFDM0_XATxIQImbalance,
-									 bMaskDWord,
-									 priv->txbbgain_table[4].txbbgain_value);
-							rtl8192_setBBreg(dev,
-									 rOFDM0_XCTxIQImbalance,
-									 bMaskDWord, priv->txbbgain_table[4].txbbgain_value);
-						}
-					} else {
-						if (priv->rfa_txpowertrackingindex > 0) {
-							priv->rfa_txpowertrackingindex--;
-							if (priv->rfa_txpowertrackingindex_real > 4) {
-								priv->rfa_txpowertrackingindex_real--;
-								rtl8192_setBBreg(dev,
-										 rOFDM0_XATxIQImbalance,
-										 bMaskDWord,
-										 priv->txbbgain_table[priv->rfa_txpowertrackingindex_real].txbbgain_value);
-							}
-						} else
-							rtl8192_setBBreg(dev, rOFDM0_XATxIQImbalance,
-									 bMaskDWord, priv->txbbgain_table[4].txbbgain_value);
+			}
+			if (Avg_TSSI_Meas_from_driver < TSSI_13dBm - E_FOR_TX_POWER_TRACK) {
+				if (RF_Type == RF_2T4R) {
 
-					}
-				} else {
-					if (RF_Type == RF_2T4R) {
-						if ((priv->rfa_txpowertrackingindex <
-						    TxBBGainTableLength - 1) &&
-						    (priv->rfc_txpowertrackingindex <
-						    TxBBGainTableLength - 1)) {
-							priv->rfa_txpowertrackingindex++;
-							priv->rfa_txpowertrackingindex_real++;
+					if ((priv->rfa_txpowertrackingindex > 0) &&
+					    (priv->rfc_txpowertrackingindex > 0)) {
+						priv->rfa_txpowertrackingindex--;
+						if (priv->rfa_txpowertrackingindex_real > 4) {
+							priv->rfa_txpowertrackingindex_real--;
 							rtl8192_setBBreg(dev,
 								 rOFDM0_XATxIQImbalance,
 								 bMaskDWord,
-								 priv->txbbgain_table
-								 [priv->rfa_txpowertrackingindex_real].txbbgain_value);
-							priv->rfc_txpowertrackingindex++;
-							priv->rfc_txpowertrackingindex_real++;
+								 priv->txbbgain_table[priv->rfa_txpowertrackingindex_real].txbbgain_value);
+						}
+
+						priv->rfc_txpowertrackingindex--;
+						if (priv->rfc_txpowertrackingindex_real > 4) {
+							priv->rfc_txpowertrackingindex_real--;
 							rtl8192_setBBreg(dev,
 								 rOFDM0_XCTxIQImbalance,
 								 bMaskDWord,
 								 priv->txbbgain_table[priv->rfc_txpowertrackingindex_real].txbbgain_value);
-						} else {
-							rtl8192_setBBreg(dev,
-								 rOFDM0_XATxIQImbalance,
-								 bMaskDWord,
-								 priv->txbbgain_table[TxBBGainTableLength - 1].txbbgain_value);
-							rtl8192_setBBreg(dev,
-								 rOFDM0_XCTxIQImbalance,
-								 bMaskDWord, priv->txbbgain_table[TxBBGainTableLength - 1].txbbgain_value);
 						}
 					} else {
-						if (priv->rfa_txpowertrackingindex < (TxBBGainTableLength - 1)) {
-							priv->rfa_txpowertrackingindex++;
-							priv->rfa_txpowertrackingindex_real++;
-							rtl8192_setBBreg(dev, rOFDM0_XATxIQImbalance,
+						rtl8192_setBBreg(dev, rOFDM0_XATxIQImbalance,
+								 bMaskDWord,
+								 priv->txbbgain_table[4].txbbgain_value);
+						rtl8192_setBBreg(dev,
+								 rOFDM0_XCTxIQImbalance,
+								 bMaskDWord, priv->txbbgain_table[4].txbbgain_value);
+					}
+				} else {
+					if (priv->rfa_txpowertrackingindex > 0) {
+						priv->rfa_txpowertrackingindex--;
+						if (priv->rfa_txpowertrackingindex_real > 4) {
+							priv->rfa_txpowertrackingindex_real--;
+							rtl8192_setBBreg(dev,
+									 rOFDM0_XATxIQImbalance,
 									 bMaskDWord,
 									 priv->txbbgain_table[priv->rfa_txpowertrackingindex_real].txbbgain_value);
-						} else
-							rtl8192_setBBreg(dev, rOFDM0_XATxIQImbalance,
-									 bMaskDWord,
-									 priv->txbbgain_table[TxBBGainTableLength - 1].txbbgain_value);
-					}
+						}
+					} else
+						rtl8192_setBBreg(dev, rOFDM0_XATxIQImbalance,
+								 bMaskDWord, priv->txbbgain_table[4].txbbgain_value);
+
 				}
+			} else {
 				if (RF_Type == RF_2T4R) {
-					priv->CCKPresentAttentuation_difference
-						= priv->rfa_txpowertrackingindex - priv->rfa_txpowertracking_default;
+					if ((priv->rfa_txpowertrackingindex <
+					    TxBBGainTableLength - 1) &&
+					    (priv->rfc_txpowertrackingindex <
+					    TxBBGainTableLength - 1)) {
+						priv->rfa_txpowertrackingindex++;
+						priv->rfa_txpowertrackingindex_real++;
+						rtl8192_setBBreg(dev,
+							 rOFDM0_XATxIQImbalance,
+							 bMaskDWord,
+							 priv->txbbgain_table
+							 [priv->rfa_txpowertrackingindex_real].txbbgain_value);
+						priv->rfc_txpowertrackingindex++;
+						priv->rfc_txpowertrackingindex_real++;
+						rtl8192_setBBreg(dev,
+							 rOFDM0_XCTxIQImbalance,
+							 bMaskDWord,
+							 priv->txbbgain_table[priv->rfc_txpowertrackingindex_real].txbbgain_value);
+					} else {
+						rtl8192_setBBreg(dev,
+							 rOFDM0_XATxIQImbalance,
+							 bMaskDWord,
+							 priv->txbbgain_table[TxBBGainTableLength - 1].txbbgain_value);
+						rtl8192_setBBreg(dev,
+							 rOFDM0_XCTxIQImbalance,
+							 bMaskDWord, priv->txbbgain_table[TxBBGainTableLength - 1].txbbgain_value);
+					}
 				} else {
-					priv->CCKPresentAttentuation_difference
-						= priv->rfa_txpowertrackingindex_real - priv->rfa_txpowertracking_default;
-				}
-
-				if (priv->CurrentChannelBW == HT_CHANNEL_WIDTH_20)
-					priv->CCKPresentAttentuation =
-						 priv->CCKPresentAttentuation_20Mdefault +
-						 priv->CCKPresentAttentuation_difference;
-				else
-					priv->CCKPresentAttentuation =
-						 priv->CCKPresentAttentuation_40Mdefault +
-						 priv->CCKPresentAttentuation_difference;
-
-				if (priv->CCKPresentAttentuation > (CCKTxBBGainTableLength-1))
-					priv->CCKPresentAttentuation = CCKTxBBGainTableLength-1;
-				if (priv->CCKPresentAttentuation < 0)
-					priv->CCKPresentAttentuation = 0;
-
-				if (priv->CCKPresentAttentuation > -1 &&
-				    priv->CCKPresentAttentuation < CCKTxBBGainTableLength) {
-					if (priv->rtllib->current_network.channel == 14 &&
-					    !priv->bcck_in_ch14) {
-						priv->bcck_in_ch14 = true;
-						dm_cck_txpower_adjust(dev, priv->bcck_in_ch14);
-					} else if (priv->rtllib->current_network.channel != 14 && priv->bcck_in_ch14) {
-						priv->bcck_in_ch14 = false;
-						dm_cck_txpower_adjust(dev, priv->bcck_in_ch14);
+					if (priv->rfa_txpowertrackingindex < (TxBBGainTableLength - 1)) {
+						priv->rfa_txpowertrackingindex++;
+						priv->rfa_txpowertrackingindex_real++;
+						rtl8192_setBBreg(dev, rOFDM0_XATxIQImbalance,
+								 bMaskDWord,
+								 priv->txbbgain_table[priv->rfa_txpowertrackingindex_real].txbbgain_value);
 					} else
-						dm_cck_txpower_adjust(dev, priv->bcck_in_ch14);
+						rtl8192_setBBreg(dev, rOFDM0_XATxIQImbalance,
+								 bMaskDWord,
+								 priv->txbbgain_table[TxBBGainTableLength - 1].txbbgain_value);
 				}
-				RT_TRACE(COMP_POWER_TRACKING,
-					 "priv->rfa_txpowertrackingindex = %d\n",
-					 priv->rfa_txpowertrackingindex);
-				RT_TRACE(COMP_POWER_TRACKING,
-					 "priv->rfa_txpowertrackingindex_real = %d\n",
-					 priv->rfa_txpowertrackingindex_real);
-				RT_TRACE(COMP_POWER_TRACKING,
-					 "priv->CCKPresentAttentuation_difference = %d\n",
-					 priv->CCKPresentAttentuation_difference);
-				RT_TRACE(COMP_POWER_TRACKING,
-					 "priv->CCKPresentAttentuation = %d\n",
-					 priv->CCKPresentAttentuation);
+			}
+			if (RF_Type == RF_2T4R) {
+				priv->CCKPresentAttentuation_difference
+					= priv->rfa_txpowertrackingindex - priv->rfa_txpowertracking_default;
+			} else {
+				priv->CCKPresentAttentuation_difference
+					= priv->rfa_txpowertrackingindex_real - priv->rfa_txpowertracking_default;
+			}
 
-				if (priv->CCKPresentAttentuation_difference <= -12 || priv->CCKPresentAttentuation_difference >= 24) {
-					priv->rtllib->bdynamic_txpower_enable = true;
-					write_nic_byte(dev, Pw_Track_Flag, 0);
-					write_nic_byte(dev, FW_Busy_Flag, 0);
-					RT_TRACE(COMP_POWER_TRACKING, "tx power track--->limited\n");
-					return;
-				}
+			if (priv->CurrentChannelBW == HT_CHANNEL_WIDTH_20)
+				priv->CCKPresentAttentuation =
+					 priv->CCKPresentAttentuation_20Mdefault +
+					 priv->CCKPresentAttentuation_difference;
+			else
+				priv->CCKPresentAttentuation =
+					 priv->CCKPresentAttentuation_40Mdefault +
+					 priv->CCKPresentAttentuation_difference;
+
+			if (priv->CCKPresentAttentuation > (CCKTxBBGainTableLength-1))
+				priv->CCKPresentAttentuation = CCKTxBBGainTableLength-1;
+			if (priv->CCKPresentAttentuation < 0)
+				priv->CCKPresentAttentuation = 0;
+
+			if (priv->CCKPresentAttentuation > -1 &&
+			    priv->CCKPresentAttentuation < CCKTxBBGainTableLength) {
+				if (priv->rtllib->current_network.channel == 14 &&
+				    !priv->bcck_in_ch14) {
+					priv->bcck_in_ch14 = true;
+					dm_cck_txpower_adjust(dev, priv->bcck_in_ch14);
+				} else if (priv->rtllib->current_network.channel != 14 && priv->bcck_in_ch14) {
+					priv->bcck_in_ch14 = false;
+					dm_cck_txpower_adjust(dev, priv->bcck_in_ch14);
+				} else
+					dm_cck_txpower_adjust(dev, priv->bcck_in_ch14);
+			}
+			RT_TRACE(COMP_POWER_TRACKING,
+				 "priv->rfa_txpowertrackingindex = %d\n",
+				 priv->rfa_txpowertrackingindex);
+			RT_TRACE(COMP_POWER_TRACKING,
+				 "priv->rfa_txpowertrackingindex_real = %d\n",
+				 priv->rfa_txpowertrackingindex_real);
+			RT_TRACE(COMP_POWER_TRACKING,
+				 "priv->CCKPresentAttentuation_difference = %d\n",
+				 priv->CCKPresentAttentuation_difference);
+			RT_TRACE(COMP_POWER_TRACKING,
+				 "priv->CCKPresentAttentuation = %d\n",
+				 priv->CCKPresentAttentuation);
+
+			if (priv->CCKPresentAttentuation_difference <= -12 || priv->CCKPresentAttentuation_difference >= 24) {
+				priv->rtllib->bdynamic_txpower_enable = true;
+				write_nic_byte(dev, Pw_Track_Flag, 0);
+				write_nic_byte(dev, FW_Busy_Flag, 0);
+				RT_TRACE(COMP_POWER_TRACKING, "tx power track--->limited\n");
+				return;
 			}
+
 			write_nic_byte(dev, Pw_Track_Flag, 0);
 			Avg_TSSI_Meas_from_driver = 0;
 			for (k = 0; k < 5; k++)
@@ -757,8 +754,8 @@ static void dm_TXPowerTrackingCallback_ThermalMeter(struct net_device *dev)
 		for (i = 0; i < CCK_Table_length; i++) {
 			if (TempCCk == (u32)CCKSwingTable_Ch1_Ch13[i][0]) {
 				priv->CCK_index = (u8) i;
-				RT_TRACE(COMP_POWER_TRACKING, "Initial reg0x%x"
-					 " = 0x%x, CCK_index = 0x%x\n",
+				RT_TRACE(COMP_POWER_TRACKING,
+					 "Initial reg0x%x = 0x%x, CCK_index = 0x%x\n",
 					 rCCK0_TxFilter1, TempCCk,
 					 priv->CCK_index);
 				break;
@@ -803,8 +800,8 @@ static void dm_TXPowerTrackingCallback_ThermalMeter(struct net_device *dev)
 
 	priv->Record_CCK_20Mindex = tmpCCK20Mindex;
 	priv->Record_CCK_40Mindex = tmpCCK40Mindex;
-	RT_TRACE(COMP_POWER_TRACKING, "Record_CCK_20Mindex / Record_CCK_40"
-		 "Mindex = %d / %d.\n",
+	RT_TRACE(COMP_POWER_TRACKING,
+		 "Record_CCK_20Mindex / Record_CCK_40Mindex = %d / %d.\n",
 		 priv->Record_CCK_20Mindex, priv->Record_CCK_40Mindex);
 
 	if (priv->rtllib->current_network.channel == 14 &&
@@ -1401,13 +1398,12 @@ static void dm_CheckTXPowerTracking_ThermalMeter(struct net_device *dev)
 		TxPowerCheckCnt = 5;
 	else
 		TxPowerCheckCnt = 2;
-	if (!priv->btxpower_tracking) {
+	if (!priv->btxpower_tracking)
+		return;
+
+	if (priv->txpower_count  <= TxPowerCheckCnt) {
+		priv->txpower_count++;
 		return;
-	} else {
-		if (priv->txpower_count  <= TxPowerCheckCnt) {
-			priv->txpower_count++;
-			return;
-		}
 	}
 
 	if (!TM_Trigger) {
@@ -1419,14 +1415,12 @@ static void dm_CheckTXPowerTracking_ThermalMeter(struct net_device *dev)
 		}
 		TM_Trigger = 1;
 		return;
-	} else {
-	    printk(KERN_INFO "===============>Schedule TxPowerTrackingWorkItem\n");
-
-		queue_delayed_work_rsl(priv->priv_wq, &priv->txpower_tracking_wq, 0);
-		TM_Trigger = 0;
-		}
-
 	}
+	netdev_info(dev, "===============>Schedule TxPowerTrackingWorkItem\n");
+	queue_delayed_work_rsl(priv->priv_wq, &priv->txpower_tracking_wq, 0);
+	TM_Trigger = 0;
+
+}
 
 static void dm_check_txpower_tracking(struct net_device *dev)
 {
@@ -1446,7 +1440,7 @@ static void dm_CCKTxPowerAdjust_TSSI(struct net_device *dev, bool  bInCH14)
 	TempVal = 0;
 	if (!bInCH14) {
 		TempVal = (u32)(priv->cck_txbbgain_table[(u8)(priv->CCKPresentAttentuation)].ccktxbb_valuearray[0] +
-			  (priv->cck_txbbgain_table[(u8)(priv->CCKPresentAttentuation)].ccktxbb_valuearray[1]<<8)) ;
+			  (priv->cck_txbbgain_table[(u8)(priv->CCKPresentAttentuation)].ccktxbb_valuearray[1]<<8));
 
 		rtl8192_setBBreg(dev, rCCK0_TxFilter1, bMaskHWord, TempVal);
 		TempVal = (u32)(priv->cck_txbbgain_table[(u8)(priv->CCKPresentAttentuation)].ccktxbb_valuearray[2] +
@@ -1455,12 +1449,12 @@ static void dm_CCKTxPowerAdjust_TSSI(struct net_device *dev, bool  bInCH14)
 			  (priv->cck_txbbgain_table[(u8)(priv->CCKPresentAttentuation)].ccktxbb_valuearray[5]<<24));
 		rtl8192_setBBreg(dev, rCCK0_TxFilter2, bMaskDWord, TempVal);
 		TempVal = (u32)(priv->cck_txbbgain_table[(u8)(priv->CCKPresentAttentuation)].ccktxbb_valuearray[6] +
-			  (priv->cck_txbbgain_table[(u8)(priv->CCKPresentAttentuation)].ccktxbb_valuearray[7]<<8)) ;
+			  (priv->cck_txbbgain_table[(u8)(priv->CCKPresentAttentuation)].ccktxbb_valuearray[7]<<8));
 
 		rtl8192_setBBreg(dev, rCCK0_DebugPort, bMaskLWord, TempVal);
 	} else {
 		TempVal = (u32)(priv->cck_txbbgain_ch14_table[(u8)(priv->CCKPresentAttentuation)].ccktxbb_valuearray[0] +
-			  (priv->cck_txbbgain_ch14_table[(u8)(priv->CCKPresentAttentuation)].ccktxbb_valuearray[1]<<8)) ;
+			  (priv->cck_txbbgain_ch14_table[(u8)(priv->CCKPresentAttentuation)].ccktxbb_valuearray[1]<<8));
 
 		rtl8192_setBBreg(dev, rCCK0_TxFilter1, bMaskHWord, TempVal);
 		TempVal = (u32)(priv->cck_txbbgain_ch14_table[(u8)(priv->CCKPresentAttentuation)].ccktxbb_valuearray[2] +
@@ -1469,7 +1463,7 @@ static void dm_CCKTxPowerAdjust_TSSI(struct net_device *dev, bool  bInCH14)
 			  (priv->cck_txbbgain_ch14_table[(u8)(priv->CCKPresentAttentuation)].ccktxbb_valuearray[5]<<24));
 		rtl8192_setBBreg(dev, rCCK0_TxFilter2, bMaskDWord, TempVal);
 		TempVal = (u32)(priv->cck_txbbgain_ch14_table[(u8)(priv->CCKPresentAttentuation)].ccktxbb_valuearray[6] +
-			  (priv->cck_txbbgain_ch14_table[(u8)(priv->CCKPresentAttentuation)].ccktxbb_valuearray[7]<<8)) ;
+			  (priv->cck_txbbgain_ch14_table[(u8)(priv->CCKPresentAttentuation)].ccktxbb_valuearray[7]<<8));
 
 		rtl8192_setBBreg(dev, rCCK0_DebugPort, bMaskLWord, TempVal);
 	}
@@ -1485,7 +1479,7 @@ static void dm_CCKTxPowerAdjust_ThermalMeter(struct net_device *dev,	bool  bInCH
 	TempVal = 0;
 	if (!bInCH14) {
 		TempVal =	CCKSwingTable_Ch1_Ch13[priv->CCK_index][0] +
-					(CCKSwingTable_Ch1_Ch13[priv->CCK_index][1]<<8) ;
+					(CCKSwingTable_Ch1_Ch13[priv->CCK_index][1]<<8);
 		rtl8192_setBBreg(dev, rCCK0_TxFilter1, bMaskHWord, TempVal);
 		RT_TRACE(COMP_POWER_TRACKING, "CCK not chnl 14, reg 0x%x = 0x%x\n",
 			rCCK0_TxFilter1, TempVal);
@@ -1497,14 +1491,14 @@ static void dm_CCKTxPowerAdjust_ThermalMeter(struct net_device *dev,	bool  bInCH
 		RT_TRACE(COMP_POWER_TRACKING, "CCK not chnl 14, reg 0x%x = 0x%x\n",
 			rCCK0_TxFilter2, TempVal);
 		TempVal =	CCKSwingTable_Ch1_Ch13[priv->CCK_index][6] +
-					(CCKSwingTable_Ch1_Ch13[priv->CCK_index][7]<<8) ;
+					(CCKSwingTable_Ch1_Ch13[priv->CCK_index][7]<<8);
 
 		rtl8192_setBBreg(dev, rCCK0_DebugPort, bMaskLWord, TempVal);
 		RT_TRACE(COMP_POWER_TRACKING, "CCK not chnl 14, reg 0x%x = 0x%x\n",
 			rCCK0_DebugPort, TempVal);
 	} else {
 		TempVal =	CCKSwingTable_Ch14[priv->CCK_index][0] +
-					(CCKSwingTable_Ch14[priv->CCK_index][1]<<8) ;
+					(CCKSwingTable_Ch14[priv->CCK_index][1]<<8);
 
 		rtl8192_setBBreg(dev, rCCK0_TxFilter1, bMaskHWord, TempVal);
 		RT_TRACE(COMP_POWER_TRACKING, "CCK chnl 14, reg 0x%x = 0x%x\n",
@@ -1517,7 +1511,7 @@ static void dm_CCKTxPowerAdjust_ThermalMeter(struct net_device *dev,	bool  bInCH
 		RT_TRACE(COMP_POWER_TRACKING, "CCK chnl 14, reg 0x%x = 0x%x\n",
 			rCCK0_TxFilter2, TempVal);
 		TempVal =	CCKSwingTable_Ch14[priv->CCK_index][6] +
-					(CCKSwingTable_Ch14[priv->CCK_index][7]<<8) ;
+					(CCKSwingTable_Ch14[priv->CCK_index][7]<<8);
 
 		rtl8192_setBBreg(dev, rCCK0_DebugPort, bMaskLWord, TempVal);
 		RT_TRACE(COMP_POWER_TRACKING, "CCK chnl 14, reg 0x%x = 0x%x\n",
@@ -1569,7 +1563,7 @@ void dm_restore_dynamic_mechanism_state(struct net_device *dev)
 	u32	reg_ratr = priv->rate_adaptive.last_ratr;
 	u32 ratr_value;
 
-	if (IS_NIC_DOWN(priv)) {
+	if (!priv->up) {
 		RT_TRACE(COMP_RATE, "<---- dm_restore_dynamic_mechanism_state(): driver is going to unload\n");
 		return;
 	}
@@ -1830,12 +1824,11 @@ static void dm_ctrl_initgain_byrssi_by_fwfalse_alarm(
 	if ((priv->undecorated_smoothed_pwdb > dm_digtable.rssi_low_thresh) &&
 		(priv->undecorated_smoothed_pwdb < dm_digtable.rssi_high_thresh))
 		return;
-	if ((priv->undecorated_smoothed_pwdb <= dm_digtable.rssi_low_thresh)) {
+	if (priv->undecorated_smoothed_pwdb <= dm_digtable.rssi_low_thresh) {
 		if (dm_digtable.dig_state == DM_STA_DIG_OFF &&
 			(priv->reset_count == reset_cnt))
 			return;
-		else
-			reset_cnt = priv->reset_count;
+		reset_cnt = priv->reset_count;
 
 		dm_digtable.dig_highpwr_state = DM_STA_DIG_MAX;
 		dm_digtable.dig_state = DM_STA_DIG_OFF;
@@ -1857,19 +1850,18 @@ static void dm_ctrl_initgain_byrssi_by_fwfalse_alarm(
 		return;
 	}
 
-	if ((priv->undecorated_smoothed_pwdb >= dm_digtable.rssi_high_thresh)) {
+	if (priv->undecorated_smoothed_pwdb >= dm_digtable.rssi_high_thresh) {
 		u8 reset_flag = 0;
 
 		if (dm_digtable.dig_state == DM_STA_DIG_ON &&
 		    (priv->reset_count == reset_cnt)) {
 			dm_ctrl_initgain_byrssi_highpwr(dev);
 			return;
-		} else {
-			if (priv->reset_count != reset_cnt)
-				reset_flag = 1;
-
-			reset_cnt = priv->reset_count;
 		}
+		if (priv->reset_count != reset_cnt)
+			reset_flag = 1;
+
+		reset_cnt = priv->reset_count;
 
 		dm_digtable.dig_state = DM_STA_DIG_ON;
 
@@ -1911,8 +1903,7 @@ static void dm_ctrl_initgain_byrssi_highpwr(struct net_device *dev)
 		if (dm_digtable.dig_highpwr_state == DM_STA_DIG_ON &&
 			(priv->reset_count == reset_cnt_highpwr))
 			return;
-		else
-			dm_digtable.dig_highpwr_state = DM_STA_DIG_ON;
+		dm_digtable.dig_highpwr_state = DM_STA_DIG_ON;
 
 		if (priv->CurrentChannelBW != HT_CHANNEL_WIDTH_20)
 				write_nic_byte(dev, (rOFDM0_XATxAFE+3), 0x10);
@@ -1922,8 +1913,7 @@ static void dm_ctrl_initgain_byrssi_highpwr(struct net_device *dev)
 		if (dm_digtable.dig_highpwr_state == DM_STA_DIG_OFF &&
 			(priv->reset_count == reset_cnt_highpwr))
 			return;
-		else
-			dm_digtable.dig_highpwr_state = DM_STA_DIG_OFF;
+		dm_digtable.dig_highpwr_state = DM_STA_DIG_OFF;
 
 		if (priv->undecorated_smoothed_pwdb < dm_digtable.rssi_high_power_lowthresh &&
 			 priv->undecorated_smoothed_pwdb >= dm_digtable.rssi_high_thresh) {
@@ -2008,7 +1998,7 @@ static void dm_pd_th(struct net_device *dev)
 		if (dm_digtable.CurSTAConnectState == DIG_STA_CONNECT) {
 			if (dm_digtable.rssi_val >= dm_digtable.rssi_high_power_highthresh)
 				dm_digtable.curpd_thstate = DIG_PD_AT_HIGH_POWER;
-			else if ((dm_digtable.rssi_val <= dm_digtable.rssi_low_thresh))
+			else if (dm_digtable.rssi_val <= dm_digtable.rssi_low_thresh)
 				dm_digtable.curpd_thstate = DIG_PD_AT_LOW_POWER;
 			else if ((dm_digtable.rssi_val >= dm_digtable.rssi_high_thresh) &&
 					(dm_digtable.rssi_val < dm_digtable.rssi_high_power_lowthresh))
@@ -2065,9 +2055,9 @@ static	void dm_cs_ratio(struct net_device *dev)
 
 	if (dm_digtable.PreSTAConnectState == dm_digtable.CurSTAConnectState) {
 		if (dm_digtable.CurSTAConnectState == DIG_STA_CONNECT) {
-			if ((dm_digtable.rssi_val <= dm_digtable.rssi_low_thresh))
+			if (dm_digtable.rssi_val <= dm_digtable.rssi_low_thresh)
 				dm_digtable.curcs_ratio_state = DIG_CS_RATIO_LOWER;
-			else if ((dm_digtable.rssi_val >= dm_digtable.rssi_high_thresh))
+			else if (dm_digtable.rssi_val >= dm_digtable.rssi_high_thresh)
 				dm_digtable.curcs_ratio_state = DIG_CS_RATIO_HIGHER;
 			else
 				dm_digtable.curcs_ratio_state = dm_digtable.precs_ratio_state;
@@ -2131,10 +2121,10 @@ static void dm_check_edca_turbo(struct net_device *dev)
 		static int wb_tmp;
 
 		if (wb_tmp == 0) {
-			printk(KERN_INFO "%s():iot peer is %s, bssid:"
-			       " %pM\n", __func__,
-			       peername[pHTInfo->IOTPeer],
-			       priv->rtllib->current_network.bssid);
+			netdev_info(dev,
+				    "%s():iot peer is %s, bssid: %pM\n",
+				    __func__, peername[pHTInfo->IOTPeer],
+				    priv->rtllib->current_network.bssid);
 			wb_tmp = 1;
 		}
 	}
@@ -2638,9 +2628,10 @@ void dm_fsync_timer_callback(unsigned long data)
 		}
 		priv->rate_record = rate_count;
 		priv->rateCountDiffRecord = rate_count_diff;
-		RT_TRACE(COMP_HALDM, "rateRecord %d rateCount %d, rate"
-			 "Countdiff %d bSwitchFsync %d\n", priv->rate_record,
-			 rate_count, rate_count_diff, priv->bswitch_fsync);
+		RT_TRACE(COMP_HALDM,
+			 "rateRecord %d rateCount %d, rateCountdiff %d bSwitchFsync %d\n",
+			 priv->rate_record, rate_count, rate_count_diff,
+			 priv->bswitch_fsync);
 		if (priv->undecorated_smoothed_pwdb >
 		    priv->rtllib->fsync_rssi_threshold &&
 		    bSwitchFromCountDiff) {
@@ -2665,14 +2656,14 @@ void dm_fsync_timer_callback(unsigned long data)
 			if (timer_pending(&priv->fsync_timer))
 				del_timer_sync(&priv->fsync_timer);
 			priv->fsync_timer.expires = jiffies +
-				 MSECS(priv->rtllib->fsync_time_interval *
+				 msecs_to_jiffies(priv->rtllib->fsync_time_interval *
 				 priv->rtllib->fsync_multiple_timeinterval);
 			add_timer(&priv->fsync_timer);
 		} else {
 			if (timer_pending(&priv->fsync_timer))
 				del_timer_sync(&priv->fsync_timer);
 			priv->fsync_timer.expires = jiffies +
-				 MSECS(priv->rtllib->fsync_time_interval);
+				 msecs_to_jiffies(priv->rtllib->fsync_time_interval);
 			add_timer(&priv->fsync_timer);
 		}
 	} else {
@@ -2685,9 +2676,10 @@ void dm_fsync_timer_callback(unsigned long data)
 		write_nic_dword(dev, rOFDM0_RxDetector2, 0x465c52cd);
 	}
 	RT_TRACE(COMP_HALDM, "ContinueDiffCount %d\n", priv->ContinueDiffCount);
-	RT_TRACE(COMP_HALDM, "rateRecord %d rateCount %d, rateCountdiff %d "
-		 "bSwitchFsync %d\n", priv->rate_record, rate_count,
-		 rate_count_diff, priv->bswitch_fsync);
+	RT_TRACE(COMP_HALDM,
+		 "rateRecord %d rateCount %d, rateCountdiff %d bSwitchFsync %d\n",
+		 priv->rate_record, rate_count, rate_count_diff,
+		 priv->bswitch_fsync);
 }
 
 static void dm_StartHWFsync(struct net_device *dev)
@@ -2762,7 +2754,7 @@ static void dm_StartSWFsync(struct net_device *dev)
 	if (timer_pending(&priv->fsync_timer))
 		del_timer_sync(&priv->fsync_timer);
 	priv->fsync_timer.expires = jiffies +
-				    MSECS(priv->rtllib->fsync_time_interval);
+				    msecs_to_jiffies(priv->rtllib->fsync_time_interval);
 	add_timer(&priv->fsync_timer);
 
 	write_nic_dword(dev, rOFDM0_RxDetector2, 0x465c12cd);
@@ -2778,12 +2770,14 @@ void dm_check_fsync(struct net_device *dev)
 	static u8 reg_c38_State = RegC38_Default;
 	static u32 reset_cnt;
 
-	RT_TRACE(COMP_HALDM, "RSSI %d TimeInterval %d MultipleTimeInterval "
-		 "%d\n", priv->rtllib->fsync_rssi_threshold,
+	RT_TRACE(COMP_HALDM,
+		 "RSSI %d TimeInterval %d MultipleTimeInterval %d\n",
+		 priv->rtllib->fsync_rssi_threshold,
 		 priv->rtllib->fsync_time_interval,
 		 priv->rtllib->fsync_multiple_timeinterval);
-	RT_TRACE(COMP_HALDM, "RateBitmap 0x%x FirstDiffRateThreshold %d Second"
-		 "DiffRateThreshold %d\n", priv->rtllib->fsync_rate_bitmap,
+	RT_TRACE(COMP_HALDM,
+		 "RateBitmap 0x%x FirstDiffRateThreshold %d SecondDiffRateThreshold %d\n",
+		 priv->rtllib->fsync_rate_bitmap,
 		 priv->rtllib->fsync_firstdiff_ratethreshold,
 		 priv->rtllib->fsync_seconddiff_ratethreshold);
 
diff --git a/drivers/staging/rtl8192e/rtl8192e/rtl_dm.h b/drivers/staging/rtl8192e/rtl8192e/rtl_dm.h
index ab44a9a6927c..3f02e11cfc57 100644
--- a/drivers/staging/rtl8192e/rtl8192e/rtl_dm.h
+++ b/drivers/staging/rtl8192e/rtl8192e/rtl_dm.h
@@ -255,14 +255,6 @@ extern	u8			test_flag;
 /*------------------------Export global variable----------------------------*/
 
 
-/*------------------------Export Marco Definition---------------------------*/
-#define DM_APInitGainChangeNotify(Event)		\
-	{						\
-		dm_digtable.CurAPConnectState = Event;	\
-	}
-/*------------------------Export Marco Definition---------------------------*/
-
-
 /*--------------------------Exported Function prototype---------------------*/
 /*--------------------------Exported Function prototype---------------------*/
 extern  void    init_hal_dm(struct net_device *dev);
diff --git a/drivers/staging/rtl8192e/rtl8192e/rtl_pci.c b/drivers/staging/rtl8192e/rtl8192e/rtl_pci.c
index 2ad92eee50c2..51f53be2de17 100644
--- a/drivers/staging/rtl8192e/rtl8192e/rtl_pci.c
+++ b/drivers/staging/rtl8192e/rtl8192e/rtl_pci.c
@@ -65,27 +65,32 @@ bool rtl8192_pci_findadapter(struct pci_dev *pdev, struct net_device *dev)
 	if (DeviceID == 0x8172) {
 		switch (RevisionID) {
 		case HAL_HW_PCI_REVISION_ID_8192PCIE:
-			printk(KERN_INFO "Adapter(8192 PCI-E) is found - "
-			       "DeviceID=%x\n", DeviceID);
+			dev_info(&pdev->dev,
+				 "Adapter(8192 PCI-E) is found - DeviceID=%x\n",
+				 DeviceID);
 			priv->card_8192 = NIC_8192E;
 			break;
 		case HAL_HW_PCI_REVISION_ID_8192SE:
-			printk(KERN_INFO "Adapter(8192SE) is found - "
-			       "DeviceID=%x\n", DeviceID);
+			dev_info(&pdev->dev,
+				 "Adapter(8192SE) is found - DeviceID=%x\n",
+				 DeviceID);
 			priv->card_8192 = NIC_8192SE;
 			break;
 		default:
-			printk(KERN_INFO "UNKNOWN nic type(%4x:%4x)\n",
-			       pdev->vendor, pdev->device);
+			dev_info(&pdev->dev,
+				 "UNKNOWN nic type(%4x:%4x)\n",
+				 pdev->vendor, pdev->device);
 			priv->card_8192 = NIC_UNKNOWN;
 			return false;
 		}
 	}
 
 	if (priv->ops->nic_type != priv->card_8192) {
-		printk(KERN_INFO "Detect info(%x) and hardware info(%x) not match!\n",
-				priv->ops->nic_type, priv->card_8192);
-		printk(KERN_INFO "Please select proper driver before install!!!!\n");
+		dev_info(&pdev->dev,
+			 "Detect info(%x) and hardware info(%x) not match!\n",
+			 priv->ops->nic_type, priv->card_8192);
+		dev_info(&pdev->dev,
+			 "Please select proper driver before install!!!!\n");
 		return false;
 	}
 
diff --git a/drivers/staging/rtl8192e/rtl8192e/rtl_pm.c b/drivers/staging/rtl8192e/rtl8192e/rtl_pm.c
index 0b4f76481bf4..ca6ecfc8299e 100644
--- a/drivers/staging/rtl8192e/rtl8192e/rtl_pm.c
+++ b/drivers/staging/rtl8192e/rtl8192e/rtl_pm.c
@@ -29,14 +29,14 @@ int rtl8192E_suspend(struct pci_dev *pdev, pm_message_t state)
 	struct r8192_priv *priv = rtllib_priv(dev);
 	u32	ulRegRead;
 
-	printk(KERN_INFO "============> r8192E suspend call.\n");
+	netdev_info(dev, "============> r8192E suspend call.\n");
 	del_timer_sync(&priv->gpio_polling_timer);
 	cancel_delayed_work(&priv->gpio_change_rf_wq);
 	priv->polling_timer_on = 0;
 
 	if (!netif_running(dev)) {
-		printk(KERN_INFO "RTL819XE:UI is open out of suspend "
-		       "function\n");
+		netdev_info(dev,
+			    "RTL819XE:UI is open out of suspend function\n");
 		goto out_pci_suspend;
 	}
 
@@ -57,10 +57,10 @@ int rtl8192E_suspend(struct pci_dev *pdev, pm_message_t state)
 		write_nic_byte(dev, MacBlkCtrl, 0xa);
 	}
 out_pci_suspend:
-	printk("r8192E support WOL call??????????????????????\n");
+	netdev_info(dev, "r8192E support WOL call??????????????????????\n");
 	if (priv->rtllib->bSupportRemoteWakeUp)
-		RT_TRACE(COMP_POWER, "r8192E support WOL call!!!!!!!"
-			 "!!!!!!!!!!!.\n");
+		RT_TRACE(COMP_POWER,
+			 "r8192E support WOL call!!!!!!!!!!!!!!!!!!.\n");
 	pci_save_state(pdev);
 	pci_disable_device(pdev);
 	pci_enable_wake(pdev, pci_choose_state(pdev, state),
@@ -79,14 +79,13 @@ int rtl8192E_resume(struct pci_dev *pdev)
 	int err;
 	u32 val;
 
-	printk(KERN_INFO "================>r8192E resume call.\n");
+	netdev_info(dev, "================>r8192E resume call.\n");
 
 	pci_set_power_state(pdev, PCI_D0);
 
 	err = pci_enable_device(pdev);
 	if (err) {
-		printk(KERN_ERR "%s: pci_enable_device failed on resume\n",
-		       dev->name);
+		netdev_err(dev, "pci_enable_device failed on resume\n");
 		return err;
 	}
 	pci_restore_state(pdev);
@@ -101,8 +100,8 @@ int rtl8192E_resume(struct pci_dev *pdev)
 		check_rfctrl_gpio_timer((unsigned long)dev);
 
 	if (!netif_running(dev)) {
-		printk(KERN_INFO "RTL819XE:UI is open out of resume "
-		       "function\n");
+		netdev_info(dev,
+			    "RTL819XE:UI is open out of resume function\n");
 		goto out;
 	}
 
diff --git a/drivers/staging/rtl8192e/rtl8192e/rtl_ps.c b/drivers/staging/rtl8192e/rtl8192e/rtl_ps.c
index 4856b76292f9..0bbffec0c2ae 100644
--- a/drivers/staging/rtl8192e/rtl8192e/rtl_ps.c
+++ b/drivers/staging/rtl8192e/rtl8192e/rtl_ps.c
@@ -37,8 +37,8 @@ static void rtl8192_hw_sleep_down(struct net_device *dev)
 	spin_lock_irqsave(&priv->rf_ps_lock, flags);
 	if (priv->RFChangeInProgress) {
 		spin_unlock_irqrestore(&priv->rf_ps_lock, flags);
-		RT_TRACE(COMP_DBG, "rtl8192_hw_sleep_down(): RF Change in "
-			 "progress!\n");
+		RT_TRACE(COMP_DBG,
+			 "rtl8192_hw_sleep_down(): RF Change in progress!\n");
 		return;
 	}
 	spin_unlock_irqrestore(&priv->rf_ps_lock, flags);
@@ -64,10 +64,11 @@ void rtl8192_hw_wakeup(struct net_device *dev)
 	spin_lock_irqsave(&priv->rf_ps_lock, flags);
 	if (priv->RFChangeInProgress) {
 		spin_unlock_irqrestore(&priv->rf_ps_lock, flags);
-		RT_TRACE(COMP_DBG, "rtl8192_hw_wakeup(): RF Change in "
-			 "progress!\n");
+		RT_TRACE(COMP_DBG,
+			 "rtl8192_hw_wakeup(): RF Change in progress!\n");
 		queue_delayed_work_rsl(priv->rtllib->wq,
-				       &priv->rtllib->hw_wakeup_wq, MSECS(10));
+				       &priv->rtllib->hw_wakeup_wq,
+				       msecs_to_jiffies(10));
 		return;
 	}
 	spin_unlock_irqrestore(&priv->rf_ps_lock, flags);
@@ -95,18 +96,18 @@ void rtl8192_hw_to_sleep(struct net_device *dev, u64 time)
 
 	spin_lock_irqsave(&priv->ps_lock, flags);
 
-	time -= MSECS(8+16+7);
+	time -= msecs_to_jiffies(8 + 16 + 7);
 
-	if ((time - jiffies) <= MSECS(MIN_SLEEP_TIME)) {
+	if ((time - jiffies) <= msecs_to_jiffies(MIN_SLEEP_TIME)) {
 		spin_unlock_irqrestore(&priv->ps_lock, flags);
-		printk(KERN_INFO "too short to sleep::%lld < %ld\n",
-		       time - jiffies, MSECS(MIN_SLEEP_TIME));
+		netdev_info(dev, "too short to sleep::%lld < %ld\n",
+			    time - jiffies, msecs_to_jiffies(MIN_SLEEP_TIME));
 		return;
 	}
 
-	if ((time - jiffies) > MSECS(MAX_SLEEP_TIME)) {
-		printk(KERN_INFO "========>too long to sleep:%lld > %ld\n",
-		       time - jiffies,  MSECS(MAX_SLEEP_TIME));
+	if ((time - jiffies) > msecs_to_jiffies(MAX_SLEEP_TIME)) {
+		netdev_info(dev, "========>too long to sleep:%lld > %ld\n",
+			    time - jiffies, msecs_to_jiffies(MAX_SLEEP_TIME));
 		spin_unlock_irqrestore(&priv->ps_lock, flags);
 		return;
 	}
@@ -201,12 +202,11 @@ void rtllib_ips_leave_wq(struct net_device *dev)
 				RT_TRACE(COMP_ERR, "%s(): RF is OFF.\n",
 					 __func__);
 				return;
-			} else {
-				printk(KERN_INFO "=========>%s(): IPSLeave\n",
-				       __func__);
-				queue_work_rsl(priv->rtllib->wq,
-					       &priv->rtllib->ips_leave_wq);
 			}
+			netdev_info(dev, "=========>%s(): IPSLeave\n",
+				    __func__);
+			queue_work_rsl(priv->rtllib->wq,
+				       &priv->rtllib->ips_leave_wq);
 		}
 	}
 }
@@ -239,8 +239,8 @@ static bool MgntActSet_802_11_PowerSaveMode(struct net_device *dev,
 		priv->rtllib->sta_sleep = LPS_IS_WAKE;
 
 		spin_lock_irqsave(&(priv->rtllib->mgmt_tx_lock), flags);
-		RT_TRACE(COMP_DBG, "LPS leave: notify AP we are awaked"
-			 " ++++++++++ SendNullFunctionData\n");
+		RT_TRACE(COMP_DBG,
+			 "LPS leave: notify AP we are awaked ++++++++++ SendNullFunctionData\n");
 		rtllib_sta_ps_send_null_frame(priv->rtllib, 0);
 		spin_unlock_irqrestore(&(priv->rtllib->mgmt_tx_lock), flags);
 	}
@@ -255,8 +255,8 @@ void LeisurePSEnter(struct net_device *dev)
 					&(priv->rtllib->PowerSaveControl);
 
 	RT_TRACE(COMP_PS, "LeisurePSEnter()...\n");
-	RT_TRACE(COMP_PS, "pPSC->bLeisurePs = %d, ieee->ps = %d,pPSC->LpsIdle"
-		 "Count is %d,RT_CHECK_FOR_HANG_PERIOD is %d\n",
+	RT_TRACE(COMP_PS,
+		 "pPSC->bLeisurePs = %d, ieee->ps = %d,pPSC->LpsIdleCount is %d,RT_CHECK_FOR_HANG_PERIOD is %d\n",
 		 pPSC->bLeisurePs, priv->rtllib->ps, pPSC->LpsIdleCount,
 		 RT_CHECK_FOR_HANG_PERIOD);
 
@@ -271,8 +271,8 @@ void LeisurePSEnter(struct net_device *dev)
 
 			if (priv->rtllib->ps == RTLLIB_PS_DISABLED) {
 
-				RT_TRACE(COMP_LPS, "LeisurePSEnter(): Enter "
-					 "802.11 power save mode...\n");
+				RT_TRACE(COMP_LPS,
+					 "LeisurePSEnter(): Enter 802.11 power save mode...\n");
 
 				if (!pPSC->bFwCtrlLPS) {
 					if (priv->rtllib->SetFwCmdHandler)
@@ -301,8 +301,8 @@ void LeisurePSLeave(struct net_device *dev)
 
 	if (pPSC->bLeisurePs) {
 		if (priv->rtllib->ps != RTLLIB_PS_DISABLED) {
-			RT_TRACE(COMP_LPS, "LeisurePSLeave(): Busy Traffic , "
-				 "Leave 802.11 power save..\n");
+			RT_TRACE(COMP_LPS,
+				 "LeisurePSLeave(): Busy Traffic , Leave 802.11 power save..\n");
 			MgntActSet_802_11_PowerSaveMode(dev,
 					 RTLLIB_PS_DISABLED);
 
diff --git a/drivers/staging/rtl8192e/rtl8192e/rtl_wx.c b/drivers/staging/rtl8192e/rtl8192e/rtl_wx.c
index df0323f00f69..8d6a109e023b 100644
--- a/drivers/staging/rtl8192e/rtl8192e/rtl_wx.c
+++ b/drivers/staging/rtl8192e/rtl8192e/rtl_wx.c
@@ -67,7 +67,7 @@ static int r8192_wx_set_rate(struct net_device *dev,
 	int ret;
 	struct r8192_priv *priv = rtllib_priv(dev);
 
-	if (priv->bHwRadioOff == true)
+	if (priv->bHwRadioOff)
 		return 0;
 
 	down(&priv->wx_sem);
@@ -87,7 +87,7 @@ static int r8192_wx_set_rts(struct net_device *dev,
 	int ret;
 	struct r8192_priv *priv = rtllib_priv(dev);
 
-	if (priv->bHwRadioOff == true)
+	if (priv->bHwRadioOff)
 		return 0;
 
 	down(&priv->wx_sem);
@@ -115,9 +115,10 @@ static int r8192_wx_set_power(struct net_device *dev,
 	int ret;
 	struct r8192_priv *priv = rtllib_priv(dev);
 
-	if (priv->bHwRadioOff == true) {
-		RT_TRACE(COMP_ERR, "%s():Hw is Radio Off, we can't set "
-			 "Power,return\n", __func__);
+	if (priv->bHwRadioOff) {
+		RT_TRACE(COMP_ERR,
+			 "%s():Hw is Radio Off, we can't set Power,return\n",
+			 __func__);
 		return 0;
 	}
 	down(&priv->wx_sem);
@@ -145,7 +146,7 @@ static int r8192_wx_set_rawtx(struct net_device *dev,
 	struct r8192_priv *priv = rtllib_priv(dev);
 	int ret;
 
-	if (priv->bHwRadioOff == true)
+	if (priv->bHwRadioOff)
 		return 0;
 
 	down(&priv->wx_sem);
@@ -254,10 +255,11 @@ static int r8192se_wx_set_radio(struct net_device *dev,
 
 	down(&priv->wx_sem);
 
-	printk(KERN_INFO "%s(): set radio ! extra is %d\n", __func__, *extra);
+	netdev_info(dev, "%s(): set radio ! extra is %d\n", __func__, *extra);
 	if ((*extra != 0) && (*extra != 1)) {
-		RT_TRACE(COMP_ERR, "%s(): set radio an err value,must 0(radio "
-			 "off) or 1(radio on)\n", __func__);
+		RT_TRACE(COMP_ERR,
+			 "%s(): set radio an err value,must 0(radio off) or 1(radio on)\n",
+			 __func__);
 		up(&priv->wx_sem);
 		return -1;
 	}
@@ -277,8 +279,8 @@ static int r8192se_wx_set_lps_awake_interval(struct net_device *dev,
 
 	down(&priv->wx_sem);
 
-	printk(KERN_INFO "%s(): set lps awake interval ! extra is %d\n",
-	       __func__, *extra);
+	netdev_info(dev, "%s(): set lps awake interval ! extra is %d\n",
+		    __func__, *extra);
 
 	pPSC->RegMaxLPSAwakeIntvl = *extra;
 	up(&priv->wx_sem);
@@ -293,8 +295,9 @@ static int r8192se_wx_set_force_lps(struct net_device *dev,
 
 	down(&priv->wx_sem);
 
-	printk(KERN_INFO "%s(): force LPS ! extra is %d (1 is open 0 is "
-	       "close)\n", __func__, *extra);
+	netdev_info(dev,
+		    "%s(): force LPS ! extra is %d (1 is open 0 is close)\n",
+		    __func__, *extra);
 	priv->force_lps = *extra;
 	up(&priv->wx_sem);
 	return 0;
@@ -308,11 +311,11 @@ static int r8192_wx_set_debugflag(struct net_device *dev,
 	struct r8192_priv *priv = rtllib_priv(dev);
 	u8 c = *extra;
 
-	if (priv->bHwRadioOff == true)
+	if (priv->bHwRadioOff)
 		return 0;
 
-	printk(KERN_INFO "=====>%s(), *extra:%x, debugflag:%x\n", __func__,
-	       *extra, rt_global_debug_component);
+	netdev_info(dev, "=====>%s(), *extra:%x, debugflag:%x\n", __func__,
+		    *extra, rt_global_debug_component);
 	if (c > 0)
 		rt_global_debug_component |= (1<<c);
 	else
@@ -329,7 +332,7 @@ static int r8192_wx_set_mode(struct net_device *dev, struct iw_request_info *a,
 	enum rt_rf_power_state rtState;
 	int ret;
 
-	if (priv->bHwRadioOff == true)
+	if (priv->bHwRadioOff)
 		return 0;
 	rtState = priv->rtllib->eRFPowerState;
 	down(&priv->wx_sem);
@@ -343,13 +346,12 @@ static int r8192_wx_set_mode(struct net_device *dev, struct iw_request_info *a,
 						 __func__);
 					up(&priv->wx_sem);
 					return -1;
-				} else {
-					printk(KERN_INFO "=========>%s(): "
-					       "IPSLeave\n", __func__);
-					down(&priv->rtllib->ips_sem);
-					IPSLeave(dev);
-					up(&priv->rtllib->ips_sem);
 				}
+				netdev_info(dev,  "=========>%s(): IPSLeave\n",
+					    __func__);
+				down(&priv->rtllib->ips_sem);
+				IPSLeave(dev);
+				up(&priv->rtllib->ips_sem);
 			}
 		}
 	}
@@ -470,9 +472,9 @@ static int r8192_wx_set_scan(struct net_device *dev, struct iw_request_info *a,
 			return 0;
 	}
 
-	if (priv->bHwRadioOff == true) {
-		printk(KERN_INFO "================>%s(): hwradio off\n",
-		       __func__);
+	if (priv->bHwRadioOff) {
+		netdev_info(dev, "================>%s(): hwradio off\n",
+			    __func__);
 		return 0;
 	}
 	rtState = priv->rtllib->eRFPowerState;
@@ -500,17 +502,17 @@ static int r8192_wx_set_scan(struct net_device *dev, struct iw_request_info *a,
 			if (rtState == eRfOff) {
 				if (priv->rtllib->RfOffReason >
 				    RF_CHANGE_BY_IPS) {
-					RT_TRACE(COMP_ERR, "%s(): RF is "
-						 "OFF.\n", __func__);
+					RT_TRACE(COMP_ERR,
+						 "%s(): RF is OFF.\n",
+						 __func__);
 					up(&priv->wx_sem);
 					return -1;
-				} else {
-					RT_TRACE(COMP_PS, "=========>%s(): "
-						 "IPSLeave\n", __func__);
-					down(&priv->rtllib->ips_sem);
-					IPSLeave(dev);
-					up(&priv->rtllib->ips_sem);
 				}
+				RT_TRACE(COMP_PS, "=========>%s(): IPSLeave\n",
+					 __func__);
+				down(&priv->rtllib->ips_sem);
+				IPSLeave(dev);
+				up(&priv->rtllib->ips_sem);
 			}
 		}
 		rtllib_stop_scan(priv->rtllib);
@@ -552,7 +554,7 @@ static int r8192_wx_get_scan(struct net_device *dev, struct iw_request_info *a,
 	if (!priv->up)
 		return -ENETDOWN;
 
-	if (priv->bHwRadioOff == true)
+	if (priv->bHwRadioOff)
 		return 0;
 
 
@@ -572,13 +574,10 @@ static int r8192_wx_set_essid(struct net_device *dev,
 	struct r8192_priv *priv = rtllib_priv(dev);
 	int ret;
 
-	if ((rtllib_act_scanning(priv->rtllib, false)) &&
-	    !(priv->rtllib->softmac_features & IEEE_SOFTMAC_SCAN)) {
-		;	/* TODO - get rid of if */
-	}
-	if (priv->bHwRadioOff == true) {
-		printk(KERN_INFO "=========>%s():hw radio off,or Rf state is "
-		       "eRfOff, return\n", __func__);
+	if (priv->bHwRadioOff) {
+		netdev_info(dev,
+			    "=========>%s():hw radio off,or Rf state is eRfOff, return\n",
+			    __func__);
 		return 0;
 	}
 	down(&priv->wx_sem);
@@ -642,7 +641,7 @@ static int r8192_wx_set_freq(struct net_device *dev, struct iw_request_info *a,
 	int ret;
 	struct r8192_priv *priv = rtllib_priv(dev);
 
-	if (priv->bHwRadioOff == true)
+	if (priv->bHwRadioOff)
 		return 0;
 
 	down(&priv->wx_sem);
@@ -669,7 +668,7 @@ static int r8192_wx_set_frag(struct net_device *dev,
 {
 	struct r8192_priv *priv = rtllib_priv(dev);
 
-	if (priv->bHwRadioOff == true)
+	if (priv->bHwRadioOff)
 		return 0;
 
 	if (wrqu->frag.disabled)
@@ -708,12 +707,7 @@ static int r8192_wx_set_wap(struct net_device *dev,
 	int ret;
 	struct r8192_priv *priv = rtllib_priv(dev);
 
-	if ((rtllib_act_scanning(priv->rtllib, false)) &&
-	    !(priv->rtllib->softmac_features & IEEE_SOFTMAC_SCAN)) {
-		;	/* TODO - get rid of if */
-	}
-
-	if (priv->bHwRadioOff == true)
+	if (priv->bHwRadioOff)
 		return 0;
 
 	down(&priv->wx_sem);
@@ -763,10 +757,7 @@ static int r8192_wx_set_enc(struct net_device *dev,
 			     {0x00, 0x00, 0x00, 0x00, 0x00, 0x03} };
 	int i;
 
-	if ((rtllib_act_scanning(priv->rtllib, false)) &&
-	   !(priv->rtllib->softmac_features & IEEE_SOFTMAC_SCAN))
-		;	/* TODO - get rid of if */
-	if (priv->bHwRadioOff == true)
+	if (priv->bHwRadioOff)
 		return 0;
 
 	if (!priv->up)
@@ -838,7 +829,8 @@ static int r8192_wx_set_enc(struct net_device *dev,
 			set_swcam(dev, key_idx, key_idx, KEY_TYPE_WEP104,
 				  zero_addr[key_idx], 0, hwkey, 0);
 		} else {
-			 printk(KERN_INFO "wrong type in WEP, not WEP40 and WEP104\n");
+			netdev_info(dev,
+				    "wrong type in WEP, not WEP40 and WEP104\n");
 		}
 	}
 
@@ -855,7 +847,7 @@ static int r8192_wx_set_scan_type(struct net_device *dev,
 	int *parms = (int *)p;
 	int mode = parms[0];
 
-	if (priv->bHwRadioOff == true)
+	if (priv->bHwRadioOff)
 		return 0;
 
 	priv->rtllib->active_scan = mode;
@@ -873,7 +865,7 @@ static int r8192_wx_set_retry(struct net_device *dev,
 	struct r8192_priv *priv = rtllib_priv(dev);
 	int err = 0;
 
-	if (priv->bHwRadioOff == true)
+	if (priv->bHwRadioOff)
 		return 0;
 
 	down(&priv->wx_sem);
@@ -956,7 +948,7 @@ static int r8192_wx_set_sens(struct net_device *dev,
 
 	short err = 0;
 
-	if (priv->bHwRadioOff == true)
+	if (priv->bHwRadioOff)
 		return 0;
 
 	down(&priv->wx_sem);
@@ -983,7 +975,7 @@ static int r8192_wx_set_enc_ext(struct net_device *dev,
 	struct r8192_priv *priv = rtllib_priv(dev);
 	struct rtllib_device *ieee = priv->rtllib;
 
-	if (priv->bHwRadioOff == true)
+	if (priv->bHwRadioOff)
 		return 0;
 
 	down(&priv->wx_sem);
@@ -1064,7 +1056,7 @@ static int r8192_wx_set_auth(struct net_device *dev,
 
 	struct r8192_priv *priv = rtllib_priv(dev);
 
-	if (priv->bHwRadioOff == true)
+	if (priv->bHwRadioOff)
 		return 0;
 
 	down(&priv->wx_sem);
@@ -1082,7 +1074,7 @@ static int r8192_wx_set_mlme(struct net_device *dev,
 
 	struct r8192_priv *priv = rtllib_priv(dev);
 
-	if (priv->bHwRadioOff == true)
+	if (priv->bHwRadioOff)
 		return 0;
 
 	down(&priv->wx_sem);
@@ -1099,7 +1091,7 @@ static int r8192_wx_set_gen_ie(struct net_device *dev,
 
 	struct r8192_priv *priv = rtllib_priv(dev);
 
-	if (priv->bHwRadioOff == true)
+	if (priv->bHwRadioOff)
 		return 0;
 
 	down(&priv->wx_sem);
@@ -1160,8 +1152,10 @@ static int r8192_wx_set_PromiscuousMode(struct net_device *dev,
 			(rtllib_EnableIntelPromiscuousMode(dev, false)) :
 			(rtllib_DisableIntelPromiscuousMode(dev, false));
 
-		printk(KERN_INFO "=======>%s(), on = %d, filter src sta = %d\n",
-		       __func__, bPromiscuousOn, bFilterSourceStationFrame);
+		netdev_info(dev,
+			    "=======>%s(), on = %d, filter src sta = %d\n",
+			    __func__, bPromiscuousOn,
+			    bFilterSourceStationFrame);
 	} else {
 		return -1;
 	}
@@ -1227,11 +1221,10 @@ static iw_handler r8192_wx_handlers[] = {
 	IW_IOCTL(SIOCSIWENCODEEXT) = r8192_wx_set_enc_ext,
 };
 
-/*
- * the following rule need to be following,
+/* the following rule need to be following,
  * Odd : get (world access),
  * even : set (root access)
- * */
+ */
 static const struct iw_priv_args r8192_private_args[] = {
 	{
 		SIOCIWFIRSTPRIV + 0x0,
diff --git a/drivers/staging/rtl8192e/rtl819x_BAProc.c b/drivers/staging/rtl8192e/rtl819x_BAProc.c
index 0415e02b4eff..26258ea8de4a 100644
--- a/drivers/staging/rtl8192e/rtl819x_BAProc.c
+++ b/drivers/staging/rtl8192e/rtl819x_BAProc.c
@@ -26,7 +26,7 @@ static void ActivateBAEntry(struct rtllib_device *ieee, struct ba_record *pBA,
 {
 	pBA->bValid = true;
 	if (Time != 0)
-		mod_timer(&pBA->Timer, jiffies + MSECS(Time));
+		mod_timer(&pBA->Timer, jiffies + msecs_to_jiffies(Time));
 }
 
 static void DeActivateBAEntry(struct rtllib_device *ieee, struct ba_record *pBA)
@@ -83,9 +83,9 @@ static struct sk_buff *rtllib_ADDBA(struct rtllib_device *ieee, u8 *Dst,
 	u8 *tag = NULL;
 	u16 len = ieee->tx_headroom + 9;
 
-	RTLLIB_DEBUG(RTLLIB_DL_TRACE | RTLLIB_DL_BA, "========>%s(), frame(%d)"
-		     " sentd to: %pM, ieee->dev:%p\n", __func__,
-		     type, Dst, ieee->dev);
+	RTLLIB_DEBUG(RTLLIB_DL_TRACE | RTLLIB_DL_BA,
+		     "========>%s(), frame(%d) sentd to: %pM, ieee->dev:%p\n",
+		     __func__, type, Dst, ieee->dev);
 	if (pBA == NULL) {
 		RTLLIB_DEBUG(RTLLIB_DL_ERR, "pBA is NULL\n");
 		return NULL;
@@ -148,9 +148,8 @@ static struct sk_buff *rtllib_DELBA(struct rtllib_device *ieee, u8 *dst,
 
 	if (net_ratelimit())
 		RTLLIB_DEBUG(RTLLIB_DL_TRACE | RTLLIB_DL_BA,
-			     "========>%s(), Reason"
-			     "Code(%d) sentd to: %pM\n", __func__,
-			     ReasonCode, dst);
+			     "========>%s(), ReasonCode(%d) sentd to: %pM\n",
+			     __func__, ReasonCode, dst);
 
 	memset(&DelbaParamSet, 0, 2);
 
@@ -203,8 +202,8 @@ static void rtllib_send_ADDBAReq(struct rtllib_device *ieee, u8 *dst,
 		RT_TRACE(COMP_DBG, "====>to send ADDBAREQ!!!!!\n");
 		softmac_mgmt_xmit(skb, ieee);
 	} else {
-		RTLLIB_DEBUG(RTLLIB_DL_ERR, "alloc skb error in function"
-			     " %s()\n", __func__);
+		RTLLIB_DEBUG(RTLLIB_DL_ERR,
+			     "alloc skb error in function %s()\n", __func__);
 	}
 }
 
@@ -217,8 +216,8 @@ static void rtllib_send_ADDBARsp(struct rtllib_device *ieee, u8 *dst,
 	if (skb)
 		softmac_mgmt_xmit(skb, ieee);
 	else
-		RTLLIB_DEBUG(RTLLIB_DL_ERR, "alloc skb error in function"
-			     " %s()\n", __func__);
+		RTLLIB_DEBUG(RTLLIB_DL_ERR,
+			     "alloc skb error in function %s()\n", __func__);
 }
 
 static void rtllib_send_DELBA(struct rtllib_device *ieee, u8 *dst,
@@ -231,8 +230,8 @@ static void rtllib_send_DELBA(struct rtllib_device *ieee, u8 *dst,
 	if (skb)
 		softmac_mgmt_xmit(skb, ieee);
 	else
-		RTLLIB_DEBUG(RTLLIB_DL_ERR, "alloc skb error in function"
-			     " %s()\n", __func__);
+		RTLLIB_DEBUG(RTLLIB_DL_ERR,
+			     "alloc skb error in function %s()\n", __func__);
 }
 
 int rtllib_rx_ADDBAReq(struct rtllib_device *ieee, struct sk_buff *skb)
@@ -247,8 +246,9 @@ int rtllib_rx_ADDBAReq(struct rtllib_device *ieee, struct sk_buff *skb)
 	struct rx_ts_record *pTS = NULL;
 
 	if (skb->len < sizeof(struct rtllib_hdr_3addr) + 9) {
-		RTLLIB_DEBUG(RTLLIB_DL_ERR, " Invalid skb len in BAREQ(%d / "
-			     "%d)\n", (int)skb->len,
+		RTLLIB_DEBUG(RTLLIB_DL_ERR,
+			     " Invalid skb len in BAREQ(%d / %d)\n",
+			     (int)skb->len,
 			     (int)(sizeof(struct rtllib_hdr_3addr) + 9));
 		return -1;
 	}
@@ -269,8 +269,8 @@ int rtllib_rx_ADDBAReq(struct rtllib_device *ieee, struct sk_buff *skb)
 	    (ieee->pHTInfo->bCurrentHTSupport == false) ||
 	    (ieee->pHTInfo->IOTAction & HT_IOT_ACT_REJECT_ADDBA_REQ)) {
 		rc = ADDBA_STATUS_REFUSED;
-		RTLLIB_DEBUG(RTLLIB_DL_ERR, "Failed to reply on ADDBA_REQ as "
-			     "some capability is not ready(%d, %d)\n",
+		RTLLIB_DEBUG(RTLLIB_DL_ERR,
+			     "Failed to reply on ADDBA_REQ as some capability is not ready(%d, %d)\n",
 			     ieee->current_network.qos_data.active,
 			     ieee->pHTInfo->bCurrentHTSupport);
 		goto OnADDBAReq_Fail;
@@ -285,8 +285,8 @@ int rtllib_rx_ADDBAReq(struct rtllib_device *ieee, struct sk_buff *skb)
 
 	if (pBaParamSet->field.BAPolicy == BA_POLICY_DELAYED) {
 		rc = ADDBA_STATUS_INVALID_PARAM;
-		RTLLIB_DEBUG(RTLLIB_DL_ERR, "BA Policy is not correct in "
-			     "%s()\n", __func__);
+		RTLLIB_DEBUG(RTLLIB_DL_ERR,
+			     "BA Policy is not correct in %s()\n", __func__);
 		goto OnADDBAReq_Fail;
 	}
 
@@ -333,8 +333,9 @@ int rtllib_rx_ADDBARsp(struct rtllib_device *ieee, struct sk_buff *skb)
 	u16			ReasonCode;
 
 	if (skb->len < sizeof(struct rtllib_hdr_3addr) + 9) {
-		RTLLIB_DEBUG(RTLLIB_DL_ERR, " Invalid skb len in BARSP(%d / "
-			     "%d)\n", (int)skb->len,
+		RTLLIB_DEBUG(RTLLIB_DL_ERR,
+			     "Invalid skb len in BARSP(%d / %d)\n",
+			     (int)skb->len,
 			     (int)(sizeof(struct rtllib_hdr_3addr) + 9));
 		return -1;
 	}
@@ -351,8 +352,8 @@ int rtllib_rx_ADDBARsp(struct rtllib_device *ieee, struct sk_buff *skb)
 	if (ieee->current_network.qos_data.active == 0  ||
 	    ieee->pHTInfo->bCurrentHTSupport == false ||
 	    ieee->pHTInfo->bCurrentAMPDUEnable == false) {
-		RTLLIB_DEBUG(RTLLIB_DL_ERR, "reject to ADDBA_RSP as some capab"
-			     "ility is not ready(%d, %d, %d)\n",
+		RTLLIB_DEBUG(RTLLIB_DL_ERR,
+			     "reject to ADDBA_RSP as some capability is not ready(%d, %d, %d)\n",
 			     ieee->current_network.qos_data.active,
 			     ieee->pHTInfo->bCurrentHTSupport,
 			     ieee->pHTInfo->bCurrentAMPDUEnable);
@@ -374,18 +375,19 @@ int rtllib_rx_ADDBARsp(struct rtllib_device *ieee, struct sk_buff *skb)
 
 
 	if (pAdmittedBA->bValid == true) {
-		RTLLIB_DEBUG(RTLLIB_DL_BA, "OnADDBARsp(): Recv ADDBA Rsp."
-			     " Drop because already admit it!\n");
+		RTLLIB_DEBUG(RTLLIB_DL_BA,
+			     "OnADDBARsp(): Recv ADDBA Rsp. Drop because already admit it!\n");
 		return -1;
 	} else if ((pPendingBA->bValid == false) ||
 		   (*pDialogToken != pPendingBA->DialogToken)) {
-		RTLLIB_DEBUG(RTLLIB_DL_ERR,  "OnADDBARsp(): Recv ADDBA Rsp. "
-			     "BA invalid, DELBA!\n");
+		RTLLIB_DEBUG(RTLLIB_DL_ERR,
+			     "OnADDBARsp(): Recv ADDBA Rsp. BA invalid, DELBA!\n");
 		ReasonCode = DELBA_REASON_UNKNOWN_BA;
 		goto OnADDBARsp_Reject;
 	} else {
-		RTLLIB_DEBUG(RTLLIB_DL_BA, "OnADDBARsp(): Recv ADDBA Rsp. BA "
-			     "is admitted! Status code:%X\n", *pStatusCode);
+		RTLLIB_DEBUG(RTLLIB_DL_BA,
+			     "OnADDBARsp(): Recv ADDBA Rsp. BA is admitted! Status code:%X\n",
+			     *pStatusCode);
 		DeActivateBAEntry(ieee, pPendingBA);
 	}
 
@@ -432,16 +434,17 @@ int rtllib_rx_DELBA(struct rtllib_device *ieee, struct sk_buff *skb)
 	u8 *dst = NULL;
 
 	if (skb->len < sizeof(struct rtllib_hdr_3addr) + 6) {
-		RTLLIB_DEBUG(RTLLIB_DL_ERR, " Invalid skb len in DELBA(%d /"
-			     " %d)\n", (int)skb->len,
+		RTLLIB_DEBUG(RTLLIB_DL_ERR,
+			     "Invalid skb len in DELBA(%d / %d)\n",
+			     (int)skb->len,
 			     (int)(sizeof(struct rtllib_hdr_3addr) + 6));
 		return -1;
 	}
 
 	if (ieee->current_network.qos_data.active == 0  ||
 		ieee->pHTInfo->bCurrentHTSupport == false) {
-		RTLLIB_DEBUG(RTLLIB_DL_ERR, "received DELBA while QOS or HT "
-			     "is not supported(%d, %d)\n",
+		RTLLIB_DEBUG(RTLLIB_DL_ERR,
+			     "received DELBA while QOS or HT is not supported(%d, %d)\n",
 			     ieee->current_network. qos_data.active,
 			     ieee->pHTInfo->bCurrentHTSupport);
 		return -1;
@@ -459,8 +462,9 @@ int rtllib_rx_DELBA(struct rtllib_device *ieee, struct sk_buff *skb)
 
 		if (!GetTs(ieee, (struct ts_common_info **)&pRxTs, dst,
 		    (u8)pDelBaParamSet->field.TID, RX_DIR, false)) {
-			RTLLIB_DEBUG(RTLLIB_DL_ERR,  "can't get TS for RXTS in "
-				     "%s().dst: %pM TID:%d\n", __func__, dst,
+			RTLLIB_DEBUG(RTLLIB_DL_ERR,
+				     "can't get TS for RXTS in %s().dst: %pM TID:%d\n",
+				     __func__, dst,
 				     (u8)pDelBaParamSet->field.TID);
 			return -1;
 		}
@@ -471,8 +475,9 @@ int rtllib_rx_DELBA(struct rtllib_device *ieee, struct sk_buff *skb)
 
 		if (!GetTs(ieee, (struct ts_common_info **)&pTxTs, dst,
 			   (u8)pDelBaParamSet->field.TID, TX_DIR, false)) {
-			RTLLIB_DEBUG(RTLLIB_DL_ERR,  "can't get TS for TXTS in "
-				     "%s()\n", __func__);
+			RTLLIB_DEBUG(RTLLIB_DL_ERR,
+				     "can't get TS for TXTS in %s()\n",
+				     __func__);
 			return -1;
 		}
 
diff --git a/drivers/staging/rtl8192e/rtl819x_HT.h b/drivers/staging/rtl8192e/rtl819x_HT.h
index 13f41057461a..f7076d7ddc53 100644
--- a/drivers/staging/rtl8192e/rtl819x_HT.h
+++ b/drivers/staging/rtl8192e/rtl819x_HT.h
@@ -77,43 +77,6 @@ enum chnl_op {
 	CHNLOP_SWCHNL = 3,
 };
 
-#define CHHLOP_IN_PROGRESS(_pHTInfo)	\
-		((_pHTInfo)->ChnlOp > CHNLOP_NONE) ? true : false
-
-/*
-union ht_capability {
-	u16	ShortData;
-	u8	CharData[2];
-	struct
-	{
-		u16	AdvCoding:1;
-		u16	ChlWidth:1;
-		u16	MimoPwrSave:2;
-		u16	GreenField:1;
-		u16	ShortGI20Mhz:1;
-		u16	ShortGI40Mhz:1;
-		u16	STBC:1;
-		u16	BeamForm:1;
-		u16	DelayBA:1;
-		u16	MaxAMSDUSize:1;
-		u16	DssCCk:1;
-		u16	PSMP:1;
-		u16	Rsvd:3;
-	}Field;
-};
-
-union ht_capability_macpara {
-	u8	ShortData;
-	u8	CharData[1];
-	struct
-	{
-		u8	MaxRxAMPDU:2;
-		u8	MPDUDensity:2;
-		u8	Rsvd:4;
-	}Field;
-};
-*/
-
 enum ht_action {
 	ACT_RECOMMAND_WIDTH		= 0,
 	ACT_MIMO_PWR_SAVE		= 1,
@@ -380,16 +343,6 @@ struct false_alarm_stats {
 extern u8 MCS_FILTER_ALL[16];
 extern u8 MCS_FILTER_1SS[16];
 
-#define PICK_RATE(_nLegacyRate, _nMcsRate)	\
-		((_nMcsRate == 0) ? (_nLegacyRate&0x7f) : (_nMcsRate))
-#define	LEGACY_WIRELESS_MODE	IEEE_MODE_MASK
-
-#define CURRENT_RATE(WirelessMode, LegacyRate, HTRate)	\
-			((WirelessMode & (LEGACY_WIRELESS_MODE)) != 0) ? \
-			(LegacyRate) : (PICK_RATE(LegacyRate, HTRate))
-
-
-
 #define	RATE_ADPT_1SS_MASK		0xFF
 #define	RATE_ADPT_2SS_MASK		0xF0
 #define	RATE_ADPT_MCS32_MASK		0x01
diff --git a/drivers/staging/rtl8192e/rtl819x_HTProc.c b/drivers/staging/rtl8192e/rtl819x_HTProc.c
index 1ea426b7b7ac..7f103114d5d2 100644
--- a/drivers/staging/rtl8192e/rtl819x_HTProc.c
+++ b/drivers/staging/rtl8192e/rtl819x_HTProc.c
@@ -117,160 +117,6 @@ void HTUpdateDefaultSetting(struct rtllib_device *ieee)
 	pHTInfo->RxReorderPendingTime = 30;
 }
 
-void HTDebugHTCapability(u8 *CapIE, u8 *TitleString)
-{
-
-	static u8	EWC11NHTCap[] = {0x00, 0x90, 0x4c, 0x33};
-	struct ht_capab_ele *pCapELE;
-
-	if (!memcmp(CapIE, EWC11NHTCap, sizeof(EWC11NHTCap))) {
-		RTLLIB_DEBUG(RTLLIB_DL_HT, "EWC IE in %s()\n", __func__);
-		pCapELE = (struct ht_capab_ele *)(&CapIE[4]);
-	} else
-		pCapELE = (struct ht_capab_ele *)(&CapIE[0]);
-
-	RTLLIB_DEBUG(RTLLIB_DL_HT, "<Log HT Capability>. Called by %s\n",
-		     TitleString);
-
-	RTLLIB_DEBUG(RTLLIB_DL_HT,  "\tSupported Channel Width = %s\n",
-		     (pCapELE->ChlWidth) ? "20MHz" : "20/40MHz");
-	RTLLIB_DEBUG(RTLLIB_DL_HT,  "\tSupport Short GI for 20M = %s\n",
-		     (pCapELE->ShortGI20Mhz) ? "YES" : "NO");
-	RTLLIB_DEBUG(RTLLIB_DL_HT,  "\tSupport Short GI for 40M = %s\n",
-		     (pCapELE->ShortGI40Mhz) ? "YES" : "NO");
-	RTLLIB_DEBUG(RTLLIB_DL_HT,  "\tSupport TX STBC = %s\n",
-		     (pCapELE->TxSTBC) ? "YES" : "NO");
-	RTLLIB_DEBUG(RTLLIB_DL_HT,  "\tMax AMSDU Size = %s\n",
-		     (pCapELE->MaxAMSDUSize) ? "3839" : "7935");
-	RTLLIB_DEBUG(RTLLIB_DL_HT,  "\tSupport CCK in 20/40 mode = %s\n",
-		     (pCapELE->DssCCk) ? "YES" : "NO");
-	RTLLIB_DEBUG(RTLLIB_DL_HT,  "\tMax AMPDU Factor = %d\n",
-		     pCapELE->MaxRxAMPDUFactor);
-	RTLLIB_DEBUG(RTLLIB_DL_HT,  "\tMPDU Density = %d\n",
-		     pCapELE->MPDUDensity);
-	RTLLIB_DEBUG(RTLLIB_DL_HT,  "\tMCS Rate Set = [%x][%x][%x][%x][%x]\n",
-		     pCapELE->MCS[0], pCapELE->MCS[1], pCapELE->MCS[2],
-		     pCapELE->MCS[3], pCapELE->MCS[4]);
-	return;
-
-}
-
-void HTDebugHTInfo(u8 *InfoIE, u8 *TitleString)
-{
-
-	static u8	EWC11NHTInfo[] = {0x00, 0x90, 0x4c, 0x34};
-	struct ht_info_ele *pHTInfoEle;
-
-	if (!memcmp(InfoIE, EWC11NHTInfo, sizeof(EWC11NHTInfo))) {
-		RTLLIB_DEBUG(RTLLIB_DL_HT, "EWC IE in %s()\n", __func__);
-		pHTInfoEle = (struct ht_info_ele *)(&InfoIE[4]);
-	} else
-		pHTInfoEle = (struct ht_info_ele *)(&InfoIE[0]);
-
-
-	RTLLIB_DEBUG(RTLLIB_DL_HT, "<Log HT Information Element>. "
-		     "Called by %s\n", TitleString);
-
-	RTLLIB_DEBUG(RTLLIB_DL_HT, "\tPrimary channel = %d\n",
-		     pHTInfoEle->ControlChl);
-	RTLLIB_DEBUG(RTLLIB_DL_HT, "\tSenondary channel =");
-	switch (pHTInfoEle->ExtChlOffset) {
-	case 0:
-		RTLLIB_DEBUG(RTLLIB_DL_HT, "Not Present\n");
-		break;
-	case 1:
-		RTLLIB_DEBUG(RTLLIB_DL_HT, "Upper channel\n");
-		break;
-	case 2:
-		RTLLIB_DEBUG(RTLLIB_DL_HT, "Reserved. Eooro!!!\n");
-		break;
-	case 3:
-		RTLLIB_DEBUG(RTLLIB_DL_HT, "Lower Channel\n");
-		break;
-	}
-	RTLLIB_DEBUG(RTLLIB_DL_HT, "\tRecommended channel width = %s\n",
-		     (pHTInfoEle->RecommemdedTxWidth) ? "20Mhz" : "40Mhz");
-
-	RTLLIB_DEBUG(RTLLIB_DL_HT, "\tOperation mode for protection = ");
-	switch (pHTInfoEle->OptMode) {
-	case 0:
-		RTLLIB_DEBUG(RTLLIB_DL_HT, "No Protection\n");
-		break;
-	case 1:
-		RTLLIB_DEBUG(RTLLIB_DL_HT, "HT non-member protection mode\n");
-		break;
-	case 2:
-		RTLLIB_DEBUG(RTLLIB_DL_HT, "Suggest to open protection\n");
-		break;
-	case 3:
-		RTLLIB_DEBUG(RTLLIB_DL_HT, "HT mixed mode\n");
-		break;
-	}
-
-	RTLLIB_DEBUG(RTLLIB_DL_HT, "\tBasic MCS Rate Set = [%x][%x][%x][%x]"
-		     "[%x]\n", pHTInfoEle->BasicMSC[0], pHTInfoEle->BasicMSC[1],
-		     pHTInfoEle->BasicMSC[2], pHTInfoEle->BasicMSC[3],
-		     pHTInfoEle->BasicMSC[4]);
-}
-
-static bool IsHTHalfNmode40Bandwidth(struct rtllib_device *ieee)
-{
-	bool			retValue = false;
-	struct rt_hi_throughput *pHTInfo = ieee->pHTInfo;
-
-	if (pHTInfo->bCurrentHTSupport == false)
-		retValue = false;
-	else if (pHTInfo->bRegBW40MHz == false)
-		retValue = false;
-	else if (!ieee->GetHalfNmodeSupportByAPsHandler(ieee->dev))
-		retValue = false;
-	else if (((struct ht_capab_ele *)(pHTInfo->PeerHTCapBuf))->ChlWidth)
-		retValue = true;
-	else
-		retValue = false;
-
-	return retValue;
-}
-
-static bool IsHTHalfNmodeSGI(struct rtllib_device *ieee, bool is40MHz)
-{
-	bool			retValue = false;
-	struct rt_hi_throughput *pHTInfo = ieee->pHTInfo;
-
-	if (pHTInfo->bCurrentHTSupport == false)
-		retValue = false;
-	else if (!ieee->GetHalfNmodeSupportByAPsHandler(ieee->dev))
-		retValue = false;
-	else if (is40MHz) {
-		if (((struct ht_capab_ele *)
-		    (pHTInfo->PeerHTCapBuf))->ShortGI40Mhz)
-			retValue = true;
-		else
-			retValue = false;
-	} else {
-		if (((struct ht_capab_ele *)
-		   (pHTInfo->PeerHTCapBuf))->ShortGI20Mhz)
-			retValue = true;
-		else
-			retValue = false;
-	}
-
-	return retValue;
-}
-
-u16 HTHalfMcsToDataRate(struct rtllib_device *ieee, u8 nMcsRate)
-{
-
-	u8	is40MHz;
-	u8	isShortGI;
-
-	is40MHz  =  (IsHTHalfNmode40Bandwidth(ieee)) ? 1 : 0;
-	isShortGI = (IsHTHalfNmodeSGI(ieee, is40MHz)) ? 1 : 0;
-
-	return MCS_DATA_RATE[is40MHz][isShortGI][(nMcsRate & 0x7f)];
-}
-
-
 u16 HTMcsToDataRate(struct rtllib_device *ieee, u8 nMcsRate)
 {
 	struct rt_hi_throughput *pHTInfo = ieee->pHTInfo;
@@ -289,25 +135,22 @@ u16  TxCountToDataRate(struct rtllib_device *ieee, u8 nDataRate)
 	u8	is40MHz = 0;
 	u8	isShortGI = 0;
 
-	if (nDataRate < 12) {
+	if (nDataRate < 12)
 		return CCKOFDMRate[nDataRate];
-	} else {
-		if (nDataRate >= 0x10 && nDataRate <= 0x1f) {
-			is40MHz = 0;
-			isShortGI = 0;
-		} else if (nDataRate >= 0x20  && nDataRate <= 0x2f) {
-			is40MHz = 1;
-			isShortGI = 0;
-
-		} else if (nDataRate >= 0x30  && nDataRate <= 0x3f) {
-			is40MHz = 0;
-			isShortGI = 1;
-		} else if (nDataRate >= 0x40  && nDataRate <= 0x4f) {
-			is40MHz = 1;
-			isShortGI = 1;
-		}
-		return MCS_DATA_RATE[is40MHz][isShortGI][nDataRate&0xf];
+	if (nDataRate >= 0x10 && nDataRate <= 0x1f) {
+		is40MHz = 0;
+		isShortGI = 0;
+	} else if (nDataRate >= 0x20  && nDataRate <= 0x2f) {
+		is40MHz = 1;
+		isShortGI = 0;
+	} else if (nDataRate >= 0x30  && nDataRate <= 0x3f) {
+		is40MHz = 0;
+		isShortGI = 1;
+	} else if (nDataRate >= 0x40  && nDataRate <= 0x4f) {
+		is40MHz = 1;
+		isShortGI = 1;
 	}
+	return MCS_DATA_RATE[is40MHz][isShortGI][nDataRate&0xf];
 }
 
 bool IsHTHalfNmodeAPs(struct rtllib_device *ieee)
@@ -386,9 +229,7 @@ static u8 HTIOTActIsDisableMCS14(struct rtllib_device *ieee, u8 *PeerMacAddr)
 
 static bool HTIOTActIsDisableMCS15(struct rtllib_device *ieee)
 {
-	bool retValue = false;
-
-	return retValue;
+	return false;
 }
 
 static bool HTIOTActIsDisableMCSTwoSpatialStream(struct rtllib_device *ieee)
@@ -450,8 +291,8 @@ void HTConstructCapabilityElement(struct rtllib_device *ieee, u8 *posHTCap,
 	struct ht_capab_ele *pCapELE = NULL;
 
 	if ((posHTCap == NULL) || (pHT == NULL)) {
-		RTLLIB_DEBUG(RTLLIB_DL_ERR, "posHTCap or pHTInfo can't be "
-			     "null in HTConstructCapabilityElement()\n");
+		RTLLIB_DEBUG(RTLLIB_DL_ERR,
+			     "posHTCap or pHTInfo can't be null in HTConstructCapabilityElement()\n");
 		return;
 	}
 	memset(posHTCap, 0, *len);
@@ -487,9 +328,9 @@ void HTConstructCapabilityElement(struct rtllib_device *ieee, u8 *posHTCap,
 	pCapELE->LSigTxopProtect = 0;
 
 
-	RTLLIB_DEBUG(RTLLIB_DL_HT, "TX HT cap/info ele BW=%d MaxAMSDUSize:%d "
-		     "DssCCk:%d\n", pCapELE->ChlWidth, pCapELE->MaxAMSDUSize,
-		     pCapELE->DssCCk);
+	RTLLIB_DEBUG(RTLLIB_DL_HT,
+		     "TX HT cap/info ele BW=%d MaxAMSDUSize:%d DssCCk:%d\n",
+		     pCapELE->ChlWidth, pCapELE->MaxAMSDUSize, pCapELE->DssCCk);
 
 	if (IsEncrypt) {
 		pCapELE->MPDUDensity	= 7;
@@ -532,8 +373,8 @@ void HTConstructInfoElement(struct rtllib_device *ieee, u8 *posHTInfo,
 	struct ht_info_ele *pHTInfoEle = (struct ht_info_ele *)posHTInfo;
 
 	if ((posHTInfo == NULL) || (pHTInfoEle == NULL)) {
-		RTLLIB_DEBUG(RTLLIB_DL_ERR, "posHTInfo or pHTInfoEle can't be "
-			     "null in HTConstructInfoElement()\n");
+		RTLLIB_DEBUG(RTLLIB_DL_ERR,
+			     "posHTInfo or pHTInfoEle can't be null in HTConstructInfoElement()\n");
 		return;
 	}
 
@@ -572,8 +413,8 @@ void HTConstructRT2RTAggElement(struct rtllib_device *ieee, u8 *posRT2RTAgg,
 				u8 *len)
 {
 	if (posRT2RTAgg == NULL) {
-		RTLLIB_DEBUG(RTLLIB_DL_ERR, "posRT2RTAgg can't be null in "
-			     "HTConstructRT2RTAggElement()\n");
+		RTLLIB_DEBUG(RTLLIB_DL_ERR,
+			     "posRT2RTAgg can't be null in HTConstructRT2RTAggElement()\n");
 		return;
 	}
 	memset(posRT2RTAgg, 0, *len);
@@ -596,8 +437,8 @@ static u8 HT_PickMCSRate(struct rtllib_device *ieee, u8 *pOperateMCS)
 	u8 i;
 
 	if (pOperateMCS == NULL) {
-		RTLLIB_DEBUG(RTLLIB_DL_ERR, "pOperateMCS can't be null"
-			     " in HT_PickMCSRate()\n");
+		RTLLIB_DEBUG(RTLLIB_DL_ERR,
+			     "pOperateMCS can't be null in HT_PickMCSRate()\n");
 		return false;
 	}
 
@@ -631,8 +472,8 @@ u8 HTGetHighestMCSRate(struct rtllib_device *ieee, u8 *pMCSRateSet,
 	u8		availableMcsRate[16];
 
 	if (pMCSRateSet == NULL || pMCSFilter == NULL) {
-		RTLLIB_DEBUG(RTLLIB_DL_ERR, "pMCSRateSet or pMCSFilter can't "
-			     "be null in HTGetHighestMCSRate()\n");
+		RTLLIB_DEBUG(RTLLIB_DL_ERR,
+			     "pMCSRateSet or pMCSFilter can't be null in HTGetHighestMCSRate()\n");
 		return false;
 	}
 	for (i = 0; i < 16; i++)
@@ -654,7 +495,7 @@ u8 HTGetHighestMCSRate(struct rtllib_device *ieee, u8 *pMCSRateSet,
 					    HTMcsToDataRate(ieee, mcsRate))
 						mcsRate = (8*i+j);
 				}
-				bitMap = bitMap>>1;
+				bitMap >>= 1;
 			}
 		}
 	}
@@ -697,8 +538,8 @@ void HTOnAssocRsp(struct rtllib_device *ieee)
 	static u8 EWC11NHTInfo[] = {0x00, 0x90, 0x4c, 0x34};
 
 	if (pHTInfo->bCurrentHTSupport == false) {
-		RTLLIB_DEBUG(RTLLIB_DL_ERR, "<=== HTOnAssocRsp(): "
-			     "HT_DISABLE\n");
+		RTLLIB_DEBUG(RTLLIB_DL_ERR,
+			     "<=== HTOnAssocRsp(): HT_DISABLE\n");
 		return;
 	}
 	RTLLIB_DEBUG(RTLLIB_DL_HT, "===> HTOnAssocRsp_wq(): HT_ENABLE\n");
@@ -878,7 +719,8 @@ void HTResetSelfAndSavePeerSetting(struct rtllib_device *ieee,
 
 	RTLLIB_DEBUG(RTLLIB_DL_HT, "==============>%s()\n", __func__);
 	/* unmark bEnableHT flag here is the same reason why unmarked in
-	 * function rtllib_softmac_new_net. WB 2008.09.10*/
+	 * function rtllib_softmac_new_net. WB 2008.09.10
+	 */
 	if (pNetwork->bssht.bdSupportHT) {
 		pHTInfo->bCurrentHTSupport = true;
 		pHTInfo->ePeerHTSpecVer = pNetwork->bssht.bdHTSpecVer;
@@ -999,8 +841,8 @@ u8 HTCCheck(struct rtllib_device *ieee, u8 *pFrame)
 {
 	if (ieee->pHTInfo->bCurrentHTSupport) {
 		if ((IsQoSDataFrame(pFrame) && Frame_Order(pFrame)) == 1) {
-			RTLLIB_DEBUG(RTLLIB_DL_HT, "HT CONTROL FILED "
-				     "EXIST!!\n");
+			RTLLIB_DEBUG(RTLLIB_DL_HT,
+				     "HT CONTROL FILED EXIST!!\n");
 			return true;
 		}
 	}
diff --git a/drivers/staging/rtl8192e/rtl819x_Qos.h b/drivers/staging/rtl8192e/rtl819x_Qos.h
index 973342b8a06d..55ef7ec33f65 100644
--- a/drivers/staging/rtl8192e/rtl819x_Qos.h
+++ b/drivers/staging/rtl8192e/rtl819x_Qos.h
@@ -19,8 +19,6 @@
 #ifndef __INC_QOS_TYPE_H
 #define __INC_QOS_TYPE_H
 
-#include "rtllib_endianfree.h"
-
 #define BIT0		    0x00000001
 #define BIT1		    0x00000002
 #define BIT2		    0x00000004
@@ -340,18 +338,6 @@ struct sta_qos {
 };
 
 #define QBSS_LOAD_SIZE				5
-#define GET_QBSS_LOAD_STA_COUNT(__pStart)	\
-		ReadEF2Byte(__pStart)
-#define SET_QBSS_LOAD_STA_COUNT(__pStart, __Value)	\
-		WriteEF2Byte(__pStart, __Value)
-#define GET_QBSS_LOAD_CHNL_UTILIZATION(__pStart)	\
-		ReadEF1Byte((u8 *)(__pStart) + 2)
-#define SET_QBSS_LOAD_CHNL_UTILIZATION(__pStart, __Value)	\
-		WriteEF1Byte((u8 *)(__pStart) + 2, __Value)
-#define GET_QBSS_LOAD_AVAILABLE_CAPACITY(__pStart)	\
-		ReadEF2Byte((u8 *)(__pStart) + 3)
-#define SET_QBSS_LOAD_AVAILABLE_CAPACITY(__pStart, __Value) \
-		WriteEF2Byte((u8 *)(__pStart) + 3, __Value)
 
 struct bss_qos {
 	QOS_MODE bdQoSMode;
@@ -367,10 +353,6 @@ struct bss_qos {
 	bool bQBssLoadValid;
 };
 
-#define sQoSCtlLng	2
-#define QOS_CTRL_LEN(_QosMode)	((_QosMode > QOS_DISABLE) ? sQoSCtlLng : 0)
-
-
 #define IsACValid(ac)		((ac >= 0 && ac <= 7) ? true : false)
 
 
diff --git a/drivers/staging/rtl8192e/rtl819x_TSProc.c b/drivers/staging/rtl8192e/rtl819x_TSProc.c
index 294847d1a903..7d77d056228d 100644
--- a/drivers/staging/rtl8192e/rtl819x_TSProc.c
+++ b/drivers/staging/rtl8192e/rtl819x_TSProc.c
@@ -58,9 +58,9 @@ static void RxPktPendingTimeout(unsigned long data)
 					pRxTs->RxIndicateSeq =
 					      (pRxTs->RxIndicateSeq + 1) % 4096;
 
-				RTLLIB_DEBUG(RTLLIB_DL_REORDER, "%s(): Indicate"
-					     " SeqNum: %d\n", __func__,
-					     pReorderEntry->SeqNum);
+				RTLLIB_DEBUG(RTLLIB_DL_REORDER,
+					     "%s(): Indicate SeqNum: %d\n",
+					     __func__, pReorderEntry->SeqNum);
 				ieee->stats_IndicateArray[index] =
 							 pReorderEntry->prxb;
 				index++;
@@ -78,8 +78,8 @@ static void RxPktPendingTimeout(unsigned long data)
 		pRxTs->RxTimeoutIndicateSeq = 0xffff;
 
 		if (index > REORDER_WIN_SIZE) {
-			RTLLIB_DEBUG(RTLLIB_DL_ERR, "RxReorderIndicatePacket():"
-				     " Rx Reorder struct buffer full!!\n");
+			RTLLIB_DEBUG(RTLLIB_DL_ERR,
+				     "RxReorderIndicatePacket(): Rx Reorder struct buffer full!!\n");
 			spin_unlock_irqrestore(&(ieee->reorder_spinlock),
 					       flags);
 			return;
@@ -91,7 +91,7 @@ static void RxPktPendingTimeout(unsigned long data)
 	if (bPktInBuf && (pRxTs->RxTimeoutIndicateSeq == 0xffff)) {
 		pRxTs->RxTimeoutIndicateSeq = pRxTs->RxIndicateSeq;
 		mod_timer(&pRxTs->RxPktPendingTimer,  jiffies +
-			  MSECS(ieee->pHTInfo->RxReorderPendingTime));
+			  msecs_to_jiffies(ieee->pHTInfo->RxReorderPendingTime));
 	}
 	spin_unlock_irqrestore(&(ieee->reorder_spinlock), flags);
 }
@@ -104,8 +104,8 @@ static void TsAddBaProcess(unsigned long data)
 				     TxTsRecord[num]);
 
 	TsInitAddBA(ieee, pTxTs, BA_POLICY_IMMEDIATE, false);
-	RTLLIB_DEBUG(RTLLIB_DL_BA, "TsAddBaProcess(): ADDBA Req is "
-		     "started!!\n");
+	RTLLIB_DEBUG(RTLLIB_DL_BA,
+		     "TsAddBaProcess(): ADDBA Req is started!!\n");
 }
 
 static void ResetTsCommonInfo(struct ts_common_info *pTsCommonInfo)
@@ -223,7 +223,7 @@ static void AdmitTS(struct rtllib_device *ieee,
 
 	if (InactTime != 0)
 		mod_timer(&pTsCommonInfo->InactTimer, jiffies +
-			  MSECS(InactTime));
+			  msecs_to_jiffies(InactTime));
 }
 
 static struct ts_common_info *SearchAdmitTRStream(struct rtllib_device *ieee,
@@ -311,18 +311,24 @@ bool GetTs(struct rtllib_device *ieee, struct ts_common_info **ppTS,
 	   u8 *Addr, u8 TID, enum tr_select TxRxSelect, bool bAddNewTs)
 {
 	u8	UP = 0;
+	union tspec_body TSpec;
+	union qos_tsinfo *pTSInfo = &TSpec.f.TSInfo;
+	struct list_head *pUnusedList;
+	struct list_head *pAddmitList;
+	enum direction_value Dir;
 
 	if (is_multicast_ether_addr(Addr)) {
-		RTLLIB_DEBUG(RTLLIB_DL_ERR, "ERR! get TS for Broadcast or "
-			     "Multicast\n");
+		RTLLIB_DEBUG(RTLLIB_DL_ERR,
+			     "ERR! get TS for Broadcast or Multicast\n");
 		return false;
 	}
 	if (ieee->current_network.qos_data.supported == 0) {
 		UP = 0;
 	} else {
 		if (!IsACValid(TID)) {
-			RTLLIB_DEBUG(RTLLIB_DL_ERR, "ERR! in %s(), TID(%d) is "
-				     "not valid\n", __func__, TID);
+			RTLLIB_DEBUG(RTLLIB_DL_ERR,
+				     "ERR! in %s(), TID(%d) is not valid\n",
+				     __func__, TID);
 			return false;
 		}
 
@@ -347,77 +353,70 @@ bool GetTs(struct rtllib_device *ieee, struct ts_common_info **ppTS,
 	}
 
 	*ppTS = SearchAdmitTRStream(ieee, Addr, UP, TxRxSelect);
-	if (*ppTS != NULL) {
+	if (*ppTS != NULL)
 		return true;
-	} else {
-		if (!bAddNewTs) {
-			RTLLIB_DEBUG(RTLLIB_DL_TS, "add new TS failed"
-				     "(tid:%d)\n", UP);
-			return false;
-		} else {
-			union tspec_body TSpec;
-			union qos_tsinfo *pTSInfo = &TSpec.f.TSInfo;
-			struct list_head *pUnusedList =
-				(TxRxSelect == TX_DIR) ?
+
+	if (!bAddNewTs) {
+		RTLLIB_DEBUG(RTLLIB_DL_TS,
+			     "add new TS failed(tid:%d)\n", UP);
+		return false;
+	}
+
+	pUnusedList = (TxRxSelect == TX_DIR) ?
 				(&ieee->Tx_TS_Unused_List) :
 				(&ieee->Rx_TS_Unused_List);
 
-			struct list_head *pAddmitList =
-				(TxRxSelect == TX_DIR) ?
+	pAddmitList = (TxRxSelect == TX_DIR) ?
 				(&ieee->Tx_TS_Admit_List) :
 				(&ieee->Rx_TS_Admit_List);
 
-			enum direction_value Dir =
-				 (ieee->iw_mode == IW_MODE_MASTER) ?
-				 ((TxRxSelect == TX_DIR) ? DIR_DOWN : DIR_UP) :
-				 ((TxRxSelect == TX_DIR) ? DIR_UP : DIR_DOWN);
-			RTLLIB_DEBUG(RTLLIB_DL_TS, "to add Ts\n");
-			if (!list_empty(pUnusedList)) {
-				(*ppTS) = list_entry(pUnusedList->next,
-					  struct ts_common_info, List);
-				list_del_init(&(*ppTS)->List);
-				if (TxRxSelect == TX_DIR) {
-					struct tx_ts_record *tmp =
-						container_of(*ppTS,
-						struct tx_ts_record,
-						TsCommonInfo);
-					ResetTxTsEntry(tmp);
-				} else {
-					struct rx_ts_record *tmp =
-						 container_of(*ppTS,
-						 struct rx_ts_record,
-						 TsCommonInfo);
-					ResetRxTsEntry(tmp);
-				}
-
-				RTLLIB_DEBUG(RTLLIB_DL_TS, "to init current TS"
-					     ", UP:%d, Dir:%d, addr: %pM"
-					     " ppTs=%p\n", UP, Dir,
-					      Addr, *ppTS);
-				pTSInfo->field.ucTrafficType = 0;
-				pTSInfo->field.ucTSID = UP;
-				pTSInfo->field.ucDirection = Dir;
-				pTSInfo->field.ucAccessPolicy = 1;
-				pTSInfo->field.ucAggregation = 0;
-				pTSInfo->field.ucPSB = 0;
-				pTSInfo->field.ucUP = UP;
-				pTSInfo->field.ucTSInfoAckPolicy = 0;
-				pTSInfo->field.ucSchedule = 0;
-
-				MakeTSEntry(*ppTS, Addr, &TSpec, NULL, 0, 0);
-				AdmitTS(ieee, *ppTS, 0);
-				list_add_tail(&((*ppTS)->List), pAddmitList);
-
-				return true;
-			} else {
-				RTLLIB_DEBUG(RTLLIB_DL_ERR, "ERR!!in function "
-					     "%s() There is not enough dir=%d"
-					     "(0=up down=1) TS record to be "
-					     "used!!", __func__, Dir);
-				return false;
-			}
+	Dir = (ieee->iw_mode == IW_MODE_MASTER) ?
+				((TxRxSelect == TX_DIR) ? DIR_DOWN : DIR_UP) :
+				((TxRxSelect == TX_DIR) ? DIR_UP : DIR_DOWN);
+
+	RTLLIB_DEBUG(RTLLIB_DL_TS, "to add Ts\n");
+	if (!list_empty(pUnusedList)) {
+		(*ppTS) = list_entry(pUnusedList->next,
+			  struct ts_common_info, List);
+		list_del_init(&(*ppTS)->List);
+		if (TxRxSelect == TX_DIR) {
+			struct tx_ts_record *tmp =
+				container_of(*ppTS,
+				struct tx_ts_record,
+				TsCommonInfo);
+			ResetTxTsEntry(tmp);
+		} else {
+			struct rx_ts_record *tmp =
+				 container_of(*ppTS,
+				 struct rx_ts_record,
+				 TsCommonInfo);
+			ResetRxTsEntry(tmp);
 		}
+
+		RTLLIB_DEBUG(RTLLIB_DL_TS,
+			     "to init current TS, UP:%d, Dir:%d, addr: %pM ppTs=%p\n",
+			     UP, Dir, Addr, *ppTS);
+		pTSInfo->field.ucTrafficType = 0;
+		pTSInfo->field.ucTSID = UP;
+		pTSInfo->field.ucDirection = Dir;
+		pTSInfo->field.ucAccessPolicy = 1;
+		pTSInfo->field.ucAggregation = 0;
+		pTSInfo->field.ucPSB = 0;
+		pTSInfo->field.ucUP = UP;
+		pTSInfo->field.ucTSInfoAckPolicy = 0;
+		pTSInfo->field.ucSchedule = 0;
+
+		MakeTSEntry(*ppTS, Addr, &TSpec, NULL, 0, 0);
+		AdmitTS(ieee, *ppTS, 0);
+		list_add_tail(&((*ppTS)->List), pAddmitList);
+
+		return true;
 	}
+
+	RTLLIB_DEBUG(RTLLIB_DL_ERR,
+		     "ERR!!in function %s() There is not enough dir=%d(0=up down=1) TS record to be used!!",
+		     __func__, Dir);
+	return false;
 }
 
 static void RemoveTsEntry(struct rtllib_device *ieee, struct ts_common_info *pTs,
@@ -438,8 +437,8 @@ static void RemoveTsEntry(struct rtllib_device *ieee, struct ts_common_info *pTs
 			pRxReorderEntry = (struct rx_reorder_entry *)
 					list_entry(pRxTS->RxPendingPktList.prev,
 					struct rx_reorder_entry, List);
-			RTLLIB_DEBUG(RTLLIB_DL_REORDER, "%s(): Delete SeqNum "
-				     "%d!\n", __func__,
+			RTLLIB_DEBUG(RTLLIB_DL_REORDER,
+				     "%s(): Delete SeqNum %d!\n", __func__,
 				     pRxReorderEntry->SeqNum);
 			list_del_init(&pRxReorderEntry->List);
 			{
@@ -467,7 +466,7 @@ void RemovePeerTS(struct rtllib_device *ieee, u8 *Addr)
 {
 	struct ts_common_info *pTS, *pTmpTS;
 
-	printk(KERN_INFO "===========>RemovePeerTS, %pM\n", Addr);
+	netdev_info(ieee->dev, "===========>RemovePeerTS, %pM\n", Addr);
 
 	list_for_each_entry_safe(pTS, pTmpTS, &ieee->Tx_TS_Pending_List, List) {
 		if (memcmp(pTS->Addr, Addr, 6) == 0) {
@@ -479,7 +478,8 @@ void RemovePeerTS(struct rtllib_device *ieee, u8 *Addr)
 
 	list_for_each_entry_safe(pTS, pTmpTS, &ieee->Tx_TS_Admit_List, List) {
 		if (memcmp(pTS->Addr, Addr, 6) == 0) {
-			printk(KERN_INFO "====>remove Tx_TS_admin_list\n");
+			netdev_info(ieee->dev,
+				    "====>remove Tx_TS_admin_list\n");
 			RemoveTsEntry(ieee, pTS, TX_DIR);
 			list_del_init(&pTS->List);
 			list_add_tail(&pTS->List, &ieee->Tx_TS_Unused_List);
@@ -539,13 +539,13 @@ void TsStartAddBaProcess(struct rtllib_device *ieee, struct tx_ts_record *pTxTS)
 		pTxTS->bAddBaReqInProgress = true;
 
 		if (pTxTS->bAddBaReqDelayed) {
-			RTLLIB_DEBUG(RTLLIB_DL_BA, "TsStartAddBaProcess(): "
-				     "Delayed Start ADDBA after 60 sec!!\n");
+			RTLLIB_DEBUG(RTLLIB_DL_BA,
+				     "TsStartAddBaProcess(): Delayed Start ADDBA after 60 sec!!\n");
 			mod_timer(&pTxTS->TsAddBaTimer, jiffies +
-				  MSECS(TS_ADDBA_DELAY));
+				  msecs_to_jiffies(TS_ADDBA_DELAY));
 		} else {
-			RTLLIB_DEBUG(RTLLIB_DL_BA, "TsStartAddBaProcess(): "
-				     "Immediately Start ADDBA now!!\n");
+			RTLLIB_DEBUG(RTLLIB_DL_BA,
+				     "TsStartAddBaProcess(): Immediately Start ADDBA now!!\n");
 			mod_timer(&pTxTS->TsAddBaTimer, jiffies+10);
 		}
 	} else
diff --git a/drivers/staging/rtl8192e/rtllib.h b/drivers/staging/rtl8192e/rtllib.h
index cef2dc27103f..3c8b708df5c3 100644
--- a/drivers/staging/rtl8192e/rtllib.h
+++ b/drivers/staging/rtl8192e/rtllib.h
@@ -66,25 +66,8 @@
 #define IW_CUSTOM_MAX	256	/* In bytes */
 #endif
 
-#ifndef container_of
-/**
- * container_of - cast a member of a structure out to the containing structure
- *
- * @ptr:	the pointer to the member.
- * @type:       the type of the container struct this is embedded in.
- * @member:     the name of the member within the struct.
- *
- */
-#define container_of(ptr, type, member) ({		      \
-	const typeof(((type *)0)->member)*__mptr = (ptr);    \
-	(type *)((char *)__mptr - offsetof(type, member)); })
-#endif
-
 #define skb_tail_pointer_rsl(skb) skb_tail_pointer(skb)
 
-#define EXPORT_SYMBOL_RSL(x) EXPORT_SYMBOL(x)
-
-
 #define queue_delayed_work_rsl(x, y, z) queue_delayed_work(x, y, z)
 #define INIT_DELAYED_WORK_RSL(x, y, z) INIT_DELAYED_WORK(x, y)
 
@@ -482,9 +465,6 @@ enum rt_op_mode {
 #define	IEEE_CRYPT_ALG_NAME_LEN			16
 
 #define MAX_IE_LEN  0xff
-#define RT_ASSERT_RET(_Exp) do {} while (0)
-#define RT_ASSERT_RET_VALUE(_Exp, Ret)		\
-	do {} while (0)
 
 struct ieee_param {
 	u32 cmd;
@@ -525,17 +505,17 @@ struct ieee_param {
 #define IW_QUAL_NOISE_UPDATED  0x4
 #endif
 
-#define MSECS(t) msecs_to_jiffies(t)
 #define msleep_interruptible_rsl  msleep_interruptible
 
 #define RTLLIB_DATA_LEN		2304
 /* Maximum size for the MA-UNITDATA primitive, 802.11 standard section
-   6.2.1.1.2.
-
-   The figure in section 7.1.2 suggests a body size of up to 2312
-   bytes is allowed, which is a bit confusing, I suspect this
-   represents the 2304 bytes of real data, plus a possible 8 bytes of
-   WEP IV and ICV. (this interpretation suggested by Ramiro Barreiro) */
+ * 6.2.1.1.2.
+ *
+ * The figure in section 7.1.2 suggests a body size of up to 2312
+ * bytes is allowed, which is a bit confusing, I suspect this
+ * represents the 2304 bytes of real data, plus a possible 8 bytes of
+ * WEP IV and ICV. (this interpretation suggested by Ramiro Barreiro)
+ */
 #define RTLLIB_1ADDR_LEN 10
 #define RTLLIB_2ADDR_LEN 16
 #define RTLLIB_3ADDR_LEN 24
@@ -711,20 +691,13 @@ do {								\
 #define RTLLIB_DEBUG_DATA(level, data, datalen)	\
 	do {							\
 		if ((rtllib_debug_level & (level)) == (level)) {	\
-			int i;					\
-			u8 *pdata = (u8 *)data;			\
 			printk(KERN_DEBUG "rtllib: %s()\n", __func__);	\
-			for (i = 0; i < (int)(datalen); i++)	{	\
-				printk("%2.2x ", pdata[i]);		\
-				if ((i+1)%16 == 0)			\
-					printk("\n");	\
-			}				\
-			printk("\n");			\
+			print_hex_dump_bytes(KERN_DEBUG, DUMP_PREFIX_NONE, \
+					     data, datalen); \
 		}					\
 	} while (0)
 
-/*
- * To use the debug system;
+/* To use the debug system;
  *
  * If you are defining a new debug classification, simply add it to the #define
  * list here in the form of:
@@ -743,8 +716,6 @@ do {								\
  * % cat /proc/net/ipw/debug_level
  *
  * you simply need to add your entry to the ipw_debug_levels array.
- *
- *
  */
 
 #define RTLLIB_DL_INFO	  (1<<0)
@@ -769,8 +740,8 @@ do {								\
 #define RTLLIB_DL_TRACE	   (1<<29)
 #define RTLLIB_DL_DATA	   (1<<30)
 #define RTLLIB_DL_ERR	   (1<<31)
-#define RTLLIB_ERROR(f, a...) printk(KERN_ERR "rtllib: " f, ## a)
-#define RTLLIB_WARNING(f, a...) printk(KERN_WARNING "rtllib: " f, ## a)
+#define RTLLIB_ERROR(f, a...) pr_err("rtllib: " f, ## a)
+#define RTLLIB_WARNING(f, a...) pr_warn("rtllib: " f, ## a)
 #define RTLLIB_DEBUG_INFO(f, a...)   RTLLIB_DEBUG(RTLLIB_DL_INFO, f, ## a)
 
 #define RTLLIB_DEBUG_WX(f, a...)     RTLLIB_DEBUG(RTLLIB_DL_WX, f, ## a)
@@ -784,26 +755,6 @@ do {								\
 #define RTLLIB_DEBUG_RX(f, a...)  RTLLIB_DEBUG(RTLLIB_DL_RX, f, ## a)
 #define RTLLIB_DEBUG_QOS(f, a...)  RTLLIB_DEBUG(RTLLIB_DL_QOS, f, ## a)
 
-/* Added by Annie, 2005-11-22. */
-#define MAX_STR_LEN     64
-/* I want to see ASCII 33 to 126 only. Otherwise, I print '?'. */
-#define PRINTABLE(_ch)  (_ch > '!' && _ch < '~')
-#define RTLLIB_PRINT_STR(_Comp, _TitleString, _Ptr, _Len)		\
-	if ((_Comp) & level) {					       \
-		int	     __i;				    \
-		u8  struct buffer[MAX_STR_LEN];				\
-		int length = (_Len < MAX_STR_LEN) ? _Len : (MAX_STR_LEN-1) ;\
-		memset(struct buffer, 0, MAX_STR_LEN);		\
-		memcpy(struct buffer, (u8 *)_Ptr, length);		\
-		for (__i = 0; __i < MAX_STR_LEN; __i++) {		\
-			if (!PRINTABLE(struct buffer[__i]))		\
-				struct buffer[__i] = '?';		\
-		}							\
-		struct buffer[length] = '\0';				\
-		printk(KERN_INFO "Rtl819x: ");				\
-		printk(_TitleString);					\
-		printk(": %d, <%s>\n", _Len, struct buffer);		\
-	}
 #ifndef ETH_P_PAE
 #define ETH_P_PAE 0x888E /* Port Access Entity (IEEE 802.1X) */
 #define ETH_P_IP	0x0800		/* Internet Protocol packet	*/
@@ -1022,7 +973,8 @@ struct rtllib_rx_stats {
 /* IEEE 802.11 requires that STA supports concurrent reception of at least
  * three fragmented frames. This define can be increased to support more
  * concurrent frames, but it should be noted that each entry can consume about
- * 2 kB of RAM and increasing cache size will slow down frame reassembly. */
+ * 2 kB of RAM and increasing cache size will slow down frame reassembly.
+ */
 #define RTLLIB_FRAG_CACHE_LEN 4
 
 struct rtllib_frag_entry {
@@ -1100,16 +1052,15 @@ struct rtllib_security {
 } __packed;
 
 
-/*
- 802.11 data frame from AP
-      ,-------------------------------------------------------------------.
-Bytes |  2   |  2   |    6    |    6    |    6    |  2   | 0..2312 |   4  |
-      |------|------|---------|---------|---------|------|---------|------|
-Desc. | ctrl | dura |  DA/RA  |   TA    |    SA   | Sequ |  frame  |  fcs |
-      |      | tion | (BSSID) |	 |	 | ence |  data   |      |
-      `-------------------------------------------------------------------'
-Total: 28-2340 bytes
-*/
+/* 802.11 data frame from AP
+ *       ,-------------------------------------------------------------------.
+ * Bytes |  2   |  2   |    6    |    6    |    6    |  2   | 0..2312 |   4  |
+ *       |------|------|---------|---------|---------|------|---------|------|
+ * Desc. | ctrl | dura |  DA/RA  |   TA    |    SA   | Sequ |  frame  |  fcs |
+ *       |      | tion | (BSSID) |         |         | ence |  data   |      |
+ *       `-------------------------------------------------------------------'
+ * Total: 28-2340 bytes
+ */
 
 /* Management Frame Information Element Types */
 enum rtllib_mfie {
@@ -1147,7 +1098,8 @@ enum rtllib_mfie {
 
 /* Minimal header; can be used for passing 802.11 frames with sufficient
  * information to determine what type of underlying data type is actually
- * stored in the data. */
+ * stored in the data.
+ */
 struct rtllib_pspoll_hdr {
 	__le16 frame_ctl;
 	__le16 aid;
@@ -1257,7 +1209,8 @@ struct rtllib_probe_response {
 	__le16 beacon_interval;
 	__le16 capability;
 	/* SSID, supported rates, FH params, DS params,
-	 * CF params, IBSS params, TIM (if beacon), RSN */
+	 * CF params, IBSS params, TIM (if beacon), RSN
+	 */
 	struct rtllib_info_element info_element[0];
 } __packed;
 
@@ -1332,7 +1285,8 @@ union frameqos {
 /* MAX_RATES_LENGTH needs to be 12.  The spec says 8, and many APs
  * only use 8, and then use extended rates for the remaining supported
  * rates.  Other APs, however, stick all of their supported rates on the
- * main rates information element... */
+ * main rates information element...
+ */
 #define MAX_RATES_LENGTH		  ((u8)12)
 #define MAX_RATES_EX_LENGTH	       ((u8)16)
 #define MAX_NETWORK_COUNT		  96
@@ -1519,22 +1473,21 @@ struct rtllib_info_element_hdr {
 	u8 len;
 } __packed;
 
-/*
- * These are the data types that can make up management packets
+/* These are the data types that can make up management packets
  *
-	u16 auth_algorithm;
-	u16 auth_sequence;
-	u16 beacon_interval;
-	u16 capability;
-	u8 current_ap[ETH_ALEN];
-	u16 listen_interval;
-	struct {
-		u16 association_id:14, reserved:2;
-	} __packed;
-	u32 time_stamp[2];
-	u16 reason;
-	u16 status;
-*/
+ * u16 auth_algorithm;
+ * u16 auth_sequence;
+ * u16 beacon_interval;
+ * u16 capability;
+ * u8 current_ap[ETH_ALEN];
+ * u16 listen_interval;
+ * struct {
+ *   u16 association_id:14, reserved:2;
+ * } __packed;
+ * u32 time_stamp[2];
+ * u16 reason;
+ * u16 status;
+ */
 
 #define RTLLIB_DEFAULT_TX_ESSID "Penguin"
 #define RTLLIB_DEFAULT_BASIC_RATE 2
@@ -1670,7 +1623,6 @@ struct rtllib_network {
 	struct list_head list;
 };
 
-#if 1
 enum rtllib_state {
 
 	/* the card is not linked at all */
@@ -1708,17 +1660,6 @@ enum rtllib_state {
 	 */
 	RTLLIB_LINKED_SCANNING,
 };
-#else
-enum rtllib_state {
-	RTLLIB_UNINITIALIZED = 0,
-	RTLLIB_INITIALIZED,
-	RTLLIB_ASSOCIATING,
-	RTLLIB_ASSOCIATED,
-	RTLLIB_AUTHENTICATING,
-	RTLLIB_AUTHENTICATED,
-	RTLLIB_SHUTDOWN
-};
-#endif
 
 #define DEFAULT_MAX_SCAN_AGE (15 * HZ)
 #define DEFAULT_FTS 2346
@@ -1736,11 +1677,6 @@ enum rtllib_state {
 #define RTLLIB_52GHZ_MAX_CHANNEL 165
 #define RTLLIB_52GHZ_CHANNELS (RTLLIB_52GHZ_MAX_CHANNEL - \
 				  RTLLIB_52GHZ_MIN_CHANNEL + 1)
-#ifndef eqMacAddr
-#define eqMacAddr(a, b)					\
-	(((a)[0] == (b)[0] && (a)[1] == (b)[1] && (a)[2] == (b)[2] &&	\
-	(a)[3] == (b)[3] && (a)[4] == (b)[4] && (a)[5] == (b)[5]) ? 1 : 0)
-#endif
 struct tx_pending {
 	int frag;
 	struct rtllib_txb *txb;
@@ -2149,14 +2085,16 @@ struct rtllib_device {
 	spinlock_t wpax_suitlist_lock;
 
 	int tx_headroom; /* Set to size of any additional room needed at front
-			  * of allocated Tx SKBs */
+			  * of allocated Tx SKBs
+			  */
 	u32 config;
 
 	/* WEP and other encryption related settings at the device level */
 	int open_wep; /* Set to 1 to allow unencrypted frames */
 	int auth_mode;
 	int reset_on_keychange; /* Set to 1 if the HW needs to be reset on
-				 * WEP key changes */
+				 * WEP key changes
+				 */
 
 	/* If the host performs {en,de}cryption, then set to 1 */
 	int host_encrypt;
@@ -2276,7 +2214,7 @@ struct rtllib_device {
 	short raw_tx;
 	/* used if IEEE_SOFTMAC_TX_QUEUE is set */
 	short queue_stop;
-	short scanning_continue ;
+	short scanning_continue;
 	short proto_started;
 	short proto_stoppping;
 
@@ -2426,7 +2364,7 @@ struct rtllib_device {
 	/* OK this is complementing to data_poll_hard_stop */
 	void (*data_hard_resume)(struct net_device *dev);
 
-	/* ask to the driver to retune the radio .
+	/* ask to the driver to retune the radio.
 	 * This function can sleep. the driver should ensure
 	 * the radio has been switched before return.
 	 */
@@ -2520,7 +2458,8 @@ struct rtllib_device {
 	void (*rtllib_rfkill_poll)(struct net_device *dev);
 
 	/* This must be the last item so that it points to the data
-	 * allocated beyond this structure by alloc_rtllib */
+	 * allocated beyond this structure by alloc_rtllib
+	 */
 	u8 priv[0];
 };
 
@@ -2549,7 +2488,8 @@ struct rtllib_device {
 
 /* The ieee802.11 stack will manage the netif queue
  * wake/stop for the driver, taking care of 802.11
- * fragmentation. See softmac.c for details. */
+ * fragmentation. See softmac.c for details.
+ */
 #define IEEE_SOFTMAC_TX_QUEUE (1<<7)
 
 /* Uses only the softmac_data_hard_start_xmit
@@ -2586,11 +2526,9 @@ static inline int rtllib_is_empty_essid(const char *essid, int essid_len)
 
 static inline int rtllib_is_valid_mode(struct rtllib_device *ieee, int mode)
 {
-	/*
-	 * It is possible for both access points and our device to support
+	/* It is possible for both access points and our device to support
 	 * combinations of modes, so as long as there is one valid combination
 	 * of ap/device supported modes, then return success
-	 *
 	 */
 	if ((mode & IEEE_A) &&
 	    (ieee->modulation & RTLLIB_OFDM_MODULATION) &&
@@ -2882,8 +2820,6 @@ extern int rtllib_wx_get_rts(struct rtllib_device *ieee,
 			     struct iw_request_info *info,
 			     union iwreq_data *wrqu, char *extra);
 #define MAX_RECEIVE_BUFFER_SIZE 9100
-extern void HTDebugHTCapability(u8 *CapIE, u8 *TitleString);
-extern void HTDebugHTInfo(u8 *InfoIE, u8 *TitleString);
 
 void HTSetConnectBwMode(struct rtllib_device *ieee,
 			enum ht_channel_width Bandwidth,
@@ -2906,11 +2842,10 @@ extern void HT_update_self_and_peer_setting(struct rtllib_device *ieee,
 extern u8 HTGetHighestMCSRate(struct rtllib_device *ieee, u8 *pMCSRateSet,
 			      u8 *pMCSFilter);
 extern u8 MCS_FILTER_ALL[];
-extern u16 MCS_DATA_RATE[2][2][77] ;
+extern u16 MCS_DATA_RATE[2][2][77];
 extern u8 HTCCheck(struct rtllib_device *ieee, u8 *pFrame);
 extern void HTResetIOTSetting(struct rt_hi_throughput *pHTInfo);
 extern bool IsHTHalfNmodeAPs(struct rtllib_device *ieee);
-extern u16 HTHalfMcsToDataRate(struct rtllib_device *ieee, u8 nMcsRate);
 extern u16 HTMcsToDataRate(struct rtllib_device *ieee, u8 nMcsRate);
 extern u16  TxCountToDataRate(struct rtllib_device *ieee, u8 nDataRate);
 extern int rtllib_rx_ADDBAReq(struct rtllib_device *ieee, struct sk_buff *skb);
diff --git a/drivers/staging/rtl8192e/rtllib_crypt.c b/drivers/staging/rtl8192e/rtllib_crypt.c
index 60c0cedefb86..1e6ae9bead23 100644
--- a/drivers/staging/rtl8192e/rtllib_crypt.c
+++ b/drivers/staging/rtl8192e/rtllib_crypt.c
@@ -62,8 +62,9 @@ void rtllib_crypt_deinit_handler(unsigned long data)
 	spin_lock_irqsave(info->lock, flags);
 	rtllib_crypt_deinit_entries(info, 0);
 	if (!list_empty(&info->crypt_deinit_list)) {
-		printk(KERN_DEBUG "%s: entries remaining in delayed crypt "
-		       "deletion list\n", info->name);
+		printk(KERN_DEBUG
+		       "%s: entries remaining in delayed crypt deletion list\n",
+		       info->name);
 		info->crypt_deinit_timer.expires = jiffies + HZ;
 		add_timer(&info->crypt_deinit_timer);
 	}
@@ -86,7 +87,8 @@ void rtllib_crypt_delayed_deinit(struct lib80211_crypt_info *info,
 
 	/* must not run ops->deinit() while there may be pending encrypt or
 	 * decrypt operations. Use a list of delayed deinits to avoid needing
-	 * locking. */
+	 * locking.
+	 */
 
 	spin_lock_irqsave(info->lock, flags);
 	list_add(&tmp->list, &info->crypt_deinit_list);
@@ -145,8 +147,8 @@ int rtllib_unregister_crypto_ops(struct lib80211_crypto_ops *ops)
 	spin_unlock_irqrestore(&hcrypt->lock, flags);
 
 	if (del_alg) {
-		printk(KERN_DEBUG "rtllib_crypt: unregistered algorithm "
-		       "'%s'\n", ops->name);
+		printk(KERN_DEBUG "rtllib_crypt: unregistered algorithm '%s'\n",
+		       ops->name);
 		kfree(del_alg);
 	}
 
@@ -237,8 +239,9 @@ void __exit rtllib_crypto_deinit(void)
 		struct rtllib_crypto_alg *alg =
 			(struct rtllib_crypto_alg *) ptr;
 		list_del(ptr);
-		printk(KERN_DEBUG "rtllib_crypt: unregistered algorithm "
-		       "'%s' (deinit)\n", alg->ops->name);
+		printk(KERN_DEBUG
+		       "rtllib_crypt: unregistered algorithm '%s' (deinit)\n",
+		       alg->ops->name);
 		kfree(alg);
 	}
 
diff --git a/drivers/staging/rtl8192e/rtllib_crypt.h b/drivers/staging/rtl8192e/rtllib_crypt.h
index e177c9287b44..b8cf59f39a60 100644
--- a/drivers/staging/rtl8192e/rtllib_crypt.h
+++ b/drivers/staging/rtl8192e/rtllib_crypt.h
@@ -17,8 +17,7 @@
  * more details.
  */
 
-/*
- * This file defines the interface to the rtllib crypto module.
+/* This file defines the interface to the rtllib crypto module.
  */
 #ifndef RTLLIB_CRYPT_H
 #define RTLLIB_CRYPT_H
diff --git a/drivers/staging/rtl8192e/rtllib_crypt_ccmp.c b/drivers/staging/rtl8192e/rtllib_crypt_ccmp.c
index 01f85db98e0d..7d486e8887f8 100644
--- a/drivers/staging/rtl8192e/rtllib_crypt_ccmp.c
+++ b/drivers/staging/rtl8192e/rtllib_crypt_ccmp.c
@@ -69,8 +69,7 @@ static void *rtllib_ccmp_init(int key_idx)
 
 	priv->tfm = (void *)crypto_alloc_cipher("aes", 0, CRYPTO_ALG_ASYNC);
 	if (IS_ERR(priv->tfm)) {
-		pr_debug("rtllib_crypt_ccmp: could not allocate "
-		       "crypto API aes\n");
+		pr_debug("rtllib_crypt_ccmp: could not allocate crypto API aes\n");
 		priv->tfm = NULL;
 		goto fail;
 	}
@@ -121,10 +120,7 @@ static void ccmp_init_blocks(struct crypto_tfm *tfm,
 	fc = le16_to_cpu(hdr->frame_ctl);
 	a4_included = ((fc & (RTLLIB_FCTL_TODS | RTLLIB_FCTL_FROMDS)) ==
 		       (RTLLIB_FCTL_TODS | RTLLIB_FCTL_FROMDS));
-	/*
-	qc_included = ((WLAN_FC_GET_TYPE(fc) == RTLLIB_FTYPE_DATA) &&
-		       (WLAN_FC_GET_STYPE(fc) & 0x08));
-	*/
+
 	qc_included = ((WLAN_FC_GET_TYPE(fc) == RTLLIB_FTYPE_DATA) &&
 		       (WLAN_FC_GET_STYPE(fc) & 0x80));
 	aad_len = 22;
@@ -141,7 +137,8 @@ static void ccmp_init_blocks(struct crypto_tfm *tfm,
 	 * Flag (Include authentication header, M=3 (8-octet MIC),
 	 *       L=1 (2-octet Dlen))
 	 * Nonce: 0x00 | A2 | PN
-	 * Dlen */
+	 * Dlen
+	 */
 	b0[0] = 0x59;
 	b0[1] = qc;
 	memcpy(b0 + 2, hdr->addr2, ETH_ALEN);
@@ -278,23 +275,22 @@ static int rtllib_ccmp_decrypt(struct sk_buff *skb, int hdr_len, void *priv)
 	keyidx = pos[3];
 	if (!(keyidx & (1 << 5))) {
 		if (net_ratelimit()) {
-			pr_debug("CCMP: received packet without ExtIV"
-			       " flag from %pM\n", hdr->addr2);
+			pr_debug("CCMP: received packet without ExtIV flag from %pM\n",
+				 hdr->addr2);
 		}
 		key->dot11RSNAStatsCCMPFormatErrors++;
 		return -2;
 	}
 	keyidx >>= 6;
 	if (key->key_idx != keyidx) {
-		pr_debug("CCMP: RX tkey->key_idx=%d frame "
-		       "keyidx=%d priv=%p\n", key->key_idx, keyidx, priv);
+		pr_debug("CCMP: RX tkey->key_idx=%d frame keyidx=%d priv=%p\n",
+			 key->key_idx, keyidx, priv);
 		return -6;
 	}
 	if (!key->key_set) {
 		if (net_ratelimit()) {
-			pr_debug("CCMP: received packet from %pM"
-			       " with keyid=%d that does not have a configured"
-			       " key\n", hdr->addr2, keyidx);
+			pr_debug("CCMP: received packet from %pM with keyid=%d that does not have a configured key\n",
+				 hdr->addr2, keyidx);
 		}
 		return -3;
 	}
@@ -341,8 +337,8 @@ static int rtllib_ccmp_decrypt(struct sk_buff *skb, int hdr_len, void *priv)
 
 		if (memcmp(mic, a, CCMP_MIC_LEN) != 0) {
 			if (net_ratelimit()) {
-				pr_debug("CCMP: decrypt failed: STA="
-				" %pM\n", hdr->addr2);
+				pr_debug("CCMP: decrypt failed: STA= %pM\n",
+					 hdr->addr2);
 			}
 			key->dot11RSNAStatsCCMPDecryptErrors++;
 			return -5;
@@ -419,9 +415,7 @@ static void rtllib_ccmp_print_stats(struct seq_file *m, void *priv)
 	struct rtllib_ccmp_data *ccmp = priv;
 
 	seq_printf(m,
-		   "key[%d] alg=CCMP key_set=%d "
-		   "tx_pn=%pM rx_pn=%pM "
-		   "format_errors=%d replays=%d decrypt_errors=%d\n",
+		   "key[%d] alg=CCMP key_set=%d tx_pn=%pM rx_pn=%pM format_errors=%d replays=%d decrypt_errors=%d\n",
 		   ccmp->key_idx, ccmp->key_set,
 		   ccmp->tx_pn, ccmp->rx_pn,
 		   ccmp->dot11RSNAStatsCCMPFormatErrors,
diff --git a/drivers/staging/rtl8192e/rtllib_crypt_tkip.c b/drivers/staging/rtl8192e/rtllib_crypt_tkip.c
index 78db2b6826f7..656b4b359c50 100644
--- a/drivers/staging/rtl8192e/rtllib_crypt_tkip.c
+++ b/drivers/staging/rtl8192e/rtllib_crypt_tkip.c
@@ -66,8 +66,8 @@ static void *rtllib_tkip_init(int key_idx)
 	priv->tx_tfm_arc4 = crypto_alloc_blkcipher("ecb(arc4)", 0,
 			CRYPTO_ALG_ASYNC);
 	if (IS_ERR(priv->tx_tfm_arc4)) {
-		printk(KERN_DEBUG "rtllib_crypt_tkip: could not allocate "
-				"crypto API arc4\n");
+		printk(KERN_DEBUG
+		       "rtllib_crypt_tkip: could not allocate crypto API arc4\n");
 		priv->tx_tfm_arc4 = NULL;
 		goto fail;
 	}
@@ -75,8 +75,8 @@ static void *rtllib_tkip_init(int key_idx)
 	priv->tx_tfm_michael = crypto_alloc_hash("michael_mic", 0,
 			CRYPTO_ALG_ASYNC);
 	if (IS_ERR(priv->tx_tfm_michael)) {
-		printk(KERN_DEBUG "rtllib_crypt_tkip: could not allocate "
-				"crypto API michael_mic\n");
+		printk(KERN_DEBUG
+		       "rtllib_crypt_tkip: could not allocate crypto API michael_mic\n");
 		priv->tx_tfm_michael = NULL;
 		goto fail;
 	}
@@ -84,8 +84,8 @@ static void *rtllib_tkip_init(int key_idx)
 	priv->rx_tfm_arc4 = crypto_alloc_blkcipher("ecb(arc4)", 0,
 			CRYPTO_ALG_ASYNC);
 	if (IS_ERR(priv->rx_tfm_arc4)) {
-		printk(KERN_DEBUG "rtllib_crypt_tkip: could not allocate "
-				"crypto API arc4\n");
+		printk(KERN_DEBUG
+		       "rtllib_crypt_tkip: could not allocate crypto API arc4\n");
 		priv->rx_tfm_arc4 = NULL;
 		goto fail;
 	}
@@ -93,8 +93,8 @@ static void *rtllib_tkip_init(int key_idx)
 	priv->rx_tfm_michael = crypto_alloc_hash("michael_mic", 0,
 			CRYPTO_ALG_ASYNC);
 	if (IS_ERR(priv->rx_tfm_michael)) {
-		printk(KERN_DEBUG "rtllib_crypt_tkip: could not allocate "
-				"crypto API michael_mic\n");
+		printk(KERN_DEBUG
+		       "rtllib_crypt_tkip: could not allocate crypto API michael_mic\n");
 		priv->rx_tfm_michael = NULL;
 		goto fail;
 	}
@@ -249,7 +249,8 @@ static void tkip_mixing_phase2(u8 *WEPSeed, const u8 *TK, const u16 *TTAK,
 			       u16 IV16)
 {
 	/* Make temporary area overlap WEP seed so that the final copy can be
-	 * avoided on little endian hosts. */
+	 * avoided on little endian hosts.
+	 */
 	u16 *PPK = (u16 *) &WEPSeed[4];
 
 	/* Step 1 - make copy of TTAK and bring in TSC */
@@ -276,7 +277,8 @@ static void tkip_mixing_phase2(u8 *WEPSeed, const u8 *TK, const u16 *TTAK,
 	PPK[5] += RotR1(PPK[4]);
 
 	/* Step 3 - bring in last of TK bits, assign 24-bit WEP IV value
-	 * WEPSeed[0..2] is transmitted as WEP IV */
+	 * WEPSeed[0..2] is transmitted as WEP IV
+	 */
 	WEPSeed[0] = Hi8(IV16);
 	WEPSeed[1] = (Hi8(IV16) | 0x20) & 0x7F;
 	WEPSeed[2] = Lo8(IV16);
@@ -399,22 +401,24 @@ static int rtllib_tkip_decrypt(struct sk_buff *skb, int hdr_len, void *priv)
 	keyidx = pos[3];
 	if (!(keyidx & (1 << 5))) {
 		if (net_ratelimit()) {
-			printk(KERN_DEBUG "TKIP: received packet without ExtIV"
-			       " flag from %pM\n", hdr->addr2);
+			printk(KERN_DEBUG
+			       "TKIP: received packet without ExtIV flag from %pM\n",
+			       hdr->addr2);
 		}
 		return -2;
 	}
 	keyidx >>= 6;
 	if (tkey->key_idx != keyidx) {
-		printk(KERN_DEBUG "TKIP: RX tkey->key_idx=%d frame "
-		       "keyidx=%d priv=%p\n", tkey->key_idx, keyidx, priv);
+		printk(KERN_DEBUG
+		       "TKIP: RX tkey->key_idx=%d frame keyidx=%d priv=%p\n",
+		       tkey->key_idx, keyidx, priv);
 		return -6;
 	}
 	if (!tkey->key_set) {
 		if (net_ratelimit()) {
-			printk(KERN_DEBUG "TKIP: received packet from %pM"
-			       " with keyid=%d that does not have a configured"
-			       " key\n", hdr->addr2, keyidx);
+			printk(KERN_DEBUG
+			       "TKIP: received packet from %pM with keyid=%d that does not have a configured key\n",
+			       hdr->addr2, keyidx);
 		}
 		return -3;
 	}
@@ -427,10 +431,10 @@ static int rtllib_tkip_decrypt(struct sk_buff *skb, int hdr_len, void *priv)
 		    (iv32 == tkey->rx_iv32 && iv16 <= tkey->rx_iv16)) &&
 		    tkey->initialized) {
 			if (net_ratelimit()) {
-				printk(KERN_DEBUG "TKIP: replay detected: STA="
-				       " %pM previous TSC %08x%04x received "
-				      "TSC %08x%04x\n", hdr->addr2,
-				      tkey->rx_iv32, tkey->rx_iv16, iv32, iv16);
+				printk(KERN_DEBUG
+				       "TKIP: replay detected: STA= %pM previous TSC %08x%04x received TSC %08x%04x\n",
+				       hdr->addr2, tkey->rx_iv32, tkey->rx_iv16,
+				       iv32, iv16);
 			}
 			tkey->dot11RSNAStatsTKIPReplays++;
 			return -4;
@@ -451,8 +455,8 @@ static int rtllib_tkip_decrypt(struct sk_buff *skb, int hdr_len, void *priv)
 		crypto_blkcipher_setkey(tkey->rx_tfm_arc4, rc4key, 16);
 		if (crypto_blkcipher_decrypt(&desc, &sg, &sg, plen + 4)) {
 			if (net_ratelimit()) {
-				printk(KERN_DEBUG ": TKIP: failed to decrypt "
-				       "received packet from %pM\n",
+				printk(KERN_DEBUG
+				       ": TKIP: failed to decrypt received packet from %pM\n",
 				       hdr->addr2);
 			}
 			return -7;
@@ -468,12 +472,14 @@ static int rtllib_tkip_decrypt(struct sk_buff *skb, int hdr_len, void *priv)
 			if (iv32 != tkey->rx_iv32) {
 				/* Previously cached Phase1 result was already
 				 * lost, so it needs to be recalculated for the
-				 * next packet. */
+				 * next packet.
+				 */
 				tkey->rx_phase1_done = 0;
 			}
 			if (net_ratelimit()) {
-				printk(KERN_DEBUG "TKIP: ICV error detected: STA="
-				" %pM\n", hdr->addr2);
+				printk(KERN_DEBUG
+				       "TKIP: ICV error detected: STA= %pM\n",
+				       hdr->addr2);
 			}
 			tkey->dot11RSNAStatsTKIPICVErrors++;
 			return -5;
@@ -482,7 +488,8 @@ static int rtllib_tkip_decrypt(struct sk_buff *skb, int hdr_len, void *priv)
 	}
 
 	/* Update real counters only after Michael MIC verification has
-	 * completed */
+	 * completed
+	 */
 	tkey->rx_iv32_new = iv32;
 	tkey->rx_iv16_new = iv16;
 
@@ -502,7 +509,7 @@ static int michael_mic(struct crypto_hash *tfm_michael, u8 *key, u8 *hdr,
 	struct scatterlist sg[2];
 
 	if (tfm_michael == NULL) {
-		printk(KERN_WARNING "michael_mic: tfm_michael == NULL\n");
+		pr_warn("michael_mic: tfm_michael == NULL\n");
 		return -1;
 	}
 	sg_init_table(sg, 2);
@@ -557,8 +564,8 @@ static int rtllib_michael_mic_add(struct sk_buff *skb, int hdr_len, void *priv)
 	hdr = (struct rtllib_hdr_4addr *) skb->data;
 
 	if (skb_tailroom(skb) < 8 || skb->len < hdr_len) {
-		printk(KERN_DEBUG "Invalid packet for Michael MIC add "
-		       "(tailroom=%d hdr_len=%d skb->len=%d)\n",
+		printk(KERN_DEBUG
+		       "Invalid packet for Michael MIC add (tailroom=%d hdr_len=%d skb->len=%d)\n",
 		       skb_tailroom(skb), hdr_len, skb->len);
 		return -1;
 	}
@@ -621,14 +628,14 @@ static int rtllib_michael_mic_verify(struct sk_buff *skb, int keyidx,
 		struct rtllib_hdr_4addr *hdr;
 
 		hdr = (struct rtllib_hdr_4addr *) skb->data;
-		printk(KERN_DEBUG "%s: Michael MIC verification failed for "
-		       "MSDU from %pM keyidx=%d\n",
+		printk(KERN_DEBUG
+		       "%s: Michael MIC verification failed for MSDU from %pM keyidx=%d\n",
 		       skb->dev ? skb->dev->name : "N/A", hdr->addr2,
 		       keyidx);
 		printk(KERN_DEBUG "%d\n",
 		       memcmp(mic, skb->data + skb->len - 8, 8) != 0);
 		if (skb->dev) {
-			printk(KERN_INFO "skb->dev != NULL\n");
+			pr_info("skb->dev != NULL\n");
 			rtllib_michael_mic_failure(skb->dev, hdr, keyidx);
 		}
 		tkey->dot11RSNAStatsTKIPLocalMICFailures++;
@@ -636,7 +643,8 @@ static int rtllib_michael_mic_verify(struct sk_buff *skb, int keyidx,
 	}
 
 	/* Update TSC counters for RX now that the packet verification has
-	 * completed. */
+	 * completed.
+	 */
 	tkey->rx_iv32 = tkey->rx_iv32_new;
 	tkey->rx_iv16 = tkey->rx_iv16_new;
 
@@ -717,10 +725,7 @@ static void rtllib_tkip_print_stats(struct seq_file *m, void *priv)
 	struct rtllib_tkip_data *tkip = priv;
 
 	seq_printf(m,
-		   "key[%d] alg=TKIP key_set=%d "
-		   "tx_pn=%02x%02x%02x%02x%02x%02x "
-		   "rx_pn=%02x%02x%02x%02x%02x%02x "
-		   "replays=%d icv_errors=%d local_mic_failures=%d\n",
+		   "key[%d] alg=TKIP key_set=%d tx_pn=%02x%02x%02x%02x%02x%02x rx_pn=%02x%02x%02x%02x%02x%02x replays=%d icv_errors=%d local_mic_failures=%d\n",
 		   tkip->key_idx, tkip->key_set,
 		   (tkip->tx_iv32 >> 24) & 0xff,
 		   (tkip->tx_iv32 >> 16) & 0xff,
diff --git a/drivers/staging/rtl8192e/rtllib_crypt_wep.c b/drivers/staging/rtl8192e/rtllib_crypt_wep.c
index 75e7b02a144a..21d7eee4c9a9 100644
--- a/drivers/staging/rtl8192e/rtllib_crypt_wep.c
+++ b/drivers/staging/rtl8192e/rtllib_crypt_wep.c
@@ -44,15 +44,13 @@ static void *prism2_wep_init(int keyidx)
 
 	priv->tx_tfm = crypto_alloc_blkcipher("ecb(arc4)", 0, CRYPTO_ALG_ASYNC);
 	if (IS_ERR(priv->tx_tfm)) {
-		pr_debug("rtllib_crypt_wep: could not allocate "
-		       "crypto API arc4\n");
+		pr_debug("rtllib_crypt_wep: could not allocate crypto API arc4\n");
 		priv->tx_tfm = NULL;
 		goto fail;
 	}
 	priv->rx_tfm = crypto_alloc_blkcipher("ecb(arc4)", 0, CRYPTO_ALG_ASYNC);
 	if (IS_ERR(priv->rx_tfm)) {
-		pr_debug("rtllib_crypt_wep: could not allocate "
-		       "crypto API arc4\n");
+		pr_debug("rtllib_crypt_wep: could not allocate crypto API arc4\n");
 		priv->rx_tfm = NULL;
 		goto fail;
 	}
@@ -108,9 +106,8 @@ static int prism2_wep_encrypt(struct sk_buff *skb, int hdr_len, void *priv)
 
 	if (skb_headroom(skb) < 4 || skb_tailroom(skb) < 4 ||
 	    skb->len < hdr_len){
-		printk(KERN_ERR "Error!!! headroom=%d tailroom=%d skblen=%d"
-		       " hdr_len=%d\n", skb_headroom(skb), skb_tailroom(skb),
-		       skb->len, hdr_len);
+		pr_err("Error!!! headroom=%d tailroom=%d skblen=%d hdr_len=%d\n",
+		       skb_headroom(skb), skb_tailroom(skb), skb->len, hdr_len);
 		return -1;
 	}
 	len = skb->len - hdr_len;
@@ -124,7 +121,8 @@ static int prism2_wep_encrypt(struct sk_buff *skb, int hdr_len, void *priv)
 
 	/* Fluhrer, Mantin, and Shamir have reported weaknesses in the key
 	 * scheduling algorithm of RC4. At least IVs (KeyByte + 3, 0xff, N)
-	 * can be used to speedup attacks, so avoid using them. */
+	 * can be used to speedup attacks, so avoid using them.
+	 */
 	if ((wep->iv & 0xff00) == 0xff00) {
 		u8 B = (wep->iv >> 16) & 0xff;
 
diff --git a/drivers/staging/rtl8192e/rtllib_debug.h b/drivers/staging/rtl8192e/rtllib_debug.h
index 7537dae89a75..119729d31c74 100644
--- a/drivers/staging/rtl8192e/rtllib_debug.h
+++ b/drivers/staging/rtl8192e/rtllib_debug.h
@@ -80,9 +80,9 @@ do {			\
 #define assert(expr) \
 do {	\
 	if (!(expr)) {				  \
-		printk(KERN_INFO "Assertion failed! %s,%s,%s,line=%d\n", \
+		pr_info("Assertion failed! %s,%s,%s,line=%d\n", \
 		#expr, __FILE__, __func__, __LINE__);	  \
 	}	\
-} while (0);
+} while (0)
 
 #endif
diff --git a/drivers/staging/rtl8192e/rtllib_endianfree.h b/drivers/staging/rtl8192e/rtllib_endianfree.h
deleted file mode 100644
index b189fa5a45e4..000000000000
--- a/drivers/staging/rtl8192e/rtllib_endianfree.h
+++ /dev/null
@@ -1,122 +0,0 @@
-#ifndef __INC_ENDIANFREE_H
-#define __INC_ENDIANFREE_H
-
-/*
- *	Call endian free function when
- *		1. Read/write packet content.
- *		2. Before write integer to IO.
- *		3. After read integer from IO.
- */
-
-#define __MACHINE_LITTLE_ENDIAN 1234    /* LSB first: i386, vax */
-#define __MACHINE_BIG_ENDIAN    4321    /* MSB first: 68000, ibm, net, ppc */
-
-#define BYTE_ORDER __MACHINE_LITTLE_ENDIAN
-
-#if BYTE_ORDER == __MACHINE_LITTLE_ENDIAN
-#define EF1Byte(_val)			((u8)(_val))
-#define EF2Byte(_val)			((u16)(_val))
-#define EF4Byte(_val)			((u32)(_val))
-
-#else
-#define EF1Byte(_val)			((u8)(_val))
-#define EF2Byte(_val)			\
-	(((((u16)(_val))&0x00ff)<<8)|((((u16)(_val))&0xff00)>>8))
-#define EF4Byte(_val)			\
-	(((((u32)(_val))&0x000000ff)<<24)|\
-	((((u32)(_val))&0x0000ff00)<<8)|\
-	((((u32)(_val))&0x00ff0000)>>8)|\
-	((((u32)(_val))&0xff000000)>>24))
-#endif
-
-#define ReadEF1Byte(_ptr)		EF1Byte(*((u8 *)(_ptr)))
-#define ReadEF2Byte(_ptr)		EF2Byte(*((u16 *)(_ptr)))
-#define ReadEF4Byte(_ptr)		EF4Byte(*((u32 *)(_ptr)))
-
-#define WriteEF1Byte(_ptr, _val)	((*((u8 *)(_ptr))) = EF1Byte(_val))
-#define WriteEF2Byte(_ptr, _val)	((*((u16 *)(_ptr))) = EF2Byte(_val))
-#define WriteEF4Byte(_ptr, _val)	((*((u32 *)(_ptr))) = EF4Byte(_val))
-#if BYTE_ORDER == __MACHINE_LITTLE_ENDIAN
-#define H2N1BYTE(_val)	((u8)(_val))
-#define H2N2BYTE(_val)	(((((u16)(_val))&0x00ff)<<8)|\
-			((((u16)(_val))&0xff00)>>8))
-#define H2N4BYTE(_val)	(((((u32)(_val))&0x000000ff)<<24)|\
-			((((u32)(_val))&0x0000ff00)<<8)	|\
-			((((u32)(_val))&0x00ff0000)>>8)	|\
-			((((u32)(_val))&0xff000000)>>24))
-#else
-#define H2N1BYTE(_val)			((u8)(_val))
-#define H2N2BYTE(_val)			((u16)(_val))
-#define H2N4BYTE(_val)			((u32)(_val))
-#endif
-
-#if BYTE_ORDER == __MACHINE_LITTLE_ENDIAN
-#define N2H1BYTE(_val)	((u8)(_val))
-#define N2H2BYTE(_val)	(((((u16)(_val))&0x00ff)<<8)|\
-			((((u16)(_val))&0xff00)>>8))
-#define N2H4BYTE(_val)	(((((u32)(_val))&0x000000ff)<<24)|\
-			((((u32)(_val))&0x0000ff00)<<8)	|\
-			((((u32)(_val))&0x00ff0000)>>8)	|\
-			((((u32)(_val))&0xff000000)>>24))
-#else
-#define N2H1BYTE(_val)			((u8)(_val))
-#define N2H2BYTE(_val)			((u16)(_val))
-#define N2H4BYTE(_val)			((u32)(_val))
-#endif
-
-#define BIT_LEN_MASK_32(__BitLen) (0xFFFFFFFF >> (32 - (__BitLen)))
-#define BIT_OFFSET_LEN_MASK_32(__BitOffset, __BitLen)			\
-	(BIT_LEN_MASK_32(__BitLen) << (__BitOffset))
-
-#define LE_P4BYTE_TO_HOST_4BYTE(__pStart) (EF4Byte(*((u32 *)(__pStart))))
-
-#define LE_BITS_TO_4BYTE(__pStart, __BitOffset, __BitLen) \
-	( \
-	  (LE_P4BYTE_TO_HOST_4BYTE(__pStart) >> (__BitOffset)) \
-	  & \
-	  BIT_LEN_MASK_32(__BitLen) \
-	)
-
-#define LE_BITS_CLEARED_TO_4BYTE(__pStart, __BitOffset, __BitLen) \
-	( \
-	  LE_P4BYTE_TO_HOST_4BYTE(__pStart) \
-	  & \
-	  (~BIT_OFFSET_LEN_MASK_32(__BitOffset, __BitLen)) \
-	)
-
-#define BIT_LEN_MASK_16(__BitLen) \
-	(0xFFFF >> (16 - (__BitLen)))
-
-#define BIT_OFFSET_LEN_MASK_16(__BitOffset, __BitLen) \
-	(BIT_LEN_MASK_16(__BitLen) << (__BitOffset))
-
-#define LE_P2BYTE_TO_HOST_2BYTE(__pStart) \
-	(EF2Byte(*((u16 *)(__pStart))))
-
-#define LE_BITS_TO_2BYTE(__pStart, __BitOffset, __BitLen) \
-	( \
-	  (LE_P2BYTE_TO_HOST_2BYTE(__pStart) >> (__BitOffset)) \
-	  & \
-	  BIT_LEN_MASK_16(__BitLen) \
-	)
-
-#define BIT_LEN_MASK_8(__BitLen) \
-	(0xFF >> (8 - (__BitLen)))
-
-#define BIT_OFFSET_LEN_MASK_8(__BitOffset, __BitLen) \
-	(BIT_LEN_MASK_8(__BitLen) << (__BitOffset))
-
-#define LE_P1BYTE_TO_HOST_1BYTE(__pStart) \
-	(EF1Byte(*((u8 *)(__pStart))))
-
-#define LE_BITS_TO_1BYTE(__pStart, __BitOffset, __BitLen) \
-	( \
-	  (LE_P1BYTE_TO_HOST_1BYTE(__pStart) >> (__BitOffset)) \
-	  & \
-	  BIT_LEN_MASK_8(__BitLen) \
-	)
-
-#define	N_BYTE_ALIGMENT(__Value, __Aligment)			\
-	 ((__Aligment == 1) ? (__Value) : (((__Value + __Aligment - 1) / \
-	__Aligment) * __Aligment))
-#endif
diff --git a/drivers/staging/rtl8192e/rtllib_module.c b/drivers/staging/rtl8192e/rtllib_module.c
index 0cf38091f8c5..32cc8df9d3a7 100644
--- a/drivers/staging/rtl8192e/rtllib_module.c
+++ b/drivers/staging/rtl8192e/rtllib_module.c
@@ -72,11 +72,8 @@ static inline int rtllib_networks_allocate(struct rtllib_device *ieee)
 	ieee->networks = kzalloc(
 		MAX_NETWORK_COUNT * sizeof(struct rtllib_network),
 		GFP_KERNEL);
-	if (!ieee->networks) {
-		printk(KERN_WARNING "%s: Out of memory allocating beacons\n",
-		       ieee->dev->name);
+	if (!ieee->networks)
 		return -ENOMEM;
-	}
 
 	return 0;
 }
@@ -161,10 +158,9 @@ struct net_device *alloc_rtllib(int sizeof_priv)
 	rtllib_softmac_init(ieee);
 
 	ieee->pHTInfo = kzalloc(sizeof(struct rt_hi_throughput), GFP_KERNEL);
-	if (ieee->pHTInfo == NULL) {
-		RTLLIB_DEBUG(RTLLIB_DL_ERR, "can't alloc memory for HTInfo\n");
+	if (ieee->pHTInfo == NULL)
 		return NULL;
-	}
+
 	HTUpdateDefaultSetting(ieee);
 	HTInitializeHTInfo(ieee);
 	TSInitialize(ieee);
@@ -207,7 +203,9 @@ static struct proc_dir_entry *rtllib_proc;
 
 static int show_debug_level(struct seq_file *m, void *v)
 {
-	return seq_printf(m, "0x%08X\n", rtllib_debug_level);
+	seq_printf(m, "0x%08X\n", rtllib_debug_level);
+
+	return 0;
 }
 
 static ssize_t write_debug_level(struct file *file, const char __user *buffer,
diff --git a/drivers/staging/rtl8192e/rtllib_rx.c b/drivers/staging/rtl8192e/rtllib_rx.c
index 1664040efdab..fe3e7e1273ff 100644
--- a/drivers/staging/rtl8192e/rtllib_rx.c
+++ b/drivers/staging/rtl8192e/rtllib_rx.c
@@ -143,7 +143,8 @@ rtllib_frag_cache_get(struct rtllib_device *ieee,
 		memcpy(entry->dst_addr, hdr->addr1, ETH_ALEN);
 	} else {
 		/* received a fragment of a frame for which the head fragment
-		 * should have already been received */
+		 * should have already been received
+		 */
 		entry = rtllib_frag_cache_find(ieee, seq, frag, tid, hdr->addr2,
 						  hdr->addr1);
 		if (entry != NULL) {
@@ -199,7 +200,8 @@ static int rtllib_frag_cache_invalidate(struct rtllib_device *ieee,
  *
  * Responsible for handling management control frames
  *
- * Called by rtllib_rx */
+ * Called by rtllib_rx
+ */
 static inline int
 rtllib_rx_frame_mgmt(struct rtllib_device *ieee, struct sk_buff *skb,
 			struct rtllib_rx_stats *rx_stats, u16 type,
@@ -224,8 +226,9 @@ rtllib_rx_frame_mgmt(struct rtllib_device *ieee, struct sk_buff *skb,
 	return 0;
 }
 
-/* See IEEE 802.1H for LLC/SNAP encapsulation/decapsulation */
-/* Ethernet-II snap header (RFC1042 for most EtherTypes) */
+/* See IEEE 802.1H for LLC/SNAP encapsulation/decapsulation
+ * Ethernet-II snap header (RFC1042 for most EtherTypes)
+ */
 static unsigned char rfc1042_header[] = {
 	0xaa, 0xaa, 0x03, 0x00, 0x00, 0x00
 };
@@ -394,10 +397,9 @@ static int is_duplicate_packet(struct rtllib_device *ieee,
 		}
 		if (p == &ieee->ibss_mac_hash[index]) {
 			entry = kmalloc(sizeof(struct ieee_ibss_seq), GFP_ATOMIC);
-			if (!entry) {
-				printk(KERN_WARNING "Cannot malloc new mac entry\n");
+			if (!entry)
 				return 0;
-			}
+
 			memcpy(entry->mac, mac, ETH_ALEN);
 			entry->seq_num[tid] = seq;
 			entry->frag_num[tid] = frag;
@@ -483,7 +485,8 @@ void rtllib_indicate_packets(struct rtllib_device *ieee, struct rtllib_rxb **prx
 			    ethertype != ETH_P_AARP && ethertype != ETH_P_IPX) ||
 			    memcmp(sub_skb->data, bridge_tunnel_header, SNAP_SIZE) == 0)) {
 				/* remove RFC1042 or Bridge-Tunnel encapsulation
-				 * and replace EtherType */
+				 * and replace EtherType
+				 */
 				skb_pull(sub_skb, SNAP_SIZE);
 				memcpy(skb_push(sub_skb, ETH_ALEN), prxb->src, ETH_ALEN);
 				memcpy(skb_push(sub_skb, ETH_ALEN), prxb->dst, ETH_ALEN);
@@ -524,7 +527,9 @@ void rtllib_FlushRxTsPendingPkts(struct rtllib_device *ieee,	struct rx_ts_record
 	del_timer_sync(&pTS->RxPktPendingTimer);
 	while (!list_empty(&pTS->RxPendingPktList)) {
 		if (RfdCnt >= REORDER_WIN_SIZE) {
-			printk(KERN_INFO "-------------->%s() error! RfdCnt >= REORDER_WIN_SIZE\n", __func__);
+			netdev_info(ieee->dev,
+				    "-------------->%s() error! RfdCnt >= REORDER_WIN_SIZE\n",
+				    __func__);
 			break;
 		}
 
@@ -581,8 +586,7 @@ static void RxReorderIndicatePacket(struct rtllib_device *ieee,
 		return;
 	}
 
-	/*
-	 * Sliding window manipulation. Conditions includes:
+	/* Sliding window manipulation. Conditions includes:
 	 * 1. Incoming SeqNum is equal to WinStart =>Window shift 1
 	 * 2. Incoming SeqNum is larger than the WinEnd => Window shift N
 	 */
@@ -597,13 +601,12 @@ static void RxReorderIndicatePacket(struct rtllib_device *ieee,
 		RTLLIB_DEBUG(RTLLIB_DL_REORDER, "Window Shift! IndicateSeq: %d, NewSeq: %d\n", pTS->RxIndicateSeq, SeqNum);
 	}
 
-	/*
-	 * Indication process.
+	/* Indication process.
 	 * After Packet dropping and Sliding Window shifting as above, we can
 	 * now just indicate the packets with the SeqNum smaller than latest
 	 * WinStart and struct buffer other packets.
-	 */
-	/* For Rx Reorder condition:
+	 *
+	 * For Rx Reorder condition:
 	 * 1. All packets with SeqNum smaller than WinStart => Indicate
 	 * 2. All packets with SeqNum larger than or equal to
 	 *	 WinStart => Buffer it.
@@ -646,8 +649,7 @@ static void RxReorderIndicatePacket(struct rtllib_device *ieee,
 					 pTS->RxIndicateSeq, SeqNum);
 			}
 		} else {
-			/*
-			 * Packets are dropped if there are not enough reorder
+			/* Packets are dropped if there are not enough reorder
 			 * entries. This part should be modified!! We can just
 			 * indicate all the packets in struct buffer and get
 			 * reorder entries.
@@ -697,7 +699,8 @@ static void RxReorderIndicatePacket(struct rtllib_device *ieee,
 	}
 
 	/* Handling pending timer. Set this timer to prevent from long time
-	 * Rx buffering.*/
+	 * Rx buffering.
+	 */
 	if (index > 0) {
 		if (timer_pending(&pTS->RxPktPendingTimer))
 			del_timer_sync(&pTS->RxPktPendingTimer);
@@ -718,7 +721,7 @@ static void RxReorderIndicatePacket(struct rtllib_device *ieee,
 			     __func__);
 		pTS->RxTimeoutIndicateSeq = pTS->RxIndicateSeq;
 		mod_timer(&pTS->RxPktPendingTimer, jiffies +
-			  MSECS(pHTInfo->RxReorderPendingTime));
+			  msecs_to_jiffies(pHTInfo->RxReorderPendingTime));
 	}
 	spin_unlock_irqrestore(&(ieee->reorder_spinlock), flags);
 }
@@ -782,69 +785,74 @@ static u8 parse_subframe(struct rtllib_device *ieee, struct sk_buff *skb,
 		memcpy(rxb->dst, dst, ETH_ALEN);
 		rxb->subframes[0]->dev = ieee->dev;
 		return 1;
-	} else {
-		rxb->nr_subframes = 0;
-		memcpy(rxb->src, src, ETH_ALEN);
-		memcpy(rxb->dst, dst, ETH_ALEN);
-		while (skb->len > ETHERNET_HEADER_SIZE) {
-			/* Offset 12 denote 2 mac address */
-			nSubframe_Length = *((u16 *)(skb->data + 12));
-			nSubframe_Length = (nSubframe_Length >> 8) +
-					   (nSubframe_Length << 8);
-
-			if (skb->len < (ETHERNET_HEADER_SIZE + nSubframe_Length)) {
-				printk(KERN_INFO "%s: A-MSDU parse error!! pRfd->nTotalSubframe : %d\n",\
-				       __func__, rxb->nr_subframes);
-				printk(KERN_INFO "%s: A-MSDU parse error!! Subframe Length: %d\n", __func__,
-				       nSubframe_Length);
-				printk(KERN_INFO "nRemain_Length is %d and nSubframe_Length is : %d\n", skb->len,
-				       nSubframe_Length);
-				printk(KERN_INFO "The Packet SeqNum is %d\n", SeqNum);
-				return 0;
-			}
+	}
 
-			/* move the data point to data content */
-			skb_pull(skb, ETHERNET_HEADER_SIZE);
+	rxb->nr_subframes = 0;
+	memcpy(rxb->src, src, ETH_ALEN);
+	memcpy(rxb->dst, dst, ETH_ALEN);
+	while (skb->len > ETHERNET_HEADER_SIZE) {
+		/* Offset 12 denote 2 mac address */
+		nSubframe_Length = *((u16 *)(skb->data + 12));
+		nSubframe_Length = (nSubframe_Length >> 8) +
+				   (nSubframe_Length << 8);
+
+		if (skb->len < (ETHERNET_HEADER_SIZE + nSubframe_Length)) {
+			netdev_info(ieee->dev,
+				    "%s: A-MSDU parse error!! pRfd->nTotalSubframe : %d\n",
+				    __func__, rxb->nr_subframes);
+			netdev_info(ieee->dev,
+				    "%s: A-MSDU parse error!! Subframe Length: %d\n",
+				    __func__, nSubframe_Length);
+			netdev_info(ieee->dev,
+				    "nRemain_Length is %d and nSubframe_Length is : %d\n",
+				    skb->len, nSubframe_Length);
+			netdev_info(ieee->dev,
+				    "The Packet SeqNum is %d\n",
+				    SeqNum);
+			return 0;
+		}
 
-			/* altered by clark 3/30/2010
-			 * The struct buffer size of the skb indicated to upper layer
-			 * must be less than 5000, or the defraged IP datagram
-			 * in the IP layer will exceed "ipfrag_high_tresh" and be
-			 * discarded. so there must not use the function
-			 * "skb_copy" and "skb_clone" for "skb".
-			 */
+		/* move the data point to data content */
+		skb_pull(skb, ETHERNET_HEADER_SIZE);
 
-			/* Allocate new skb for releasing to upper layer */
-			sub_skb = dev_alloc_skb(nSubframe_Length + 12);
-			if (!sub_skb)
-				return 0;
-			skb_reserve(sub_skb, 12);
-			data_ptr = (u8 *)skb_put(sub_skb, nSubframe_Length);
-			memcpy(data_ptr, skb->data, nSubframe_Length);
-
-			sub_skb->dev = ieee->dev;
-			rxb->subframes[rxb->nr_subframes++] = sub_skb;
-			if (rxb->nr_subframes >= MAX_SUBFRAME_COUNT) {
-				RTLLIB_DEBUG_RX("ParseSubframe(): Too many Subframes! Packets dropped!\n");
-				break;
-			}
-			skb_pull(skb, nSubframe_Length);
-
-			if (skb->len != 0) {
-				nPadding_Length = 4 - ((nSubframe_Length +
-						  ETHERNET_HEADER_SIZE) % 4);
-				if (nPadding_Length == 4)
-					nPadding_Length = 0;
+		/* altered by clark 3/30/2010
+		 * The struct buffer size of the skb indicated to upper layer
+		 * must be less than 5000, or the defraged IP datagram
+		 * in the IP layer will exceed "ipfrag_high_tresh" and be
+		 * discarded. so there must not use the function
+		 * "skb_copy" and "skb_clone" for "skb".
+		 */
 
-				if (skb->len < nPadding_Length)
-					return 0;
+		/* Allocate new skb for releasing to upper layer */
+		sub_skb = dev_alloc_skb(nSubframe_Length + 12);
+		if (!sub_skb)
+			return 0;
+		skb_reserve(sub_skb, 12);
+		data_ptr = (u8 *)skb_put(sub_skb, nSubframe_Length);
+		memcpy(data_ptr, skb->data, nSubframe_Length);
 
-				skb_pull(skb, nPadding_Length);
-			}
+		sub_skb->dev = ieee->dev;
+		rxb->subframes[rxb->nr_subframes++] = sub_skb;
+		if (rxb->nr_subframes >= MAX_SUBFRAME_COUNT) {
+			RTLLIB_DEBUG_RX("ParseSubframe(): Too many Subframes! Packets dropped!\n");
+			break;
 		}
+		skb_pull(skb, nSubframe_Length);
 
-		return rxb->nr_subframes;
+		if (skb->len != 0) {
+			nPadding_Length = 4 - ((nSubframe_Length +
+					  ETHERNET_HEADER_SIZE) % 4);
+			if (nPadding_Length == 4)
+				nPadding_Length = 0;
+
+			if (skb->len < nPadding_Length)
+				return 0;
+
+			skb_pull(skb, nPadding_Length);
+		}
 	}
+
+	return rxb->nr_subframes;
 }
 
 
@@ -859,7 +867,8 @@ static size_t rtllib_rx_get_hdrlen(struct rtllib_device *ieee,
 	hdrlen = rtllib_get_hdrlen(fc);
 	if (HTCCheck(ieee, skb->data)) {
 		if (net_ratelimit())
-			printk(KERN_INFO "%s: find HTCControl!\n", __func__);
+			netdev_info(ieee->dev, "%s: find HTCControl!\n",
+				    __func__);
 		hdrlen += 4;
 		rx_stats->bContainHTC = true;
 	}
@@ -966,7 +975,8 @@ static int rtllib_rx_data_filter(struct rtllib_device *ieee, u16 fc,
 	}
 
 	/* Nullfunc frames may have PS-bit set, so they must be passed to
-	 * hostap_handle_sta_rx() before being dropped here. */
+	 * hostap_handle_sta_rx() before being dropped here.
+	 */
 	if (!ieee->IntelPromiscuousModeInfo.bPromiscuousOn) {
 		if (stype != RTLLIB_STYPE_DATA &&
 		    stype != RTLLIB_STYPE_DATA_CFACK &&
@@ -1008,7 +1018,8 @@ static int rtllib_rx_get_crypt(struct rtllib_device *ieee, struct sk_buff *skb,
 
 		*crypt = ieee->crypt_info.crypt[idx];
 		/* allow NULL decrypt to indicate an station specific override
-		 * for default encryption */
+		 * for default encryption
+		 */
 		if (*crypt && ((*crypt)->ops == NULL ||
 			      (*crypt)->ops->decrypt_mpdu == NULL))
 			*crypt = NULL;
@@ -1017,7 +1028,8 @@ static int rtllib_rx_get_crypt(struct rtllib_device *ieee, struct sk_buff *skb,
 			/* This seems to be triggered by some (multicast?)
 			 * frames from other than current BSS, so just drop the
 			 * frames silently instead of filling system log with
-			 * these reports. */
+			 * these reports.
+			 */
 			RTLLIB_DEBUG_DROP("Decryption failed (not set) (SA= %pM)\n",
 					     hdr->addr2);
 			ieee->ieee_stats.rx_discards_undecryptable++;
@@ -1049,7 +1061,7 @@ static int rtllib_rx_decrypt(struct rtllib_device *ieee, struct sk_buff *skb,
 
 	keyidx = rtllib_rx_frame_decrypt(ieee, skb, crypt);
 	if (ieee->host_decrypt && (fc & RTLLIB_FCTL_WEP) && (keyidx < 0)) {
-		printk(KERN_INFO "%s: decrypt frame error\n", __func__);
+		netdev_info(ieee->dev, "%s: decrypt frame error\n", __func__);
 		return -1;
 	}
 
@@ -1072,19 +1084,22 @@ static int rtllib_rx_decrypt(struct rtllib_device *ieee, struct sk_buff *skb,
 			flen -= hdrlen;
 
 		if (frag_skb->tail + flen > frag_skb->end) {
-			printk(KERN_WARNING "%s: host decrypted and reassembled frame did not fit skb\n",
-			       __func__);
+			netdev_warn(ieee->dev,
+				    "%s: host decrypted and reassembled frame did not fit skb\n",
+				    __func__);
 			rtllib_frag_cache_invalidate(ieee, hdr);
 			return -1;
 		}
 
 		if (frag == 0) {
 			/* copy first fragment (including full headers) into
-			 * beginning of the fragment cache skb */
+			 * beginning of the fragment cache skb
+			 */
 			memcpy(skb_put(frag_skb, flen), skb->data, flen);
 		} else {
 			/* append frame payload to the end of the fragment
-			 * cache skb */
+			 * cache skb
+			 */
 			memcpy(skb_put(frag_skb, flen), skb->data + hdrlen,
 			       flen);
 		}
@@ -1094,22 +1109,25 @@ static int rtllib_rx_decrypt(struct rtllib_device *ieee, struct sk_buff *skb,
 		if (fc & RTLLIB_FCTL_MOREFRAGS) {
 			/* more fragments expected - leave the skb in fragment
 			 * cache for now; it will be delivered to upper layers
-			 * after all fragments have been received */
+			 * after all fragments have been received
+			 */
 			return -2;
 		}
 
 		/* this was the last fragment and the frame will be
-		 * delivered, so remove skb from fragment cache */
+		 * delivered, so remove skb from fragment cache
+		 */
 		skb = frag_skb;
 		hdr = (struct rtllib_hdr_4addr *) skb->data;
 		rtllib_frag_cache_invalidate(ieee, hdr);
 	}
 
 	/* skb: hdr + (possible reassembled) full MSDU payload; possibly still
-	 * encrypted/authenticated */
+	 * encrypted/authenticated
+	 */
 	if (ieee->host_decrypt && (fc & RTLLIB_FCTL_WEP) &&
 		rtllib_rx_frame_decrypt_msdu(ieee, skb, keyidx, crypt)) {
-		printk(KERN_INFO "%s: ==>decrypt msdu error\n", __func__);
+		netdev_info(ieee->dev, "%s: ==>decrypt msdu error\n", __func__);
 		return -1;
 	}
 
@@ -1119,7 +1137,8 @@ static int rtllib_rx_decrypt(struct rtllib_device *ieee, struct sk_buff *skb,
 		    rtllib_is_eapol_frame(ieee, skb, hdrlen)) {
 
 			/* pass unencrypted EAPOL frames even if encryption is
-			 * configured */
+			 * configured
+			 */
 			struct eapol *eap = (struct eapol *)(skb->data +
 				24);
 			RTLLIB_DEBUG_EAP("RX: IEEE 802.1X EAPOL frame: %s\n",
@@ -1149,7 +1168,7 @@ static int rtllib_rx_decrypt(struct rtllib_device *ieee, struct sk_buff *skb,
 	}
 
 	if (rtllib_is_eapol_frame(ieee, skb, hdrlen))
-		printk(KERN_WARNING "RX: IEEE802.1X EAPOL frame!\n");
+		netdev_warn(ieee->dev, "RX: IEEE802.1X EAPOL frame!\n");
 
 	return 0;
 }
@@ -1158,7 +1177,7 @@ static void rtllib_rx_check_leave_lps(struct rtllib_device *ieee, u8 unicast, u8
 {
 	if (unicast) {
 
-		if ((ieee->state == RTLLIB_LINKED)) {
+		if (ieee->state == RTLLIB_LINKED) {
 			if (((ieee->LinkDetectInfo.NumRxUnicastOkInPeriod +
 			    ieee->LinkDetectInfo.NumTxOkInPeriod) > 8) ||
 			    (ieee->LinkDetectInfo.NumRxUnicastOkInPeriod > 2)) {
@@ -1181,8 +1200,8 @@ static void rtllib_rx_indicate_pkt_legacy(struct rtllib_device *ieee,
 	int i = 0;
 
 	if (rxb == NULL) {
-		printk(KERN_INFO "%s: rxb is NULL!!\n", __func__);
-		return ;
+		netdev_info(dev, "%s: rxb is NULL!!\n", __func__);
+		return;
 	}
 
 	for (i = 0; i < rxb->nr_subframes; i++) {
@@ -1196,7 +1215,8 @@ static void rtllib_rx_indicate_pkt_legacy(struct rtllib_device *ieee,
 				ethertype != ETH_P_AARP && ethertype != ETH_P_IPX) ||
 				memcmp(sub_skb->data, bridge_tunnel_header, SNAP_SIZE) == 0)) {
 				/* remove RFC1042 or Bridge-Tunnel encapsulation and
-				 * replace EtherType */
+				 * replace EtherType
+				 */
 				skb_pull(sub_skb, SNAP_SIZE);
 				memcpy(skb_push(sub_skb, ETH_ALEN), src, ETH_ALEN);
 				memcpy(skb_push(sub_skb, ETH_ALEN), dst, ETH_ALEN);
@@ -1226,7 +1246,6 @@ static void rtllib_rx_indicate_pkt_legacy(struct rtllib_device *ieee,
 		}
 	}
 	kfree(rxb);
-	rxb = NULL;
 }
 
 static int rtllib_rx_InfraAdhoc(struct rtllib_device *ieee, struct sk_buff *skb,
@@ -1263,7 +1282,8 @@ static int rtllib_rx_InfraAdhoc(struct rtllib_device *ieee, struct sk_buff *skb,
 	/*Filter pkt has too small length */
 	hdrlen = rtllib_rx_get_hdrlen(ieee, skb, rx_stats);
 	if (skb->len < hdrlen) {
-		printk(KERN_INFO "%s():ERR!!! skb->len is smaller than hdrlen\n", __func__);
+		netdev_info(dev, "%s():ERR!!! skb->len is smaller than hdrlen\n",
+			    __func__);
 		goto rx_dropped;
 	}
 
@@ -1345,11 +1365,9 @@ static int rtllib_rx_InfraAdhoc(struct rtllib_device *ieee, struct sk_buff *skb,
 	/* skb: hdr + (possible reassembled) full plaintext payload */
 	payload = skb->data + hdrlen;
 	rxb = kmalloc(sizeof(struct rtllib_rxb), GFP_ATOMIC);
-	if (rxb == NULL) {
-		RTLLIB_DEBUG(RTLLIB_DL_ERR,
-			     "%s(): kmalloc rxb error\n", __func__);
+	if (rxb == NULL)
 		goto rx_dropped;
-	}
+
 	/* to parse amsdu packets */
 	/* qos data packets & reserved bit is 1 */
 	if (parse_subframe(ieee, skb, rx_stats, rxb, src, dst) == 0) {
@@ -1390,7 +1408,8 @@ static int rtllib_rx_InfraAdhoc(struct rtllib_device *ieee, struct sk_buff *skb,
 
 	/* Returning 0 indicates to caller that we have not handled the SKB--
 	 * so it is still allocated and can be used again by underlying
-	 * hardware as a DMA target */
+	 * hardware as a DMA target
+	 */
 	return 0;
 }
 
@@ -1408,13 +1427,16 @@ static int rtllib_rx_Monitor(struct rtllib_device *ieee, struct sk_buff *skb,
 	size_t hdrlen = rtllib_get_hdrlen(fc);
 
 	if (skb->len < hdrlen) {
-		printk(KERN_INFO "%s():ERR!!! skb->len is smaller than hdrlen\n", __func__);
+		netdev_info(ieee->dev,
+			    "%s():ERR!!! skb->len is smaller than hdrlen\n",
+			    __func__);
 		return 0;
 	}
 
 	if (HTCCheck(ieee, skb->data)) {
 		if (net_ratelimit())
-			printk(KERN_INFO "%s: Find HTCControl!\n", __func__);
+			netdev_info(ieee->dev, "%s: Find HTCControl!\n",
+				    __func__);
 		hdrlen += 4;
 	}
 
@@ -1433,18 +1455,19 @@ static int rtllib_rx_Mesh(struct rtllib_device *ieee, struct sk_buff *skb,
 
 /* All received frames are sent to this function. @skb contains the frame in
  * IEEE 802.11 format, i.e., in the format it was sent over air.
- * This function is called only as a tasklet (software IRQ). */
+ * This function is called only as a tasklet (software IRQ).
+ */
 int rtllib_rx(struct rtllib_device *ieee, struct sk_buff *skb,
 		 struct rtllib_rx_stats *rx_stats)
 {
 	int ret = 0;
 
 	if ((NULL == ieee) || (NULL == skb) || (NULL == rx_stats)) {
-		printk(KERN_INFO "%s: Input parameters NULL!\n", __func__);
+		pr_info("%s: Input parameters NULL!\n", __func__);
 		goto rx_dropped;
 	}
 	if (skb->len < 10) {
-		printk(KERN_INFO "%s: SKB length < 10\n", __func__);
+		netdev_info(ieee->dev, "%s: SKB length < 10\n", __func__);
 		goto rx_dropped;
 	}
 
@@ -1464,7 +1487,7 @@ int rtllib_rx(struct rtllib_device *ieee, struct sk_buff *skb,
 		ret = rtllib_rx_Mesh(ieee, skb, rx_stats);
 		break;
 	default:
-		printk(KERN_INFO"%s: ERR iw mode!!!\n", __func__);
+		netdev_info(ieee->dev, "%s: ERR iw mode!!!\n", __func__);
 		break;
 	}
 
@@ -1479,10 +1502,7 @@ EXPORT_SYMBOL(rtllib_rx);
 
 static u8 qos_oui[QOS_OUI_LEN] = { 0x00, 0x50, 0xF2 };
 
-/*
-* Make ther structure we read from the beacon packet has
-* the right values
-*/
+/* Make ther structure we read from the beacon packet has the right values */
 static int rtllib_verify_qos_info(struct rtllib_qos_information_element
 				     *info_element, int sub_type)
 {
@@ -1500,9 +1520,7 @@ static int rtllib_verify_qos_info(struct rtllib_qos_information_element
 }
 
 
-/*
- * Parse a QoS parameter element
- */
+/* Parse a QoS parameter element */
 static int rtllib_read_qos_param_element(struct rtllib_qos_parameter_info
 					    *element_param, struct rtllib_info_element
 					    *info_element)
@@ -1526,9 +1544,7 @@ static int rtllib_read_qos_param_element(struct rtllib_qos_parameter_info
 	return ret;
 }
 
-/*
- * Parse a QoS information element
- */
+/* Parse a QoS information element */
 static int rtllib_read_qos_info_element(struct
 					   rtllib_qos_information_element
 					   *element_info, struct rtllib_info_element
@@ -1557,9 +1573,7 @@ static int rtllib_read_qos_info_element(struct
 }
 
 
-/*
- * Write QoS parameters from the ac parameters.
- */
+/* Write QoS parameters from the ac parameters. */
 static int rtllib_qos_convert_ac_to_parameters(struct rtllib_qos_parameter_info *param_elm,
 		struct rtllib_qos_data *qos_data)
 {
@@ -1618,8 +1632,7 @@ static int rtllib_qos_convert_ac_to_parameters(struct rtllib_qos_parameter_info
 	return 0;
 }
 
-/*
- * we have a generic data element which it may contain QoS information or
+/* we have a generic data element which it may contain QoS information or
  * parameters element. check the information element length to decide
  * which type to read
  */
@@ -1705,7 +1718,9 @@ static inline void rtllib_extract_country_ie(
 
 			if (!IS_COUNTRY_IE_VALID(ieee)) {
 				if (rtllib_act_scanning(ieee, false) && ieee->FirstIe_InScan)
-					printk(KERN_INFO "Received beacon ContryIE, SSID: <%s>\n", network->ssid);
+					netdev_info(ieee->dev,
+						    "Received beacon ContryIE, SSID: <%s>\n",
+						    network->ssid);
 				Dot11d_UpdateCountryIe(ieee, addr2, info_element->len, info_element->data);
 			}
 		}
@@ -1739,7 +1754,8 @@ int rtllib_parse_info_param(struct rtllib_device *ieee,
 					     length, info_element->id);
 			/* We stop processing but don't return an error here
 			 * because some misbehaviour APs break this rule. ie.
-			 * Orinoco AP1000. */
+			 * Orinoco AP1000.
+			 */
 			break;
 		}
 
@@ -1908,7 +1924,7 @@ int rtllib_parse_info_param(struct rtllib_device *ieee,
 				   info_element->data[2] == 0x4c &&
 				   info_element->data[3] == 0x033) {
 
-						tmp_htcap_len = min(info_element->len, (u8)MAX_IE_LEN);
+						tmp_htcap_len = min_t(u8, info_element->len, MAX_IE_LEN);
 						if (tmp_htcap_len != 0) {
 							network->bssht.bdHTSpecVer = HT_SPEC_VER_EWC;
 							network->bssht.bdHTCapLen = tmp_htcap_len > sizeof(network->bssht.bdHTCapBuf) ?
@@ -1932,7 +1948,7 @@ int rtllib_parse_info_param(struct rtllib_device *ieee,
 				    info_element->data[1] == 0x90 &&
 				    info_element->data[2] == 0x4c &&
 				    info_element->data[3] == 0x034) {
-					tmp_htinfo_len = min(info_element->len, (u8)MAX_IE_LEN);
+					tmp_htinfo_len = min_t(u8, info_element->len, MAX_IE_LEN);
 					if (tmp_htinfo_len != 0) {
 						network->bssht.bdHTSpecVer = HT_SPEC_VER_EWC;
 						if (tmp_htinfo_len) {
@@ -1953,7 +1969,7 @@ int rtllib_parse_info_param(struct rtllib_device *ieee,
 					    info_element->data[1] == 0xe0 &&
 					    info_element->data[2] == 0x4c &&
 					    info_element->data[3] == 0x02) {
-						ht_realtek_agg_len = min(info_element->len, (u8)MAX_IE_LEN);
+						ht_realtek_agg_len = min_t(u8, info_element->len, MAX_IE_LEN);
 						memcpy(ht_realtek_agg_buf, info_element->data, info_element->len);
 					}
 					if (ht_realtek_agg_len >= 5) {
@@ -2083,7 +2099,7 @@ int rtllib_parse_info_param(struct rtllib_device *ieee,
 		case MFIE_TYPE_HT_CAP:
 			RTLLIB_DEBUG_SCAN("MFIE_TYPE_HT_CAP: %d bytes\n",
 					     info_element->len);
-			tmp_htcap_len = min(info_element->len, (u8)MAX_IE_LEN);
+			tmp_htcap_len = min_t(u8, info_element->len, MAX_IE_LEN);
 			if (tmp_htcap_len != 0) {
 				network->bssht.bdHTSpecVer = HT_SPEC_VER_EWC;
 				network->bssht.bdHTCapLen = tmp_htcap_len > sizeof(network->bssht.bdHTCapBuf) ?
@@ -2110,7 +2126,7 @@ int rtllib_parse_info_param(struct rtllib_device *ieee,
 		case MFIE_TYPE_HT_INFO:
 			RTLLIB_DEBUG_SCAN("MFIE_TYPE_HT_INFO: %d bytes\n",
 					     info_element->len);
-			tmp_htinfo_len = min(info_element->len, (u8)MAX_IE_LEN);
+			tmp_htinfo_len = min_t(u8, info_element->len, MAX_IE_LEN);
 			if (tmp_htinfo_len) {
 				network->bssht.bdHTSpecVer = HT_SPEC_VER_IEEE;
 				network->bssht.bdHTInfoLen = tmp_htinfo_len >
@@ -2142,8 +2158,8 @@ int rtllib_parse_info_param(struct rtllib_device *ieee,
 			}
 			break;
 		case MFIE_TYPE_QOS_PARAMETER:
-			printk(KERN_ERR
-			       "QoS Error need to parse QOS_PARAMETER IE\n");
+			netdev_err(ieee->dev,
+				   "QoS Error need to parse QOS_PARAMETER IE\n");
 			break;
 
 		case MFIE_TYPE_COUNTRY:
@@ -2192,13 +2208,6 @@ static inline int rtllib_network_init(
 	struct rtllib_network *network,
 	struct rtllib_rx_stats *stats)
 {
-
-	/*
-	network->qos_data.active = 0;
-	network->qos_data.supported = 0;
-	network->qos_data.param_count = 0;
-	network->qos_data.old_param_count = 0;
-	*/
 	memset(&network->qos_data, 0, sizeof(struct rtllib_qos_data));
 
 	/* Pull out fixed field data */
@@ -2291,7 +2300,8 @@ static inline int is_same_network(struct rtllib_network *src,
 	/* A network is only a duplicate if the channel, BSSID, ESSID
 	 * and the capability field (in particular IBSS and BSS) all match.
 	 * We treat all <hidden> with the same BSSID and channel
-	 * as one network */
+	 * as one network
+	 */
 	return (((src->ssid_len == dst->ssid_len) || (!ssidbroad)) &&
 		(src->channel == dst->channel) &&
 		!memcmp(src->bssid, dst->bssid, ETH_ALEN) &&
@@ -2422,7 +2432,7 @@ static inline int is_beacon(__le16 fc)
 static int IsPassiveChannel(struct rtllib_device *rtllib, u8 channel)
 {
 	if (MAX_CHANNEL_NUMBER < channel) {
-		printk(KERN_INFO "%s(): Invalid Channel\n", __func__);
+		netdev_info(rtllib->dev, "%s(): Invalid Channel\n", __func__);
 		return 0;
 	}
 
@@ -2435,7 +2445,7 @@ static int IsPassiveChannel(struct rtllib_device *rtllib, u8 channel)
 int rtllib_legal_channel(struct rtllib_device *rtllib, u8 channel)
 {
 	if (MAX_CHANNEL_NUMBER < channel) {
-		printk(KERN_INFO "%s(): Invalid Channel\n", __func__);
+		netdev_info(rtllib->dev, "%s(): Invalid Channel\n", __func__);
 		return 0;
 	}
 	if (rtllib->active_channel_map[channel] > 0)
@@ -2501,8 +2511,9 @@ static inline void rtllib_process_probe_response(
 	if (WLAN_FC_GET_STYPE(le16_to_cpu(beacon->header.frame_ctl)) ==
 	    RTLLIB_STYPE_PROBE_RESP) {
 		if (IsPassiveChannel(ieee, network->channel)) {
-			printk(KERN_INFO "GetScanInfo(): For Global Domain, filter probe response at channel(%d).\n",
-			       network->channel);
+			netdev_info(ieee->dev,
+				    "GetScanInfo(): For Global Domain, filter probe response at channel(%d).\n",
+				    network->channel);
 			goto free_network;
 		}
 	}
@@ -2512,10 +2523,12 @@ static inline void rtllib_process_probe_response(
 	 *
 	 * NOTE:  This search is definitely not optimized.  Once its doing
 	 *	the "right thing" we'll optimize it for efficiency if
-	 *	necessary */
+	 *	necessary
+	 */
 
 	/* Search for this entry in the list and update it if it is
-	 * already there. */
+	 * already there.
+	 */
 
 	spin_lock_irqsave(&ieee->lock, flags);
 	if (is_same_network(&ieee->current_network, network,
@@ -2544,7 +2557,8 @@ static inline void rtllib_process_probe_response(
 	}
 
 	/* If we didn't find a match, then get a new network slot to initialize
-	 * with this beacon's information */
+	 * with this beacon's information
+	 */
 	if (&target->list == &ieee->network_list) {
 		if (list_empty(&ieee->network_free_list)) {
 			/* If there are no more slots, expire the oldest */
@@ -2611,7 +2625,6 @@ static inline void rtllib_process_probe_response(
 	}
 free_network:
 	kfree(network);
-	return;
 }
 
 void rtllib_rx_mgt(struct rtllib_device *ieee,
diff --git a/drivers/staging/rtl8192e/rtllib_softmac.c b/drivers/staging/rtl8192e/rtllib_softmac.c
index d992a754e72d..23b7a4c3b699 100644
--- a/drivers/staging/rtl8192e/rtllib_softmac.c
+++ b/drivers/staging/rtl8192e/rtllib_softmac.c
@@ -68,7 +68,8 @@ static void rtllib_MFIE_Brate(struct rtllib_device *ieee, u8 **tag_p)
 	}
 
 	/* We may add an option for custom rates that specific HW
-	 * might support */
+	 * might support
+	 */
 	*tag_p = tag;
 }
 
@@ -89,7 +90,8 @@ static void rtllib_MFIE_Grate(struct rtllib_device *ieee, u8 **tag_p)
 		*tag++ = RTLLIB_BASIC_RATE_MASK | RTLLIB_OFDM_RATE_54MB;
 	}
 	/* We may add an option for custom rates that specific HW might
-	 * support */
+	 * support
+	 */
 	*tag_p = tag;
 }
 
@@ -124,7 +126,7 @@ void rtllib_TURBO_Info(struct rtllib_device *ieee, u8 **tag_p)
 	*tag++ = 0x00;
 
 	*tag_p = tag;
-	printk(KERN_ALERT "This is enable turbo mode IE process\n");
+	netdev_alert(ieee->dev, "This is enable turbo mode IE process\n");
 }
 
 static void enqueue_mgmt(struct rtllib_device *ieee, struct sk_buff *skb)
@@ -133,8 +135,7 @@ static void enqueue_mgmt(struct rtllib_device *ieee, struct sk_buff *skb)
 
 	nh = (ieee->mgmt_queue_head + 1) % MGMT_QUEUE_NUM;
 
-/*
- * if the queue is full but we have newer frames then
+/* if the queue is full but we have newer frames then
  * just overwrites the oldest.
  *
  * if (nh == ieee->mgmt_queue_tail)
@@ -188,7 +189,7 @@ MgntQuery_TxRateExcludeCCKRates(struct rtllib_device *ieee)
 
 	if (QueryRate == 0) {
 		QueryRate = 12;
-		printk(KERN_INFO "No BasicRate found!!\n");
+		netdev_info(ieee->dev, "No BasicRate found!!\n");
 	}
 	return QueryRate;
 }
@@ -273,12 +274,14 @@ inline void softmac_mgmt_xmit(struct sk_buff *skb, struct rtllib_device *ieee)
 		if (!ieee->check_nic_enough_desc(ieee->dev, tcb_desc->queue_index) ||
 		    (skb_queue_len(&ieee->skb_waitQ[tcb_desc->queue_index]) != 0) ||
 		    (ieee->queue_stop)) {
-			/* insert the skb packet to the management queue */
-			/* as for the completion function, it does not need
+			/* insert the skb packet to the management queue
+			 *
+			 * as for the completion function, it does not need
 			 * to check it any more.
-			 * */
-			printk(KERN_INFO "%s():insert to waitqueue, queue_index"
-			       ":%d!\n", __func__, tcb_desc->queue_index);
+			 */
+			netdev_info(ieee->dev,
+			       "%s():insert to waitqueue, queue_index:%d!\n",
+			       __func__, tcb_desc->queue_index);
 			skb_queue_tail(&ieee->skb_waitQ[tcb_desc->queue_index],
 				       skb);
 		} else {
@@ -401,7 +404,7 @@ static void rtllib_send_beacon(struct rtllib_device *ieee)
 
 	if (ieee->beacon_txing && ieee->ieee_up)
 		mod_timer(&ieee->beacon_timer, jiffies +
-			  (MSECS(ieee->current_network.beacon_interval - 5)));
+			  (msecs_to_jiffies(ieee->current_network.beacon_interval - 5)));
 }
 
 
@@ -416,40 +419,33 @@ static void rtllib_send_beacon_cb(unsigned long _ieee)
 	spin_unlock_irqrestore(&ieee->beacon_lock, flags);
 }
 
-/*
- * Description:
- *	      Enable network monitor mode, all rx packets will be received.
- */
+/* Enables network monitor mode, all rx packets will be received. */
 void rtllib_EnableNetMonitorMode(struct net_device *dev,
 		bool bInitState)
 {
 	struct rtllib_device *ieee = netdev_priv_rsl(dev);
 
-	printk(KERN_INFO "========>Enter Monitor Mode\n");
+	netdev_info(dev, "========>Enter Monitor Mode\n");
 
 	ieee->AllowAllDestAddrHandler(dev, true, !bInitState);
 }
 
 
-/*
- *      Description:
- *	      Disable network network monitor mode, only packets destinated to
- *	      us will be received.
+/* Disables network monitor mode. Only packets destinated to
+ * us will be received.
  */
 void rtllib_DisableNetMonitorMode(struct net_device *dev,
 		bool bInitState)
 {
 	struct rtllib_device *ieee = netdev_priv_rsl(dev);
 
-	printk(KERN_INFO "========>Exit Monitor Mode\n");
+	netdev_info(dev, "========>Exit Monitor Mode\n");
 
 	ieee->AllowAllDestAddrHandler(dev, false, !bInitState);
 }
 
 
-/*
- * Description:
- * This enables the specialized promiscuous mode required by Intel.
+/* Enables the specialized promiscuous mode required by Intel.
  * In this mode, Intel intends to hear traffics from/to other STAs in the
  * same BSS. Therefore we don't have to disable checking BSSID and we only need
  * to allow all dest. BUT: if we enable checking BSSID then we can't recv
@@ -462,7 +458,7 @@ void rtllib_EnableIntelPromiscuousMode(struct net_device *dev,
 
 	struct rtllib_device *ieee = netdev_priv_rsl(dev);
 
-	printk(KERN_INFO "========>Enter Intel Promiscuous Mode\n");
+	netdev_info(dev, "========>Enter Intel Promiscuous Mode\n");
 
 	ieee->AllowAllDestAddrHandler(dev, true, !bInitState);
 	ieee->SetHwRegHandler(dev, HW_VAR_CECHK_BSSID,
@@ -473,10 +469,8 @@ void rtllib_EnableIntelPromiscuousMode(struct net_device *dev,
 EXPORT_SYMBOL(rtllib_EnableIntelPromiscuousMode);
 
 
-/*
- * Description:
- *	      This disables the specialized promiscuous mode required by Intel.
- *	      See MgntEnableIntelPromiscuousMode for detail.
+/* Disables the specialized promiscuous mode required by Intel.
+ * See MgntEnableIntelPromiscuousMode for detail.
  */
 void rtllib_DisableIntelPromiscuousMode(struct net_device *dev,
 		bool bInitState)
@@ -485,7 +479,7 @@ void rtllib_DisableIntelPromiscuousMode(struct net_device *dev,
 
 	struct rtllib_device *ieee = netdev_priv_rsl(dev);
 
-	printk(KERN_INFO "========>Exit Intel Promiscuous Mode\n");
+	netdev_info(dev, "========>Exit Intel Promiscuous Mode\n");
 
 	ieee->AllowAllDestAddrHandler(dev, false, !bInitState);
 	ieee->SetHwRegHandler(dev, HW_VAR_CECHK_BSSID,
@@ -569,7 +563,8 @@ void rtllib_softmac_scan_syncro(struct rtllib_device *ieee, u8 is_mesh)
 		if (ieee->state == RTLLIB_LINKED)
 			goto out;
 		if (ieee->sync_scan_hurryup) {
-			printk(KERN_INFO "============>sync_scan_hurryup out\n");
+			netdev_info(ieee->dev,
+				    "============>sync_scan_hurryup out\n");
 			goto out;
 		}
 
@@ -614,8 +609,9 @@ static void rtllib_softmac_scan_wq(void *data)
 	down(&ieee->scan_sem);
 
 	if (ieee->eRFPowerState == eRfOff) {
-		printk(KERN_INFO "======>%s():rf state is eRfOff, return\n",
-		       __func__);
+		netdev_info(ieee->dev,
+			    "======>%s():rf state is eRfOff, return\n",
+			    __func__);
 		goto out1;
 	}
 
@@ -639,7 +635,7 @@ static void rtllib_softmac_scan_wq(void *data)
 		rtllib_send_probe_requests(ieee, 0);
 
 	queue_delayed_work_rsl(ieee->wq, &ieee->softmac_scan_wq,
-			       MSECS(RTLLIB_SOFTMAC_SCAN_TIME));
+			       msecs_to_jiffies(RTLLIB_SOFTMAC_SCAN_TIME));
 
 	up(&ieee->scan_sem);
 	return;
@@ -1238,8 +1234,8 @@ inline struct sk_buff *rtllib_association_req(struct rtllib_network *beacon,
 	PMKCacheIdx = SecIsInPMKIDList(ieee, ieee->current_network.bssid);
 	if (PMKCacheIdx >= 0) {
 		wpa_ie_len += 18;
-		printk(KERN_INFO "[PMK cache]: WPA2 IE length: %x\n",
-		       wpa_ie_len);
+		netdev_info(ieee->dev, "[PMK cache]: WPA2 IE length: %x\n",
+			    wpa_ie_len);
 	}
 	len = sizeof(struct rtllib_assoc_request_frame) + 2
 		+ beacon->ssid_len
@@ -1418,8 +1414,9 @@ inline struct sk_buff *rtllib_association_req(struct rtllib_network *beacon,
 	if (ieee->assocreq_ies)
 		memcpy(ieee->assocreq_ies, ies, ieee->assocreq_ies_len);
 	else {
-		printk(KERN_INFO "%s()Warning: can't alloc memory for assocreq"
-		       "_ies\n", __func__);
+		netdev_info(ieee->dev,
+			    "%s()Warning: can't alloc memory for assocreq_ies\n",
+			    __func__);
 		ieee->assocreq_ies_len = 0;
 	}
 	return skb;
@@ -1474,7 +1471,7 @@ static void rtllib_associate_step1(struct rtllib_device *ieee, u8 *daddr)
 	if (!skb)
 		rtllib_associate_abort(ieee);
 	else {
-		ieee->state = RTLLIB_ASSOCIATING_AUTHENTICATING ;
+		ieee->state = RTLLIB_ASSOCIATING_AUTHENTICATING;
 		RTLLIB_DEBUG_MGMT("Sending authentication request\n");
 		softmac_mgmt_xmit(skb, ieee);
 		if (!timer_pending(&ieee->associate_timer)) {
@@ -1503,8 +1500,7 @@ static void rtllib_auth_challenge(struct rtllib_device *ieee, u8 *challenge, int
 		*(c++) = chlen;
 		memcpy(c, challenge, chlen);
 
-		RTLLIB_DEBUG_MGMT("Sending authentication challenge "
-				  "response\n");
+		RTLLIB_DEBUG_MGMT("Sending authentication challenge response\n");
 
 		rtllib_encrypt_fragment(ieee, skb,
 					sizeof(struct rtllib_hdr_3addr));
@@ -1543,9 +1539,9 @@ static void rtllib_associate_complete_wq(void *data)
 				     associate_complete_wq);
 	struct rt_pwr_save_ctrl *pPSC = (struct rt_pwr_save_ctrl *)
 					(&(ieee->PowerSaveControl));
-	printk(KERN_INFO "Associated successfully\n");
+	netdev_info(ieee->dev, "Associated successfully\n");
 	if (!ieee->is_silent_reset) {
-		printk(KERN_INFO "normal associate\n");
+		netdev_info(ieee->dev, "normal associate\n");
 		notify_wx_assoc_event(ieee);
 	}
 
@@ -1554,20 +1550,20 @@ static void rtllib_associate_complete_wq(void *data)
 	if (rtllib_is_54g(&ieee->current_network) &&
 	   (ieee->modulation & RTLLIB_OFDM_MODULATION)) {
 		ieee->rate = 108;
-		printk(KERN_INFO"Using G rates:%d\n", ieee->rate);
+		netdev_info(ieee->dev, "Using G rates:%d\n", ieee->rate);
 	} else {
 		ieee->rate = 22;
 		ieee->SetWirelessMode(ieee->dev, IEEE_B);
-		printk(KERN_INFO"Using B rates:%d\n", ieee->rate);
+		netdev_info(ieee->dev, "Using B rates:%d\n", ieee->rate);
 	}
 	if (ieee->pHTInfo->bCurrentHTSupport && ieee->pHTInfo->bEnableHT) {
-		printk(KERN_INFO "Successfully associated, ht enabled\n");
+		netdev_info(ieee->dev, "Successfully associated, ht enabled\n");
 		HTOnAssocRsp(ieee);
 	} else {
-		printk(KERN_INFO "Successfully associated, ht not "
-		       "enabled(%d, %d)\n",
-		       ieee->pHTInfo->bCurrentHTSupport,
-		       ieee->pHTInfo->bEnableHT);
+		netdev_info(ieee->dev,
+			    "Successfully associated, ht not enabled(%d, %d)\n",
+			    ieee->pHTInfo->bCurrentHTSupport,
+			    ieee->pHTInfo->bEnableHT);
 		memset(ieee->dot11HTOperationalRateSet, 0, 16);
 	}
 	ieee->LinkDetectInfo.SlotNum = 2 * (1 +
@@ -1582,7 +1578,7 @@ static void rtllib_associate_complete_wq(void *data)
 	ieee->link_change(ieee->dev);
 
 	if (ieee->is_silent_reset) {
-		printk(KERN_INFO "silent reset associate\n");
+		netdev_info(ieee->dev, "silent reset associate\n");
 		ieee->is_silent_reset = false;
 	}
 
@@ -1623,8 +1619,9 @@ static void rtllib_associate_procedure_wq(void *data)
 		 ieee->current_network.channel);
 	HTSetConnectBwMode(ieee, HT_CHANNEL_WIDTH_20, HT_EXTCHNL_OFFSET_NO_EXT);
 	if (ieee->eRFPowerState == eRfOff) {
-		RT_TRACE(COMP_DBG, "=============>%s():Rf state is eRfOff,"
-			 " schedule ipsleave wq again,return\n", __func__);
+		RT_TRACE(COMP_DBG,
+			 "=============>%s():Rf state is eRfOff, schedule ipsleave wq again,return\n",
+			 __func__);
 		if (ieee->rtllib_ips_leave_wq != NULL)
 			ieee->rtllib_ips_leave_wq(ieee->dev);
 		up(&ieee->wx_sem);
@@ -1705,8 +1702,8 @@ inline void rtllib_softmac_new_net(struct rtllib_device *ieee,
 		   (!apset && ssidset && ssidbroad && ssidmatch) ||
 		   (ieee->is_roaming && ssidset && ssidbroad && ssidmatch)) {
 			/* if the essid is hidden replace it with the
-			* essid provided by the user.
-			*/
+			 * essid provided by the user.
+			 */
 			if (!ssidbroad) {
 				strncpy(tmp_ssid, ieee->current_network.ssid,
 					IW_ESSID_MAX_SIZE);
@@ -1719,15 +1716,15 @@ inline void rtllib_softmac_new_net(struct rtllib_device *ieee,
 					IW_ESSID_MAX_SIZE);
 				ieee->current_network.ssid_len = tmp_ssid_len;
 			}
-			printk(KERN_INFO"Linking with %s,channel:%d, qos:%d, "
-			       "myHT:%d, networkHT:%d, mode:%x cur_net.flags"
-			       ":0x%x\n", ieee->current_network.ssid,
-			       ieee->current_network.channel,
-			       ieee->current_network.qos_data.supported,
-			       ieee->pHTInfo->bEnableHT,
-			       ieee->current_network.bssht.bdSupportHT,
-			       ieee->current_network.mode,
-			       ieee->current_network.flags);
+			netdev_info(ieee->dev,
+				    "Linking with %s,channel:%d, qos:%d, myHT:%d, networkHT:%d, mode:%x cur_net.flags:0x%x\n",
+				    ieee->current_network.ssid,
+				    ieee->current_network.channel,
+				    ieee->current_network.qos_data.supported,
+				    ieee->pHTInfo->bEnableHT,
+				    ieee->current_network.bssht.bdSupportHT,
+				    ieee->current_network.mode,
+				    ieee->current_network.flags);
 
 			if ((rtllib_act_scanning(ieee, false)) &&
 			   !(ieee->softmac_features & IEEE_SOFTMAC_SCAN))
@@ -1758,11 +1755,11 @@ inline void rtllib_softmac_new_net(struct rtllib_device *ieee,
 					(ieee->modulation & RTLLIB_OFDM_MODULATION)) {
 					ieee->rate = 108;
 					ieee->SetWirelessMode(ieee->dev, IEEE_G);
-					printk(KERN_INFO"Using G rates\n");
+					netdev_info(ieee->dev, "Using G rates\n");
 				} else {
 					ieee->rate = 22;
 					ieee->SetWirelessMode(ieee->dev, IEEE_B);
-					printk(KERN_INFO"Using B rates\n");
+					netdev_info(ieee->dev, "Using B rates\n");
 				}
 				memset(ieee->dot11HTOperationalRateSet, 0, 16);
 				ieee->state = RTLLIB_LINKED;
@@ -1965,7 +1962,7 @@ static inline void rtllib_rx_assoc_rq(struct rtllib_device *ieee,
 	if (assoc_rq_parse(skb, dest) != -1)
 		rtllib_resp_to_assoc_rq(ieee, dest);
 
-	printk(KERN_INFO"New client associated: %pM\n", dest);
+	netdev_info(ieee->dev, "New client associated: %pM\n", dest);
 }
 
 void rtllib_sta_ps_send_null_frame(struct rtllib_device *ieee, short pwr)
@@ -2004,13 +2001,16 @@ static short rtllib_sta_ps_sleep(struct rtllib_device *ieee, u64 *time)
 	timeout = ieee->current_network.beacon_interval;
 	ieee->current_network.dtim_data = RTLLIB_DTIM_INVALID;
 	/* there's no need to nofity AP that I find you buffered
-	 * with broadcast packet */
+	 * with broadcast packet
+	 */
 	if (dtim & (RTLLIB_DTIM_UCAST & ieee->ps))
 		return 2;
 
-	if (!time_after(jiffies, ieee->dev->trans_start + MSECS(timeout)))
+	if (!time_after(jiffies,
+			ieee->dev->trans_start + msecs_to_jiffies(timeout)))
 		return 0;
-	if (!time_after(jiffies, ieee->last_rx_ps_time + MSECS(timeout)))
+	if (!time_after(jiffies,
+			ieee->last_rx_ps_time + msecs_to_jiffies(timeout)))
 		return 0;
 	if ((ieee->softmac_features & IEEE_SOFTMAC_SINGLE_QUEUE) &&
 	    (ieee->mgmt_queue_tail != ieee->mgmt_queue_head))
@@ -2060,7 +2060,7 @@ static short rtllib_sta_ps_sleep(struct rtllib_device *ieee, u64 *time)
 			}
 
 		*time = ieee->current_network.last_dtim_sta_time
-			+ MSECS(ieee->current_network.beacon_interval *
+			+ msecs_to_jiffies(ieee->current_network.beacon_interval *
 			LPSAwakeIntvl_tmp);
 	}
 	}
@@ -2081,10 +2081,9 @@ static inline void rtllib_sta_ps(struct rtllib_device *ieee)
 	if ((ieee->ps == RTLLIB_PS_DISABLED ||
 	     ieee->iw_mode != IW_MODE_INFRA ||
 	     ieee->state != RTLLIB_LINKED)) {
-		RT_TRACE(COMP_DBG, "=====>%s(): no need to ps,wake up!! "
-			 "ieee->ps is %d, ieee->iw_mode is %d, ieee->state"
-			 " is %d\n", __func__, ieee->ps, ieee->iw_mode,
-			  ieee->state);
+		RT_TRACE(COMP_DBG,
+			 "=====>%s(): no need to ps,wake up!! ieee->ps is %d, ieee->iw_mode is %d, ieee->state is %d\n",
+			 __func__, ieee->ps, ieee->iw_mode, ieee->state);
 		spin_lock_irqsave(&ieee->mgmt_tx_lock, flags2);
 		rtllib_sta_wakeup(ieee, 1);
 
@@ -2199,8 +2198,8 @@ static void rtllib_process_action(struct rtllib_device *ieee, struct sk_buff *sk
 	u8 category = 0;
 
 	if (act == NULL) {
-		RTLLIB_DEBUG(RTLLIB_DL_ERR, "error to get payload of "
-			     "action frame\n");
+		RTLLIB_DEBUG(RTLLIB_DL_ERR,
+			     "error to get payload of action frame\n");
 		return;
 	}
 
@@ -2223,7 +2222,6 @@ static void rtllib_process_action(struct rtllib_device *ieee, struct sk_buff *sk
 	default:
 		break;
 	}
-	return;
 }
 
 inline int rtllib_rx_assoc_resp(struct rtllib_device *ieee, struct sk_buff *skb,
@@ -2285,16 +2283,18 @@ inline int rtllib_rx_assoc_resp(struct rtllib_device *ieee, struct sk_buff *skb,
 				memcpy(ieee->assocresp_ies, ies,
 				       ieee->assocresp_ies_len);
 			else {
-				printk(KERN_INFO "%s()Warning: can't alloc "
-				       "memory for assocresp_ies\n", __func__);
+				netdev_info(ieee->dev,
+					    "%s()Warning: can't alloc memory for assocresp_ies\n",
+					    __func__);
 				ieee->assocresp_ies_len = 0;
 			}
 			rtllib_associate_complete(ieee);
 		} else {
 			/* aid could not been allocated */
 			ieee->softmac_stats.rx_ass_err++;
-			printk(KERN_INFO "Association response status code 0x%x\n",
-				errcode);
+			netdev_info(ieee->dev,
+				    "Association response status code 0x%x\n",
+				    errcode);
 			RTLLIB_DEBUG_MGMT(
 				"Association response status code 0x%x\n",
 				errcode);
@@ -2308,72 +2308,71 @@ inline int rtllib_rx_assoc_resp(struct rtllib_device *ieee, struct sk_buff *skb,
 	return 0;
 }
 
-inline int rtllib_rx_auth(struct rtllib_device *ieee, struct sk_buff *skb,
-			  struct rtllib_rx_stats *rx_stats)
+static void rtllib_rx_auth_resp(struct rtllib_device *ieee, struct sk_buff *skb)
 {
 	u16 errcode;
 	u8 *challenge;
 	int chlen = 0;
 	bool bSupportNmode = true, bHalfSupportNmode = false;
 
-	if (ieee->softmac_features & IEEE_SOFTMAC_ASSOCIATE) {
-		if (ieee->state == RTLLIB_ASSOCIATING_AUTHENTICATING &&
-		    (ieee->iw_mode == IW_MODE_INFRA)) {
-			RTLLIB_DEBUG_MGMT("Received authentication response");
+	errcode = auth_parse(skb, &challenge, &chlen);
 
-			errcode = auth_parse(skb, &challenge, &chlen);
-			if (0 == errcode) {
-				if (ieee->open_wep || !challenge) {
-					ieee->state = RTLLIB_ASSOCIATING_AUTHENTICATED;
-					ieee->softmac_stats.rx_auth_rs_ok++;
-					if (!(ieee->pHTInfo->IOTAction &
-					    HT_IOT_ACT_PURE_N_MODE)) {
-						if (!ieee->GetNmodeSupportBySecCfg(ieee->dev)) {
-							if (IsHTHalfNmodeAPs(ieee)) {
-								bSupportNmode = true;
-								bHalfSupportNmode = true;
-							} else {
-								bSupportNmode = false;
-								bHalfSupportNmode = false;
-							}
-						}
-					}
-					/* Dummy wirless mode setting to avoid
-					 * encryption issue */
-					if (bSupportNmode) {
-						ieee->SetWirelessMode(ieee->dev,
-						   ieee->current_network.mode);
-					} else {
-						/*TODO*/
-						ieee->SetWirelessMode(ieee->dev,
-								      IEEE_G);
-					}
-
-					if (ieee->current_network.mode ==
-					    IEEE_N_24G && bHalfSupportNmode) {
-						printk(KERN_INFO "======>enter "
-						       "half N mode\n");
-						ieee->bHalfWirelessN24GMode =
-									 true;
-					} else
-						ieee->bHalfWirelessN24GMode =
-									 false;
-
-					rtllib_associate_step2(ieee);
+	if (errcode) {
+		ieee->softmac_stats.rx_auth_rs_err++;
+		RTLLIB_DEBUG_MGMT("Authentication respose status code 0x%x",
+				  errcode);
+
+		netdev_info(ieee->dev,
+			    "Authentication respose status code 0x%x", errcode);
+		rtllib_associate_abort(ieee);
+		return;
+	}
+
+	if (ieee->open_wep || !challenge) {
+		ieee->state = RTLLIB_ASSOCIATING_AUTHENTICATED;
+		ieee->softmac_stats.rx_auth_rs_ok++;
+		if (!(ieee->pHTInfo->IOTAction & HT_IOT_ACT_PURE_N_MODE)) {
+			if (!ieee->GetNmodeSupportBySecCfg(ieee->dev)) {
+				if (IsHTHalfNmodeAPs(ieee)) {
+					bSupportNmode = true;
+					bHalfSupportNmode = true;
 				} else {
-					rtllib_auth_challenge(ieee, challenge,
-							      chlen);
+					bSupportNmode = false;
+					bHalfSupportNmode = false;
 				}
-			} else {
-				ieee->softmac_stats.rx_auth_rs_err++;
-				RTLLIB_DEBUG_MGMT("Authentication respose"
-						  " status code 0x%x", errcode);
-
-				printk(KERN_INFO "Authentication respose "
-				       "status code 0x%x", errcode);
-				rtllib_associate_abort(ieee);
 			}
+		}
+		/* Dummy wirless mode setting to avoid encryption issue */
+		if (bSupportNmode) {
+			ieee->SetWirelessMode(ieee->dev,
+					      ieee->current_network.mode);
+		} else {
+			/*TODO*/
+			ieee->SetWirelessMode(ieee->dev, IEEE_G);
+		}
 
+		if ((ieee->current_network.mode == IEEE_N_24G) &&
+		    bHalfSupportNmode) {
+			netdev_info(ieee->dev, "======>enter half N mode\n");
+			ieee->bHalfWirelessN24GMode = true;
+		} else {
+			ieee->bHalfWirelessN24GMode = false;
+		}
+		rtllib_associate_step2(ieee);
+	} else {
+		rtllib_auth_challenge(ieee, challenge,  chlen);
+	}
+}
+
+inline int rtllib_rx_auth(struct rtllib_device *ieee, struct sk_buff *skb,
+			  struct rtllib_rx_stats *rx_stats)
+{
+
+	if (ieee->softmac_features & IEEE_SOFTMAC_ASSOCIATE) {
+		if (ieee->state == RTLLIB_ASSOCIATING_AUTHENTICATING &&
+		    (ieee->iw_mode == IW_MODE_INFRA)) {
+			RTLLIB_DEBUG_MGMT("Received authentication response");
+			rtllib_rx_auth_resp(ieee, skb);
 		} else if (ieee->iw_mode == IW_MODE_MASTER) {
 			rtllib_rx_auth_rq(ieee, skb);
 		}
@@ -2389,15 +2388,15 @@ inline int rtllib_rx_deauth(struct rtllib_device *ieee, struct sk_buff *skb)
 		return 0;
 
 	/* FIXME for now repeat all the association procedure
-	* both for disassociation and deauthentication
-	*/
+	 * both for disassociation and deauthentication
+	 */
 	if ((ieee->softmac_features & IEEE_SOFTMAC_ASSOCIATE) &&
 	    ieee->state == RTLLIB_LINKED &&
 	    (ieee->iw_mode == IW_MODE_INFRA)) {
-		printk(KERN_INFO "==========>received disassoc/deauth(%x) "
-		       "frame, reason code:%x\n",
-		       WLAN_FC_GET_STYPE(header->frame_ctl),
-		       ((struct rtllib_disassoc *)skb->data)->reason);
+		netdev_info(ieee->dev,
+			    "==========>received disassoc/deauth(%x) frame, reason code:%x\n",
+			    WLAN_FC_GET_STYPE(header->frame_ctl),
+			    ((struct rtllib_disassoc *)skb->data)->reason);
 		ieee->state = RTLLIB_ASSOCIATING;
 		ieee->softmac_stats.reassoc++;
 		ieee->is_roaming = true;
@@ -2450,7 +2449,6 @@ inline int rtllib_rx_frame_softmac(struct rtllib_device *ieee,
 		break;
 	default:
 		return -1;
-		break;
 	}
 	return 0;
 }
@@ -2494,16 +2492,17 @@ void rtllib_softmac_xmit(struct rtllib_txb *txb, struct rtllib_device *ieee)
 		ieee->stats.multicast++;
 
 	/* if xmit available, just xmit it immediately, else just insert it to
-	 * the wait queue */
+	 * the wait queue
+	 */
 	for (i = 0; i < txb->nr_frags; i++) {
 		queue_len = skb_queue_len(&ieee->skb_waitQ[queue_index]);
-		if ((queue_len  != 0) ||\
+		if ((queue_len  != 0) ||
 		    (!ieee->check_nic_enough_desc(ieee->dev, queue_index)) ||
 		    (ieee->queue_stop)) {
-			/* insert the skb packet to the wait queue */
-			/* as for the completion function, it does not need
+			/* insert the skb packet to the wait queue
+			 * as for the completion function, it does not need
 			 * to check it any more.
-			 * */
+			 */
 			if (queue_len < 200)
 				skb_queue_tail(&ieee->skb_waitQ[queue_index],
 					       txb->fragments[i]);
@@ -2533,13 +2532,12 @@ static void rtllib_resume_tx(struct rtllib_device *ieee)
 		if (ieee->queue_stop) {
 			ieee->tx_pending.frag = i;
 			return;
-		} else {
-
-			ieee->softmac_data_hard_start_xmit(
-				ieee->tx_pending.txb->fragments[i],
-				ieee->dev, ieee->rate);
-			ieee->stats.tx_packets++;
 		}
+
+		ieee->softmac_data_hard_start_xmit(
+			ieee->tx_pending.txb->fragments[i],
+			ieee->dev, ieee->rate);
+		ieee->stats.tx_packets++;
 	}
 
 	rtllib_txb_free(ieee->tx_pending.txb);
@@ -2688,7 +2686,7 @@ static void rtllib_start_ibss_wq(void *data)
 	 * on the semaphore
 	 */
 	if (!ieee->proto_started) {
-		printk(KERN_INFO "==========oh driver down return\n");
+		netdev_info(ieee->dev, "==========oh driver down return\n");
 		return;
 	}
 	down(&ieee->wx_sem);
@@ -2724,7 +2722,7 @@ static void rtllib_start_ibss_wq(void *data)
 
 	/* the network definitively is not here.. create a new cell */
 	if (ieee->state == RTLLIB_NOLINK) {
-		printk(KERN_INFO "creating new IBSS cell\n");
+		netdev_info(ieee->dev, "creating new IBSS cell\n");
 		ieee->current_network.channel = ieee->IbssStartChnl;
 		if (!ieee->wap_set)
 			rtllib_randomize_cell(ieee);
@@ -2778,7 +2776,7 @@ static void rtllib_start_ibss_wq(void *data)
 		ieee->current_network.capability = WLAN_CAPABILITY_IBSS;
 	}
 
-	printk(KERN_INFO "%s(): ieee->mode = %d\n", __func__, ieee->mode);
+	netdev_info(ieee->dev, "%s(): ieee->mode = %d\n", __func__, ieee->mode);
 	if ((ieee->mode == IEEE_N_24G) || (ieee->mode == IEEE_N_5G))
 		HTUseDefaultSetting(ieee);
 	else
@@ -2808,7 +2806,8 @@ static void rtllib_start_ibss_wq(void *data)
 
 inline void rtllib_start_ibss(struct rtllib_device *ieee)
 {
-	queue_delayed_work_rsl(ieee->wq, &ieee->start_ibss_wq, MSECS(150));
+	queue_delayed_work_rsl(ieee->wq, &ieee->start_ibss_wq,
+			       msecs_to_jiffies(150));
 }
 
 /* this is called only in user context, with wx_sem held */
@@ -2881,18 +2880,18 @@ static void rtllib_associate_retry_wq(void *data)
 		goto exit;
 
 	/* until we do not set the state to RTLLIB_NOLINK
-	* there are no possibility to have someone else trying
-	* to start an association procedure (we get here with
-	* ieee->state = RTLLIB_ASSOCIATING).
-	* When we set the state to RTLLIB_NOLINK it is possible
-	* that the RX path run an attempt to associate, but
-	* both rtllib_softmac_check_all_nets and the
-	* RX path works with ieee->lock held so there are no
-	* problems. If we are still disassociated then start a scan.
-	* the lock here is necessary to ensure no one try to start
-	* an association procedure when we have just checked the
-	* state and we are going to start the scan.
-	*/
+	 * there are no possibility to have someone else trying
+	 * to start an association procedure (we get here with
+	 * ieee->state = RTLLIB_ASSOCIATING).
+	 * When we set the state to RTLLIB_NOLINK it is possible
+	 * that the RX path run an attempt to associate, but
+	 * both rtllib_softmac_check_all_nets and the
+	 * RX path works with ieee->lock held so there are no
+	 * problems. If we are still disassociated then start a scan.
+	 * the lock here is necessary to ensure no one try to start
+	 * an association procedure when we have just checked the
+	 * state and we are going to start the scan.
+	 */
 	ieee->beinretry = true;
 	ieee->state = RTLLIB_NOLINK;
 
@@ -3180,8 +3179,9 @@ void rtllib_softmac_free(struct rtllib_device *ieee)
 static int rtllib_wpa_enable(struct rtllib_device *ieee, int value)
 {
 	/* This is called when wpa_supplicant loads and closes the driver
-	 * interface. */
-	printk(KERN_INFO "%s WPA\n", value ? "enabling" : "disabling");
+	 * interface.
+	 */
+	netdev_info(ieee->dev, "%s WPA\n", value ? "enabling" : "disabling");
 	ieee->wpa_enabled = value;
 	memset(ieee->ap_mac_addr, 0, 6);
 	return 0;
@@ -3212,7 +3212,7 @@ static int rtllib_wpa_mlme(struct rtllib_device *ieee, int command, int reason)
 		break;
 
 	default:
-		printk(KERN_INFO "Unknown MLME request: %d\n", command);
+		netdev_info(ieee->dev, "Unknown MLME request: %d\n", command);
 		ret = -EOPNOTSUPP;
 	}
 
@@ -3343,7 +3343,7 @@ static int rtllib_wpa_set_param(struct rtllib_device *ieee, u8 name, u32 value)
 		break;
 
 	default:
-		printk(KERN_INFO "Unknown WPA param: %d\n", name);
+		netdev_info(ieee->dev, "Unknown WPA param: %d\n", name);
 		ret = -EOPNOTSUPP;
 	}
 
@@ -3369,8 +3369,8 @@ static int rtllib_wpa_set_encryption(struct rtllib_device *ieee,
 	if (param_len !=
 	    (int) ((char *) param->u.crypt.key - (char *) param) +
 	    param->u.crypt.key_len) {
-		printk(KERN_INFO "Len mismatch %d, %d\n", param_len,
-			       param->u.crypt.key_len);
+		netdev_info(ieee->dev, "Len mismatch %d, %d\n", param_len,
+			    param->u.crypt.key_len);
 		return -EINVAL;
 	}
 	if (is_broadcast_ether_addr(param->sta_addr)) {
@@ -3410,8 +3410,8 @@ static int rtllib_wpa_set_encryption(struct rtllib_device *ieee,
 		ops = lib80211_get_crypto_ops(param->u.crypt.alg);
 	}
 	if (ops == NULL) {
-		printk(KERN_INFO "unknown crypto alg '%s'\n",
-		       param->u.crypt.alg);
+		netdev_info(ieee->dev, "unknown crypto alg '%s'\n",
+			    param->u.crypt.alg);
 		param->u.crypt.err = IEEE_CRYPT_ERR_UNKNOWN_ALG;
 		ret = -EINVAL;
 		goto done;
@@ -3421,12 +3421,11 @@ static int rtllib_wpa_set_encryption(struct rtllib_device *ieee,
 
 		lib80211_crypt_delayed_deinit(&ieee->crypt_info, crypt);
 
-		new_crypt = kmalloc(sizeof(*new_crypt), GFP_KERNEL);
+		new_crypt = kzalloc(sizeof(*new_crypt), GFP_KERNEL);
 		if (new_crypt == NULL) {
 			ret = -ENOMEM;
 			goto done;
 		}
-		memset(new_crypt, 0, sizeof(struct lib80211_crypt_data));
 		new_crypt->ops = ops;
 		if (new_crypt->ops)
 			new_crypt->priv =
@@ -3446,7 +3445,7 @@ static int rtllib_wpa_set_encryption(struct rtllib_device *ieee,
 	    (*crypt)->ops->set_key(param->u.crypt.key,
 	    param->u.crypt.key_len, param->u.crypt.seq,
 	    (*crypt)->priv) < 0) {
-		printk(KERN_INFO "key setting failed\n");
+		netdev_info(ieee->dev, "key setting failed\n");
 		param->u.crypt.err = IEEE_CRYPT_ERR_KEY_SET_FAILED;
 		ret = -EINVAL;
 		goto done;
@@ -3486,12 +3485,13 @@ static int rtllib_wpa_set_encryption(struct rtllib_device *ieee,
 	 * generate new IEEE 802.11 authentication which may end up in looping
 	 * with IEEE 802.1X.  If your hardware requires a reset after WEP
 	 * configuration (for example... Prism2), implement the reset_port in
-	 * the callbacks structures used to initialize the 802.11 stack. */
+	 * the callbacks structures used to initialize the 802.11 stack.
+	 */
 	if (ieee->reset_on_keychange &&
 	    ieee->iw_mode != IW_MODE_INFRA &&
 	    ieee->reset_port &&
 	    ieee->reset_port(ieee->dev)) {
-		printk(KERN_INFO "reset_port failed\n");
+		netdev_info(ieee->dev, "reset_port failed\n");
 		param->u.crypt.err = IEEE_CRYPT_ERR_CARD_CONF_FAILED;
 		return -EINVAL;
 	}
@@ -3634,8 +3634,8 @@ int rtllib_wpa_supplicant_ioctl(struct rtllib_device *ieee, struct iw_point *p,
 		break;
 
 	default:
-		printk(KERN_INFO "Unknown WPA supplicant request: %d\n",
-		       param->cmd);
+		netdev_info(ieee->dev, "Unknown WPA supplicant request: %d\n",
+			    param->cmd);
 		ret = -EOPNOTSUPP;
 		break;
 	}
@@ -3749,9 +3749,9 @@ void notify_wx_assoc_event(struct rtllib_device *ieee)
 		       ETH_ALEN);
 	else {
 
-		printk(KERN_INFO "%s(): Tell user space disconnected\n",
-		       __func__);
-		memset(wrqu.ap_addr.sa_data, 0, ETH_ALEN);
+		netdev_info(ieee->dev, "%s(): Tell user space disconnected\n",
+			    __func__);
+		eth_zero_addr(wrqu.ap_addr.sa_data);
 	}
 	wireless_send_event(ieee->dev, SIOCGIWAP, &wrqu, NULL);
 }
diff --git a/drivers/staging/rtl8192e/rtllib_softmac_wx.c b/drivers/staging/rtl8192e/rtllib_softmac_wx.c
index 835f3d78a16f..9715a793fd37 100644
--- a/drivers/staging/rtl8192e/rtllib_softmac_wx.c
+++ b/drivers/staging/rtl8192e/rtllib_softmac_wx.c
@@ -120,7 +120,7 @@ int rtllib_wx_get_wap(struct rtllib_device *ieee,
 		ieee->state != RTLLIB_LINKED_SCANNING &&
 		ieee->wap_set == 0)
 
-		memset(wrqu->ap_addr.sa_data, 0, ETH_ALEN);
+		eth_zero_addr(wrqu->ap_addr.sa_data);
 	else
 		memcpy(wrqu->ap_addr.sa_data,
 		       ieee->current_network.bssid, ETH_ALEN);
@@ -527,8 +527,8 @@ int rtllib_wx_set_rawtx(struct rtllib_device *ieee,
 	else
 		ieee->raw_tx = 0;
 
-	printk(KERN_INFO"raw TX is %s\n",
-	      ieee->raw_tx ? "enabled" : "disabled");
+	netdev_info(ieee->dev, "raw TX is %s\n",
+		    ieee->raw_tx ? "enabled" : "disabled");
 
 	if (ieee->iw_mode == IW_MODE_MONITOR) {
 		if (prev == 0 && ieee->raw_tx) {
@@ -575,8 +575,9 @@ int rtllib_wx_set_power(struct rtllib_device *ieee,
 	if ((!ieee->sta_wake_up) ||
 	    (!ieee->enter_sleep_state) ||
 	    (!ieee->ps_is_queue_empty)) {
-		RTLLIB_DEBUG(RTLLIB_DL_ERR, "%s(): PS mode is tried to be use "
-			     "but driver missed a callback\n\n", __func__);
+		RTLLIB_DEBUG(RTLLIB_DL_ERR,
+			     "%s(): PS mode is tried to be use but driver missed a callback\n\n",
+			     __func__);
 		return -1;
 	}
 
diff --git a/drivers/staging/rtl8192e/rtllib_tx.c b/drivers/staging/rtl8192e/rtllib_tx.c
index 4f68ffe41475..3b159638bba2 100644
--- a/drivers/staging/rtl8192e/rtllib_tx.c
+++ b/drivers/staging/rtl8192e/rtllib_tx.c
@@ -53,102 +53,99 @@
 
 #include "rtllib.h"
 
-/*
-
-
-802.11 Data Frame
-
-
-802.11 frame_control for data frames - 2 bytes
-     ,-----------------------------------------------------------------------------------------.
-bits | 0  |  1  |  2  |  3  |  4  |  5  |  6  |  7  |  8  |  9  |  a  |  b  |  c  |  d  |  e   |
-     |----|-----|-----|-----|-----|-----|-----|-----|-----|-----|-----|-----|-----|-----|------|
-val  | 0  |  0  |  0  |  1  |  x  |  0  |  0  |  0  |  1  |  0  |  x  |  x  |  x  |  x  |  x   |
-     |----|-----|-----|-----|-----|-----|-----|-----|-----|-----|-----|-----|-----|-----|------|
-desc | ^-ver-^  |  ^type-^  |  ^-----subtype-----^  | to  |from |more |retry| pwr |more |wep   |
-     |	  |	   | x=0 data,x=1 data+ack | DS  | DS  |frag |     | mgm |data |      |
-     '-----------------------------------------------------------------------------------------'
-						    /\
-						    |
-802.11 Data Frame				   |
-	   ,--------- 'ctrl' expands to >-----------'
-	  |
-      ,--'---,-------------------------------------------------------------.
-Bytes |  2   |  2   |    6    |    6    |    6    |  2   | 0..2312 |   4  |
-      |------|------|---------|---------|---------|------|---------|------|
-Desc. | ctrl | dura |  DA/RA  |   TA    |    SA   | Sequ |  Frame  |  fcs |
-      |      | tion | (BSSID) |	 |	 | ence |  data   |      |
-      `--------------------------------------------------|	 |------'
-Total: 28 non-data bytes				 `----.----'
-							      |
-       .- 'Frame data' expands to <---------------------------'
-       |
-       V
-      ,---------------------------------------------------.
-Bytes |  1   |  1   |    1    |    3     |  2   |  0-2304 |
-      |------|------|---------|----------|------|---------|
-Desc. | SNAP | SNAP | Control |Eth Tunnel| Type | IP      |
-      | DSAP | SSAP |	 |	  |      | Packet  |
-      | 0xAA | 0xAA |0x03 (UI)|0x00-00-F8|      |	 |
-      `-----------------------------------------|	 |
-Total: 8 non-data bytes			 `----.----'
-						     |
-       .- 'IP Packet' expands, if WEP enabled, to <--'
-       |
-       V
-      ,-----------------------.
-Bytes |  4  |   0-2296  |  4  |
-      |-----|-----------|-----|
-Desc. | IV  | Encrypted | ICV |
-      |     | IP Packet |     |
-      `-----------------------'
-Total: 8 non-data bytes
-
-
-802.3 Ethernet Data Frame
-
-      ,-----------------------------------------.
-Bytes |   6   |   6   |  2   |  Variable |   4  |
-      |-------|-------|------|-----------|------|
-Desc. | Dest. | Source| Type | IP Packet |  fcs |
-      |  MAC  |  MAC  |      |	   |      |
-      `-----------------------------------------'
-Total: 18 non-data bytes
-
-In the event that fragmentation is required, the incoming payload is split into
-N parts of size ieee->fts.  The first fragment contains the SNAP header and the
-remaining packets are just data.
-
-If encryption is enabled, each fragment payload size is reduced by enough space
-to add the prefix and postfix (IV and ICV totalling 8 bytes in the case of WEP)
-So if you have 1500 bytes of payload with ieee->fts set to 500 without
-encryption it will take 3 frames.  With WEP it will take 4 frames as the
-payload of each frame is reduced to 492 bytes.
-
-* SKB visualization
-*
-*  ,- skb->data
-* |
-* |    ETHERNET HEADER	,-<-- PAYLOAD
-* |			   |     14 bytes from skb->data
-* |  2 bytes for Type --> ,T. |     (sizeof ethhdr)
-* |		       | | |
-* |,-Dest.--. ,--Src.---. | | |
-* |  6 bytes| | 6 bytes | | | |
-* v	 | |	 | | | |
-* 0	 | v       1 | v | v	   2
-* 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5
-*     ^     | ^	 | ^ |
-*     |     | |	 | | |
-*     |     | |	 | `T' <---- 2 bytes for Type
-*     |     | |	 |
-*     |     | '---SNAP--' <-------- 6 bytes for SNAP
-*     |     |
-*     `-IV--' <-------------------- 4 bytes for IV (WEP)
-*
-*      SNAP HEADER
-*
-*/
+/* 802.11 Data Frame
+ *
+ *
+ * 802.11 frame_control for data frames - 2 bytes
+ *      ,-----------------------------------------------------------------------------------------.
+ * bits | 0  |  1  |  2  |  3  |  4  |  5  |  6  |  7  |  8  |  9  |  a  |  b  |  c  |  d  |  e   |
+ *      |----|-----|-----|-----|-----|-----|-----|-----|-----|-----|-----|-----|-----|-----|------|
+ * val  | 0  |  0  |  0  |  1  |  x  |  0  |  0  |  0  |  1  |  0  |  x  |  x  |  x  |  x  |  x   |
+ *      |----|-----|-----|-----|-----|-----|-----|-----|-----|-----|-----|-----|-----|-----|------|
+ * desc | ^-ver-^  |  ^type-^  |  ^-----subtype-----^  | to  |from |more |retry| pwr |more |wep   |
+ *      |          |           | x=0 data,x=1 data+ack | DS  | DS  |frag |     | mgm |data |      |
+ *      '-----------------------------------------------------------------------------------------'
+ *                                                   /\
+ *                                                   |
+ * 802.11 Data Frame                                 |
+ *          ,--------- 'ctrl' expands to >-----------'
+ *          |
+ *       ,--'---,-------------------------------------------------------------.
+ * Bytes |  2   |  2   |    6    |    6    |    6    |  2   | 0..2312 |   4  |
+ *       |------|------|---------|---------|---------|------|---------|------|
+ * Desc. | ctrl | dura |  DA/RA  |   TA    |    SA   | Sequ |  Frame  |  fcs |
+ *       |      | tion | (BSSID) |         |         | ence |  data   |      |
+ *       `--------------------------------------------------|         |------'
+ * Total: 28 non-data bytes                                 `----.----'
+ *                                                               |
+ *        .- 'Frame data' expands to <---------------------------'
+ *        |
+ *        V
+ *       ,---------------------------------------------------.
+ * Bytes |  1   |  1   |    1    |    3     |  2   |  0-2304 |
+ *       |------|------|---------|----------|------|---------|
+ * Desc. | SNAP | SNAP | Control |Eth Tunnel| Type | IP      |
+ *       | DSAP | SSAP |         |          |      | Packet  |
+ *       | 0xAA | 0xAA |0x03 (UI)|0x00-00-F8|      |         |
+ *       `-----------------------------------------|         |
+ * Total: 8 non-data bytes                         `----.----'
+ *                                                      |
+ *        .- 'IP Packet' expands, if WEP enabled, to <--'
+ *        |
+ *        V
+ *       ,-----------------------.
+ * Bytes |  4  |   0-2296  |  4  |
+ *       |-----|-----------|-----|
+ * Desc. | IV  | Encrypted | ICV |
+ *       |     | IP Packet |     |
+ *       `-----------------------'
+ * Total: 8 non-data bytes
+ *
+ *
+ * 802.3 Ethernet Data Frame
+ *
+ *       ,-----------------------------------------.
+ * Bytes |   6   |   6   |  2   |  Variable |   4  |
+ *       |-------|-------|------|-----------|------|
+ * Desc. | Dest. | Source| Type | IP Packet |  fcs |
+ *       |  MAC  |  MAC  |      |	   |      |
+ *       `-----------------------------------------'
+ * Total: 18 non-data bytes
+ *
+ * In the event that fragmentation is required, the incoming payload is split into
+ * N parts of size ieee->fts.  The first fragment contains the SNAP header and the
+ * remaining packets are just data.
+ *
+ * If encryption is enabled, each fragment payload size is reduced by enough space
+ * to add the prefix and postfix (IV and ICV totalling 8 bytes in the case of WEP)
+ * So if you have 1500 bytes of payload with ieee->fts set to 500 without
+ * encryption it will take 3 frames.  With WEP it will take 4 frames as the
+ * payload of each frame is reduced to 492 bytes.
+ *
+ * SKB visualization
+ *
+ * ,- skb->data
+ * |
+ * |    ETHERNET HEADER        ,-<-- PAYLOAD
+ * |                           |     14 bytes from skb->data
+ * |  2 bytes for Type --> ,T. |     (sizeof ethhdr)
+ * |                       | | |
+ * |,-Dest.--. ,--Src.---. | | |
+ * |  6 bytes| | 6 bytes | | | |
+ * v         | |         | | | |
+ * 0         | v       1 | v | v           2
+ * 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5
+ *     ^     | ^         | ^ |
+ *     |     | |         | | |
+ *     |     | |         | `T' <---- 2 bytes for Type
+ *     |     | |         |
+ *     |     | '---SNAP--' <-------- 6 bytes for SNAP
+ *     |     |
+ *     `-IV--' <-------------------- 4 bytes for IV (WEP)
+ *
+ *      SNAP HEADER
+ *
+ */
 
 static u8 P802_1H_OUI[P80211_OUI_LEN] = { 0x00, 0x00, 0xf8 };
 static u8 RFC1042_OUI[P80211_OUI_LEN] = { 0x00, 0x00, 0x00 };
@@ -185,14 +182,17 @@ int rtllib_encrypt_fragment(struct rtllib_device *ieee, struct sk_buff *frag,
 	crypt = ieee->crypt_info.crypt[ieee->crypt_info.tx_keyidx];
 
 	if (!(crypt && crypt->ops)) {
-		printk(KERN_INFO "=========>%s(), crypt is null\n", __func__);
+		netdev_info(ieee->dev, "=========>%s(), crypt is null\n",
+			    __func__);
 		return -1;
 	}
 	/* To encrypt, frame format is:
-	 * IV (4 bytes), clear payload (including SNAP), ICV (4 bytes) */
+	 * IV (4 bytes), clear payload (including SNAP), ICV (4 bytes)
+	 */
 
 	/* Host-based IEEE 802.11 fragmentation for TX is not yet supported, so
-	 * call both MSDU and MPDU encryption functions from here. */
+	 * call both MSDU and MPDU encryption functions from here.
+	 */
 	atomic_inc(&crypt->refcnt);
 	res = 0;
 	if (crypt->ops->encrypt_msdu)
@@ -202,8 +202,8 @@ int rtllib_encrypt_fragment(struct rtllib_device *ieee, struct sk_buff *frag,
 
 	atomic_dec(&crypt->refcnt);
 	if (res < 0) {
-		printk(KERN_INFO "%s: Encryption failed: len=%d.\n",
-		       ieee->dev->name, frag->len);
+		netdev_info(ieee->dev, "%s: Encryption failed: len=%d.\n",
+			    ieee->dev->name, frag->len);
 		ieee->ieee_stats.tx_discards++;
 		return -1;
 	}
@@ -311,7 +311,7 @@ static void rtllib_tx_query_agg_cap(struct rtllib_device *ieee,
 	if (pHTInfo->bCurrentAMPDUEnable) {
 		if (!GetTs(ieee, (struct ts_common_info **)(&pTxTs), hdr->addr1,
 		    skb->priority, TX_DIR, true)) {
-			printk(KERN_INFO "%s: can't get TS\n", __func__);
+			netdev_info(ieee->dev, "%s: can't get TS\n", __func__);
 			return;
 		}
 		if (pTxTs->TxAdmittedBARecord.bValid == false) {
@@ -412,6 +412,8 @@ static void rtllib_query_protectionmode(struct rtllib_device *ieee,
 					struct cb_desc *tcb_desc,
 					struct sk_buff *skb)
 {
+	struct rt_hi_throughput *pHTInfo;
+
 	tcb_desc->bRTSSTBC			= false;
 	tcb_desc->bRTSUseShortGI		= false;
 	tcb_desc->bCTSEnable			= false;
@@ -434,50 +436,50 @@ static void rtllib_query_protectionmode(struct rtllib_device *ieee,
 			tcb_desc->rts_rate = MGN_24M;
 		}
 		return;
-	} else {
-		struct rt_hi_throughput *pHTInfo = ieee->pHTInfo;
+	}
 
-		while (true) {
-			if (pHTInfo->IOTAction & HT_IOT_ACT_FORCED_CTS2SELF) {
-				tcb_desc->bCTSEnable	= true;
-				tcb_desc->rts_rate  =	MGN_24M;
-				tcb_desc->bRTSEnable = true;
-				break;
-			} else if (pHTInfo->IOTAction & (HT_IOT_ACT_FORCED_RTS |
-				   HT_IOT_ACT_PURE_N_MODE)) {
-				tcb_desc->bRTSEnable = true;
-				tcb_desc->rts_rate  =	MGN_24M;
-				break;
-			}
-			if (ieee->current_network.buseprotection) {
-				tcb_desc->bRTSEnable = true;
-				tcb_desc->bCTSEnable = true;
-				tcb_desc->rts_rate = MGN_24M;
-				break;
-			}
-			if (pHTInfo->bCurrentHTSupport  && pHTInfo->bEnableHT) {
-				u8 HTOpMode = pHTInfo->CurrentOpMode;
-
-				if ((pHTInfo->bCurBW40MHz && (HTOpMode == 2 ||
-				     HTOpMode == 3)) ||
-				     (!pHTInfo->bCurBW40MHz && HTOpMode == 3)) {
-					tcb_desc->rts_rate = MGN_24M;
-					tcb_desc->bRTSEnable = true;
-					break;
-				}
-			}
-			if (skb->len > ieee->rts) {
+	pHTInfo = ieee->pHTInfo;
+
+	while (true) {
+		if (pHTInfo->IOTAction & HT_IOT_ACT_FORCED_CTS2SELF) {
+			tcb_desc->bCTSEnable	= true;
+			tcb_desc->rts_rate  =	MGN_24M;
+			tcb_desc->bRTSEnable = true;
+			break;
+		} else if (pHTInfo->IOTAction & (HT_IOT_ACT_FORCED_RTS |
+			   HT_IOT_ACT_PURE_N_MODE)) {
+			tcb_desc->bRTSEnable = true;
+			tcb_desc->rts_rate  =	MGN_24M;
+			break;
+		}
+		if (ieee->current_network.buseprotection) {
+			tcb_desc->bRTSEnable = true;
+			tcb_desc->bCTSEnable = true;
+			tcb_desc->rts_rate = MGN_24M;
+			break;
+		}
+		if (pHTInfo->bCurrentHTSupport  && pHTInfo->bEnableHT) {
+			u8 HTOpMode = pHTInfo->CurrentOpMode;
+
+			if ((pHTInfo->bCurBW40MHz && (HTOpMode == 2 ||
+			     HTOpMode == 3)) ||
+			     (!pHTInfo->bCurBW40MHz && HTOpMode == 3)) {
 				tcb_desc->rts_rate = MGN_24M;
 				tcb_desc->bRTSEnable = true;
 				break;
 			}
-			if (tcb_desc->bAMPDUEnable) {
-				tcb_desc->rts_rate = MGN_24M;
-				tcb_desc->bRTSEnable = false;
-				break;
-			}
-			goto NO_PROTECTION;
 		}
+		if (skb->len > ieee->rts) {
+			tcb_desc->rts_rate = MGN_24M;
+			tcb_desc->bRTSEnable = true;
+			break;
+		}
+		if (tcb_desc->bAMPDUEnable) {
+			tcb_desc->rts_rate = MGN_24M;
+			tcb_desc->bRTSEnable = false;
+			break;
+		}
+		goto NO_PROTECTION;
 	}
 	if (ieee->current_network.capability & WLAN_CAPABILITY_SHORT_PREAMBLE)
 		tcb_desc->bUseShortPreamble = true;
@@ -549,6 +551,17 @@ static int wme_downgrade_ac(struct sk_buff *skb)
 	}
 }
 
+static u8 rtllib_current_rate(struct rtllib_device *ieee)
+{
+	if (ieee->mode & IEEE_MODE_MASK)
+		return ieee->rate;
+
+	if (ieee->HTCurrentOperaRate)
+		return ieee->HTCurrentOperaRate;
+	else
+		return ieee->rate & 0x7F;
+}
+
 int rtllib_xmit_inter(struct sk_buff *skb, struct net_device *dev)
 {
 	struct rtllib_device *ieee = (struct rtllib_device *)
@@ -577,21 +590,21 @@ int rtllib_xmit_inter(struct sk_buff *skb, struct net_device *dev)
 	spin_lock_irqsave(&ieee->lock, flags);
 
 	/* If there is no driver handler to take the TXB, don't bother
-	 * creating it... */
+	 * creating it...
+	 */
 	if ((!ieee->hard_start_xmit && !(ieee->softmac_features &
 	   IEEE_SOFTMAC_TX_QUEUE)) ||
 	   ((!ieee->softmac_data_hard_start_xmit &&
 	   (ieee->softmac_features & IEEE_SOFTMAC_TX_QUEUE)))) {
-		printk(KERN_WARNING "%s: No xmit handler.\n",
-		       ieee->dev->name);
+		netdev_warn(ieee->dev, "No xmit handler.\n");
 		goto success;
 	}
 
 
 	if (likely(ieee->raw_tx == 0)) {
 		if (unlikely(skb->len < SNAP_SIZE + sizeof(u16))) {
-			printk(KERN_WARNING "%s: skb too small (%d).\n",
-			ieee->dev->name, skb->len);
+			netdev_warn(ieee->dev, "skb too small (%d).\n",
+				    skb->len);
 			goto success;
 		}
 		/* Save source and destination addresses */
@@ -604,9 +617,8 @@ int rtllib_xmit_inter(struct sk_buff *skb, struct net_device *dev)
 		if (ieee->iw_mode == IW_MODE_MONITOR) {
 			txb = rtllib_alloc_txb(1, skb->len, GFP_ATOMIC);
 			if (unlikely(!txb)) {
-				printk(KERN_WARNING "%s: Could not allocate "
-				       "TXB\n",
-				ieee->dev->name);
+				netdev_warn(ieee->dev,
+					    "Could not allocate TXB\n");
 				goto failed;
 			}
 
@@ -636,8 +648,8 @@ int rtllib_xmit_inter(struct sk_buff *skb, struct net_device *dev)
 					}
 				}
 			} else if (ETH_P_ARP == ether_type) {
-				printk(KERN_INFO "=================>DHCP "
-				       "Protocol start tx ARP pkt!!\n");
+				netdev_info(ieee->dev,
+					    "=================>DHCP Protocol start tx ARP pkt!!\n");
 				bdhcp = true;
 				ieee->LPSDelayCnt =
 					 ieee->current_network.tim.tim_count;
@@ -680,7 +692,8 @@ int rtllib_xmit_inter(struct sk_buff *skb, struct net_device *dev)
 		if (ieee->iw_mode == IW_MODE_INFRA) {
 			fc |= RTLLIB_FCTL_TODS;
 			/* To DS: Addr1 = BSSID, Addr2 = SA,
-			Addr3 = DA */
+			 * Addr3 = DA
+			 */
 			memcpy(&header.addr1, ieee->current_network.bssid,
 			       ETH_ALEN);
 			memcpy(&header.addr2, &src, ETH_ALEN);
@@ -691,7 +704,8 @@ int rtllib_xmit_inter(struct sk_buff *skb, struct net_device *dev)
 				memcpy(&header.addr3, &dest, ETH_ALEN);
 		} else if (ieee->iw_mode == IW_MODE_ADHOC) {
 			/* not From/To DS: Addr1 = DA, Addr2 = SA,
-			Addr3 = BSSID */
+			 * Addr3 = BSSID
+			 */
 			memcpy(&header.addr1, dest, ETH_ALEN);
 			memcpy(&header.addr2, src, ETH_ALEN);
 			memcpy(&header.addr3, ieee->current_network.bssid,
@@ -703,7 +717,8 @@ int rtllib_xmit_inter(struct sk_buff *skb, struct net_device *dev)
 		header.frame_ctl = cpu_to_le16(fc);
 
 		/* Determine fragmentation size based on destination (multicast
-		* and broadcast are not fragmented) */
+		 * and broadcast are not fragmented)
+		 */
 		if (bIsMulticast) {
 			frag_size = MAX_FRAG_THRESHOLD;
 			qos_ctl |= QOS_CTL_NOTCONTAIN_ACK;
@@ -717,10 +732,11 @@ int rtllib_xmit_inter(struct sk_buff *skb, struct net_device *dev)
 
 		/* in case we are a client verify acm is not set for this ac */
 		while (unlikely(ieee->wmm_acm & (0x01 << skb->priority))) {
-			printk(KERN_INFO "skb->priority = %x\n", skb->priority);
+			netdev_info(ieee->dev, "skb->priority = %x\n",
+				    skb->priority);
 			if (wme_downgrade_ac(skb))
 				break;
-			printk(KERN_INFO "converted skb->priority = %x\n",
+			netdev_info(ieee->dev, "converted skb->priority = %x\n",
 			       skb->priority);
 		 }
 			qos_ctl |= skb->priority;
@@ -731,14 +747,16 @@ int rtllib_xmit_inter(struct sk_buff *skb, struct net_device *dev)
 		/* Determine amount of payload per fragment.  Regardless of if
 		 * this stack is providing the full 802.11 header, one will
 		 * eventually be affixed to this fragment -- so we must account
-		 * for it when determining the amount of payload space. */
+		 * for it when determining the amount of payload space.
+		 */
 		bytes_per_frag = frag_size - hdr_len;
 		if (ieee->config &
 		   (CFG_RTLLIB_COMPUTE_FCS | CFG_RTLLIB_RESERVE_FCS))
 			bytes_per_frag -= RTLLIB_FCS_LEN;
 
 		/* Each fragment may need to have room for encrypting
-		 * pre/postfix */
+		 * pre/postfix
+		 */
 		if (encrypt) {
 			bytes_per_frag -= crypt->ops->extra_mpdu_prefix_len +
 				crypt->ops->extra_mpdu_postfix_len +
@@ -746,7 +764,8 @@ int rtllib_xmit_inter(struct sk_buff *skb, struct net_device *dev)
 				crypt->ops->extra_msdu_postfix_len;
 		}
 		/* Number of fragments is the total bytes_per_frag /
-		* payload_per_fragment */
+		 * payload_per_fragment
+		 */
 		nr_frags = bytes / bytes_per_frag;
 		bytes_last_frag = bytes % bytes_per_frag;
 		if (bytes_last_frag)
@@ -756,12 +775,12 @@ int rtllib_xmit_inter(struct sk_buff *skb, struct net_device *dev)
 
 		/* When we allocate the TXB we allocate enough space for the
 		 * reserve and full fragment bytes (bytes_per_frag doesn't
-		 * include prefix, postfix, header, FCS, etc.) */
+		 * include prefix, postfix, header, FCS, etc.)
+		 */
 		txb = rtllib_alloc_txb(nr_frags, frag_size +
 				       ieee->tx_headroom, GFP_ATOMIC);
 		if (unlikely(!txb)) {
-			printk(KERN_WARNING "%s: Could not allocate TXB\n",
-			ieee->dev->name);
+			netdev_warn(ieee->dev, "Could not allocate TXB\n");
 			goto failed;
 		}
 		txb->encrypted = encrypt;
@@ -801,7 +820,8 @@ int rtllib_xmit_inter(struct sk_buff *skb, struct net_device *dev)
 			memcpy(frag_hdr, &header, hdr_len);
 
 			/* If this is not the last fragment, then add the
-			 * MOREFRAGS bit to the frame control */
+			 * MOREFRAGS bit to the frame control
+			 */
 			if (i != nr_frags - 1) {
 				frag_hdr->frame_ctl = cpu_to_le16(
 					fc | RTLLIB_FCTL_MOREFRAGS);
@@ -836,7 +856,8 @@ int rtllib_xmit_inter(struct sk_buff *skb, struct net_device *dev)
 
 			/* Encryption routine will move the header forward in
 			 * order to insert the IV between the header and the
-			 * payload */
+			 * payload
+			 */
 			if (encrypt)
 				rtllib_encrypt_fragment(ieee, skb_frag,
 							hdr_len);
@@ -858,15 +879,14 @@ int rtllib_xmit_inter(struct sk_buff *skb, struct net_device *dev)
 		}
 	} else {
 		if (unlikely(skb->len < sizeof(struct rtllib_hdr_3addr))) {
-			printk(KERN_WARNING "%s: skb too small (%d).\n",
-			ieee->dev->name, skb->len);
+			netdev_warn(ieee->dev, "skb too small (%d).\n",
+				    skb->len);
 			goto success;
 		}
 
 		txb = rtllib_alloc_txb(1, skb->len, GFP_ATOMIC);
 		if (!txb) {
-			printk(KERN_WARNING "%s: Could not allocate TXB\n",
-			ieee->dev->name);
+			netdev_warn(ieee->dev, "Could not allocate TXB\n");
 			goto failed;
 		}
 
@@ -906,8 +926,7 @@ int rtllib_xmit_inter(struct sk_buff *skb, struct net_device *dev)
 			if (tcb_desc->bMulticast ||  tcb_desc->bBroadcast)
 				tcb_desc->data_rate = ieee->basic_rate;
 			else
-				tcb_desc->data_rate = CURRENT_RATE(ieee->mode,
-					ieee->rate, ieee->HTCurrentOperaRate);
+				tcb_desc->data_rate = rtllib_current_rate(ieee);
 
 			if (bdhcp) {
 				if (ieee->pHTInfo->IOTAction &
diff --git a/drivers/staging/rtl8192e/rtllib_wx.c b/drivers/staging/rtl8192e/rtllib_wx.c
index 9e0f975c152f..6234aae5b069 100644
--- a/drivers/staging/rtl8192e/rtllib_wx.c
+++ b/drivers/staging/rtl8192e/rtllib_wx.c
@@ -32,7 +32,7 @@
 #include <linux/wireless.h>
 #include <linux/kmod.h>
 #include <linux/module.h>
-
+#include <linux/etherdevice.h>
 #include "rtllib.h"
 struct modes_unit {
 	char *mode_string;
@@ -65,7 +65,7 @@ static inline char *rtl819x_translate_scan(struct rtllib_device *ieee,
 	/* First entry *MUST* be the AP MAC address */
 	iwe.cmd = SIOCGIWAP;
 	iwe.u.ap_addr.sa_family = ARPHRD_ETHER;
-	memcpy(iwe.u.ap_addr.sa_data, network->bssid, ETH_ALEN);
+	ether_addr_copy(iwe.u.ap_addr.sa_data, network->bssid);
 	start = iwe_stream_add_event_rsl(info, start, stop,
 					 &iwe, IW_EV_ADDR_LEN);
 	/* Remaining entries will be displayed in the order we provide them */
@@ -74,7 +74,7 @@ static inline char *rtl819x_translate_scan(struct rtllib_device *ieee,
 	iwe.cmd = SIOCGIWESSID;
 	iwe.u.data.flags = 1;
 	if (network->ssid_len > 0) {
-		iwe.u.data.length = min(network->ssid_len, (u8)32);
+		iwe.u.data.length = min_t(u8, network->ssid_len, 32);
 		start = iwe_stream_add_point_rsl(info, start, stop, &iwe,
 						 network->ssid);
 	} else if (network->hidden_ssid_len == 0) {
@@ -82,7 +82,7 @@ static inline char *rtl819x_translate_scan(struct rtllib_device *ieee,
 		start = iwe_stream_add_point_rsl(info, start, stop,
 						 &iwe, "<hidden>");
 	} else {
-		iwe.u.data.length = min(network->hidden_ssid_len, (u8)32);
+		iwe.u.data.length = min_t(u8, network->hidden_ssid_len, 32);
 		start = iwe_stream_add_point_rsl(info, start, stop, &iwe,
 						 network->hidden_ssid);
 	}
@@ -113,8 +113,6 @@ static inline char *rtl819x_translate_scan(struct rtllib_device *ieee,
 
 	/* Add frequency/channel */
 	iwe.cmd = SIOCGIWFREQ;
-/*	iwe.u.freq.m = rtllib_frequency(network->channel, network->mode);
-	iwe.u.freq.e = 3; */
 	iwe.u.freq.m = network->channel;
 	iwe.u.freq.e = 0;
 	iwe.u.freq.i = 0;
@@ -241,7 +239,8 @@ static inline char *rtl819x_translate_scan(struct rtllib_device *ieee,
 	}
 
 	/* Add EXTRA: Age to display seconds since last beacon/probe response
-	 * for given network. */
+	 * for given network.
+	 */
 	iwe.cmd = IWEVCUSTOM;
 	p = custom;
 	p += snprintf(p, MAX_CUSTOM_LEN - (p - custom),
@@ -282,8 +281,7 @@ int rtllib_wx_get_scan(struct rtllib_device *ieee,
 			ev = rtl819x_translate_scan(ieee, ev, stop, network,
 						    info);
 		else
-			RTLLIB_DEBUG_SCAN("Not showing network '%s ("
-				" %pM)' due to age (%lums).\n",
+			RTLLIB_DEBUG_SCAN("Not showing network '%s ( %pM)' due to age (%lums).\n",
 				escape_essid(network->ssid,
 					     network->ssid_len),
 				network->bssid,
@@ -338,13 +336,14 @@ int rtllib_wx_set_encode(struct rtllib_device *ieee,
 			RTLLIB_DEBUG_WX("Disabling encryption.\n");
 
 		/* Check all the keys to see if any are still configured,
-		 * and if no key index was provided, de-init them all */
+		 * and if no key index was provided, de-init them all
+		 */
 		for (i = 0; i < NUM_WEP_KEYS; i++) {
 			if (ieee->crypt_info.crypt[i] != NULL) {
 				if (key_provided)
 					break;
 				lib80211_crypt_delayed_deinit(&ieee->crypt_info,
-							    &ieee->crypt_info.crypt[i]);
+						    &ieee->crypt_info.crypt[i]);
 			}
 		}
 
@@ -365,7 +364,8 @@ int rtllib_wx_set_encode(struct rtllib_device *ieee,
 	if (*crypt != NULL && (*crypt)->ops != NULL &&
 	    strcmp((*crypt)->ops->name, "R-WEP") != 0) {
 		/* changing to use WEP; deinit previously used algorithm
-		 * on this key */
+		 * on this key
+		 */
 		lib80211_crypt_delayed_deinit(&ieee->crypt_info, crypt);
 	}
 
@@ -390,9 +390,9 @@ int rtllib_wx_set_encode(struct rtllib_device *ieee,
 			kfree(new_crypt);
 			new_crypt = NULL;
 
-			printk(KERN_WARNING "%s: could not initialize WEP: "
-			       "load module rtllib_crypt_wep\n",
-			       dev->name);
+			netdev_warn(dev,
+				    "%s: could not initialize WEP: load module rtllib_crypt_wep\n",
+				    dev->name);
 			return -EOPNOTSUPP;
 		}
 		*crypt = new_crypt;
@@ -413,7 +413,8 @@ int rtllib_wx_set_encode(struct rtllib_device *ieee,
 				       (*crypt)->priv);
 		sec.flags |= (1 << key);
 		/* This ensures a key will be activated if no key is
-		 * explicitly set */
+		 * explicitly set
+		 */
 		if (key == sec.active_key)
 			sec.flags |= SEC_ACTIVE_KEY;
 		ieee->crypt_info.tx_keyidx = key;
@@ -423,11 +424,9 @@ int rtllib_wx_set_encode(struct rtllib_device *ieee,
 					     NULL, (*crypt)->priv);
 		if (len == 0) {
 			/* Set a default key of all 0 */
-			printk(KERN_INFO "Setting key %d to all zero.\n",
+			netdev_info(ieee->dev, "Setting key %d to all zero.\n",
 					   key);
 
-			RTLLIB_DEBUG_WX("Setting key %d to all zero.\n",
-					   key);
 			memset(sec.keys[key], 0, 13);
 			(*crypt)->ops->set_key(sec.keys[key], 13, NULL,
 					       (*crypt)->priv);
@@ -437,8 +436,8 @@ int rtllib_wx_set_encode(struct rtllib_device *ieee,
 
 		/* No key data - just set the default TX key index */
 		if (key_provided) {
-			RTLLIB_DEBUG_WX(
-				"Setting key %d to default Tx key.\n", key);
+			RTLLIB_DEBUG_WX("Setting key %d to default Tx key.\n",
+					key);
 			ieee->crypt_info.tx_keyidx = key;
 			sec.active_key = key;
 			sec.flags |= SEC_ACTIVE_KEY;
@@ -454,7 +453,8 @@ int rtllib_wx_set_encode(struct rtllib_device *ieee,
 			   "OPEN" : "SHARED KEY");
 
 	/* For now we just support WEP, so only set that security level...
-	 * TODO: When WPA is added this is one place that needs to change */
+	 * TODO: When WPA is added this is one place that needs to change
+	 */
 	sec.flags |= SEC_LEVEL;
 	sec.level = SEC_LEVEL_1; /* 40 and 104 bit WEP */
 
@@ -465,11 +465,12 @@ int rtllib_wx_set_encode(struct rtllib_device *ieee,
 	 * generate new IEEE 802.11 authentication which may end up in looping
 	 * with IEEE 802.1X.  If your hardware requires a reset after WEP
 	 * configuration (for example... Prism2), implement the reset_port in
-	 * the callbacks structures used to initialize the 802.11 stack. */
+	 * the callbacks structures used to initialize the 802.11 stack.
+	 */
 	if (ieee->reset_on_keychange &&
 	    ieee->iw_mode != IW_MODE_INFRA &&
 	    ieee->reset_port && ieee->reset_port(dev)) {
-		printk(KERN_DEBUG "%s: reset_port failed\n", dev->name);
+		netdev_dbg(dev, "%s: reset_port failed\n", dev->name);
 		return -EINVAL;
 	}
 	return 0;
@@ -596,7 +597,7 @@ int rtllib_wx_set_encode_ext(struct rtllib_device *ieee,
 		ret = -EINVAL;
 		goto done;
 	}
-	printk(KERN_INFO "alg name:%s\n", alg);
+	netdev_info(dev, "alg name:%s\n", alg);
 
 	ops = lib80211_get_crypto_ops(alg);
 	if (ops == NULL) {
@@ -608,9 +609,7 @@ int rtllib_wx_set_encode_ext(struct rtllib_device *ieee,
 		ops = lib80211_get_crypto_ops(alg);
 	}
 	if (ops == NULL) {
-		RTLLIB_DEBUG_WX("%s: unknown crypto alg %d\n",
-				   dev->name, ext->alg);
-		printk(KERN_INFO "========>unknown crypto alg %d\n", ext->alg);
+		netdev_info(dev, "========>unknown crypto alg %d\n", ext->alg);
 		ret = -EINVAL;
 		goto done;
 	}
@@ -641,8 +640,7 @@ int rtllib_wx_set_encode_ext(struct rtllib_device *ieee,
 	if (ext->key_len > 0 && (*crypt)->ops->set_key &&
 	    (*crypt)->ops->set_key(ext->key, ext->key_len, ext->rx_seq,
 				   (*crypt)->priv) < 0) {
-		RTLLIB_DEBUG_WX("%s: key setting failed\n", dev->name);
-		printk(KERN_INFO "key setting failed\n");
+		netdev_info(dev, "key setting failed\n");
 		ret = -EINVAL;
 		goto done;
 	}
@@ -759,9 +757,9 @@ int rtllib_wx_set_mlme(struct rtllib_device *ieee,
 
 	case IW_MLME_DISASSOC:
 		if (deauth)
-			printk(KERN_INFO "disauth packet !\n");
+			netdev_info(ieee->dev, "disauth packet !\n");
 		else
-			printk(KERN_INFO "dis associate packet!\n");
+			netdev_info(ieee->dev, "dis associate packet!\n");
 
 		ieee->cannot_notify = true;
 
@@ -797,8 +795,7 @@ int rtllib_wx_set_auth(struct rtllib_device *ieee,
 	case IW_AUTH_CIPHER_PAIRWISE:
 	case IW_AUTH_CIPHER_GROUP:
 	case IW_AUTH_KEY_MGMT:
-		/*
-		 * Host AP driver does not use these parameters and allows
+		/* Host AP driver does not use these parameters and allows
 		 * wpa_supplicant to control them internally.
 		 */
 		break;
diff --git a/drivers/staging/rtl8192u/ieee80211/dot11d.c b/drivers/staging/rtl8192u/ieee80211/dot11d.c
index 90ace791f2d7..82d60380bb40 100644
--- a/drivers/staging/rtl8192u/ieee80211/dot11d.c
+++ b/drivers/staging/rtl8192u/ieee80211/dot11d.c
@@ -6,7 +6,7 @@ void Dot11d_Init(struct ieee80211_device *ieee)
 {
 	PRT_DOT11D_INFO pDot11dInfo = GET_DOT11D_INFO(ieee);
 
-	pDot11dInfo->bEnabled = 0;
+	pDot11dInfo->bEnabled = false;
 
 	pDot11dInfo->State = DOT11D_STATE_NONE;
 	pDot11dInfo->CountryIeLen = 0;
diff --git a/drivers/staging/rtl8192u/ieee80211/dot11d.h b/drivers/staging/rtl8192u/ieee80211/dot11d.h
index bd75e29adc2c..8ae673b217d8 100644
--- a/drivers/staging/rtl8192u/ieee80211/dot11d.h
+++ b/drivers/staging/rtl8192u/ieee80211/dot11d.h
@@ -53,7 +53,7 @@ typedef struct _RT_DOT11D_INFO {
 
 #define CIE_WATCHDOG_TH 1
 #define GET_CIE_WATCHDOG(__pIeeeDev) (GET_DOT11D_INFO(__pIeeeDev)->CountryIeWatchdog)
-#define RESET_CIE_WATCHDOG(__pIeeeDev) GET_CIE_WATCHDOG(__pIeeeDev) = 0
+#define RESET_CIE_WATCHDOG(__pIeeeDev) (GET_CIE_WATCHDOG(__pIeeeDev) = 0)
 #define UPDATE_CIE_WATCHDOG(__pIeeeDev) (++GET_CIE_WATCHDOG(__pIeeeDev))
 
 #define IS_DOT11D_STATE_DONE(__pIeeeDev) (GET_DOT11D_INFO(__pIeeeDev)->State == DOT11D_STATE_DONE)
diff --git a/drivers/staging/rtl8192u/ieee80211/ieee80211.h b/drivers/staging/rtl8192u/ieee80211/ieee80211.h
index b44aa17d30a7..0f53c6a97578 100644
--- a/drivers/staging/rtl8192u/ieee80211/ieee80211.h
+++ b/drivers/staging/rtl8192u/ieee80211/ieee80211.h
@@ -606,33 +606,6 @@ do { if (ieee80211_debug_level & (level)) \
 #define IEEE80211_DEBUG_RX(f, a...)  IEEE80211_DEBUG(IEEE80211_DL_RX, f, ## a)
 #define IEEE80211_DEBUG_QOS(f, a...)  IEEE80211_DEBUG(IEEE80211_DL_QOS, f, ## a)
 
-#ifdef CONFIG_IEEE80211_DEBUG
-/* Added by Annie, 2005-11-22. */
-#define MAX_STR_LEN     64
-/* I want to see ASCII 33 to 126 only. Otherwise, I print '?'. Annie, 2005-11-22.*/
-#define PRINTABLE(_ch)  (_ch>'!' && _ch<'~')
-#define IEEE80211_PRINT_STR(_Comp, _TitleString, _Ptr, _Len)					\
-			if((_Comp) & level)							\
-			{                                                                       \
-				int             __i;                                            \
-				u8  buffer[MAX_STR_LEN];					\
-				int length = (_Len<MAX_STR_LEN)? _Len : (MAX_STR_LEN-1) ;	\
-				memset(buffer, 0, MAX_STR_LEN);					\
-				memcpy(buffer, (u8 *)_Ptr, length );				\
-				for( __i=0; __i<MAX_STR_LEN; __i++ )                            \
-				{                                                               \
-				     if( !PRINTABLE(buffer[__i]) )   buffer[__i] = '?';		\
-				}                                                               \
-				buffer[length] = '\0';                                          \
-				printk("Rtl819x: ");						\
-				printk(_TitleString);                                         \
-				printk(": %d, <%s>\n", _Len, buffer);                         \
-			}
-#else
-#define IEEE80211_PRINT_STR(_Comp, _TitleString, _Ptr, _Len)  do {} while (0)
-#endif
-
-#include <linux/netdevice.h>
 #include <linux/if_arp.h> /* ARPHRD_ETHER */
 
 #ifndef WIRELESS_SPY
@@ -661,7 +634,7 @@ struct ieee80211_snap_hdr {
 	u8    ctrl;   /* always 0x03 */
 	u8    oui[P80211_OUI_LEN];    /* organizational universal id */
 
-} __attribute__ ((packed));
+} __packed;
 
 #define SNAP_SIZE sizeof(struct ieee80211_snap_hdr)
 
@@ -996,7 +969,7 @@ struct ieee80211_security {
 	u8 keys[WEP_KEYS][SCM_KEY_LEN];
 	u8 level;
 	u16 flags;
-} __attribute__ ((packed));
+} __packed;
 
 
 /*
@@ -1051,14 +1024,14 @@ struct ieee80211_hdr {
 	__le16 frame_ctl;
 	__le16 duration_id;
 	u8 payload[0];
-} __attribute__ ((packed));
+} __packed;
 
 struct ieee80211_hdr_1addr {
 	__le16 frame_ctl;
 	__le16 duration_id;
 	u8 addr1[ETH_ALEN];
 	u8 payload[0];
-} __attribute__ ((packed));
+} __packed;
 
 struct ieee80211_hdr_2addr {
 	__le16 frame_ctl;
@@ -1066,7 +1039,7 @@ struct ieee80211_hdr_2addr {
 	u8 addr1[ETH_ALEN];
 	u8 addr2[ETH_ALEN];
 	u8 payload[0];
-} __attribute__ ((packed));
+} __packed;
 
 struct ieee80211_hdr_3addr {
 	__le16 frame_ctl;
@@ -1076,7 +1049,7 @@ struct ieee80211_hdr_3addr {
 	u8 addr3[ETH_ALEN];
 	__le16 seq_ctl;
 	u8 payload[0];
-} __attribute__ ((packed));
+} __packed;
 
 struct ieee80211_hdr_4addr {
 	__le16 frame_ctl;
@@ -1087,7 +1060,7 @@ struct ieee80211_hdr_4addr {
 	__le16 seq_ctl;
 	u8 addr4[ETH_ALEN];
 	u8 payload[0];
-} __attribute__ ((packed));
+} __packed;
 
 struct ieee80211_hdr_3addrqos {
 	__le16 frame_ctl;
@@ -1098,7 +1071,7 @@ struct ieee80211_hdr_3addrqos {
 	__le16 seq_ctl;
 	u8 payload[0];
 	__le16 qos_ctl;
-} __attribute__ ((packed));
+} __packed;
 
 struct ieee80211_hdr_4addrqos {
 	__le16 frame_ctl;
@@ -1110,13 +1083,13 @@ struct ieee80211_hdr_4addrqos {
 	u8 addr4[ETH_ALEN];
 	u8 payload[0];
 	__le16 qos_ctl;
-} __attribute__ ((packed));
+} __packed;
 
 struct ieee80211_info_element {
 	u8 id;
 	u8 len;
 	u8 data[0];
-} __attribute__ ((packed));
+} __packed;
 
 struct ieee80211_authentication {
 	struct ieee80211_hdr_3addr header;
@@ -1125,18 +1098,18 @@ struct ieee80211_authentication {
 	__le16 status;
 	/*challenge*/
 	struct ieee80211_info_element info_element[0];
-} __attribute__ ((packed));
+} __packed;
 
 struct ieee80211_disassoc {
 	struct ieee80211_hdr_3addr header;
 	__le16 reason;
-} __attribute__ ((packed));
+} __packed;
 
 struct ieee80211_probe_request {
 	struct ieee80211_hdr_3addr header;
 	/* SSID, supported rates */
 	struct ieee80211_info_element info_element[0];
-} __attribute__ ((packed));
+} __packed;
 
 struct ieee80211_probe_response {
 	struct ieee80211_hdr_3addr header;
@@ -1146,7 +1119,7 @@ struct ieee80211_probe_response {
 	/* SSID, supported rates, FH params, DS params,
 	 * CF params, IBSS params, TIM (if beacon), RSN */
 	struct ieee80211_info_element info_element[0];
-} __attribute__ ((packed));
+} __packed;
 
 /* Alias beacon for probe_response */
 #define ieee80211_beacon ieee80211_probe_response
@@ -1157,7 +1130,7 @@ struct ieee80211_assoc_request_frame {
 	__le16 listen_interval;
 	/* SSID, supported rates, RSN */
 	struct ieee80211_info_element info_element[0];
-} __attribute__ ((packed));
+} __packed;
 
 struct ieee80211_reassoc_request_frame {
 	struct ieee80211_hdr_3addr header;
@@ -1166,7 +1139,7 @@ struct ieee80211_reassoc_request_frame {
 	u8 current_ap[ETH_ALEN];
 	/* SSID, supported rates, RSN */
 	struct ieee80211_info_element info_element[0];
-} __attribute__ ((packed));
+} __packed;
 
 struct ieee80211_assoc_response_frame {
 	struct ieee80211_hdr_3addr header;
@@ -1174,7 +1147,7 @@ struct ieee80211_assoc_response_frame {
 	__le16 status;
 	__le16 aid;
 	struct ieee80211_info_element info_element[0]; /* supported rates */
-} __attribute__ ((packed));
+} __packed;
 
 struct ieee80211_txb {
 	u8 nr_frags;
@@ -1191,7 +1164,7 @@ struct ieee80211_txb {
 struct ieee80211_drv_agg_txb {
 	u8 nr_drv_agg_frames;
 	struct sk_buff *tx_agg_frames[MAX_TX_AGG_COUNT];
-}__attribute__((packed));
+} __packed;
 
 #define MAX_SUBFRAME_COUNT		  64
 struct ieee80211_rxb {
@@ -1199,7 +1172,7 @@ struct ieee80211_rxb {
 	struct sk_buff *subframes[MAX_SUBFRAME_COUNT];
 	u8 dst[ETH_ALEN];
 	u8 src[ETH_ALEN];
-}__attribute__((packed));
+} __packed;
 
 typedef union _frameqos {
 	u16 shortdata;
@@ -1267,19 +1240,19 @@ struct ieee80211_qos_information_element {
 	u8 qui_subtype;
 	u8 version;
 	u8 ac_info;
-} __attribute__ ((packed));
+} __packed;
 
 struct ieee80211_qos_ac_parameter {
 	u8 aci_aifsn;
 	u8 ecw_min_max;
 	__le16 tx_op_limit;
-} __attribute__ ((packed));
+} __packed;
 
 struct ieee80211_qos_parameter_info {
 	struct ieee80211_qos_information_element info_element;
 	u8 reserved;
 	struct ieee80211_qos_ac_parameter ac_params_record[QOS_QUEUE_NUM];
-} __attribute__ ((packed));
+} __packed;
 
 struct ieee80211_qos_parameters {
 	__le16 cw_min[QOS_QUEUE_NUM];
@@ -1287,7 +1260,7 @@ struct ieee80211_qos_parameters {
 	u8 aifs[QOS_QUEUE_NUM];
 	u8 flag[QOS_QUEUE_NUM];
 	__le16 tx_op_limit[QOS_QUEUE_NUM];
-} __attribute__ ((packed));
+} __packed;
 
 struct ieee80211_qos_data {
 	struct ieee80211_qos_parameters parameters;
@@ -1300,7 +1273,7 @@ struct ieee80211_qos_data {
 struct ieee80211_tim_parameters {
 	u8 tim_count;
 	u8 tim_period;
-} __attribute__ ((packed));
+} __packed;
 
 //#else
 struct ieee80211_wmm_ac_param {
@@ -1313,7 +1286,7 @@ struct ieee80211_wmm_ts_info {
 	u8 ac_dir_tid;
 	u8 ac_up_psb;
 	u8 reserved;
-} __attribute__ ((packed));
+} __packed;
 
 struct ieee80211_wmm_tspec_elem {
 	struct ieee80211_wmm_ts_info ts_info;
@@ -1332,7 +1305,7 @@ struct ieee80211_wmm_tspec_elem {
 	u32 min_phy_rate;
 	u16 surp_band_allow;
 	u16 medium_time;
-}__attribute__((packed));
+} __packed;
 enum eap_type {
 	EAP_PACKET = 0,
 	EAPOL_START,
@@ -1371,7 +1344,7 @@ struct eapol {
 	u8 version;
 	u8 type;
 	u16 length;
-} __attribute__ ((packed));
+} __packed;
 
 struct ieee80211_softmac_stats{
 	unsigned int rx_ass_ok;
@@ -1401,7 +1374,7 @@ struct ieee80211_softmac_stats{
 struct ieee80211_info_element_hdr {
 	u8 id;
 	u8 len;
-} __attribute__ ((packed));
+} __packed;
 
 /*
  * These are the data types that can make up management packets
@@ -1414,7 +1387,7 @@ struct ieee80211_info_element_hdr {
 	u16 listen_interval;
 	struct {
 		u16 association_id:14, reserved:2;
-	} __attribute__ ((packed));
+	} __packed;
 	u32 time_stamp[2];
 	u16 reason;
 	u16 status;
@@ -1475,7 +1448,7 @@ struct	ether_header {
 	u8 ether_dhost[ETHER_ADDR_LEN];
 	u8 ether_shost[ETHER_ADDR_LEN];
 	u16 ether_type;
-} __attribute__((packed));
+} __packed;
 
 #ifndef ETHERTYPE_PAE
 #define	ETHERTYPE_PAE	0x888e		/* EAPOL PAE/802.1x */
@@ -1484,24 +1457,6 @@ struct	ether_header {
 #define	ETHERTYPE_IP	0x0800		/* IP protocol */
 #endif
 
-typedef struct _bss_ht{
-
-	bool				support_ht;
-
-	// HT related elements
-	u8					ht_cap_buf[32];
-	u16					ht_cap_len;
-	u8					ht_info_buf[32];
-	u16					ht_info_len;
-
-	HT_SPEC_VER			ht_spec_ver;
-	//HT_CAPABILITY_ELE			bdHTCapEle;
-	//HT_INFORMATION_ELE		bdHTInfoEle;
-
-	bool				aggregation;
-	bool				long_slot_time;
-}bss_ht, *pbss_ht;
-
 typedef enum _erp_t{
 	ERP_NonERPpresent	= 0x01,
 	ERP_UseProtection	= 0x02,
diff --git a/drivers/staging/rtl8192u/ieee80211/ieee80211_crypt.c b/drivers/staging/rtl8192u/ieee80211/ieee80211_crypt.c
index 55332217c29f..3995620b3442 100644
--- a/drivers/staging/rtl8192u/ieee80211/ieee80211_crypt.c
+++ b/drivers/staging/rtl8192u/ieee80211/ieee80211_crypt.c
@@ -11,12 +11,11 @@
  *
  */
 
-//#include <linux/config.h>
 #include <linux/module.h>
 #include <linux/init.h>
 #include <linux/slab.h>
-#include <asm/string.h>
-#include <asm/errno.h>
+#include <linux/string.h>
+#include <linux/errno.h>
 
 #include "ieee80211.h"
 
@@ -66,8 +65,8 @@ void ieee80211_crypt_deinit_handler(unsigned long data)
 	spin_lock_irqsave(&ieee->lock, flags);
 	ieee80211_crypt_deinit_entries(ieee, 0);
 	if (!list_empty(&ieee->crypt_deinit_list)) {
-		printk(KERN_DEBUG "%s: entries remaining in delayed crypt "
-		       "deletion list\n", ieee->dev->name);
+		netdev_dbg(ieee->dev, "%s: entries remaining in delayed crypt deletion list\n",
+				ieee->dev->name);
 		ieee->crypt_deinit_timer.expires = jiffies + HZ;
 		add_timer(&ieee->crypt_deinit_timer);
 	}
@@ -118,7 +117,7 @@ int ieee80211_register_crypto_ops(struct ieee80211_crypto_ops *ops)
 	list_add(&alg->list, &hcrypt->algs);
 	spin_unlock_irqrestore(&hcrypt->lock, flags);
 
-	printk(KERN_DEBUG "ieee80211_crypt: registered algorithm '%s'\n",
+	pr_debug("ieee80211_crypt: registered algorithm '%s'\n",
 	       ops->name);
 
 	return 0;
@@ -146,8 +145,8 @@ int ieee80211_unregister_crypto_ops(struct ieee80211_crypto_ops *ops)
 	spin_unlock_irqrestore(&hcrypt->lock, flags);
 
 	if (del_alg) {
-		printk(KERN_DEBUG "ieee80211_crypt: unregistered algorithm "
-		       "'%s'\n", ops->name);
+		pr_debug("ieee80211_crypt: unregistered algorithm '%s'\n",
+				ops->name);
 		kfree(del_alg);
 	}
 
@@ -232,8 +231,8 @@ void __exit ieee80211_crypto_deinit(void)
 		struct ieee80211_crypto_alg *alg =
 			(struct ieee80211_crypto_alg *) ptr;
 		list_del(ptr);
-		printk(KERN_DEBUG "ieee80211_crypt: unregistered algorithm "
-		       "'%s' (deinit)\n", alg->ops->name);
+		pr_debug("ieee80211_crypt: unregistered algorithm '%s' (deinit)\n",
+				alg->ops->name);
 		kfree(alg);
 	}
 
diff --git a/drivers/staging/rtl8192u/ieee80211/ieee80211_crypt_ccmp.c b/drivers/staging/rtl8192u/ieee80211/ieee80211_crypt_ccmp.c
index 143b682477e5..788704b800c4 100644
--- a/drivers/staging/rtl8192u/ieee80211/ieee80211_crypt_ccmp.c
+++ b/drivers/staging/rtl8192u/ieee80211/ieee80211_crypt_ccmp.c
@@ -9,7 +9,6 @@
  * more details.
  */
 
-//#include <linux/config.h>
 #include <linux/module.h>
 #include <linux/init.h>
 #include <linux/slab.h>
@@ -18,7 +17,7 @@
 #include <linux/netdevice.h>
 #include <linux/if_ether.h>
 #include <linux/if_arp.h>
-#include <asm/string.h>
+#include <linux/string.h>
 #include <linux/wireless.h>
 
 #include "ieee80211.h"
@@ -130,9 +129,9 @@ static void ccmp_init_blocks(struct crypto_tfm *tfm,
 	qc_included = ((WLAN_FC_GET_TYPE(fc) == IEEE80211_FTYPE_DATA) &&
 		       (WLAN_FC_GET_STYPE(fc) & 0x08));
 	*/
-	// fixed by David :2006.9.6
-	qc_included = ((WLAN_FC_GET_TYPE(fc) == IEEE80211_FTYPE_DATA) &&
-		       (WLAN_FC_GET_STYPE(fc) & 0x80));
+	/* fixed by David :2006.9.6 */
+	qc_included = (WLAN_FC_GET_TYPE(fc) == IEEE80211_FTYPE_DATA) &&
+		       (WLAN_FC_GET_STYPE(fc) & 0x80);
 	aad_len = 22;
 	if (a4_included)
 		aad_len += 6;
@@ -209,7 +208,7 @@ static int ieee80211_ccmp_encrypt(struct sk_buff *skb, int hdr_len, void *priv)
 	pos = skb_push(skb, CCMP_HDR_LEN);
 	memmove(pos, pos + CCMP_HDR_LEN, hdr_len);
 	pos += hdr_len;
-//	mic = skb_put(skb, CCMP_MIC_LEN);
+	/* mic = skb_put(skb, CCMP_MIC_LEN); */
 
 	i = CCMP_PN_LEN - 1;
 	while (i >= 0) {
@@ -239,7 +238,7 @@ static int ieee80211_ccmp_encrypt(struct sk_buff *skb, int hdr_len, void *priv)
 		u8 *e = key->tx_e;
 		u8 *s0 = key->tx_s0;
 
-		//mic is moved to here by john
+		/* mic is moved to here by john */
 		mic = skb_put(skb, CCMP_MIC_LEN);
 
 		ccmp_init_blocks(key->tfm, hdr, key->tx_pn, data_len, b0, b, s0);
@@ -444,7 +443,7 @@ static char *ieee80211_ccmp_print_stats(char *p, void *priv)
 
 void ieee80211_ccmp_null(void)
 {
-//    printk("============>%s()\n", __func__);
+	/* printk("============>%s()\n", __func__); */
 	return;
 }
 
diff --git a/drivers/staging/rtl8192u/ieee80211/ieee80211_crypt_tkip.c b/drivers/staging/rtl8192u/ieee80211/ieee80211_crypt_tkip.c
index fcc90a5700d6..e815c81b45dc 100644
--- a/drivers/staging/rtl8192u/ieee80211/ieee80211_crypt_tkip.c
+++ b/drivers/staging/rtl8192u/ieee80211/ieee80211_crypt_tkip.c
@@ -9,7 +9,6 @@
  * more details.
  */
 
-//#include <linux/config.h>
 #include <linux/module.h>
 #include <linux/init.h>
 #include <linux/slab.h>
@@ -18,7 +17,7 @@
 #include <linux/netdevice.h>
 #include <linux/if_ether.h>
 #include <linux/if_arp.h>
-#include <asm/string.h>
+#include <linux/string.h>
 
 #include "ieee80211.h"
 
diff --git a/drivers/staging/rtl8192u/ieee80211/ieee80211_crypt_wep.c b/drivers/staging/rtl8192u/ieee80211/ieee80211_crypt_wep.c
index 8c1bf1f56883..0a17f84bb809 100644
--- a/drivers/staging/rtl8192u/ieee80211/ieee80211_crypt_wep.c
+++ b/drivers/staging/rtl8192u/ieee80211/ieee80211_crypt_wep.c
@@ -9,13 +9,12 @@
  * more details.
  */
 
-//#include <linux/config.h>
 #include <linux/module.h>
 #include <linux/init.h>
 #include <linux/slab.h>
 #include <linux/random.h>
 #include <linux/skbuff.h>
-#include <asm/string.h>
+#include <linux/string.h>
 
 #include "ieee80211.h"
 
@@ -49,14 +48,14 @@ static void *prism2_wep_init(int keyidx)
 
 	priv->tx_tfm = crypto_alloc_blkcipher("ecb(arc4)", 0, CRYPTO_ALG_ASYNC);
 	if (IS_ERR(priv->tx_tfm)) {
-		printk(KERN_DEBUG "ieee80211_crypt_wep: could not allocate "
+		pr_debug("ieee80211_crypt_wep: could not allocate "
 		       "crypto API arc4\n");
 		priv->tx_tfm = NULL;
 		goto fail;
 	}
 	priv->rx_tfm = crypto_alloc_blkcipher("ecb(arc4)", 0, CRYPTO_ALG_ASYNC);
 	if (IS_ERR(priv->rx_tfm)) {
-		printk(KERN_DEBUG "ieee80211_crypt_wep: could not allocate "
+		pr_debug("ieee80211_crypt_wep: could not allocate "
 		       "crypto API arc4\n");
 		priv->rx_tfm = NULL;
 		goto fail;
@@ -110,6 +109,7 @@ static int prism2_wep_encrypt(struct sk_buff *skb, int hdr_len, void *priv)
 	u32 crc;
 	u8 *icv;
 	struct scatterlist sg;
+
 	if (skb_headroom(skb) < 4 || skb_tailroom(skb) < 4 ||
 	    skb->len < hdr_len)
 		return -1;
@@ -128,6 +128,7 @@ static int prism2_wep_encrypt(struct sk_buff *skb, int hdr_len, void *priv)
 	 * can be used to speedup attacks, so avoid using them. */
 	if ((wep->iv & 0xff00) == 0xff00) {
 		u8 B = (wep->iv >> 16) & 0xff;
+
 		if (B >= 3 && B < klen)
 			wep->iv += 0x0100;
 	}
@@ -180,6 +181,7 @@ static int prism2_wep_decrypt(struct sk_buff *skb, int hdr_len, void *priv)
 	u32 crc;
 	u8 icv[4];
 	struct scatterlist sg;
+
 	if (skb->len < hdr_len + 8)
 		return -1;
 
@@ -256,6 +258,7 @@ static int prism2_wep_get_key(void *key, int len, u8 *seq, void *priv)
 static char *prism2_wep_print_stats(char *p, void *priv)
 {
 	struct prism2_wep_data *wep = priv;
+
 	p += sprintf(p, "key[%d] alg=WEP len=%d\n",
 		     wep->key_idx, wep->key_len);
 	return p;
diff --git a/drivers/staging/rtl8192u/ieee80211/ieee80211_module.c b/drivers/staging/rtl8192u/ieee80211/ieee80211_module.c
index afbd09d7300a..31233d895ee9 100644
--- a/drivers/staging/rtl8192u/ieee80211/ieee80211_module.c
+++ b/drivers/staging/rtl8192u/ieee80211/ieee80211_module.c
@@ -31,7 +31,7 @@
 *******************************************************************************/
 
 #include <linux/compiler.h>
-//#include <linux/config.h>
+/* #include <linux/config.h> */
 #include <linux/errno.h>
 #include <linux/if_arp.h>
 #include <linux/in6.h>
@@ -133,15 +133,14 @@ struct net_device *alloc_ieee80211(int sizeof_priv)
 	ieee->ieee802_1x = 1; /* Default to supporting 802.1x */
 
 	INIT_LIST_HEAD(&ieee->crypt_deinit_list);
-	init_timer(&ieee->crypt_deinit_timer);
-	ieee->crypt_deinit_timer.data = (unsigned long)ieee;
-	ieee->crypt_deinit_timer.function = ieee80211_crypt_deinit_handler;
+	setup_timer(&ieee->crypt_deinit_timer,
+		    ieee80211_crypt_deinit_handler, (unsigned long)ieee);
 
 	spin_lock_init(&ieee->lock);
 	spin_lock_init(&ieee->wpax_suitlist_lock);
 	spin_lock_init(&ieee->bw_spinlock);
 	spin_lock_init(&ieee->reorder_spinlock);
-	//added by WB
+	/* added by WB */
 	atomic_set(&(ieee->atm_chnlop), 0);
 	atomic_set(&(ieee->atm_swbw), 0);
 
@@ -153,7 +152,7 @@ struct net_device *alloc_ieee80211(int sizeof_priv)
 	ieee->ieee802_1x = 1;
 	ieee->raw_tx = 0;
 	//ieee->hwsec_support = 1; //defalt support hw security. //use module_param instead.
-	ieee->hwsec_active = 0; //disable hwsec, switch it on when necessary.
+	ieee->hwsec_active = 0; /* disable hwsec, switch it on when necessary. */
 
 	ieee80211_softmac_init(ieee);
 
@@ -164,7 +163,7 @@ struct net_device *alloc_ieee80211(int sizeof_priv)
 		goto failed;
 	}
 	HTUpdateDefaultSetting(ieee);
-	HTInitializeHTInfo(ieee); //may move to other place.
+	HTInitializeHTInfo(ieee); /* may move to other place. */
 	TSInitialize(ieee);
 
 	for (i = 0; i < IEEE_IBSS_MAC_HASH_SIZE; i++)
@@ -176,7 +175,7 @@ struct net_device *alloc_ieee80211(int sizeof_priv)
 	  ieee->last_packet_time[i] = 0;
 	}
 
-//These function were added to load crypte module autoly
+/* These function were added to load crypte module autoly */
 	ieee80211_tkip_null();
 	ieee80211_wep_null();
 	ieee80211_ccmp_null();
@@ -195,7 +194,7 @@ void free_ieee80211(struct net_device *dev)
 {
 	struct ieee80211_device *ieee = netdev_priv(dev);
 	int i;
-	//struct list_head *p, *q;
+	/* struct list_head *p, *q; */
 //	del_timer_sync(&ieee->SwBwTimer);
 	kfree(ieee->pHTInfo);
 	ieee->pHTInfo = NULL;
@@ -239,13 +238,15 @@ static int debug = \
 	//		    IEEE80211_DL_REORDER|
 //			    IEEE80211_DL_TRACE  |
 			    //IEEE80211_DL_DATA	|
-			    IEEE80211_DL_ERR	  //awayls open this flags to show error out
+			    IEEE80211_DL_ERR	  /* awayls open this flags to show error out */
 			    ;
 static struct proc_dir_entry *ieee80211_proc;
 
 static int show_debug_level(struct seq_file *m, void *v)
 {
-	return seq_printf(m, "0x%08X\n", ieee80211_debug_level);
+	seq_printf(m, "0x%08X\n", ieee80211_debug_level);
+
+	return 0;
 }
 
 static ssize_t write_debug_level(struct file *file, const char __user *buffer,
@@ -284,7 +285,7 @@ int __init ieee80211_debug_init(void)
 				" proc directory\n");
 		return -EIO;
 	}
-	e = proc_create("debug_level", S_IRUGO | S_IWUSR, 
+	e = proc_create("debug_level", S_IRUGO | S_IWUSR,
 			      ieee80211_proc, &fops);
 	if (!e) {
 		remove_proc_entry(DRV_NAME, init_net.proc_net);
@@ -303,7 +304,6 @@ void __exit ieee80211_debug_exit(void)
 	}
 }
 
-#include <linux/moduleparam.h>
 module_param(debug, int, 0444);
 MODULE_PARM_DESC(debug, "debug output mask");
 #endif
diff --git a/drivers/staging/rtl8192u/ieee80211/ieee80211_rx.c b/drivers/staging/rtl8192u/ieee80211/ieee80211_rx.c
index d401dbf4c7c6..9fbb53d8c6bf 100644
--- a/drivers/staging/rtl8192u/ieee80211/ieee80211_rx.c
+++ b/drivers/staging/rtl8192u/ieee80211/ieee80211_rx.c
@@ -22,7 +22,6 @@
 
 
 #include <linux/compiler.h>
-//#include <linux/config.h>
 #include <linux/errno.h>
 #include <linux/if_arp.h>
 #include <linux/in6.h>
@@ -39,7 +38,7 @@
 #include <linux/types.h>
 #include <linux/wireless.h>
 #include <linux/etherdevice.h>
-#include <asm/uaccess.h>
+#include <linux/uaccess.h>
 #include <linux/ctype.h>
 
 #include "ieee80211.h"
@@ -56,7 +55,7 @@ static inline void ieee80211_monitor_rx(struct ieee80211_device *ieee,
 
 	skb_pull(skb, ieee80211_get_hdrlen(fc));
 	skb->pkt_type = PACKET_OTHERHOST;
-	skb->protocol = __constant_htons(ETH_P_80211_RAW);
+	skb->protocol = htons(ETH_P_80211_RAW);
 	memset(skb->cb, 0, sizeof(skb->cb));
 	netif_rx(skb);
 }
@@ -222,8 +221,8 @@ ieee80211_rx_frame_mgmt(struct ieee80211_device *ieee, struct sk_buff *skb,
 
 	rx_stats->len = skb->len;
 	ieee80211_rx_mgt(ieee,(struct ieee80211_hdr_4addr *)skb->data,rx_stats);
-	//if ((ieee->state == IEEE80211_LINKED) && (memcmp(hdr->addr3, ieee->current_network.bssid, ETH_ALEN)))
-	if ((memcmp(hdr->addr1, ieee->dev->dev_addr, ETH_ALEN)))//use ADDR1 to perform address matching for Management frames
+	/* if ((ieee->state == IEEE80211_LINKED) && (memcmp(hdr->addr3, ieee->current_network.bssid, ETH_ALEN))) */
+	if ((memcmp(hdr->addr1, ieee->dev->dev_addr, ETH_ALEN)))/* use ADDR1 to perform address matching for Management frames */
 	{
 		dev_kfree_skb_any(skb);
 		return 0;
@@ -609,12 +608,12 @@ static void RxReorderIndicatePacket(struct ieee80211_device *ieee,
 	bool			bMatchWinStart = false, bPktInBuf = false;
 	IEEE80211_DEBUG(IEEE80211_DL_REORDER,"%s(): Seq is %d,pTS->RxIndicateSeq is %d, WinSize is %d\n",__func__,SeqNum,pTS->RxIndicateSeq,WinSize);
 	/* Rx Reorder initialize condition.*/
-	if(pTS->RxIndicateSeq == 0xffff) {
+	if (pTS->RxIndicateSeq == 0xffff) {
 		pTS->RxIndicateSeq = SeqNum;
 	}
 
 	/* Drop out the packet which SeqNum is smaller than WinStart */
-	if(SN_LESS(SeqNum, pTS->RxIndicateSeq)) {
+	if (SN_LESS(SeqNum, pTS->RxIndicateSeq)) {
 		IEEE80211_DEBUG(IEEE80211_DL_REORDER,"Packet Drop! IndicateSeq: %d, NewSeq: %d\n",
 				 pTS->RxIndicateSeq, SeqNum);
 		pHTInfo->RxReorderDropCounter++;
@@ -717,7 +716,7 @@ static void RxReorderIndicatePacket(struct ieee80211_device *ieee,
 		    SN_EQUAL(pReorderEntry->SeqNum, pTS->RxIndicateSeq))
 		{
 			/* This protect buffer from overflow. */
-			if(index >= REORDER_WIN_SIZE) {
+			if (index >= REORDER_WIN_SIZE) {
 				IEEE80211_DEBUG(IEEE80211_DL_ERR, "RxReorderIndicatePacket(): Buffer overflow!! \n");
 				bPktInBuf = true;
 				break;
@@ -741,7 +740,7 @@ static void RxReorderIndicatePacket(struct ieee80211_device *ieee,
 	}
 
 	/* Handling pending timer. Set this timer to prevent from long time Rx buffering.*/
-	if(index>0) {
+	if (index>0) {
 		// Cancel previous pending timer.
 	//	del_timer_sync(&pTS->RxPktPendingTimer);
 		pTS->RxTimeoutIndicateSeq = 0xffff;
@@ -754,7 +753,7 @@ static void RxReorderIndicatePacket(struct ieee80211_device *ieee,
 		ieee80211_indicate_packets(ieee, prxbIndicateArray, index);
 	}
 
-	if(bPktInBuf && pTS->RxTimeoutIndicateSeq==0xffff) {
+	if (bPktInBuf && pTS->RxTimeoutIndicateSeq==0xffff) {
 		// Set new pending timer.
 		IEEE80211_DEBUG(IEEE80211_DL_REORDER,"%s(): SET rx timeout timer\n", __func__);
 		pTS->RxTimeoutIndicateSeq = pTS->RxIndicateSeq;
@@ -784,16 +783,16 @@ static u8 parse_subframe(struct sk_buff *skb,
 	/* just for debug purpose */
 	SeqNum = WLAN_GET_SEQ_SEQ(le16_to_cpu(hdr->seq_ctl));
 
-	if((IEEE80211_QOS_HAS_SEQ(fc))&&\
+	if ((IEEE80211_QOS_HAS_SEQ(fc))&&\
 			(((frameqos *)(skb->data + IEEE80211_3ADDR_LEN))->field.reserved)) {
 		bIsAggregateFrame = true;
 	}
 
-	if(IEEE80211_QOS_HAS_SEQ(fc)) {
+	if (IEEE80211_QOS_HAS_SEQ(fc)) {
 		LLCOffset += 2;
 	}
 
-	if(rx_stats->bContainHTC) {
+	if (rx_stats->bContainHTC) {
 		LLCOffset += sHTCLng;
 	}
 	//printk("ChkLength = %d\n", LLCOffset);
@@ -827,7 +826,7 @@ static u8 parse_subframe(struct sk_buff *skb,
 			//==m==>change the length order
 			nSubframe_Length = (nSubframe_Length>>8) + (nSubframe_Length<<8);
 
-			if(skb->len<(ETHERNET_HEADER_SIZE + nSubframe_Length)) {
+			if (skb->len<(ETHERNET_HEADER_SIZE + nSubframe_Length)) {
 				printk("%s: A-MSDU parse error!! pRfd->nTotalSubframe : %d\n",\
 						__func__, rxb->nr_subframes);
 				printk("%s: A-MSDU parse error!! Subframe Length: %d\n",__func__, nSubframe_Length);
@@ -853,19 +852,19 @@ static u8 parse_subframe(struct sk_buff *skb,
 			memcpy(data_ptr, skb->data, nSubframe_Length);
 #endif
 			rxb->subframes[rxb->nr_subframes++] = sub_skb;
-			if(rxb->nr_subframes >= MAX_SUBFRAME_COUNT) {
+			if (rxb->nr_subframes >= MAX_SUBFRAME_COUNT) {
 				IEEE80211_DEBUG_RX("ParseSubframe(): Too many Subframes! Packets dropped!\n");
 				break;
 			}
 			skb_pull(skb, nSubframe_Length);
 
-			if(skb->len != 0) {
+			if (skb->len != 0) {
 				nPadding_Length = 4 - ((nSubframe_Length + ETHERNET_HEADER_SIZE) % 4);
-				if(nPadding_Length == 4) {
+				if (nPadding_Length == 4) {
 					nPadding_Length = 0;
 				}
 
-				if(skb->len < nPadding_Length) {
+				if (skb->len < nPadding_Length) {
 					return 0;
 				}
 
@@ -939,12 +938,12 @@ int ieee80211_rx(struct ieee80211_device *ieee, struct sk_buff *skb,
 	frag = WLAN_GET_SEQ_FRAG(sc);
 	hdrlen = ieee80211_get_hdrlen(fc);
 
-	if(HTCCheck(ieee, skb->data))
+	if (HTCCheck(ieee, skb->data))
 	{
 		if(net_ratelimit())
 		printk("find HTCControl\n");
 		hdrlen += 4;
-		rx_stats->bContainHTC = 1;
+		rx_stats->bContainHTC = true;
 	}
 
 	//IEEE80211_DEBUG_DATA(IEEE80211_DL_DATA, skb->data, skb->len);
@@ -1274,13 +1273,13 @@ int ieee80211_rx(struct ieee80211_device *ieee, struct sk_buff *skb,
 	}
 */
 //added by amy for reorder
-	if(ieee->current_network.qos_data.active && IsQoSDataFrame(skb->data)
+	if (ieee->current_network.qos_data.active && IsQoSDataFrame(skb->data)
 		&& !is_multicast_ether_addr(hdr->addr1))
 	{
 		TID = Frame_QoSTID(skb->data);
 		SeqNum = WLAN_GET_SEQ_SEQ(sc);
 		GetTs(ieee,(PTS_COMMON_INFO *) &pTS,hdr->addr2,TID,RX_DIR,true);
-		if(TID !=0 && TID !=3)
+		if (TID !=0 && TID !=3)
 		{
 			ieee->bis_any_nonbepkts = true;
 		}
@@ -1290,7 +1289,7 @@ int ieee80211_rx(struct ieee80211_device *ieee, struct sk_buff *skb,
 	payload = skb->data + hdrlen;
 	//ethertype = (payload[6] << 8) | payload[7];
 	rxb = kmalloc(sizeof(struct ieee80211_rxb), GFP_ATOMIC);
-	if(rxb == NULL)
+	if (rxb == NULL)
 	{
 		IEEE80211_DEBUG(IEEE80211_DL_ERR,"%s(): kmalloc rxb error\n",__func__);
 		goto rx_dropped;
@@ -1336,7 +1335,7 @@ int ieee80211_rx(struct ieee80211_device *ieee, struct sk_buff *skb,
 
 				stats->rx_packets++;
 				stats->rx_bytes += sub_skb->len;
-				if(is_multicast_ether_addr(dst)) {
+				if (is_multicast_ether_addr(dst)) {
 					stats->multicast++;
 				}
 
@@ -1590,14 +1589,14 @@ static inline void ieee80211_extract_country_ie(
 	u8 *addr2
 )
 {
-	if(IS_DOT11D_ENABLE(ieee))
+	if (IS_DOT11D_ENABLE(ieee))
 	{
-		if(info_element->len!= 0)
+		if (info_element->len!= 0)
 		{
 			memcpy(network->CountryIeBuf, info_element->data, info_element->len);
 			network->CountryIeLen = info_element->len;
 
-			if(!IS_COUNTRY_IE_VALID(ieee))
+			if (!IS_COUNTRY_IE_VALID(ieee))
 			{
 				Dot11d_UpdateCountryIe(ieee, addr2, info_element->len, info_element->data);
 			}
@@ -1608,7 +1607,7 @@ static inline void ieee80211_extract_country_ie(
 		// some AP (e.g. Cisco 1242) don't include country IE in their
 		// probe response frame.
 		//
-		if(IS_EQUAL_CIE_SRC(ieee, addr2) )
+		if (IS_EQUAL_CIE_SRC(ieee, addr2) )
 		{
 			UPDATE_CIE_WATCHDOG(ieee);
 		}
@@ -1705,7 +1704,7 @@ int ieee80211_parse_info_param(struct ieee80211_device *ieee,
 #ifdef CONFIG_IEEE80211_DEBUG
 				p += snprintf(p, sizeof(rates_str) -
 					      (p - rates_str), "%02X ",
-					      network->rates[i]);
+					      network->rates_ex[i]);
 #endif
 				if (ieee80211_is_ofdm_rate
 				    (info_element->data[i])) {
@@ -1888,7 +1887,7 @@ int ieee80211_parse_info_param(struct ieee80211_device *ieee,
 
 			//if(tmp_htcap_len !=0  ||  tmp_htinfo_len != 0)
 			{
-				if((info_element->len >= 3 &&
+				if ((info_element->len >= 3 &&
 					 info_element->data[0] == 0x00 &&
 					 info_element->data[1] == 0x05 &&
 					 info_element->data[2] == 0xb5) ||
@@ -1940,7 +1939,7 @@ int ieee80211_parse_info_param(struct ieee80211_device *ieee,
 			else
 				network->cisco_cap_exist = false;
 			//added by amy for LEAP of cisco
-			if(info_element->len > 4 &&
+			if (info_element->len > 4 &&
 				info_element->data[0] == 0x00 &&
 				info_element->data[1] == 0x40 &&
 				info_element->data[2] == 0x96 &&
@@ -1976,7 +1975,7 @@ int ieee80211_parse_info_param(struct ieee80211_device *ieee,
 					network->bCcxRmEnable = false;
 				}
 			}
-			if(info_element->len > 4  &&
+			if (info_element->len > 4  &&
 				info_element->data[0] == 0x00 &&
 				info_element->data[1] == 0x40 &&
 				info_element->data[2] == 0x96 &&
@@ -2296,7 +2295,7 @@ static inline void update_network(struct ieee80211_network *dst,
 	dst->rates_len = src->rates_len;
 	memcpy(dst->rates_ex, src->rates_ex, src->rates_ex_len);
 	dst->rates_ex_len = src->rates_ex_len;
-	if(src->ssid_len > 0)
+	if (src->ssid_len > 0)
 	{
 		memset(dst->ssid, 0, dst->ssid_len);
 		dst->ssid_len = src->ssid_len;
@@ -2352,7 +2351,7 @@ static inline void update_network(struct ieee80211_network *dst,
 		dst->qos_data.param_count = src->qos_data.param_count;
 	}
 
-	if(dst->qos_data.supported == 1) {
+	if (dst->qos_data.supported == 1) {
 		dst->QoS_Enable = 1;
 		if(dst->ssid_len)
 			IEEE80211_DEBUG_QOS
@@ -2367,7 +2366,7 @@ static inline void update_network(struct ieee80211_network *dst,
 
 	/* dst->last_associate is not overwritten */
 	dst->wmm_info = src->wmm_info; //sure to exist in beacon or probe response frame.
-	if(src->wmm_param[0].ac_aci_acm_aifsn|| \
+	if (src->wmm_param[0].ac_aci_acm_aifsn|| \
 	   src->wmm_param[1].ac_aci_acm_aifsn|| \
 	   src->wmm_param[2].ac_aci_acm_aifsn|| \
 	   src->wmm_param[3].ac_aci_acm_aifsn) {
@@ -2456,7 +2455,7 @@ static inline void ieee80211_process_probe_response(
 
 	if (!IsLegalChannel(ieee, network.channel))
 		return;
-	if(ieee->bGlobalDomain)
+	if (ieee->bGlobalDomain)
 	{
 		if (WLAN_FC_GET_STYPE(beacon->header.frame_ctl) == IEEE80211_STYPE_PROBE_RESP)
 		{
@@ -2472,7 +2471,7 @@ static inline void ieee80211_process_probe_response(
 			else
 			{
 				// Filter over channel ch12~14
-				if(network.channel > 11)
+				if (network.channel > 11)
 				{
 					printk("GetScanInfo(): For Global Domain, filter probe response at channel(%d).\n", network.channel);
 					return;
@@ -2493,7 +2492,7 @@ static inline void ieee80211_process_probe_response(
 			else
 			{
 				// Filter over channel ch12~14
-				if(network.channel > 14)
+				if (network.channel > 14)
 				{
 					printk("GetScanInfo(): For Global Domain, filter beacon at channel(%d).\n",network.channel);
 					return;
@@ -2514,9 +2513,9 @@ static inline void ieee80211_process_probe_response(
 
 	spin_lock_irqsave(&ieee->lock, flags);
 
-	if(is_same_network(&ieee->current_network, &network, ieee)) {
+	if (is_same_network(&ieee->current_network, &network, ieee)) {
 		update_network(&ieee->current_network, &network);
-		if((ieee->current_network.mode == IEEE_N_24G || ieee->current_network.mode == IEEE_G)
+		if ((ieee->current_network.mode == IEEE_N_24G || ieee->current_network.mode == IEEE_G)
 		&& ieee->current_network.berp_info_valid){
 		if(ieee->current_network.erp_value& ERP_UseProtection)
 			ieee->current_network.buseprotection = true;
@@ -2606,7 +2605,7 @@ static inline void ieee80211_process_probe_response(
 	spin_unlock_irqrestore(&ieee->lock, flags);
 	if (is_beacon(beacon->header.frame_ctl)&&is_same_network(&ieee->current_network, &network, ieee)&&\
 		(ieee->state == IEEE80211_LINKED)) {
-		if(ieee->handle_beacon != NULL) {
+		if (ieee->handle_beacon != NULL) {
 			ieee->handle_beacon(ieee->dev,beacon,&ieee->current_network);
 		}
 	}
diff --git a/drivers/staging/rtl8192u/ieee80211/ieee80211_softmac.c b/drivers/staging/rtl8192u/ieee80211/ieee80211_softmac.c
index d1471877e19d..c2388812d4fd 100644
--- a/drivers/staging/rtl8192u/ieee80211/ieee80211_softmac.c
+++ b/drivers/staging/rtl8192u/ieee80211/ieee80211_softmac.c
@@ -62,7 +62,7 @@ static void ieee80211_MFIE_Brate(struct ieee80211_device *ieee, u8 **tag_p)
 {
 	u8 *tag = *tag_p;
 
-	if (ieee->modulation & IEEE80211_CCK_MODULATION){
+	if (ieee->modulation & IEEE80211_CCK_MODULATION) {
 		*tag++ = MFIE_TYPE_RATES;
 		*tag++ = 4;
 		*tag++ = IEEE80211_BASIC_RATE_MASK | IEEE80211_CCK_RATE_1MB;
@@ -79,7 +79,7 @@ static void ieee80211_MFIE_Grate(struct ieee80211_device *ieee, u8 **tag_p)
 {
 	u8 *tag = *tag_p;
 
-		if (ieee->modulation & IEEE80211_OFDM_MODULATION){
+		if (ieee->modulation & IEEE80211_OFDM_MODULATION) {
 
 		*tag++ = MFIE_TYPE_RATES_EX;
 		*tag++ = 8;
@@ -192,7 +192,7 @@ static u8 MgntQuery_MgntFrameTxRate(struct ieee80211_device *ieee)
 	else
 		rate = ieee->basic_rate & 0x7f;
 
-	if(rate == 0){
+	if (rate == 0) {
 		// 2005.01.26, by rcnjko.
 		if(ieee->mode == IEEE_A||
 		   ieee->mode== IEEE_N_5G||
@@ -369,7 +369,7 @@ static void ieee80211_send_beacon(struct ieee80211_device *ieee)
 	//unsigned long flags;
 	skb = ieee80211_get_beacon_(ieee);
 
-	if (skb){
+	if (skb) {
 		softmac_mgmt_xmit(skb, ieee);
 		ieee->softmac_stats.tx_beacons++;
 		//dev_kfree_skb_any(skb);//edit by thomas
@@ -378,7 +378,7 @@ static void ieee80211_send_beacon(struct ieee80211_device *ieee)
 //		(MSECS( ieee->current_network.beacon_interval -5));
 
 	//spin_lock_irqsave(&ieee->beacon_lock,flags);
-	if(ieee->beacon_txing && ieee->ieee_up){
+	if (ieee->beacon_txing && ieee->ieee_up) {
 //		if(!timer_pending(&ieee->beacon_timer))
 //			add_timer(&ieee->beacon_timer);
 		mod_timer(&ieee->beacon_timer,jiffies+(MSECS(ieee->current_network.beacon_interval-5)));
@@ -404,7 +404,7 @@ static void ieee80211_send_probe(struct ieee80211_device *ieee)
 	struct sk_buff *skb;
 
 	skb = ieee80211_probe_req(ieee);
-	if (skb){
+	if (skb) {
 		softmac_mgmt_xmit(skb, ieee);
 		ieee->softmac_stats.tx_probe_rq++;
 		//dev_kfree_skb_any(skb);//edit by thomas
@@ -413,7 +413,7 @@ static void ieee80211_send_probe(struct ieee80211_device *ieee)
 
 static void ieee80211_send_probe_requests(struct ieee80211_device *ieee)
 {
-	if (ieee->active_scan && (ieee->softmac_features & IEEE_SOFTMAC_PROBERQ)){
+	if (ieee->active_scan && (ieee->softmac_features & IEEE_SOFTMAC_PROBERQ)) {
 		ieee80211_send_probe(ieee);
 		ieee80211_send_probe(ieee);
 	}
@@ -587,7 +587,7 @@ static void ieee80211_softmac_stop_scan(struct ieee80211_device *ieee)
 	down(&ieee->scan_sem);
 //	spin_lock_irqsave(&ieee->lock, flags);
 
-	if (ieee->scanning == 1){
+	if (ieee->scanning == 1) {
 		ieee->scanning = 0;
 
 		cancel_delayed_work(&ieee->softmac_scan_wq);
@@ -609,15 +609,15 @@ EXPORT_SYMBOL(ieee80211_stop_scan);
 /* called with ieee->lock held */
 static void ieee80211_start_scan(struct ieee80211_device *ieee)
 {
-	if(IS_DOT11D_ENABLE(ieee) )
+	if (IS_DOT11D_ENABLE(ieee) )
 	{
-		if(IS_COUNTRY_IE_VALID(ieee))
+		if (IS_COUNTRY_IE_VALID(ieee))
 		{
 			RESET_CIE_WATCHDOG(ieee);
 		}
 	}
 	if (ieee->softmac_features & IEEE_SOFTMAC_SCAN){
-		if (ieee->scanning == 0){
+		if (ieee->scanning == 0) {
 			ieee->scanning = 1;
 			queue_delayed_work(ieee->wq, &ieee->softmac_scan_wq, 0);
 		}
@@ -629,9 +629,9 @@ static void ieee80211_start_scan(struct ieee80211_device *ieee)
 /* called with wx_sem held */
 void ieee80211_start_scan_syncro(struct ieee80211_device *ieee)
 {
-	if(IS_DOT11D_ENABLE(ieee) )
+	if (IS_DOT11D_ENABLE(ieee) )
 	{
-		if(IS_COUNTRY_IE_VALID(ieee))
+		if (IS_COUNTRY_IE_VALID(ieee))
 		{
 			RESET_CIE_WATCHDOG(ieee);
 		}
@@ -739,7 +739,7 @@ static struct sk_buff *ieee80211_probe_resp(struct ieee80211_device *ieee, u8 *d
 	HTConstructInfoElement(ieee,tmp_ht_info_buf,&tmp_ht_info_len, encrypt);
 
 
-	if(pHTInfo->bRegRT2RTAggregation)
+	if (pHTInfo->bRegRT2RTAggregation)
 	{
 		tmp_generic_ie_buf = ieee->pHTInfo->szRT2RTAggBuffer;
 		tmp_generic_ie_len = sizeof(ieee->pHTInfo->szRT2RTAggBuffer);
@@ -803,7 +803,7 @@ static struct sk_buff *ieee80211_probe_resp(struct ieee80211_device *ieee, u8 *d
 	*(tag++) = 1;
 	*(tag++) = ieee->current_network.channel;
 
-	if(atim_len){
+	if (atim_len) {
 		*(tag++) = MFIE_TYPE_IBSS_SET;
 		*(tag++) = 2;
 
@@ -812,12 +812,12 @@ static struct sk_buff *ieee80211_probe_resp(struct ieee80211_device *ieee, u8 *d
 		tag+=2;
 	}
 
-	if(erp_len){
+	if (erp_len) {
 		*(tag++) = MFIE_TYPE_ERP;
 		*(tag++) = 1;
 		*(tag++) = erpinfo_content;
 	}
-	if(rate_ex_len){
+	if (rate_ex_len) {
 		*(tag++) = MFIE_TYPE_RATES_EX;
 		*(tag++) = rate_ex_len-2;
 		memcpy(tag, ieee->current_network.rates_ex, rate_ex_len-2);
@@ -877,7 +877,7 @@ static struct sk_buff *ieee80211_assoc_resp(struct ieee80211_device *ieee,
 		crypt = ieee->crypt[ieee->tx_keyidx];
 	else crypt = NULL;
 
-	encrypt = (crypt && crypt->ops);
+	encrypt = crypt && crypt->ops;
 
 	if (encrypt)
 		assoc->capability |= cpu_to_le16(WLAN_CAPABILITY_PRIVACY);
@@ -1016,12 +1016,12 @@ inline struct sk_buff *ieee80211_association_req(struct ieee80211_network *beaco
 	encrypt = ieee->host_encrypt && crypt && crypt->ops && ((0 == strcmp(crypt->ops->name,"WEP") || wpa_ie_len));
 
 	//Include High Throuput capability && Realtek proprietary
-	if(ieee->pHTInfo->bCurrentHTSupport&&ieee->pHTInfo->bEnableHT)
+	if (ieee->pHTInfo->bCurrentHTSupport&&ieee->pHTInfo->bEnableHT)
 	{
 		ht_cap_buf = (u8 *)&(ieee->pHTInfo->SelfHTCap);
 		ht_cap_len = sizeof(ieee->pHTInfo->SelfHTCap);
 		HTConstructCapabilityElement(ieee, ht_cap_buf, &ht_cap_len, encrypt);
-		if(ieee->pHTInfo->bCurrentRT2RTAggregation)
+		if (ieee->pHTInfo->bCurrentRT2RTAggregation)
 		{
 			realtek_ie_buf = ieee->pHTInfo->szRT2RTAggBuffer;
 			realtek_ie_len = sizeof( ieee->pHTInfo->szRT2RTAggBuffer);
@@ -1029,16 +1029,16 @@ inline struct sk_buff *ieee80211_association_req(struct ieee80211_network *beaco
 
 		}
 	}
-	if(ieee->qos_support){
+	if (ieee->qos_support) {
 		wmm_info_len = beacon->qos_data.supported?9:0;
 	}
 
 
-	if(beacon->bCkipSupported)
+	if (beacon->bCkipSupported)
 	{
 		ckip_ie_len = 30+2;
 	}
-	if(beacon->bCcxRmEnable)
+	if (beacon->bCcxRmEnable)
 	{
 		ccxrm_ie_len = 6+2;
 	}
@@ -1141,7 +1141,7 @@ inline struct sk_buff *ieee80211_association_req(struct ieee80211_network *beaco
 		tag += osCcxAironetIE.Length;
 	}
 
-	if(beacon->bCcxRmEnable)
+	if (beacon->bCcxRmEnable)
 	{
 		static u8 CcxRmCapBuf[] = {0x00, 0x40, 0x96, 0x01, 0x01, 0x00};
 		OCTET_STRING osCcxRmCap;
@@ -1168,8 +1168,8 @@ inline struct sk_buff *ieee80211_association_req(struct ieee80211_network *beaco
 		tag += osCcxVerNum.Length;
 	}
 	//HT cap element
-	if(ieee->pHTInfo->bCurrentHTSupport&&ieee->pHTInfo->bEnableHT){
-		if(ieee->pHTInfo->ePeerHTSpecVer != HT_SPEC_VER_EWC)
+	if (ieee->pHTInfo->bCurrentHTSupport && ieee->pHTInfo->bEnableHT) {
+		if (ieee->pHTInfo->ePeerHTSpecVer != HT_SPEC_VER_EWC)
 		{
 			tag = skb_put(skb, ht_cap_len);
 			*tag++ = MFIE_TYPE_HT_CAP;
@@ -1182,22 +1182,22 @@ inline struct sk_buff *ieee80211_association_req(struct ieee80211_network *beaco
 
 	//choose what wpa_supplicant gives to associate.
 	tag = skb_put(skb, wpa_ie_len);
-	if (wpa_ie_len){
+	if (wpa_ie_len) {
 		memcpy(tag, ieee->wpa_ie, ieee->wpa_ie_len);
 	}
 
 	tag = skb_put(skb, wmm_info_len);
-	if(wmm_info_len) {
+	if (wmm_info_len) {
 	  ieee80211_WMM_Info(ieee, &tag);
 	}
 #ifdef THOMAS_TURBO
 	tag = skb_put(skb, turbo_info_len);
-	if(turbo_info_len) {
+	if (turbo_info_len) {
 		ieee80211_TURBO_Info(ieee, &tag);
 	}
 #endif
 
-	if(ieee->pHTInfo->bCurrentHTSupport&&ieee->pHTInfo->bEnableHT){
+	if (ieee->pHTInfo->bCurrentHTSupport && ieee->pHTInfo->bEnableHT) {
 		if(ieee->pHTInfo->ePeerHTSpecVer == HT_SPEC_VER_EWC)
 		{
 			tag = skb_put(skb, ht_cap_len);
@@ -1207,7 +1207,7 @@ inline struct sk_buff *ieee80211_association_req(struct ieee80211_network *beaco
 			tag += ht_cap_len -2;
 		}
 
-		if(ieee->pHTInfo->bCurrentRT2RTAggregation){
+		if (ieee->pHTInfo->bCurrentRT2RTAggregation) {
 			tag = skb_put(skb, realtek_ie_len);
 			*tag++ = MFIE_TYPE_GENERIC;
 			*tag++ = realtek_ie_len - 2;
@@ -1273,7 +1273,7 @@ static void ieee80211_associate_step1(struct ieee80211_device *ieee)
 		//printk(KERN_WARNING "Sending authentication request\n");
 		softmac_mgmt_xmit(skb, ieee);
 		//BUGON when you try to add_timer twice, using mod_timer may be better, john0709
-		if(!timer_pending(&ieee->associate_timer)){
+		if (!timer_pending(&ieee->associate_timer)) {
 			ieee->associate_timer.expires = jiffies + (HZ / 2);
 			add_timer(&ieee->associate_timer);
 		}
@@ -1358,7 +1358,7 @@ static void ieee80211_associate_complete_wq(struct work_struct *work)
 	}
 	ieee->LinkDetectInfo.SlotNum = 2 * (1 + ieee->current_network.beacon_interval/500);
 	// To prevent the immediately calling watch_dog after association.
-	if(ieee->LinkDetectInfo.NumRecvBcnInPeriod==0||ieee->LinkDetectInfo.NumRecvDataInPeriod==0 )
+	if (ieee->LinkDetectInfo.NumRecvBcnInPeriod==0||ieee->LinkDetectInfo.NumRecvDataInPeriod==0 )
 	{
 		ieee->LinkDetectInfo.NumRecvBcnInPeriod = 1;
 		ieee->LinkDetectInfo.NumRecvDataInPeriod= 1;
@@ -1371,7 +1371,7 @@ static void ieee80211_associate_complete_wq(struct work_struct *work)
 	else if(ieee->is_silent_reset == 1)
 	{
 		printk("==================>silent reset associate\n");
-		ieee->is_silent_reset = 0;
+		ieee->is_silent_reset = false;
 	}
 
 	if (ieee->data_hard_resume)
@@ -1430,7 +1430,7 @@ inline void ieee80211_softmac_new_net(struct ieee80211_device *ieee, struct ieee
 		return;
 
 
-	if (ieee->iw_mode == IW_MODE_INFRA || ieee->iw_mode == IW_MODE_ADHOC){
+	if (ieee->iw_mode == IW_MODE_INFRA || ieee->iw_mode == IW_MODE_ADHOC) {
 		/* if the user specified the AP MAC, we need also the essid
 		 * This could be obtained by beacons or, if the network does not
 		 * broadcast it, it can be put manually.
@@ -1458,13 +1458,13 @@ inline void ieee80211_softmac_new_net(struct ieee80211_device *ieee, struct ieee
 				/* if the essid is hidden replace it with the
 				* essid provided by the user.
 				*/
-				if (!ssidbroad){
+				if (!ssidbroad) {
 					strncpy(tmp_ssid, ieee->current_network.ssid, IW_ESSID_MAX_SIZE);
 					tmp_ssid_len = ieee->current_network.ssid_len;
 				}
 				memcpy(&ieee->current_network, net, sizeof(struct ieee80211_network));
 
-				if (!ssidbroad){
+				if (!ssidbroad) {
 					strncpy(ieee->current_network.ssid, tmp_ssid, IW_ESSID_MAX_SIZE);
 					ieee->current_network.ssid_len = tmp_ssid_len;
 				}
@@ -1541,16 +1541,16 @@ static inline u16 auth_parse(struct sk_buff *skb, u8 **challenge, int *chlen)
 {
 	struct ieee80211_authentication *a;
 	u8 *t;
-	if (skb->len <  (sizeof(struct ieee80211_authentication)-sizeof(struct ieee80211_info_element))){
+	if (skb->len < (sizeof(struct ieee80211_authentication) - sizeof(struct ieee80211_info_element))) {
 		IEEE80211_DEBUG_MGMT("invalid len in auth resp: %d\n",skb->len);
 		return 0xcafe;
 	}
 	*challenge = NULL;
 	a = (struct ieee80211_authentication *) skb->data;
-	if(skb->len > (sizeof(struct ieee80211_authentication) +3)){
+	if (skb->len > (sizeof(struct ieee80211_authentication) + 3)) {
 		t = skb->data + sizeof(struct ieee80211_authentication);
 
-		if(*(t++) == MFIE_TYPE_CHALLENGE){
+		if (*(t++) == MFIE_TYPE_CHALLENGE) {
 			*chlen = *(t++);
 			*challenge = kmemdup(t, *chlen, GFP_ATOMIC);
 			if (!*challenge)
@@ -1567,7 +1567,7 @@ static int auth_rq_parse(struct sk_buff *skb, u8 *dest)
 {
 	struct ieee80211_authentication *a;
 
-	if (skb->len <  (sizeof(struct ieee80211_authentication)-sizeof(struct ieee80211_info_element))){
+	if (skb->len < (sizeof(struct ieee80211_authentication) - sizeof(struct ieee80211_info_element))) {
 		IEEE80211_DEBUG_MGMT("invalid len in auth request: %d\n",skb->len);
 		return -1;
 	}
@@ -1601,7 +1601,7 @@ static short probe_rq_parse(struct ieee80211_device *ieee, struct sk_buff *skb,
 	tag = skb->data + sizeof (struct ieee80211_hdr_3addr  );
 
 	while (tag+1 < skbend){
-		if (*tag == 0){
+		if (*tag == 0) {
 			ssid = tag+2;
 			ssidlen = *(tag+1);
 			break;
@@ -1642,7 +1642,7 @@ static inline u16 assoc_parse(struct ieee80211_device *ieee, struct sk_buff *skb
 	struct ieee80211_assoc_response_frame *response_head;
 	u16 status_code;
 
-	if (skb->len <  sizeof(struct ieee80211_assoc_response_frame)){
+	if (skb->len < sizeof(struct ieee80211_assoc_response_frame)) {
 		IEEE80211_DEBUG_MGMT("invalid len in auth resp: %d\n", skb->len);
 		return 0xcafe;
 	}
@@ -1672,7 +1672,7 @@ ieee80211_rx_probe_rq(struct ieee80211_device *ieee, struct sk_buff *skb)
 	//IEEE80211DMESG("Rx probe");
 	ieee->softmac_stats.rx_probe_rq++;
 	//DMESG("Dest is "MACSTR, MAC2STR(dest));
-	if (probe_rq_parse(ieee, skb, dest)){
+	if (probe_rq_parse(ieee, skb, dest)) {
 		//IEEE80211DMESG("Was for me!");
 		ieee->softmac_stats.tx_probe_rs++;
 		ieee80211_resp_to_probe(ieee, dest);
@@ -1703,7 +1703,7 @@ ieee80211_rx_assoc_rq(struct ieee80211_device *ieee, struct sk_buff *skb)
 	//unsigned long flags;
 
 	ieee->softmac_stats.rx_ass_rq++;
-	if (assoc_rq_parse(skb,dest) != -1){
+	if (assoc_rq_parse(skb, dest) != -1) {
 		ieee80211_resp_to_assoc_rq(ieee, dest);
 	}
 
@@ -1755,13 +1755,13 @@ static short ieee80211_sta_ps_sleep(struct ieee80211_device *ieee, u32 *time_h,
 		(ieee->mgmt_queue_tail != ieee->mgmt_queue_head))
 		return 0;
 
-	if(time_l){
+	if (time_l) {
 		*time_l = ieee->current_network.last_dtim_sta_time[0]
 			+ (ieee->current_network.beacon_interval
 			* ieee->current_network.dtim_period) * 1000;
 	}
 
-	if(time_h){
+	if (time_h) {
 		*time_h = ieee->current_network.last_dtim_sta_time[1];
 		if(time_l && *time_l < ieee->current_network.last_dtim_sta_time[0])
 			*time_h += 1;
@@ -1782,7 +1782,7 @@ static inline void ieee80211_sta_ps(struct ieee80211_device *ieee)
 
 	spin_lock_irqsave(&ieee->lock, flags);
 
-	if((ieee->ps == IEEE80211_PS_DISABLED ||
+	if ((ieee->ps == IEEE80211_PS_DISABLED ||
 		ieee->iw_mode != IW_MODE_INFRA ||
 		ieee->state != IEEE80211_LINKED)){
 
@@ -1841,8 +1841,8 @@ out:
 
 void ieee80211_sta_wakeup(struct ieee80211_device *ieee, short nl)
 {
-	if(ieee->sta_sleep == 0){
-		if(nl){
+	if (ieee->sta_sleep == 0) {
+		if (nl) {
 			printk("Warning: driver is probably failing to report TX ps error\n");
 			ieee->ps_request_tx_ack(ieee->dev);
 			ieee80211_sta_ps_send_null_frame(ieee, 0);
@@ -1856,7 +1856,7 @@ void ieee80211_sta_wakeup(struct ieee80211_device *ieee, short nl)
 
 	ieee->sta_sleep = 0;
 
-	if(nl){
+	if (nl) {
 		ieee->ps_request_tx_ack(ieee->dev);
 		ieee80211_sta_ps_send_null_frame(ieee, 0);
 	}
@@ -1870,7 +1870,7 @@ void ieee80211_ps_tx_ack(struct ieee80211_device *ieee, short success)
 
 	if(ieee->sta_sleep == 2){
 		/* Null frame with PS bit set */
-		if(success){
+		if (success) {
 			ieee->sta_sleep = 1;
 			ieee->enter_sleep_state(ieee->dev,ieee->ps_th,ieee->ps_tl);
 		}
@@ -1881,7 +1881,7 @@ void ieee80211_ps_tx_ack(struct ieee80211_device *ieee, short success)
 	/* 21112005 - tx again null without PS bit if lost */
 	else {
 
-		if((ieee->sta_sleep == 0) && !success){
+		if ((ieee->sta_sleep == 0) && !success) {
 			spin_lock_irqsave(&ieee->mgmt_tx_lock, flags2);
 			ieee80211_sta_ps_send_null_frame(ieee, 0);
 			spin_unlock_irqrestore(&ieee->mgmt_tx_lock, flags2);
@@ -1920,6 +1920,66 @@ static void ieee80211_process_action(struct ieee80211_device *ieee,
 	return;
 
 }
+
+static void ieee80211_check_auth_response(struct ieee80211_device *ieee,
+					  struct sk_buff *skb)
+{
+	/* default support N mode, disable halfNmode */
+	bool bSupportNmode = true, bHalfSupportNmode = false;
+	u16 errcode;
+	u8 *challenge;
+	int chlen = 0;
+	u32 iotAction;
+
+	errcode = auth_parse(skb, &challenge, &chlen);
+	if (!errcode) {
+		if (ieee->open_wep || !challenge) {
+			ieee->state = IEEE80211_ASSOCIATING_AUTHENTICATED;
+			ieee->softmac_stats.rx_auth_rs_ok++;
+			iotAction = ieee->pHTInfo->IOTAction;
+			if (!(iotAction & HT_IOT_ACT_PURE_N_MODE)) {
+				if (!ieee->GetNmodeSupportBySecCfg(ieee->dev)) {
+					/* WEP or TKIP encryption */
+					if (IsHTHalfNmodeAPs(ieee)) {
+						bSupportNmode = true;
+						bHalfSupportNmode = true;
+					} else {
+						bSupportNmode = false;
+						bHalfSupportNmode = false;
+					}
+					netdev_dbg(ieee->dev, "SEC(%d, %d)\n",
+							bSupportNmode,
+							bHalfSupportNmode);
+				}
+			}
+			/* Dummy wirless mode setting- avoid encryption issue */
+			if (bSupportNmode) {
+				/* N mode setting */
+				ieee->SetWirelessMode(ieee->dev,
+						ieee->current_network.mode);
+			} else {
+				/* b/g mode setting - TODO */
+				ieee->SetWirelessMode(ieee->dev, IEEE_G);
+			}
+
+			if (ieee->current_network.mode == IEEE_N_24G &&
+					bHalfSupportNmode == true) {
+				netdev_dbg(ieee->dev, "enter half N mode\n");
+				ieee->bHalfWirelessN24GMode = true;
+			} else
+				ieee->bHalfWirelessN24GMode = false;
+
+			ieee80211_associate_step2(ieee);
+		} else {
+			ieee80211_auth_challenge(ieee, challenge, chlen);
+		}
+	} else {
+		ieee->softmac_stats.rx_auth_rs_err++;
+		IEEE80211_DEBUG_MGMT("Auth response status code 0x%x", errcode);
+		ieee80211_associate_abort(ieee);
+	}
+}
+
 inline int
 ieee80211_rx_frame_softmac(struct ieee80211_device *ieee, struct sk_buff *skb,
 			struct ieee80211_rx_stats *rx_stats, u16 type,
@@ -1927,12 +1987,9 @@ ieee80211_rx_frame_softmac(struct ieee80211_device *ieee, struct sk_buff *skb,
 {
 	struct ieee80211_hdr_3addr *header = (struct ieee80211_hdr_3addr *) skb->data;
 	u16 errcode;
-	u8 *challenge;
-	int chlen=0;
 	int aid;
 	struct ieee80211_assoc_response_frame *assoc_resp;
 //	struct ieee80211_info_element *info_element;
-	bool bSupportNmode = true, bHalfSupportNmode = false; //default support N mode, disable halfNmode
 
 	if(!ieee->proto_started)
 		return 0;
@@ -1967,7 +2024,7 @@ ieee80211_rx_frame_softmac(struct ieee80211_device *ieee, struct sk_buff *skb,
 				ieee->softmac_stats.rx_ass_ok++;
 				/* station support qos */
 				/* Let the register setting defaultly with Legacy station */
-				if(ieee->qos_support) {
+				if (ieee->qos_support) {
 					assoc_resp = (struct ieee80211_assoc_response_frame *)skb->data;
 					memset(network, 0, sizeof(*network));
 					if (ieee80211_parse_info_param(ieee,assoc_resp->info_element,\
@@ -2013,68 +2070,16 @@ ieee80211_rx_frame_softmac(struct ieee80211_device *ieee, struct sk_buff *skb,
 
 	case IEEE80211_STYPE_AUTH:
 
-		if (ieee->softmac_features & IEEE_SOFTMAC_ASSOCIATE){
-			if (ieee->state == IEEE80211_ASSOCIATING_AUTHENTICATING &&
-			ieee->iw_mode == IW_MODE_INFRA){
+		if (ieee->softmac_features & IEEE_SOFTMAC_ASSOCIATE) {
+			if (ieee->state == IEEE80211_ASSOCIATING_AUTHENTICATING
+				&& ieee->iw_mode == IW_MODE_INFRA) {
 
-					IEEE80211_DEBUG_MGMT("Received authentication response");
-
-					errcode = auth_parse(skb, &challenge, &chlen);
-					if (!errcode) {
-						if(ieee->open_wep || !challenge){
-							ieee->state = IEEE80211_ASSOCIATING_AUTHENTICATED;
-							ieee->softmac_stats.rx_auth_rs_ok++;
-							if(!(ieee->pHTInfo->IOTAction&HT_IOT_ACT_PURE_N_MODE))
-							{
-								if (!ieee->GetNmodeSupportBySecCfg(ieee->dev))
-								{
-											// WEP or TKIP encryption
-									if(IsHTHalfNmodeAPs(ieee))
-									{
-										bSupportNmode = true;
-										bHalfSupportNmode = true;
-									}
-									else
-									{
-										bSupportNmode = false;
-										bHalfSupportNmode = false;
-									}
-								printk("==========>to link with AP using SEC(%d, %d)", bSupportNmode, bHalfSupportNmode);
-								}
-							}
-							/* Dummy wirless mode setting to avoid encryption issue */
-							if(bSupportNmode) {
-								//N mode setting
-								ieee->SetWirelessMode(ieee->dev, \
-										ieee->current_network.mode);
-							}else{
-								//b/g mode setting
-								/*TODO*/
-								ieee->SetWirelessMode(ieee->dev, IEEE_G);
-							}
-
-							if (ieee->current_network.mode == IEEE_N_24G && bHalfSupportNmode == true)
-							{
-								printk("===============>entern half N mode\n");
-								ieee->bHalfWirelessN24GMode = true;
-							}
-							else
-								ieee->bHalfWirelessN24GMode = false;
-
-							ieee80211_associate_step2(ieee);
-						}else{
-							ieee80211_auth_challenge(ieee, challenge, chlen);
-						}
-					}else{
-						ieee->softmac_stats.rx_auth_rs_err++;
-						IEEE80211_DEBUG_MGMT("Authentication response status code 0x%x",errcode);
-						ieee80211_associate_abort(ieee);
-					}
-
-				}else if (ieee->iw_mode == IW_MODE_MASTER){
-					ieee80211_rx_auth_rq(ieee, skb);
-				}
+				IEEE80211_DEBUG_MGMT("Received auth response");
+				ieee80211_check_auth_response(ieee, skb);
+			} else if (ieee->iw_mode == IW_MODE_MASTER) {
+				ieee80211_rx_auth_rq(ieee, skb);
 			}
+		}
 		break;
 
 	case IEEE80211_STYPE_PROBE_REQ:
@@ -2110,27 +2115,26 @@ ieee80211_rx_frame_softmac(struct ieee80211_device *ieee, struct sk_buff *skb,
 		break;
 	default:
 		return -1;
-		break;
 	}
 
 	//dev_kfree_skb_any(skb);
 	return 0;
 }
 
-/* following are for a simpler TX queue management.
- * Instead of using netif_[stop/wake]_queue the driver
- * will uses these two function (plus a reset one), that
- * will internally uses the kernel netif_* and takes
- * care of the ieee802.11 fragmentation.
- * So the driver receives a fragment per time and might
- * call the stop function when it want without take care
- * to have enought room to TX an entire packet.
- * This might be useful if each fragment need it's own
- * descriptor, thus just keep a total free memory > than
- * the max fragmentation treshold is not enought.. If the
- * ieee802.11 stack passed a TXB struct then you needed
+/* The following are for a simpler TX queue management.
+ * Instead of using netif_[stop/wake]_queue, the driver
+ * will use these two functions (plus a reset one) that
+ * will internally call the kernel netif_* and take care
+ * of the ieee802.11 fragmentation.
+ * So, the driver receives a fragment at a time and might
+ * call the stop function when it wants, without taking
+ * care to have enough room to TX an entire packet.
+ * This might be useful if each fragment needs its own
+ * descriptor. Thus, just keeping a total free memory > than
+ * the max fragmentation threshold is not enough. If the
+ * ieee802.11 stack passed a TXB struct, then you would need
  * to keep N free descriptors where
- * N = MAX_PACKET_SIZE / MIN_FRAG_TRESHOLD
+ * N = MAX_PACKET_SIZE / MIN_FRAG_THRESHOLD.
  * In this way you need just one and the 802.11 stack
  * will take care of buffering fragments and pass them to
  * to the driver later, when it wakes the queue.
@@ -2152,7 +2156,7 @@ void ieee80211_softmac_xmit(struct ieee80211_txb *txb, struct ieee80211_device *
 	ieee->stats.tx_bytes += txb->payload_size;
 	ieee->stats.tx_packets++;
 	tcb_desc = (cb_desc *)(txb->fragments[0]->cb + MAX_DEV_ADDR_SIZE);
-	if(tcb_desc->bMulticast) {
+	if (tcb_desc->bMulticast) {
 		ieee->stats.multicast++;
 	}
 	/* if xmit available, just xmit it immediately, else just insert it to the wait queue */
@@ -2224,7 +2228,7 @@ void ieee80211_reset_queue(struct ieee80211_device *ieee)
 
 	spin_lock_irqsave(&ieee->lock, flags);
 	init_mgmt_queue(ieee);
-	if (ieee->tx_pending.txb){
+	if (ieee->tx_pending.txb) {
 		ieee80211_txb_free(ieee->tx_pending.txb);
 		ieee->tx_pending.txb = NULL;
 	}
@@ -2246,7 +2250,7 @@ void ieee80211_wake_queue(struct ieee80211_device *ieee)
 
 	ieee->queue_stop = 0;
 
-	if(ieee->softmac_features & IEEE_SOFTMAC_SINGLE_QUEUE){
+	if (ieee->softmac_features & IEEE_SOFTMAC_SINGLE_QUEUE) {
 		while (!ieee->queue_stop && (skb = dequeue_mgmt(ieee))){
 
 			header = (struct ieee80211_hdr_3addr  *) skb->data;
@@ -2265,7 +2269,7 @@ void ieee80211_wake_queue(struct ieee80211_device *ieee)
 	if (!ieee->queue_stop && ieee->tx_pending.txb)
 		ieee80211_resume_tx(ieee);
 
-	if (!ieee->queue_stop && netif_queue_stopped(ieee->dev)){
+	if (!ieee->queue_stop && netif_queue_stopped(ieee->dev)) {
 		ieee->softmac_stats.swtxawake++;
 		netif_wake_queue(ieee->dev);
 	}
@@ -2280,7 +2284,7 @@ void ieee80211_stop_queue(struct ieee80211_device *ieee)
 	//unsigned long flags;
 	//spin_lock_irqsave(&ieee->lock,flags);
 
-	if (! netif_queue_stopped(ieee->dev)){
+	if (!netif_queue_stopped(ieee->dev)) {
 		netif_stop_queue(ieee->dev);
 		ieee->softmac_stats.swtxstop++;
 	}
@@ -2301,7 +2305,7 @@ void ieee80211_start_master_bss(struct ieee80211_device *ieee)
 {
 	ieee->assoc_id = 1;
 
-	if (ieee->current_network.ssid_len == 0){
+	if (ieee->current_network.ssid_len == 0) {
 		strncpy(ieee->current_network.ssid,
 			IEEE80211_DEFAULT_TX_ESSID,
 			IW_ESSID_MAX_SIZE);
@@ -2325,7 +2329,7 @@ void ieee80211_start_master_bss(struct ieee80211_device *ieee)
 
 static void ieee80211_start_monitor_mode(struct ieee80211_device *ieee)
 {
-	if(ieee->raw_tx){
+	if (ieee->raw_tx) {
 
 		if (ieee->data_hard_resume)
 			ieee->data_hard_resume(ieee->dev);
@@ -2345,13 +2349,13 @@ static void ieee80211_start_ibss_wq(struct work_struct *work)
 	 * (abort) this wq (when syncro scanning) before sleeping
 	 * on the semaphore
 	 */
-	if(!ieee->proto_started){
+	if (!ieee->proto_started) {
 		printk("==========oh driver down return\n");
 		return;
 	}
 	down(&ieee->wx_sem);
 
-	if (ieee->current_network.ssid_len == 0){
+	if (ieee->current_network.ssid_len == 0) {
 		strcpy(ieee->current_network.ssid, IEEE80211_DEFAULT_TX_ESSID);
 		ieee->current_network.ssid_len = strlen(IEEE80211_DEFAULT_TX_ESSID);
 		ieee->ssid_set = 1;
@@ -2382,7 +2386,7 @@ static void ieee80211_start_ibss_wq(struct work_struct *work)
 		ieee80211_start_scan_syncro(ieee);
 
 	/* the network definitively is not here.. create a new cell */
-	if (ieee->state == IEEE80211_NOLINK){
+	if (ieee->state == IEEE80211_NOLINK) {
 		printk("creating new IBSS cell\n");
 		if(!ieee->wap_set)
 			ieee80211_randomize_cell(ieee);
@@ -2456,9 +2460,9 @@ void ieee80211_start_bss(struct ieee80211_device *ieee)
 	// Ref: 802.11d 11.1.3.3
 	// STA shall not start a BSS unless properly formed Beacon frame including a Country IE.
 	//
-	if(IS_DOT11D_ENABLE(ieee) && !IS_COUNTRY_IE_VALID(ieee))
+	if (IS_DOT11D_ENABLE(ieee) && !IS_COUNTRY_IE_VALID(ieee))
 	{
-		if(! ieee->bGlobalDomain)
+		if (! ieee->bGlobalDomain)
 		{
 			return;
 		}
@@ -2479,7 +2483,7 @@ void ieee80211_start_bss(struct ieee80211_device *ieee)
 	 */
 	spin_lock_irqsave(&ieee->lock, flags);
 
-	if (ieee->state == IEEE80211_NOLINK){
+	if (ieee->state == IEEE80211_NOLINK) {
 		ieee->actscanning = true;
 		ieee80211_start_scan(ieee);
 	}
@@ -2633,7 +2637,7 @@ void ieee80211_start_protocol(struct ieee80211_device *ieee)
 
 	ieee->proto_started = 1;
 
-	if (ieee->current_network.channel == 0){
+	if (ieee->current_network.channel == 0) {
 		do{
 			ch++;
 			if (ch > MAX_CHANNEL_NUMBER)
@@ -2719,16 +2723,14 @@ void ieee80211_softmac_init(struct ieee80211_device *ieee)
 	ieee->sta_edca_param[2] = 0x005E4342;
 	ieee->sta_edca_param[3] = 0x002F3262;
 	ieee->aggregation = true;
-	ieee->enable_rx_imm_BA = 1;
+	ieee->enable_rx_imm_BA = true;
 	ieee->tx_pending.txb = NULL;
 
-	init_timer(&ieee->associate_timer);
-	ieee->associate_timer.data = (unsigned long)ieee;
-	ieee->associate_timer.function = ieee80211_associate_abort_cb;
+	setup_timer(&ieee->associate_timer, ieee80211_associate_abort_cb,
+		    (unsigned long)ieee);
 
-	init_timer(&ieee->beacon_timer);
-	ieee->beacon_timer.data = (unsigned long) ieee;
-	ieee->beacon_timer.function = ieee80211_send_beacon_cb;
+	setup_timer(&ieee->beacon_timer, ieee80211_send_beacon_cb,
+		    (unsigned long)ieee);
 
 	ieee->wq = create_workqueue(DRV_NAME);
 
@@ -3027,12 +3029,11 @@ static int ieee80211_wpa_set_encryption(struct ieee80211_device *ieee,
 
 		ieee80211_crypt_delayed_deinit(ieee, crypt);
 
-		new_crypt = kmalloc(sizeof(*new_crypt), GFP_KERNEL);
+		new_crypt = kzalloc(sizeof(*new_crypt), GFP_KERNEL);
 		if (new_crypt == NULL) {
 			ret = -ENOMEM;
 			goto done;
 		}
-		memset(new_crypt, 0, sizeof(struct ieee80211_crypt_data));
 		new_crypt->ops = ops;
 		if (new_crypt->ops && try_module_get(new_crypt->ops->owner))
 			new_crypt->priv =
@@ -3140,7 +3141,7 @@ SendDisassociation(
 		struct ieee80211_network *beacon = &ieee->current_network;
 		struct sk_buff *skb;
 		skb = ieee80211_disassociate_skb(beacon,ieee,asRsn);
-		if (skb){
+		if (skb) {
 				softmac_mgmt_xmit(skb, ieee);
 				//dev_kfree_skb_any(skb);//edit by thomas
 		}
@@ -3155,7 +3156,7 @@ int ieee80211_wpa_supplicant_ioctl(struct ieee80211_device *ieee, struct iw_poin
 	down(&ieee->wx_sem);
 	//IEEE_DEBUG_INFO("wpa_supplicant: len=%d\n", p->length);
 
-	if (p->length < sizeof(struct ieee_param) || !p->pointer){
+	if (p->length < sizeof(struct ieee_param) || !p->pointer) {
 		ret = -EINVAL;
 		goto out;
 	}
@@ -3210,7 +3211,7 @@ void notify_wx_assoc_event(struct ieee80211_device *ieee)
 	if (ieee->state == IEEE80211_LINKED)
 		memcpy(wrqu.ap_addr.sa_data, ieee->current_network.bssid, ETH_ALEN);
 	else
-		memset(wrqu.ap_addr.sa_data, 0, ETH_ALEN);
+		eth_zero_addr(wrqu.ap_addr.sa_data);
 	wireless_send_event(ieee->dev, SIOCGIWAP, &wrqu, NULL);
 }
 EXPORT_SYMBOL(notify_wx_assoc_event);
diff --git a/drivers/staging/rtl8192u/ieee80211/ieee80211_softmac_wx.c b/drivers/staging/rtl8192u/ieee80211/ieee80211_softmac_wx.c
index 644368df6342..714fbcace72b 100644
--- a/drivers/staging/rtl8192u/ieee80211/ieee80211_softmac_wx.c
+++ b/drivers/staging/rtl8192u/ieee80211/ieee80211_softmac_wx.c
@@ -36,7 +36,7 @@ int ieee80211_wx_set_freq(struct ieee80211_device *ieee, struct iw_request_info
 
 	down(&ieee->wx_sem);
 
-	if(ieee->iw_mode == IW_MODE_INFRA){
+	if (ieee->iw_mode == IW_MODE_INFRA) {
 		ret = -EOPNOTSUPP;
 		goto out;
 	}
@@ -121,7 +121,7 @@ int ieee80211_wx_get_wap(struct ieee80211_device *ieee,
 		ieee->state != IEEE80211_LINKED_SCANNING &&
 		ieee->wap_set == 0)
 
-		memset(wrqu->ap_addr.sa_data, 0, ETH_ALEN);
+		eth_zero_addr(wrqu->ap_addr.sa_data);
 	else
 		memcpy(wrqu->ap_addr.sa_data,
 		       ieee->current_network.bssid, ETH_ALEN);
@@ -148,12 +148,12 @@ int ieee80211_wx_set_wap(struct ieee80211_device *ieee,
 
 	down(&ieee->wx_sem);
 	/* use ifconfig hw ether */
-	if (ieee->iw_mode == IW_MODE_MASTER){
+	if (ieee->iw_mode == IW_MODE_MASTER) {
 		ret = -1;
 		goto out;
 	}
 
-	if (temp->sa_family != ARPHRD_ETHER){
+	if (temp->sa_family != ARPHRD_ETHER) {
 		ret = -EINVAL;
 		goto out;
 	}
@@ -345,7 +345,7 @@ void ieee80211_wx_sync_scan_wq(struct work_struct *work)
 	ieee->state = IEEE80211_LINKED;
 	ieee->link_change(ieee->dev);
 	// To prevent the immediately calling watch_dog after scan.
-	if(ieee->LinkDetectInfo.NumRecvBcnInPeriod==0||ieee->LinkDetectInfo.NumRecvDataInPeriod==0 )
+	if (ieee->LinkDetectInfo.NumRecvBcnInPeriod==0||ieee->LinkDetectInfo.NumRecvDataInPeriod==0 )
 	{
 		ieee->LinkDetectInfo.NumRecvBcnInPeriod = 1;
 		ieee->LinkDetectInfo.NumRecvDataInPeriod= 1;
@@ -369,7 +369,7 @@ int ieee80211_wx_set_scan(struct ieee80211_device *ieee, struct iw_request_info
 
 	down(&ieee->wx_sem);
 
-	if (ieee->iw_mode == IW_MODE_MONITOR || !(ieee->proto_started)){
+	if (ieee->iw_mode == IW_MODE_MONITOR || !(ieee->proto_started)) {
 		ret = -1;
 		goto out;
 	}
@@ -400,12 +400,12 @@ int ieee80211_wx_set_essid(struct ieee80211_device *ieee,
 
 	proto_started = ieee->proto_started;
 
-	if (wrqu->essid.length > IW_ESSID_MAX_SIZE){
+	if (wrqu->essid.length > IW_ESSID_MAX_SIZE) {
 		ret= -E2BIG;
 		goto out;
 	}
 
-	if (ieee->iw_mode == IW_MODE_MONITOR){
+	if (ieee->iw_mode == IW_MODE_MONITOR) {
 		ret= -1;
 		goto out;
 	}
@@ -469,9 +469,9 @@ EXPORT_SYMBOL(ieee80211_wx_get_mode);
 	printk(KERN_INFO"raw TX is %s\n",
 	      ieee->raw_tx ? "enabled" : "disabled");
 
-	if(ieee->iw_mode == IW_MODE_MONITOR)
+	if (ieee->iw_mode == IW_MODE_MONITOR)
 	{
-		if(prev == 0 && ieee->raw_tx){
+		if (prev == 0 && ieee->raw_tx) {
 			if (ieee->data_hard_resume)
 				ieee->data_hard_resume(ieee->dev);
 
@@ -522,7 +522,7 @@ int ieee80211_wx_set_power(struct ieee80211_device *ieee,
 	int ret = 0;
 	down(&ieee->wx_sem);
 
-	if (wrqu->power.disabled){
+	if (wrqu->power.disabled) {
 		ieee->ps = IEEE80211_PS_DISABLED;
 		goto exit;
 	}
@@ -572,7 +572,7 @@ int ieee80211_wx_get_power(struct ieee80211_device *ieee,
 {
 	down(&ieee->wx_sem);
 
-	if(ieee->ps == IEEE80211_PS_DISABLED){
+	if (ieee->ps == IEEE80211_PS_DISABLED) {
 		wrqu->power.disabled = 1;
 		goto exit;
 	}
diff --git a/drivers/staging/rtl8192u/ieee80211/ieee80211_tx.c b/drivers/staging/rtl8192u/ieee80211/ieee80211_tx.c
index fca73c7c9fbe..9f68c652fb2b 100644
--- a/drivers/staging/rtl8192u/ieee80211/ieee80211_tx.c
+++ b/drivers/staging/rtl8192u/ieee80211/ieee80211_tx.c
@@ -32,7 +32,6 @@
 ******************************************************************************/
 
 #include <linux/compiler.h>
-//#include <linux/config.h>
 #include <linux/errno.h>
 #include <linux/if_arp.h>
 #include <linux/in6.h>
@@ -554,16 +553,16 @@ static void ieee80211_txrate_selectmode(struct ieee80211_device *ieee,
 #ifdef TO_DO_LIST
 	if(!IsDataFrame(pFrame))
 	{
-		pTcb->bTxDisableRateFallBack = TRUE;
-		pTcb->bTxUseDriverAssingedRate = TRUE;
+		pTcb->bTxDisableRateFallBack = true;
+		pTcb->bTxUseDriverAssingedRate = true;
 		pTcb->RATRIndex = 7;
 		return;
 	}
 
 	if(pMgntInfo->ForcedDataRate!= 0)
 	{
-		pTcb->bTxDisableRateFallBack = TRUE;
-		pTcb->bTxUseDriverAssingedRate = TRUE;
+		pTcb->bTxDisableRateFallBack = true;
+		pTcb->bTxUseDriverAssingedRate = true;
 		return;
 	}
 #endif
diff --git a/drivers/staging/rtl8192u/ieee80211/rtl819x_BAProc.c b/drivers/staging/rtl8192u/ieee80211/rtl819x_BAProc.c
index 1b4623c3f95e..618d2cbc049e 100644
--- a/drivers/staging/rtl8192u/ieee80211/rtl819x_BAProc.c
+++ b/drivers/staging/rtl8192u/ieee80211/rtl819x_BAProc.c
@@ -46,14 +46,14 @@ static u8 TxTsDeleteBA(struct ieee80211_device *ieee, PTX_TS_RECORD pTxTs)
 	u8			bSendDELBA = false;
 
 	// Delete pending BA
-	if(pPendingBa->bValid)
+	if (pPendingBa->bValid)
 	{
 		DeActivateBAEntry(ieee, pPendingBa);
 		bSendDELBA = true;
 	}
 
 	// Delete admitted BA
-	if(pAdmittedBa->bValid)
+	if (pAdmittedBa->bValid)
 	{
 		DeActivateBAEntry(ieee, pAdmittedBa);
 		bSendDELBA = true;
@@ -74,7 +74,7 @@ static u8 RxTsDeleteBA(struct ieee80211_device *ieee, PRX_TS_RECORD pRxTs)
 	PBA_RECORD		pBa = &pRxTs->RxAdmittedBARecord;
 	u8			bSendDELBA = false;
 
-	if(pBa->bValid)
+	if (pBa->bValid)
 	{
 		DeActivateBAEntry(ieee, pBa);
 		bSendDELBA = true;
@@ -363,7 +363,7 @@ int ieee80211_rx_ADDBAReq(struct ieee80211_device *ieee, struct sk_buff *skb)
 
 	printk("====================>rx ADDBAREQ from :%pM\n", dst);
 //some other capability is not ready now.
-	if(	(ieee->current_network.qos_data.active == 0) ||
+	if ((ieee->current_network.qos_data.active == 0) ||
 		(ieee->pHTInfo->bCurrentHTSupport == false)) //||
 	//	(ieee->pStaQos->bEnableRxImmBA == false)	)
 	{
@@ -373,7 +373,7 @@ int ieee80211_rx_ADDBAReq(struct ieee80211_device *ieee, struct sk_buff *skb)
 	}
 	// Search for related traffic stream.
 	// If there is no matched TS, reject the ADDBA request.
-	if(	!GetTs(
+	if (!GetTs(
 			ieee,
 			(PTS_COMMON_INFO *)(&pTS),
 			dst,
@@ -390,7 +390,7 @@ int ieee80211_rx_ADDBAReq(struct ieee80211_device *ieee, struct sk_buff *skb)
 	// We can do much more check here, including BufferSize, AMSDU_Support, Policy, StartSeqCtrl...
 	// I want to check StartSeqCtrl to make sure when we start aggregation!!!
 	//
-	if(pBaParamSet->field.BAPolicy == BA_POLICY_DELAYED)
+	if (pBaParamSet->field.BAPolicy == BA_POLICY_DELAYED)
 	{
 		rc = ADDBA_STATUS_INVALID_PARAM;
 		IEEE80211_DEBUG(IEEE80211_DL_ERR, "BA Policy is not correct in %s()\n", __func__);
@@ -522,7 +522,7 @@ int ieee80211_rx_ADDBARsp(struct ieee80211_device *ieee, struct sk_buff *skb)
 		// We can compare the value of BA parameter set that Peer returned and Self sent.
 		// If it is OK, then admitted. Or we can send DELBA to cancel BA mechanism.
 		//
-		if(pBaParamSet->field.BAPolicy == BA_POLICY_DELAYED)
+		if (pBaParamSet->field.BAPolicy == BA_POLICY_DELAYED)
 		{
 			// Since this is a kind of ADDBA failed, we delay next ADDBA process.
 			pTS->bAddBaReqDelayed = true;
@@ -582,7 +582,7 @@ int ieee80211_rx_DELBA(struct ieee80211_device *ieee, struct sk_buff *skb)
 		return -1;
 	}
 
-	if(ieee->current_network.qos_data.active == 0 ||
+	if (ieee->current_network.qos_data.active == 0 ||
 		ieee->pHTInfo->bCurrentHTSupport == false )
 	{
 		IEEE80211_DEBUG(IEEE80211_DL_ERR, "received DELBA while QOS or HT is not supported(%d, %d)\n",ieee->current_network.qos_data.active, ieee->pHTInfo->bCurrentHTSupport);
@@ -600,7 +600,7 @@ int ieee80211_rx_DELBA(struct ieee80211_device *ieee, struct sk_buff *skb)
 	{
 		PRX_TS_RECORD	pRxTs;
 
-		if( !GetTs(
+		if (!GetTs(
 				ieee,
 				(PTS_COMMON_INFO *)&pRxTs,
 				dst,
@@ -618,7 +618,7 @@ int ieee80211_rx_DELBA(struct ieee80211_device *ieee, struct sk_buff *skb)
 	{
 		PTX_TS_RECORD	pTxTs;
 
-		if(!GetTs(
+		if (!GetTs(
 			ieee,
 			(PTS_COMMON_INFO *)&pTxTs,
 			dst,
diff --git a/drivers/staging/rtl8192u/ieee80211/rtl819x_HTProc.c b/drivers/staging/rtl8192u/ieee80211/rtl819x_HTProc.c
index e60d926a3973..c2588f80625b 100644
--- a/drivers/staging/rtl8192u/ieee80211/rtl819x_HTProc.c
+++ b/drivers/staging/rtl8192u/ieee80211/rtl819x_HTProc.c
@@ -471,12 +471,10 @@ static bool HTIOTActIsDisableMCS15(struct ieee80211_device *ieee)
 static bool HTIOTActIsDisableMCSTwoSpatialStream(struct ieee80211_device *ieee,
 						 u8 *PeerMacAddr)
 {
-	bool retValue = false;
-
 #ifdef TODO
 	// Apply for 819u only
 #endif
-	return retValue;
+	return false;
 }
 
 /********************************************************************************************************************
@@ -488,11 +486,8 @@ static bool HTIOTActIsDisableMCSTwoSpatialStream(struct ieee80211_device *ieee,
   * *****************************************************************************************************************/
 static u8 HTIOTActIsDisableEDCATurbo(struct ieee80211_device *ieee,
 				     u8 *PeerMacAddr)
-{
-	u8	retValue = false;	// default enable EDCA Turbo mode.
-	// Set specific EDCA parameter for different AP in DM handler.
-
-	return retValue;
+{	/* default enable EDCA Turbo mode. */
+	return false;
 }
 
 /********************************************************************************************************************
@@ -508,7 +503,7 @@ static u8 HTIOTActIsMgntUseCCK6M(struct ieee80211_network *network)
 	// 2008/01/25 MH Judeg if we need to use OFDM to sned MGNT frame for broadcom AP.
 	// 2008/01/28 MH We must prevent that we select null bssid to link.
 
-	if(network->broadcom_cap_exist)
+	if (network->broadcom_cap_exist)
 	{
 		retValue = 1;
 	}
@@ -625,7 +620,7 @@ void HTConstructCapabilityElement(struct ieee80211_device *ieee, u8 *posHTCap, u
 
 	// 2008.06.12
 	// For RTL819X, if pairwisekey = wep/tkip, ap is ralink, we support only MCS0~7.
-	if(ieee->GetHalfNmodeSupportByAPsHandler(ieee->dev))
+	if (ieee->GetHalfNmodeSupportByAPsHandler(ieee->dev))
 	{
 		int i;
 		for(i = 1; i< 16; i++)
@@ -752,7 +747,7 @@ void HTConstructRT2RTAggElement(struct ieee80211_device *ieee, u8 *posRT2RTAgg,
 	*posRT2RTAgg++ = 0x01;
 	*posRT2RTAgg = 0x10;//*posRT2RTAgg = 0x02;
 
-	if(ieee->bSupportRemoteWakeUp) {
+	if (ieee->bSupportRemoteWakeUp) {
 		*posRT2RTAgg |= 0x08;//RT_HT_CAP_USE_WOW;
 	}
 
@@ -879,17 +874,17 @@ u8 HTGetHighestMCSRate(struct ieee80211_device *ieee, u8 *pMCSRateSet, u8 *pMCSF
 
 	for(i = 0; i < 16; i++)
 	{
-		if(availableMcsRate[i] != 0)
+		if (availableMcsRate[i] != 0)
 		{
 			bitMap = availableMcsRate[i];
 			for(j = 0; j < 8; j++)
 			{
-				if((bitMap%2) != 0)
+				if ((bitMap%2) != 0)
 				{
 					if(HTMcsToDataRate(ieee, (8*i+j)) > HTMcsToDataRate(ieee, mcsRate))
 						mcsRate = (8*i+j);
 				}
-				bitMap = bitMap>>1;
+				bitMap >>= 1;
 			}
 		}
 	}
@@ -1067,7 +1062,7 @@ void HTOnAssocRsp(struct ieee80211_device *ieee)
 
 	// Lanhsin: mark for tmp to avoid deauth by ap from  s3
 	//if(memcmp(pMgntInfo->Bssid, NETGEAR834Bv2_BROADCOM, 3)==0)
-	if(0)
+	if (0)
 		{
 
 			pHTInfo->bCurrentAMPDUEnable = false;
@@ -1303,7 +1298,7 @@ void HTUpdateSelfAndPeerSetting(struct ieee80211_device *ieee,	struct ieee80211_
 //	PHT_CAPABILITY_ELE		pPeerHTCap = (PHT_CAPABILITY_ELE)pNetwork->bssht.bdHTCapBuf;
 	PHT_INFORMATION_ELE		pPeerHTInfo = (PHT_INFORMATION_ELE)pNetwork->bssht.bdHTInfoBuf;
 
-	if(pHTInfo->bCurrentHTSupport)
+	if (pHTInfo->bCurrentHTSupport)
 	{
 		//
 		// Config current operation mode.
@@ -1328,7 +1323,7 @@ EXPORT_SYMBOL(HTUpdateSelfAndPeerSetting);
 ********************************************************************************************************************/
 u8 HTCCheck(struct ieee80211_device *ieee, u8 *pFrame)
 {
-	if(ieee->pHTInfo->bCurrentHTSupport)
+	if (ieee->pHTInfo->bCurrentHTSupport)
 	{
 		if ((IsQoSDataFrame(pFrame) && Frame_Order(pFrame)) == 1) {
 			IEEE80211_DEBUG(IEEE80211_DL_HT, "HT CONTROL FILED EXIST!!\n");
@@ -1357,7 +1352,7 @@ void HTSetConnectBwMode(struct ieee80211_device *ieee, HT_CHANNEL_WIDTH	Bandwidt
 //		return;
 
 //	spin_lock_irqsave(&(ieee->bw_spinlock), flags);
-	if(pHTInfo->bSwBwInProgress) {
+	if (pHTInfo->bSwBwInProgress) {
 //		spin_unlock_irqrestore(&(ieee->bw_spinlock), flags);
 		return;
 	}
diff --git a/drivers/staging/rtl8192u/ieee80211/rtl819x_TS.h b/drivers/staging/rtl8192u/ieee80211/rtl819x_TS.h
index 7ed7243b1fb0..873969c9f226 100644
--- a/drivers/staging/rtl8192u/ieee80211/rtl819x_TS.h
+++ b/drivers/staging/rtl8192u/ieee80211/rtl819x_TS.h
@@ -1,14 +1,14 @@
 #ifndef _TSTYPE_H_
 #define _TSTYPE_H_
 #include "rtl819x_Qos.h"
-#define TS_SETUP_TIMEOUT	60  // In millisecond
+#define TS_SETUP_TIMEOUT	60  /*  In millisecond */
 #define TS_INACT_TIMEOUT	60
 #define TS_ADDBA_DELAY		60
 
 #define TOTAL_TS_NUM		16
 #define TCLAS_NUM		4
 
-// This define the Tx/Rx directions
+/*  This define the Tx/Rx directions */
 typedef enum _TR_SELECT {
 	TX_DIR = 0,
 	RX_DIR = 1,
@@ -28,9 +28,9 @@ typedef struct _TS_COMMON_INFO{
 typedef struct _TX_TS_RECORD{
 	TS_COMMON_INFO		TsCommonInfo;
 	u16				TxCurSeq;
-	BA_RECORD			TxPendingBARecord;	// For BA Originator
-	BA_RECORD			TxAdmittedBARecord;	// For BA Originator
-//	QOS_DL_RECORD		DLRecord;
+	BA_RECORD			TxPendingBARecord;	/*  For BA Originator */
+	BA_RECORD			TxAdmittedBARecord;	/*  For BA Originator */
+/* 	QOS_DL_RECORD		DLRecord; */
 	u8				bAddBaReqInProgress;
 	u8				bAddBaReqDelayed;
 	u8				bUsingBa;
@@ -44,11 +44,11 @@ typedef struct _RX_TS_RECORD {
 	u16				RxTimeoutIndicateSeq;
 	struct list_head		RxPendingPktList;
 	struct timer_list		RxPktPendingTimer;
-	BA_RECORD			RxAdmittedBARecord;	 // For BA Recipient
+	BA_RECORD			RxAdmittedBARecord;	 /*  For BA Recipient */
 	u16				RxLastSeqNum;
 	u8				RxLastFragNum;
 	u8				num;
-//	QOS_DL_RECORD		DLRecord;
+/* 	QOS_DL_RECORD		DLRecord; */
 } RX_TS_RECORD, *PRX_TS_RECORD;
 
 
diff --git a/drivers/staging/rtl8192u/ieee80211/rtl819x_TSProc.c b/drivers/staging/rtl8192u/ieee80211/rtl819x_TSProc.c
index acaa723817e7..ea92fdebe5a7 100644
--- a/drivers/staging/rtl8192u/ieee80211/rtl819x_TSProc.c
+++ b/drivers/staging/rtl8192u/ieee80211/rtl819x_TSProc.c
@@ -156,26 +156,16 @@ void TSInitialize(struct ieee80211_device *ieee)
 		pTxTS->num = count;
 		// The timers for the operation of Traffic Stream and Block Ack.
 		// DLS related timer will be add here in the future!!
-		init_timer(&pTxTS->TsCommonInfo.SetupTimer);
-		pTxTS->TsCommonInfo.SetupTimer.data = (unsigned long)pTxTS;
-		pTxTS->TsCommonInfo.SetupTimer.function = TsSetupTimeOut;
-
-		init_timer(&pTxTS->TsCommonInfo.InactTimer);
-		pTxTS->TsCommonInfo.InactTimer.data = (unsigned long)pTxTS;
-		pTxTS->TsCommonInfo.InactTimer.function = TsInactTimeout;
-
-		init_timer(&pTxTS->TsAddBaTimer);
-		pTxTS->TsAddBaTimer.data = (unsigned long)pTxTS;
-		pTxTS->TsAddBaTimer.function = TsAddBaProcess;
-
-		init_timer(&pTxTS->TxPendingBARecord.Timer);
-		pTxTS->TxPendingBARecord.Timer.data = (unsigned long)pTxTS;
-		pTxTS->TxPendingBARecord.Timer.function = BaSetupTimeOut;
-
-		init_timer(&pTxTS->TxAdmittedBARecord.Timer);
-		pTxTS->TxAdmittedBARecord.Timer.data = (unsigned long)pTxTS;
-		pTxTS->TxAdmittedBARecord.Timer.function = TxBaInactTimeout;
-
+		setup_timer(&pTxTS->TsCommonInfo.SetupTimer, TsSetupTimeOut,
+			    (unsigned long)pTxTS);
+		setup_timer(&pTxTS->TsCommonInfo.InactTimer, TsInactTimeout,
+			    (unsigned long)pTxTS);
+		setup_timer(&pTxTS->TsAddBaTimer, TsAddBaProcess,
+			    (unsigned long)pTxTS);
+		setup_timer(&pTxTS->TxPendingBARecord.Timer, BaSetupTimeOut,
+			    (unsigned long)pTxTS);
+		setup_timer(&pTxTS->TxAdmittedBARecord.Timer,
+			    TxBaInactTimeout, (unsigned long)pTxTS);
 		ResetTxTsEntry(pTxTS);
 		list_add_tail(&pTxTS->TsCommonInfo.List, &ieee->Tx_TS_Unused_List);
 		pTxTS++;
@@ -189,23 +179,14 @@ void TSInitialize(struct ieee80211_device *ieee)
 	{
 		pRxTS->num = count;
 		INIT_LIST_HEAD(&pRxTS->RxPendingPktList);
-
-		init_timer(&pRxTS->TsCommonInfo.SetupTimer);
-		pRxTS->TsCommonInfo.SetupTimer.data = (unsigned long)pRxTS;
-		pRxTS->TsCommonInfo.SetupTimer.function = TsSetupTimeOut;
-
-		init_timer(&pRxTS->TsCommonInfo.InactTimer);
-		pRxTS->TsCommonInfo.InactTimer.data = (unsigned long)pRxTS;
-		pRxTS->TsCommonInfo.InactTimer.function = TsInactTimeout;
-
-		init_timer(&pRxTS->RxAdmittedBARecord.Timer);
-		pRxTS->RxAdmittedBARecord.Timer.data = (unsigned long)pRxTS;
-		pRxTS->RxAdmittedBARecord.Timer.function = RxBaInactTimeout;
-
-		init_timer(&pRxTS->RxPktPendingTimer);
-		pRxTS->RxPktPendingTimer.data = (unsigned long)pRxTS;
-		pRxTS->RxPktPendingTimer.function = RxPktPendingTimeout;
-
+		setup_timer(&pRxTS->TsCommonInfo.SetupTimer, TsSetupTimeOut,
+			    (unsigned long)pRxTS);
+		setup_timer(&pRxTS->TsCommonInfo.InactTimer, TsInactTimeout,
+			    (unsigned long)pRxTS);
+		setup_timer(&pRxTS->RxAdmittedBARecord.Timer,
+			    RxBaInactTimeout, (unsigned long)pRxTS);
+		setup_timer(&pRxTS->RxPktPendingTimer, RxPktPendingTimeout,
+			    (unsigned long)pRxTS);
 		ResetRxTsEntry(pRxTS);
 		list_add_tail(&pRxTS->TsCommonInfo.List, &ieee->Rx_TS_Unused_List);
 		pRxTS++;
@@ -288,7 +269,7 @@ static PTS_COMMON_INFO SearchAdmitTRStream(struct ieee80211_device *ieee,
 	//for(dir = DIR_UP; dir <= DIR_BI_DIR; dir++)
 	for(dir = 0; dir <= DIR_BI_DIR; dir++)
 	{
-		if(search_dir[dir] ==false )
+		if (!search_dir[dir])
 			continue;
 		list_for_each_entry(pRet, psearch_list, List){
 	//		IEEE80211_DEBUG(IEEE80211_DL_TS, "ADD:%pM, TID:%d, dir:%d\n", pRet->Addr, pRet->TSpec.f.TSInfo.field.ucTSID, pRet->TSpec.f.TSInfo.field.ucDirection);
@@ -400,8 +381,7 @@ bool GetTs(
 	}
 	else
 	{
-		if(bAddNewTs == false)
-		{
+		if (!bAddNewTs) {
 			IEEE80211_DEBUG(IEEE80211_DL_TS, "add new TS failed(tid:%d)\n", UP);
 			return false;
 		}
diff --git a/drivers/staging/rtl8192u/r8190_rtl8256.c b/drivers/staging/rtl8192u/r8190_rtl8256.c
index 1868352d3789..e00032947e0f 100644
--- a/drivers/staging/rtl8192u/r8190_rtl8256.c
+++ b/drivers/staging/rtl8192u/r8190_rtl8256.c
@@ -225,7 +225,7 @@ void PHY_SetRF8256CCKTxPower(struct net_device *dev, u8 powerlevel)
 	struct r8192_priv *priv = ieee80211_priv(dev);
 	TxAGC = powerlevel;
 
-	if (priv->bDynamicTxLowPower == TRUE) {
+	if (priv->bDynamicTxLowPower) {
 		if (priv->CustomerID == RT_CID_819x_Netcore)
 			TxAGC = 0x22;
 		else
@@ -275,7 +275,7 @@ void PHY_SetRF8256OFDMTxPower(struct net_device *dev, u8 powerlevel)
 			priv->Pwr_Track = writeVal_tmp;
 		}
 
-		if (priv->bDynamicTxHighPower == TRUE) {
+		if (priv->bDynamicTxHighPower) {
 			/*Add by Jacken 2008/03/06
 			 *Emily, 20080613. Set low tx power for both MCS and legacy OFDM
 			 */
diff --git a/drivers/staging/rtl8192u/r8190_rtl8256.h b/drivers/staging/rtl8192u/r8190_rtl8256.h
index fa6dd37d85e6..6e5662f7951c 100644
--- a/drivers/staging/rtl8192u/r8190_rtl8256.h
+++ b/drivers/staging/rtl8192u/r8190_rtl8256.h
@@ -13,8 +13,9 @@
 #ifndef RTL8225H
 #define RTL8225H
 
-#define RTL819X_TOTAL_RF_PATH 2 //for 8192U
-extern void PHY_SetRF8256Bandwidth(struct net_device *dev , HT_CHANNEL_WIDTH Bandwidth);
+#define RTL819X_TOTAL_RF_PATH 2 /* for 8192U */
+extern void PHY_SetRF8256Bandwidth(struct net_device *dev,
+				   HT_CHANNEL_WIDTH Bandwidth);
 extern void PHY_RF8256_Config(struct net_device *dev);
 extern void phy_RF8256_Config_ParaFile(struct net_device *dev);
 extern void PHY_SetRF8256CCKTxPower(struct net_device *dev, u8	powerlevel);
diff --git a/drivers/staging/rtl8192u/r8192U.h b/drivers/staging/rtl8192u/r8192U.h
index c9d8c102cca3..6c2e438c9af4 100644
--- a/drivers/staging/rtl8192u/r8192U.h
+++ b/drivers/staging/rtl8192u/r8192U.h
@@ -40,8 +40,6 @@
 #define RTL8192U
 #define RTL819xU_MODULE_NAME "rtl819xU"
 /* HW security */
-#define FALSE 0
-#define TRUE 1
 #define MAX_KEY_LEN     61
 #define KEY_BUF_SIZE    5
 
diff --git a/drivers/staging/rtl8192u/r8192U_core.c b/drivers/staging/rtl8192u/r8192U_core.c
index e031a253e2ae..a4795afeeb9c 100644
--- a/drivers/staging/rtl8192u/r8192U_core.c
+++ b/drivers/staging/rtl8192u/r8192U_core.c
@@ -408,9 +408,8 @@ inline void force_pci_posting(struct net_device *dev)
 }
 
 static struct net_device_stats *rtl8192_stats(struct net_device *dev);
-void rtl8192_commit(struct net_device *dev);
-void rtl8192_restart(struct work_struct *work);
-void watch_dog_timer_callback(unsigned long data);
+static void rtl8192_restart(struct work_struct *work);
+static void watch_dog_timer_callback(unsigned long data);
 
 /****************************************************************************
  *   -----------------------------PROCFS STUFF-------------------------
@@ -827,7 +826,6 @@ void rtl8192_rtx_disable(struct net_device *dev)
 		netdev_warn(dev, "skb_queue not empty\n");
 
 	skb_queue_purge(&priv->skb_queue);
-	return;
 }
 
 inline u16 ieeerate2rtlrate(int rate)
@@ -966,8 +964,6 @@ static void rtl8192_hard_data_xmit(struct sk_buff *skb, struct net_device *dev,
 	ret = rtl8192_tx(dev, skb);
 
 	spin_unlock_irqrestore(&priv->tx_lock, flags);
-
-	return;
 }
 
 /* This is a rough attempt to TX a frame
@@ -1766,7 +1762,7 @@ void rtl8192_usb_deleteendpoints(struct net_device *dev)
 }
 #endif
 
-extern void rtl8192_update_ratr_table(struct net_device *dev);
+static void rtl8192_update_ratr_table(struct net_device *dev);
 static void rtl8192_link_change(struct net_device *dev)
 {
 	struct r8192_priv *priv = ieee80211_priv(dev);
@@ -1971,7 +1967,7 @@ static int rtl8192_handle_assoc_response(struct net_device *dev,
 }
 
 
-void rtl8192_update_ratr_table(struct net_device *dev)
+static void rtl8192_update_ratr_table(struct net_device *dev)
 {
 	struct r8192_priv *priv = ieee80211_priv(dev);
 	struct ieee80211_device *ieee = priv->ieee80211;
@@ -2067,7 +2063,6 @@ static void rtl8192_refresh_supportrate(struct r8192_priv *priv)
 		memcpy(ieee->Regdot11HTOperationalRateSet, ieee->RegHTSuppRateSet, 16);
 	else
 		memset(ieee->Regdot11HTOperationalRateSet, 0, 16);
-	return;
 }
 
 static u8 rtl8192_getSupportedWireleeMode(struct net_device *dev)
@@ -2078,10 +2073,10 @@ static u8 rtl8192_getSupportedWireleeMode(struct net_device *dev)
 	case RF_8225:
 	case RF_8256:
 	case RF_PSEUDO_11N:
-		ret = (WIRELESS_MODE_N_24G|WIRELESS_MODE_G|WIRELESS_MODE_B);
+		ret = WIRELESS_MODE_N_24G|WIRELESS_MODE_G|WIRELESS_MODE_B;
 		break;
 	case RF_8258:
-		ret = (WIRELESS_MODE_A|WIRELESS_MODE_N_5G);
+		ret = WIRELESS_MODE_A|WIRELESS_MODE_N_5G;
 		break;
 	default:
 		ret = WIRELESS_MODE_B;
@@ -2143,7 +2138,7 @@ static void rtl8192_init_priv_variable(struct net_device *dev)
 	//for silent reset
 	priv->IrpPendingCount = 1;
 	priv->ResetProgress = RESET_TYPE_NORESET;
-	priv->bForcedSilentReset = 0;
+	priv->bForcedSilentReset = false;
 	priv->bDisableNormalResetCheck = false;
 	priv->force_reset = false;
 
@@ -2247,9 +2242,9 @@ static void rtl8192_init_priv_lock(struct r8192_priv *priv)
 	mutex_init(&priv->mutex);
 }
 
-extern  void    rtl819x_watchdog_wqcallback(struct work_struct *work);
+static void rtl819x_watchdog_wqcallback(struct work_struct *work);
 
-void rtl8192_irq_rx_tasklet(struct r8192_priv *priv);
+static void rtl8192_irq_rx_tasklet(struct r8192_priv *priv);
 //init tasklet and wait_queue here. only 2.6 above kernel is considered
 #define DRV_NAME "wlan0"
 static void rtl8192_init_priv_task(struct net_device *dev)
@@ -2310,11 +2305,11 @@ static void rtl8192_read_eeprom_info(struct net_device *dev)
 	}
 
 	if (bLoad_From_EEPOM) {
-		tmpValue = eprom_read(dev, (EEPROM_VID>>1));
+		tmpValue = eprom_read(dev, EEPROM_VID>>1);
 		priv->eeprom_vid = endian_swap(&tmpValue);
-		priv->eeprom_pid = eprom_read(dev, (EEPROM_PID>>1));
-		tmpValue = eprom_read(dev, (EEPROM_ChannelPlan>>1));
-		priv->eeprom_ChannelPlan = ((tmpValue&0xff00)>>8);
+		priv->eeprom_pid = eprom_read(dev, EEPROM_PID>>1);
+		tmpValue = eprom_read(dev, EEPROM_ChannelPlan>>1);
+		priv->eeprom_ChannelPlan = (tmpValue & 0xff00)>>8;
 		priv->btxpowerdata_readfromEEPORM = true;
 		priv->eeprom_CustomerID = eprom_read(dev, (EEPROM_Customer_ID>>1)) >>8;
 	} else {
@@ -2397,7 +2392,8 @@ static void rtl8192_read_eeprom_info(struct net_device *dev)
 			}
 		} else if (priv->EEPROM_Def_Ver == 1) {
 			if (bLoad_From_EEPOM) {
-				tmpValue = eprom_read(dev, (EEPROM_TxPwIndex_CCK_V1>>1));
+				tmpValue = eprom_read(dev,
+						EEPROM_TxPwIndex_CCK_V1 >> 1);
 				tmpValue = (tmpValue & 0xff00) >> 8;
 			} else {
 				tmpValue = 0x10;
@@ -2410,7 +2406,8 @@ static void rtl8192_read_eeprom_info(struct net_device *dev)
 				tmpValue = 0x1010;
 			*((u16 *)(&priv->EEPROMTxPowerLevelCCK_V1[1])) = tmpValue;
 			if (bLoad_From_EEPOM)
-				tmpValue = eprom_read(dev, (EEPROM_TxPwIndex_OFDM_24G_V1>>1));
+				tmpValue = eprom_read(dev,
+					EEPROM_TxPwIndex_OFDM_24G_V1 >> 1);
 			else
 				tmpValue = 0x1010;
 			*((u16 *)(&priv->EEPROMTxPowerLevelOFDM24G[0])) = tmpValue;
@@ -2453,7 +2450,7 @@ static void rtl8192_read_eeprom_info(struct net_device *dev)
 		// Antenna B gain offset to antenna A, bit0~3
 		priv->AntennaTxPwDiff[0] = (priv->EEPROMTxPowerDiff & 0xf);
 		// Antenna C gain offset to antenna A, bit4~7
-		priv->AntennaTxPwDiff[1] = ((priv->EEPROMTxPowerDiff & 0xf0)>>4);
+		priv->AntennaTxPwDiff[1] = (priv->EEPROMTxPowerDiff & 0xf0)>>4;
 		// CrystalCap, bit12~15
 		priv->CrystalCap = priv->EEPROMCrystalCap;
 		// ThermalMeter, bit0~3 for RFIC1, bit4~7 for RFIC2
@@ -2505,7 +2502,6 @@ static void rtl8192_read_eeprom_info(struct net_device *dev)
 	//we need init DIG RATR table here again.
 
 	RT_TRACE(COMP_EPROM, "<===========%s()\n", __func__);
-	return;
 }
 
 static short rtl8192_get_channel_map(struct net_device *dev)
@@ -2547,9 +2543,8 @@ static short rtl8192_init(struct net_device *dev)
 	rtl8192_read_eeprom_info(dev);
 	rtl8192_get_channel_map(dev);
 	init_hal_dm(dev);
-	init_timer(&priv->watch_dog_timer);
-	priv->watch_dog_timer.data = (unsigned long)dev;
-	priv->watch_dog_timer.function = watch_dog_timer_callback;
+	setup_timer(&priv->watch_dog_timer, watch_dog_timer_callback,
+		    (unsigned long)dev);
 	if (rtl8192_usb_initendpoints(dev) != 0) {
 		DMESG("Endopoints initialization failed");
 		return -ENOMEM;
@@ -2686,7 +2681,7 @@ static bool rtl8192_adapter_start(struct net_device *dev)
 
 	read_nic_dword(dev, CPU_GEN, &dwRegRead);
 	if (priv->LoopbackMode == RTL819xU_NO_LOOPBACK)
-		dwRegRead = ((dwRegRead & CPU_GEN_NO_LOOPBACK_MSK) | CPU_GEN_NO_LOOPBACK_SET);
+		dwRegRead = (dwRegRead & CPU_GEN_NO_LOOPBACK_MSK) | CPU_GEN_NO_LOOPBACK_SET;
 	else if (priv->LoopbackMode == RTL819xU_MAC_LOOPBACK)
 		dwRegRead |= CPU_CCK_LOOPBACK;
 	else
@@ -2767,7 +2762,7 @@ static bool rtl8192_adapter_start(struct net_device *dev)
 	//
 #ifdef TO_DO_LIST
 	if (Adapter->ResetProgress == RESET_TYPE_NORESET) {
-		if (pMgntInfo->RegRfOff == TRUE) { /* User disable RF via registry. */
+		if (pMgntInfo->RegRfOff == true) { /* User disable RF via registry. */
 			RT_TRACE((COMP_INIT|COMP_RF), DBG_LOUD, ("InitializeAdapter819xUsb(): Turn off RF for RegRfOff ----------\n"));
 			MgntActSet_RF_State(Adapter, eRfOff, RF_CHANGE_BY_SW);
 			// Those actions will be discard in MgntActSet_RF_State because of the same state
@@ -2814,15 +2809,15 @@ static bool rtl8192_adapter_start(struct net_device *dev)
 		u8 tmpvalue;
 		read_nic_byte(dev, 0x301, &tmpvalue);
 		if (tmpvalue == 0x03) {
-			priv->bDcut = TRUE;
+			priv->bDcut = true;
 			RT_TRACE(COMP_POWER_TRACKING, "D-cut\n");
 		} else {
-			priv->bDcut = FALSE;
+			priv->bDcut = false;
 			RT_TRACE(COMP_POWER_TRACKING, "C-cut\n");
 		}
 		dm_initialize_txpower_tracking(dev);
 
-		if (priv->bDcut == TRUE) {
+		if (priv->bDcut) {
 			u32 i, TempCCk;
 			u32 tmpRegA = rtl8192_QueryBBReg(dev, rOFDM0_XATxIQImbalance, bMaskDWord);
 			for (i = 0; i < TxBBGainTableLength; i++) {
@@ -2874,11 +2869,11 @@ static bool HalTxCheckStuck819xUsb(struct net_device *dev)
 {
 	struct r8192_priv *priv = ieee80211_priv(dev);
 	u16		RegTxCounter;
-	bool		bStuck = FALSE;
+	bool		bStuck = false;
 	read_nic_word(dev, 0x128, &RegTxCounter);
 	RT_TRACE(COMP_RESET, "%s():RegTxCounter is %d,TxCounter is %d\n", __func__, RegTxCounter, priv->TxCounter);
 	if (priv->TxCounter == RegTxCounter)
-		bStuck = TRUE;
+		bStuck = true;
 
 	priv->TxCounter = RegTxCounter;
 
@@ -2920,7 +2915,7 @@ static bool HalRxCheckStuck819xUsb(struct net_device *dev)
 {
 	u16	RegRxCounter;
 	struct r8192_priv *priv = ieee80211_priv(dev);
-	bool bStuck = FALSE;
+	bool bStuck = false;
 	static u8	rx_chk_cnt;
 	read_nic_word(dev, 0x130, &RegRxCounter);
 	RT_TRACE(COMP_RESET, "%s(): RegRxCounter is %d,RxCounter is %d\n", __func__, RegRxCounter, priv->RxCounter);
@@ -2951,7 +2946,7 @@ static bool HalRxCheckStuck819xUsb(struct net_device *dev)
 	}
 
 	if (priv->RxCounter == RegRxCounter)
-		bStuck = TRUE;
+		bStuck = true;
 
 	priv->RxCounter = RegRxCounter;
 
@@ -2961,10 +2956,10 @@ static bool HalRxCheckStuck819xUsb(struct net_device *dev)
 static RESET_TYPE RxCheckStuck(struct net_device *dev)
 {
 	struct r8192_priv *priv = ieee80211_priv(dev);
-	bool        bRxCheck = FALSE;
+	bool        bRxCheck = false;
 
 	if (priv->IrpPendingCount > 1)
-		bRxCheck = TRUE;
+		bRxCheck = true;
 
 	if (bRxCheck) {
 		if (HalRxCheckStuck819xUsb(dev)) {
@@ -3021,9 +3016,9 @@ static RESET_TYPE rtl819x_ifcheck_resetornot(struct net_device *dev)
 
 }
 
-void rtl8192_cancel_deferred_work(struct r8192_priv *priv);
-int _rtl8192_up(struct net_device *dev);
-int rtl8192_close(struct net_device *dev);
+static void rtl8192_cancel_deferred_work(struct r8192_priv *priv);
+static int _rtl8192_up(struct net_device *dev);
+static int rtl8192_close(struct net_device *dev);
 
 
 
@@ -3642,7 +3637,7 @@ static u8 HwRateToMRate90(bool bIsHT, u8 rate)
 				ret_rate = MGN_MCS15;
 				break;
 			case DESC90_RATEMCS32:
-				ret_rate = (0x80|0x20);
+				ret_rate = 0x80|0x20;
 				break;
 
 			default:
@@ -4038,7 +4033,7 @@ static void rtl8192_query_rxphystatus(struct r8192_priv *priv,
 
 		if (!priv->bCckHighPower) {
 			report = pcck_buf->cck_agc_rpt & 0xc0;
-			report = report>>6;
+			report >>= 6;
 			switch (report) {
 				//Fixed by Jacken from Bryant 2008-03-20
 				//Original value is -38 , -26 , -14 , -2
@@ -4058,7 +4053,7 @@ static void rtl8192_query_rxphystatus(struct r8192_priv *priv,
 			}
 		} else {
 			report = pcck_buf->cck_agc_rpt & 0x60;
-			report = report>>5;
+			report >>= 5;
 			switch (report) {
 			case 0x3:
 				rx_pwr_all = -35 - ((pcck_buf->cck_agc_rpt & 0x1f)<<1);
@@ -4208,7 +4203,7 @@ static void TranslateRxSignalStuff819xUsb(struct sk_buff *skb,
 	struct net_device *dev = info->dev;
 	struct r8192_priv *priv = (struct r8192_priv *)ieee80211_priv(dev);
 	bool bpacket_match_bssid, bpacket_toself;
-	bool bPacketBeacon = FALSE, bToSelfBA = FALSE;
+	bool bPacketBeacon = false, bToSelfBA = false;
 	static struct ieee80211_rx_stats  previous_stats;
 	struct ieee80211_hdr_3addr *hdr;//by amy
 	u16 fc, type;
@@ -4227,9 +4222,9 @@ static void TranslateRxSignalStuff819xUsb(struct sk_buff *skb,
 	praddr = hdr->addr1;
 
 	/* Check if the received packet is acceptable. */
-	bpacket_match_bssid = ((IEEE80211_FTYPE_CTL != type) &&
+	bpacket_match_bssid = (IEEE80211_FTYPE_CTL != type) &&
 			       (eqMacAddr(priv->ieee80211->current_network.bssid,  (fc & IEEE80211_FCTL_TODS) ? hdr->addr1 : (fc & IEEE80211_FCTL_FROMDS) ? hdr->addr2 : hdr->addr3))
-			       && (!pstats->bHwError) && (!pstats->bCRC) && (!pstats->bICV));
+			       && (!pstats->bHwError) && (!pstats->bCRC) && (!pstats->bICV);
 	bpacket_toself =  bpacket_match_bssid & (eqMacAddr(praddr, priv->ieee80211->dev->dev_addr));
 
 	if (WLAN_FC_GET_FRAMETYPE(fc) == IEEE80211_STYPE_BEACON)
@@ -4474,13 +4469,10 @@ static void query_rxdesc_status(struct sk_buff *skb,
 		skb_pull(skb, stats->RxBufShift + stats->RxDrvInfoSize);
 	}
 
-	/* for debug 2008.5.29 */
-
-	//added by vivi, for MP, 20080108
-	stats->RxIs40MHzPacket = driver_info->BW;
-	if (stats->RxDrvInfoSize != 0)
+	if (driver_info) {
+		stats->RxIs40MHzPacket = driver_info->BW;
 		TranslateRxSignalStuff819xUsb(skb, stats, driver_info);
-
+	}
 }
 
 static void rtl8192_rx_nomal(struct sk_buff *skb)
diff --git a/drivers/staging/rtl8192u/r8192U_dm.c b/drivers/staging/rtl8192u/r8192U_dm.c
index ee6b936efef2..12dd19e1159b 100644
--- a/drivers/staging/rtl8192u/r8192U_dm.c
+++ b/drivers/staging/rtl8192u/r8192U_dm.c
@@ -36,11 +36,12 @@ static u32 edca_setting_UL[HT_IOT_PEER_MAX] = {
 
 /*------------------------Define global variable-----------------------------*/
 /* Debug variable ? */
-dig_t	dm_digtable;
+struct dig dm_digtable;
 /* Store current software write register content for MAC PHY. */
 u8		dm_shadow[16][256] = { {0} };
 /* For Dynamic Rx Path Selection by Signal Strength */
-DRxPathSel	DM_RxPathSelTable;
+struct dynamic_rx_path_sel DM_RxPathSelTable;
+
 /*------------------------Define global variable-----------------------------*/
 
 
@@ -502,7 +503,7 @@ static u8	CCKSwingTable_Ch14[CCK_Table_length][8] = {
 static void dm_TXPowerTrackingCallback_TSSI(struct net_device *dev)
 {
 	struct r8192_priv *priv = ieee80211_priv(dev);
-	bool						bHighpowerstate, viviflag = FALSE;
+	bool						bHighpowerstate, viviflag = false;
 	DCMD_TXCMD_T			tx_cmd;
 	u8						powerlevelOFDM24G;
 	int						i = 0, j = 0, k = 0;
@@ -558,13 +559,13 @@ static void dm_TXPowerTrackingCallback_TSSI(struct net_device *dev)
 			/* check if the report value is right */
 			for (k = 0; k < 5; k++) {
 				if (tmp_report[k] <= 20) {
-					viviflag = TRUE;
+					viviflag = true;
 					break;
 				}
 			}
-			if (viviflag == TRUE) {
+			if (viviflag == true) {
 				write_nic_byte(dev, 0x1ba, 0);
-				viviflag = FALSE;
+				viviflag = false;
 				RT_TRACE(COMP_POWER_TRACKING, "we filtered the data\n");
 				for (k = 0; k < 5; k++)
 					tmp_report[k] = 0;
@@ -587,7 +588,7 @@ static void dm_TXPowerTrackingCallback_TSSI(struct net_device *dev)
 				delta = TSSI_13dBm - Avg_TSSI_Meas_from_driver;
 
 			if (delta <= E_FOR_TX_POWER_TRACK) {
-				priv->ieee80211->bdynamic_txpower_enable = TRUE;
+				priv->ieee80211->bdynamic_txpower_enable = true;
 				write_nic_byte(dev, 0x1ba, 0);
 				RT_TRACE(COMP_POWER_TRACKING, "tx power track is done\n");
 				RT_TRACE(COMP_POWER_TRACKING, "priv->rfa_txpowertrackingindex = %d\n", priv->rfa_txpowertrackingindex);
@@ -624,10 +625,10 @@ static void dm_TXPowerTrackingCallback_TSSI(struct net_device *dev)
 
 			if (priv->cck_present_attentuation > -1 && priv->cck_present_attentuation < 23) {
 				if (priv->ieee80211->current_network.channel == 14 && !priv->bcck_in_ch14) {
-					priv->bcck_in_ch14 = TRUE;
+					priv->bcck_in_ch14 = true;
 					dm_cck_txpower_adjust(dev, priv->bcck_in_ch14);
 				} else if (priv->ieee80211->current_network.channel != 14 && priv->bcck_in_ch14) {
-					priv->bcck_in_ch14 = FALSE;
+					priv->bcck_in_ch14 = false;
 					dm_cck_txpower_adjust(dev, priv->bcck_in_ch14);
 				} else
 					dm_cck_txpower_adjust(dev, priv->bcck_in_ch14);
@@ -638,7 +639,7 @@ static void dm_TXPowerTrackingCallback_TSSI(struct net_device *dev)
 			RT_TRACE(COMP_POWER_TRACKING, "priv->cck_present_attentuation = %d\n", priv->cck_present_attentuation);
 
 			if (priv->cck_present_attentuation_difference <= -12 || priv->cck_present_attentuation_difference >= 24) {
-				priv->ieee80211->bdynamic_txpower_enable = TRUE;
+				priv->ieee80211->bdynamic_txpower_enable = true;
 				write_nic_byte(dev, 0x1ba, 0);
 				RT_TRACE(COMP_POWER_TRACKING, "tx power track--->limited\n");
 				return;
@@ -651,7 +652,7 @@ static void dm_TXPowerTrackingCallback_TSSI(struct net_device *dev)
 			break;
 		}
 	}
-	priv->ieee80211->bdynamic_txpower_enable = TRUE;
+	priv->ieee80211->bdynamic_txpower_enable = true;
 	write_nic_byte(dev, 0x1ba, 0);
 }
 
@@ -684,7 +685,7 @@ static void dm_TXPowerTrackingCallback_ThermalMeter(struct net_device *dev)
 				break;
 			}
 		}
-		priv->btxpower_trackingInit = TRUE;
+		priv->btxpower_trackingInit = true;
 		/*pHalData->TXPowercount = 0;*/
 		return;
 	}
@@ -717,7 +718,7 @@ static void dm_TXPowerTrackingCallback_ThermalMeter(struct net_device *dev)
 		if (tmpCCK40Mindex >= CCK_Table_length)
 			tmpCCK40Mindex = CCK_Table_length-1;
 	} else {
-		tmpval = ((u8)tmpRegA - priv->ThermalMeter[0]);
+		tmpval = (u8)tmpRegA - priv->ThermalMeter[0];
 
 		if (tmpval >= 6) /* higher temperature */
 			tmpOFDMindex = tmpCCK20Mindex = 0; /* max to +6dB */
@@ -734,10 +735,10 @@ static void dm_TXPowerTrackingCallback_ThermalMeter(struct net_device *dev)
 		tmpCCKindex = tmpCCK20Mindex;
 
 	if (priv->ieee80211->current_network.channel == 14 && !priv->bcck_in_ch14) {
-		priv->bcck_in_ch14 = TRUE;
+		priv->bcck_in_ch14 = true;
 		CCKSwingNeedUpdate = 1;
 	} else if (priv->ieee80211->current_network.channel != 14 && priv->bcck_in_ch14) {
-		priv->bcck_in_ch14 = FALSE;
+		priv->bcck_in_ch14 = false;
 		CCKSwingNeedUpdate = 1;
 	}
 
@@ -765,7 +766,7 @@ void dm_txpower_trackingcallback(struct work_struct *work)
 	struct r8192_priv *priv = container_of(dwork, struct r8192_priv, txpower_tracking_wq);
 	struct net_device *dev = priv->ieee80211->dev;
 
-	if (priv->bDcut == TRUE)
+	if (priv->bDcut == true)
 		dm_TXPowerTrackingCallback_TSSI(dev);
 	else
 		dm_TXPowerTrackingCallback_ThermalMeter(dev);
@@ -1273,9 +1274,9 @@ static void dm_InitializeTXPowerTracking_TSSI(struct net_device *dev)
 	priv->cck_txbbgain_ch14_table[22].ccktxbb_valuearray[6] = 0x00;
 	priv->cck_txbbgain_ch14_table[22].ccktxbb_valuearray[7] = 0x00;
 
-	priv->btxpower_tracking = TRUE;
+	priv->btxpower_tracking = true;
 	priv->txpower_count       = 0;
-	priv->btxpower_trackingInit = FALSE;
+	priv->btxpower_trackingInit = false;
 
 }
 
@@ -1289,18 +1290,18 @@ static void dm_InitializeTXPowerTracking_ThermalMeter(struct net_device *dev)
 	 * 3-wire by driver causes RF to go into a wrong state.
 	 */
 	if (priv->ieee80211->FwRWRF)
-		priv->btxpower_tracking = TRUE;
+		priv->btxpower_tracking = true;
 	else
-		priv->btxpower_tracking = FALSE;
+		priv->btxpower_tracking = false;
 	priv->txpower_count       = 0;
-	priv->btxpower_trackingInit = FALSE;
+	priv->btxpower_trackingInit = false;
 }
 
 void dm_initialize_txpower_tracking(struct net_device *dev)
 {
 	struct r8192_priv *priv = ieee80211_priv(dev);
 
-	if (priv->bDcut == TRUE)
+	if (priv->bDcut == true)
 		dm_InitializeTXPowerTracking_TSSI(dev);
 	else
 		dm_InitializeTXPowerTracking_ThermalMeter(dev);
@@ -1356,7 +1357,7 @@ static void dm_check_txpower_tracking(struct net_device *dev)
 #ifdef RTL8190P
 	dm_CheckTXPowerTracking_TSSI(dev);
 #else
-	if (priv->bDcut == TRUE)
+	if (priv->bDcut == true)
 		dm_CheckTXPowerTracking_TSSI(dev);
 	else
 		dm_CheckTXPowerTracking_ThermalMeter(dev);
@@ -1466,7 +1467,7 @@ void dm_cck_txpower_adjust(struct net_device *dev, bool binch14)
 {	/*  dm_CCKTxPowerAdjust */
 	struct r8192_priv *priv = ieee80211_priv(dev);
 
-	if (priv->bDcut == TRUE)
+	if (priv->bDcut == true)
 		dm_CCKTxPowerAdjust_TSSI(dev, binch14);
 	else
 		dm_CCKTxPowerAdjust_ThermalMeter(dev, binch14);
@@ -1628,8 +1629,8 @@ void dm_change_dynamic_initgain_thresh(struct net_device *dev, u32 dm_type,
 		dm_digtable.rssi_low_thresh = dm_value;
 	} else if (dm_type == DIG_TYPE_THRESH_HIGHPWR_HIGH) {
 		dm_digtable.rssi_high_power_highthresh = dm_value;
-	} else if (dm_type == DIG_TYPE_THRESH_HIGHPWR_HIGH) {
-		dm_digtable.rssi_high_power_highthresh = dm_value;
+	} else if (dm_type == DIG_TYPE_THRESH_HIGHPWR_LOW) {
+		dm_digtable.rssi_high_power_lowthresh = dm_value;
 	} else if (dm_type == DIG_TYPE_ENABLE) {
 		dm_digtable.dig_state		= DM_STA_DIG_MAX;
 		dm_digtable.dig_enable_flag	= true;
@@ -2270,10 +2271,10 @@ static void dm_check_edca_turbo(
 				/*  For Each time updating EDCA parameter, reset EDCA turbo mode status. */
 				dm_init_edca_turbo(dev);
 				u1bAIFS = qos_parameters->aifs[0] * ((mode&(IEEE_G|IEEE_N_24G)) ? 9 : 20) + aSifsTime;
-				u4bAcParam = ((((u32)(qos_parameters->tx_op_limit[0])) << AC_PARAM_TXOP_LIMIT_OFFSET)|
+				u4bAcParam = (((u32)(qos_parameters->tx_op_limit[0])) << AC_PARAM_TXOP_LIMIT_OFFSET)|
 					(((u32)(qos_parameters->cw_max[0])) << AC_PARAM_ECW_MAX_OFFSET)|
 					(((u32)(qos_parameters->cw_min[0])) << AC_PARAM_ECW_MIN_OFFSET)|
-					((u32)u1bAIFS << AC_PARAM_AIFS_OFFSET));
+					((u32)u1bAIFS << AC_PARAM_AIFS_OFFSET);
 				/*write_nic_dword(dev, WDCAPARA_ADD[i], u4bAcParam);*/
 				write_nic_dword(dev, EDCAPARA_BE,  u4bAcParam);
 
@@ -2314,7 +2315,7 @@ static void dm_init_ctstoself(struct net_device *dev)
 {
 	struct r8192_priv *priv = ieee80211_priv((struct net_device *)dev);
 
-	priv->ieee80211->bCTSToSelfEnable = TRUE;
+	priv->ieee80211->bCTSToSelfEnable = true;
 	priv->ieee80211->CTSToSelfTH = CTSToSelfTHVal;
 }
 
@@ -2327,7 +2328,7 @@ static void dm_ctstoself(struct net_device *dev)
 	unsigned long						curTxOkCnt = 0;
 	unsigned long						curRxOkCnt = 0;
 
-	if (priv->ieee80211->bCTSToSelfEnable != TRUE) {
+	if (priv->ieee80211->bCTSToSelfEnable != true) {
 		pHTInfo->IOTAction &= ~HT_IOT_ACT_FORCED_CTS2SELF;
 		return;
 	}
@@ -2419,9 +2420,9 @@ void dm_rf_pathcheck_workitemcallback(struct work_struct *work)
 	/* Check Bit 0-3, it means if RF A-D is enabled. */
 	for (i = 0; i < RF90_PATH_MAX; i++) {
 		if (rfpath & (0x01<<i))
-			priv->brfpath_rxenable[i] = 1;
+			priv->brfpath_rxenable[i] = true;
 		else
-			priv->brfpath_rxenable[i] = 0;
+			priv->brfpath_rxenable[i] = false;
 	}
 	if (!DM_RxPathSelTable.Enable)
 		return;
@@ -2681,10 +2682,8 @@ static void dm_init_fsync(struct net_device *dev)
 	priv->ieee80211->fsync_seconddiff_ratethreshold = 200;
 	priv->ieee80211->fsync_state = Default_Fsync;
 	priv->framesyncMonitor = 1;	/* current default 0xc38 monitor on */
-
-	init_timer(&priv->fsync_timer);
-	priv->fsync_timer.data = (unsigned long)dev;
-	priv->fsync_timer.function = dm_fsync_timer_callback;
+	setup_timer(&priv->fsync_timer, dm_fsync_timer_callback,
+		    (unsigned long)dev);
 }
 
 static void dm_deInit_fsync(struct net_device *dev)
diff --git a/drivers/staging/rtl8192u/r8192U_dm.h b/drivers/staging/rtl8192u/r8192U_dm.h
index 3008f91ad4cf..6cd32eb44085 100644
--- a/drivers/staging/rtl8192u/r8192U_dm.h
+++ b/drivers/staging/rtl8192u/r8192U_dm.h
@@ -67,7 +67,7 @@
 
 /*------------------------------Define structure----------------------------*/
 /* 2007/10/04 MH Define upper and lower threshold of DIG enable or disable. */
-typedef struct _dynamic_initial_gain_threshold_ {
+struct dig {
 	u8		dig_enable_flag;
 	u8		dig_algorithm;
 	u8		dbg_mode;
@@ -98,7 +98,7 @@ typedef struct _dynamic_initial_gain_threshold_ {
 	bool		initialgain_lowerbound_state;
 
 	long		rssi_val;
-} dig_t;
+};
 
 typedef enum tag_dynamic_init_gain_state_definition {
 	DM_STA_DIG_OFF = 0,
@@ -163,7 +163,7 @@ typedef enum tag_dig_cck_cs_ratio_state_definition {
 	DIG_CS_RATIO_HIGHER = 1,
 	DIG_CS_MAX
 } dm_dig_cs_ratio_e;
-typedef struct _Dynamic_Rx_Path_Selection_ {
+struct dynamic_rx_path_sel {
 	u8		Enable;
 	u8		DbgMode;
 	u8		cck_method;
@@ -177,7 +177,7 @@ typedef struct _Dynamic_Rx_Path_Selection_ {
 	u8		rf_rssi[4];
 	u8		rf_enable_rssi_th[4];
 	long		cck_pwdb_sta[4];
-} DRxPathSel;
+};
 
 typedef enum tag_CCK_Rx_Path_Method_Definition {
 	CCK_Rx_Version_1 = 0,
@@ -200,9 +200,9 @@ typedef struct tag_Tx_Config_Cmd_Format {
 
 
 /*------------------------Export global variable----------------------------*/
-extern dig_t dm_digtable;
+extern struct dig dm_digtable;
 extern u8 dm_shadow[16][256];
-extern DRxPathSel DM_RxPathSelTable;
+extern struct dynamic_rx_path_sel DM_RxPathSelTable;
 /*------------------------Export global variable----------------------------*/
 
 
diff --git a/drivers/staging/rtl8192u/r8192U_wx.c b/drivers/staging/rtl8192u/r8192U_wx.c
index 361d2d0c3df1..83597051a448 100644
--- a/drivers/staging/rtl8192u/r8192U_wx.c
+++ b/drivers/staging/rtl8192u/r8192U_wx.c
@@ -139,7 +139,7 @@ static int r8192_wx_force_reset(struct net_device *dev,
 
 	down(&priv->wx_sem);
 
-	printk("%s(): force reset ! extra is %d\n", __func__, *extra);
+	netdev_dbg(dev, "%s(): force reset ! extra is %d\n", __func__, *extra);
 	priv->force_reset = *extra;
 	up(&priv->wx_sem);
 	return 0;
@@ -335,7 +335,7 @@ static int r8192_wx_set_scan(struct net_device *dev, struct iw_request_info *a,
 	if (!priv->up)
 		return -ENETDOWN;
 
-	if (priv->ieee80211->LinkDetectInfo.bBusyTraffic == true)
+	if (priv->ieee80211->LinkDetectInfo.bBusyTraffic)
 		return -EAGAIN;
 	if (wrqu->data.flags & IW_SCAN_THIS_ESSID) {
 		struct iw_scan_req *req = (struct iw_scan_req *)b;
diff --git a/drivers/staging/rtl8192u/r8192U_wx.h b/drivers/staging/rtl8192u/r8192U_wx.h
index ae7a617740a3..d6a2d9756531 100644
--- a/drivers/staging/rtl8192u/r8192U_wx.h
+++ b/drivers/staging/rtl8192u/r8192U_wx.h
@@ -1,20 +1,21 @@
 /*
-	This is part of rtl8180 OpenSource driver - v 0.3
-	Copyright (C) Andrea Merello 2004  <andrea.merello@gmail.com>
-	Released under the terms of GPL (General Public Licence)
+ * This is part of rtl8180 OpenSource driver - v 0.3
+ * Copyright (C) Andrea Merello 2004  <andrea.merello@gmail.com>
+ * Released under the terms of GPL (General Public Licence)
+ *
+ * Parts of this driver are based on the GPL part of the official realtek driver
+ * Parts of this driver are based on the rtl8180 driver skeleton from Patric
+ * Schenke & Andres Salomon
+ * Parts of this driver are based on the Intel Pro Wireless 2100 GPL driver
+ *
+ * We want to thank the Authors of such projects and the Ndiswrapper project
+ * Authors.
+ */
 
-	Parts of this driver are based on the GPL part of the official realtek driver
-	Parts of this driver are based on the rtl8180 driver skeleton from Patric Schenke & Andres Salomon
-	Parts of this driver are based on the Intel Pro Wireless 2100 GPL driver
-
-	We want to thank the Authors of such projects and the Ndiswrapper project Authors.
-*/
-
-/* this file (will) contains wireless extension handlers*/
+/* this file (will) contains wireless extension handlers */
 
 #ifndef R8180_WX_H
 #define R8180_WX_H
-//#include <linux/wireless.h>
 
 extern struct iw_handler_def r8192_wx_handlers_def;
 /* Enable  the rtl819x_core.c to share this function, david 2008.9.22 */
diff --git a/drivers/staging/rtl8192u/r819xU_firmware.c b/drivers/staging/rtl8192u/r819xU_firmware.c
index c230be290ab6..d27b1e24ca4a 100644
--- a/drivers/staging/rtl8192u/r819xU_firmware.c
+++ b/drivers/staging/rtl8192u/r819xU_firmware.c
@@ -37,7 +37,6 @@ static bool fw_download_code(struct net_device *dev, u8 *code_virtual_address,
 	bool		    rt_status = true;
 	u16		    frag_threshold;
 	u16		    frag_length, frag_offset = 0;
-	//u16		    total_size;
 	int		    i;
 
 	rt_firmware	    *pfirmware = priv->pFirmware;
@@ -48,7 +47,7 @@ static bool fw_download_code(struct net_device *dev, u8 *code_virtual_address,
 	u8		    index;
 
 	firmware_init_param(dev);
-	//Fragmentation might be required
+	/* Fragmentation might be required */
 	frag_threshold = pfirmware->cmdpacket_frag_thresold;
 	do {
 		if ((buffer_len - frag_offset) > frag_threshold) {
@@ -107,19 +106,20 @@ static bool fw_download_code(struct net_device *dev, u8 *code_virtual_address,
 
 }
 
-//-----------------------------------------------------------------------------
-// Procedure:    Check whether main code is download OK. If OK, turn on CPU
-//
-// Description:   CPU register locates in different page against general register.
-//			    Switch to CPU register in the begin and switch back before return
-//
-//
-// Arguments:   The pointer of the adapter
-//
-// Returns:
-//        NDIS_STATUS_FAILURE - the following initialization process should be terminated
-//        NDIS_STATUS_SUCCESS - if firmware initialization process success
-//-----------------------------------------------------------------------------
+/*
+ * Procedure:	Check whether main code is download OK. If OK, turn on CPU
+ *
+ * Description:	CPU register locates in different page against general register.
+ *	    Switch to CPU register in the begin and switch back before return
+ *
+ *
+ * Arguments:   The pointer of the adapter
+ *
+ * Returns:
+ *        NDIS_STATUS_FAILURE - the following initialization process should
+ *				be terminated
+ *        NDIS_STATUS_SUCCESS - if firmware initialization process success
+ */
 static bool CPUcheck_maincodeok_turnonCPU(struct net_device *dev)
 {
 	bool		rt_status = true;
@@ -164,7 +164,7 @@ static bool CPUcheck_maincodeok_turnonCPU(struct net_device *dev)
 
 CPUCheckMainCodeOKAndTurnOnCPU_Fail:
 	RT_TRACE(COMP_ERR, "ERR in %s()\n", __func__);
-	rt_status = FALSE;
+	rt_status = false;
 	return rt_status;
 }
 
@@ -201,7 +201,7 @@ CPUCheckFirmwareReady_Fail:
 bool init_firmware(struct net_device *dev)
 {
 	struct r8192_priv	*priv = ieee80211_priv(dev);
-	bool			rt_status = TRUE;
+	bool			rt_status = true;
 
 	u32			file_length = 0;
 	u8			*mapped_file = NULL;
@@ -222,7 +222,7 @@ bool init_firmware(struct net_device *dev)
 		/* it is called by reset */
 		rst_opt = OPT_SYSTEM_RESET;
 		starting_state = FW_INIT_STEP0_BOOT;
-		// TODO: system reset
+		/* TODO: system reset */
 
 	} else if (pfirmware->firmware_status == FW_STATUS_5_READY) {
 		/* it is called by Initialize */
@@ -281,7 +281,7 @@ bool init_firmware(struct net_device *dev)
 		if (rst_opt == OPT_SYSTEM_RESET)
 			release_firmware(fw_entry);
 
-		if (rt_status != TRUE)
+		if (!rt_status)
 			goto download_firmware_fail;
 
 		switch (init_step) {
@@ -291,7 +291,7 @@ bool init_firmware(struct net_device *dev)
 			 * will set polling bit when firmware code is also configured
 			 */
 			pfirmware->firmware_status = FW_STATUS_1_MOVE_BOOT_CODE;
-			//mdelay(1000);
+			/* mdelay(1000); */
 			/*
 			 * To initialize IMEM, CPU move code  from 0x80000080,
 			 * hence, we send 0x80 byte packet
@@ -304,7 +304,7 @@ bool init_firmware(struct net_device *dev)
 
 			/* Check Put Code OK and Turn On CPU */
 			rt_status = CPUcheck_maincodeok_turnonCPU(dev);
-			if (rt_status != TRUE) {
+			if (!rt_status) {
 				RT_TRACE(COMP_ERR, "CPUcheck_maincodeok_turnonCPU fail!\n");
 				goto download_firmware_fail;
 			}
@@ -318,7 +318,7 @@ bool init_firmware(struct net_device *dev)
 			mdelay(1);
 
 			rt_status = CPUcheck_firmware_ready(dev);
-			if (rt_status != TRUE) {
+			if (!rt_status) {
 				RT_TRACE(COMP_ERR, "CPUcheck_firmware_ready fail(%d)!\n",rt_status);
 				goto download_firmware_fail;
 			}
@@ -330,13 +330,11 @@ bool init_firmware(struct net_device *dev)
 	}
 
 	RT_TRACE(COMP_FIRMWARE, "Firmware Download Success\n");
-	//assert(pfirmware->firmware_status == FW_STATUS_5_READY, ("Firmware Download Fail\n"));
-
 	return rt_status;
 
 download_firmware_fail:
 	RT_TRACE(COMP_ERR, "ERR in %s()\n", __func__);
-	rt_status = FALSE;
+	rt_status = false;
 	return rt_status;
 
 }
diff --git a/drivers/staging/rtl8192u/r819xU_phy.c b/drivers/staging/rtl8192u/r819xU_phy.c
index 058960251bac..e5dbaca9e518 100644
--- a/drivers/staging/rtl8192u/r819xU_phy.c
+++ b/drivers/staging/rtl8192u/r819xU_phy.c
@@ -106,10 +106,10 @@ void rtl8192_setBBreg(struct net_device *dev, u32 reg_addr, u32 bitmask,
 /******************************************************************************
  * function:  This function reads specific bits from BB register
  * input:     net_device	*dev
- *            u32	 	reg_addr   //target addr to be readback
- *            u32	 	bitmask    //taget bit pos to be readback
+ *            u32		reg_addr   //target addr to be readback
+ *            u32		bitmask    //taget bit pos to be readback
  * output:    none
- * return:    u32	 	data       //the readback register value
+ * return:    u32		data       //the readback register value
  * notice:
  ******************************************************************************/
 u32 rtl8192_QueryBBReg(struct net_device *dev, u32 reg_addr, u32 bitmask)
@@ -352,16 +352,14 @@ u32 rtl8192_phy_QueryRFReg(struct net_device *dev, RF90_RADIO_PATH_E eRFPath,
 		return 0;
 	if (priv->Rf_Mode == RF_OP_By_FW) {
 		reg = phy_FwRFSerialRead(dev, eRFPath, reg_addr);
-		bitshift =  rtl8192_CalculateBitShift(bitmask);
-		reg = (reg & bitmask) >> bitshift;
 		udelay(200);
-		return reg;
 	} else {
 		reg = rtl8192_phy_RFSerialRead(dev, eRFPath, reg_addr);
-		bitshift =  rtl8192_CalculateBitShift(bitmask);
-		reg = (reg & bitmask) >> bitshift;
-		return reg;
 	}
+	bitshift =  rtl8192_CalculateBitShift(bitmask);
+	reg = (reg & bitmask) >> bitshift;
+	return reg;
+
 }
 
 /******************************************************************************
@@ -478,7 +476,7 @@ static void phy_FwRFSerialWrite(struct net_device *dev,
 /******************************************************************************
  * function:  This function reads BB parameters from header file we generate,
  *            and do register read/write
- * input:     net_device 	*dev
+ * input:     net_device	*dev
  * output:    none
  * return:    none
  * notice:    BB parameters may change all the time, so please make
@@ -825,8 +823,8 @@ static void rtl8192_BB_Config_ParaFile(struct net_device *dev)
 	write_nic_byte_E(dev, 0x5e, 0x00);
 	if (priv->card_8192_version == (u8)VERSION_819xU_A) {
 		/* Antenna gain offset from B/C/D to A */
-		reg_u32 = (priv->AntennaTxPwDiff[1]<<4 |
-			   priv->AntennaTxPwDiff[0]);
+		reg_u32 = priv->AntennaTxPwDiff[1]<<4 |
+			   priv->AntennaTxPwDiff[0];
 		rtl8192_setBBreg(dev, rFPGA0_TxGainStage, (bXBTxAGC|bXCTxAGC),
 				 reg_u32);
 
@@ -900,7 +898,7 @@ void rtl8192_phy_getTxPower(struct net_device *dev)
 	read_nic_byte(dev, rOFDM0_RxDetector3, &priv->framesync);
 	read_nic_byte(dev, rOFDM0_RxDetector2, &tmp);
 	priv->framesyncC34 = tmp;
-	RT_TRACE(COMP_INIT, "Default framesync (0x%x) = 0x%x \n",
+	RT_TRACE(COMP_INIT, "Default framesync (0x%x) = 0x%x\n",
 		rOFDM0_RxDetector3, priv->framesync);
 
 	/* Read SIFS (save the value read fome MACPHY_REG.txt) */
@@ -1101,7 +1099,7 @@ bool rtl8192_SetRFPowerState(struct net_device *dev,
 	if (eRFPowerState == priv->ieee80211->eRFPowerState)
 		return false;
 
-	if (priv->SetRFPowerStateInProgress == true)
+	if (priv->SetRFPowerStateInProgress)
 		return false;
 
 	priv->SetRFPowerStateInProgress = true;
@@ -1187,7 +1185,7 @@ bool rtl8192_SetRFPowerState(struct net_device *dev,
 				/* Turn on RF we are still linked, which might
 				   happen when we quickly turn off and on HW RF.
 				 */
-				if (pMgntInfo->bMediaConnect == TRUE)
+				if (pMgntInfo->bMediaConnect)
 					Adapter->HalFunc.LedControlHandler(Adapter, LED_CTL_LINK);
 				else
 					/* Turn off LED if RF is not ON. */
@@ -1344,7 +1342,6 @@ static u8 rtl8192_phy_SwChnlStepByStep(struct net_device *dev, u8 channel,
 	default:
 		RT_TRACE(COMP_ERR, "Unknown RFChipID: %d\n", priv->rf_chip);
 		return true;
-		break;
 	}
 
 
@@ -1365,11 +1362,10 @@ static u8 rtl8192_phy_SwChnlStepByStep(struct net_device *dev, u8 channel,
 			if ((*stage) == 2) {
 				(*delay) = CurrentCmd->msDelay;
 				return true;
-			} else {
-				(*stage)++;
-				(*step) = 0;
-				continue;
 			}
+			(*stage)++;
+			(*step) = 0;
+			continue;
 		}
 
 		switch (CurrentCmd->CmdID) {
@@ -1579,10 +1575,10 @@ void rtl8192_SetBWModeWorkItem(struct net_device *dev)
 			 priv->cck_present_attentuation);
 
 		if (priv->chan == 14 && !priv->bcck_in_ch14) {
-			priv->bcck_in_ch14 = TRUE;
+			priv->bcck_in_ch14 = true;
 			dm_cck_txpower_adjust(dev, priv->bcck_in_ch14);
 		} else if (priv->chan != 14 && priv->bcck_in_ch14) {
-			priv->bcck_in_ch14 = FALSE;
+			priv->bcck_in_ch14 = false;
 			dm_cck_txpower_adjust(dev, priv->bcck_in_ch14);
 		} else {
 			dm_cck_txpower_adjust(dev, priv->bcck_in_ch14);
@@ -1746,13 +1742,13 @@ void InitialGainOperateWorkItemCallBack(struct work_struct *work)
 		RT_TRACE(COMP_SCAN, "Scan InitialGainBackup 0xa0a is %x\n",
 			 priv->initgain_backup.cca);
 
-		RT_TRACE(COMP_SCAN, "Write scan initial gain = 0x%x \n",
+		RT_TRACE(COMP_SCAN, "Write scan initial gain = 0x%x\n",
 			 initial_gain);
 		write_nic_byte(dev, rOFDM0_XAAGCCore1, initial_gain);
 		write_nic_byte(dev, rOFDM0_XBAGCCore1, initial_gain);
 		write_nic_byte(dev, rOFDM0_XCAGCCore1, initial_gain);
 		write_nic_byte(dev, rOFDM0_XDAGCCore1, initial_gain);
-		RT_TRACE(COMP_SCAN, "Write scan 0xa0a = 0x%x \n",
+		RT_TRACE(COMP_SCAN, "Write scan 0xa0a = 0x%x\n",
 			 POWER_DETECTION_TH);
 		write_nic_byte(dev, 0xa0a, POWER_DETECTION_TH);
 		break;
@@ -1793,7 +1789,7 @@ void InitialGainOperateWorkItemCallBack(struct work_struct *work)
 			rtl8192_setBBreg(dev, UFWP, bMaskByte1, 0x1);
 		break;
 	default:
-		RT_TRACE(COMP_SCAN, "Unknown IG Operation. \n");
+		RT_TRACE(COMP_SCAN, "Unknown IG Operation.\n");
 		break;
 	}
 }
diff --git a/drivers/staging/rtl8712/hal_init.c b/drivers/staging/rtl8712/hal_init.c
index 324da34383ea..0a1c6313e781 100644
--- a/drivers/staging/rtl8712/hal_init.c
+++ b/drivers/staging/rtl8712/hal_init.c
@@ -92,7 +92,7 @@ static u32 rtl871x_open_fw(struct _adapter *padapter, const u8 **ppmappedfw)
 
 static void fill_fwpriv(struct _adapter *padapter, struct fw_priv *pfwpriv)
 {
-	struct dvobj_priv *pdvobj = (struct dvobj_priv *)&padapter->dvobjpriv;
+	struct dvobj_priv *pdvobj = &padapter->dvobjpriv;
 	struct registry_priv *pregpriv = &padapter->registrypriv;
 
 	memset(pfwpriv, 0, sizeof(struct fw_priv));
diff --git a/drivers/staging/rtl8712/mlme_linux.c b/drivers/staging/rtl8712/mlme_linux.c
index 354bd03e700f..8c5a475f05e7 100644
--- a/drivers/staging/rtl8712/mlme_linux.c
+++ b/drivers/staging/rtl8712/mlme_linux.c
@@ -32,60 +32,61 @@
 #include "drv_types.h"
 #include "mlme_osdep.h"
 
-static void sitesurvey_ctrl_handler(void *FunctionContext)
+static void sitesurvey_ctrl_handler(unsigned long data)
 {
-	struct _adapter *adapter = (struct _adapter *)FunctionContext;
+	struct _adapter *adapter = (struct _adapter *)data;
 
 	_r8712_sitesurvey_ctrl_handler(adapter);
-	_set_timer(&adapter->mlmepriv.sitesurveyctrl.sitesurvey_ctrl_timer,
-		   3000);
+	mod_timer(&adapter->mlmepriv.sitesurveyctrl.sitesurvey_ctrl_timer,
+		  jiffies + msecs_to_jiffies(3000));
 }
 
-static void join_timeout_handler (void *FunctionContext)
+static void join_timeout_handler (unsigned long data)
 {
-	struct _adapter *adapter = (struct _adapter *)FunctionContext;
+	struct _adapter *adapter = (struct _adapter *)data;
 
 	_r8712_join_timeout_handler(adapter);
 }
 
-static void _scan_timeout_handler (void *FunctionContext)
+static void _scan_timeout_handler (unsigned long data)
 {
-	struct _adapter *adapter = (struct _adapter *)FunctionContext;
+	struct _adapter *adapter = (struct _adapter *)data;
 
 	r8712_scan_timeout_handler(adapter);
 }
 
-static void dhcp_timeout_handler (void *FunctionContext)
+static void dhcp_timeout_handler (unsigned long data)
 {
-	struct _adapter *adapter = (struct _adapter *)FunctionContext;
+	struct _adapter *adapter = (struct _adapter *)data;
 
 	_r8712_dhcp_timeout_handler(adapter);
 }
 
-static void wdg_timeout_handler (void *FunctionContext)
+static void wdg_timeout_handler (unsigned long data)
 {
-	struct _adapter *adapter = (struct _adapter *)FunctionContext;
+	struct _adapter *adapter = (struct _adapter *)data;
 
 	_r8712_wdg_timeout_handler(adapter);
 
-	_set_timer(&adapter->mlmepriv.wdg_timer, 2000);
+	mod_timer(&adapter->mlmepriv.wdg_timer,
+		  jiffies + msecs_to_jiffies(2000));
 }
 
 void r8712_init_mlme_timer(struct _adapter *padapter)
 {
 	struct	mlme_priv *pmlmepriv = &padapter->mlmepriv;
 
-	_init_timer(&(pmlmepriv->assoc_timer), padapter->pnetdev,
-		    join_timeout_handler, (pmlmepriv->nic_hdl));
-	_init_timer(&(pmlmepriv->sitesurveyctrl.sitesurvey_ctrl_timer),
-		    padapter->pnetdev, sitesurvey_ctrl_handler,
-		    (u8 *)(pmlmepriv->nic_hdl));
-	_init_timer(&(pmlmepriv->scan_to_timer), padapter->pnetdev,
-		    _scan_timeout_handler, (pmlmepriv->nic_hdl));
-	_init_timer(&(pmlmepriv->dhcp_timer), padapter->pnetdev,
-		    dhcp_timeout_handler, (u8 *)(pmlmepriv->nic_hdl));
-	_init_timer(&(pmlmepriv->wdg_timer), padapter->pnetdev,
-		    wdg_timeout_handler, (u8 *)(pmlmepriv->nic_hdl));
+	setup_timer(&pmlmepriv->assoc_timer, join_timeout_handler,
+		    (unsigned long)padapter);
+	setup_timer(&pmlmepriv->sitesurveyctrl.sitesurvey_ctrl_timer,
+		    sitesurvey_ctrl_handler,
+		    (unsigned long)padapter);
+	setup_timer(&pmlmepriv->scan_to_timer, _scan_timeout_handler,
+		    (unsigned long)padapter);
+	setup_timer(&pmlmepriv->dhcp_timer, dhcp_timeout_handler,
+		    (unsigned long)padapter);
+	setup_timer(&pmlmepriv->wdg_timer, wdg_timeout_handler,
+		    (unsigned long)padapter);
 }
 
 void r8712_os_indicate_connect(struct _adapter *adapter)
@@ -117,9 +118,9 @@ void r8712_os_indicate_disconnect(struct _adapter *adapter)
 					   btkip_countermeasure;
 		memset((unsigned char *)&adapter->securitypriv, 0,
 			 sizeof(struct security_priv));
-		_init_timer(&(adapter->securitypriv.tkip_timer),
-			    adapter->pnetdev, r8712_use_tkipkey_handler,
-			    adapter);
+		setup_timer(&adapter->securitypriv.tkip_timer,
+			    r8712_use_tkipkey_handler,
+			    (unsigned long)adapter);
 		/* Restore the PMK information to securitypriv structure
 		 * for the following connection. */
 		memcpy(&adapter->securitypriv.PMKIDList[0],
diff --git a/drivers/staging/rtl8712/os_intfs.c b/drivers/staging/rtl8712/os_intfs.c
index 13debb59ad97..6e776e5433f6 100644
--- a/drivers/staging/rtl8712/os_intfs.c
+++ b/drivers/staging/rtl8712/os_intfs.c
@@ -177,7 +177,7 @@ static uint loadparam(struct _adapter *padapter, struct  net_device *pnetdev)
 
 static int r871x_net_set_mac_address(struct net_device *pnetdev, void *p)
 {
-	struct _adapter *padapter = (struct _adapter *)netdev_priv(pnetdev);
+	struct _adapter *padapter = netdev_priv(pnetdev);
 	struct sockaddr *addr = p;
 
 	if (padapter->bup == false)
@@ -187,7 +187,7 @@ static int r871x_net_set_mac_address(struct net_device *pnetdev, void *p)
 
 static struct net_device_stats *r871x_net_get_stats(struct net_device *pnetdev)
 {
-	struct _adapter *padapter = (struct _adapter *) netdev_priv(pnetdev);
+	struct _adapter *padapter = netdev_priv(pnetdev);
 	struct xmit_priv *pxmitpriv = &(padapter->xmitpriv);
 	struct recv_priv *precvpriv = &(padapter->recvpriv);
 
@@ -221,7 +221,7 @@ struct net_device *r8712_init_netdev(void)
 		strcpy(ifname, "wlan%d");
 		dev_alloc_name(pnetdev, ifname);
 	}
-	padapter = (struct _adapter *) netdev_priv(pnetdev);
+	padapter = netdev_priv(pnetdev);
 	padapter->pnetdev = pnetdev;
 	pr_info("r8712u: register rtl8712_netdev_ops to netdev_ops\n");
 	pnetdev->netdev_ops = &rtl8712_netdev_ops;
@@ -255,20 +255,20 @@ void r8712_stop_drv_threads(struct _adapter *padapter)
 
 static void start_drv_timers(struct _adapter *padapter)
 {
-	_set_timer(&padapter->mlmepriv.sitesurveyctrl.sitesurvey_ctrl_timer,
-		   5000);
-	_set_timer(&padapter->mlmepriv.wdg_timer, 2000);
+	mod_timer(&padapter->mlmepriv.sitesurveyctrl.sitesurvey_ctrl_timer,
+		  jiffies + msecs_to_jiffies(5000));
+	mod_timer(&padapter->mlmepriv.wdg_timer,
+		  jiffies + msecs_to_jiffies(2000));
 }
 
 void r8712_stop_drv_timers(struct _adapter *padapter)
 {
-	_cancel_timer_ex(&padapter->mlmepriv.assoc_timer);
-	_cancel_timer_ex(&padapter->securitypriv.tkip_timer);
-	_cancel_timer_ex(&padapter->mlmepriv.scan_to_timer);
-	_cancel_timer_ex(&padapter->mlmepriv.dhcp_timer);
-	_cancel_timer_ex(&padapter->mlmepriv.wdg_timer);
-	_cancel_timer_ex(&padapter->mlmepriv.sitesurveyctrl.
-			 sitesurvey_ctrl_timer);
+	del_timer_sync(&padapter->mlmepriv.assoc_timer);
+	del_timer_sync(&padapter->securitypriv.tkip_timer);
+	del_timer_sync(&padapter->mlmepriv.scan_to_timer);
+	del_timer_sync(&padapter->mlmepriv.dhcp_timer);
+	del_timer_sync(&padapter->mlmepriv.wdg_timer);
+	del_timer_sync(&padapter->mlmepriv.sitesurveyctrl.sitesurvey_ctrl_timer);
 }
 
 static u8 init_default_value(struct _adapter *padapter)
@@ -322,8 +322,8 @@ u8 r8712_init_drv_sw(struct _adapter *padapter)
 	_r8712_init_recv_priv(&padapter->recvpriv, padapter);
 	memset((unsigned char *)&padapter->securitypriv, 0,
 	       sizeof(struct security_priv));
-	_init_timer(&(padapter->securitypriv.tkip_timer), padapter->pnetdev,
-		    r8712_use_tkipkey_handler, padapter);
+	setup_timer(&padapter->securitypriv.tkip_timer,
+		    r8712_use_tkipkey_handler, (unsigned long)padapter);
 	_r8712_init_sta_priv(&padapter->stapriv);
 	padapter->stapriv.padapter = padapter;
 	r8712_init_bcmc_stainfo(padapter);
@@ -384,7 +384,7 @@ static void enable_video_mode(struct _adapter *padapter, int cbw40_value)
  */
 static int netdev_open(struct net_device *pnetdev)
 {
-	struct _adapter *padapter = (struct _adapter *)netdev_priv(pnetdev);
+	struct _adapter *padapter = netdev_priv(pnetdev);
 
 	mutex_lock(&padapter->mutex_start);
 	if (padapter->bup == false) {
@@ -452,7 +452,7 @@ netdev_open_error:
  */
 static int netdev_close(struct net_device *pnetdev)
 {
-	struct _adapter *padapter = (struct _adapter *) netdev_priv(pnetdev);
+	struct _adapter *padapter = netdev_priv(pnetdev);
 
 	/* Close LED*/
 	padapter->ledpriv.LedControlHandler(padapter, LED_CTL_POWER_OFF);
diff --git a/drivers/staging/rtl8712/osdep_service.h b/drivers/staging/rtl8712/osdep_service.h
index 36348d900d34..0a7f58c59df5 100644
--- a/drivers/staging/rtl8712/osdep_service.h
+++ b/drivers/staging/rtl8712/osdep_service.h
@@ -60,26 +60,6 @@ struct	__queue	{
 #define LIST_CONTAINOR(ptr, type, member) \
 	((type *)((char *)(ptr)-(SIZE_T)(&((type *)0)->member)))
 
-static inline void _init_timer(struct timer_list *ptimer,
-			       struct  net_device *padapter,
-			       void *pfunc, void *cntx)
-{
-	ptimer->function = pfunc;
-	ptimer->data = (addr_t)cntx;
-	init_timer(ptimer);
-}
-
-static inline void _set_timer(struct timer_list *ptimer, u32 delay_time)
-{
-	mod_timer(ptimer, (jiffies+msecs_to_jiffies(delay_time)));
-}
-
-static inline void _cancel_timer(struct timer_list *ptimer, u8 *bcancelled)
-{
-	del_timer(ptimer);
-	*bcancelled = true; /*true ==1; false==0*/
-}
-
 #ifndef BIT
 	#define BIT(x)	(1 << (x))
 #endif
@@ -106,11 +86,6 @@ static inline void sleep_schedulable(int ms)
 	schedule_timeout(delta);
 }
 
-static inline unsigned char _cancel_timer_ex(struct timer_list *ptimer)
-{
-	return del_timer(ptimer);
-}
-
 static inline void flush_signals_thread(void)
 {
 	if (signal_pending(current))
diff --git a/drivers/staging/rtl8712/recv_linux.c b/drivers/staging/rtl8712/recv_linux.c
index 409c8c897256..799a0f9a5b2d 100644
--- a/drivers/staging/rtl8712/recv_linux.c
+++ b/drivers/staging/rtl8712/recv_linux.c
@@ -96,7 +96,7 @@ void r8712_handle_tkip_mic_err(struct _adapter *padapter, u8 bgroup)
 	else
 		ev.flags |= IW_MICFAILURE_PAIRWISE;
 	ev.src_addr.sa_family = ARPHRD_ETHER;
-	memcpy(ev.src_addr.sa_data, &pmlmepriv->assoc_bssid[0], ETH_ALEN);
+	ether_addr_copy(ev.src_addr.sa_data, &pmlmepriv->assoc_bssid[0]);
 	memset(&wrqu, 0x00, sizeof(wrqu));
 	wrqu.data.length = sizeof(ev);
 	wireless_send_event(padapter->pnetdev, IWEVMICHAELMICFAILURE, &wrqu,
@@ -137,18 +137,17 @@ _recv_indicatepkt_drop:
 	 precvpriv->rx_drop++;
 }
 
-static void _r8712_reordering_ctrl_timeout_handler (void *FunctionContext)
+static void _r8712_reordering_ctrl_timeout_handler (unsigned long data)
 {
 	struct recv_reorder_ctrl *preorder_ctrl =
-			 (struct recv_reorder_ctrl *)FunctionContext;
+			 (struct recv_reorder_ctrl *)data;
 
 	r8712_reordering_ctrl_timeout_handler(preorder_ctrl);
 }
 
 void r8712_init_recv_timer(struct recv_reorder_ctrl *preorder_ctrl)
 {
-	struct _adapter *padapter = preorder_ctrl->padapter;
-
-	_init_timer(&(preorder_ctrl->reordering_ctrl_timer), padapter->pnetdev,
-		    _r8712_reordering_ctrl_timeout_handler, preorder_ctrl);
+	setup_timer(&preorder_ctrl->reordering_ctrl_timer,
+		     _r8712_reordering_ctrl_timeout_handler,
+		     (unsigned long)preorder_ctrl);
 }
diff --git a/drivers/staging/rtl8712/rtl8712_cmd.c b/drivers/staging/rtl8712/rtl8712_cmd.c
index 62e53cc1d8b9..007f0a3ab13d 100644
--- a/drivers/staging/rtl8712/rtl8712_cmd.c
+++ b/drivers/staging/rtl8712/rtl8712_cmd.c
@@ -95,7 +95,7 @@ static void query_fw_rx_phy_status(struct _adapter *padapter)
 			val32 = r8712_read32(padapter, IOCMD_DATA_REG);
 		else /* time out */
 			val32 = 0;
-		val32 = val32 >> 4;
+		val32 >>= 4;
 		padapter->recvpriv.fw_rssi =
 			 (u8)r8712_signal_scale_mapping(val32);
 	}
diff --git a/drivers/staging/rtl8712/rtl8712_led.c b/drivers/staging/rtl8712/rtl8712_led.c
index 56e8add30a20..f1d47a0676c3 100644
--- a/drivers/staging/rtl8712/rtl8712_led.c
+++ b/drivers/staging/rtl8712/rtl8712_led.c
@@ -97,7 +97,8 @@ static void InitLed871x(struct _adapter *padapter, struct LED_871x *pLed,
 	pLed->bLedBlinkInProgress = false;
 	pLed->BlinkTimes = 0;
 	pLed->BlinkingLedState = LED_UNKNOWN;
-	_init_timer(&(pLed->BlinkTimer), nic, BlinkTimerCallback, pLed);
+	setup_timer(&pLed->BlinkTimer, BlinkTimerCallback,
+		    (unsigned long)pLed);
 	INIT_WORK(&pLed->BlinkWorkItem, BlinkWorkItemCallback);
 }
 
@@ -107,7 +108,7 @@ static void InitLed871x(struct _adapter *padapter, struct LED_871x *pLed,
  */
 static void DeInitLed871x(struct LED_871x *pLed)
 {
-	_cancel_timer_ex(&(pLed->BlinkTimer));
+	del_timer_sync(&pLed->BlinkTimer);
 	/* We should reset bLedBlinkInProgress if we cancel
 	 * the LedControlTimer, */
 	pLed->bLedBlinkInProgress = false;
@@ -258,21 +259,21 @@ static void SwLedBlink(struct LED_871x *pLed)
 		/* Schedule a timer to toggle LED state. */
 		switch (pLed->CurrLedState) {
 		case LED_BLINK_NORMAL:
-			_set_timer(&(pLed->BlinkTimer),
-				   LED_BLINK_NORMAL_INTERVAL);
+			mod_timer(&pLed->BlinkTimer, jiffies +
+				  msecs_to_jiffies(LED_BLINK_NORMAL_INTERVAL));
 			break;
 		case LED_BLINK_SLOWLY:
 		case LED_BLINK_StartToBlink:
-			_set_timer(&(pLed->BlinkTimer),
-				   LED_BLINK_SLOWLY_INTERVAL);
+			mod_timer(&pLed->BlinkTimer, jiffies +
+				  msecs_to_jiffies(LED_BLINK_SLOWLY_INTERVAL));
 			break;
 		case LED_BLINK_WPS:
-			_set_timer(&(pLed->BlinkTimer),
-					LED_BLINK_LONG_INTERVAL);
+			mod_timer(&pLed->BlinkTimer, jiffies +
+				  msecs_to_jiffies(LED_BLINK_LONG_INTERVAL));
 			break;
 		default:
-			_set_timer(&(pLed->BlinkTimer),
-				   LED_BLINK_SLOWLY_INTERVAL);
+			mod_timer(&pLed->BlinkTimer, jiffies +
+				  msecs_to_jiffies(LED_BLINK_SLOWLY_INTERVAL));
 			break;
 		}
 	}
@@ -315,16 +316,16 @@ static void SwLedBlink1(struct LED_871x *pLed)
 			pLed->BlinkingLedState = LED_OFF;
 		else
 			pLed->BlinkingLedState = LED_ON;
-		_set_timer(&(pLed->BlinkTimer),
-			   LED_BLINK_NO_LINK_INTERVAL_ALPHA);
+		mod_timer(&pLed->BlinkTimer, jiffies +
+			  msecs_to_jiffies(LED_BLINK_NO_LINK_INTERVAL_ALPHA));
 		break;
 	case LED_BLINK_NORMAL:
 		if (pLed->bLedOn)
 			pLed->BlinkingLedState = LED_OFF;
 		else
 			pLed->BlinkingLedState = LED_ON;
-		_set_timer(&(pLed->BlinkTimer),
-			   LED_BLINK_LINK_INTERVAL_ALPHA);
+		mod_timer(&pLed->BlinkTimer, jiffies +
+			  msecs_to_jiffies(LED_BLINK_LINK_INTERVAL_ALPHA));
 		break;
 	case LED_SCAN_BLINK:
 		pLed->BlinkTimes--;
@@ -338,8 +339,8 @@ static void SwLedBlink1(struct LED_871x *pLed)
 					pLed->BlinkingLedState = LED_OFF;
 				else
 					pLed->BlinkingLedState = LED_ON;
-				_set_timer(&(pLed->BlinkTimer),
-					   LED_BLINK_LINK_INTERVAL_ALPHA);
+				mod_timer(&pLed->BlinkTimer, jiffies +
+					  msecs_to_jiffies(LED_BLINK_LINK_INTERVAL_ALPHA));
 			} else if (!check_fwstate(pmlmepriv, _FW_LINKED)) {
 				pLed->bLedNoLinkBlinkInProgress = true;
 				pLed->CurrLedState = LED_BLINK_SLOWLY;
@@ -347,8 +348,8 @@ static void SwLedBlink1(struct LED_871x *pLed)
 					pLed->BlinkingLedState = LED_OFF;
 				else
 					pLed->BlinkingLedState = LED_ON;
-				_set_timer(&(pLed->BlinkTimer),
-					   LED_BLINK_NO_LINK_INTERVAL_ALPHA);
+				mod_timer(&pLed->BlinkTimer, jiffies +
+					  msecs_to_jiffies(LED_BLINK_NO_LINK_INTERVAL_ALPHA));
 			}
 			pLed->bLedScanBlinkInProgress = false;
 		} else {
@@ -356,8 +357,8 @@ static void SwLedBlink1(struct LED_871x *pLed)
 				pLed->BlinkingLedState = LED_OFF;
 			else
 				pLed->BlinkingLedState = LED_ON;
-			_set_timer(&(pLed->BlinkTimer),
-				   LED_BLINK_SCAN_INTERVAL_ALPHA);
+			mod_timer(&pLed->BlinkTimer, jiffies +
+				  msecs_to_jiffies(LED_BLINK_SCAN_INTERVAL_ALPHA));
 		}
 		break;
 	case LED_TXRX_BLINK:
@@ -372,8 +373,8 @@ static void SwLedBlink1(struct LED_871x *pLed)
 					pLed->BlinkingLedState = LED_OFF;
 				else
 					pLed->BlinkingLedState = LED_ON;
-				_set_timer(&(pLed->BlinkTimer),
-					   LED_BLINK_LINK_INTERVAL_ALPHA);
+				mod_timer(&pLed->BlinkTimer, jiffies +
+					  msecs_to_jiffies(LED_BLINK_LINK_INTERVAL_ALPHA));
 			} else if (!check_fwstate(pmlmepriv, _FW_LINKED)) {
 				pLed->bLedNoLinkBlinkInProgress = true;
 				pLed->CurrLedState = LED_BLINK_SLOWLY;
@@ -381,8 +382,8 @@ static void SwLedBlink1(struct LED_871x *pLed)
 					pLed->BlinkingLedState = LED_OFF;
 				else
 					pLed->BlinkingLedState = LED_ON;
-				_set_timer(&(pLed->BlinkTimer),
-					   LED_BLINK_NO_LINK_INTERVAL_ALPHA);
+				mod_timer(&pLed->BlinkTimer, jiffies +
+					  msecs_to_jiffies(LED_BLINK_NO_LINK_INTERVAL_ALPHA));
 			}
 			pLed->BlinkTimes = 0;
 			pLed->bLedBlinkInProgress = false;
@@ -391,8 +392,8 @@ static void SwLedBlink1(struct LED_871x *pLed)
 				pLed->BlinkingLedState = LED_OFF;
 			else
 				pLed->BlinkingLedState = LED_ON;
-			_set_timer(&(pLed->BlinkTimer),
-				   LED_BLINK_FASTER_INTERVAL_ALPHA);
+			mod_timer(&pLed->BlinkTimer, jiffies +
+				  msecs_to_jiffies(LED_BLINK_FASTER_INTERVAL_ALPHA));
 		}
 		break;
 	case LED_BLINK_WPS:
@@ -400,14 +401,14 @@ static void SwLedBlink1(struct LED_871x *pLed)
 			pLed->BlinkingLedState = LED_OFF;
 		else
 			pLed->BlinkingLedState = LED_ON;
-		_set_timer(&(pLed->BlinkTimer),
-			   LED_BLINK_SCAN_INTERVAL_ALPHA);
+		mod_timer(&pLed->BlinkTimer, jiffies +
+			  msecs_to_jiffies(LED_BLINK_SCAN_INTERVAL_ALPHA));
 		break;
 	case LED_BLINK_WPS_STOP:	/* WPS success */
 		if (pLed->BlinkingLedState == LED_ON) {
 			pLed->BlinkingLedState = LED_OFF;
-			_set_timer(&(pLed->BlinkTimer),
-				   LED_BLINK_WPS_SUCESS_INTERVAL_ALPHA);
+			mod_timer(&pLed->BlinkTimer, jiffies +
+				  msecs_to_jiffies(LED_BLINK_WPS_SUCESS_INTERVAL_ALPHA));
 			bStopBlinking = false;
 		} else
 			bStopBlinking = true;
@@ -418,8 +419,8 @@ static void SwLedBlink1(struct LED_871x *pLed)
 				pLed->BlinkingLedState = LED_OFF;
 			else
 				pLed->BlinkingLedState = LED_ON;
-			_set_timer(&(pLed->BlinkTimer),
-				   LED_BLINK_LINK_INTERVAL_ALPHA);
+			mod_timer(&pLed->BlinkTimer, jiffies +
+				  msecs_to_jiffies(LED_BLINK_LINK_INTERVAL_ALPHA));
 		}
 		pLed->bLedWPSBlinkInProgress = false;
 		break;
@@ -460,8 +461,8 @@ static void SwLedBlink2(struct LED_871x *pLed)
 				pLed->BlinkingLedState = LED_OFF;
 			else
 				pLed->BlinkingLedState = LED_ON;
-			_set_timer(&(pLed->BlinkTimer),
-				   LED_BLINK_SCAN_INTERVAL_ALPHA);
+			mod_timer(&pLed->BlinkTimer, jiffies +
+				  msecs_to_jiffies(LED_BLINK_SCAN_INTERVAL_ALPHA));
 		}
 		break;
 	case LED_TXRX_BLINK:
@@ -484,8 +485,8 @@ static void SwLedBlink2(struct LED_871x *pLed)
 				pLed->BlinkingLedState = LED_OFF;
 			else
 				pLed->BlinkingLedState = LED_ON;
-			_set_timer(&(pLed->BlinkTimer),
-				   LED_BLINK_FASTER_INTERVAL_ALPHA);
+			mod_timer(&pLed->BlinkTimer, jiffies +
+				  msecs_to_jiffies(LED_BLINK_FASTER_INTERVAL_ALPHA));
 		}
 		break;
 	default:
@@ -528,8 +529,8 @@ static void SwLedBlink3(struct LED_871x *pLed)
 				pLed->BlinkingLedState = LED_OFF;
 			else
 				pLed->BlinkingLedState = LED_ON;
-			_set_timer(&(pLed->BlinkTimer),
-				   LED_BLINK_SCAN_INTERVAL_ALPHA);
+			mod_timer(&pLed->BlinkTimer, jiffies +
+				  msecs_to_jiffies(LED_BLINK_SCAN_INTERVAL_ALPHA));
 		}
 		break;
 	case LED_TXRX_BLINK:
@@ -554,8 +555,8 @@ static void SwLedBlink3(struct LED_871x *pLed)
 				pLed->BlinkingLedState = LED_OFF;
 			else
 				pLed->BlinkingLedState = LED_ON;
-			_set_timer(&(pLed->BlinkTimer),
-				   LED_BLINK_FASTER_INTERVAL_ALPHA);
+			mod_timer(&pLed->BlinkTimer, jiffies +
+				  msecs_to_jiffies(LED_BLINK_FASTER_INTERVAL_ALPHA));
 		}
 		break;
 	case LED_BLINK_WPS:
@@ -563,14 +564,14 @@ static void SwLedBlink3(struct LED_871x *pLed)
 			pLed->BlinkingLedState = LED_OFF;
 		else
 			pLed->BlinkingLedState = LED_ON;
-		_set_timer(&(pLed->BlinkTimer),
-			   LED_BLINK_SCAN_INTERVAL_ALPHA);
+		mod_timer(&pLed->BlinkTimer, jiffies +
+			  msecs_to_jiffies(LED_BLINK_SCAN_INTERVAL_ALPHA));
 		break;
 	case LED_BLINK_WPS_STOP:	/*WPS success*/
 		if (pLed->BlinkingLedState == LED_ON) {
 			pLed->BlinkingLedState = LED_OFF;
-			_set_timer(&(pLed->BlinkTimer),
-				   LED_BLINK_WPS_SUCESS_INTERVAL_ALPHA);
+			mod_timer(&pLed->BlinkTimer, jiffies +
+				  msecs_to_jiffies(LED_BLINK_WPS_SUCESS_INTERVAL_ALPHA));
 			bStopBlinking = false;
 		} else
 			bStopBlinking = true;
@@ -610,18 +611,18 @@ static void SwLedBlink4(struct LED_871x *pLed)
 			pLed->BlinkingLedState = LED_OFF;
 		else
 			pLed->BlinkingLedState = LED_ON;
-		_set_timer(&(pLed->BlinkTimer),
-			   LED_BLINK_NO_LINK_INTERVAL_ALPHA);
+		mod_timer(&pLed->BlinkTimer, jiffies +
+			  msecs_to_jiffies(LED_BLINK_NO_LINK_INTERVAL_ALPHA));
 		break;
 	case LED_BLINK_StartToBlink:
 		if (pLed->bLedOn) {
 			pLed->BlinkingLedState = LED_OFF;
-			_set_timer(&(pLed->BlinkTimer),
-				   LED_BLINK_SLOWLY_INTERVAL);
+			mod_timer(&pLed->BlinkTimer, jiffies +
+				  msecs_to_jiffies(LED_BLINK_SLOWLY_INTERVAL));
 		} else {
 			pLed->BlinkingLedState = LED_ON;
-			_set_timer(&(pLed->BlinkTimer),
-				   LED_BLINK_NORMAL_INTERVAL);
+			mod_timer(&pLed->BlinkTimer, jiffies +
+				  msecs_to_jiffies(LED_BLINK_NORMAL_INTERVAL));
 		}
 		break;
 	case LED_SCAN_BLINK:
@@ -635,16 +636,16 @@ static void SwLedBlink4(struct LED_871x *pLed)
 				pLed->BlinkingLedState = LED_OFF;
 			else
 				pLed->BlinkingLedState = LED_ON;
-			_set_timer(&(pLed->BlinkTimer),
-				   LED_BLINK_NO_LINK_INTERVAL_ALPHA);
+			mod_timer(&pLed->BlinkTimer, jiffies +
+				  msecs_to_jiffies(LED_BLINK_NO_LINK_INTERVAL_ALPHA));
 			pLed->bLedScanBlinkInProgress = false;
 		} else {
 			if (pLed->bLedOn)
 				pLed->BlinkingLedState = LED_OFF;
 			else
 				pLed->BlinkingLedState = LED_ON;
-			_set_timer(&(pLed->BlinkTimer),
-				   LED_BLINK_SCAN_INTERVAL_ALPHA);
+			mod_timer(&pLed->BlinkTimer, jiffies +
+				  msecs_to_jiffies(LED_BLINK_SCAN_INTERVAL_ALPHA));
 		}
 		break;
 	case LED_TXRX_BLINK:
@@ -658,27 +659,27 @@ static void SwLedBlink4(struct LED_871x *pLed)
 				pLed->BlinkingLedState = LED_OFF;
 			else
 				pLed->BlinkingLedState = LED_ON;
-			_set_timer(&(pLed->BlinkTimer),
-				   LED_BLINK_NO_LINK_INTERVAL_ALPHA);
+			mod_timer(&pLed->BlinkTimer, jiffies +
+				  msecs_to_jiffies(LED_BLINK_NO_LINK_INTERVAL_ALPHA));
 			pLed->bLedBlinkInProgress = false;
 		} else {
 			 if (pLed->bLedOn)
 				pLed->BlinkingLedState = LED_OFF;
 			else
 				pLed->BlinkingLedState = LED_ON;
-			_set_timer(&(pLed->BlinkTimer),
-				   LED_BLINK_FASTER_INTERVAL_ALPHA);
+			mod_timer(&pLed->BlinkTimer, jiffies +
+				  msecs_to_jiffies(LED_BLINK_FASTER_INTERVAL_ALPHA));
 		}
 		break;
 	case LED_BLINK_WPS:
 		if (pLed->bLedOn) {
 			pLed->BlinkingLedState = LED_OFF;
-			_set_timer(&(pLed->BlinkTimer),
-				   LED_BLINK_SLOWLY_INTERVAL);
+			mod_timer(&pLed->BlinkTimer, jiffies +
+				  msecs_to_jiffies(LED_BLINK_SLOWLY_INTERVAL));
 		} else {
 			pLed->BlinkingLedState = LED_ON;
-			_set_timer(&(pLed->BlinkTimer),
-				   LED_BLINK_NORMAL_INTERVAL);
+			mod_timer(&pLed->BlinkTimer, jiffies +
+				  msecs_to_jiffies(LED_BLINK_NORMAL_INTERVAL));
 		}
 		break;
 	case LED_BLINK_WPS_STOP:	/*WPS authentication fail*/
@@ -686,7 +687,8 @@ static void SwLedBlink4(struct LED_871x *pLed)
 			pLed->BlinkingLedState = LED_OFF;
 		else
 			pLed->BlinkingLedState = LED_ON;
-		_set_timer(&(pLed->BlinkTimer), LED_BLINK_NORMAL_INTERVAL);
+		mod_timer(&pLed->BlinkTimer, jiffies +
+			  msecs_to_jiffies(LED_BLINK_NORMAL_INTERVAL));
 		break;
 	case LED_BLINK_WPS_STOP_OVERLAP:	/*WPS session overlap */
 		pLed->BlinkTimes--;
@@ -699,15 +701,15 @@ static void SwLedBlink4(struct LED_871x *pLed)
 		if (bStopBlinking) {
 			pLed->BlinkTimes = 10;
 			pLed->BlinkingLedState = LED_ON;
-			_set_timer(&(pLed->BlinkTimer),
-				   LED_BLINK_LINK_INTERVAL_ALPHA);
+			mod_timer(&pLed->BlinkTimer, jiffies +
+				  msecs_to_jiffies(LED_BLINK_LINK_INTERVAL_ALPHA));
 		} else {
 			if (pLed->bLedOn)
 				pLed->BlinkingLedState = LED_OFF;
 			else
 				pLed->BlinkingLedState = LED_ON;
-			_set_timer(&(pLed->BlinkTimer),
-				   LED_BLINK_NORMAL_INTERVAL);
+			mod_timer(&pLed->BlinkTimer, jiffies +
+				  msecs_to_jiffies(LED_BLINK_NORMAL_INTERVAL));
 		}
 		break;
 	default:
@@ -734,16 +736,16 @@ static void SwLedBlink5(struct LED_871x *pLed)
 			pLed->CurrLedState = LED_ON;
 			pLed->BlinkingLedState = LED_ON;
 			if (!pLed->bLedOn)
-				_set_timer(&(pLed->BlinkTimer),
-					   LED_BLINK_FASTER_INTERVAL_ALPHA);
+				mod_timer(&pLed->BlinkTimer, jiffies +
+					  msecs_to_jiffies(LED_BLINK_FASTER_INTERVAL_ALPHA));
 			pLed->bLedScanBlinkInProgress = false;
 		} else {
 			if (pLed->bLedOn)
 				pLed->BlinkingLedState = LED_OFF;
 			else
 				pLed->BlinkingLedState = LED_ON;
-			_set_timer(&(pLed->BlinkTimer),
-				   LED_BLINK_SCAN_INTERVAL_ALPHA);
+			mod_timer(&pLed->BlinkTimer, jiffies +
+				  msecs_to_jiffies(LED_BLINK_SCAN_INTERVAL_ALPHA));
 		}
 		break;
 	case LED_TXRX_BLINK:
@@ -754,16 +756,16 @@ static void SwLedBlink5(struct LED_871x *pLed)
 			pLed->CurrLedState = LED_ON;
 			pLed->BlinkingLedState = LED_ON;
 			if (!pLed->bLedOn)
-				_set_timer(&(pLed->BlinkTimer),
-					   LED_BLINK_FASTER_INTERVAL_ALPHA);
+				mod_timer(&pLed->BlinkTimer, jiffies +
+					  msecs_to_jiffies(LED_BLINK_FASTER_INTERVAL_ALPHA));
 			pLed->bLedBlinkInProgress = false;
 		} else {
 			 if (pLed->bLedOn)
 				pLed->BlinkingLedState = LED_OFF;
 			else
 				pLed->BlinkingLedState = LED_ON;
-			_set_timer(&(pLed->BlinkTimer),
-				   LED_BLINK_FASTER_INTERVAL_ALPHA);
+			mod_timer(&pLed->BlinkTimer, jiffies +
+				  msecs_to_jiffies(LED_BLINK_FASTER_INTERVAL_ALPHA));
 		}
 		break;
 	default:
@@ -797,8 +799,8 @@ static void SwLedBlink6(struct LED_871x *pLed)
 				pLed->BlinkingLedState = LED_OFF;
 			else
 				pLed->BlinkingLedState = LED_ON;
-			_set_timer(&(pLed->BlinkTimer),
-				   LED_BLINK_FASTER_INTERVAL_ALPHA);
+			mod_timer(&pLed->BlinkTimer, jiffies +
+				  msecs_to_jiffies(LED_BLINK_FASTER_INTERVAL_ALPHA));
 		}
 		break;
 	case LED_BLINK_WPS:
@@ -806,7 +808,8 @@ static void SwLedBlink6(struct LED_871x *pLed)
 			pLed->BlinkingLedState = LED_OFF;
 		else
 			pLed->BlinkingLedState = LED_ON;
-		_set_timer(&(pLed->BlinkTimer), LED_BLINK_SCAN_INTERVAL_ALPHA);
+		mod_timer(&pLed->BlinkTimer, jiffies +
+			  msecs_to_jiffies(LED_BLINK_SCAN_INTERVAL_ALPHA));
 		break;
 
 	default:
@@ -895,11 +898,11 @@ static void SwLedControlMode1(struct _adapter *padapter,
 			  IS_LED_WPS_BLINKING(pLed))
 				return;
 			if (pLed->bLedLinkBlinkInProgress == true) {
-				_cancel_timer_ex(&(pLed->BlinkTimer));
+				del_timer_sync(&pLed->BlinkTimer);
 				pLed->bLedLinkBlinkInProgress = false;
 			}
 			if (pLed->bLedBlinkInProgress == true) {
-				_cancel_timer_ex(&(pLed->BlinkTimer));
+				del_timer_sync(&pLed->BlinkTimer);
 				pLed->bLedBlinkInProgress = false;
 			}
 			pLed->bLedNoLinkBlinkInProgress = true;
@@ -908,8 +911,8 @@ static void SwLedControlMode1(struct _adapter *padapter,
 				pLed->BlinkingLedState = LED_OFF;
 			else
 				pLed->BlinkingLedState = LED_ON;
-			_set_timer(&(pLed->BlinkTimer),
-				   LED_BLINK_NO_LINK_INTERVAL_ALPHA);
+			mod_timer(&pLed->BlinkTimer, jiffies +
+				  msecs_to_jiffies(LED_BLINK_NO_LINK_INTERVAL_ALPHA));
 		}
 		break;
 	case LED_CTL_LINK:
@@ -918,11 +921,11 @@ static void SwLedControlMode1(struct _adapter *padapter,
 			    IS_LED_WPS_BLINKING(pLed))
 				return;
 			if (pLed->bLedNoLinkBlinkInProgress == true) {
-				_cancel_timer_ex(&(pLed->BlinkTimer));
+				del_timer_sync(&pLed->BlinkTimer);
 				pLed->bLedNoLinkBlinkInProgress = false;
 			}
 			if (pLed->bLedBlinkInProgress == true) {
-				_cancel_timer_ex(&(pLed->BlinkTimer));
+				del_timer_sync(&pLed->BlinkTimer);
 				pLed->bLedBlinkInProgress = false;
 			}
 			pLed->bLedLinkBlinkInProgress = true;
@@ -931,8 +934,8 @@ static void SwLedControlMode1(struct _adapter *padapter,
 				pLed->BlinkingLedState = LED_OFF;
 			else
 				pLed->BlinkingLedState = LED_ON;
-			_set_timer(&(pLed->BlinkTimer),
-				   LED_BLINK_LINK_INTERVAL_ALPHA);
+			mod_timer(&pLed->BlinkTimer, jiffies +
+				  msecs_to_jiffies(LED_BLINK_LINK_INTERVAL_ALPHA));
 		}
 		break;
 	case LED_CTL_SITE_SURVEY:
@@ -943,15 +946,15 @@ static void SwLedControlMode1(struct _adapter *padapter,
 			if (IS_LED_WPS_BLINKING(pLed))
 				return;
 			if (pLed->bLedNoLinkBlinkInProgress == true) {
-				_cancel_timer_ex(&(pLed->BlinkTimer));
+				del_timer_sync(&pLed->BlinkTimer);
 				pLed->bLedNoLinkBlinkInProgress = false;
 			}
 			if (pLed->bLedLinkBlinkInProgress == true) {
-				_cancel_timer_ex(&(pLed->BlinkTimer));
+				del_timer_sync(&pLed->BlinkTimer);
 				 pLed->bLedLinkBlinkInProgress = false;
 			}
 			if (pLed->bLedBlinkInProgress == true) {
-				_cancel_timer_ex(&(pLed->BlinkTimer));
+				del_timer_sync(&pLed->BlinkTimer);
 				pLed->bLedBlinkInProgress = false;
 			}
 			pLed->bLedScanBlinkInProgress = true;
@@ -961,8 +964,8 @@ static void SwLedControlMode1(struct _adapter *padapter,
 				pLed->BlinkingLedState = LED_OFF;
 			else
 				pLed->BlinkingLedState = LED_ON;
-			_set_timer(&(pLed->BlinkTimer),
-				   LED_BLINK_SCAN_INTERVAL_ALPHA);
+			mod_timer(&pLed->BlinkTimer, jiffies +
+				  msecs_to_jiffies(LED_BLINK_SCAN_INTERVAL_ALPHA));
 		 }
 		break;
 	case LED_CTL_TX:
@@ -972,11 +975,11 @@ static void SwLedControlMode1(struct _adapter *padapter,
 			    IS_LED_WPS_BLINKING(pLed))
 				return;
 			if (pLed->bLedNoLinkBlinkInProgress == true) {
-				_cancel_timer_ex(&(pLed->BlinkTimer));
+				del_timer_sync(&pLed->BlinkTimer);
 				pLed->bLedNoLinkBlinkInProgress = false;
 			}
 			if (pLed->bLedLinkBlinkInProgress == true) {
-				_cancel_timer_ex(&(pLed->BlinkTimer));
+				del_timer_sync(&pLed->BlinkTimer);
 				pLed->bLedLinkBlinkInProgress = false;
 			}
 			pLed->bLedBlinkInProgress = true;
@@ -986,8 +989,8 @@ static void SwLedControlMode1(struct _adapter *padapter,
 				pLed->BlinkingLedState = LED_OFF;
 			else
 				pLed->BlinkingLedState = LED_ON;
-			_set_timer(&(pLed->BlinkTimer),
-				   LED_BLINK_FASTER_INTERVAL_ALPHA);
+			mod_timer(&pLed->BlinkTimer, jiffies +
+				  msecs_to_jiffies(LED_BLINK_FASTER_INTERVAL_ALPHA));
 		}
 		break;
 
@@ -995,19 +998,19 @@ static void SwLedControlMode1(struct _adapter *padapter,
 	case LED_CTL_START_WPS_BOTTON:
 		 if (pLed->bLedWPSBlinkInProgress == false) {
 			if (pLed->bLedNoLinkBlinkInProgress == true) {
-				_cancel_timer_ex(&(pLed->BlinkTimer));
+				del_timer_sync(&pLed->BlinkTimer);
 				pLed->bLedNoLinkBlinkInProgress = false;
 			}
 			if (pLed->bLedLinkBlinkInProgress == true) {
-				_cancel_timer_ex(&(pLed->BlinkTimer));
+				del_timer_sync(&pLed->BlinkTimer);
 				 pLed->bLedLinkBlinkInProgress = false;
 			}
 			if (pLed->bLedBlinkInProgress == true) {
-				_cancel_timer_ex(&(pLed->BlinkTimer));
+				del_timer_sync(&pLed->BlinkTimer);
 				pLed->bLedBlinkInProgress = false;
 			}
 			if (pLed->bLedScanBlinkInProgress == true) {
-				_cancel_timer_ex(&(pLed->BlinkTimer));
+				del_timer_sync(&pLed->BlinkTimer);
 				pLed->bLedScanBlinkInProgress = false;
 			}
 			pLed->bLedWPSBlinkInProgress = true;
@@ -1016,44 +1019,45 @@ static void SwLedControlMode1(struct _adapter *padapter,
 				pLed->BlinkingLedState = LED_OFF;
 			else
 				pLed->BlinkingLedState = LED_ON;
-			_set_timer(&(pLed->BlinkTimer),
-				   LED_BLINK_SCAN_INTERVAL_ALPHA);
+			mod_timer(&pLed->BlinkTimer, jiffies +
+				  msecs_to_jiffies(LED_BLINK_SCAN_INTERVAL_ALPHA));
 		}
 		break;
 	case LED_CTL_STOP_WPS:
 		if (pLed->bLedNoLinkBlinkInProgress == true) {
-			_cancel_timer_ex(&(pLed->BlinkTimer));
+			del_timer_sync(&pLed->BlinkTimer);
 			pLed->bLedNoLinkBlinkInProgress = false;
 		}
 		if (pLed->bLedLinkBlinkInProgress == true) {
-			_cancel_timer_ex(&(pLed->BlinkTimer));
+			del_timer_sync(&pLed->BlinkTimer);
 			 pLed->bLedLinkBlinkInProgress = false;
 		}
 		if (pLed->bLedBlinkInProgress == true) {
-			_cancel_timer_ex(&(pLed->BlinkTimer));
+			del_timer_sync(&pLed->BlinkTimer);
 			pLed->bLedBlinkInProgress = false;
 		}
 		if (pLed->bLedScanBlinkInProgress == true) {
-			_cancel_timer_ex(&(pLed->BlinkTimer));
+			del_timer_sync(&pLed->BlinkTimer);
 			pLed->bLedScanBlinkInProgress = false;
 		}
 		if (pLed->bLedWPSBlinkInProgress)
-			_cancel_timer_ex(&(pLed->BlinkTimer));
+			del_timer_sync(&pLed->BlinkTimer);
 		else
 			pLed->bLedWPSBlinkInProgress = true;
 		pLed->CurrLedState = LED_BLINK_WPS_STOP;
 		if (pLed->bLedOn) {
 			pLed->BlinkingLedState = LED_OFF;
-			_set_timer(&(pLed->BlinkTimer),
-				   LED_BLINK_WPS_SUCESS_INTERVAL_ALPHA);
+			mod_timer(&pLed->BlinkTimer, jiffies +
+				  msecs_to_jiffies(LED_BLINK_WPS_SUCESS_INTERVAL_ALPHA));
 		} else {
 			pLed->BlinkingLedState = LED_ON;
-			_set_timer(&(pLed->BlinkTimer), 0);
+			mod_timer(&pLed->BlinkTimer,
+				  jiffies + msecs_to_jiffies(0));
 		}
 		break;
 	case LED_CTL_STOP_WPS_FAIL:
 		if (pLed->bLedWPSBlinkInProgress) {
-			_cancel_timer_ex(&(pLed->BlinkTimer));
+			del_timer_sync(&pLed->BlinkTimer);
 			pLed->bLedWPSBlinkInProgress = false;
 		}
 		pLed->bLedNoLinkBlinkInProgress = true;
@@ -1062,33 +1066,34 @@ static void SwLedControlMode1(struct _adapter *padapter,
 			pLed->BlinkingLedState = LED_OFF;
 		else
 			pLed->BlinkingLedState = LED_ON;
-		_set_timer(&(pLed->BlinkTimer),
-			   LED_BLINK_NO_LINK_INTERVAL_ALPHA);
+		mod_timer(&pLed->BlinkTimer, jiffies +
+			  msecs_to_jiffies(LED_BLINK_NO_LINK_INTERVAL_ALPHA));
 		break;
 	case LED_CTL_POWER_OFF:
 		pLed->CurrLedState = LED_OFF;
 		pLed->BlinkingLedState = LED_OFF;
 		if (pLed->bLedNoLinkBlinkInProgress) {
-			_cancel_timer_ex(&(pLed->BlinkTimer));
+			del_timer_sync(&pLed->BlinkTimer);
 			pLed->bLedNoLinkBlinkInProgress = false;
 		}
 		if (pLed->bLedLinkBlinkInProgress) {
-			_cancel_timer_ex(&(pLed->BlinkTimer));
+			del_timer_sync(&pLed->BlinkTimer);
 			pLed->bLedLinkBlinkInProgress = false;
 		}
 		if (pLed->bLedBlinkInProgress) {
-			_cancel_timer_ex(&(pLed->BlinkTimer));
+			del_timer_sync(&pLed->BlinkTimer);
 			pLed->bLedBlinkInProgress = false;
 		}
 		if (pLed->bLedWPSBlinkInProgress) {
-			_cancel_timer_ex(&(pLed->BlinkTimer));
+			del_timer_sync(&pLed->BlinkTimer);
 			pLed->bLedWPSBlinkInProgress = false;
 		}
 		if (pLed->bLedScanBlinkInProgress) {
-			_cancel_timer_ex(&(pLed->BlinkTimer));
+			del_timer_sync(&pLed->BlinkTimer);
 			pLed->bLedScanBlinkInProgress = false;
 		}
-		_set_timer(&(pLed->BlinkTimer), 0);
+		mod_timer(&pLed->BlinkTimer,
+			  jiffies + msecs_to_jiffies(0));
 		break;
 	default:
 		break;
@@ -1111,7 +1116,7 @@ static void SwLedControlMode2(struct _adapter *padapter,
 				return;
 
 			if (pLed->bLedBlinkInProgress == true) {
-				_cancel_timer_ex(&(pLed->BlinkTimer));
+				del_timer_sync(&pLed->BlinkTimer);
 				pLed->bLedBlinkInProgress = false;
 			}
 			pLed->bLedScanBlinkInProgress = true;
@@ -1121,8 +1126,8 @@ static void SwLedControlMode2(struct _adapter *padapter,
 				pLed->BlinkingLedState = LED_OFF;
 			else
 				pLed->BlinkingLedState = LED_ON;
-			_set_timer(&(pLed->BlinkTimer),
-				   LED_BLINK_SCAN_INTERVAL_ALPHA);
+			mod_timer(&pLed->BlinkTimer, jiffies +
+				  msecs_to_jiffies(LED_BLINK_SCAN_INTERVAL_ALPHA));
 		 }
 		break;
 
@@ -1140,8 +1145,8 @@ static void SwLedControlMode2(struct _adapter *padapter,
 				pLed->BlinkingLedState = LED_OFF;
 			else
 				pLed->BlinkingLedState = LED_ON;
-			_set_timer(&(pLed->BlinkTimer),
-				   LED_BLINK_FASTER_INTERVAL_ALPHA);
+			mod_timer(&pLed->BlinkTimer, jiffies +
+				  msecs_to_jiffies(LED_BLINK_FASTER_INTERVAL_ALPHA));
 		}
 		break;
 
@@ -1149,32 +1154,34 @@ static void SwLedControlMode2(struct _adapter *padapter,
 		pLed->CurrLedState = LED_ON;
 		pLed->BlinkingLedState = LED_ON;
 		if (pLed->bLedBlinkInProgress) {
-			_cancel_timer_ex(&(pLed->BlinkTimer));
+			del_timer_sync(&pLed->BlinkTimer);
 			pLed->bLedBlinkInProgress = false;
 		}
 		if (pLed->bLedScanBlinkInProgress) {
-			_cancel_timer_ex(&(pLed->BlinkTimer));
+			del_timer_sync(&pLed->BlinkTimer);
 			pLed->bLedScanBlinkInProgress = false;
 		}
 
-		_set_timer(&(pLed->BlinkTimer), 0);
+		mod_timer(&pLed->BlinkTimer,
+			  jiffies + msecs_to_jiffies(0));
 		break;
 
 	case LED_CTL_START_WPS: /*wait until xinpin finish*/
 	case LED_CTL_START_WPS_BOTTON:
 		if (pLed->bLedWPSBlinkInProgress == false) {
 			if (pLed->bLedBlinkInProgress == true) {
-				_cancel_timer_ex(&(pLed->BlinkTimer));
+				del_timer_sync(&pLed->BlinkTimer);
 				pLed->bLedBlinkInProgress = false;
 			}
 			if (pLed->bLedScanBlinkInProgress == true) {
-				_cancel_timer_ex(&(pLed->BlinkTimer));
+				del_timer_sync(&pLed->BlinkTimer);
 				pLed->bLedScanBlinkInProgress = false;
 			}
 			pLed->bLedWPSBlinkInProgress = true;
 			pLed->CurrLedState = LED_ON;
 			pLed->BlinkingLedState = LED_ON;
-			_set_timer(&(pLed->BlinkTimer), 0);
+			mod_timer(&pLed->BlinkTimer,
+				  jiffies + msecs_to_jiffies(0));
 		 }
 		break;
 
@@ -1182,14 +1189,16 @@ static void SwLedControlMode2(struct _adapter *padapter,
 		pLed->bLedWPSBlinkInProgress = false;
 		pLed->CurrLedState = LED_ON;
 		pLed->BlinkingLedState = LED_ON;
-		_set_timer(&(pLed->BlinkTimer), 0);
+		mod_timer(&pLed->BlinkTimer,
+			  jiffies + msecs_to_jiffies(0));
 		break;
 
 	case LED_CTL_STOP_WPS_FAIL:
 		pLed->bLedWPSBlinkInProgress = false;
 		pLed->CurrLedState = LED_OFF;
 		pLed->BlinkingLedState = LED_OFF;
-		_set_timer(&(pLed->BlinkTimer), 0);
+		mod_timer(&pLed->BlinkTimer,
+			  jiffies + msecs_to_jiffies(0));
 		break;
 
 	case LED_CTL_START_TO_LINK:
@@ -1197,25 +1206,27 @@ static void SwLedControlMode2(struct _adapter *padapter,
 		if (!IS_LED_BLINKING(pLed)) {
 			pLed->CurrLedState = LED_OFF;
 			pLed->BlinkingLedState = LED_OFF;
-			_set_timer(&(pLed->BlinkTimer), 0);
+			mod_timer(&pLed->BlinkTimer,
+				  jiffies + msecs_to_jiffies(0));
 		}
 		break;
 	case LED_CTL_POWER_OFF:
 		pLed->CurrLedState = LED_OFF;
 		pLed->BlinkingLedState = LED_OFF;
 		if (pLed->bLedBlinkInProgress) {
-			_cancel_timer_ex(&(pLed->BlinkTimer));
+			del_timer_sync(&pLed->BlinkTimer);
 			pLed->bLedBlinkInProgress = false;
 		}
 		if (pLed->bLedScanBlinkInProgress) {
-			_cancel_timer_ex(&(pLed->BlinkTimer));
+			del_timer_sync(&pLed->BlinkTimer);
 			pLed->bLedScanBlinkInProgress = false;
 		}
 		if (pLed->bLedWPSBlinkInProgress) {
-			_cancel_timer_ex(&(pLed->BlinkTimer));
+			del_timer_sync(&pLed->BlinkTimer);
 			pLed->bLedWPSBlinkInProgress = false;
 		}
-		_set_timer(&(pLed->BlinkTimer), 0);
+		mod_timer(&pLed->BlinkTimer,
+			  jiffies + msecs_to_jiffies(0));
 		break;
 	default:
 		break;
@@ -1237,7 +1248,7 @@ static void SwLedControlMode3(struct _adapter *padapter,
 			if (IS_LED_WPS_BLINKING(pLed))
 				return;
 			if (pLed->bLedBlinkInProgress == true) {
-				_cancel_timer_ex(&(pLed->BlinkTimer));
+				del_timer_sync(&pLed->BlinkTimer);
 				pLed->bLedBlinkInProgress = false;
 			}
 			pLed->bLedScanBlinkInProgress = true;
@@ -1247,8 +1258,8 @@ static void SwLedControlMode3(struct _adapter *padapter,
 				pLed->BlinkingLedState = LED_OFF;
 			else
 				pLed->BlinkingLedState = LED_ON;
-			_set_timer(&(pLed->BlinkTimer),
-				   LED_BLINK_SCAN_INTERVAL_ALPHA);
+			mod_timer(&pLed->BlinkTimer, jiffies +
+				  msecs_to_jiffies(LED_BLINK_SCAN_INTERVAL_ALPHA));
 		}
 		break;
 	case LED_CTL_TX:
@@ -1265,8 +1276,8 @@ static void SwLedControlMode3(struct _adapter *padapter,
 				pLed->BlinkingLedState = LED_OFF;
 			else
 				pLed->BlinkingLedState = LED_ON;
-			_set_timer(&(pLed->BlinkTimer),
-				   LED_BLINK_FASTER_INTERVAL_ALPHA);
+			mod_timer(&pLed->BlinkTimer, jiffies +
+				  msecs_to_jiffies(LED_BLINK_FASTER_INTERVAL_ALPHA));
 		}
 		break;
 	case LED_CTL_LINK:
@@ -1275,24 +1286,25 @@ static void SwLedControlMode3(struct _adapter *padapter,
 		pLed->CurrLedState = LED_ON;
 		pLed->BlinkingLedState = LED_ON;
 		if (pLed->bLedBlinkInProgress) {
-			_cancel_timer_ex(&(pLed->BlinkTimer));
+			del_timer_sync(&pLed->BlinkTimer);
 			pLed->bLedBlinkInProgress = false;
 		}
 		if (pLed->bLedScanBlinkInProgress) {
-			_cancel_timer_ex(&(pLed->BlinkTimer));
+			del_timer_sync(&pLed->BlinkTimer);
 			pLed->bLedScanBlinkInProgress = false;
 		}
-		_set_timer(&(pLed->BlinkTimer), 0);
+		mod_timer(&pLed->BlinkTimer,
+			  jiffies + msecs_to_jiffies(0));
 		break;
 	case LED_CTL_START_WPS: /* wait until xinpin finish */
 	case LED_CTL_START_WPS_BOTTON:
 		if (pLed->bLedWPSBlinkInProgress == false) {
 			if (pLed->bLedBlinkInProgress == true) {
-				_cancel_timer_ex(&(pLed->BlinkTimer));
+				del_timer_sync(&pLed->BlinkTimer);
 				pLed->bLedBlinkInProgress = false;
 			}
 			if (pLed->bLedScanBlinkInProgress == true) {
-				_cancel_timer_ex(&(pLed->BlinkTimer));
+				del_timer_sync(&pLed->BlinkTimer);
 				pLed->bLedScanBlinkInProgress = false;
 			}
 			pLed->bLedWPSBlinkInProgress = true;
@@ -1301,59 +1313,63 @@ static void SwLedControlMode3(struct _adapter *padapter,
 				pLed->BlinkingLedState = LED_OFF;
 			else
 				pLed->BlinkingLedState = LED_ON;
-			_set_timer(&(pLed->BlinkTimer),
-				   LED_BLINK_SCAN_INTERVAL_ALPHA);
+			mod_timer(&pLed->BlinkTimer, jiffies +
+				  msecs_to_jiffies(LED_BLINK_SCAN_INTERVAL_ALPHA));
 		}
 		break;
 	case LED_CTL_STOP_WPS:
 		if (pLed->bLedWPSBlinkInProgress) {
-			_cancel_timer_ex(&(pLed->BlinkTimer));
+			del_timer_sync(&(pLed->BlinkTimer));
 			pLed->bLedWPSBlinkInProgress = false;
 		} else
 			pLed->bLedWPSBlinkInProgress = true;
 		pLed->CurrLedState = LED_BLINK_WPS_STOP;
 		if (pLed->bLedOn) {
 			pLed->BlinkingLedState = LED_OFF;
-			_set_timer(&(pLed->BlinkTimer),
-				   LED_BLINK_WPS_SUCESS_INTERVAL_ALPHA);
+			mod_timer(&pLed->BlinkTimer, jiffies +
+				  msecs_to_jiffies(LED_BLINK_WPS_SUCESS_INTERVAL_ALPHA));
 		} else {
 			pLed->BlinkingLedState = LED_ON;
-			_set_timer(&(pLed->BlinkTimer), 0);
+			mod_timer(&pLed->BlinkTimer,
+				  jiffies + msecs_to_jiffies(0));
 		}
 		break;
 	case LED_CTL_STOP_WPS_FAIL:
 		if (pLed->bLedWPSBlinkInProgress) {
-			_cancel_timer_ex(&(pLed->BlinkTimer));
+			del_timer_sync(&pLed->BlinkTimer);
 			pLed->bLedWPSBlinkInProgress = false;
 		}
 		pLed->CurrLedState = LED_OFF;
 		pLed->BlinkingLedState = LED_OFF;
-		_set_timer(&(pLed->BlinkTimer), 0);
+		mod_timer(&pLed->BlinkTimer,
+			  jiffies + msecs_to_jiffies(0));
 		break;
 	case LED_CTL_START_TO_LINK:
 	case LED_CTL_NO_LINK:
 		if (!IS_LED_BLINKING(pLed)) {
 			pLed->CurrLedState = LED_OFF;
 			pLed->BlinkingLedState = LED_OFF;
-			_set_timer(&(pLed->BlinkTimer), 0);
+			mod_timer(&pLed->BlinkTimer,
+				  jiffies + msecs_to_jiffies(0));
 		}
 		break;
 	case LED_CTL_POWER_OFF:
 		pLed->CurrLedState = LED_OFF;
 		pLed->BlinkingLedState = LED_OFF;
 		if (pLed->bLedBlinkInProgress) {
-			_cancel_timer_ex(&(pLed->BlinkTimer));
+			del_timer_sync(&pLed->BlinkTimer);
 			pLed->bLedBlinkInProgress = false;
 		}
 		if (pLed->bLedScanBlinkInProgress) {
-			_cancel_timer_ex(&(pLed->BlinkTimer));
+			del_timer_sync(&pLed->BlinkTimer);
 			pLed->bLedScanBlinkInProgress = false;
 		}
 		if (pLed->bLedWPSBlinkInProgress) {
-			_cancel_timer_ex(&(pLed->BlinkTimer));
+			del_timer_sync(&pLed->BlinkTimer);
 			pLed->bLedWPSBlinkInProgress = false;
 		}
-		_set_timer(&(pLed->BlinkTimer), 0);
+		mod_timer(&pLed->BlinkTimer,
+			  jiffies + msecs_to_jiffies(0));
 		break;
 	default:
 		break;
@@ -1372,34 +1388,35 @@ static void SwLedControlMode4(struct _adapter *padapter,
 	case LED_CTL_START_TO_LINK:
 		if (pLed1->bLedWPSBlinkInProgress) {
 			pLed1->bLedWPSBlinkInProgress = false;
-			_cancel_timer_ex(&(pLed1->BlinkTimer));
+			del_timer_sync(&pLed1->BlinkTimer);
 			pLed1->BlinkingLedState = LED_OFF;
 			pLed1->CurrLedState = LED_OFF;
 			if (pLed1->bLedOn)
-				_set_timer(&(pLed->BlinkTimer), 0);
+				mod_timer(&pLed->BlinkTimer,
+					  jiffies + msecs_to_jiffies(0));
 		}
 		if (pLed->bLedStartToLinkBlinkInProgress == false) {
 			if (pLed->CurrLedState == LED_SCAN_BLINK ||
 			    IS_LED_WPS_BLINKING(pLed))
 				return;
 			if (pLed->bLedBlinkInProgress == true) {
-				_cancel_timer_ex(&(pLed->BlinkTimer));
+				del_timer_sync(&pLed->BlinkTimer);
 				pLed->bLedBlinkInProgress = false;
 			}
 			if (pLed->bLedNoLinkBlinkInProgress == true) {
-				_cancel_timer_ex(&(pLed->BlinkTimer));
+				del_timer_sync(&pLed->BlinkTimer);
 				pLed->bLedNoLinkBlinkInProgress = false;
 			}
 			pLed->bLedStartToLinkBlinkInProgress = true;
 			pLed->CurrLedState = LED_BLINK_StartToBlink;
 			if (pLed->bLedOn) {
 				pLed->BlinkingLedState = LED_OFF;
-				_set_timer(&(pLed->BlinkTimer),
-					   LED_BLINK_SLOWLY_INTERVAL);
+				mod_timer(&pLed->BlinkTimer, jiffies +
+					  msecs_to_jiffies(LED_BLINK_SLOWLY_INTERVAL));
 			} else {
 				pLed->BlinkingLedState = LED_ON;
-				_set_timer(&(pLed->BlinkTimer),
-					   LED_BLINK_NORMAL_INTERVAL);
+				mod_timer(&pLed->BlinkTimer, jiffies +
+					  msecs_to_jiffies(LED_BLINK_NORMAL_INTERVAL));
 			}
 		}
 		break;
@@ -1409,11 +1426,12 @@ static void SwLedControlMode4(struct _adapter *padapter,
 		if (LedAction == LED_CTL_LINK) {
 			if (pLed1->bLedWPSBlinkInProgress) {
 				pLed1->bLedWPSBlinkInProgress = false;
-				_cancel_timer_ex(&(pLed1->BlinkTimer));
+				del_timer_sync(&pLed1->BlinkTimer);
 				pLed1->BlinkingLedState = LED_OFF;
 				pLed1->CurrLedState = LED_OFF;
 				if (pLed1->bLedOn)
-					_set_timer(&(pLed->BlinkTimer), 0);
+					mod_timer(&pLed->BlinkTimer,
+						  jiffies + msecs_to_jiffies(0));
 			}
 		}
 		if (pLed->bLedNoLinkBlinkInProgress == false) {
@@ -1421,7 +1439,7 @@ static void SwLedControlMode4(struct _adapter *padapter,
 			    IS_LED_WPS_BLINKING(pLed))
 				return;
 			if (pLed->bLedBlinkInProgress == true) {
-				_cancel_timer_ex(&(pLed->BlinkTimer));
+				del_timer_sync(&pLed->BlinkTimer);
 				pLed->bLedBlinkInProgress = false;
 			}
 			pLed->bLedNoLinkBlinkInProgress = true;
@@ -1430,8 +1448,8 @@ static void SwLedControlMode4(struct _adapter *padapter,
 				pLed->BlinkingLedState = LED_OFF;
 			else
 				pLed->BlinkingLedState = LED_ON;
-			_set_timer(&(pLed->BlinkTimer),
-				   LED_BLINK_NO_LINK_INTERVAL_ALPHA);
+			mod_timer(&pLed->BlinkTimer, jiffies +
+				  msecs_to_jiffies(LED_BLINK_NO_LINK_INTERVAL_ALPHA));
 		}
 		break;
 	case LED_CTL_SITE_SURVEY:
@@ -1442,11 +1460,11 @@ static void SwLedControlMode4(struct _adapter *padapter,
 			if (IS_LED_WPS_BLINKING(pLed))
 				return;
 			if (pLed->bLedNoLinkBlinkInProgress == true) {
-				_cancel_timer_ex(&(pLed->BlinkTimer));
+				del_timer_sync(&pLed->BlinkTimer);
 				pLed->bLedNoLinkBlinkInProgress = false;
 			}
 			if (pLed->bLedBlinkInProgress == true) {
-				_cancel_timer_ex(&(pLed->BlinkTimer));
+				del_timer_sync(&pLed->BlinkTimer);
 				pLed->bLedBlinkInProgress = false;
 			}
 			pLed->bLedScanBlinkInProgress = true;
@@ -1456,8 +1474,8 @@ static void SwLedControlMode4(struct _adapter *padapter,
 				pLed->BlinkingLedState = LED_OFF;
 			else
 				pLed->BlinkingLedState = LED_ON;
-			_set_timer(&(pLed->BlinkTimer),
-				   LED_BLINK_SCAN_INTERVAL_ALPHA);
+			mod_timer(&pLed->BlinkTimer, jiffies +
+				  msecs_to_jiffies(LED_BLINK_SCAN_INTERVAL_ALPHA));
 		}
 		break;
 	case LED_CTL_TX:
@@ -1467,7 +1485,7 @@ static void SwLedControlMode4(struct _adapter *padapter,
 			    IS_LED_WPS_BLINKING(pLed))
 				return;
 			if (pLed->bLedNoLinkBlinkInProgress == true) {
-				_cancel_timer_ex(&(pLed->BlinkTimer));
+				del_timer_sync(&pLed->BlinkTimer);
 				pLed->bLedNoLinkBlinkInProgress = false;
 			}
 			pLed->bLedBlinkInProgress = true;
@@ -1477,49 +1495,50 @@ static void SwLedControlMode4(struct _adapter *padapter,
 				pLed->BlinkingLedState = LED_OFF;
 			else
 				pLed->BlinkingLedState = LED_ON;
-			_set_timer(&(pLed->BlinkTimer),
-				   LED_BLINK_FASTER_INTERVAL_ALPHA);
+			mod_timer(&pLed->BlinkTimer, jiffies +
+				  msecs_to_jiffies(LED_BLINK_FASTER_INTERVAL_ALPHA));
 		}
 		break;
 	case LED_CTL_START_WPS: /*wait until xinpin finish*/
 	case LED_CTL_START_WPS_BOTTON:
 		if (pLed1->bLedWPSBlinkInProgress) {
 			pLed1->bLedWPSBlinkInProgress = false;
-			_cancel_timer_ex(&(pLed1->BlinkTimer));
+			del_timer_sync(&(pLed1->BlinkTimer));
 			pLed1->BlinkingLedState = LED_OFF;
 			pLed1->CurrLedState = LED_OFF;
 			if (pLed1->bLedOn)
-				_set_timer(&(pLed->BlinkTimer), 0);
+				mod_timer(&pLed->BlinkTimer,
+					  jiffies + msecs_to_jiffies(0));
 		}
 		if (pLed->bLedWPSBlinkInProgress == false) {
 			if (pLed->bLedNoLinkBlinkInProgress == true) {
-				_cancel_timer_ex(&(pLed->BlinkTimer));
+				del_timer_sync(&pLed->BlinkTimer);
 				pLed->bLedNoLinkBlinkInProgress = false;
 			}
 			if (pLed->bLedBlinkInProgress == true) {
-				_cancel_timer_ex(&(pLed->BlinkTimer));
+				del_timer_sync(&pLed->BlinkTimer);
 				pLed->bLedBlinkInProgress = false;
 			}
 			if (pLed->bLedScanBlinkInProgress == true) {
-				_cancel_timer_ex(&(pLed->BlinkTimer));
+				del_timer_sync(&pLed->BlinkTimer);
 				pLed->bLedScanBlinkInProgress = false;
 			}
 			pLed->bLedWPSBlinkInProgress = true;
 			pLed->CurrLedState = LED_BLINK_WPS;
 			if (pLed->bLedOn) {
 				pLed->BlinkingLedState = LED_OFF;
-				_set_timer(&(pLed->BlinkTimer),
-					   LED_BLINK_SLOWLY_INTERVAL);
+				mod_timer(&pLed->BlinkTimer, jiffies +
+					  msecs_to_jiffies(LED_BLINK_SLOWLY_INTERVAL));
 			} else {
 				pLed->BlinkingLedState = LED_ON;
-				_set_timer(&(pLed->BlinkTimer),
-					   LED_BLINK_NORMAL_INTERVAL);
+				mod_timer(&pLed->BlinkTimer, jiffies +
+					  msecs_to_jiffies(LED_BLINK_NORMAL_INTERVAL));
 			}
 		}
 		break;
 	case LED_CTL_STOP_WPS:	/*WPS connect success*/
 		if (pLed->bLedWPSBlinkInProgress) {
-			_cancel_timer_ex(&(pLed->BlinkTimer));
+			del_timer_sync(&pLed->BlinkTimer);
 			pLed->bLedWPSBlinkInProgress = false;
 		}
 		pLed->bLedNoLinkBlinkInProgress = true;
@@ -1528,12 +1547,12 @@ static void SwLedControlMode4(struct _adapter *padapter,
 			pLed->BlinkingLedState = LED_OFF;
 		else
 			pLed->BlinkingLedState = LED_ON;
-		_set_timer(&(pLed->BlinkTimer),
-			   LED_BLINK_NO_LINK_INTERVAL_ALPHA);
+		mod_timer(&pLed->BlinkTimer, jiffies +
+			  msecs_to_jiffies(LED_BLINK_NO_LINK_INTERVAL_ALPHA));
 		break;
 	case LED_CTL_STOP_WPS_FAIL:	/*WPS authentication fail*/
 		if (pLed->bLedWPSBlinkInProgress) {
-			_cancel_timer_ex(&(pLed->BlinkTimer));
+			del_timer_sync(&pLed->BlinkTimer);
 			pLed->bLedWPSBlinkInProgress = false;
 		}
 		pLed->bLedNoLinkBlinkInProgress = true;
@@ -1542,11 +1561,11 @@ static void SwLedControlMode4(struct _adapter *padapter,
 			pLed->BlinkingLedState = LED_OFF;
 		else
 			pLed->BlinkingLedState = LED_ON;
-		_set_timer(&(pLed->BlinkTimer),
-			   LED_BLINK_NO_LINK_INTERVAL_ALPHA);
+		mod_timer(&pLed->BlinkTimer, jiffies +
+			  msecs_to_jiffies(LED_BLINK_NO_LINK_INTERVAL_ALPHA));
 		/*LED1 settings*/
 		if (pLed1->bLedWPSBlinkInProgress)
-			_cancel_timer_ex(&(pLed1->BlinkTimer));
+			del_timer_sync(&pLed1->BlinkTimer);
 		else
 			pLed1->bLedWPSBlinkInProgress = true;
 		pLed1->CurrLedState = LED_BLINK_WPS_STOP;
@@ -1554,11 +1573,12 @@ static void SwLedControlMode4(struct _adapter *padapter,
 			pLed1->BlinkingLedState = LED_OFF;
 		else
 			pLed1->BlinkingLedState = LED_ON;
-		_set_timer(&(pLed->BlinkTimer), LED_BLINK_NORMAL_INTERVAL);
+		mod_timer(&pLed->BlinkTimer, jiffies +
+			  msecs_to_jiffies(LED_BLINK_NORMAL_INTERVAL));
 		break;
 	case LED_CTL_STOP_WPS_FAIL_OVERLAP:	/*WPS session overlap*/
 		if (pLed->bLedWPSBlinkInProgress) {
-			_cancel_timer_ex(&(pLed->BlinkTimer));
+			del_timer_sync(&pLed->BlinkTimer);
 			pLed->bLedWPSBlinkInProgress = false;
 		}
 		pLed->bLedNoLinkBlinkInProgress = true;
@@ -1567,11 +1587,11 @@ static void SwLedControlMode4(struct _adapter *padapter,
 			pLed->BlinkingLedState = LED_OFF;
 		else
 			pLed->BlinkingLedState = LED_ON;
-		_set_timer(&(pLed->BlinkTimer),
-			   LED_BLINK_NO_LINK_INTERVAL_ALPHA);
+		mod_timer(&pLed->BlinkTimer, jiffies +
+			  msecs_to_jiffies(LED_BLINK_NO_LINK_INTERVAL_ALPHA));
 		/*LED1 settings*/
 		if (pLed1->bLedWPSBlinkInProgress)
-			_cancel_timer_ex(&(pLed1->BlinkTimer));
+			del_timer_sync(&pLed1->BlinkTimer);
 		else
 			pLed1->bLedWPSBlinkInProgress = true;
 		pLed1->CurrLedState = LED_BLINK_WPS_STOP_OVERLAP;
@@ -1580,37 +1600,38 @@ static void SwLedControlMode4(struct _adapter *padapter,
 			pLed1->BlinkingLedState = LED_OFF;
 		else
 			pLed1->BlinkingLedState = LED_ON;
-		_set_timer(&(pLed->BlinkTimer), LED_BLINK_NORMAL_INTERVAL);
+		mod_timer(&pLed->BlinkTimer, jiffies +
+			  msecs_to_jiffies(LED_BLINK_NORMAL_INTERVAL));
 		break;
 	case LED_CTL_POWER_OFF:
 		pLed->CurrLedState = LED_OFF;
 		pLed->BlinkingLedState = LED_OFF;
 		if (pLed->bLedNoLinkBlinkInProgress) {
-			_cancel_timer_ex(&(pLed->BlinkTimer));
+			del_timer_sync(&pLed->BlinkTimer);
 			pLed->bLedNoLinkBlinkInProgress = false;
 		}
 		if (pLed->bLedLinkBlinkInProgress) {
-			_cancel_timer_ex(&(pLed->BlinkTimer));
+			del_timer_sync(&pLed->BlinkTimer);
 			pLed->bLedLinkBlinkInProgress = false;
 		}
 		if (pLed->bLedBlinkInProgress) {
-			_cancel_timer_ex(&(pLed->BlinkTimer));
+			del_timer_sync(&pLed->BlinkTimer);
 			pLed->bLedBlinkInProgress = false;
 		}
 		if (pLed->bLedWPSBlinkInProgress) {
-			_cancel_timer_ex(&(pLed->BlinkTimer));
+			del_timer_sync(&pLed->BlinkTimer);
 			pLed->bLedWPSBlinkInProgress = false;
 		}
 		if (pLed->bLedScanBlinkInProgress) {
-			_cancel_timer_ex(&(pLed->BlinkTimer));
+			del_timer_sync(&pLed->BlinkTimer);
 			pLed->bLedScanBlinkInProgress = false;
 		}
 		if (pLed->bLedStartToLinkBlinkInProgress) {
-			_cancel_timer_ex(&(pLed->BlinkTimer));
+			del_timer_sync(&pLed->BlinkTimer);
 			pLed->bLedStartToLinkBlinkInProgress = false;
 		}
 		if (pLed1->bLedWPSBlinkInProgress) {
-			_cancel_timer_ex(&(pLed1->BlinkTimer));
+			del_timer_sync(&pLed1->BlinkTimer);
 			pLed1->bLedWPSBlinkInProgress = false;
 		}
 		pLed1->BlinkingLedState = LED_UNKNOWN;
@@ -1641,7 +1662,8 @@ static void SwLedControlMode5(struct _adapter *padapter,
 		pLed->CurrLedState = LED_ON;
 		pLed->BlinkingLedState = LED_ON;
 		pLed->bLedBlinkInProgress = false;
-		_set_timer(&(pLed->BlinkTimer), 0);
+		mod_timer(&pLed->BlinkTimer,
+			  jiffies + msecs_to_jiffies(0));
 		break;
 	case LED_CTL_SITE_SURVEY:
 		if ((pmlmepriv->sitesurveyctrl.traffic_busy) &&
@@ -1649,7 +1671,7 @@ static void SwLedControlMode5(struct _adapter *padapter,
 			; /* dummy branch */
 		else if (pLed->bLedScanBlinkInProgress == false) {
 			if (pLed->bLedBlinkInProgress == true) {
-				_cancel_timer_ex(&(pLed->BlinkTimer));
+				del_timer_sync(&pLed->BlinkTimer);
 				pLed->bLedBlinkInProgress = false;
 			}
 			pLed->bLedScanBlinkInProgress = true;
@@ -1659,8 +1681,8 @@ static void SwLedControlMode5(struct _adapter *padapter,
 				pLed->BlinkingLedState = LED_OFF;
 			else
 				pLed->BlinkingLedState = LED_ON;
-			_set_timer(&(pLed->BlinkTimer),
-				   LED_BLINK_SCAN_INTERVAL_ALPHA);
+			mod_timer(&pLed->BlinkTimer, jiffies +
+				  msecs_to_jiffies(LED_BLINK_SCAN_INTERVAL_ALPHA));
 		}
 		break;
 	case LED_CTL_TX:
@@ -1675,15 +1697,15 @@ static void SwLedControlMode5(struct _adapter *padapter,
 				pLed->BlinkingLedState = LED_OFF;
 			else
 				pLed->BlinkingLedState = LED_ON;
-			_set_timer(&(pLed->BlinkTimer),
-				   LED_BLINK_FASTER_INTERVAL_ALPHA);
+			mod_timer(&pLed->BlinkTimer, jiffies +
+				  msecs_to_jiffies(LED_BLINK_FASTER_INTERVAL_ALPHA));
 		}
 		break;
 	case LED_CTL_POWER_OFF:
 		pLed->CurrLedState = LED_OFF;
 		pLed->BlinkingLedState = LED_OFF;
 		if (pLed->bLedBlinkInProgress) {
-			_cancel_timer_ex(&(pLed->BlinkTimer));
+			del_timer_sync(&pLed->BlinkTimer);
 			pLed->bLedBlinkInProgress = false;
 		}
 		SwLedOff(padapter, pLed);
@@ -1711,7 +1733,7 @@ static void SwLedControlMode6(struct _adapter *padapter,
 		pLed->CurrLedState = LED_ON;
 		pLed->BlinkingLedState = LED_ON;
 		pLed->bLedBlinkInProgress = false;
-		_set_timer(&(pLed->BlinkTimer), 0);
+		mod_timer(&(pLed->BlinkTimer), jiffies + msecs_to_jiffies(0));
 		break;
 	case LED_CTL_TX:
 	case LED_CTL_RX:
@@ -1726,15 +1748,15 @@ static void SwLedControlMode6(struct _adapter *padapter,
 				pLed->BlinkingLedState = LED_OFF;
 			else
 				pLed->BlinkingLedState = LED_ON;
-			_set_timer(&(pLed->BlinkTimer),
-				   LED_BLINK_FASTER_INTERVAL_ALPHA);
+			mod_timer(&pLed->BlinkTimer, jiffies +
+				  msecs_to_jiffies(LED_BLINK_FASTER_INTERVAL_ALPHA));
 		}
 		break;
 	case LED_CTL_START_WPS: /*wait until xinpin finish*/
 	case LED_CTL_START_WPS_BOTTON:
 		if (pLed->bLedWPSBlinkInProgress == false) {
 			if (pLed->bLedBlinkInProgress == true) {
-				_cancel_timer_ex(&(pLed->BlinkTimer));
+				del_timer_sync(&pLed->BlinkTimer);
 				pLed->bLedBlinkInProgress = false;
 			}
 			pLed->bLedWPSBlinkInProgress = true;
@@ -1743,29 +1765,30 @@ static void SwLedControlMode6(struct _adapter *padapter,
 				pLed->BlinkingLedState = LED_OFF;
 			else
 				pLed->BlinkingLedState = LED_ON;
-			_set_timer(&(pLed->BlinkTimer),
-				   LED_BLINK_SCAN_INTERVAL_ALPHA);
+			mod_timer(&pLed->BlinkTimer, jiffies +
+				  msecs_to_jiffies(LED_BLINK_SCAN_INTERVAL_ALPHA));
 		}
 		break;
 	case LED_CTL_STOP_WPS_FAIL:
 	case LED_CTL_STOP_WPS:
 		if (pLed->bLedWPSBlinkInProgress) {
-			_cancel_timer_ex(&(pLed->BlinkTimer));
+			del_timer_sync(&pLed->BlinkTimer);
 			pLed->bLedWPSBlinkInProgress = false;
 		}
 		pLed->CurrLedState = LED_ON;
 		pLed->BlinkingLedState = LED_ON;
-		_set_timer(&(pLed->BlinkTimer), 0);
+		mod_timer(&pLed->BlinkTimer,
+			  jiffies + msecs_to_jiffies(0));
 		break;
 	case LED_CTL_POWER_OFF:
 		pLed->CurrLedState = LED_OFF;
 		pLed->BlinkingLedState = LED_OFF;
 		if (pLed->bLedBlinkInProgress) {
-			_cancel_timer_ex(&(pLed->BlinkTimer));
+			del_timer_sync(&pLed->BlinkTimer);
 			pLed->bLedBlinkInProgress = false;
 		}
 		if (pLed->bLedWPSBlinkInProgress) {
-			_cancel_timer_ex(&(pLed->BlinkTimer));
+			del_timer_sync(&pLed->BlinkTimer);
 			pLed->bLedWPSBlinkInProgress = false;
 		}
 		SwLedOff(padapter, pLed);
diff --git a/drivers/staging/rtl8712/rtl8712_recv.c b/drivers/staging/rtl8712/rtl8712_recv.c
index cd8b444b255b..50227b598e0c 100644
--- a/drivers/staging/rtl8712/rtl8712_recv.c
+++ b/drivers/staging/rtl8712/rtl8712_recv.c
@@ -161,12 +161,12 @@ static void update_recvframe_attrib_from_recvstat(struct rx_pkt_attrib *pattrib,
 	u16 drvinfo_sz = 0;
 
 	drvinfo_sz = (le32_to_cpu(prxstat->rxdw0)&0x000f0000)>>16;
-	drvinfo_sz = drvinfo_sz<<3;
+	drvinfo_sz <<= 3;
 	/*TODO:
 	 * Offset 0 */
 	pattrib->bdecrypted = ((le32_to_cpu(prxstat->rxdw0) & BIT(27)) >> 27)
 				 ? 0 : 1;
-	pattrib->crc_err = ((le32_to_cpu(prxstat->rxdw0) & BIT(14)) >> 14);
+	pattrib->crc_err = (le32_to_cpu(prxstat->rxdw0) & BIT(14)) >> 14;
 	/*Offset 4*/
 	/*Offset 8*/
 	/*Offset 12*/
@@ -435,8 +435,8 @@ void r8712_rxcmd_event_hdl(struct _adapter *padapter, void *prxcmdbuf)
 	poffset = (u8 *)prxcmdbuf;
 	voffset = *(uint *)poffset;
 	prxstat = (struct recv_stat *)prxcmdbuf;
-	drvinfo_sz = ((le32_to_cpu(prxstat->rxdw0) & 0x000f0000) >> 16);
-	drvinfo_sz = drvinfo_sz << 3;
+	drvinfo_sz = (le32_to_cpu(prxstat->rxdw0) & 0x000f0000) >> 16;
+	drvinfo_sz <<= 3;
 	poffset += RXDESC_SIZE + drvinfo_sz;
 	do {
 		voffset  = *(uint *)poffset;
@@ -604,12 +604,12 @@ static int recv_indicatepkt_reorder(struct _adapter *padapter,
 	 */
 	if (r8712_recv_indicatepkts_in_order(padapter, preorder_ctrl, false) ==
 	    true) {
-		_set_timer(&preorder_ctrl->reordering_ctrl_timer,
-			   REORDER_WAIT_TIME);
+		mod_timer(&preorder_ctrl->reordering_ctrl_timer,
+			  jiffies + msecs_to_jiffies(REORDER_WAIT_TIME));
 		spin_unlock_irqrestore(&ppending_recvframe_queue->lock, irql);
 	} else {
 		spin_unlock_irqrestore(&ppending_recvframe_queue->lock, irql);
-		_cancel_timer_ex(&preorder_ctrl->reordering_ctrl_timer);
+		del_timer(&preorder_ctrl->reordering_ctrl_timer);
 	}
 	return _SUCCESS;
 _err_exit:
@@ -749,7 +749,7 @@ static void query_rx_phy_status(struct _adapter *padapter,
 		 */
 		if (!cck_highpwr) {
 			report = pcck_buf->cck_agc_rpt & 0xc0;
-			report = report >> 6;
+			report >>= 6;
 			switch (report) {
 			/* Modify the RF RNA gain value to -40, -20,
 			 * -2, 14 by Jenyu's suggestion
@@ -775,7 +775,7 @@ static void query_rx_phy_status(struct _adapter *padapter,
 		} else {
 			report = ((u8)(le32_to_cpu(pphy_stat->phydw1) >> 8)) &
 				 0x60;
-			report = report >> 5;
+			report >>= 5;
 			switch (report) {
 			case 0x3:
 				rx_pwr_all = -40 - ((pcck_buf->cck_agc_rpt &
@@ -1039,7 +1039,7 @@ static int recvbuf2recvframe(struct _adapter *padapter, struct sk_buff *pskb)
 		frag = (le32_to_cpu(prxstat->rxdw2) >> 12) & 0xf;
 		/* uint 2^3 = 8 bytes */
 		drvinfo_sz = (le32_to_cpu(prxstat->rxdw0) & 0x000f0000) >> 16;
-		drvinfo_sz = drvinfo_sz<<3;
+		drvinfo_sz <<= 3;
 		if (pkt_len <= 0)
 			goto  _exit_recvbuf2recvframe;
 		/* Qos data, wireless lan header length is 26 */
diff --git a/drivers/staging/rtl8712/rtl871x_cmd.c b/drivers/staging/rtl8712/rtl871x_cmd.c
index fe5e315319f7..1a1c38f885d6 100644
--- a/drivers/staging/rtl8712/rtl871x_cmd.c
+++ b/drivers/staging/rtl8712/rtl871x_cmd.c
@@ -247,7 +247,8 @@ u8 r8712_sitesurvey_cmd(struct _adapter *padapter,
 	}
 	set_fwstate(pmlmepriv, _FW_UNDER_SURVEY);
 	r8712_enqueue_cmd(pcmdpriv, ph2c);
-	_set_timer(&pmlmepriv->scan_to_timer, SCANNING_TIMEOUT);
+	mod_timer(&pmlmepriv->scan_to_timer,
+		  jiffies + msecs_to_jiffies(SCANNING_TIMEOUT));
 	padapter->ledpriv.LedControlHandler(padapter, LED_CTL_SITE_SURVEY);
 	padapter->blnEnableRxFF0Filter = 0;
 	return _SUCCESS;
@@ -530,8 +531,8 @@ u8 r8712_joinbss_cmd(struct _adapter  *padapter, struct wlan_network *pnetwork)
 	 * the driver just has the bssid information for PMKIDList searching.
 	 */
 	if (pmlmepriv->assoc_by_bssid == false)
-		memcpy(&pmlmepriv->assoc_bssid[0],
-			&pnetwork->network.MacAddress[0], ETH_ALEN);
+		ether_addr_copy(&pmlmepriv->assoc_bssid[0],
+				&pnetwork->network.MacAddress[0]);
 	psecnetwork->IELength = r8712_restruct_sec_ie(padapter,
 						&pnetwork->network.IEs[0],
 						&psecnetwork->IEs[0],
@@ -682,7 +683,7 @@ u8 r8712_setstakey_cmd(struct _adapter *padapter, u8 *psta, u8 unicast_key)
 	init_h2fwcmd_w_parm_no_rsp(ph2c, psetstakey_para, _SetStaKey_CMD_);
 	ph2c->rsp = (u8 *) psetstakey_rsp;
 	ph2c->rspsz = sizeof(struct set_stakey_rsp);
-	memcpy(psetstakey_para->addr, sta->hwaddr, ETH_ALEN);
+	ether_addr_copy(psetstakey_para->addr, sta->hwaddr);
 	if (check_fwstate(pmlmepriv, WIFI_STATION_STATE))
 		psetstakey_para->algorithm = (unsigned char)
 					    psecuritypriv->PrivacyAlgrthm;
@@ -784,7 +785,7 @@ u8 r8712_setMacAddr_cmd(struct _adapter *padapter, u8 *mac_addr)
 	}
 	init_h2fwcmd_w_parm_no_rsp(ph2c, psetMacAddr_para,
 				   _SetMacAddress_CMD_);
-	memcpy(psetMacAddr_para->MacAddr, mac_addr, ETH_ALEN);
+	ether_addr_copy(psetMacAddr_para->MacAddr, mac_addr);
 	r8712_enqueue_cmd(pcmdpriv, ph2c);
 	return _SUCCESS;
 }
@@ -813,7 +814,7 @@ u8 r8712_setassocsta_cmd(struct _adapter *padapter, u8 *mac_addr)
 	init_h2fwcmd_w_parm_no_rsp(ph2c, psetassocsta_para, _SetAssocSta_CMD_);
 	ph2c->rsp = (u8 *) psetassocsta_rsp;
 	ph2c->rspsz = sizeof(struct set_assocsta_rsp);
-	memcpy(psetassocsta_para->addr, mac_addr, ETH_ALEN);
+	ether_addr_copy(psetassocsta_para->addr, mac_addr);
 	r8712_enqueue_cmd(pcmdpriv, ph2c);
 	return _SUCCESS;
 }
@@ -890,7 +891,8 @@ void r8712_joinbss_cmd_callback(struct _adapter *padapter, struct cmd_obj *pcmd)
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 
 	if (pcmd->res != H2C_SUCCESS)
-		_set_timer(&pmlmepriv->assoc_timer, 1);
+		mod_timer(&pmlmepriv->assoc_timer,
+			  jiffies + msecs_to_jiffies(1));
 	r8712_free_cmd_obj(pcmd);
 }
 
@@ -898,7 +900,6 @@ void r8712_createbss_cmd_callback(struct _adapter *padapter,
 				  struct cmd_obj *pcmd)
 {
 	unsigned long irqL;
-	u8 timer_cancelled;
 	struct sta_info *psta = NULL;
 	struct wlan_network *pwlan = NULL;
 	struct	mlme_priv *pmlmepriv = &padapter->mlmepriv;
@@ -907,8 +908,9 @@ void r8712_createbss_cmd_callback(struct _adapter *padapter,
 	struct wlan_network *tgt_network = &(pmlmepriv->cur_network);
 
 	if (pcmd->res != H2C_SUCCESS)
-		_set_timer(&pmlmepriv->assoc_timer, 1);
-	_cancel_timer(&pmlmepriv->assoc_timer, &timer_cancelled);
+		mod_timer(&pmlmepriv->assoc_timer,
+			  jiffies + msecs_to_jiffies(1));
+	del_timer_sync(&pmlmepriv->assoc_timer);
 #ifdef __BIG_ENDIAN
 	/* endian_convert */
 	pnetwork->Length = le32_to_cpu(pnetwork->Length);
diff --git a/drivers/staging/rtl8712/rtl871x_eeprom.c b/drivers/staging/rtl8712/rtl871x_eeprom.c
index 2f145d63fcec..50339e67da07 100644
--- a/drivers/staging/rtl8712/rtl871x_eeprom.c
+++ b/drivers/staging/rtl8712/rtl871x_eeprom.c
@@ -64,7 +64,7 @@ static void shift_out_bits(struct _adapter *padapter, u16 data, u16 count)
 		udelay(CLOCK_RATE);
 		up_clk(padapter, &x);
 		down_clk(padapter, &x);
-		mask = mask >> 1;
+		mask >>= 1;
 	} while (mask);
 	if (padapter->bSurpriseRemoved == true)
 		goto out;
@@ -83,7 +83,7 @@ static u16 shift_in_bits(struct _adapter *padapter)
 	x &= ~(_EEDO | _EEDI);
 	d = 0;
 	for (i = 0; i < 16; i++) {
-		d = d << 1;
+		d <<= 1;
 		up_clk(padapter, &x);
 		if (padapter->bSurpriseRemoved == true)
 			goto out;
diff --git a/drivers/staging/rtl8712/rtl871x_ioctl_linux.c b/drivers/staging/rtl8712/rtl871x_ioctl_linux.c
index 9bb364f04fd4..42fba3f5b593 100644
--- a/drivers/staging/rtl8712/rtl871x_ioctl_linux.c
+++ b/drivers/staging/rtl8712/rtl871x_ioctl_linux.c
@@ -46,6 +46,8 @@
 #include <linux/semaphore.h>
 #include <net/iw_handler.h>
 #include <linux/if_arp.h>
+#include <linux/etherdevice.h>
+
 
 #define RTL_IOCTL_WPA_SUPPLICANT	(SIOCIWFIRSTPRIV + 0x1E)
 
@@ -112,7 +114,7 @@ void r8712_indicate_wx_disassoc_event(struct _adapter *padapter)
 	union iwreq_data wrqu;
 
 	wrqu.ap_addr.sa_family = ARPHRD_ETHER;
-	memset(wrqu.ap_addr.sa_data, 0, ETH_ALEN);
+	eth_zero_addr(wrqu.ap_addr.sa_data);
 	wireless_send_event(padapter->pnetdev, SIOCGIWAP, &wrqu, NULL);
 }
 
@@ -129,7 +131,8 @@ static inline void handle_pairwise_key(struct sta_info *psta,
 		memcpy(psta->tkiprxmickey. skey, &(param->u.crypt.
 			key[24]), 8);
 		padapter->securitypriv. busetkipkey = false;
-		_set_timer(&padapter->securitypriv.tkip_timer, 50);
+		mod_timer(&padapter->securitypriv.tkip_timer,
+			  jiffies + msecs_to_jiffies(50));
 	}
 	r8712_setstakey_cmd(padapter, (unsigned char *)psta, true);
 }
@@ -153,8 +156,8 @@ static inline void handle_group_key(struct ieee_param *param,
 		if (padapter->registrypriv.power_mgnt > PS_MODE_ACTIVE) {
 			if (padapter->registrypriv.power_mgnt != padapter->
 			    pwrctrlpriv.pwr_mode)
-				_set_timer(&(padapter->mlmepriv.dhcp_timer),
-					   60000);
+				mod_timer(&padapter->mlmepriv.dhcp_timer,
+					  jiffies + msecs_to_jiffies(60000));
 		}
 	}
 }
@@ -182,7 +185,7 @@ static inline char *translate_scan(struct _adapter *padapter,
 	/* AP MAC address */
 	iwe.cmd = SIOCGIWAP;
 	iwe.u.ap_addr.sa_family = ARPHRD_ETHER;
-	memcpy(iwe.u.ap_addr.sa_data, pnetwork->network.MacAddress, ETH_ALEN);
+	ether_addr_copy(iwe.u.ap_addr.sa_data, pnetwork->network.MacAddress);
 	start = iwe_stream_add_event(info, start, stop, &iwe, IW_EV_ADDR_LEN);
 	/* Add the ESSID */
 	iwe.cmd = SIOCGIWESSID;
@@ -360,7 +363,7 @@ static inline char *translate_scan(struct _adapter *padapter,
 
 static int wpa_set_auth_algs(struct net_device *dev, u32 value)
 {
-	struct _adapter *padapter = (struct _adapter *) netdev_priv(dev);
+	struct _adapter *padapter = netdev_priv(dev);
 	int ret = 0;
 
 	if ((value & AUTH_ALG_SHARED_KEY) && (value & AUTH_ALG_OPEN_SYSTEM)) {
@@ -392,7 +395,7 @@ static int wpa_set_encryption(struct net_device *dev, struct ieee_param *param,
 	int ret = 0;
 	u32 wep_key_idx, wep_key_len = 0;
 	struct NDIS_802_11_WEP	 *pwep = NULL;
-	struct _adapter *padapter = (struct _adapter *)netdev_priv(dev);
+	struct _adapter *padapter = netdev_priv(dev);
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 	struct security_priv *psecuritypriv = &padapter->securitypriv;
 
@@ -626,7 +629,7 @@ static int r8711_wx_get_name(struct net_device *dev,
 			     struct iw_request_info *info,
 			     union iwreq_data *wrqu, char *extra)
 {
-	struct _adapter *padapter = (struct _adapter *)netdev_priv(dev);
+	struct _adapter *padapter = netdev_priv(dev);
 	u32 ht_ielen = 0;
 	char *p;
 	u8 ht_cap = false;
@@ -682,7 +685,7 @@ static int r8711_wx_set_freq(struct net_device *dev,
 			     struct iw_request_info *info,
 			     union iwreq_data *wrqu, char *extra)
 {
-	struct _adapter *padapter = (struct _adapter *)netdev_priv(dev);
+	struct _adapter *padapter = netdev_priv(dev);
 	struct iw_freq *fwrq = &wrqu->freq;
 	int rc = 0;
 
@@ -718,7 +721,7 @@ static int r8711_wx_get_freq(struct net_device *dev,
 			     struct iw_request_info *info,
 			     union iwreq_data *wrqu, char *extra)
 {
-	struct _adapter *padapter = (struct _adapter *)netdev_priv(dev);
+	struct _adapter *padapter = netdev_priv(dev);
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 	struct ndis_wlan_bssid_ex *pcur_bss = &pmlmepriv->cur_network.network;
 
@@ -737,7 +740,7 @@ static int r8711_wx_set_mode(struct net_device *dev,
 			     struct iw_request_info *a,
 			     union iwreq_data *wrqu, char *b)
 {
-	struct _adapter *padapter = (struct _adapter *)netdev_priv(dev);
+	struct _adapter *padapter = netdev_priv(dev);
 	enum NDIS_802_11_NETWORK_INFRASTRUCTURE networkType;
 
 	switch (wrqu->mode) {
@@ -768,7 +771,7 @@ static int r8711_wx_set_mode(struct net_device *dev,
 static int r8711_wx_get_mode(struct net_device *dev, struct iw_request_info *a,
 			     union iwreq_data *wrqu, char *b)
 {
-	struct _adapter *padapter = (struct _adapter *)netdev_priv(dev);
+	struct _adapter *padapter = netdev_priv(dev);
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 
 	if (check_fwstate(pmlmepriv, WIFI_STATION_STATE) == true)
@@ -787,7 +790,7 @@ static int r871x_wx_set_pmkid(struct net_device *dev,
 			     struct iw_request_info *a,
 			     union iwreq_data *wrqu, char *extra)
 {
-	struct _adapter *padapter = (struct _adapter *)netdev_priv(dev);
+	struct _adapter *padapter = netdev_priv(dev);
 	struct security_priv *psecuritypriv = &padapter->securitypriv;
 	struct iw_pmksa *pPMK = (struct iw_pmksa *) extra;
 	u8 strZeroMacAddress[ETH_ALEN] = {0x00};
@@ -851,8 +854,7 @@ static int r871x_wx_set_pmkid(struct net_device *dev,
 			    strIssueBssid, ETH_ALEN)) {
 				/* BSSID is matched, the same AP => Remove
 				 * this PMKID information and reset it. */
-				memset(psecuritypriv->PMKIDList[j].Bssid,
-					0x00, ETH_ALEN);
+				eth_zero_addr(psecuritypriv->PMKIDList[j].Bssid);
 				psecuritypriv->PMKIDList[j].bUsed = false;
 				break;
 			}
@@ -1059,7 +1061,7 @@ static int r8711_wx_set_wap(struct net_device *dev,
 			 char *extra)
 {
 	int ret = -EINPROGRESS;
-	struct _adapter *padapter = (struct _adapter *) netdev_priv(dev);
+	struct _adapter *padapter = netdev_priv(dev);
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 	struct  __queue *queue = &pmlmepriv->scanned_queue;
 	struct sockaddr *temp = (struct sockaddr *)awrq;
@@ -1108,16 +1110,16 @@ static int r8711_wx_get_wap(struct net_device *dev,
 				struct iw_request_info *info,
 				union iwreq_data *wrqu, char *extra)
 {
-	struct _adapter *padapter = (struct _adapter *)netdev_priv(dev);
+	struct _adapter *padapter = netdev_priv(dev);
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 	struct ndis_wlan_bssid_ex *pcur_bss = &pmlmepriv->cur_network.network;
 
 	wrqu->ap_addr.sa_family = ARPHRD_ETHER;
 	if (check_fwstate(pmlmepriv, _FW_LINKED | WIFI_ADHOC_MASTER_STATE |
 				     WIFI_AP_STATE))
-		memcpy(wrqu->ap_addr.sa_data, pcur_bss->MacAddress, ETH_ALEN);
+		ether_addr_copy(wrqu->ap_addr.sa_data, pcur_bss->MacAddress);
 	else
-		memset(wrqu->ap_addr.sa_data, 0, ETH_ALEN);
+		eth_zero_addr(wrqu->ap_addr.sa_data);
 	return 0;
 }
 
@@ -1126,7 +1128,7 @@ static int r871x_wx_set_mlme(struct net_device *dev,
 			     union iwreq_data *wrqu, char *extra)
 {
 	int ret = 0;
-	struct _adapter *padapter = (struct _adapter *)netdev_priv(dev);
+	struct _adapter *padapter = netdev_priv(dev);
 	struct iw_mlme *mlme = (struct iw_mlme *) extra;
 
 	if (mlme == NULL)
@@ -1158,7 +1160,7 @@ static int r8711_wx_set_scan(struct net_device *dev,
 			struct iw_request_info *a,
 			union iwreq_data *wrqu, char *extra)
 {
-	struct _adapter *padapter = (struct _adapter *)netdev_priv(dev);
+	struct _adapter *padapter = netdev_priv(dev);
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 	u8 status = true;
 
@@ -1207,7 +1209,7 @@ static int r8711_wx_get_scan(struct net_device *dev,
 				struct iw_request_info *a,
 				union iwreq_data *wrqu, char *extra)
 {
-	struct _adapter *padapter = (struct _adapter *)netdev_priv(dev);
+	struct _adapter *padapter = netdev_priv(dev);
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 	struct  __queue *queue = &pmlmepriv->scanned_queue;
 	struct wlan_network *pnetwork = NULL;
@@ -1261,7 +1263,7 @@ static int r8711_wx_set_essid(struct net_device *dev,
 				struct iw_request_info *a,
 				union iwreq_data *wrqu, char *extra)
 {
-	struct _adapter *padapter = (struct _adapter *)netdev_priv(dev);
+	struct _adapter *padapter = netdev_priv(dev);
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 	struct  __queue *queue = &pmlmepriv->scanned_queue;
 	struct wlan_network *pnetwork = NULL;
@@ -1324,7 +1326,7 @@ static int r8711_wx_get_essid(struct net_device *dev,
 				struct iw_request_info *a,
 				union iwreq_data *wrqu, char *extra)
 {
-	struct _adapter *padapter = (struct _adapter *)netdev_priv(dev);
+	struct _adapter *padapter = netdev_priv(dev);
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 	struct ndis_wlan_bssid_ex *pcur_bss = &pmlmepriv->cur_network.network;
 	u32 len, ret = 0;
@@ -1344,7 +1346,7 @@ static int r8711_wx_set_rate(struct net_device *dev,
 				struct iw_request_info *a,
 				union iwreq_data *wrqu, char *extra)
 {
-	struct _adapter *padapter = (struct _adapter *)netdev_priv(dev);
+	struct _adapter *padapter = netdev_priv(dev);
 	u32 target_rate = wrqu->bitrate.value;
 	u32 fixed = wrqu->bitrate.fixed;
 	u32 ratevalue = 0;
@@ -1416,7 +1418,7 @@ static int r8711_wx_get_rate(struct net_device *dev,
 			     struct iw_request_info *info,
 			     union iwreq_data *wrqu, char *extra)
 {
-	struct _adapter *padapter = (struct _adapter *)netdev_priv(dev);
+	struct _adapter *padapter = netdev_priv(dev);
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 	struct ndis_wlan_bssid_ex *pcur_bss = &pmlmepriv->cur_network.network;
 	struct ieee80211_ht_cap *pht_capie;
@@ -1473,7 +1475,7 @@ static int r8711_wx_get_rts(struct net_device *dev,
 				struct iw_request_info *info,
 				union iwreq_data *wrqu, char *extra)
 {
-	struct _adapter *padapter = (struct _adapter *)netdev_priv(dev);
+	struct _adapter *padapter = netdev_priv(dev);
 
 	wrqu->rts.value = padapter->registrypriv.rts_thresh;
 	wrqu->rts.fixed = 0;	/* no auto select */
@@ -1484,7 +1486,7 @@ static int r8711_wx_set_frag(struct net_device *dev,
 				struct iw_request_info *info,
 				union iwreq_data *wrqu, char *extra)
 {
-	struct _adapter *padapter = (struct _adapter *)netdev_priv(dev);
+	struct _adapter *padapter = netdev_priv(dev);
 
 	if (wrqu->frag.disabled)
 		padapter->xmitpriv.frag_len = MAX_FRAG_THRESHOLD;
@@ -1501,7 +1503,7 @@ static int r8711_wx_get_frag(struct net_device *dev,
 				struct iw_request_info *info,
 				union iwreq_data *wrqu, char *extra)
 {
-	struct _adapter *padapter = (struct _adapter *) netdev_priv(dev);
+	struct _adapter *padapter = netdev_priv(dev);
 
 	wrqu->frag.value = padapter->xmitpriv.frag_len;
 	wrqu->frag.fixed = 0;	/* no auto select */
@@ -1527,7 +1529,7 @@ static int r8711_wx_set_enc(struct net_device *dev,
 	struct NDIS_802_11_WEP	 wep;
 	enum NDIS_802_11_AUTHENTICATION_MODE authmode;
 	struct iw_point *erq = &(wrqu->encoding);
-	struct _adapter *padapter = (struct _adapter *) netdev_priv(dev);
+	struct _adapter *padapter = netdev_priv(dev);
 
 	key = erq->flags & IW_ENCODE_INDEX;
 	memset(&wep, 0, sizeof(struct NDIS_802_11_WEP));
@@ -1619,7 +1621,7 @@ static int r8711_wx_get_enc(struct net_device *dev,
 				union iwreq_data *wrqu, char *keybuf)
 {
 	uint key, ret = 0;
-	struct _adapter *padapter = (struct _adapter *) netdev_priv(dev);
+	struct _adapter *padapter = netdev_priv(dev);
 	struct iw_point *erq = &(wrqu->encoding);
 	struct	mlme_priv	*pmlmepriv = &(padapter->mlmepriv);
 
@@ -1691,7 +1693,7 @@ static int r871x_wx_set_gen_ie(struct net_device *dev,
 				struct iw_request_info *info,
 				union iwreq_data *wrqu, char *extra)
 {
-	struct _adapter *padapter = (struct _adapter *)netdev_priv(dev);
+	struct _adapter *padapter = netdev_priv(dev);
 
 	return r871x_set_wpa_ie(padapter, extra, wrqu->data.length);
 }
@@ -1700,7 +1702,7 @@ static int r871x_wx_set_auth(struct net_device *dev,
 				struct iw_request_info *info,
 				union iwreq_data *wrqu, char *extra)
 {
-	struct _adapter *padapter = (struct _adapter *)netdev_priv(dev);
+	struct _adapter *padapter = netdev_priv(dev);
 	struct iw_param *param = (struct iw_param *)&(wrqu->param);
 	int paramid;
 	int paramval;
@@ -1847,7 +1849,7 @@ static int r8711_wx_read32(struct net_device *dev,
 				struct iw_request_info *info,
 				union iwreq_data *wrqu, char *keybuf)
 {
-	struct _adapter *padapter = (struct _adapter *) netdev_priv(dev);
+	struct _adapter *padapter = netdev_priv(dev);
 	u32 addr;
 	u32 data32;
 
@@ -1864,7 +1866,7 @@ static int r8711_wx_write32(struct net_device *dev,
 				 struct iw_request_info *info,
 				 union iwreq_data *wrqu, char *keybuf)
 {
-	struct _adapter *padapter = (struct _adapter *) netdev_priv(dev);
+	struct _adapter *padapter = netdev_priv(dev);
 	u32 addr;
 	u32 data32;
 
@@ -1892,7 +1894,7 @@ static int r871x_mp_ioctl_hdl(struct net_device *dev,
 				struct iw_request_info *info,
 				union iwreq_data *wrqu, char *extra)
 {
-	struct _adapter *padapter = (struct _adapter *)netdev_priv(dev);
+	struct _adapter *padapter = netdev_priv(dev);
 	struct iw_point *p = &wrqu->data;
 	struct oid_par_priv oid_par;
 	struct mp_ioctl_handler *phandler;
@@ -1910,13 +1912,9 @@ static int r871x_mp_ioctl_hdl(struct net_device *dev,
 	bset = (u8)(p->flags & 0xFFFF);
 	len = p->length;
 	pparmbuf = NULL;
-	pparmbuf = kmalloc(len, GFP_ATOMIC);
-	if (pparmbuf == NULL) {
-		ret = -ENOMEM;
-		goto _r871x_mp_ioctl_hdl_exit;
-	}
-	if (copy_from_user(pparmbuf, p->pointer, len)) {
-		ret = -EFAULT;
+	pparmbuf = memdup_user(p->pointer, len);
+	if (IS_ERR(pparmbuf)) {
+		ret = PTR_ERR(pparmbuf);
 		goto _r871x_mp_ioctl_hdl_exit;
 	}
 	poidparam = (struct mp_ioctl_param *)pparmbuf;
@@ -1975,7 +1973,7 @@ static int r871x_get_ap_info(struct net_device *dev,
 				struct iw_request_info *info,
 				union iwreq_data *wrqu, char *extra)
 {
-	struct _adapter *padapter = (struct _adapter *)netdev_priv(dev);
+	struct _adapter *padapter = netdev_priv(dev);
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 	struct  __queue *queue = &pmlmepriv->scanned_queue;
 	struct iw_point *pdata = &wrqu->data;
@@ -2046,7 +2044,7 @@ static int r871x_set_pid(struct net_device *dev,
 				struct iw_request_info *info,
 				union iwreq_data *wrqu, char *extra)
 {
-	struct _adapter *padapter = (struct _adapter *) netdev_priv(dev);
+	struct _adapter *padapter = netdev_priv(dev);
 	struct iw_point *pdata = &wrqu->data;
 
 	if ((padapter->bDriverStopped) || (pdata == NULL))
@@ -2061,7 +2059,7 @@ static int r871x_set_chplan(struct net_device *dev,
 				union iwreq_data *wrqu, char *extra)
 {
 	int ret = 0;
-	struct _adapter *padapter = (struct _adapter *) netdev_priv(dev);
+	struct _adapter *padapter = netdev_priv(dev);
 	struct iw_point *pdata = &wrqu->data;
 	int ch_plan = -1;
 
@@ -2081,7 +2079,7 @@ static int r871x_wps_start(struct net_device *dev,
 			   struct iw_request_info *info,
 			   union iwreq_data *wrqu, char *extra)
 {
-	struct _adapter *padapter = (struct _adapter *)netdev_priv(dev);
+	struct _adapter *padapter = netdev_priv(dev);
 	struct iw_point *pdata = &wrqu->data;
 	u32   u32wps_start = 0;
 
@@ -2105,7 +2103,7 @@ static int r871x_wps_start(struct net_device *dev,
 
 static int wpa_set_param(struct net_device *dev, u8 name, u32 value)
 {
-	struct _adapter *padapter = (struct _adapter *) netdev_priv(dev);
+	struct _adapter *padapter = netdev_priv(dev);
 
 	switch (name) {
 	case IEEE_PARAM_WPA_ENABLED:
@@ -2157,7 +2155,7 @@ static int wpa_set_param(struct net_device *dev, u8 name, u32 value)
 
 static int wpa_mlme(struct net_device *dev, u32 command, u32 reason)
 {
-	struct _adapter *padapter = (struct _adapter *) netdev_priv(dev);
+	struct _adapter *padapter = netdev_priv(dev);
 
 	switch (command) {
 	case IEEE_MLME_STA_DEAUTH:
@@ -2178,7 +2176,7 @@ static int wpa_supplicant_ioctl(struct net_device *dev, struct iw_point *p)
 {
 	struct ieee_param *param;
 	int ret = 0;
-	struct _adapter *padapter = (struct _adapter *) netdev_priv(dev);
+	struct _adapter *padapter = netdev_priv(dev);
 
 	if (p->length < sizeof(struct ieee_param) || !p->pointer)
 		return -EINVAL;
@@ -2331,7 +2329,7 @@ static iw_handler r8711_private_handler[] = {
 
 static struct iw_statistics *r871x_get_wireless_stats(struct net_device *dev)
 {
-	struct _adapter *padapter = (struct _adapter *) netdev_priv(dev);
+	struct _adapter *padapter = netdev_priv(dev);
 	struct iw_statistics *piwstats = &padapter->iwstats;
 	int tmp_level = 0;
 	int tmp_qual = 0;
diff --git a/drivers/staging/rtl8712/rtl871x_ioctl_set.c b/drivers/staging/rtl8712/rtl871x_ioctl_set.c
index 6318a0e65e6e..22262b3558ba 100644
--- a/drivers/staging/rtl8712/rtl871x_ioctl_set.c
+++ b/drivers/staging/rtl8712/rtl871x_ioctl_set.c
@@ -85,7 +85,8 @@ static u8 do_join(struct _adapter *padapter)
 
 		ret = r8712_select_and_join_from_scan(pmlmepriv);
 		if (ret == _SUCCESS)
-			_set_timer(&pmlmepriv->assoc_timer, MAX_JOIN_TIMEOUT);
+			mod_timer(&pmlmepriv->assoc_timer,
+				  jiffies + msecs_to_jiffies(MAX_JOIN_TIMEOUT));
 		else {
 			if (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE)) {
 				/* submit r8712_createbss_cmd to change to an
@@ -265,8 +266,7 @@ void r8712_set_802_11_infrastructure_mode(struct _adapter *padapter,
 		/* clear WIFI_STATION_STATE; WIFI_AP_STATE; WIFI_ADHOC_STATE;
 		 * WIFI_ADHOC_MASTER_STATE */
 		_clr_fwstate_(pmlmepriv, WIFI_STATION_STATE | WIFI_AP_STATE |
-			      WIFI_ADHOC_STATE | WIFI_ADHOC_MASTER_STATE |
-			      WIFI_AP_STATE);
+			      WIFI_ADHOC_STATE | WIFI_ADHOC_MASTER_STATE);
 		switch (networktype) {
 		case Ndis802_11IBSS:
 			set_fwstate(pmlmepriv, WIFI_ADHOC_STATE);
diff --git a/drivers/staging/rtl8712/rtl871x_mlme.c b/drivers/staging/rtl8712/rtl871x_mlme.c
index 977a83358056..fb2b195b90af 100644
--- a/drivers/staging/rtl8712/rtl871x_mlme.c
+++ b/drivers/staging/rtl8712/rtl871x_mlme.c
@@ -582,9 +582,7 @@ void r8712_surveydone_event_callback(struct _adapter *adapter, u8 *pbuf)
 	spin_lock_irqsave(&pmlmepriv->lock, irqL);
 
 	if (check_fwstate(pmlmepriv, _FW_UNDER_SURVEY) == true) {
-		u8 timer_cancelled;
-
-		_cancel_timer(&pmlmepriv->scan_to_timer, &timer_cancelled);
+		del_timer_sync(&pmlmepriv->scan_to_timer);
 
 		_clr_fwstate_(pmlmepriv, _FW_UNDER_SURVEY);
 	}
@@ -596,8 +594,8 @@ void r8712_surveydone_event_callback(struct _adapter *adapter, u8 *pbuf)
 
 				if (r8712_select_and_join_from_scan(pmlmepriv)
 				    == _SUCCESS)
-					_set_timer(&pmlmepriv->assoc_timer,
-						   MAX_JOIN_TIMEOUT);
+					mod_timer(&pmlmepriv->assoc_timer, jiffies +
+						  msecs_to_jiffies(MAX_JOIN_TIMEOUT));
 				else {
 					struct wlan_bssid_ex *pdev_network =
 					  &(adapter->registrypriv.dev_network);
@@ -622,8 +620,8 @@ void r8712_surveydone_event_callback(struct _adapter *adapter, u8 *pbuf)
 			set_fwstate(pmlmepriv, _FW_UNDER_LINKING);
 			if (r8712_select_and_join_from_scan(pmlmepriv) ==
 			    _SUCCESS)
-				_set_timer(&pmlmepriv->assoc_timer,
-					   MAX_JOIN_TIMEOUT);
+				mod_timer(&pmlmepriv->assoc_timer, jiffies +
+					  msecs_to_jiffies(MAX_JOIN_TIMEOUT));
 			else
 				_clr_fwstate_(pmlmepriv, _FW_UNDER_LINKING);
 		}
@@ -679,7 +677,8 @@ void r8712_indicate_connect(struct _adapter *padapter)
 	padapter->ledpriv.LedControlHandler(padapter, LED_CTL_LINK);
 	r8712_os_indicate_connect(padapter);
 	if (padapter->registrypriv.power_mgnt > PS_MODE_ACTIVE)
-		_set_timer(&pmlmepriv->dhcp_timer, 60000);
+		mod_timer(&pmlmepriv->dhcp_timer,
+			  jiffies + msecs_to_jiffies(60000));
 }
 
 
@@ -697,7 +696,7 @@ void r8712_ind_disconnect(struct _adapter *padapter)
 	}
 	if (padapter->pwrctrlpriv.pwr_mode !=
 	    padapter->registrypriv.power_mgnt) {
-		_cancel_timer_ex(&pmlmepriv->dhcp_timer);
+		del_timer_sync(&pmlmepriv->dhcp_timer);
 		r8712_set_ps_mode(padapter, padapter->registrypriv.power_mgnt,
 				  padapter->registrypriv.smart_ps);
 	}
@@ -716,7 +715,6 @@ void r8712_ind_disconnect(struct _adapter *padapter)
 void r8712_joinbss_event_callback(struct _adapter *adapter, u8 *pbuf)
 {
 	unsigned long irqL = 0, irqL2;
-	u8 timer_cancelled;
 	struct sta_info	*ptarget_sta = NULL, *pcur_sta = NULL;
 	struct sta_priv	*pstapriv = &adapter->stapriv;
 	struct mlme_priv	*pmlmepriv = &adapter->mlmepriv;
@@ -727,6 +725,8 @@ void r8712_joinbss_event_callback(struct _adapter *adapter, u8 *pbuf)
 
 	if (sizeof(struct list_head) == 4 * sizeof(u32)) {
 		pnetwork = kmalloc(sizeof(struct wlan_network), GFP_ATOMIC);
+		if (!pnetwork)
+			return;
 		memcpy((u8 *)pnetwork+16, (u8 *)pbuf + 8,
 			sizeof(struct wlan_network) - 16);
 	} else
@@ -910,13 +910,13 @@ void r8712_joinbss_event_callback(struct _adapter *adapter, u8 *pbuf)
 			if (check_fwstate(pmlmepriv, WIFI_STATION_STATE)
 				== true)
 				r8712_indicate_connect(adapter);
-			_cancel_timer(&pmlmepriv->assoc_timer,
-				      &timer_cancelled);
+			del_timer_sync(&pmlmepriv->assoc_timer);
 		} else
 			goto ignore_joinbss_callback;
 	} else {
 		if (check_fwstate(pmlmepriv, _FW_UNDER_LINKING) == true) {
-			_set_timer(&pmlmepriv->assoc_timer, 1);
+			mod_timer(&pmlmepriv->assoc_timer,
+				  jiffies + msecs_to_jiffies(1));
 			_clr_fwstate_(pmlmepriv, _FW_UNDER_LINKING);
 		}
 	}
@@ -1599,17 +1599,15 @@ sint r8712_restruct_sec_ie(struct _adapter *adapter, u8 *in_ie,
 	iEntry = SecIsInPMKIDList(adapter, pmlmepriv->assoc_bssid);
 	if (iEntry < 0)
 		return ielength;
-	else {
-		if (authmode == _WPA2_IE_ID_) {
-			out_ie[ielength] = 1;
-			ielength++;
-			out_ie[ielength] = 0;	/*PMKID count = 0x0100*/
-			ielength++;
-			memcpy(&out_ie[ielength],
-				&psecuritypriv->PMKIDList[iEntry].PMKID, 16);
-			ielength += 16;
-			out_ie[13] += 18;/*PMKID length = 2+16*/
-		}
+	if (authmode == _WPA2_IE_ID_) {
+		out_ie[ielength] = 1;
+		ielength++;
+		out_ie[ielength] = 0;	/*PMKID count = 0x0100*/
+		ielength++;
+		memcpy(&out_ie[ielength],
+			&psecuritypriv->PMKIDList[iEntry].PMKID, 16);
+		ielength += 16;
+		out_ie[13] += 18;/*PMKID length = 2+16*/
 	}
 	return ielength;
 }
diff --git a/drivers/staging/rtl8712/rtl871x_mp.c b/drivers/staging/rtl8712/rtl871x_mp.c
index 3d913b9701bb..26201ea3cca6 100644
--- a/drivers/staging/rtl8712/rtl871x_mp.c
+++ b/drivers/staging/rtl8712/rtl871x_mp.c
@@ -327,8 +327,8 @@ void r8712_SetTxAGCOffset(struct _adapter *pAdapter, u32 ulTxAGCOffset)
 	u32 TxAGCOffset_B, TxAGCOffset_C, TxAGCOffset_D, tmpAGC;
 
 	TxAGCOffset_B = (ulTxAGCOffset&0x000000ff);
-	TxAGCOffset_C = ((ulTxAGCOffset&0x0000ff00)>>8);
-	TxAGCOffset_D = ((ulTxAGCOffset&0x00ff0000)>>16);
+	TxAGCOffset_C = (ulTxAGCOffset & 0x0000ff00)>>8;
+	TxAGCOffset_D = (ulTxAGCOffset & 0x00ff0000)>>16;
 	tmpAGC = (TxAGCOffset_D<<8 | TxAGCOffset_C<<4 | TxAGCOffset_B);
 	set_bb_reg(pAdapter, rFPGA0_TxGainStage,
 			(bXBTxAGC|bXCTxAGC|bXDTxAGC), tmpAGC);
diff --git a/drivers/staging/rtl8712/rtl871x_pwrctrl.c b/drivers/staging/rtl8712/rtl871x_pwrctrl.c
index ed2844d2b02a..aaa584435c87 100644
--- a/drivers/staging/rtl8712/rtl871x_pwrctrl.c
+++ b/drivers/staging/rtl8712/rtl871x_pwrctrl.c
@@ -103,7 +103,7 @@ void r8712_cpwm_int_hdl(struct _adapter *padapter,
 
 	if (pwrpriv->cpwm_tog == ((preportpwrstate->state) & 0x80))
 		return;
-	_cancel_timer_ex(&padapter->pwrctrlpriv. rpwm_check_timer);
+	del_timer_sync(&padapter->pwrctrlpriv.rpwm_check_timer);
 	_enter_pwrlock(&pwrpriv->lock);
 	pwrpriv->cpwm = (preportpwrstate->state) & 0xf;
 	if (pwrpriv->cpwm >= PS_STATE_S2) {
@@ -165,9 +165,9 @@ static void rpwm_workitem_callback(struct work_struct *work)
 	}
 }
 
-static void rpwm_check_handler (void *FunctionContext)
+static void rpwm_check_handler (unsigned long data)
 {
-	struct _adapter *adapter = (struct _adapter *)FunctionContext;
+	struct _adapter *adapter = (struct _adapter *)data;
 
 	_rpwm_check_handler(adapter);
 }
@@ -186,8 +186,8 @@ void r8712_init_pwrctrl_priv(struct _adapter *padapter)
 	r8712_write8(padapter, 0x1025FE58, 0);
 	INIT_WORK(&pwrctrlpriv->SetPSModeWorkItem, SetPSModeWorkItemCallback);
 	INIT_WORK(&pwrctrlpriv->rpwm_workitem, rpwm_workitem_callback);
-	_init_timer(&(pwrctrlpriv->rpwm_check_timer),
-		    padapter->pnetdev, rpwm_check_handler, (u8 *)padapter);
+	setup_timer(&pwrctrlpriv->rpwm_check_timer, rpwm_check_handler,
+		    (unsigned long)padapter);
 }
 
 /*
diff --git a/drivers/staging/rtl8712/rtl871x_recv.c b/drivers/staging/rtl8712/rtl871x_recv.c
index 06f15f81c4d8..046a46c4cd7f 100644
--- a/drivers/staging/rtl8712/rtl871x_recv.c
+++ b/drivers/staging/rtl8712/rtl871x_recv.c
@@ -517,8 +517,7 @@ static sint validate_recv_data_frame(struct _adapter *adapter,
 		return _FAIL;
 	if (psta == NULL)
 		return _FAIL;
-	else
-		precv_frame->u.hdr.psta = psta;
+	precv_frame->u.hdr.psta = psta;
 	pattrib->amsdu = 0;
 	/* parsing QC field */
 	if (pattrib->qos == 1) {
diff --git a/drivers/staging/rtl8712/rtl871x_security.c b/drivers/staging/rtl8712/rtl871x_security.c
index c653ad6854b4..bcd1a5128868 100644
--- a/drivers/staging/rtl8712/rtl871x_security.c
+++ b/drivers/staging/rtl8712/rtl871x_security.c
@@ -153,7 +153,7 @@ static u32 getcrc32(u8 *buf, u32 len)
 	u8 *p;
 	u32  crc;
 
-	if (bcrc32initialized == 0)
+	if (!bcrc32initialized)
 		crc32_init();
 	crc = 0xffffffff; /* preload shift register, per CRC-32 spec */
 	for (p = buf; len > 0; ++p, --len)
@@ -1045,7 +1045,7 @@ static sint aes_cipher(u8 *key, uint	hdrlen,
 	uint	frtype  = GetFrameType(pframe);
 	uint	frsubtype  = GetFrameSubType(pframe);
 
-	frsubtype = frsubtype >> 4;
+	frsubtype >>= 4;
 	memset((void *)mic_iv, 0, 16);
 	memset((void *)mic_header1, 0, 16);
 	memset((void *)mic_header2, 0, 16);
@@ -1086,7 +1086,7 @@ static sint aes_cipher(u8 *key, uint	hdrlen,
 	payload_remainder = plen % 16;
 	num_blocks = plen / 16;
 	/* Find start of payload */
-	payload_index = (hdrlen + 8);
+	payload_index = hdrlen + 8;
 	/* Calculate MIC */
 	aes128k128d(key, mic_iv, aes_out);
 	bitwise_xor(aes_out, mic_header1, chain_buffer);
@@ -1216,7 +1216,7 @@ static sint aes_decipher(u8 *key, uint	hdrlen,
 	uint frtype  = GetFrameType(pframe);
 	uint frsubtype  = GetFrameSubType(pframe);
 
-	frsubtype = frsubtype >> 4;
+	frsubtype >>= 4;
 	memset((void *)mic_iv, 0, 16);
 	memset((void *)mic_header1, 0, 16);
 	memset((void *)mic_header2, 0, 16);
@@ -1292,7 +1292,7 @@ static sint aes_decipher(u8 *key, uint	hdrlen,
 	payload_remainder = (plen - 8) % 16;
 	num_blocks = (plen - 8) / 16;
 	/* Find start of payload */
-	payload_index = (hdrlen + 8);
+	payload_index = hdrlen + 8;
 	/* Calculate MIC */
 	aes128k128d(key, mic_iv, aes_out);
 	bitwise_xor(aes_out, mic_header1, chain_buffer);
@@ -1392,9 +1392,9 @@ u32 r8712_aes_decrypt(struct _adapter *padapter, u8 *precvframe)
 	return _SUCCESS;
 }
 
-void r8712_use_tkipkey_handler(void *FunctionContext)
+void r8712_use_tkipkey_handler(unsigned long data)
 {
-	struct _adapter *padapter = (struct _adapter *)FunctionContext;
+	struct _adapter *padapter = (struct _adapter *)data;
 
 	padapter->securitypriv.busetkipkey = true;
 }
diff --git a/drivers/staging/rtl8712/rtl871x_security.h b/drivers/staging/rtl8712/rtl871x_security.h
index c732aeab8d2c..2295f0e64dc2 100644
--- a/drivers/staging/rtl8712/rtl871x_security.h
+++ b/drivers/staging/rtl8712/rtl871x_security.h
@@ -216,7 +216,7 @@ void r8712_wep_encrypt(struct _adapter *padapter, u8  *pxmitframe);
 u32 r8712_aes_decrypt(struct _adapter *padapter, u8  *precvframe);
 u32 r8712_tkip_decrypt(struct _adapter *padapter, u8  *precvframe);
 void r8712_wep_decrypt(struct _adapter *padapter, u8  *precvframe);
-void r8712_use_tkipkey_handler(void *FunctionContext);
+void r8712_use_tkipkey_handler(unsigned long data);
 
 #endif	/*__RTL871X_SECURITY_H_ */
 
diff --git a/drivers/staging/rtl8712/rtl871x_sta_mgt.c b/drivers/staging/rtl8712/rtl871x_sta_mgt.c
index 1752121ff494..7bb96c47f188 100644
--- a/drivers/staging/rtl8712/rtl871x_sta_mgt.c
+++ b/drivers/staging/rtl8712/rtl871x_sta_mgt.c
@@ -198,7 +198,7 @@ void r8712_free_stainfo(struct _adapter *padapter, struct sta_info *psta)
 	 * cancel reordering_ctrl_timer */
 	for (i = 0; i < 16; i++) {
 		preorder_ctrl = &psta->recvreorder_ctrl[i];
-		_cancel_timer_ex(&preorder_ctrl->reordering_ctrl_timer);
+		del_timer_sync(&preorder_ctrl->reordering_ctrl_timer);
 	}
 	spin_lock(&(pfree_sta_queue->lock));
 	/* insert into free_sta_queue; 20061114 */
diff --git a/drivers/staging/rtl8712/rtl871x_xmit.c b/drivers/staging/rtl8712/rtl871x_xmit.c
index a28af03c9d8a..2e4fa88951ad 100644
--- a/drivers/staging/rtl8712/rtl871x_xmit.c
+++ b/drivers/staging/rtl8712/rtl871x_xmit.c
@@ -203,13 +203,12 @@ sint r8712_update_attrib(struct _adapter *padapter, _pkt *pkt,
 	pattrib->ether_type = ntohs(etherhdr.h_proto);
 
 {
-	u8 bool;
 	/*If driver xmit ARP packet, driver can set ps mode to initial
 	 * setting. It stands for getting DHCP or fix IP.*/
 	if (pattrib->ether_type == 0x0806) {
 		if (padapter->pwrctrlpriv.pwr_mode !=
 		    padapter->registrypriv.power_mgnt) {
-			_cancel_timer(&(pmlmepriv->dhcp_timer), &bool);
+			del_timer_sync(&pmlmepriv->dhcp_timer);
 			r8712_set_ps_mode(padapter, padapter->registrypriv.
 				power_mgnt, padapter->registrypriv.smart_ps);
 		}
diff --git a/drivers/staging/rtl8712/wifi.h b/drivers/staging/rtl8712/wifi.h
index 73d7cd280607..17f513122f48 100644
--- a/drivers/staging/rtl8712/wifi.h
+++ b/drivers/staging/rtl8712/wifi.h
@@ -235,11 +235,6 @@ enum WIFI_REG_DOMAIN {
 #define GetPrivacy(pbuf)	(((*(unsigned short *)(pbuf)) & \
 				le16_to_cpu(_PRIVACY_)) != 0)
 
-#define ClearPrivacy(pbuf) ({ \
-	*(unsigned short *)(pbuf) &= (~cpu_to_le16(_PRIVACY_)); \
-})
-
-
 #define GetOrder(pbuf)	(((*(unsigned short *)(pbuf)) & \
 			le16_to_cpu(_ORDER_)) != 0)
 
@@ -248,9 +243,9 @@ enum WIFI_REG_DOMAIN {
 
 #define SetFrameType(pbuf, type)	\
 	do {	\
-		*(unsigned short *)(pbuf) &= __constant_cpu_to_le16(~(BIT(3) | \
+		*(unsigned short *)(pbuf) &= cpu_to_le16(~(BIT(3) | \
 		BIT(2))); \
-		*(unsigned short *)(pbuf) |= __constant_cpu_to_le16(type); \
+		*(unsigned short *)(pbuf) |= cpu_to_le16(type); \
 	} while (0)
 
 #define GetFrameSubType(pbuf)	(cpu_to_le16(*(unsigned short *)(pbuf)) & \
@@ -270,16 +265,6 @@ enum WIFI_REG_DOMAIN {
 #define GetFragNum(pbuf)	(cpu_to_le16(*(unsigned short *)((addr_t)\
 				(pbuf) + 22)) & 0x0f)
 
-#define GetTupleCache(pbuf)	(cpu_to_le16(*(unsigned short *)\
-				((addr_t)(pbuf) + 22)))
-
-#define SetFragNum(pbuf, num) ({ \
-	*(unsigned short *)((addr_t)(pbuf) + 22) = \
-	((*(unsigned short *)((addr_t)(pbuf) + 22)) & \
-	le16_to_cpu(~(0x000f))) | \
-	cpu_to_le16(0x0f & (num));     \
-})
-
 #define SetSeqNum(pbuf, num) ({ \
 	*(unsigned short *)((addr_t)(pbuf) + 22) = \
 	((*(unsigned short *)((addr_t)(pbuf) + 22)) & \
@@ -306,17 +291,9 @@ enum WIFI_REG_DOMAIN {
 
 #define GetAMsdu(pbuf) (((le16_to_cpu(*(unsigned short *)pbuf)) >> 7) & 0x1)
 
-#define SetAMsdu(pbuf, amsdu) ({ \
-	*(unsigned short *)(pbuf) |= cpu_to_le16((amsdu & 1) << 7); \
-})
-
 #define GetAid(pbuf)	(cpu_to_le16(*(unsigned short *)((addr_t)(pbuf) + 2)) \
 			& 0x3fff)
 
-#define GetTid(pbuf)	(cpu_to_le16(*(unsigned short *)((addr_t)(pbuf) + \
-			(((GetToDs(pbuf) << 1)|GetFrDs(pbuf)) == 3 ? \
-			30 : 24))) & 0x000f)
-
 #define GetAddr1Ptr(pbuf)	((unsigned char *)((addr_t)(pbuf) + 4))
 
 #define GetAddr2Ptr(pbuf)	((unsigned char *)((addr_t)(pbuf) + 10))
diff --git a/drivers/staging/rtl8712/xmit_linux.c b/drivers/staging/rtl8712/xmit_linux.c
index 039b598152bc..d15fb1ad45b9 100644
--- a/drivers/staging/rtl8712/xmit_linux.c
+++ b/drivers/staging/rtl8712/xmit_linux.c
@@ -162,7 +162,7 @@ void r8712_xmit_complete(struct _adapter *padapter, struct xmit_frame *pxframe)
 int r8712_xmit_entry(_pkt *pkt, struct  net_device *pnetdev)
 {
 	struct xmit_frame *pxmitframe = NULL;
-	struct _adapter *padapter = (struct _adapter *)netdev_priv(pnetdev);
+	struct _adapter *padapter = netdev_priv(pnetdev);
 	struct xmit_priv *pxmitpriv = &(padapter->xmitpriv);
 	int ret = 0;
 
diff --git a/drivers/staging/rtl8723au/core/rtw_ap.c b/drivers/staging/rtl8723au/core/rtw_ap.c
index c6327c072918..645668950e9c 100644
--- a/drivers/staging/rtl8723au/core/rtw_ap.c
+++ b/drivers/staging/rtl8723au/core/rtw_ap.c
@@ -20,6 +20,7 @@
 #include <wifi.h>
 #include <rtl8723a_cmd.h>
 #include <rtl8723a_hal.h>
+#include <asm/unaligned.h>
 
 extern unsigned char WMM_OUI23A[];
 extern unsigned char WPS_OUI23A[];
@@ -72,11 +73,8 @@ static void update_BCNTIM(struct rtw_adapter *padapter)
 	struct wlan_bssid_ex *pnetwork_mlmeext = &pmlmeinfo->network;
 	unsigned char *pie = pnetwork_mlmeext->IEs;
 	u8 *p, *dst_ie, *premainder_ie = NULL, *pbackup_remainder_ie = NULL;
-	__le16 tim_bitmap_le;
 	uint offset, tmp_len, tim_ielen, tim_ie_offset, remainder_ielen;
 
-	tim_bitmap_le = cpu_to_le16(pstapriv->tim_bitmap);
-
 	p = rtw_get_ie23a(pie, WLAN_EID_TIM, &tim_ielen,
 			  pnetwork_mlmeext->IELength);
 	if (p != NULL && tim_ielen > 0) {
@@ -143,9 +141,9 @@ static void update_BCNTIM(struct rtw_adapter *padapter)
 		*dst_ie++ = 0;
 
 	if (tim_ielen == 4) {
-		*dst_ie++ = *(u8 *)&tim_bitmap_le;
+		*dst_ie++ = pstapriv->tim_bitmap & 0xff;
 	} else if (tim_ielen == 5) {
-		memcpy(dst_ie, &tim_bitmap_le, 2);
+		put_unaligned_le16(pstapriv->tim_bitmap, dst_ie);
 		dst_ie += 2;
 	}
 
@@ -262,15 +260,17 @@ void	expire_timeout_chk23a(struct rtw_adapter *padapter)
 			list_del_init(&psta->asoc_list);
 			pstapriv->asoc_list_cnt--;
 
-			DBG_8723A("asoc expire "MAC_FMT", state = 0x%x\n", MAC_ARG(psta->hwaddr), psta->state);
+			DBG_8723A("asoc expire %pM, state = 0x%x\n",
+				  psta->hwaddr, psta->state);
 			updated = ap_free_sta23a(padapter, psta, false, WLAN_REASON_DEAUTH_LEAVING);
 		} else {
 			/* TODO: Aging mechanism to digest frames in sleep_q to avoid running out of xmitframe */
 			if (psta->sleepq_len > (NR_XMITFRAME/pstapriv->asoc_list_cnt)
 				&& padapter->xmitpriv.free_xmitframe_cnt < ((NR_XMITFRAME/pstapriv->asoc_list_cnt)/2)
 			) {
-				DBG_8723A("%s sta:"MAC_FMT", sleepq_len:%u, free_xmitframe_cnt:%u, asoc_list_cnt:%u, clear sleep_q\n", __func__,
-					  MAC_ARG(psta->hwaddr),
+				DBG_8723A("%s sta:%pM, sleepq_len:%u, free_xmitframe_cnt:%u, asoc_list_cnt:%u, clear sleep_q\n",
+					  __func__,
+					  psta->hwaddr,
 					  psta->sleepq_len,
 					  padapter->xmitpriv.free_xmitframe_cnt,
 					  pstapriv->asoc_list_cnt);
@@ -307,7 +307,8 @@ void	expire_timeout_chk23a(struct rtw_adapter *padapter)
 
 		psta->keep_alive_trycnt++;
 		if (ret == _SUCCESS) {
-			DBG_8723A("asoc check, sta(" MAC_FMT ") is alive\n", MAC_ARG(psta->hwaddr));
+			DBG_8723A("asoc check, sta(%pM) is alive\n",
+				  psta->hwaddr);
 			psta->expire_to = pstapriv->expire_to;
 			psta->keep_alive_trycnt = 0;
 			continue;
@@ -319,7 +320,8 @@ void	expire_timeout_chk23a(struct rtw_adapter *padapter)
 
 		psta->keep_alive_trycnt = 0;
 
-		DBG_8723A("asoc expire "MAC_FMT", state = 0x%x\n", MAC_ARG(psta->hwaddr), psta->state);
+		DBG_8723A("asoc expire %pM, state = 0x%x\n",
+			  psta->hwaddr, psta->state);
 		spin_lock_bh(&pstapriv->asoc_list_lock);
 		if (!list_empty(&psta->asoc_list)) {
 			list_del_init(&psta->asoc_list);
@@ -785,6 +787,7 @@ int rtw_check_beacon_data23a(struct rtw_adapter *padapter,
 	struct wlan_bssid_ex *pbss_network = &pmlmepriv->cur_network.network;
 	u8 *ie = pbss_network->IEs;
 	u8 *pbuf = mgmt->u.beacon.variable;
+
 	len -= offsetof(struct ieee80211_mgmt, u.beacon.variable);
 	/* SSID */
 	/* Supported rates */
@@ -1045,7 +1048,7 @@ int rtw_acl_add_sta23a(struct rtw_adapter *padapter, u8 *addr)
 	struct wlan_acl_pool *pacl_list = &pstapriv->acl_list;
 	struct rtw_queue *pacl_node_q = &pacl_list->acl_node_q;
 
-	DBG_8723A("%s(acl_num =%d) =" MAC_FMT "\n", __func__, pacl_list->num, MAC_ARG(addr));
+	DBG_8723A("%s(acl_num =%d) =%pM\n", __func__, pacl_list->num, addr);
 
 	if ((NUM_ACL-1) < pacl_list->num)
 		return -1;
@@ -1477,8 +1480,8 @@ void bss_cap_update_on_sta_join23a(struct rtw_adapter *padapter, struct sta_info
 	if (psta->flags & WLAN_STA_HT) {
 		u16 ht_capab = le16_to_cpu(psta->htpriv.ht_cap.cap_info);
 
-		DBG_8723A("HT: STA " MAC_FMT " HT Capabilities "
-			   "Info: 0x%04x\n", MAC_ARG(psta->hwaddr), ht_capab);
+		DBG_8723A("HT: STA %pM HT Capabilities Info: 0x%04x\n",
+			  psta->hwaddr, ht_capab);
 
 		if (psta->no_ht_set) {
 			psta->no_ht_set = 0;
@@ -1490,10 +1493,9 @@ void bss_cap_update_on_sta_join23a(struct rtw_adapter *padapter, struct sta_info
 				psta->no_ht_gf_set = 1;
 				pmlmepriv->num_sta_ht_no_gf++;
 			}
-			DBG_8723A("%s STA " MAC_FMT " - no "
-				   "greenfield, num of non-gf stations %d\n",
-				   __func__, MAC_ARG(psta->hwaddr),
-				   pmlmepriv->num_sta_ht_no_gf);
+			DBG_8723A("%s STA %pM - no greenfield, num of non-gf stations %d\n",
+				  __func__, psta->hwaddr,
+				  pmlmepriv->num_sta_ht_no_gf);
 		}
 
 		if ((ht_capab & IEEE80211_HT_CAP_SUP_WIDTH_20_40) == 0) {
@@ -1501,10 +1503,9 @@ void bss_cap_update_on_sta_join23a(struct rtw_adapter *padapter, struct sta_info
 				psta->ht_20mhz_set = 1;
 				pmlmepriv->num_sta_ht_20mhz++;
 			}
-			DBG_8723A("%s STA " MAC_FMT " - 20 MHz HT, "
-				   "num of 20MHz HT STAs %d\n",
-				   __func__, MAC_ARG(psta->hwaddr),
-				   pmlmepriv->num_sta_ht_20mhz);
+			DBG_8723A("%s STA %pM - 20 MHz HT, num of 20MHz HT STAs %d\n",
+				  __func__, psta->hwaddr,
+				  pmlmepriv->num_sta_ht_20mhz);
 		}
 
 	} else {
@@ -1513,10 +1514,9 @@ void bss_cap_update_on_sta_join23a(struct rtw_adapter *padapter, struct sta_info
 			pmlmepriv->num_sta_no_ht++;
 		}
 		if (pmlmepriv->htpriv.ht_option) {
-			DBG_8723A("%s STA " MAC_FMT
-				   " - no HT, num of non-HT stations %d\n",
-				   __func__, MAC_ARG(psta->hwaddr),
-				   pmlmepriv->num_sta_no_ht);
+			DBG_8723A("%s STA %pM - no HT, num of non-HT stations %d\n",
+				  __func__, psta->hwaddr,
+				  pmlmepriv->num_sta_no_ht);
 		}
 	}
 
diff --git a/drivers/staging/rtl8723au/core/rtw_cmd.c b/drivers/staging/rtl8723au/core/rtw_cmd.c
index 2447a56df838..46aea16cbf78 100644
--- a/drivers/staging/rtl8723au/core/rtw_cmd.c
+++ b/drivers/staging/rtl8723au/core/rtw_cmd.c
@@ -299,9 +299,8 @@ post_process:
 		pcmd_callback =	rtw_cmd_callback[pcmd->cmdcode].callback;
 		if (!pcmd_callback) {
 			RT_TRACE(_module_rtl871x_cmd_c_, _drv_info_,
-				 ("mlme_cmd_hdl(): pcmd_callback = 0x%p, "
-				  "cmdcode = 0x%x\n",
-				  pcmd_callback, pcmd->cmdcode));
+				 "mlme_cmd_hdl(): pcmd_callback = 0x%p, cmdcode = 0x%x\n",
+				 pcmd_callback, pcmd->cmdcode);
 			rtw_free_cmd_obj23a(pcmd);
 		} else {
 			/* need consider that free cmd_obj in
@@ -310,8 +309,8 @@ post_process:
 		}
 	} else {
 		RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_,
-			 ("%s: cmdcode = 0x%x callback not defined!\n",
-			  __func__, pcmd->cmdcode));
+			 "%s: cmdcode = 0x%x callback not defined!\n",
+			 __func__, pcmd->cmdcode);
 		rtw_free_cmd_obj23a(pcmd);
 	}
 }
@@ -343,7 +342,7 @@ int rtw_sitesurvey_cmd23a(struct rtw_adapter *padapter,
 	rtw_free_network_queue23a(padapter);
 
 	RT_TRACE(_module_rtl871x_cmd_c_, _drv_info_,
-		 ("%s: flush network queue\n", __func__));
+		 "%s: flush network queue\n", __func__);
 
 	init_h2fwcmd_w_parm_no_rsp(ph2c, psurveyPara,
 				   GEN_CMD_CODE(_SiteSurvey));
@@ -412,12 +411,12 @@ int rtw_createbss_cmd23a(struct rtw_adapter  *padapter)
 
 	if (pmlmepriv->assoc_ssid.ssid_len == 0) {
 		RT_TRACE(_module_rtl871x_cmd_c_, _drv_info_,
-			 (" createbss for Any SSid:%s\n",
-			  pmlmepriv->assoc_ssid.ssid));
+			 "createbss for Any SSid:%s\n",
+			 pmlmepriv->assoc_ssid.ssid);
 	} else {
 		RT_TRACE(_module_rtl871x_cmd_c_, _drv_info_,
-			 (" createbss for SSid:%s\n",
-			  pmlmepriv->assoc_ssid.ssid));
+			 "createbss for SSid:%s\n",
+			 pmlmepriv->assoc_ssid.ssid);
 	}
 
 	pcmd = kzalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
@@ -460,19 +459,18 @@ int rtw_joinbss_cmd23a(struct rtw_adapter *padapter,
 
 	if (pmlmepriv->assoc_ssid.ssid_len == 0) {
 		RT_TRACE(_module_rtl871x_cmd_c_, _drv_info_,
-			 ("+Join cmd: Any SSid\n"));
+			 "+Join cmd: Any SSid\n");
 	} else {
 		RT_TRACE(_module_rtl871x_cmd_c_, _drv_notice_,
-			 ("+Join cmd: SSid =[%s]\n",
-			  pmlmepriv->assoc_ssid.ssid));
+			 "+Join cmd: SSid =[%s]\n",
+			 pmlmepriv->assoc_ssid.ssid);
 	}
 
 	pcmd = kzalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
 	if (!pcmd) {
 		res = _FAIL;
 		RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_,
-			 ("rtw_joinbss_cmd23a: memory allocate for cmd_obj "
-			  "fail!!!\n"));
+			 "rtw_joinbss_cmd23a: memory allocate for cmd_obj fail!!!\n");
 		goto exit;
 	}
 
@@ -497,7 +495,7 @@ int rtw_joinbss_cmd23a(struct rtw_adapter *padapter,
 		res = _FAIL;
 
 		RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_,
-			 ("rtw_joinbss_cmd23a :psecnetwork == NULL!!!\n"));
+			 "rtw_joinbss_cmd23a :psecnetwork == NULL!!!\n");
 
 		goto exit;
 	}
@@ -599,7 +597,7 @@ int rtw_disassoc_cmd23a(struct rtw_adapter *padapter, u32 deauth_timeout_ms,
 	int res = _SUCCESS;
 
 	RT_TRACE(_module_rtl871x_cmd_c_, _drv_notice_,
-		 ("+rtw_disassoc_cmd23a\n"));
+		 "+rtw_disassoc_cmd23a\n");
 
 	/* prepare cmd parameter */
 	param = kzalloc(sizeof(*param), GFP_ATOMIC);
@@ -1291,8 +1289,7 @@ void rtw_survey_cmd_callback23a(struct rtw_adapter *padapter,
 		mod_timer(&pmlmepriv->scan_to_timer,
 			  jiffies + msecs_to_jiffies(1));
 		RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_,
-			 ("\n ********Error: MgntActrtw_set_802_11_bssid23a_"
-			  "LIST_SCAN Fail ************\n\n."));
+			 "********Error: MgntActrtw_set_802_11_bssid23a_LIST_SCAN Fail ************\n");
 	}
 
 	/*  free cmd */
@@ -1309,7 +1306,7 @@ void rtw_disassoc_cmd23a_callback(struct rtw_adapter *padapter,
 		set_fwstate(pmlmepriv, _FW_LINKED);
 		spin_unlock_bh(&pmlmepriv->lock);
 		RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_,
-			 ("\n ***Error: disconnect_cmd_callback Fail ***\n."));
+			 "***Error: disconnect_cmd_callback Fail ***\n");
 		return;
 	}
 
@@ -1329,8 +1326,7 @@ void rtw_joinbss_cmd23a_callback(struct rtw_adapter *padapter,
 			  jiffies + msecs_to_jiffies(1));
 	} else if (pcmd->res != H2C_SUCCESS) {
 		RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_,
-			 ("********Error:rtw_select_and_join_from_scanned_"
-			  "queue Wait Sema  Fail ************\n"));
+			 "********Error:rtw_select_and_join_from_scanned_queue Wait Sema  Fail ************\n");
 		mod_timer(&pmlmepriv->assoc_timer,
 			  jiffies + msecs_to_jiffies(1));
 	}
@@ -1349,8 +1345,7 @@ void rtw_createbss_cmd23a_callback(struct rtw_adapter *padapter,
 
 	if (pcmd->res != H2C_SUCCESS) {
 		RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_,
-			 ("\n ********Error: rtw_createbss_cmd23a_callback  "
-			  "Fail ************\n\n."));
+			 "********Error: rtw_createbss_cmd23a_callback  Fail ************\n");
 		mod_timer(&pmlmepriv->assoc_timer,
 			  jiffies + msecs_to_jiffies(1));
 	}
@@ -1366,9 +1361,8 @@ void rtw_createbss_cmd23a_callback(struct rtw_adapter *padapter,
 						    GFP_KERNEL);
 			if (!psta) {
 				RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_,
-					 ("\nCan't alloc sta_info when "
-					  "createbss_cmd_callback\n"));
-				goto createbss_cmd_fail ;
+					 "Can't alloc sta_info when createbss_cmd_callback\n");
+				goto createbss_cmd_fail;
 			}
 		}
 
@@ -1382,8 +1376,7 @@ void rtw_createbss_cmd23a_callback(struct rtw_adapter *padapter,
 			pwlan = rtw_get_oldest_wlan_network23a(&pmlmepriv->scanned_queue);
 			if (!pwlan) {
 				RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_,
-					 ("\n Error:  can't get pwlan in "
-					  "rtw23a_joinbss_event_cb\n"));
+					 "Error:  can't get pwlan in rtw23a_joinbss_event_cb\n");
 				spin_unlock_bh(&pmlmepriv->scanned_queue.lock);
 				goto createbss_cmd_fail;
 			}
@@ -1432,8 +1425,7 @@ void rtw_setstaKey_cmdrsp_callback23a(struct rtw_adapter *padapter,
 
 	if (!psta) {
 		RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_,
-			 ("\nERROR: rtw_setstaKey_cmdrsp_callback23a => "
-			  "can't get sta_info\n\n"));
+			 "ERROR: rtw_setstaKey_cmdrsp_callback23a => can't get sta_info\n");
 		goto exit;
 	}
 
@@ -1457,8 +1449,7 @@ void rtw_setassocsta_cmdrsp_callback23a(struct rtw_adapter *padapter,
 
 	if (psta == NULL) {
 		RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_,
-			 ("\nERROR: setassocsta_cmdrsp_callbac => can't "
-			  "get sta_info\n\n"));
+			 "ERROR: setassocsta_cmdrsp_callbac => can't get sta_info\n");
 		goto exit;
 	}
 
diff --git a/drivers/staging/rtl8723au/core/rtw_efuse.c b/drivers/staging/rtl8723au/core/rtw_efuse.c
index a6deddc02291..92a34db3bd4e 100644
--- a/drivers/staging/rtl8723au/core/rtw_efuse.c
+++ b/drivers/staging/rtl8723au/core/rtw_efuse.c
@@ -304,8 +304,7 @@ EFUSE_Read1Byte23a(struct rtw_adapter *Adapter, u16 Address)
 		}
 		data = rtl8723au_read8(Adapter, EFUSE_CTRL);
 		return data;
-	}
-	else
+	} else
 		return 0xFF;
 }/* EFUSE_Read1Byte23a */
 
@@ -336,7 +335,6 @@ EFUSE_Write1Byte(struct rtw_adapter *Adapter, u16 Address, u8 Value)
 	u32	k = 0;
 	u16	contentLen = 0;
 
-	/* RT_TRACE(COMP_EFUSE, DBG_LOUD, ("Addr =%x Data =%x\n", Address, Value)); */
 	EFUSE_GetEfuseDefinition23a(Adapter, EFUSE_WIFI,
 				 TYPE_EFUSE_REAL_CONTENT_LEN,
 				 (void *)&contentLen);
@@ -405,8 +403,6 @@ efuse_OneByteWrite23a(struct rtw_adapter *pAdapter, u16 addr, u8 data)
 	u8	tmpidx = 0;
 	int	bResult;
 
-	/* RT_TRACE(COMP_EFUSE, DBG_LOUD, ("Addr = %x Data =%x\n", addr, data)); */
-
 	/* return	0; */
 
 	/*  -----------------e-fuse reg ctrl --------------------------------- */
diff --git a/drivers/staging/rtl8723au/core/rtw_ieee80211.c b/drivers/staging/rtl8723au/core/rtw_ieee80211.c
index bbbcfc8257da..cdd7bc402ece 100644
--- a/drivers/staging/rtl8723au/core/rtw_ieee80211.c
+++ b/drivers/staging/rtl8723au/core/rtw_ieee80211.c
@@ -472,8 +472,8 @@ int rtw_parse_wpa_ie23a(const u8* wpa_ie, int wpa_ie_len, int *group_cipher, int
 		left -= WPA_SELECTOR_LEN;
 	} else if (left > 0) {
 		RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_,
-			 ("%s: ie length mismatch, %u too much",
-			  __func__, left));
+			 "%s: ie length mismatch, %u too much\n",
+			 __func__, left);
 
 		return _FAIL;
 	}
@@ -487,9 +487,8 @@ int rtw_parse_wpa_ie23a(const u8* wpa_ie, int wpa_ie_len, int *group_cipher, int
 
 		if (count == 0 || left < count * WPA_SELECTOR_LEN) {
 			RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_,
-				 ("%s: ie count botch (pairwise), "
-				  "count %u left %u", __func__,
-				  count, left));
+				 "%s: ie count botch (pairwise), count %u left %u\n",
+				 __func__, count, left);
 			return _FAIL;
 		}
 
@@ -501,7 +500,7 @@ int rtw_parse_wpa_ie23a(const u8* wpa_ie, int wpa_ie_len, int *group_cipher, int
 		}
 	} else if (left == 1) {
 		RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_,
-			 ("%s: ie too short (for key mgmt)", __func__));
+			 "%s: ie too short (for key mgmt)\n", __func__);
 		return _FAIL;
 	}
 
@@ -510,8 +509,8 @@ int rtw_parse_wpa_ie23a(const u8* wpa_ie, int wpa_ie_len, int *group_cipher, int
 			pos += 2;
 			if (!memcmp(pos, RTW_WPA_OUI23A_TYPE, 4)) {
 				RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_,
-					 ("%s : there has 802.1x auth\n",
-					  __func__));
+					 "%s : there has 802.1x auth\n",
+					 __func__);
 				*is_8021x = 1;
 			}
 		}
@@ -549,8 +548,8 @@ int rtw_parse_wpa2_ie23a(const u8 *rsn_ie, int rsn_ie_len, int *group_cipher,
 		left -= RSN_SELECTOR_LEN;
 	} else if (left > 0) {
 		RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_,
-			 ("%s: ie length mismatch, %u too much",
-			  __func__, left));
+			 "%s: ie length mismatch, %u too much\n",
+			 __func__, left);
 		return _FAIL;
 	}
 
@@ -563,9 +562,8 @@ int rtw_parse_wpa2_ie23a(const u8 *rsn_ie, int rsn_ie_len, int *group_cipher,
 
 		if (count == 0 || left < count * RSN_SELECTOR_LEN) {
 			RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_,
-				 ("%s: ie count botch (pairwise), "
-				  "count %u left %u",
-				  __func__, count, left));
+				 "%s: ie count botch (pairwise), count %u left %u\n",
+				 __func__, count, left);
 			return _FAIL;
 		}
 
@@ -577,7 +575,7 @@ int rtw_parse_wpa2_ie23a(const u8 *rsn_ie, int rsn_ie_len, int *group_cipher,
 		}
 	} else if (left == 1) {
 		RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_,
-			 ("%s: ie too short (for key mgmt)",  __func__));
+			 "%s: ie too short (for key mgmt)\n",  __func__);
 
 		return _FAIL;
 	}
@@ -587,8 +585,8 @@ int rtw_parse_wpa2_ie23a(const u8 *rsn_ie, int rsn_ie_len, int *group_cipher,
 			pos += 2;
 			if (!memcmp(pos, SUITE_1X, 4)) {
 				RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_,
-					 ("%s (): there has 802.1x auth\n",
-					  __func__));
+					 "%s (): there has 802.1x auth\n",
+					 __func__);
 				*is_8021x = 1;
 			}
 		}
@@ -700,7 +698,7 @@ static int rtw_get_cipher_info(struct wlan_network *pnetwork)
 
 	if (pbuf && pbuf[1] > 0) {
 		RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_,
-			 ("rtw_get_cipher_info: wpa_ielen: %d", pbuf[1]));
+			 "rtw_get_cipher_info: wpa_ielen: %d\n", pbuf[1]);
 		r = rtw_parse_wpa_ie23a(pbuf, pbuf[1] + 2, &group_cipher,
 				     &pairwise_cipher, &is8021x);
 		if (r == _SUCCESS) {
@@ -708,10 +706,9 @@ static int rtw_get_cipher_info(struct wlan_network *pnetwork)
 			pnetwork->BcnInfo.group_cipher = group_cipher;
 			pnetwork->BcnInfo.is_8021x = is8021x;
 			RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_,
-				 ("%s: pnetwork->pairwise_cipher: %d, is_"
-				  "8021x is %d", __func__,
-				  pnetwork->BcnInfo.pairwise_cipher,
-				  pnetwork->BcnInfo.is_8021x));
+				 "%s: pnetwork->pairwise_cipher: %d, is_8021x is %d\n",
+				 __func__, pnetwork->BcnInfo.pairwise_cipher,
+				 pnetwork->BcnInfo.is_8021x);
 			ret = _SUCCESS;
 		}
 	} else {
@@ -719,24 +716,23 @@ static int rtw_get_cipher_info(struct wlan_network *pnetwork)
 
 		if (pbuf && pbuf[1] > 0) {
 			RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_,
-				 ("get RSN IE\n"));
+				 "get RSN IE\n");
 			r = rtw_parse_wpa2_ie23a(pbuf, pbuf[1] + 2,
 					      &group_cipher, &pairwise_cipher,
 					      &is8021x);
 			if (r == _SUCCESS) {
 				RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_,
-					 ("get RSN IE  OK!!!\n"));
+					 "get RSN IE  OK!!!\n");
 				pnetwork->BcnInfo.pairwise_cipher =
 					pairwise_cipher;
 				pnetwork->BcnInfo.group_cipher = group_cipher;
 				pnetwork->BcnInfo.is_8021x = is8021x;
 				RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_,
-					 ("%s: pnetwork->pairwise_cipher: %d,"
-					  "pnetwork->group_cipher is %d, "
-					  "is_8021x is %d", __func__,
-					  pnetwork->BcnInfo.pairwise_cipher,
-					  pnetwork->BcnInfo.group_cipher,
-					  pnetwork->BcnInfo.is_8021x));
+					 "%s: pnetwork->pairwise_cipher: %d,pnetwork->group_cipher is %d, is_8021x is %d\n",
+					 __func__,
+					 pnetwork->BcnInfo.pairwise_cipher,
+					 pnetwork->BcnInfo.group_cipher,
+					 pnetwork->BcnInfo.is_8021x);
 				ret = _SUCCESS;
 			}
 		}
@@ -759,7 +755,7 @@ void rtw_get_bcn_info23a(struct wlan_network *pnetwork)
 		pnetwork->BcnInfo.encryp_protocol = ENCRYP_PROTOCOL_OPENSYS;
 
 	RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_,
-		 ("%s: ssid =%s\n", __func__, pnetwork->network.Ssid.ssid));
+		 "%s: ssid =%s\n", __func__, pnetwork->network.Ssid.ssid);
 
 	pie = pnetwork->network.IEs;
 	pie_len = pnetwork->network.IELength;
@@ -776,11 +772,11 @@ void rtw_get_bcn_info23a(struct wlan_network *pnetwork)
 			pnetwork->BcnInfo.encryp_protocol = ENCRYP_PROTOCOL_WEP;
 	}
 	RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_,
-		 ("%s: pnetwork->encryp_protocol is %x\n", __func__,
-		  pnetwork->BcnInfo.encryp_protocol));
+		 "%s: pnetwork->encryp_protocol is %x\n", __func__,
+		 pnetwork->BcnInfo.encryp_protocol);
 	RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_,
-		 ("%s: pnetwork->encryp_protocol is %x\n", __func__,
-		  pnetwork->BcnInfo.encryp_protocol));
+		 "%s: pnetwork->encryp_protocol is %x\n", __func__,
+		 pnetwork->BcnInfo.encryp_protocol);
 	rtw_get_cipher_info(pnetwork);
 
 	/* get bwmode and ch_offset */
diff --git a/drivers/staging/rtl8723au/core/rtw_mlme.c b/drivers/staging/rtl8723au/core/rtw_mlme.c
index 7299ef0a2e54..3c09ea9b7348 100644
--- a/drivers/staging/rtl8723au/core/rtw_mlme.c
+++ b/drivers/staging/rtl8723au/core/rtw_mlme.c
@@ -94,7 +94,7 @@ void rtw23a_free_mlme_priv_ie_data(struct mlme_priv *pmlmepriv)
 void rtw_free_mlme_priv23a(struct mlme_priv *pmlmepriv)
 {
 	RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_,
-		 ("rtw_free_mlme_priv23a\n"));
+		 "rtw_free_mlme_priv23a\n");
 
 	rtw23a_free_mlme_priv_ie_data(pmlmepriv);
 }
@@ -196,9 +196,8 @@ int rtw_if_up23a(struct rtw_adapter *padapter)
 	if (padapter->bDriverStopped || padapter->bSurpriseRemoved ||
 	    !check_fwstate(&padapter->mlmepriv, _FW_LINKED)) {
 		RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_,
-			 ("rtw_if_up23a:bDriverStopped(%d) OR "
-			  "bSurpriseRemoved(%d)", padapter->bDriverStopped,
-			  padapter->bSurpriseRemoved));
+			 "rtw_if_up23a:bDriverStopped(%d) OR bSurpriseRemoved(%d)\n",
+			 padapter->bDriverStopped, padapter->bSurpriseRemoved);
 		res = false;
 	} else
 		res =  true;
@@ -238,9 +237,9 @@ static void _rtw_roaming(struct rtw_adapter *padapter,
 		pnetwork = &pmlmepriv->cur_network;
 
 	if (padapter->mlmepriv.to_roaming > 0) {
-		DBG_8723A("roaming from %s("MAC_FMT"), length:%d\n",
+		DBG_8723A("roaming from %s(%pM), length:%d\n",
 			  pnetwork->network.Ssid.ssid,
-			  MAC_ARG(pnetwork->network.MacAddress),
+			  pnetwork->network.MacAddress,
 			  pnetwork->network.Ssid.ssid_len);
 		memcpy(&pmlmepriv->assoc_ssid, &pnetwork->network.Ssid,
 		       sizeof(struct cfg80211_ssid));
@@ -466,7 +465,7 @@ static void rtw_update_scanned_network(struct rtw_adapter *adapter,
 		if (!pnetwork) {
 			if (!oldest) {
 				RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_,
-					 ("\n\n\nsomething wrong here\n\n\n"));
+					 "something wrong here\n");
 				goto exit;
 			}
 			pnetwork = oldest;
@@ -568,13 +567,6 @@ static int rtw_is_desired_network(struct rtw_adapter *adapter,
 	return bselected;
 }
 
-/* TODO: Perry : For Power Management */
-void rtw_atimdone_event_callback23a(struct rtw_adapter *adapter, const u8 *pbuf)
-{
-	RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_,
-		 ("receive atimdone_evet\n"));
-}
-
 void rtw_survey_event_cb23a(struct rtw_adapter *adapter, const u8 *pbuf)
 {
 	u32 len;
@@ -585,13 +577,12 @@ void rtw_survey_event_cb23a(struct rtw_adapter *adapter, const u8 *pbuf)
 	pnetwork = survey->bss;
 
 	RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_,
-		 ("rtw_survey_event_cb23a, ssid=%s\n", pnetwork->Ssid.ssid));
+		 "rtw_survey_event_cb23a, ssid=%s\n", pnetwork->Ssid.ssid);
 
 	len = get_wlan_bssid_ex_sz(pnetwork);
 	if (len > (sizeof(struct wlan_bssid_ex))) {
 		RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_,
-			 ("\n ****rtw_survey_event_cb23a: return a wrong "
-			  "bss ***\n"));
+			 "****rtw_survey_event_cb23a: return a wrong bss ***\n");
 		return;
 	}
 
@@ -599,8 +590,6 @@ void rtw_survey_event_cb23a(struct rtw_adapter *adapter, const u8 *pbuf)
 
 	/*  update IBSS_network 's timestamp */
 	if (check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE)) {
-		/* RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_,
-		   "rtw_survey_event_cb23a : WIFI_ADHOC_MASTER_STATE\n\n"); */
 		if (ether_addr_equal(pmlmepriv->cur_network.network.MacAddress,
 				     pnetwork->MacAddress)) {
 			struct wlan_network *ibss_wlan;
@@ -660,8 +649,8 @@ rtw_surveydone_event_callback23a(struct rtw_adapter *adapter, const u8 *pbuf)
 	}
 
 	RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_,
-		 ("rtw_surveydone_event_callback23a: fw_state:%x\n\n",
-		  get_fwstate(pmlmepriv)));
+		 "rtw_surveydone_event_callback23a: fw_state:%x\n",
+		 get_fwstate(pmlmepriv));
 
 	if (check_fwstate(pmlmepriv, _FW_UNDER_SURVEY)) {
 		del_timer_sync(&pmlmepriv->scan_to_timer);
@@ -669,8 +658,8 @@ rtw_surveydone_event_callback23a(struct rtw_adapter *adapter, const u8 *pbuf)
 		_clr_fwstate_(pmlmepriv, _FW_UNDER_SURVEY);
 	} else {
 		RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_,
-			 ("nic status =%x, survey done event comes too late!\n",
-			  get_fwstate(pmlmepriv)));
+			 "nic status =%x, survey done event comes too late!\n",
+			 get_fwstate(pmlmepriv));
 	}
 
 	rtw_set_signal_stat_timer(&adapter->recvpriv);
@@ -725,7 +714,7 @@ static void free_scanqueue(struct mlme_priv *pmlmepriv)
 	struct rtw_queue *scan_queue = &pmlmepriv->scanned_queue;
 	struct list_head *plist, *phead, *ptemp;
 
-	RT_TRACE(_module_rtl871x_mlme_c_, _drv_notice_, ("+free_scanqueue\n"));
+	RT_TRACE(_module_rtl871x_mlme_c_, _drv_notice_, "+free_scanqueue\n");
 	spin_lock_bh(&scan_queue->lock);
 
 	phead = get_list_head(scan_queue);
@@ -752,11 +741,11 @@ void rtw_free_assoc_resources23a(struct rtw_adapter *adapter,
 	struct sta_info *psta;
 
 	RT_TRACE(_module_rtl871x_mlme_c_, _drv_notice_,
-		 ("+rtw_free_assoc_resources23a\n"));
+		 "+rtw_free_assoc_resources23a\n");
 	RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_,
-		 ("tgt_network->network.MacAddress="MAC_FMT" ssid=%s\n",
-		  MAC_ARG(tgt_network->network.MacAddress),
-		  tgt_network->network.Ssid.ssid));
+		 "tgt_network->network.MacAddress=%pM ssid=%s\n",
+		 tgt_network->network.MacAddress,
+		 tgt_network->network.Ssid.ssid);
 
 	if (check_fwstate(pmlmepriv, WIFI_STATION_STATE|WIFI_AP_STATE)) {
 		psta = rtw_get_stainfo23a(&adapter->stapriv,
@@ -788,7 +777,7 @@ void rtw_free_assoc_resources23a(struct rtw_adapter *adapter,
 		pwlan->fixed = false;
 	else
 		RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_,
-			 ("rtw_free_assoc_resources23a : pwlan== NULL\n"));
+			 "rtw_free_assoc_resources23a : pwlan== NULL\n");
 
 	if (check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) &&
 	    adapter->stapriv.asoc_sta_count == 1)
@@ -808,7 +797,7 @@ void rtw_indicate_connect23a(struct rtw_adapter *padapter)
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 
 	RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_,
-		 ("+rtw_indicate_connect23a\n"));
+		 "+rtw_indicate_connect23a\n");
 
 	pmlmepriv->to_join = false;
 
@@ -828,8 +817,8 @@ void rtw_indicate_connect23a(struct rtw_adapter *padapter)
 	rtw_set_scan_deny(padapter, 3000);
 
 	RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_,
-		 ("-rtw_indicate_connect23a: fw_state=0x%08x\n",
-		  get_fwstate(pmlmepriv)));
+		 "-rtw_indicate_connect23a: fw_state=0x%08x\n",
+		 get_fwstate(pmlmepriv));
 }
 
 /*
@@ -840,7 +829,7 @@ void rtw_indicate_disconnect23a(struct rtw_adapter *padapter)
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 
 	RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_,
-		 ("+rtw_indicate_disconnect23a\n"));
+		 "+rtw_indicate_disconnect23a\n");
 
 	_clr_fwstate_(pmlmepriv, _FW_UNDER_LINKING|WIFI_UNDER_WPS);
 
@@ -998,8 +987,9 @@ rtw_joinbss_update_network23a(struct rtw_adapter *padapter,
 	DBG_8723A("%s\n", __func__);
 
 	RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_,
-		 ("\nfw_state:%x, BSSID:"MAC_FMT"\n", get_fwstate(pmlmepriv),
-		  MAC_ARG(pnetwork->network.MacAddress)));
+		 "fw_state:%x, BSSID:%pM\n",
+		 get_fwstate(pmlmepriv),
+		 pnetwork->network.MacAddress);
 
 	/*  why not use ptarget_wlan?? */
 	memcpy(&cur_network->network, &pnetwork->network,
@@ -1042,7 +1032,7 @@ rtw_joinbss_update_network23a(struct rtw_adapter *padapter,
 	default:
 		pmlmepriv->fw_state = WIFI_NULL_STATE;
 		RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_,
-			 ("Invalid network_mode\n"));
+			 "Invalid network_mode\n");
 		break;
 	}
 
@@ -1077,16 +1067,16 @@ void rtw_joinbss_event_prehandle23a(struct rtw_adapter *adapter, u8 *pbuf)
 	bool the_same_macaddr;
 
 	RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_,
-		 ("joinbss event call back received with res=%d\n",
-		  pnetwork->join_res));
+		 "joinbss event call back received with res=%d\n",
+		 pnetwork->join_res);
 
 	if (pmlmepriv->assoc_ssid.ssid_len == 0) {
 		RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_,
-			 ("@@@@@   joinbss event call back  for Any SSid\n"));
+			 "@@@@@   joinbss event call back  for Any SSid\n");
 	} else {
 		RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_,
-			 ("@@@@@   rtw23a_joinbss_event_cb for SSid:%s\n",
-			  pmlmepriv->assoc_ssid.ssid));
+			 "@@@@@   rtw23a_joinbss_event_cb for SSid:%s\n",
+			 pmlmepriv->assoc_ssid.ssid);
 	}
 
 	if (ether_addr_equal(pnetwork->network.MacAddress,
@@ -1098,15 +1088,14 @@ void rtw_joinbss_event_prehandle23a(struct rtw_adapter *adapter, u8 *pbuf)
 	pnetwork->network.Length = get_wlan_bssid_ex_sz(&pnetwork->network);
 	if (pnetwork->network.Length > sizeof(struct wlan_bssid_ex)) {
 		RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_,
-			 ("\n\n ***joinbss_evt_callback return a wrong bss "
-			  "***\n\n"));
+			 "***joinbss_evt_callback return a wrong bss ***\n");
 		return;
 	}
 
 	spin_lock_bh(&pmlmepriv->lock);
 
 	RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_,
-		 ("\n rtw23a_joinbss_event_cb !! _enter_critical\n"));
+		 "rtw23a_joinbss_event_cb !! _enter_critical\n");
 
 	if (pnetwork->join_res > 0) {
 		spin_lock_bh(&pmlmepriv->scanned_queue.lock);
@@ -1155,8 +1144,7 @@ void rtw_joinbss_event_prehandle23a(struct rtw_adapter *adapter, u8 *pbuf)
 							      pnetwork);
 			else {
 				RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_,
-					 ("Can't find ptarget_wlan when "
-					  "joinbss_event callback\n"));
+					 "Can't find ptarget_wlan when joinbss_event callback\n");
 				spin_unlock_bh(&pmlmepriv->scanned_queue.lock);
 				goto ignore_joinbss_callback;
 			}
@@ -1169,8 +1157,7 @@ void rtw_joinbss_event_prehandle23a(struct rtw_adapter *adapter, u8 *pbuf)
 				if (!ptarget_sta) {
 					RT_TRACE(_module_rtl871x_mlme_c_,
 						 _drv_err_,
-						 ("Can't update stainfo when "
-						  "joinbss_event callback\n"));
+						 "Can't update stainfo when joinbss_event callback\n");
 					spin_unlock_bh(&pmlmepriv->scanned_queue.lock);
 					goto ignore_joinbss_callback;
 				}
@@ -1183,19 +1170,19 @@ void rtw_joinbss_event_prehandle23a(struct rtw_adapter *adapter, u8 *pbuf)
 				/* adhoc mode will rtw_indicate_connect23a
 				   when rtw_stassoc_event_callback23a */
 				RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_,
-					 ("adhoc mode, fw_state:%x",
-					  get_fwstate(pmlmepriv)));
+					 "adhoc mode, fw_state:%x\n",
+					 get_fwstate(pmlmepriv));
 			}
 
 			/* s5. Cancle assoc_timer */
 			del_timer_sync(&pmlmepriv->assoc_timer);
 
 			RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_,
-				 ("Cancle assoc_timer\n"));
+				 "Cancle assoc_timer\n");
 		} else {
 			RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_,
-				 ("rtw23a_joinbss_event_cb err: fw_state:%x",
-				 get_fwstate(pmlmepriv)));
+				 "rtw23a_joinbss_event_cb err: fw_state:%x\n",
+				 get_fwstate(pmlmepriv));
 			spin_unlock_bh(&pmlmepriv->scanned_queue.lock);
 			goto ignore_joinbss_callback;
 		}
@@ -1209,8 +1196,8 @@ void rtw_joinbss_event_prehandle23a(struct rtw_adapter *adapter, u8 *pbuf)
 
 		if (check_fwstate(pmlmepriv, _FW_UNDER_LINKING)) {
 			RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_,
-				 ("fail! clear _FW_UNDER_LINKING ^^^fw_state="
-				  "%x\n", get_fwstate(pmlmepriv)));
+				 "fail! clear _FW_UNDER_LINKING ^^^fw_state=%x\n",
+				 get_fwstate(pmlmepriv));
 			_clr_fwstate_(pmlmepriv, _FW_UNDER_LINKING);
 		}
 	} else {
@@ -1261,8 +1248,7 @@ void rtw_stassoc_event_callback23a(struct rtw_adapter *adapter, const u8 *pbuf)
 	if (psta != NULL) {
 		/* the sta have been in sta_info_queue => do nothing */
 		RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_,
-			 ("Error: rtw_stassoc_event_callback23a: sta has "
-			  "been in sta_hash_queue\n"));
+			 "Error: rtw_stassoc_event_callback23a: sta has been in sta_hash_queue\n");
 		/* between drv has received this event before and
 		   fw have not yet to set key to CAM_ENTRY) */
 		return;
@@ -1272,8 +1258,7 @@ void rtw_stassoc_event_callback23a(struct rtw_adapter *adapter, const u8 *pbuf)
 		GFP_KERNEL);
 	if (!psta) {
 		RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_,
-			 ("Can't alloc sta_info when "
-			  "rtw_stassoc_event_callback23a\n"));
+			 "Can't alloc sta_info when rtw_stassoc_event_callback23a\n");
 		return;
 	}
 
@@ -1330,8 +1315,7 @@ void rtw_stadel_event_callback23a(struct rtw_adapter *adapter, const u8 *pbuf)
 	else
 		mac_id = pstadel->mac_id;
 
-	DBG_8723A("%s(mac_id=%d)=" MAC_FMT "\n", __func__, mac_id,
-		  MAC_ARG(pstadel->macaddr));
+	DBG_8723A("%s(mac_id=%d)=%pM\n", __func__, mac_id, pstadel->macaddr);
 
 	if (check_fwstate(pmlmepriv, WIFI_AP_STATE))
 		return;
@@ -1582,13 +1566,12 @@ static int rtw_check_join_candidate(struct mlme_priv *pmlmepriv,
 	}
 
 	if (updated) {
-		DBG_8723A("[by_bssid:%u][assoc_ssid:%s][to_roaming:%u] "
-			  "new candidate: %s("MAC_FMT") rssi:%d\n",
+		DBG_8723A("[by_bssid:%u][assoc_ssid:%s][to_roaming:%u] new candidate: %s(%pM) rssi:%d\n",
 			  pmlmepriv->assoc_by_bssid,
 			  pmlmepriv->assoc_ssid.ssid,
 			  adapter->mlmepriv.to_roaming,
 			  (*candidate)->network.Ssid.ssid,
-			  MAC_ARG((*candidate)->network.MacAddress),
+			  (*candidate)->network.MacAddress,
 			  (int)(*candidate)->network.Rssi);
 	}
 
@@ -1653,8 +1636,8 @@ rtw_select_candidate_from_queue(struct mlme_priv *pmlmepriv)
 		pnetwork = container_of(plist, struct wlan_network, list);
 		if (!pnetwork) {
 			RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_,
-				 ("%s: return _FAIL:(pnetwork == NULL)\n",
-				  __func__));
+				 "%s: return _FAIL:(pnetwork == NULL)\n",
+				 __func__);
 			goto exit;
 		}
 
@@ -1680,7 +1663,7 @@ int rtw_do_join_adhoc(struct rtw_adapter *adapter)
 	_clr_fwstate_(pmlmepriv, _FW_UNDER_SURVEY);
 
 	RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_,
-		 ("switching to adhoc master\n"));
+		 "switching to adhoc master\n");
 
 	memcpy(&pdev_network->Ssid, &pmlmepriv->assoc_ssid,
 	       sizeof(struct cfg80211_ssid));
@@ -1693,7 +1676,7 @@ int rtw_do_join_adhoc(struct rtw_adapter *adapter)
 	ret = rtw_createbss_cmd23a(adapter);
 	if (ret != _SUCCESS) {
 		RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_,
-			 ("Error =>rtw_createbss_cmd23a status FAIL\n"));
+			 "Error =>rtw_createbss_cmd23a status FAIL\n");
 	} else  {
 		pmlmepriv->to_join = false;
 	}
@@ -1739,9 +1722,10 @@ int rtw_select_and_join_from_scanned_queue23a(struct mlme_priv *pmlmepriv)
 		ret = _FAIL;
 		goto exit;
 	} else {
-		DBG_8723A("%s: candidate: %s("MAC_FMT", ch:%u)\n", __func__,
+		DBG_8723A("%s: candidate: %s(%pM, ch:%u)\n",
+			  __func__,
 			  candidate->network.Ssid.ssid,
-			  MAC_ARG(candidate->network.MacAddress),
+			  candidate->network.MacAddress,
 			  candidate->network.DSConfig);
 	}
 
@@ -1781,8 +1765,8 @@ int rtw_set_auth23a(struct rtw_adapter *adapter,
 	pcmd->rspsz = 0;
 
 	RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_,
-		 ("after enqueue set_auth_cmd, auth_mode=%x\n",
-		  psecuritypriv->dot11AuthAlgrthm));
+		 "after enqueue set_auth_cmd, auth_mode=%x\n",
+		 psecuritypriv->dot11AuthAlgrthm);
 
 	res = rtw_enqueue_cmd23a(pcmdpriv, pcmd);
 
@@ -1822,15 +1806,13 @@ int rtw_set_key23a(struct rtw_adapter *adapter,
 		psetkeyparm->algorithm = (unsigned char)
 			psecuritypriv->dot118021XGrpPrivacy;
 		RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_,
-			 ("\n rtw_set_key23a: psetkeyparm->algorithm = "
-			  "(unsigned char)psecuritypriv->dot118021XGrpPrivacy "
-			  "=%d\n", psetkeyparm->algorithm));
+			 "rtw_set_key23a: psetkeyparm->algorithm = (unsigned char)psecuritypriv->dot118021XGrpPrivacy =%d\n",
+			 psetkeyparm->algorithm);
 	} else {
 		psetkeyparm->algorithm = (u8)psecuritypriv->dot11PrivacyAlgrthm;
 		RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_,
-			 ("\n rtw_set_key23a: psetkeyparm->algorithm = (u8)"
-			  "psecuritypriv->dot11PrivacyAlgrthm =%d\n",
-			  psetkeyparm->algorithm));
+			 "rtw_set_key23a: psetkeyparm->algorithm = (u8)psecuritypriv->dot11PrivacyAlgrthm =%d\n",
+			 psetkeyparm->algorithm);
 	}
 	psetkeyparm->keyid = keyid;/* 0~3 */
 	psetkeyparm->set_tx = set_tx;
@@ -1841,8 +1823,8 @@ int rtw_set_key23a(struct rtw_adapter *adapter,
 		  psetkeyparm->algorithm, psetkeyparm->keyid,
 		  pmlmepriv->key_mask);
 	RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_,
-		 ("\n rtw_set_key23a: psetkeyparm->algorithm =%d psetkeyparm->"
-		  "keyid = (u8)keyid =%d\n", psetkeyparm->algorithm, keyid));
+		 "rtw_set_key23a: psetkeyparm->algorithm =%d psetkeyparm->keyid = (u8)keyid =%d\n",
+		 psetkeyparm->algorithm, keyid);
 
 	switch (psetkeyparm->algorithm) {
 	case WLAN_CIPHER_SUITE_WEP40:
@@ -1869,9 +1851,8 @@ int rtw_set_key23a(struct rtw_adapter *adapter,
 		break;
 	default:
 		RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_,
-			 ("\n rtw_set_key23a:psecuritypriv->dot11PrivacyAlgrthm"
-			  " = %x (must be 1 or 2 or 4 or 5)\n",
-			  psecuritypriv->dot11PrivacyAlgrthm));
+			 "rtw_set_key23a:psecuritypriv->dot11PrivacyAlgrthm = %x (must be 1 or 2 or 4 or 5)\n",
+			 psecuritypriv->dot11PrivacyAlgrthm);
 		res = _FAIL;
 		kfree(pcmd);
 		kfree(psetkeyparm);
@@ -1998,8 +1979,8 @@ int rtw_restruct_sec_ie23a(struct rtw_adapter *adapter, u8 *in_ie, u8 *out_ie,
 	uint ndissecuritytype = psecuritypriv->ndisencryptstatus;
 
 	RT_TRACE(_module_rtl871x_mlme_c_, _drv_notice_,
-		 ("+rtw_restruct_sec_ie23a: ndisauthmode=%d "
-		  "ndissecuritytype=%d\n", ndisauthmode, ndissecuritytype));
+		 "+rtw_restruct_sec_ie23a: ndisauthmode=%d ndissecuritytype=%d\n",
+		 ndisauthmode, ndissecuritytype);
 
 	ielength = 0;
 	if (ndisauthmode == Ndis802_11AuthModeWPA ||
@@ -2065,8 +2046,8 @@ void rtw_update_registrypriv_dev_network23a(struct rtw_adapter *adapter)
 
 	pdev_network->DSConfig = pregistrypriv->channel;
 	RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_,
-		 ("pregistrypriv->channel =%d, pdev_network->DSConfig = 0x%x\n",
-		  pregistrypriv->channel, pdev_network->DSConfig));
+		 "pregistrypriv->channel =%d, pdev_network->DSConfig = 0x%x\n",
+		 pregistrypriv->channel, pdev_network->DSConfig);
 
 	if (cur_network->network.ifmode == NL80211_IFTYPE_ADHOC)
 		pdev_network->ATIMWindow = 0;
diff --git a/drivers/staging/rtl8723au/core/rtw_mlme_ext.c b/drivers/staging/rtl8723au/core/rtw_mlme_ext.c
index 0e0f73c86e53..196beafde6f0 100644
--- a/drivers/staging/rtl8723au/core/rtw_mlme_ext.c
+++ b/drivers/staging/rtl8723au/core/rtw_mlme_ext.c
@@ -657,8 +657,8 @@ void mgt_dispatcher23a(struct rtw_adapter *padapter,
 
 	if (index > 13) {
 		RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_,
-			 ("Currently we do not support reserved sub-fr-type ="
-			  "%d\n", index));
+			 "Currently we do not support reserved sub-fr-type =%d\n",
+			 index);
 		return;
 	}
 	ptable += index;
@@ -968,8 +968,7 @@ OnAuth23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame)
 	pstat = rtw_get_stainfo23a(pstapriv, sa);
 	if (!pstat) {
 		/*  allocate a new one */
-		DBG_8723A("going to alloc stainfo for sa ="MAC_FMT"\n",
-			  MAC_ARG(sa));
+		DBG_8723A("going to alloc stainfo for sa =%pM\n", sa);
 		pstat = rtw_alloc_stainfo23a(pstapriv, sa, GFP_ATOMIC);
 		if (!pstat) {
 			DBG_8723A(" Exceed the upper limit of supported "
@@ -1396,8 +1395,8 @@ OnAssocReq23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame)
 	/* now parse all ieee802_11 ie to point to elems */
 
 	if (rtw_validate_frame_ies(pos, left)) {
-		DBG_8723A("STA " MAC_FMT " sent invalid association request\n",
-			  MAC_ARG(pstat->hwaddr));
+		DBG_8723A("STA %pM sent invalid association request\n",
+			  pstat->hwaddr);
 		status = WLAN_STATUS_UNSPECIFIED_FAILURE;
 		goto OnAssocReq23aFail;
 	}
@@ -1407,8 +1406,8 @@ OnAssocReq23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame)
 	p = cfg80211_find_ie(WLAN_EID_SSID, pos, left);
 	if (!p || p[1] == 0) {
 		/*  broadcast ssid, however it is not allowed in assocreq */
-		DBG_8723A("STA " MAC_FMT " sent invalid association request "
-			  "lacking an SSID\n", MAC_ARG(pstat->hwaddr));
+		DBG_8723A("STA %pM sent invalid association request lacking an SSID\n",
+			  pstat->hwaddr);
 		status = WLAN_STATUS_UNSPECIFIED_FAILURE;
 		goto OnAssocReq23aFail;
 	} else {
@@ -1537,9 +1536,8 @@ OnAssocReq23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame)
 		int copy_len;
 
 		if (psecuritypriv->wpa_psk == 0) {
-			DBG_8723A("STA " MAC_FMT ": WPA/RSN IE in association "
-			"request, but AP don't support WPA/RSN\n",
-				  MAC_ARG(pstat->hwaddr));
+			DBG_8723A("STA %pM: WPA/RSN IE in association request, but AP don't support WPA/RSN\n",
+				  pstat->hwaddr);
 
 			status = WLAN_STATUS_INVALID_IE;
 
@@ -1614,8 +1612,8 @@ OnAssocReq23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame)
 					else
 						pstat->uapsd_be = 0;
 
+					break;
 				}
-				break;
 			} else {
 				break;
 			}
@@ -1645,8 +1643,8 @@ OnAssocReq23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame)
 	if (pstat->flags & WLAN_STA_HT &&
 	    (pstat->wpa2_pairwise_cipher & WPA_CIPHER_TKIP ||
 	     pstat->wpa_pairwise_cipher & WPA_CIPHER_TKIP)) {
-		DBG_8723A("HT: " MAC_FMT " tried to use TKIP with HT "
-			  "association\n", MAC_ARG(pstat->hwaddr));
+		DBG_8723A("HT: %pM tried to use TKIP with HT association\n",
+			  pstat->hwaddr);
 
 		/* status = WLAN_STATUS_CIPHER_REJECTED_PER_POLICY; */
 		/* goto OnAssocReq23aFail; */
@@ -2687,7 +2685,7 @@ static int _issue_probereq(struct rtw_adapter *padapter,
 	u8 bc_addr[] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
 
 	RT_TRACE(_module_rtl871x_mlme_c_, _drv_notice_,
-		 ("+%s\n", __func__));
+		 "+%s\n", __func__);
 
 	pmgntframe = alloc_mgtxmitframe23a(pxmitpriv);
 	if (!pmgntframe)
@@ -2758,7 +2756,7 @@ static int _issue_probereq(struct rtw_adapter *padapter,
 	pattrib->last_txcmdsz = pattrib->pktlen;
 
 	RT_TRACE(_module_rtl871x_mlme_c_, _drv_notice_,
-		 ("issuing probe_req, tx_len =%d\n", pattrib->last_txcmdsz));
+		 "issuing probe_req, tx_len =%d\n", pattrib->last_txcmdsz);
 
 	if (wait_ack) {
 		ret = dump_mgntframe23a_and_wait_ack23a(padapter, pmgntframe);
@@ -2806,17 +2804,16 @@ static int issue_probereq_ex(struct rtw_adapter *padapter,
 
 	if (try_cnt && wait_ms) {
 		if (da)
-			DBG_8723A("%s(%s): to "MAC_FMT", ch:%u%s, %d/%d "
-				  "in %u ms\n",	__func__,
-				  padapter->pnetdev->name,
-				  MAC_ARG(da), rtw_get_oper_ch23a(padapter),
-				  ret == _SUCCESS?", acked":"", i, try_cnt,
+			DBG_8723A("%s(%s): to %pM, ch:%u%s, %d/%d in %u ms\n",
+				  __func__, padapter->pnetdev->name,
+				  da, rtw_get_oper_ch23a(padapter),
+				  ret == _SUCCESS ? ", acked" : "", i, try_cnt,
 				  jiffies_to_msecs(jiffies - start));
 		else
 			DBG_8723A("%s(%s):, ch:%u%s, %d/%d in %u ms\n",
 				  __func__, padapter->pnetdev->name,
 				  rtw_get_oper_ch23a(padapter),
-				  ret == _SUCCESS?", acked":"", i, try_cnt,
+				  ret == _SUCCESS ? ", acked" : "", i, try_cnt,
 				  jiffies_to_msecs(jiffies - start));
 	}
 exit:
@@ -3460,17 +3457,16 @@ int issue_nulldata23a(struct rtw_adapter *padapter, unsigned char *da,
 
 	if (try_cnt && wait_ms) {
 		if (da)
-			DBG_8723A("%s(%s): to "MAC_FMT", ch:%u%s, %d/%d "
-				  "in %u ms\n", __func__,
-				  padapter->pnetdev->name,
-				  MAC_ARG(da), rtw_get_oper_ch23a(padapter),
-				  ret == _SUCCESS?", acked":"", i, try_cnt,
+			DBG_8723A("%s(%s): to %pM, ch:%u%s, %d/%d in %u ms\n",
+				  __func__, padapter->pnetdev->name,
+				  da, rtw_get_oper_ch23a(padapter),
+				  ret == _SUCCESS ? ", acked" : "", i, try_cnt,
 				  jiffies_to_msecs(jiffies - start));
 		else
 			DBG_8723A("%s(%s):, ch:%u%s, %d/%d in %u ms\n",
 				  __func__, padapter->pnetdev->name,
 				  rtw_get_oper_ch23a(padapter),
-				  ret == _SUCCESS?", acked":"", i, try_cnt,
+				  ret == _SUCCESS ? ", acked" : "", i, try_cnt,
 				  jiffies_to_msecs(jiffies - start));
 	}
 exit:
@@ -3587,17 +3583,16 @@ int issue_qos_nulldata23a(struct rtw_adapter *padapter, unsigned char *da,
 
 	if (try_cnt && wait_ms) {
 		if (da)
-			DBG_8723A("%s(%s): to "MAC_FMT", ch:%u%s, %d/%d "
-				  "in %u ms\n", __func__,
-				  padapter->pnetdev->name,
-				  MAC_ARG(da), rtw_get_oper_ch23a(padapter),
-				  ret == _SUCCESS?", acked":"", i, try_cnt,
+			DBG_8723A("%s(%s): to %pM, ch:%u%s, %d/%d in %u ms\n",
+				  __func__, padapter->pnetdev->name,
+				  da, rtw_get_oper_ch23a(padapter),
+				  ret == _SUCCESS ? ", acked" : "", i, try_cnt,
 				  jiffies_to_msecs(jiffies - start));
 		else
 			DBG_8723A("%s(%s):, ch:%u%s, %d/%d in %u ms\n",
 				  __func__, padapter->pnetdev->name,
 				  rtw_get_oper_ch23a(padapter),
-				  ret == _SUCCESS?", acked":"", i, try_cnt,
+				  ret == _SUCCESS ? ", acked" : "", i, try_cnt,
 				  jiffies_to_msecs(jiffies - start));
 	}
 exit:
@@ -3615,7 +3610,7 @@ static int _issue_deauth(struct rtw_adapter *padapter, unsigned char *da,
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 	int ret = _FAIL;
 
-	/* DBG_8723A("%s to "MAC_FMT"\n", __func__, MAC_ARG(da)); */
+	/* DBG_8723A("%s to %pM\n", __func__, da); */
 
 	pmgntframe = alloc_mgtxmitframe23a(pxmitpriv);
 	if (!pmgntframe)
@@ -3660,7 +3655,7 @@ exit:
 int issue_deauth23a(struct rtw_adapter *padapter, unsigned char *da,
 		    unsigned short reason)
 {
-	DBG_8723A("%s to "MAC_FMT"\n", __func__, MAC_ARG(da));
+	DBG_8723A("%s to %pM\n", __func__, da);
 	return _issue_deauth(padapter, da, reason, false);
 }
 
@@ -3692,17 +3687,16 @@ static int issue_deauth_ex(struct rtw_adapter *padapter, u8 *da,
 
 	if (try_cnt && wait_ms) {
 		if (da)
-			DBG_8723A("%s(%s): to "MAC_FMT", ch:%u%s, %d/%d "
-				  "in %u ms\n", __func__,
-				  padapter->pnetdev->name,
-				  MAC_ARG(da), rtw_get_oper_ch23a(padapter),
-				  ret == _SUCCESS?", acked":"", i, try_cnt,
+			DBG_8723A("%s(%s): to %pM, ch:%u%s, %d/%d in %u ms\n",
+				  __func__, padapter->pnetdev->name,
+				  da, rtw_get_oper_ch23a(padapter),
+				  ret == _SUCCESS ? ", acked" : "", i, try_cnt,
 				  jiffies_to_msecs(jiffies - start));
 		else
 			DBG_8723A("%s(%s):, ch:%u%s, %d/%d in %u ms\n",
 				  __func__, padapter->pnetdev->name,
 				  rtw_get_oper_ch23a(padapter),
-				  ret == _SUCCESS?", acked":"", i, try_cnt,
+				  ret == _SUCCESS ? ", acked" : "", i, try_cnt,
 				  jiffies_to_msecs(jiffies - start));
 	}
 exit:
@@ -3719,8 +3713,8 @@ void issue_action_spct_ch_switch23a(struct rtw_adapter *padapter,
 	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
 	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 
-	DBG_8723A("%s(%s): ra ="MAC_FMT", ch:%u, offset:%u\n", __func__,
-		  padapter->pnetdev->name, MAC_ARG(ra), new_ch, ch_offset);
+	DBG_8723A("%s(%s): ra=%pM, ch:%u, offset:%u\n",
+		  __func__, padapter->pnetdev->name, ra, new_ch, ch_offset);
 
 	pmgntframe = alloc_mgtxmitframe23a(pxmitpriv);
 	if (!pmgntframe)
@@ -3767,7 +3761,6 @@ void issue_action_BA23a(struct rtw_adapter *padapter,
 {
 	u16 start_seq;
 	u16 BA_para_set;
-	u16 BA_timeout_value;
 	u16 BA_starting_seqctrl;
 	u16 BA_para;
 	int max_rx_ampdu_factor;
@@ -3842,16 +3835,14 @@ void issue_action_BA23a(struct rtw_adapter *padapter,
 				IEEE80211_ADDBA_PARAM_BUF_SIZE_MASK;
 		} else {
 			/* immediate ack & 64 buffer size */
-			BA_para_set = (0x1002 | ((status & 0xf) << 2));
+			BA_para_set = 0x1002 | ((status & 0xf) << 2);
 		}
 
 		put_unaligned_le16(BA_para_set,
 				   &mgmt->u.action.u.addba_req.capab);
 
-		BA_timeout_value = 5000;/*  5ms */
-		BA_timeout_value = cpu_to_le16(BA_timeout_value);
-		put_unaligned_le16(BA_timeout_value,
-				   &mgmt->u.action.u.addba_req.timeout);
+		/*  5ms */
+		put_unaligned_le16(5000, &mgmt->u.action.u.addba_req.timeout);
 
 		psta = rtw_get_stainfo23a(pstapriv, raddr);
 		if (psta) {
@@ -4347,7 +4338,8 @@ static void start_create_ibss(struct rtw_adapter *padapter)
 
 		/* issue beacon */
 		if (send_beacon23a(padapter) == _FAIL) {
-			RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("issuing beacon frame fail....\n"));
+			RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_,
+				 "issuing beacon frame fail....\n");
 
 			report_join_res23a(padapter, -1);
 			pmlmeinfo->state = MSR_NOLINK;
@@ -4531,7 +4523,7 @@ static void process_80211d(struct rtw_adapter *padapter,
 
 		p += 3;
 		RT_TRACE(_module_rtl871x_mlme_c_, _drv_notice_,
-			 ("%s: 802.11d country =%s\n", __func__, country));
+			 "%s: 802.11d country =%s\n", __func__, country);
 
 		i = 0;
 		while ((ie - p) >= 3) {
@@ -4708,9 +4700,8 @@ static void process_80211d(struct rtw_adapter *padapter,
 
 				chplan_new[i].ScanType = SCAN_ACTIVE;
 				RT_TRACE(_module_rtl871x_mlme_c_, _drv_notice_,
-					 ("%s: change channel %d scan type "
-					  "from passive to active\n",
-					  __func__, channel));
+					 "%s: change channel %d scan type from passive to active\n",
+					 __func__, channel);
 			}
 			break;
 		}
@@ -4745,7 +4736,7 @@ void report_survey_event23a(struct rtw_adapter *padapter,
 	if (!pcmd_obj)
 		return;
 
-	cmdsz = (sizeof(struct survey_event) + sizeof(struct C2HEvent_Header));
+	cmdsz = sizeof(struct survey_event) + sizeof(struct C2HEvent_Header);
 	pevtcmd = kzalloc(cmdsz, GFP_ATOMIC);
 	if (!pevtcmd) {
 		kfree(pcmd_obj);
@@ -4796,7 +4787,7 @@ void report_surveydone_event23a(struct rtw_adapter *padapter)
 	if (!pcmd_obj)
 		return;
 
-	cmdsz = (sizeof(struct surveydone_event) + sizeof(struct C2HEvent_Header));
+	cmdsz = sizeof(struct surveydone_event) + sizeof(struct C2HEvent_Header);
 	pevtcmd = kzalloc(cmdsz, GFP_ATOMIC);
 	if (!pevtcmd) {
 		kfree(pcmd_obj);
@@ -4840,7 +4831,7 @@ void report_join_res23a(struct rtw_adapter *padapter, int res)
 	if (!pcmd_obj)
 		return;
 
-	cmdsz = (sizeof(struct joinbss_event) + sizeof(struct C2HEvent_Header));
+	cmdsz = sizeof(struct joinbss_event) + sizeof(struct C2HEvent_Header);
 	pevtcmd = kzalloc(cmdsz, GFP_ATOMIC);
 	if (!pevtcmd) {
 		kfree(pcmd_obj);
@@ -4890,7 +4881,7 @@ void report_del_sta_event23a(struct rtw_adapter *padapter,
 	if (!pcmd_obj)
 		return;
 
-	cmdsz = (sizeof(struct stadel_event) + sizeof(struct C2HEvent_Header));
+	cmdsz = sizeof(struct stadel_event) + sizeof(struct C2HEvent_Header);
 	pevtcmd = kzalloc(cmdsz, GFP_ATOMIC);
 	if (!pevtcmd) {
 		kfree(pcmd_obj);
@@ -4918,7 +4909,7 @@ void report_del_sta_event23a(struct rtw_adapter *padapter,
 	if (psta)
 		mac_id = (int)psta->mac_id;
 	else
-		mac_id = (-1);
+		mac_id = -1;
 
 	pdel_sta_evt->mac_id = mac_id;
 
@@ -4944,7 +4935,7 @@ void report_add_sta_event23a(struct rtw_adapter *padapter,
 	if (!pcmd_obj)
 		return;
 
-	cmdsz = (sizeof(struct stassoc_event) + sizeof(struct C2HEvent_Header));
+	cmdsz = sizeof(struct stassoc_event) + sizeof(struct C2HEvent_Header);
 	pevtcmd = kzalloc(cmdsz, GFP_ATOMIC);
 	if (!pevtcmd) {
 		kfree(pcmd_obj);
@@ -5951,7 +5942,7 @@ int set_stakey_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf)
 
 			/* 0~3 for default key, cmd_id = macid + 3,
 			   macid = aid+1; */
-			cam_id = (psta->mac_id + 3);
+			cam_id = psta->mac_id + 3;
 
 			DBG_8723A("Write CAM, mac_addr =%x:%x:%x:%x:%x:%x, "
 				  "cam_entry =%d\n", pparm->addr[0],
@@ -6066,7 +6057,7 @@ int mlme_evt_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf)
 	/*  checking if event code is valid */
 	if (evt_code >= MAX_C2HEVT) {
 		RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_,
-			 ("\nEvent Code(%d) mismatch!\n", evt_code));
+			 "Event Code(%d) mismatch!\n", evt_code);
 		goto _abort_event_;
 	}
 
@@ -6074,8 +6065,8 @@ int mlme_evt_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf)
 	if (wlanevents[evt_code].parmsize != 0 &&
 	    wlanevents[evt_code].parmsize != evt_sz) {
 		RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_,
-			 ("\nEvent(%d) Parm Size mismatch (%d vs %d)!\n",
-			  evt_code, wlanevents[evt_code].parmsize, evt_sz));
+			 "Event(%d) Parm Size mismatch (%d vs %d)!\n",
+			 evt_code, wlanevents[evt_code].parmsize, evt_sz);
 		goto _abort_event_;
 	}
 
diff --git a/drivers/staging/rtl8723au/core/rtw_pwrctrl.c b/drivers/staging/rtl8723au/core/rtw_pwrctrl.c
index e2d51afe522c..7488a104935b 100644
--- a/drivers/staging/rtl8723au/core/rtw_pwrctrl.c
+++ b/drivers/staging/rtl8723au/core/rtw_pwrctrl.c
@@ -202,17 +202,17 @@ void rtw_set_rpwm23a(struct rtw_adapter *padapter, u8 pslv)
 
 	if (pwrpriv->rpwm == pslv) {
 		RT_TRACE(_module_rtl871x_pwrctrl_c_, _drv_err_,
-			("%s: Already set rpwm[0x%02X], new = 0x%02X!\n",
-			 __func__, pwrpriv->rpwm, pslv));
+			 "%s: Already set rpwm[0x%02X], new = 0x%02X!\n",
+			 __func__, pwrpriv->rpwm, pslv);
 		return;
 	}
 
 	if (padapter->bSurpriseRemoved == true ||
 	    padapter->hw_init_completed == false) {
 		RT_TRACE(_module_rtl871x_pwrctrl_c_, _drv_err_,
-			 ("%s: SurpriseRemoved(%d) hw_init_completed(%d)\n",
-			  __func__, padapter->bSurpriseRemoved,
-			  padapter->hw_init_completed));
+			 "%s: SurpriseRemoved(%d) hw_init_completed(%d)\n",
+			 __func__, padapter->bSurpriseRemoved,
+			 padapter->hw_init_completed);
 
 		pwrpriv->cpwm = PS_STATE_S4;
 
@@ -221,22 +221,21 @@ void rtw_set_rpwm23a(struct rtw_adapter *padapter, u8 pslv)
 
 	if (padapter->bDriverStopped == true) {
 		RT_TRACE(_module_rtl871x_pwrctrl_c_, _drv_err_,
-			 ("%s: change power state(0x%02X) when DriverStopped\n",
-			  __func__, pslv));
+			 "%s: change power state(0x%02X) when DriverStopped\n",
+			 __func__, pslv);
 
 		if (pslv < PS_STATE_S2) {
 			RT_TRACE(_module_rtl871x_pwrctrl_c_, _drv_err_,
-				 ("%s: Reject to enter PS_STATE(0x%02X) lower "
-				  "than S2 when DriverStopped!!\n",
-				  __func__, pslv));
+				 "%s: Reject to enter PS_STATE(0x%02X) lower than S2 when DriverStopped!!\n",
+				 __func__, pslv);
 			return;
 		}
 	}
 
 	rpwm = pslv | pwrpriv->tog;
 	RT_TRACE(_module_rtl871x_pwrctrl_c_, _drv_notice_,
-		 ("rtw_set_rpwm23a: rpwm = 0x%02x cpwm = 0x%02x\n",
-		  rpwm, pwrpriv->cpwm));
+		 "rtw_set_rpwm23a: rpwm = 0x%02x cpwm = 0x%02x\n",
+		 rpwm, pwrpriv->cpwm);
 
 	pwrpriv->rpwm = pslv;
 
@@ -282,12 +281,12 @@ void rtw_set_ps_mode23a(struct rtw_adapter *padapter, u8 ps_mode,
 	struct pwrctrl_priv *pwrpriv = &padapter->pwrctrlpriv;
 
 	RT_TRACE(_module_rtl871x_pwrctrl_c_, _drv_notice_,
-			 ("%s: PowerMode =%d Smart_PS =%d\n",
-			  __func__, ps_mode, smart_ps));
+		 "%s: PowerMode =%d Smart_PS =%d\n",
+		 __func__, ps_mode, smart_ps);
 
 	if (ps_mode > PM_Card_Disable) {
 		RT_TRACE(_module_rtl871x_pwrctrl_c_, _drv_err_,
-			 ("ps_mode:%d error\n", ps_mode));
+			 "ps_mode:%d error\n", ps_mode);
 		return;
 	}
 
diff --git a/drivers/staging/rtl8723au/core/rtw_recv.c b/drivers/staging/rtl8723au/core/rtw_recv.c
index 559dddee2648..274a4b65c022 100644
--- a/drivers/staging/rtl8723au/core/rtw_recv.c
+++ b/drivers/staging/rtl8723au/core/rtw_recv.c
@@ -299,12 +299,9 @@ int recvframe_chkmic(struct rtw_adapter *adapter,
 
 	if (prxattrib->encrypt == WLAN_CIPHER_SUITE_TKIP) {
 		RT_TRACE(_module_rtl871x_recv_c_, _drv_info_,
-			 ("\n recvframe_chkmic:prxattrib->encrypt == WLAN_CIPHER_SUITE_TKIP\n"));
+			 "recvframe_chkmic:prxattrib->encrypt == WLAN_CIPHER_SUITE_TKIP\n");
 		RT_TRACE(_module_rtl871x_recv_c_, _drv_info_,
-			 ("\n recvframe_chkmic:da = 0x%02x:0x%02x:0x%02x:0x%02x:"
-			  "0x%02x:0x%02x\n", prxattrib->ra[0],
-			  prxattrib->ra[1], prxattrib->ra[2], prxattrib->ra[3],
-			  prxattrib->ra[4], prxattrib->ra[5]));
+			 "recvframe_chkmic:da = %pM\n", prxattrib->ra);
 
 		/* calculate mic code */
 		if (stainfo != NULL) {
@@ -312,14 +309,13 @@ int recvframe_chkmic(struct rtw_adapter *adapter,
 				mickey = &psecuritypriv->dot118021XGrprxmickey[prxattrib->key_index].skey[0];
 
 				RT_TRACE(_module_rtl871x_recv_c_, _drv_info_,
-					 ("\n recvframe_chkmic: bcmc key\n"));
+					 "recvframe_chkmic: bcmc key\n");
 
 				if (!psecuritypriv->binstallGrpkey) {
 					res = _FAIL;
 					RT_TRACE(_module_rtl871x_recv_c_,
 						 _drv_err_,
-						 ("\n recvframe_chkmic:didn't "
-						  "install group key!!!!!!\n"));
+						 "recvframe_chkmic:didn't install group key!\n");
 					DBG_8723A("\n recvframe_chkmic:didn't "
 						  "install group key!!!!!!\n");
 					goto exit;
@@ -327,8 +323,7 @@ int recvframe_chkmic(struct rtw_adapter *adapter,
 			} else {
 				mickey = &stainfo->dot11tkiprxmickey.skey[0];
 				RT_TRACE(_module_rtl871x_recv_c_, _drv_err_,
-					 ("\n recvframe_chkmic: unicast "
-					  "key\n"));
+					 "recvframe_chkmic: unicast key\n");
 			}
 
 			/* icv_len included the mic code */
@@ -339,9 +334,8 @@ int recvframe_chkmic(struct rtw_adapter *adapter,
 				prxattrib->iv_len;
 
 			RT_TRACE(_module_rtl871x_recv_c_, _drv_info_,
-				 ("\n prxattrib->iv_len =%d prxattrib->icv_len ="
-				  "%d\n", prxattrib->iv_len,
-				  prxattrib->icv_len));
+				 "prxattrib->iv_len =%d prxattrib->icv_len =%d\n",
+				 prxattrib->iv_len, prxattrib->icv_len);
 
 			/* care the length of the data */
 			rtw_seccalctkipmic23a(mickey, pframe, payload,
@@ -356,10 +350,9 @@ int recvframe_chkmic(struct rtw_adapter *adapter,
 				if (miccode[i] != *(pframemic + i)) {
 					RT_TRACE(_module_rtl871x_recv_c_,
 						 _drv_err_,
-						 ("recvframe_chkmic:miccode"
-						  "[%d](%02x) != *(pframemic+"
-						  "%d)(%02x) ", i, miccode[i],
-						  i, *(pframemic + i)));
+						 "recvframe_chkmic:miccode[%d](%02x) != *(pframemic+%d)(%02x)\n",
+						 i, miccode[i],
+						 i, *(pframemic + i));
 					bmic_err = true;
 				}
 			}
@@ -368,51 +361,44 @@ int recvframe_chkmic(struct rtw_adapter *adapter,
 				int i;
 
 				RT_TRACE(_module_rtl871x_recv_c_, _drv_err_,
-					 ("\n *(pframemic-8)-*(pframemic-1) ="
-					  "0x%02x:0x%02x:0x%02x:0x%02x:0x%02x:"
-					  "0x%02x:0x%02x:0x%02x\n",
-					  *(pframemic - 8), *(pframemic - 7),
-					  *(pframemic - 6), *(pframemic - 5),
-					  *(pframemic - 4), *(pframemic - 3),
-					  *(pframemic - 2), *(pframemic - 1)));
+					 "*(pframemic-8)-*(pframemic-1) =0x%02x:0x%02x:0x%02x:0x%02x:0x%02x:0x%02x:0x%02x:0x%02x\n",
+					 *(pframemic - 8), *(pframemic - 7),
+					 *(pframemic - 6), *(pframemic - 5),
+					 *(pframemic - 4), *(pframemic - 3),
+					 *(pframemic - 2), *(pframemic - 1));
 				RT_TRACE(_module_rtl871x_recv_c_, _drv_err_,
-					 ("\n *(pframemic-16)-*(pframemic-9) ="
-					  "0x%02x:0x%02x:0x%02x:0x%02x:0x%02x:"
-					  "0x%02x:0x%02x:0x%02x\n",
-					  *(pframemic - 16), *(pframemic - 15),
-					  *(pframemic - 14), *(pframemic - 13),
-					  *(pframemic - 12), *(pframemic - 11),
-					  *(pframemic - 10), *(pframemic - 9)));
+					 "*(pframemic-16)-*(pframemic-9) =0x%02x:0x%02x:0x%02x:0x%02x:0x%02x:0x%02x:0x%02x:0x%02x\n",
+					 *(pframemic - 16), *(pframemic - 15),
+					 *(pframemic - 14), *(pframemic - 13),
+					 *(pframemic - 12), *(pframemic - 11),
+					 *(pframemic - 10), *(pframemic - 9));
 
 				RT_TRACE(_module_rtl871x_recv_c_, _drv_err_,
-					 ("\n ====== demp packet (len =%d) ======"
-					  "\n", precvframe->pkt->len));
+					 "====== demp packet (len =%d) ======\n",
+					 precvframe->pkt->len);
 				for (i = 0; i < precvframe->pkt->len; i = i + 8) {
 					RT_TRACE(_module_rtl871x_recv_c_,
-						 _drv_err_, ("0x%02x:0x%02x:0x"
-							    "%02x:0x%02x:0x%0"
-							    "2x:0x%02x:0x%02x"
-							    ":0x%02x",
-							    *(precvframe->pkt->data+i),*(precvframe->pkt->data+i+1),
-							    *(precvframe->pkt->data+i+2),*(precvframe->pkt->data+i+3),
-							    *(precvframe->pkt->data+i+4),*(precvframe->pkt->data+i+5),
-							    *(precvframe->pkt->data+i+6),*(precvframe->pkt->data+i+7)));
+						 _drv_err_,
+						 "0x%02x:0x%02x:0x%02x:0x%02x:0x%02x:0x%02x:0x%02x:0x%02x\n",
+						 *(precvframe->pkt->data+i),
+						 *(precvframe->pkt->data+i+1),
+						 *(precvframe->pkt->data+i+2),
+						 *(precvframe->pkt->data+i+3),
+						 *(precvframe->pkt->data+i+4),
+						 *(precvframe->pkt->data+i+5),
+						 *(precvframe->pkt->data+i+6),
+						 *(precvframe->pkt->data+i+7));
 				}
 				RT_TRACE(_module_rtl871x_recv_c_, _drv_err_,
-					 ("\n ====== demp packet end [len =%d]"
-					  "======\n", precvframe->pkt->len));
+					 "====== demp packet end [len =%d]======\n",
+					 precvframe->pkt->len);
 				RT_TRACE(_module_rtl871x_recv_c_, _drv_err_,
-					 ("\n hrdlen =%d,\n",
-					  prxattrib->hdrlen));
+					 "hrdlen =%d\n", prxattrib->hdrlen);
 
 				RT_TRACE(_module_rtl871x_recv_c_, _drv_err_,
-					 ("ra = 0x%.2x 0x%.2x 0x%.2x 0x%.2x 0x%."
-					  "2x 0x%.2x psecuritypriv->"
-					  "binstallGrpkey =%d ",
-					  prxattrib->ra[0], prxattrib->ra[1],
-					  prxattrib->ra[2], prxattrib->ra[3],
-					  prxattrib->ra[4], prxattrib->ra[5],
-					  psecuritypriv->binstallGrpkey));
+					 "ra = %pM psecuritypriv->binstallGrpkey =%d\n",
+					 prxattrib->ra,
+					 psecuritypriv->binstallGrpkey);
 
 				/*  double check key_index for some timing
 				    issue, cannot compare with
@@ -426,16 +412,17 @@ int recvframe_chkmic(struct rtw_adapter *adapter,
 				if ((prxattrib->bdecrypted == true) &&
 				    (brpt_micerror == true)) {
 					rtw_handle_tkip_mic_err23a(adapter, (u8)is_multicast_ether_addr(prxattrib->ra));
-					RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, (" mic error :prxattrib->bdecrypted =%d ", prxattrib->bdecrypted));
+					RT_TRACE(_module_rtl871x_recv_c_, _drv_err_,
+						 "mic error :prxattrib->bdecrypted =%d\n",
+						 prxattrib->bdecrypted);
 					DBG_8723A(" mic error :prxattrib->"
 						  "bdecrypted =%d\n",
 						  prxattrib->bdecrypted);
 				} else {
 					RT_TRACE(_module_rtl871x_recv_c_,
 						 _drv_err_,
-						 (" mic error :prxattrib->"
-						  "bdecrypted =%d ",
-						  prxattrib->bdecrypted));
+						 "mic error :prxattrib->bdecrypted =%d\n",
+						 prxattrib->bdecrypted);
 					DBG_8723A(" mic error :prxattrib->"
 						  "bdecrypted =%d\n",
 						  prxattrib->bdecrypted);
@@ -449,14 +436,12 @@ int recvframe_chkmic(struct rtw_adapter *adapter,
 					psecuritypriv->bcheck_grpkey = 1;
 					RT_TRACE(_module_rtl871x_recv_c_,
 						 _drv_err_,
-						 ("psecuritypriv->bcheck_grp"
-						  "key = true"));
+						 "psecuritypriv->bcheck_grpkey = true\n");
 				}
 			}
 		} else {
 			RT_TRACE(_module_rtl871x_recv_c_, _drv_err_,
-				 ("recvframe_chkmic: rtw_get_stainfo23a =="
-				  "NULL!!!\n"));
+				 "recvframe_chkmic: rtw_get_stainfo23a ==NULL!!!\n");
 		}
 
 		skb_trim(precvframe->pkt, precvframe->pkt->len - 8);
@@ -481,8 +466,8 @@ struct recv_frame *decryptor(struct rtw_adapter *padapter,
 	int res = _SUCCESS;
 
 	RT_TRACE(_module_rtl871x_recv_c_, _drv_info_,
-		 ("prxstat->decrypted =%x prxattrib->encrypt = 0x%03x\n",
-		  prxattrib->bdecrypted, prxattrib->encrypt));
+		 "prxstat->decrypted =%x prxattrib->encrypt = 0x%03x\n",
+		 prxattrib->bdecrypted, prxattrib->encrypt);
 
 	if (prxattrib->encrypt > 0) {
 		u8 *iv = precv_frame->pkt->data + prxattrib->hdrlen;
@@ -565,8 +550,8 @@ static struct recv_frame *portctrl(struct rtw_adapter *adapter,
 	psta = rtw_get_stainfo23a(pstapriv, psta_addr);
 
 	RT_TRACE(_module_rtl871x_recv_c_, _drv_info_,
-		 ("########portctrl:adapter->securitypriv.dot11AuthAlgrthm ="
-		  "%d\n", adapter->securitypriv.dot11AuthAlgrthm));
+		 "########portctrl:adapter->securitypriv.dot11AuthAlgrthm =%d\n",
+		 adapter->securitypriv.dot11AuthAlgrthm);
 
 	prtnframe = precv_frame;
 
@@ -580,8 +565,7 @@ static struct recv_frame *portctrl(struct rtw_adapter *adapter,
 			/* blocked */
 			/* only accept EAPOL frame */
 			RT_TRACE(_module_rtl871x_recv_c_, _drv_info_,
-				 ("########portctrl:psta->ieee8021x_blocked =="
-				  "1\n"));
+				 "########portctrl:psta->ieee8021x_blocked ==1\n");
 
 			if (ether_type != eapol_type) {
 				/* free this frame */
@@ -608,8 +592,8 @@ int recv_decache(struct recv_frame *precv_frame, u8 bretry,
 
 	if (tid > 15) {
 		RT_TRACE(_module_rtl871x_recv_c_, _drv_notice_,
-			 ("recv_decache, (tid>15)! seq_ctrl = 0x%x, tid = 0x%x\n",
-			  seq_ctrl, tid));
+			 "recv_decache, (tid>15)! seq_ctrl = 0x%x, tid = 0x%x\n",
+			 seq_ctrl, tid);
 
 		return _FAIL;
 	}
@@ -617,9 +601,8 @@ int recv_decache(struct recv_frame *precv_frame, u8 bretry,
 	if (1) { /* if (bretry) */
 		if (seq_ctrl == prxcache->tid_rxseq[tid]) {
 			RT_TRACE(_module_rtl871x_recv_c_, _drv_notice_,
-				 ("recv_decache, seq_ctrl = 0x%x, tid = 0x%x, "
-				  "tid_rxseq = 0x%x\n",
-				  seq_ctrl, tid, prxcache->tid_rxseq[tid]));
+				 "recv_decache, seq_ctrl = 0x%x, tid = 0x%x, tid_rxseq = 0x%x\n",
+				 seq_ctrl, tid, prxcache->tid_rxseq[tid]);
 
 			return _FAIL;
 		}
@@ -777,7 +760,7 @@ static int sta2sta_data_frame(struct rtw_adapter *adapter,
 		/*  filter packets that SA is myself or multicast or broadcast */
 		if (ether_addr_equal(myhwaddr, pattrib->src)) {
 			RT_TRACE(_module_rtl871x_recv_c_, _drv_err_,
-				 (" SA == myself\n"));
+				 "SA == myself\n");
 			ret = _FAIL;
 			goto exit;
 		}
@@ -800,8 +783,7 @@ static int sta2sta_data_frame(struct rtw_adapter *adapter,
 		    and DA is my mac-address */
 		if (!ether_addr_equal(pattrib->bssid, pattrib->src)) {
 			RT_TRACE(_module_rtl871x_recv_c_, _drv_err_,
-				 ("bssid != TA under STATION_MODE; drop "
-				  "pkt\n"));
+				 "bssid != TA under STATION_MODE; drop pkt\n");
 			ret = _FAIL;
 			goto exit;
 		}
@@ -843,7 +825,8 @@ static int sta2sta_data_frame(struct rtw_adapter *adapter,
 		*psta = rtw_get_stainfo23a(pstapriv, sta_addr); /*  get ap_info */
 
 	if (*psta == NULL) {
-		RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("can't get psta under sta2sta_data_frame ; drop pkt\n"));
+		RT_TRACE(_module_rtl871x_recv_c_, _drv_err_,
+			 "can't get psta under sta2sta_data_frame ; drop pkt\n");
 		ret = _FAIL;
 		goto exit;
 	}
@@ -879,7 +862,7 @@ int ap2sta_data_frame(struct rtw_adapter *adapter,
 		/* filter packets that SA is myself or multicast or broadcast */
 		if (ether_addr_equal(myhwaddr, pattrib->src)) {
 			RT_TRACE(_module_rtl871x_recv_c_, _drv_err_,
-				 (" SA == myself\n"));
+				 "SA == myself\n");
 			ret = _FAIL;
 			goto exit;
 		}
@@ -887,8 +870,8 @@ int ap2sta_data_frame(struct rtw_adapter *adapter,
 		/*  da should be for me */
 		if (!ether_addr_equal(myhwaddr, pattrib->dst) && !bmcast) {
 			RT_TRACE(_module_rtl871x_recv_c_, _drv_info_,
-				(" ap2sta_data_frame:  compare DA fail; DA ="
-				 MAC_FMT"\n", MAC_ARG(pattrib->dst)));
+				 "ap2sta_data_frame:  compare DA failed; DA=%pM\n",
+				 pattrib->dst);
 			ret = _FAIL;
 			goto exit;
 		}
@@ -898,15 +881,14 @@ int ap2sta_data_frame(struct rtw_adapter *adapter,
 		    ether_addr_equal(mybssid, "\x0\x0\x0\x0\x0\x0") ||
 		    !ether_addr_equal(pattrib->bssid, mybssid)) {
 			RT_TRACE(_module_rtl871x_recv_c_, _drv_info_,
-				(" ap2sta_data_frame:  compare BSSID fail ; "
-				 "BSSID ="MAC_FMT"\n", MAC_ARG(pattrib->bssid)));
+				 "ap2sta_data_frame:  compare BSSID failed; BSSID=%pM\n",
+				 pattrib->bssid);
 			RT_TRACE(_module_rtl871x_recv_c_, _drv_info_,
-				 ("mybssid ="MAC_FMT"\n", MAC_ARG(mybssid)));
+				 "mybssid=%pM\n", mybssid);
 
 			if (!bmcast) {
-				DBG_8723A("issue_deauth23a to the nonassociated "
-					  "ap =" MAC_FMT " for the reason(7)\n",
-					  MAC_ARG(pattrib->bssid));
+				DBG_8723A("issue_deauth23a to the nonassociated ap=%pM for the reason(7)\n",
+					  pattrib->bssid);
 				issue_deauth23a(adapter, pattrib->bssid,
 					     WLAN_REASON_CLASS3_FRAME_FROM_NONASSOC_STA);
 			}
@@ -923,8 +905,7 @@ int ap2sta_data_frame(struct rtw_adapter *adapter,
 
 		if (*psta == NULL) {
 			RT_TRACE(_module_rtl871x_recv_c_, _drv_err_,
-				 ("ap2sta: can't get psta under STATION_MODE ;"
-				  " drop pkt\n"));
+				 "ap2sta: can't get psta under STATION_MODE; drop pkt\n");
 			ret = _FAIL;
 			goto exit;
 		}
@@ -952,7 +933,7 @@ int ap2sta_data_frame(struct rtw_adapter *adapter,
 		*psta = rtw_get_stainfo23a(pstapriv, pattrib->bssid);
 		if (*psta == NULL) {
 			RT_TRACE(_module_rtl871x_recv_c_, _drv_err_,
-				 ("can't get psta under MP_MODE ; drop pkt\n"));
+				 "can't get psta under MP_MODE ; drop pkt\n");
 			ret = _FAIL;
 			goto exit;
 		}
@@ -964,9 +945,8 @@ int ap2sta_data_frame(struct rtw_adapter *adapter,
 		if (ether_addr_equal(myhwaddr, pattrib->dst) && !bmcast) {
 			*psta = rtw_get_stainfo23a(pstapriv, pattrib->bssid);
 			if (*psta == NULL) {
-				DBG_8723A("issue_deauth23a to the ap =" MAC_FMT
-					  " for the reason(7)\n",
-					  MAC_ARG(pattrib->bssid));
+				DBG_8723A("issue_deauth23a to the ap=%pM for the reason(7)\n",
+					  pattrib->bssid);
 
 				issue_deauth23a(adapter, pattrib->bssid,
 					     WLAN_REASON_CLASS3_FRAME_FROM_NONASSOC_STA);
@@ -1010,10 +990,9 @@ int sta2ap_data_frame(struct rtw_adapter *adapter,
 		*psta = rtw_get_stainfo23a(pstapriv, pattrib->src);
 		if (*psta == NULL) {
 			RT_TRACE(_module_rtl871x_recv_c_, _drv_err_,
-				 ("can't get psta under AP_MODE; drop pkt\n"));
-			DBG_8723A("issue_deauth23a to sta =" MAC_FMT
-				  " for the reason(7)\n",
-				  MAC_ARG(pattrib->src));
+				 "can't get psta under AP_MODE; drop pkt\n");
+			DBG_8723A("issue_deauth23a to sta=%pM for the reason(7)\n",
+				  pattrib->src);
 
 			issue_deauth23a(adapter, pattrib->src,
 				     WLAN_REASON_CLASS3_FRAME_FROM_NONASSOC_STA);
@@ -1043,8 +1022,8 @@ int sta2ap_data_frame(struct rtw_adapter *adapter,
 			ret = RTW_RX_HANDLED;
 			goto exit;
 		}
-		DBG_8723A("issue_deauth23a to sta =" MAC_FMT " for the reason(7)\n",
-			  MAC_ARG(pattrib->src));
+		DBG_8723A("issue_deauth23a to sta=%pM for the reason(7)\n",
+			  pattrib->src);
 		issue_deauth23a(adapter, pattrib->src,
 			     WLAN_REASON_CLASS3_FRAME_FROM_NONASSOC_STA);
 		ret = RTW_RX_HANDLED;
@@ -1214,12 +1193,12 @@ static int validate_recv_mgnt_frame(struct rtw_adapter *padapter,
 	/* struct mlme_priv *pmlmepriv = &adapter->mlmepriv; */
 
 	RT_TRACE(_module_rtl871x_recv_c_, _drv_info_,
-		 ("+validate_recv_mgnt_frame\n"));
+		 "+validate_recv_mgnt_frame\n");
 
 	precv_frame = recvframe_chk_defrag23a(padapter, precv_frame);
 	if (precv_frame == NULL) {
 		RT_TRACE(_module_rtl871x_recv_c_, _drv_notice_,
-			 ("%s: fragment packet\n", __func__));
+			 "%s: fragment packet\n", __func__);
 		return _SUCCESS;
 	}
 
@@ -1305,7 +1284,7 @@ static int validate_recv_data_frame(struct rtw_adapter *adapter,
 		ether_addr_copy(pattrib->ra, hdr->addr1);
 		ether_addr_copy(pattrib->ta, hdr->addr2);
 		ret = _FAIL;
-		RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, (" case 3\n"));
+		RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, "case 3\n");
 		break;
 	}
 
@@ -1314,7 +1293,7 @@ static int validate_recv_data_frame(struct rtw_adapter *adapter,
 
 	if (!psta) {
 		RT_TRACE(_module_rtl871x_recv_c_, _drv_err_,
-			 (" after to_fr_ds_chk; psta == NULL\n"));
+			 "after to_fr_ds_chk; psta == NULL\n");
 		ret = _FAIL;
 		goto exit;
 	}
@@ -1357,26 +1336,25 @@ static int validate_recv_data_frame(struct rtw_adapter *adapter,
 	if (recv_decache(precv_frame, bretry, &psta->sta_recvpriv.rxcache) ==
 	    _FAIL) {
 		RT_TRACE(_module_rtl871x_recv_c_, _drv_err_,
-			 ("decache : drop pkt\n"));
+			 "decache : drop pkt\n");
 		ret = _FAIL;
 		goto exit;
 	}
 
 	if (pattrib->privacy) {
 		RT_TRACE(_module_rtl871x_recv_c_, _drv_info_,
-			 ("validate_recv_data_frame:pattrib->privacy =%x\n",
-			 pattrib->privacy));
+			 "validate_recv_data_frame:pattrib->privacy =%x\n",
+			 pattrib->privacy);
 		RT_TRACE(_module_rtl871x_recv_c_, _drv_info_,
-			 ("\n ^^^^^^^^^^^is_multicast_ether_addr"
-			  "(pattrib->ra(0x%02x)) =%d^^^^^^^^^^^^^^^6\n",
-			  pattrib->ra[0],
-			  is_multicast_ether_addr(pattrib->ra)));
+			 "^^^^^^^^^^^is_multicast_ether_addr(pattrib->ra(0x%02x)) =%d^^^^^^^^^^^^^^^6\n",
+			 pattrib->ra[0],
+			 is_multicast_ether_addr(pattrib->ra));
 
 		GET_ENCRY_ALGO(psecuritypriv, psta, pattrib->encrypt,
 			       is_multicast_ether_addr(pattrib->ra));
 
 		RT_TRACE(_module_rtl871x_recv_c_, _drv_info_,
-			 ("\n pattrib->encrypt =%d\n", pattrib->encrypt));
+			 "pattrib->encrypt =%d\n", pattrib->encrypt);
 
 		switch (pattrib->encrypt) {
 		case WLAN_CIPHER_SUITE_WEP40:
@@ -1457,7 +1435,7 @@ static int validate_recv_frame(struct rtw_adapter *adapter,
 	/* add version chk */
 	if (ver != 0) {
 		RT_TRACE(_module_rtl871x_recv_c_, _drv_err_,
-			 ("validate_recv_data_frame fail! (ver!= 0)\n"));
+			 "validate_recv_data_frame fail! (ver!= 0)\n");
 		retval = _FAIL;
 		goto exit;
 	}
@@ -1482,7 +1460,7 @@ static int validate_recv_frame(struct rtw_adapter *adapter,
 		retval = validate_recv_mgnt_frame(adapter, precv_frame);
 		if (retval == _FAIL) {
 			RT_TRACE(_module_rtl871x_recv_c_, _drv_err_,
-				 ("validate_recv_mgnt_frame fail\n"));
+				 "validate_recv_mgnt_frame fail\n");
 		}
 		retval = _FAIL; /*  only data frame return _SUCCESS */
 		break;
@@ -1490,7 +1468,7 @@ static int validate_recv_frame(struct rtw_adapter *adapter,
 		retval = validate_recv_ctrl_frame(adapter, precv_frame);
 		if (retval == _FAIL) {
 			RT_TRACE(_module_rtl871x_recv_c_, _drv_err_,
-				 ("validate_recv_ctrl_frame fail\n"));
+				 "validate_recv_ctrl_frame fail\n");
 		}
 		retval = _FAIL; /*  only data frame return _SUCCESS */
 		break;
@@ -1499,13 +1477,13 @@ static int validate_recv_frame(struct rtw_adapter *adapter,
 		retval = validate_recv_data_frame(adapter, precv_frame);
 		if (retval == _FAIL) {
 			struct recv_priv *precvpriv = &adapter->recvpriv;
-			/* RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("validate_recv_data_frame fail\n")); */
+
 			precvpriv->rx_drop++;
 		}
 		break;
 	default:
 		RT_TRACE(_module_rtl871x_recv_c_, _drv_err_,
-			 ("validate_recv_data_frame fail! type = 0x%x\n", type));
+			 "validate_recv_data_frame fail! type = 0x%x\n", type);
 		retval = _FAIL;
 		break;
 	}
@@ -1552,8 +1530,8 @@ static int wlanhdr_to_ethhdr (struct recv_frame *precvframe)
 	len = skb->len - hdrlen;
 
 	RT_TRACE(_module_rtl871x_recv_c_, _drv_info_,
-		 ("\n === pattrib->hdrlen: %x,  pattrib->iv_len:%x ===\n\n",
-		  pattrib->hdrlen,  pattrib->iv_len));
+		 "=== pattrib->hdrlen: %x,  pattrib->iv_len:%x ===\n",
+		 pattrib->hdrlen,  pattrib->iv_len);
 
 	pattrib->eth_type = eth_type;
 	if (check_fwstate(pmlmepriv, WIFI_MP_STATE)) {
@@ -1665,7 +1643,7 @@ struct recv_frame *recvframe_defrag(struct rtw_adapter *adapter,
 	rtw_free_recvframe23a_queue(defrag_q);
 
 	RT_TRACE(_module_rtl871x_recv_c_, _drv_info_,
-		 ("Performance defrag!!!!!\n"));
+		 "Performance defrag!!!!!\n");
 
 
 
@@ -1736,8 +1714,8 @@ struct recv_frame *recvframe_chk_defrag23a(struct rtw_adapter *padapter,
 			/* spin_unlock(&pdefrag_q->lock); */
 
 			RT_TRACE(_module_rtl871x_recv_c_, _drv_info_,
-				 ("Enqueuq: ismfrag = %d, fragnum = %d\n",
-				  ismfrag, fragnum));
+				 "Enqueuq: ismfrag = %d, fragnum = %d\n",
+				 ismfrag, fragnum);
 
 			prtnframe = NULL;
 
@@ -1747,8 +1725,8 @@ struct recv_frame *recvframe_chk_defrag23a(struct rtw_adapter *padapter,
 			rtw_free_recvframe23a(precv_frame);
 			prtnframe = NULL;
 			RT_TRACE(_module_rtl871x_recv_c_, _drv_err_,
-				 ("Free because pdefrag_q == NULL: ismfrag = "
-				  "%d, fragnum = %d\n", ismfrag, fragnum));
+				 "Free because pdefrag_q == NULL: ismfrag = %d, fragnum = %d\n",
+				 ismfrag, fragnum);
 		}
 	}
 
@@ -1763,8 +1741,8 @@ struct recv_frame *recvframe_chk_defrag23a(struct rtw_adapter *padapter,
 
 			/* call recvframe_defrag to defrag */
 			RT_TRACE(_module_rtl871x_recv_c_, _drv_info_,
-				 ("defrag: ismfrag = %d, fragnum = %d\n",
-				  ismfrag, fragnum));
+				 "defrag: ismfrag = %d, fragnum = %d\n",
+				 ismfrag, fragnum);
 			precv_frame = recvframe_defrag(padapter, pdefrag_q);
 			prtnframe = precv_frame;
 		} else {
@@ -1773,8 +1751,8 @@ struct recv_frame *recvframe_chk_defrag23a(struct rtw_adapter *padapter,
 			rtw_free_recvframe23a(precv_frame);
 			prtnframe = NULL;
 			RT_TRACE(_module_rtl871x_recv_c_, _drv_err_,
-				 ("Free because pdefrag_q == NULL: ismfrag = "
-				  "%d, fragnum = %d\n", ismfrag, fragnum));
+				 "Free because pdefrag_q == NULL: ismfrag = %d, fragnum = %d\n",
+				 ismfrag, fragnum);
 		}
 
 	}
@@ -1783,8 +1761,7 @@ struct recv_frame *recvframe_chk_defrag23a(struct rtw_adapter *padapter,
 		/* after defrag we must check tkip mic code */
 		if (recvframe_chkmic(padapter,  prtnframe) == _FAIL) {
 			RT_TRACE(_module_rtl871x_recv_c_, _drv_err_,
-				 ("recvframe_chkmic(padapter,  prtnframe) =="
-				  "_FAIL\n"));
+				 "recvframe_chkmic(padapter,  prtnframe) ==_FAIL\n");
 			rtw_free_recvframe23a(prtnframe);
 			prtnframe = NULL;
 		}
@@ -1883,7 +1860,6 @@ static int enqueue_reorder_recvframe23a(struct recv_reorder_ctrl *preorder_ctrl,
 			continue;
 		} else if (SN_EQUAL(pnextattrib->seq_num, pattrib->seq_num)) {
 			/* Duplicate entry is found!! Do not insert current entry. */
-			/* RT_TRACE(COMP_RX_REORDER, DBG_TRACE, ("InsertRxReorderList(): Duplicate packet is dropped!! IndicateSeq: %d, NewSeq: %d\n", pTS->RxIndicateSeq, SeqNum)); */
 
 			/* spin_unlock_irqrestore(&ppending_recvframe_queue->lock); */
 			return false;
@@ -1904,7 +1880,6 @@ static int enqueue_reorder_recvframe23a(struct recv_reorder_ctrl *preorder_ctrl,
 	/* spin_unlock_ex(&ppending_recvframe_queue->lock); */
 	/* spin_unlock_irqrestore(&ppending_recvframe_queue->lock); */
 
-	/* RT_TRACE(COMP_RX_REORDER, DBG_TRACE, ("InsertRxReorderList(): Pkt insert into buffer!! IndicateSeq: %d, NewSeq: %d\n", pTS->RxIndicateSeq, SeqNum)); */
 	return true;
 }
 
@@ -1956,10 +1931,9 @@ int recv_indicatepkts_in_order(struct rtw_adapter *padapter,
 
 		if (!SN_LESS(preorder_ctrl->indicate_seq, pattrib->seq_num)) {
 			RT_TRACE(_module_rtl871x_recv_c_, _drv_notice_,
-				 ("recv_indicatepkts_in_order: indicate =%d "
-				  "seq =%d amsdu =%d\n",
-				  preorder_ctrl->indicate_seq,
-				  pattrib->seq_num, pattrib->amsdu));
+				 "recv_indicatepkts_in_order: indicate =%d seq =%d amsdu =%d\n",
+				 preorder_ctrl->indicate_seq,
+				 pattrib->seq_num, pattrib->amsdu);
 
 			plist = plist->next;
 			list_del_init(&prframe->list);
@@ -2021,8 +1995,7 @@ int recv_indicatepkt_reorder(struct rtw_adapter *padapter,
 			if ((padapter->bDriverStopped == false) &&
 			    (padapter->bSurpriseRemoved == false)) {
 				RT_TRACE(_module_rtl871x_recv_c_, _drv_notice_,
-					 ("@@@@  recv_indicatepkt_reorder -"
-					  "recv_func recv_indicatepkt\n"));
+					 "@@@@  recv_indicatepkt_reorder -recv_func recv_indicatepkt\n");
 
 				rtw_recv_indicatepkt23a(padapter, prframe);
 				return _SUCCESS;
@@ -2055,8 +2028,8 @@ int recv_indicatepkt_reorder(struct rtw_adapter *padapter,
 	spin_lock_bh(&ppending_recvframe_queue->lock);
 
 	RT_TRACE(_module_rtl871x_recv_c_, _drv_notice_,
-		 ("recv_indicatepkt_reorder: indicate =%d seq =%d\n",
-		  preorder_ctrl->indicate_seq, pattrib->seq_num));
+		 "recv_indicatepkt_reorder: indicate =%d seq =%d\n",
+		 preorder_ctrl->indicate_seq, pattrib->seq_num);
 
 	/* s2. check if winstart_b(indicate_seq) needs to been updated */
 	if (!check_indicate_seq(preorder_ctrl, pattrib->seq_num)) {
@@ -2149,7 +2122,7 @@ int process_recv_indicatepkts(struct rtw_adapter *padapter,
 		retval = wlanhdr_to_ethhdr(prframe);
 		if (retval != _SUCCESS) {
 			RT_TRACE(_module_rtl871x_recv_c_, _drv_err_,
-				 ("wlanhdr_to_ethhdr: drop pkt\n"));
+				 "wlanhdr_to_ethhdr: drop pkt\n");
 			return retval;
 		}
 
@@ -2157,19 +2130,16 @@ int process_recv_indicatepkts(struct rtw_adapter *padapter,
 		    (padapter->bSurpriseRemoved == false)) {
 			/* indicate this recv_frame */
 			RT_TRACE(_module_rtl871x_recv_c_, _drv_notice_,
-				 ("@@@@ process_recv_indicatepkts- "
-				  "recv_func recv_indicatepkt\n"));
+				 "@@@@ process_recv_indicatepkts- recv_func recv_indicatepkt\n");
 			rtw_recv_indicatepkt23a(padapter, prframe);
 		} else {
 			RT_TRACE(_module_rtl871x_recv_c_, _drv_notice_,
-				 ("@@@@ process_recv_indicatepkts- "
-				  "recv_func free_indicatepkt\n"));
+				 "@@@@ process_recv_indicatepkts- recv_func free_indicatepkt\n");
 
 			RT_TRACE(_module_rtl871x_recv_c_, _drv_notice_,
-				 ("recv_func:bDriverStopped(%d) OR "
-				  "bSurpriseRemoved(%d)",
-				  padapter->bDriverStopped,
-				  padapter->bSurpriseRemoved));
+				 "recv_func:bDriverStopped(%d) OR bSurpriseRemoved(%d)\n",
+				 padapter->bDriverStopped,
+				 padapter->bSurpriseRemoved);
 			retval = _FAIL;
 			return retval;
 		}
@@ -2188,7 +2158,7 @@ static int recv_func_prehandle(struct rtw_adapter *padapter,
 	ret = validate_recv_frame(padapter, rframe);
 	if (ret != _SUCCESS) {
 		RT_TRACE(_module_rtl871x_recv_c_, _drv_info_,
-			 ("recv_func: validate_recv_frame fail! drop pkt\n"));
+			 "recv_func: validate_recv_frame fail! drop pkt\n");
 		rtw_free_recvframe23a(rframe);
 		goto exit;
 	}
@@ -2208,7 +2178,7 @@ static int recv_func_posthandle(struct rtw_adapter *padapter,
 	prframe = decryptor(padapter, prframe);
 	if (prframe == NULL) {
 		RT_TRACE(_module_rtl871x_recv_c_, _drv_err_,
-			 ("decryptor: drop pkt\n"));
+			 "decryptor: drop pkt\n");
 		ret = _FAIL;
 		goto _recv_data_drop;
 	}
@@ -2216,7 +2186,7 @@ static int recv_func_posthandle(struct rtw_adapter *padapter,
 	prframe = recvframe_chk_defrag23a(padapter, prframe);
 	if (!prframe) {
 		RT_TRACE(_module_rtl871x_recv_c_, _drv_err_,
-			 ("recvframe_chk_defrag23a: drop pkt\n"));
+			 "recvframe_chk_defrag23a: drop pkt\n");
 		goto _recv_data_drop;
 	}
 
@@ -2235,7 +2205,7 @@ static int recv_func_posthandle(struct rtw_adapter *padapter,
 	prframe = portctrl(padapter, prframe);
 	if (!prframe) {
 		RT_TRACE(_module_rtl871x_recv_c_, _drv_err_,
-			 ("portctrl: drop pkt\n"));
+			 "portctrl: drop pkt\n");
 		ret = _FAIL;
 		goto _recv_data_drop;
 	}
@@ -2245,7 +2215,7 @@ static int recv_func_posthandle(struct rtw_adapter *padapter,
 	ret = process_recv_indicatepkts(padapter, prframe);
 	if (ret != _SUCCESS) {
 		RT_TRACE(_module_rtl871x_recv_c_, _drv_err_,
-			 ("recv_func: process_recv_indicatepkts fail!\n"));
+			 "recv_func: process_recv_indicatepkts fail!\n");
 		rtw_free_recvframe23a(orig_prframe);/* free this recv_frame */
 		goto _recv_data_drop;
 	}
@@ -2334,8 +2304,8 @@ void rtw_signal_stat_timer_hdl23a(unsigned long data)
 
 	/* update value of signal_strength, rssi, signal_qual */
 	if (!check_fwstate(&adapter->mlmepriv, _FW_UNDER_SURVEY)) {
-		tmp_s = (avg_signal_strength + (_alpha - 1) *
-			 recvpriv->signal_strength);
+		tmp_s = avg_signal_strength + (_alpha - 1) *
+			 recvpriv->signal_strength;
 		if (tmp_s %_alpha)
 			tmp_s = tmp_s / _alpha + 1;
 		else
diff --git a/drivers/staging/rtl8723au/core/rtw_security.c b/drivers/staging/rtl8723au/core/rtw_security.c
index 715a47414bdd..af53c92fc3a2 100644
--- a/drivers/staging/rtl8723au/core/rtw_security.c
+++ b/drivers/staging/rtl8723au/core/rtw_security.c
@@ -31,11 +31,11 @@ struct arc4context {
 
 static void arcfour_init(struct arc4context *parc4ctx, u8 *key, u32 key_len)
 {
-	u32	t, u;
-	u32	keyindex;
-	u32	stateindex;
+	u32 t, u;
+	u32 keyindex;
+	u32 stateindex;
 	u8 *state;
-	u32	counter;
+	u32 counter;
 
 	state = parc4ctx->state;
 	parc4ctx->x = 0;
@@ -55,7 +55,8 @@ static void arcfour_init(struct arc4context *parc4ctx, u8 *key, u32 key_len)
 	}
 
 }
-static u32 arcfour_byte(	struct arc4context	*parc4ctx)
+
+static u32 arcfour_byte(struct arc4context *parc4ctx)
 {
 	u32 x;
 	u32 y;
@@ -75,19 +76,16 @@ static u32 arcfour_byte(	struct arc4context	*parc4ctx)
 	return state[(sx + sy) & 0xff];
 }
 
-static void arcfour_encrypt(	struct arc4context	*parc4ctx,
-	u8 *dest,
-	u8 *src,
-	u32 len)
+static void arcfour_encrypt(struct arc4context *parc4ctx, u8 *dest,
+			    u8 *src, u32 len)
 {
-	u32	i;
+	u32 i;
 
 	for (i = 0; i < len; i++)
 		dest[i] = src[i] ^ (unsigned char)arcfour_byte(parc4ctx);
-
 }
 
-static int bcrc32initialized = 0;
+static int bcrc32initialized;
 static u32 crc32_table[256];
 
 static u8 crc32_reverseBit(u8 data)
@@ -101,46 +99,48 @@ static u8 crc32_reverseBit(u8 data)
 
 static void crc32_init(void)
 {
+	int i, j;
+	u32 c;
+	u8 *p, *p1;
+	u8 k;
 
 	if (bcrc32initialized == 1)
 		return;
-	else{
-		int i, j;
-		u32 c;
-		u8 *p = (u8 *)&c, *p1;
-		u8 k;
-
-		c = 0x12340000;
-
-		for (i = 0; i < 256; ++i) {
-			k = crc32_reverseBit((u8)i);
-			for (c = ((u32)k) << 24, j = 8; j > 0; --j) {
-				c = c & 0x80000000 ? (c << 1) ^ CRC32_POLY : (c << 1);
-			}
-			p1 = (u8 *)&crc32_table[i];
-
-			p1[0] = crc32_reverseBit(p[3]);
-			p1[1] = crc32_reverseBit(p[2]);
-			p1[2] = crc32_reverseBit(p[1]);
-			p1[3] = crc32_reverseBit(p[0]);
-		}
-		bcrc32initialized = 1;
+
+	p = (u8 *) &c;
+	c = 0x12340000;
+
+	for (i = 0; i < 256; ++i) {
+		k = crc32_reverseBit((u8)i);
+
+		for (c = ((u32)k) << 24, j = 8; j > 0; --j)
+			c = c & 0x80000000 ? (c << 1) ^ CRC32_POLY : (c << 1);
+
+		p1 = (u8 *)&crc32_table[i];
+
+		p1[0] = crc32_reverseBit(p[3]);
+		p1[1] = crc32_reverseBit(p[2]);
+		p1[2] = crc32_reverseBit(p[1]);
+		p1[3] = crc32_reverseBit(p[0]);
 	}
+
+	bcrc32initialized = 1;
 }
 
 static u32 getcrc32(u8 *buf, int len)
 {
 	u8 *p;
-	u32  crc;
+	u32 crc;
 
-	if (bcrc32initialized == 0) crc32_init();
+	if (bcrc32initialized == 0)
+		crc32_init();
 
-	crc = 0xffffffff;       /* preload shift register, per CRC-32 spec */
+	crc = 0xffffffff; /* preload shift register, per CRC-32 spec */
 
 	for (p = buf; len > 0; ++p, --len)
-		crc = crc32_table[ (crc ^ *p) & 0xff] ^ (crc >> 8);
+		crc = crc32_table[(crc ^ *p) & 0xff] ^ (crc >> 8);
 
-	return ~crc;    /* transmit complement, per CRC-32 spec */
+	return ~crc; /* transmit complement, per CRC-32 spec */
 }
 
 /* Need to consider the fragment  situation */
@@ -152,7 +152,7 @@ void rtw_wep_encrypt23a(struct rtw_adapter *padapter,
 	struct arc4context mycontext;
 	int curfragnum, length, index;
 	u32 keylength;
-	u8 *pframe, *payload, *iv;    /* wepkey */
+	u8 *pframe, *payload, *iv; /* wepkey */
 	u8 wepkey[16];
 	u8 hw_hdr_offset = 0;
 	struct pkt_attrib *pattrib = &pxmitframe->attrib;
@@ -184,7 +184,7 @@ void rtw_wep_encrypt23a(struct rtw_adapter *padapter,
 		if ((curfragnum + 1) == pattrib->nr_frags) {
 			/* the last fragment */
 			length = pattrib->last_txcmdsz - pattrib->hdrlen -
-				pattrib->iv_len- pattrib->icv_len;
+				pattrib->iv_len - pattrib->icv_len;
 
 			*((u32 *)crc) = cpu_to_le32(getcrc32(payload, length));
 
@@ -210,7 +210,7 @@ void rtw_wep_decrypt23a(struct rtw_adapter *padapter,
 		     struct recv_frame *precvframe)
 {
 	/*  exclude ICV */
-	u8 crc[4];
+	u32 actual_crc, expected_crc;
 	struct arc4context mycontext;
 	int length;
 	u32 keylength;
@@ -243,19 +243,14 @@ void rtw_wep_decrypt23a(struct rtw_adapter *padapter,
 	arcfour_encrypt(&mycontext, payload, payload, length);
 
 	/* calculate icv and compare the icv */
-	*((u32 *)crc) = le32_to_cpu(getcrc32(payload, length - 4));
+	actual_crc = le32_to_cpu(getcrc32(payload, length - 4));
+	expected_crc = le32_to_cpu(get_unaligned_le32(&payload[length - 4]));
 
-	if (crc[3] != payload[length - 1] || crc[2] != payload[length - 2] ||
-	    crc[1] != payload[length - 3] || crc[0] != payload[length - 4]) {
+	if (actual_crc != expected_crc) {
 		RT_TRACE(_module_rtl871x_security_c_, _drv_err_,
-			 ("rtw_wep_decrypt23a:icv error crc[3](%x)!= payload"
-			  "[length-1](%x) || crc[2](%x)!= payload[length-2](%x)"
-			  " || crc[1](%x)!= payload[length-3](%x) || crc[0](%x)"
-			  "!= payload[length-4](%x)\n",
-			  crc[3], payload[length - 1],
-			  crc[2], payload[length - 2],
-			  crc[1], payload[length - 3],
-			  crc[0], payload[length - 4]));
+			 "%s:icv CRC mismatch: "
+			 "actual: %08x, expected: %08x\n",
+			 __func__, actual_crc, expected_crc);
 	}
 }
 
@@ -267,9 +262,8 @@ static u32 secmicgetuint32(u8 *p)
 	s32 i;
 	u32 res = 0;
 
-	for (i = 0; i<4; i++) {
-		res |= ((u32)(*p++)) << (8*i);
-	}
+	for (i = 0; i < 4; i++)
+		res |= ((u32)(*p++)) << (8 * i);
 
 	return res;
 }
@@ -279,7 +273,7 @@ static void secmicputuint32(u8 *p, u32 val)
 {
 	long i;
 
-	for (i = 0; i<4; i++) {
+	for (i = 0; i < 4; i++) {
 		*p++ = (u8) (val & 0xff);
 		val >>= 8;
 	}
@@ -312,7 +306,7 @@ void rtw_secmicappend23abyte23a(struct mic_data *pmicdata, u8 b)
 {
 
 	/*  Append the byte to our word-sized buffer */
-	pmicdata->M |= ((unsigned long)b) << (8*pmicdata->nBytesInM);
+	pmicdata->M |= ((unsigned long)b) << (8 * pmicdata->nBytesInM);
 	pmicdata->nBytesInM++;
 	/*  Process the word if it is full. */
 	if (pmicdata->nBytesInM >= 4) {
@@ -336,7 +330,7 @@ void rtw_secmicappend23a(struct mic_data *pmicdata, u8 *src, u32 nbytes)
 {
 
 	/*  This is simple */
-	while(nbytes > 0) {
+	while (nbytes > 0) {
 		rtw_secmicappend23abyte23a(pmicdata, *src++);
 		nbytes--;
 	}
@@ -353,12 +347,11 @@ void rtw_secgetmic23a(struct mic_data *pmicdata, u8 *dst)
 	rtw_secmicappend23abyte23a(pmicdata, 0);
 	rtw_secmicappend23abyte23a(pmicdata, 0);
 	/*  and then zeroes until the length is a multiple of 4 */
-	while(pmicdata->nBytesInM != 0) {
+	while (pmicdata->nBytesInM != 0)
 		rtw_secmicappend23abyte23a(pmicdata, 0);
-	}
 	/*  The appendByte function has already computed the result. */
 	secmicputuint32(dst, pmicdata->L);
-	secmicputuint32(dst+4, pmicdata->R);
+	secmicputuint32(dst + 4, pmicdata->R);
 	/*  Reset to the empty message. */
 	secmicclear(pmicdata);
 
@@ -368,23 +361,22 @@ void rtw_seccalctkipmic23a(u8 *key, u8 *header, u8 *data, u32 data_len,
 			   u8 *mic_code, u8 pri)
 {
 
-	struct mic_data	micdata;
-	u8 priority[4]={0x0, 0x0, 0x0, 0x0};
+	struct mic_data micdata;
+	u8 priority[4] = {0x0, 0x0, 0x0, 0x0};
 
 	rtw_secmicsetkey23a(&micdata, key);
-	priority[0]= pri;
+	priority[0] = pri;
 
 	/* Michael MIC pseudo header: DA, SA, 3 x 0, Priority */
-	if (header[1]&1) {   /* ToDS == 1 */
-			rtw_secmicappend23a(&micdata, &header[16], 6);  /* DA */
-		if (header[1]&2)  /* From Ds == 1 */
+	if (header[1]&1) { /* ToDS == 1 */
+			rtw_secmicappend23a(&micdata, &header[16], 6); /* DA */
+		if (header[1]&2) /* From Ds == 1 */
 			rtw_secmicappend23a(&micdata, &header[24], 6);
 		else
 			rtw_secmicappend23a(&micdata, &header[10], 6);
-	}
-	else{	/* ToDS == 0 */
-		rtw_secmicappend23a(&micdata, &header[4], 6);   /* DA */
-		if (header[1]&2)  /* From Ds == 1 */
+	} else { /* ToDS == 0 */
+		rtw_secmicappend23a(&micdata, &header[4], 6); /* DA */
+		if (header[1]&2) /* From Ds == 1 */
 			rtw_secmicappend23a(&micdata, &header[16], 6);
 		else
 			rtw_secmicappend23a(&micdata, &header[10], 6);
@@ -403,11 +395,11 @@ void rtw_seccalctkipmic23a(u8 *key, u8 *header, u8 *data, u32 data_len,
 #define   Lo8(v16)   ((u8)((v16)       & 0x00FF))
 #define   Hi8(v16)   ((u8)(((v16) >> 8) & 0x00FF))
 #define  Lo16(v32)   ((u16)((v32)       & 0xFFFF))
-#define  Hi16(v32)   ((u16)(((v32) >>16) & 0xFFFF))
+#define  Hi16(v32)   ((u16)(((v32) >> 16) & 0xFFFF))
 #define  Mk16(hi, lo) ((lo) ^ (((u16)(hi)) << 8))
 
-/* select the Nth 16-bit word of the temporal key unsigned char array TK[]   */
-#define  TK16(N)     Mk16(tk[2*(N)+1], tk[2*(N)])
+/* select the Nth 16-bit word of the temporal key unsigned char array TK[] */
+#define  TK16(N)     Mk16(tk[2 * (N) + 1], tk[2 * (N)])
 
 /* S-box lookup: 16 bits --> 16 bits */
 #define _S_(v16)     (Sbox1[0][Lo8(v16)] ^ Sbox1[1][Hi8(v16)])
@@ -420,76 +412,76 @@ void rtw_seccalctkipmic23a(u8 *key, u8 *header, u8 *data, u32 data_len,
 #define RC4_KEY_SIZE     16    /* 128-bit RC4KEY (104 bits unknown) */
 
 /* 2-unsigned char by 2-unsigned char subset of the full AES S-box table */
-static const unsigned short Sbox1[2][256]=       /* Sbox for hash (can be in ROM)     */
-{ {
-   0xC6A5, 0xF884, 0xEE99, 0xF68D, 0xFF0D, 0xD6BD, 0xDEB1, 0x9154,
-   0x6050, 0x0203, 0xCEA9, 0x567D, 0xE719, 0xB562, 0x4DE6, 0xEC9A,
-   0x8F45, 0x1F9D, 0x8940, 0xFA87, 0xEF15, 0xB2EB, 0x8EC9, 0xFB0B,
-   0x41EC, 0xB367, 0x5FFD, 0x45EA, 0x23BF, 0x53F7, 0xE496, 0x9B5B,
-   0x75C2, 0xE11C, 0x3DAE, 0x4C6A, 0x6C5A, 0x7E41, 0xF502, 0x834F,
-   0x685C, 0x51F4, 0xD134, 0xF908, 0xE293, 0xAB73, 0x6253, 0x2A3F,
-   0x080C, 0x9552, 0x4665, 0x9D5E, 0x3028, 0x37A1, 0x0A0F, 0x2FB5,
-   0x0E09, 0x2436, 0x1B9B, 0xDF3D, 0xCD26, 0x4E69, 0x7FCD, 0xEA9F,
-   0x121B, 0x1D9E, 0x5874, 0x342E, 0x362D, 0xDCB2, 0xB4EE, 0x5BFB,
-   0xA4F6, 0x764D, 0xB761, 0x7DCE, 0x527B, 0xDD3E, 0x5E71, 0x1397,
-   0xA6F5, 0xB968, 0x0000, 0xC12C, 0x4060, 0xE31F, 0x79C8, 0xB6ED,
-   0xD4BE, 0x8D46, 0x67D9, 0x724B, 0x94DE, 0x98D4, 0xB0E8, 0x854A,
-   0xBB6B, 0xC52A, 0x4FE5, 0xED16, 0x86C5, 0x9AD7, 0x6655, 0x1194,
-   0x8ACF, 0xE910, 0x0406, 0xFE81, 0xA0F0, 0x7844, 0x25BA, 0x4BE3,
-   0xA2F3, 0x5DFE, 0x80C0, 0x058A, 0x3FAD, 0x21BC, 0x7048, 0xF104,
-   0x63DF, 0x77C1, 0xAF75, 0x4263, 0x2030, 0xE51A, 0xFD0E, 0xBF6D,
-   0x814C, 0x1814, 0x2635, 0xC32F, 0xBEE1, 0x35A2, 0x88CC, 0x2E39,
-   0x9357, 0x55F2, 0xFC82, 0x7A47, 0xC8AC, 0xBAE7, 0x322B, 0xE695,
-   0xC0A0, 0x1998, 0x9ED1, 0xA37F, 0x4466, 0x547E, 0x3BAB, 0x0B83,
-   0x8CCA, 0xC729, 0x6BD3, 0x283C, 0xA779, 0xBCE2, 0x161D, 0xAD76,
-   0xDB3B, 0x6456, 0x744E, 0x141E, 0x92DB, 0x0C0A, 0x486C, 0xB8E4,
-   0x9F5D, 0xBD6E, 0x43EF, 0xC4A6, 0x39A8, 0x31A4, 0xD337, 0xF28B,
-   0xD532, 0x8B43, 0x6E59, 0xDAB7, 0x018C, 0xB164, 0x9CD2, 0x49E0,
-   0xD8B4, 0xACFA, 0xF307, 0xCF25, 0xCAAF, 0xF48E, 0x47E9, 0x1018,
-   0x6FD5, 0xF088, 0x4A6F, 0x5C72, 0x3824, 0x57F1, 0x73C7, 0x9751,
-   0xCB23, 0xA17C, 0xE89C, 0x3E21, 0x96DD, 0x61DC, 0x0D86, 0x0F85,
-   0xE090, 0x7C42, 0x71C4, 0xCCAA, 0x90D8, 0x0605, 0xF701, 0x1C12,
-   0xC2A3, 0x6A5F, 0xAEF9, 0x69D0, 0x1791, 0x9958, 0x3A27, 0x27B9,
-   0xD938, 0xEB13, 0x2BB3, 0x2233, 0xD2BB, 0xA970, 0x0789, 0x33A7,
-   0x2DB6, 0x3C22, 0x1592, 0xC920, 0x8749, 0xAAFF, 0x5078, 0xA57A,
-   0x038F, 0x59F8, 0x0980, 0x1A17, 0x65DA, 0xD731, 0x84C6, 0xD0B8,
-   0x82C3, 0x29B0, 0x5A77, 0x1E11, 0x7BCB, 0xA8FC, 0x6DD6, 0x2C3A,
-  },
-
-  {  /* second half of table is unsigned char-reversed version of first! */
-   0xA5C6, 0x84F8, 0x99EE, 0x8DF6, 0x0DFF, 0xBDD6, 0xB1DE, 0x5491,
-   0x5060, 0x0302, 0xA9CE, 0x7D56, 0x19E7, 0x62B5, 0xE64D, 0x9AEC,
-   0x458F, 0x9D1F, 0x4089, 0x87FA, 0x15EF, 0xEBB2, 0xC98E, 0x0BFB,
-   0xEC41, 0x67B3, 0xFD5F, 0xEA45, 0xBF23, 0xF753, 0x96E4, 0x5B9B,
-   0xC275, 0x1CE1, 0xAE3D, 0x6A4C, 0x5A6C, 0x417E, 0x02F5, 0x4F83,
-   0x5C68, 0xF451, 0x34D1, 0x08F9, 0x93E2, 0x73AB, 0x5362, 0x3F2A,
-   0x0C08, 0x5295, 0x6546, 0x5E9D, 0x2830, 0xA137, 0x0F0A, 0xB52F,
-   0x090E, 0x3624, 0x9B1B, 0x3DDF, 0x26CD, 0x694E, 0xCD7F, 0x9FEA,
-   0x1B12, 0x9E1D, 0x7458, 0x2E34, 0x2D36, 0xB2DC, 0xEEB4, 0xFB5B,
-   0xF6A4, 0x4D76, 0x61B7, 0xCE7D, 0x7B52, 0x3EDD, 0x715E, 0x9713,
-   0xF5A6, 0x68B9, 0x0000, 0x2CC1, 0x6040, 0x1FE3, 0xC879, 0xEDB6,
-   0xBED4, 0x468D, 0xD967, 0x4B72, 0xDE94, 0xD498, 0xE8B0, 0x4A85,
-   0x6BBB, 0x2AC5, 0xE54F, 0x16ED, 0xC586, 0xD79A, 0x5566, 0x9411,
-   0xCF8A, 0x10E9, 0x0604, 0x81FE, 0xF0A0, 0x4478, 0xBA25, 0xE34B,
-   0xF3A2, 0xFE5D, 0xC080, 0x8A05, 0xAD3F, 0xBC21, 0x4870, 0x04F1,
-   0xDF63, 0xC177, 0x75AF, 0x6342, 0x3020, 0x1AE5, 0x0EFD, 0x6DBF,
-   0x4C81, 0x1418, 0x3526, 0x2FC3, 0xE1BE, 0xA235, 0xCC88, 0x392E,
-   0x5793, 0xF255, 0x82FC, 0x477A, 0xACC8, 0xE7BA, 0x2B32, 0x95E6,
-   0xA0C0, 0x9819, 0xD19E, 0x7FA3, 0x6644, 0x7E54, 0xAB3B, 0x830B,
-   0xCA8C, 0x29C7, 0xD36B, 0x3C28, 0x79A7, 0xE2BC, 0x1D16, 0x76AD,
-   0x3BDB, 0x5664, 0x4E74, 0x1E14, 0xDB92, 0x0A0C, 0x6C48, 0xE4B8,
-   0x5D9F, 0x6EBD, 0xEF43, 0xA6C4, 0xA839, 0xA431, 0x37D3, 0x8BF2,
-   0x32D5, 0x438B, 0x596E, 0xB7DA, 0x8C01, 0x64B1, 0xD29C, 0xE049,
-   0xB4D8, 0xFAAC, 0x07F3, 0x25CF, 0xAFCA, 0x8EF4, 0xE947, 0x1810,
-   0xD56F, 0x88F0, 0x6F4A, 0x725C, 0x2438, 0xF157, 0xC773, 0x5197,
-   0x23CB, 0x7CA1, 0x9CE8, 0x213E, 0xDD96, 0xDC61, 0x860D, 0x850F,
-   0x90E0, 0x427C, 0xC471, 0xAACC, 0xD890, 0x0506, 0x01F7, 0x121C,
-   0xA3C2, 0x5F6A, 0xF9AE, 0xD069, 0x9117, 0x5899, 0x273A, 0xB927,
-   0x38D9, 0x13EB, 0xB32B, 0x3322, 0xBBD2, 0x70A9, 0x8907, 0xA733,
-   0xB62D, 0x223C, 0x9215, 0x20C9, 0x4987, 0xFFAA, 0x7850, 0x7AA5,
-   0x8F03, 0xF859, 0x8009, 0x171A, 0xDA65, 0x31D7, 0xC684, 0xB8D0,
-   0xC382, 0xB029, 0x775A, 0x111E, 0xCB7B, 0xFCA8, 0xD66D, 0x3A2C,
-  }
+static const unsigned short Sbox1[2][256] = {
+	/* Sbox for hash (can be in ROM) */
+	{
+		0xC6A5, 0xF884, 0xEE99, 0xF68D, 0xFF0D, 0xD6BD, 0xDEB1, 0x9154,
+		0x6050, 0x0203, 0xCEA9, 0x567D, 0xE719, 0xB562, 0x4DE6, 0xEC9A,
+		0x8F45, 0x1F9D, 0x8940, 0xFA87, 0xEF15, 0xB2EB, 0x8EC9, 0xFB0B,
+		0x41EC, 0xB367, 0x5FFD, 0x45EA, 0x23BF, 0x53F7, 0xE496, 0x9B5B,
+		0x75C2, 0xE11C, 0x3DAE, 0x4C6A, 0x6C5A, 0x7E41, 0xF502, 0x834F,
+		0x685C, 0x51F4, 0xD134, 0xF908, 0xE293, 0xAB73, 0x6253, 0x2A3F,
+		0x080C, 0x9552, 0x4665, 0x9D5E, 0x3028, 0x37A1, 0x0A0F, 0x2FB5,
+		0x0E09, 0x2436, 0x1B9B, 0xDF3D, 0xCD26, 0x4E69, 0x7FCD, 0xEA9F,
+		0x121B, 0x1D9E, 0x5874, 0x342E, 0x362D, 0xDCB2, 0xB4EE, 0x5BFB,
+		0xA4F6, 0x764D, 0xB761, 0x7DCE, 0x527B, 0xDD3E, 0x5E71, 0x1397,
+		0xA6F5, 0xB968, 0x0000, 0xC12C, 0x4060, 0xE31F, 0x79C8, 0xB6ED,
+		0xD4BE, 0x8D46, 0x67D9, 0x724B, 0x94DE, 0x98D4, 0xB0E8, 0x854A,
+		0xBB6B, 0xC52A, 0x4FE5, 0xED16, 0x86C5, 0x9AD7, 0x6655, 0x1194,
+		0x8ACF, 0xE910, 0x0406, 0xFE81, 0xA0F0, 0x7844, 0x25BA, 0x4BE3,
+		0xA2F3, 0x5DFE, 0x80C0, 0x058A, 0x3FAD, 0x21BC, 0x7048, 0xF104,
+		0x63DF, 0x77C1, 0xAF75, 0x4263, 0x2030, 0xE51A, 0xFD0E, 0xBF6D,
+		0x814C, 0x1814, 0x2635, 0xC32F, 0xBEE1, 0x35A2, 0x88CC, 0x2E39,
+		0x9357, 0x55F2, 0xFC82, 0x7A47, 0xC8AC, 0xBAE7, 0x322B, 0xE695,
+		0xC0A0, 0x1998, 0x9ED1, 0xA37F, 0x4466, 0x547E, 0x3BAB, 0x0B83,
+		0x8CCA, 0xC729, 0x6BD3, 0x283C, 0xA779, 0xBCE2, 0x161D, 0xAD76,
+		0xDB3B, 0x6456, 0x744E, 0x141E, 0x92DB, 0x0C0A, 0x486C, 0xB8E4,
+		0x9F5D, 0xBD6E, 0x43EF, 0xC4A6, 0x39A8, 0x31A4, 0xD337, 0xF28B,
+		0xD532, 0x8B43, 0x6E59, 0xDAB7, 0x018C, 0xB164, 0x9CD2, 0x49E0,
+		0xD8B4, 0xACFA, 0xF307, 0xCF25, 0xCAAF, 0xF48E, 0x47E9, 0x1018,
+		0x6FD5, 0xF088, 0x4A6F, 0x5C72, 0x3824, 0x57F1, 0x73C7, 0x9751,
+		0xCB23, 0xA17C, 0xE89C, 0x3E21, 0x96DD, 0x61DC, 0x0D86, 0x0F85,
+		0xE090, 0x7C42, 0x71C4, 0xCCAA, 0x90D8, 0x0605, 0xF701, 0x1C12,
+		0xC2A3, 0x6A5F, 0xAEF9, 0x69D0, 0x1791, 0x9958, 0x3A27, 0x27B9,
+		0xD938, 0xEB13, 0x2BB3, 0x2233, 0xD2BB, 0xA970, 0x0789, 0x33A7,
+		0x2DB6, 0x3C22, 0x1592, 0xC920, 0x8749, 0xAAFF, 0x5078, 0xA57A,
+		0x038F, 0x59F8, 0x0980, 0x1A17, 0x65DA, 0xD731, 0x84C6, 0xD0B8,
+		0x82C3, 0x29B0, 0x5A77, 0x1E11, 0x7BCB, 0xA8FC, 0x6DD6, 0x2C3A,
+	},
+	{ /* second half of table is unsigned char-reversed version of first! */
+		0xA5C6, 0x84F8, 0x99EE, 0x8DF6, 0x0DFF, 0xBDD6, 0xB1DE, 0x5491,
+		0x5060, 0x0302, 0xA9CE, 0x7D56, 0x19E7, 0x62B5, 0xE64D, 0x9AEC,
+		0x458F, 0x9D1F, 0x4089, 0x87FA, 0x15EF, 0xEBB2, 0xC98E, 0x0BFB,
+		0xEC41, 0x67B3, 0xFD5F, 0xEA45, 0xBF23, 0xF753, 0x96E4, 0x5B9B,
+		0xC275, 0x1CE1, 0xAE3D, 0x6A4C, 0x5A6C, 0x417E, 0x02F5, 0x4F83,
+		0x5C68, 0xF451, 0x34D1, 0x08F9, 0x93E2, 0x73AB, 0x5362, 0x3F2A,
+		0x0C08, 0x5295, 0x6546, 0x5E9D, 0x2830, 0xA137, 0x0F0A, 0xB52F,
+		0x090E, 0x3624, 0x9B1B, 0x3DDF, 0x26CD, 0x694E, 0xCD7F, 0x9FEA,
+		0x1B12, 0x9E1D, 0x7458, 0x2E34, 0x2D36, 0xB2DC, 0xEEB4, 0xFB5B,
+		0xF6A4, 0x4D76, 0x61B7, 0xCE7D, 0x7B52, 0x3EDD, 0x715E, 0x9713,
+		0xF5A6, 0x68B9, 0x0000, 0x2CC1, 0x6040, 0x1FE3, 0xC879, 0xEDB6,
+		0xBED4, 0x468D, 0xD967, 0x4B72, 0xDE94, 0xD498, 0xE8B0, 0x4A85,
+		0x6BBB, 0x2AC5, 0xE54F, 0x16ED, 0xC586, 0xD79A, 0x5566, 0x9411,
+		0xCF8A, 0x10E9, 0x0604, 0x81FE, 0xF0A0, 0x4478, 0xBA25, 0xE34B,
+		0xF3A2, 0xFE5D, 0xC080, 0x8A05, 0xAD3F, 0xBC21, 0x4870, 0x04F1,
+		0xDF63, 0xC177, 0x75AF, 0x6342, 0x3020, 0x1AE5, 0x0EFD, 0x6DBF,
+		0x4C81, 0x1418, 0x3526, 0x2FC3, 0xE1BE, 0xA235, 0xCC88, 0x392E,
+		0x5793, 0xF255, 0x82FC, 0x477A, 0xACC8, 0xE7BA, 0x2B32, 0x95E6,
+		0xA0C0, 0x9819, 0xD19E, 0x7FA3, 0x6644, 0x7E54, 0xAB3B, 0x830B,
+		0xCA8C, 0x29C7, 0xD36B, 0x3C28, 0x79A7, 0xE2BC, 0x1D16, 0x76AD,
+		0x3BDB, 0x5664, 0x4E74, 0x1E14, 0xDB92, 0x0A0C, 0x6C48, 0xE4B8,
+		0x5D9F, 0x6EBD, 0xEF43, 0xA6C4, 0xA839, 0xA431, 0x37D3, 0x8BF2,
+		0x32D5, 0x438B, 0x596E, 0xB7DA, 0x8C01, 0x64B1, 0xD29C, 0xE049,
+		0xB4D8, 0xFAAC, 0x07F3, 0x25CF, 0xAFCA, 0x8EF4, 0xE947, 0x1810,
+		0xD56F, 0x88F0, 0x6F4A, 0x725C, 0x2438, 0xF157, 0xC773, 0x5197,
+		0x23CB, 0x7CA1, 0x9CE8, 0x213E, 0xDD96, 0xDC61, 0x860D, 0x850F,
+		0x90E0, 0x427C, 0xC471, 0xAACC, 0xD890, 0x0506, 0x01F7, 0x121C,
+		0xA3C2, 0x5F6A, 0xF9AE, 0xD069, 0x9117, 0x5899, 0x273A, 0xB927,
+		0x38D9, 0x13EB, 0xB32B, 0x3322, 0xBBD2, 0x70A9, 0x8907, 0xA733,
+		0xB62D, 0x223C, 0x9215, 0x20C9, 0x4987, 0xFFAA, 0x7850, 0x7AA5,
+		0x8F03, 0xF859, 0x8009, 0x171A, 0xDA65, 0x31D7, 0xC684, 0xB8D0,
+		0xC382, 0xB029, 0x775A, 0x111E, 0xCB7B, 0xFCA8, 0xD66D, 0x3A2C,
+	}
 };
 
  /*
@@ -513,7 +505,7 @@ static void phase1(u16 *p1k, const u8 *tk, const u8 *ta, u32 iv32)
 {
 	int  i;
 
-	/* Initialize the 80 bits of P1K[] from IV32 and TA[0..5]     */
+	/* Initialize the 80 bits of P1K[] from IV32 and TA[0..5] */
 	p1k[0]      = Lo16(iv32);
 	p1k[1]      = Hi16(iv32);
 	p1k[2]      = Mk16(ta[1], ta[0]); /* use TA[] as little-endian */
@@ -522,14 +514,14 @@ static void phase1(u16 *p1k, const u8 *tk, const u8 *ta, u32 iv32)
 
 	/* Now compute an unbalanced Feistel cipher with 80-bit block */
 	/* size on the 80-bit block P1K[], using the 128-bit key TK[] */
-	for (i = 0; i < PHASE1_LOOP_CNT ;i++) {
+	for (i = 0; i < PHASE1_LOOP_CNT; i++) {
 		/* Each add operation here is mod 2**16 */
-		p1k[0] += _S_(p1k[4] ^ TK16((i&1)+0));
-		p1k[1] += _S_(p1k[0] ^ TK16((i&1)+2));
-		p1k[2] += _S_(p1k[1] ^ TK16((i&1)+4));
-		p1k[3] += _S_(p1k[2] ^ TK16((i&1)+6));
-		p1k[4] += _S_(p1k[3] ^ TK16((i&1)+0));
-		p1k[4] +=  (unsigned short)i;                    /* avoid "slide attacks" */
+		p1k[0] += _S_(p1k[4] ^ TK16((i & 1) + 0));
+		p1k[1] += _S_(p1k[0] ^ TK16((i & 1) + 2));
+		p1k[2] += _S_(p1k[1] ^ TK16((i & 1) + 4));
+		p1k[3] += _S_(p1k[2] ^ TK16((i & 1) + 6));
+		p1k[4] += _S_(p1k[3] ^ TK16((i & 1) + 0));
+		p1k[4] +=  (unsigned short) i; /* avoid "slide attacks" */
 		}
 
 }
@@ -560,42 +552,44 @@ static void phase1(u16 *p1k, const u8 *tk, const u8 *ta, u32 iv32)
 static void phase2(u8 *rc4key, const u8 *tk, const u16 *p1k, u16 iv16)
 {
 	int  i;
-	u16 PPK[6];                          /* temporary key for mixing    */
+	u16 PPK[6]; /* temporary key for mixing    */
+
+	/* Note: all adds in the PPK[] equations below are mod 2**16 */
+	for (i = 0; i < 5; i++)
+		PPK[i] = p1k[i]; /* first, copy P1K to PPK */
 
-	/* Note: all adds in the PPK[] equations below are mod 2**16         */
-	for (i = 0;i<5;i++) PPK[i]= p1k[i];      /* first, copy P1K to PPK      */
-		PPK[5]  =  p1k[4] +iv16;             /* next,  add in IV16          */
+	PPK[5] = p1k[4] + iv16; /* next,  add in IV16 */
 
-	/* Bijective non-linear mixing of the 96 bits of PPK[0..5]           */
-	PPK[0] +=    _S_(PPK[5] ^ TK16(0));   /* Mix key in each "round"     */
-	PPK[1] +=    _S_(PPK[0] ^ TK16(1));
-	PPK[2] +=    _S_(PPK[1] ^ TK16(2));
-	PPK[3] +=    _S_(PPK[2] ^ TK16(3));
-	PPK[4] +=    _S_(PPK[3] ^ TK16(4));
-	PPK[5] +=    _S_(PPK[4] ^ TK16(5));   /* Total # S-box lookups == 6  */
+	/* Bijective non-linear mixing of the 96 bits of PPK[0..5] */
+	PPK[0] += _S_(PPK[5] ^ TK16(0)); /* Mix key in each "round" */
+	PPK[1] += _S_(PPK[0] ^ TK16(1));
+	PPK[2] += _S_(PPK[1] ^ TK16(2));
+	PPK[3] += _S_(PPK[2] ^ TK16(3));
+	PPK[4] += _S_(PPK[3] ^ TK16(4));
+	PPK[5] += _S_(PPK[4] ^ TK16(5)); /* Total # S-box lookups == 6 */
 
-	/* Final sweep: bijective, "linear". Rotates kill LSB correlations   */
+	/* Final sweep: bijective, "linear". Rotates kill LSB correlations */
 	PPK[0] +=  RotR1(PPK[5] ^ TK16(6));
-	PPK[1] +=  RotR1(PPK[0] ^ TK16(7));   /* Use all of TK[] in Phase2   */
+	PPK[1] +=  RotR1(PPK[0] ^ TK16(7)); /* Use all of TK[] in Phase2 */
 	PPK[2] +=  RotR1(PPK[1]);
 	PPK[3] +=  RotR1(PPK[2]);
 	PPK[4] +=  RotR1(PPK[3]);
 	PPK[5] +=  RotR1(PPK[4]);
 	/* Note: At this point, for a given key TK[0..15], the 96-bit output */
 	/*       value PPK[0..5] is guaranteed to be unique, as a function   */
-	/*       of the 96-bit "input" value   {TA, IV32, IV16}. That is, P1K  */
-	/*       is now a keyed permutation of {TA, IV32, IV16}.               */
+	/*       of the 96-bit "input" value   {TA, IV32, IV16}. That is,    */
+	/*       P1K is now a keyed permutation of {TA, IV32, IV16}.         */
 
 	/* Set RC4KEY[0..3], which includes "cleartext" portion of RC4 key   */
-	rc4key[0] = Hi8(iv16);                /* RC4KEY[0..2] is the WEP IV  */
-	rc4key[1] = (Hi8(iv16) | 0x20) & 0x7F; /* Help avoid weak (FMS) keys  */
+	rc4key[0] = Hi8(iv16);                 /* RC4KEY[0..2] is the WEP IV */
+	rc4key[1] = (Hi8(iv16) | 0x20) & 0x7F; /* Help avoid weak (FMS) keys */
 	rc4key[2] = Lo8(iv16);
 	rc4key[3] = Lo8((PPK[5] ^ TK16(0)) >> 1);
 
-	/* Copy 96 bits of PPK[0..5] to RC4KEY[4..15]  (little-endian)       */
-	for (i = 0;i<6;i++) {
-		rc4key[4+2*i] = Lo8(PPK[i]);
-		rc4key[5+2*i] = Hi8(PPK[i]);
+	/* Copy 96 bits of PPK[0..5] to RC4KEY[4..15]  (little-endian) */
+	for (i = 0; i < 6; i++) {
+		rc4key[4 + 2 * i] = Lo8(PPK[i]);
+		rc4key[5 + 2 * i] = Hi8(PPK[i]);
 	}
 
 }
@@ -604,97 +598,107 @@ static void phase2(u8 *rc4key, const u8 *tk, const u16 *p1k, u16 iv16)
 int rtw_tkip_encrypt23a(struct rtw_adapter *padapter,
 			struct xmit_frame *pxmitframe)
 {
-	u16	pnl;
-	u32	pnh;
-	u8	rc4key[16];
-	u8   ttkey[16];
-	u8	crc[4];
-	u8   hw_hdr_offset = 0;
+	u16 pnl;
+	u32 pnh;
+	u8 rc4key[16];
+	u8 ttkey[16];
+	u8 crc[4];
+	u8 hw_hdr_offset = 0;
 	struct arc4context mycontext;
-	int			curfragnum, length;
-	u32	prwskeylen;
-	u8	*pframe, *payload, *iv, *prwskey;
+	int curfragnum, length;
+	u32 prwskeylen;
+	u8 *pframe, *payload, *iv, *prwskey;
 	union pn48 dot11txpn;
-	struct	sta_info		*stainfo;
-	struct	pkt_attrib	 *pattrib = &pxmitframe->attrib;
-	struct	security_priv	*psecuritypriv = &padapter->securitypriv;
-	struct	xmit_priv		*pxmitpriv = &padapter->xmitpriv;
+	struct sta_info *stainfo;
+	struct pkt_attrib *pattrib = &pxmitframe->attrib;
+	struct security_priv *psecuritypriv = &padapter->securitypriv;
+	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
 	int res = _SUCCESS;
 
+	if (pattrib->encrypt != WLAN_CIPHER_SUITE_TKIP)
+		return _FAIL;
+
 	if (!pxmitframe->buf_addr)
 		return _FAIL;
 
 	hw_hdr_offset = TXDESC_OFFSET;
 
 	pframe = pxmitframe->buf_addr + hw_hdr_offset;
-	/* 4 start to encrypt each fragment */
-	if (pattrib->encrypt == WLAN_CIPHER_SUITE_TKIP) {
-		if (pattrib->psta)
-			stainfo = pattrib->psta;
-		else {
-			DBG_8723A("%s, call rtw_get_stainfo()\n", __func__);
-			stainfo = rtw_get_stainfo23a(&padapter->stapriv,
-						     &pattrib->ra[0]);
-		}
 
-		if (stainfo!= NULL) {
+	if (pattrib->psta)
+		stainfo = pattrib->psta;
+	else {
+		DBG_8723A("%s, call rtw_get_stainfo()\n", __func__);
+		stainfo = rtw_get_stainfo23a(&padapter->stapriv,
+					     &pattrib->ra[0]);
+	}
 
-			if (!(stainfo->state &_FW_LINKED)) {
-				DBG_8723A("%s, psta->state(0x%x) != _FW_LINKED\n", __func__, stainfo->state);
-				return _FAIL;
-			}
+	if (stainfo == NULL) {
+		RT_TRACE(_module_rtl871x_security_c_, _drv_err_,
+			 "%s: stainfo == NULL!!!\n", __func__);
+		DBG_8723A("%s, psta == NUL\n", __func__);
+		return _FAIL;
+	}
 
-			RT_TRACE(_module_rtl871x_security_c_, _drv_err_, ("rtw_tkip_encrypt23a: stainfo!= NULL!!!\n"));
+	RT_TRACE(_module_rtl871x_security_c_, _drv_err_,
+		 "%s: stainfo!= NULL!!!\n", __func__);
 
-			if (is_multicast_ether_addr(pattrib->ra))
-				prwskey = psecuritypriv->dot118021XGrpKey[psecuritypriv->dot118021XGrpKeyid].skey;
-			else
-				prwskey = &stainfo->dot118021x_UncstKey.skey[0];
+	if (!(stainfo->state & _FW_LINKED)) {
+		DBG_8723A("%s, psta->state(0x%x) != _FW_LINKED\n", __func__, stainfo->state);
+		return _FAIL;
+	}
 
-			prwskeylen = 16;
+	if (is_multicast_ether_addr(pattrib->ra))
+		prwskey = psecuritypriv->dot118021XGrpKey[psecuritypriv->dot118021XGrpKeyid].skey;
+	else
+		prwskey = &stainfo->dot118021x_UncstKey.skey[0];
 
-			for (curfragnum = 0;curfragnum<pattrib->nr_frags;curfragnum++) {
-				iv = pframe+pattrib->hdrlen;
-				payload = pframe+pattrib->iv_len+pattrib->hdrlen;
+	prwskeylen = 16;
 
-				GET_TKIP_PN(iv, dot11txpn);
+	/* 4 start to encrypt each fragment */
+	for (curfragnum = 0; curfragnum < pattrib->nr_frags; curfragnum++) {
+		iv = pframe + pattrib->hdrlen;
+		payload = pframe + pattrib->iv_len + pattrib->hdrlen;
 
-				pnl = (u16)(dot11txpn.val);
-				pnh = (u32)(dot11txpn.val>>16);
+		GET_TKIP_PN(iv, dot11txpn);
 
-				phase1((u16 *)&ttkey[0], prwskey,&pattrib->ta[0], pnh);
+		pnl = (u16)(dot11txpn.val);
+		pnh = (u32)(dot11txpn.val>>16);
 
-				phase2(&rc4key[0], prwskey, (u16 *)&ttkey[0], pnl);
+		phase1((u16 *)&ttkey[0], prwskey, &pattrib->ta[0], pnh);
 
-				if ((curfragnum+1) == pattrib->nr_frags) {	/* 4 the last fragment */
-					length = pattrib->last_txcmdsz-pattrib->hdrlen-pattrib->iv_len- pattrib->icv_len;
-					RT_TRACE(_module_rtl871x_security_c_, _drv_info_, ("pattrib->iv_len =%x, pattrib->icv_len =%x\n", pattrib->iv_len, pattrib->icv_len));
-					*((u32 *)crc) = cpu_to_le32(getcrc32(payload, length));/* modified by Amy*/
+		phase2(&rc4key[0], prwskey, (u16 *)&ttkey[0], pnl);
 
-					arcfour_init(&mycontext, rc4key, 16);
-					arcfour_encrypt(&mycontext, payload, payload, length);
-					arcfour_encrypt(&mycontext, payload+length, crc, 4);
+		if ((curfragnum + 1) == pattrib->nr_frags) { /* 4 the last fragment */
+			length = (pattrib->last_txcmdsz -
+				  pattrib->hdrlen -
+				  pattrib->iv_len -
+				  pattrib->icv_len);
 
-				}
-				else{
-					length = pxmitpriv->frag_len-pattrib->hdrlen-pattrib->iv_len-pattrib->icv_len ;
-					*((u32 *)crc) = cpu_to_le32(getcrc32(payload, length));/* modified by Amy*/
-					arcfour_init(&mycontext, rc4key, 16);
-					arcfour_encrypt(&mycontext, payload, payload, length);
-					arcfour_encrypt(&mycontext, payload+length, crc, 4);
+			RT_TRACE(_module_rtl871x_security_c_, _drv_info_,
+				 "pattrib->iv_len =%x, pattrib->icv_len =%x\n",
+				 pattrib->iv_len,
+				 pattrib->icv_len);
+			*((u32 *)crc) = cpu_to_le32(getcrc32(payload, length));
 
-				pframe+= pxmitpriv->frag_len;
-				pframe = PTR_ALIGN(pframe, 4);
-				}
-			}
+			arcfour_init(&mycontext, rc4key, 16);
+			arcfour_encrypt(&mycontext, payload, payload, length);
+			arcfour_encrypt(&mycontext, payload + length, crc, 4);
 
-		}
-		else{
-			RT_TRACE(_module_rtl871x_security_c_, _drv_err_, ("rtw_tkip_encrypt23a: stainfo == NULL!!!\n"));
-			DBG_8723A("%s, psta == NUL\n", __func__);
-			res = _FAIL;
-		}
+		} else {
+			length = (pxmitpriv->frag_len -
+				  pattrib->hdrlen -
+				  pattrib->iv_len -
+				  pattrib->icv_len);
 
+			*((u32 *)crc) = cpu_to_le32(getcrc32(payload, length));
+			arcfour_init(&mycontext, rc4key, 16);
+			arcfour_encrypt(&mycontext, payload, payload, length);
+			arcfour_encrypt(&mycontext, payload + length, crc, 4);
+
+			pframe += pxmitpriv->frag_len;
+			pframe  = PTR_ALIGN(pframe, 4);
+		}
 	}
 
 	return res;
@@ -706,72 +710,76 @@ int rtw_tkip_decrypt23a(struct rtw_adapter *padapter,
 {
 	u16 pnl;
 	u32 pnh;
-	u8   rc4key[16];
-	u8   ttkey[16];
-	u8	crc[4];
+	u8 rc4key[16];
+	u8 ttkey[16];
+	u32 actual_crc, expected_crc;
 	struct arc4context mycontext;
-	int	length;
-	u32	prwskeylen;
-	u8	*pframe, *payload, *iv, *prwskey;
+	int length;
+	u32 prwskeylen;
+	u8 *pframe, *payload, *iv, *prwskey;
 	union pn48 dot11txpn;
-	struct	sta_info		*stainfo;
-	struct	rx_pkt_attrib *prxattrib = &precvframe->attrib;
-	struct	security_priv *psecuritypriv = &padapter->securitypriv;
+	struct sta_info *stainfo;
+	struct rx_pkt_attrib *prxattrib = &precvframe->attrib;
+	struct security_priv *psecuritypriv = &padapter->securitypriv;
 	struct sk_buff *skb = precvframe->pkt;
 	int res = _SUCCESS;
 
+	if (prxattrib->encrypt != WLAN_CIPHER_SUITE_TKIP)
+		return _FAIL;
+
 	pframe = skb->data;
 
+	stainfo = rtw_get_stainfo23a(&padapter->stapriv,
+				     &prxattrib->ta[0]);
+	if (stainfo == NULL) {
+		RT_TRACE(_module_rtl871x_security_c_, _drv_err_,
+			 "%s: stainfo == NULL!!!\n", __func__);
+		return _FAIL;
+	}
+
 	/* 4 start to decrypt recvframe */
-	if (prxattrib->encrypt == WLAN_CIPHER_SUITE_TKIP) {
+	if (is_multicast_ether_addr(prxattrib->ra)) {
+		if (psecuritypriv->binstallGrpkey == 0) {
+			res = _FAIL;
+			DBG_8723A("%s:rx bc/mc packets, but didn't install group key!!!!!!!!!!\n", __func__);
+			goto exit;
+		}
+		prwskey = psecuritypriv->dot118021XGrpKey[prxattrib->key_index].skey;
+		prwskeylen = 16;
+	} else {
+		RT_TRACE(_module_rtl871x_security_c_, _drv_err_,
+			 "%s: stainfo!= NULL!!!\n", __func__);
+		prwskey = &stainfo->dot118021x_UncstKey.skey[0];
+		prwskeylen = 16;
+	}
 
-		stainfo = rtw_get_stainfo23a(&padapter->stapriv,
-					     &prxattrib->ta[0]);
-		if (stainfo!= NULL) {
-
-			if (is_multicast_ether_addr(prxattrib->ra)) {
-				if (psecuritypriv->binstallGrpkey == 0) {
-					res = _FAIL;
-					DBG_8723A("%s:rx bc/mc packets, but didn't install group key!!!!!!!!!!\n", __func__);
-					goto exit;
-				}
-				prwskey = psecuritypriv->dot118021XGrpKey[prxattrib->key_index].skey;
-				prwskeylen = 16;
-			} else {
-			        RT_TRACE(_module_rtl871x_security_c_, _drv_err_, ("rtw_tkip_decrypt23a: stainfo!= NULL!!!\n"));
-				prwskey = &stainfo->dot118021x_UncstKey.skey[0];
-				prwskeylen = 16;
-			}
-
-			iv = pframe+prxattrib->hdrlen;
-			payload = pframe+prxattrib->iv_len+prxattrib->hdrlen;
-			length = skb->len - prxattrib->hdrlen-prxattrib->iv_len;
-
-			GET_TKIP_PN(iv, dot11txpn);
-
-			pnl = (u16)(dot11txpn.val);
-			pnh = (u32)(dot11txpn.val>>16);
-
-			phase1((u16 *)&ttkey[0], prwskey,&prxattrib->ta[0], pnh);
-			phase2(&rc4key[0], prwskey, (unsigned short *)&ttkey[0], pnl);
-
-			/* 4 decrypt payload include icv */
-			arcfour_init(&mycontext, rc4key, 16);
-			arcfour_encrypt(&mycontext, payload, payload, length);
+	iv = pframe + prxattrib->hdrlen;
+	payload = pframe + prxattrib->iv_len + prxattrib->hdrlen;
+	length = skb->len - prxattrib->hdrlen - prxattrib->iv_len;
 
-			*((u32 *)crc) = le32_to_cpu(getcrc32(payload, length-4));
+	GET_TKIP_PN(iv, dot11txpn);
 
-			if (crc[3]!= payload[length-1] || crc[2]!= payload[length-2] || crc[1]!= payload[length-3] || crc[0]!= payload[length-4])
-			{
-			    RT_TRACE(_module_rtl871x_security_c_, _drv_err_, ("rtw_wep_decrypt23a:icv error crc[3](%x)!= payload[length-1](%x) || crc[2](%x)!= payload[length-2](%x) || crc[1](%x)!= payload[length-3](%x) || crc[0](%x)!= payload[length-4](%x)\n",
-						crc[3], payload[length-1], crc[2], payload[length-2], crc[1], payload[length-3], crc[0], payload[length-4]));
-				res = _FAIL;
-			}
-		} else {
-			RT_TRACE(_module_rtl871x_security_c_, _drv_err_, ("rtw_tkip_decrypt23a: stainfo == NULL!!!\n"));
-			res = _FAIL;
-		}
+	pnl = (u16)(dot11txpn.val);
+	pnh = (u32)(dot11txpn.val>>16);
+
+	phase1((u16 *)&ttkey[0], prwskey, &prxattrib->ta[0], pnh);
+	phase2(&rc4key[0], prwskey, (unsigned short *)&ttkey[0], pnl);
+
+	/* 4 decrypt payload include icv */
+	arcfour_init(&mycontext, rc4key, 16);
+	arcfour_encrypt(&mycontext, payload, payload, length);
+
+	actual_crc = le32_to_cpu(getcrc32(payload, length - 4));
+	expected_crc = le32_to_cpu(get_unaligned_le32(&payload[length - 4]));
+
+	if (actual_crc != expected_crc) {
+		RT_TRACE(_module_rtl871x_security_c_, _drv_err_,
+			 "%s:icv CRC mismatch: "
+			 "actual: %08x, expected: %08x\n",
+			 __func__, actual_crc, expected_crc);
+		res = _FAIL;
 	}
+
 exit:
 	return res;
 }
@@ -829,7 +837,7 @@ static void xor_128(u8 *a, u8 *b, u8 *out)
 {
 	int i;
 
-	for (i = 0;i<16; i++)
+	for (i = 0; i < 16; i++)
 		out[i] = a[i] ^ b[i];
 }
 
@@ -850,8 +858,7 @@ static void next_key(u8 *key, int round)
 {
 	u8 rcon;
 	u8 sbox_key[4];
-	u8 rcon_table[12] =
-	{
+	u8 rcon_table[12] = {
 		0x01, 0x02, 0x04, 0x08, 0x10, 0x20, 0x40, 0x80,
 		0x1b, 0x36, 0x36, 0x36
 	};
@@ -876,10 +883,8 @@ static void byte_sub(u8 *in, u8 *out)
 {
 	int i;
 
-	for (i = 0; i< 16; i++) {
+	for (i = 0; i < 16; i++)
 		out[i] = sbox(in[i]);
-	}
-
 }
 
 static void shift_row(u8 *in, u8 *out)
@@ -916,19 +921,19 @@ static void mix_column(u8 *in, u8 *out)
 	u8 temp[4];
 	u8 tempb[4];
 
-	for (i = 0 ; i<4; i++) {
+	for (i = 0; i < 4; i++) {
 		if ((in[i] & 0x80) == 0x80)
-		    add1b[i] = 0x1b;
+			add1b[i] = 0x1b;
 		else
-		    add1b[i] = 0x00;
+			add1b[i] = 0x00;
 	}
 
-	swap_halfs[0] = in[2];    /* Swap halfs */
+	swap_halfs[0] = in[2]; /* Swap halfs */
 	swap_halfs[1] = in[3];
 	swap_halfs[2] = in[0];
 	swap_halfs[3] = in[1];
 
-	rotl[0] = in[3];        /* Rotate left 8 bits */
+	rotl[0] = in[3]; /* Rotate left 8 bits */
 	rotl[1] = in[0];
 	rotl[2] = in[1];
 	rotl[3] = in[2];
@@ -938,11 +943,10 @@ static void mix_column(u8 *in, u8 *out)
 	andf7[2] = in[2] & 0x7f;
 	andf7[3] = in[3] & 0x7f;
 
-	for (i = 3; i>0; i--) { /* logical shift left 1 bit */
+	for (i = 3; i > 0; i--) { /* logical shift left 1 bit */
 		andf7[i] = andf7[i] << 1;
-		if ((andf7[i-1] & 0x80) == 0x80) {
-		    andf7[i] = (andf7[i] | 0x01);
-		}
+		if ((andf7[i - 1] & 0x80) == 0x80)
+			andf7[i] = (andf7[i] | 0x01);
 	}
 	andf7[0] = andf7[0] << 1;
 	andf7[0] = andf7[0] & 0xfe;
@@ -951,7 +955,7 @@ static void mix_column(u8 *in, u8 *out)
 
 	xor_32(in, add1bf7, rotr);
 
-	temp[0] = rotr[0];         /* Rotate right 8 bits */
+	temp[0] = rotr[0]; /* Rotate right 8 bits */
 	rotr[0] = rotr[1];
 	rotr[1] = rotr[2];
 	rotr[2] = rotr[3];
@@ -971,25 +975,26 @@ static void aes128k128d(u8 *key, u8 *data, u8 *ciphertext)
 	u8 intermediateb[16];
 	u8 round_key[16];
 
-	for (i = 0; i<16; i++) round_key[i] = key[i];
+	for (i = 0; i < 16; i++)
+		round_key[i] = key[i];
 
 	for (round = 0; round < 11; round++) {
 		if (round == 0) {
-		    xor_128(round_key, data, ciphertext);
-		    next_key(round_key, round);
+			xor_128(round_key, data, ciphertext);
+			next_key(round_key, round);
 		} else if (round == 10) {
-		    byte_sub(ciphertext, intermediatea);
-		    shift_row(intermediatea, intermediateb);
-		    xor_128(intermediateb, round_key, ciphertext);
+			byte_sub(ciphertext, intermediatea);
+			shift_row(intermediatea, intermediateb);
+			xor_128(intermediateb, round_key, ciphertext);
 		} else { /* 1 - 9 */
-		    byte_sub(ciphertext, intermediatea);
-		    shift_row(intermediatea, intermediateb);
-		    mix_column(&intermediateb[0], &intermediatea[0]);
-		    mix_column(&intermediateb[4], &intermediatea[4]);
-		    mix_column(&intermediateb[8], &intermediatea[8]);
-		    mix_column(&intermediateb[12], &intermediatea[12]);
-		    xor_128(intermediatea, round_key, ciphertext);
-		    next_key(round_key, round);
+			byte_sub(ciphertext, intermediatea);
+			shift_row(intermediatea, intermediateb);
+			mix_column(&intermediateb[0], &intermediatea[0]);
+			mix_column(&intermediateb[4], &intermediatea[4]);
+			mix_column(&intermediateb[8], &intermediatea[8]);
+			mix_column(&intermediateb[12], &intermediatea[12]);
+			xor_128(intermediatea, round_key, ciphertext);
+			next_key(round_key, round);
 		}
 	}
 
@@ -1006,9 +1011,9 @@ static void construct_mic_iv(u8 *mic_iv, int qc_exists, int a4_exists, u8 *mpdu,
 
 	mic_iv[0] = 0x59;
 	if (qc_exists && a4_exists)
-		mic_iv[1] = mpdu[30] & 0x0f;    /* QoS_TC           */
+		mic_iv[1] = mpdu[30] & 0x0f; /* QoS_TC        */
 	if (qc_exists && !a4_exists)
-		mic_iv[1] = mpdu[24] & 0x0f;   /* mute bits 7-4    */
+		mic_iv[1] = mpdu[24] & 0x0f; /* mute bits 7-4 */
 	if (!qc_exists)
 		mic_iv[1] = 0x00;
 	for (i = 2; i < 8; i++)
@@ -1028,15 +1033,15 @@ static void construct_mic_header1(u8 *mic_header1, int header_length, u8 *mpdu)
 {
 	mic_header1[0] = (u8)((header_length - 2) / 256);
 	mic_header1[1] = (u8)((header_length - 2) % 256);
-	mic_header1[2] = mpdu[0] & 0xcf;    /* Mute CF poll & CF ack bits */
-	mic_header1[3] = mpdu[1] & 0xc7;    /* Mute retry, more data and pwr mgt bits */
-	mic_header1[4] = mpdu[4];       /* A1 */
+	mic_header1[2] = mpdu[0] & 0xcf; /* Mute CF poll & CF ack bits */
+	mic_header1[3] = mpdu[1] & 0xc7; /* Mute retry, more data and pwr mgt bits */
+	mic_header1[4] = mpdu[4]; /* A1 */
 	mic_header1[5] = mpdu[5];
 	mic_header1[6] = mpdu[6];
 	mic_header1[7] = mpdu[7];
 	mic_header1[8] = mpdu[8];
 	mic_header1[9] = mpdu[9];
-	mic_header1[10] = mpdu[10];     /* A2 */
+	mic_header1[10] = mpdu[10]; /* A2 */
 	mic_header1[11] = mpdu[11];
 	mic_header1[12] = mpdu[12];
 	mic_header1[13] = mpdu[13];
@@ -1046,7 +1051,7 @@ static void construct_mic_header1(u8 *mic_header1, int header_length, u8 *mpdu)
 }
 
 /************************************************/
-	/* construct_mic_header2()                      */
+/* construct_mic_header2()                      */
 /* Builds the last MIC header block from        */
 /* header fields.                               */
 /************************************************/
@@ -1055,9 +1060,10 @@ static void construct_mic_header2(u8 *mic_header2, u8 *mpdu, int a4_exists,
 {
 	int i;
 
-	for (i = 0; i<16; i++) mic_header2[i]= 0x00;
+	for (i = 0; i < 16; i++)
+		mic_header2[i] = 0x00;
 
-	mic_header2[0] = mpdu[16];    /* A3 */
+	mic_header2[0] = mpdu[16]; /* A3 */
 	mic_header2[1] = mpdu[17];
 	mic_header2[2] = mpdu[18];
 	mic_header2[3] = mpdu[19];
@@ -1068,8 +1074,8 @@ static void construct_mic_header2(u8 *mic_header2, u8 *mpdu, int a4_exists,
 	mic_header2[7] = 0x00; /* mpdu[23]; */
 
 	if (!qc_exists && a4_exists) {
-		for (i = 0;i<6;i++) mic_header2[8+i] = mpdu[24+i];   /* A4 */
-
+		for (i = 0; i < 6; i++)
+			mic_header2[8+i] = mpdu[24+i]; /* A4 */
 	}
 
 	if (qc_exists && !a4_exists) {
@@ -1078,7 +1084,8 @@ static void construct_mic_header2(u8 *mic_header2, u8 *mpdu, int a4_exists,
 	}
 
 	if (qc_exists && a4_exists) {
-		for (i = 0;i<6;i++) mic_header2[8+i] = mpdu[24+i];   /* A4 */
+		for (i = 0; i < 6; i++)
+			mic_header2[8+i] = mpdu[24+i]; /* A4 */
 
 		mic_header2[14] = mpdu[30] & 0x0f;
 		mic_header2[15] = mpdu[31] & 0x00;
@@ -1096,19 +1103,21 @@ static void construct_ctr_preload(u8 *ctr_preload, int a4_exists, int qc_exists,
 {
 	int i = 0;
 
-	for (i = 0; i<16; i++) ctr_preload[i] = 0x00;
+	for (i = 0; i < 16; i++)
+		ctr_preload[i] = 0x00;
+
 	i = 0;
 
-	ctr_preload[0] = 0x01;                                  /* flag */
+	ctr_preload[0] = 0x01; /* flag */
 	if (qc_exists && a4_exists)
-		ctr_preload[1] = mpdu[30] & 0x0f;   /* QoC_Control */
+		ctr_preload[1] = mpdu[30] & 0x0f; /* QoC_Control */
 	if (qc_exists && !a4_exists)
 		ctr_preload[1] = mpdu[24] & 0x0f;
 
 	for (i = 2; i < 8; i++)
-		ctr_preload[i] = mpdu[i + 8];                       /* ctr_preload[2:7] = A2[0:5] = mpdu[10:15] */
+		ctr_preload[i] = mpdu[i + 8]; /* ctr_preload[2:7] = A2[0:5] = mpdu[10:15] */
 	for (i = 8; i < 14; i++)
-		ctr_preload[i] =    pn_vector[13 - i];          /* ctr_preload[8:13] = PN[5:0] */
+		ctr_preload[i] = pn_vector[13 - i]; /* ctr_preload[8:13] = PN[5:0] */
 	ctr_preload[14] =  (unsigned char) (c / 256); /* Ctr */
 	ctr_preload[15] =  (unsigned char) (c % 256);
 
@@ -1128,8 +1137,8 @@ static void bitwise_xor(u8 *ina, u8 *inb, u8 *out)
 
 static int aes_cipher(u8 *key, uint hdrlen, u8 *pframe, uint plen)
 {
-	uint	qc_exists, a4_exists, i, j, payload_remainder,
-		num_blocks, payload_index;
+	uint qc_exists, a4_exists, i, j, payload_remainder,
+	     num_blocks, payload_index;
 	u8 pn_vector[6];
 	u8 mic_iv[16];
 	u8 mic_header1[16];
@@ -1177,12 +1186,12 @@ static int aes_cipher(u8 *key, uint hdrlen, u8 *pframe, uint plen)
 	} else {
 		qc_exists = 0;
 	}
-	pn_vector[0]= pframe[hdrlen];
-	pn_vector[1]= pframe[hdrlen+1];
-	pn_vector[2]= pframe[hdrlen+4];
-	pn_vector[3]= pframe[hdrlen+5];
-	pn_vector[4]= pframe[hdrlen+6];
-	pn_vector[5]= pframe[hdrlen+7];
+	pn_vector[0] = pframe[hdrlen];
+	pn_vector[1] = pframe[hdrlen + 1];
+	pn_vector[2] = pframe[hdrlen + 4];
+	pn_vector[3] = pframe[hdrlen + 5];
+	pn_vector[4] = pframe[hdrlen + 6];
+	pn_vector[5] = pframe[hdrlen + 7];
 
 	construct_mic_iv(mic_iv, qc_exists, a4_exists, pframe, plen, pn_vector);
 
@@ -1193,7 +1202,7 @@ static int aes_cipher(u8 *key, uint hdrlen, u8 *pframe, uint plen)
 	num_blocks = plen / 16;
 
 	/* Find start of payload */
-	payload_index = (hdrlen + 8);
+	payload_index = hdrlen + 8;
 
 	/* Calculate MIC */
 	aes128k128d(key, mic_iv, aes_out);
@@ -1224,12 +1233,12 @@ static int aes_cipher(u8 *key, uint hdrlen, u8 *pframe, uint plen)
 
 	/* Insert MIC into payload */
 	for (j = 0; j < 8; j++)
-		pframe[payload_index+j] = mic[j];
+		pframe[payload_index + j] = mic[j];
 
 	payload_index = hdrlen + 8;
 	for (i = 0; i < num_blocks; i++) {
 		construct_ctr_preload(ctr_preload, a4_exists, qc_exists,
-				      pframe, pn_vector, i+1);
+				      pframe, pn_vector, i + 1);
 		aes128k128d(key, ctr_preload, aes_out);
 		bitwise_xor(aes_out, &pframe[payload_index], chain_buffer);
 		for (j = 0; j < 16; j++)
@@ -1241,15 +1250,15 @@ static int aes_cipher(u8 *key, uint hdrlen, u8 *pframe, uint plen)
 		 * encrypt it and copy the unpadded part back
 		 */
 		construct_ctr_preload(ctr_preload, a4_exists, qc_exists, pframe,
-				      pn_vector, num_blocks+1);
+				      pn_vector, num_blocks + 1);
 
 		for (j = 0; j < 16; j++)
 			padded_buffer[j] = 0x00;
 		for (j = 0; j < payload_remainder; j++)
-			padded_buffer[j] = pframe[payload_index+j];
+			padded_buffer[j] = pframe[payload_index + j];
 		aes128k128d(key, ctr_preload, aes_out);
 		bitwise_xor(aes_out, padded_buffer, chain_buffer);
-		for (j = 0; j < payload_remainder;j++)
+		for (j = 0; j < payload_remainder; j++)
 			pframe[payload_index++] = chain_buffer[j];
 	}
 
@@ -1260,11 +1269,11 @@ static int aes_cipher(u8 *key, uint hdrlen, u8 *pframe, uint plen)
 	for (j = 0; j < 16; j++)
 		padded_buffer[j] = 0x00;
 	for (j = 0; j < 8; j++)
-		padded_buffer[j] = pframe[j+hdrlen+8+plen];
+		padded_buffer[j] = pframe[j + hdrlen + 8 + plen];
 
 	aes128k128d(key, ctr_preload, aes_out);
 	bitwise_xor(aes_out, padded_buffer, chain_buffer);
-	for (j = 0; j < 8;j++)
+	for (j = 0; j < 8; j++)
 		pframe[payload_index++] = chain_buffer[j];
 
 	return _SUCCESS;
@@ -1272,11 +1281,11 @@ static int aes_cipher(u8 *key, uint hdrlen, u8 *pframe, uint plen)
 
 int rtw_aes_encrypt23a(struct rtw_adapter *padapter,
 		       struct xmit_frame *pxmitframe)
-{	/*  exclude ICV */
+{	/* exclude ICV */
 	/* Intermediate Buffers */
 	int curfragnum, length;
 	u32 prwskeylen;
-	u8 *pframe, *prwskey;	/*  *payload,*iv */
+	u8 *pframe, *prwskey;
 	u8 hw_hdr_offset = 0;
 	struct sta_info *stainfo;
 	struct pkt_attrib *pattrib = &pxmitframe->attrib;
@@ -1304,18 +1313,18 @@ int rtw_aes_encrypt23a(struct rtw_adapter *padapter,
 
 	if (!stainfo) {
 		RT_TRACE(_module_rtl871x_security_c_, _drv_err_,
-			 ("rtw_aes_encrypt23a: stainfo == NULL!!!\n"));
+			 "%s: stainfo == NULL!!!\n", __func__);
 		DBG_8723A("%s, psta == NUL\n", __func__);
 		res = _FAIL;
 		goto out;
 	}
-	if (!(stainfo->state &_FW_LINKED)) {
+	if (!(stainfo->state & _FW_LINKED)) {
 		DBG_8723A("%s, psta->state(0x%x) != _FW_LINKED\n",
 			  __func__, stainfo->state);
 		return _FAIL;
 	}
 	RT_TRACE(_module_rtl871x_security_c_, _drv_err_,
-		 ("rtw_aes_encrypt23a: stainfo!= NULL!!!\n"));
+		 "%s: stainfo!= NULL!!!\n", __func__);
 
 	if (is_multicast_ether_addr(pattrib->ra))
 		prwskey = psecuritypriv->dot118021XGrpKey[psecuritypriv->dot118021XGrpKeyid].skey;
@@ -1345,12 +1354,11 @@ out:
 	return res;
 }
 
-static int aes_decipher(u8 *key, uint	hdrlen,
-			u8 *pframe, uint plen)
+static int aes_decipher(u8 *key, uint hdrlen, u8 *pframe, uint plen)
 {
-	static u8	message[MAX_MSG_SIZE];
-	uint	qc_exists, a4_exists, i, j, payload_remainder,
-			num_blocks, payload_index;
+	static u8 message[MAX_MSG_SIZE];
+	uint qc_exists, a4_exists, i, j, payload_remainder,
+	     num_blocks, payload_index;
 	int res = _SUCCESS;
 	u8 pn_vector[6];
 	u8 mic_iv[16];
@@ -1375,16 +1383,16 @@ static int aes_decipher(u8 *key, uint	hdrlen,
 
 	/* start to decrypt the payload */
 
-	num_blocks = (plen-8) / 16; /* plen including llc, payload_length and mic) */
+	num_blocks = (plen - 8) / 16; /* plen including llc, payload_length and mic) */
 
-	payload_remainder = (plen-8) % 16;
+	payload_remainder = (plen - 8) % 16;
 
 	pn_vector[0]  = pframe[hdrlen];
-	pn_vector[1]  = pframe[hdrlen+1];
-	pn_vector[2]  = pframe[hdrlen+4];
-	pn_vector[3]  = pframe[hdrlen+5];
-	pn_vector[4]  = pframe[hdrlen+6];
-	pn_vector[5]  = pframe[hdrlen+7];
+	pn_vector[1]  = pframe[hdrlen + 1];
+	pn_vector[2]  = pframe[hdrlen + 4];
+	pn_vector[3]  = pframe[hdrlen + 5];
+	pn_vector[4]  = pframe[hdrlen + 6];
+	pn_vector[5]  = pframe[hdrlen + 7];
 
 	if ((hdrlen == sizeof(struct ieee80211_hdr_3addr) ||
 	    (hdrlen == sizeof(struct ieee80211_qos_hdr))))
@@ -1413,13 +1421,13 @@ static int aes_decipher(u8 *key, uint	hdrlen,
 		qc_exists = 0;
 	}
 
-	/*  now, decrypt pframe with hdrlen offset and plen long */
+	/* now, decrypt pframe with hdrlen offset and plen long */
 
 	payload_index = hdrlen + 8; /*  8 is for extiv */
 
 	for (i = 0; i < num_blocks; i++) {
 		construct_ctr_preload(ctr_preload, a4_exists, qc_exists,
-				      pframe, pn_vector, i+1);
+				      pframe, pn_vector, i + 1);
 
 		aes128k128d(key, ctr_preload, aes_out);
 		bitwise_xor(aes_out, &pframe[payload_index], chain_buffer);
@@ -1433,12 +1441,12 @@ static int aes_decipher(u8 *key, uint	hdrlen,
 		 * encrypt it and copy the unpadded part back
 		 */
 		construct_ctr_preload(ctr_preload, a4_exists, qc_exists, pframe,
-				      pn_vector, num_blocks+1);
+				      pn_vector, num_blocks + 1);
 
 		for (j = 0; j < 16; j++)
 			padded_buffer[j] = 0x00;
 		for (j = 0; j < payload_remainder; j++)
-			padded_buffer[j] = pframe[payload_index+j];
+			padded_buffer[j] = pframe[payload_index + j];
 		aes128k128d(key, ctr_preload, aes_out);
 		bitwise_xor(aes_out, padded_buffer, chain_buffer);
 		for (j = 0; j < payload_remainder; j++)
@@ -1446,27 +1454,27 @@ static int aes_decipher(u8 *key, uint	hdrlen,
 	}
 
 	/* start to calculate the mic */
-	if ((hdrlen +plen+8) <= MAX_MSG_SIZE)
-		memcpy(message, pframe, (hdrlen+plen+8)); /* 8 is for ext iv len */
+	if ((hdrlen + plen + 8) <= MAX_MSG_SIZE)
+		memcpy(message, pframe, (hdrlen + plen + 8)); /* 8 is for ext iv len */
 
 	pn_vector[0] = pframe[hdrlen];
-	pn_vector[1] = pframe[hdrlen+1];
-	pn_vector[2] = pframe[hdrlen+4];
-	pn_vector[3] = pframe[hdrlen+5];
-	pn_vector[4] = pframe[hdrlen+6];
-	pn_vector[5] = pframe[hdrlen+7];
+	pn_vector[1] = pframe[hdrlen + 1];
+	pn_vector[2] = pframe[hdrlen + 4];
+	pn_vector[3] = pframe[hdrlen + 5];
+	pn_vector[4] = pframe[hdrlen + 6];
+	pn_vector[5] = pframe[hdrlen + 7];
 
 	construct_mic_iv(mic_iv, qc_exists, a4_exists, message,
-			 plen-8, pn_vector);
+			 plen - 8, pn_vector);
 
 	construct_mic_header1(mic_header1, hdrlen, message);
 	construct_mic_header2(mic_header2, message, a4_exists, qc_exists);
 
-	payload_remainder = (plen-8) % 16;
-	num_blocks = (plen-8) / 16;
+	payload_remainder = (plen - 8) % 16;
+	num_blocks = (plen - 8) / 16;
 
 	/* Find start of payload */
-	payload_index = (hdrlen + 8);
+	payload_index = hdrlen + 8;
 
 	/* Calculate MIC */
 	aes128k128d(key, mic_iv, aes_out);
@@ -1487,7 +1495,7 @@ static int aes_decipher(u8 *key, uint	hdrlen,
 		for (j = 0; j < 16; j++)
 			padded_buffer[j] = 0x00;
 		for (j = 0; j < payload_remainder; j++)
-		    padded_buffer[j] = message[payload_index++];
+			padded_buffer[j] = message[payload_index++];
 		bitwise_xor(aes_out, padded_buffer, chain_buffer);
 		aes128k128d(key, chain_buffer, aes_out);
 	}
@@ -1497,12 +1505,12 @@ static int aes_decipher(u8 *key, uint	hdrlen,
 
 	/* Insert MIC into payload */
 	for (j = 0; j < 8; j++)
-		message[payload_index+j] = mic[j];
+		message[payload_index + j] = mic[j];
 
 	payload_index = hdrlen + 8;
-	for (i = 0; i< num_blocks; i++) {
+	for (i = 0; i < num_blocks; i++) {
 		construct_ctr_preload(ctr_preload, a4_exists, qc_exists,
-				      message, pn_vector, i+1);
+				      message, pn_vector, i + 1);
 		aes128k128d(key, ctr_preload, aes_out);
 		bitwise_xor(aes_out, &message[payload_index], chain_buffer);
 		for (j = 0; j < 16; j++)
@@ -1514,12 +1522,12 @@ static int aes_decipher(u8 *key, uint	hdrlen,
 		 * encrypt it and copy the unpadded part back
 		 */
 		construct_ctr_preload(ctr_preload, a4_exists, qc_exists,
-				      message, pn_vector, num_blocks+1);
+				      message, pn_vector, num_blocks + 1);
 
 		for (j = 0; j < 16; j++)
-			 padded_buffer[j] = 0x00;
+			padded_buffer[j] = 0x00;
 		for (j = 0; j < payload_remainder; j++)
-			padded_buffer[j] = message[payload_index+j];
+			padded_buffer[j] = message[payload_index + j];
 		aes128k128d(key, ctr_preload, aes_out);
 		bitwise_xor(aes_out, padded_buffer, chain_buffer);
 		for (j = 0; j < payload_remainder; j++)
@@ -1533,7 +1541,7 @@ static int aes_decipher(u8 *key, uint	hdrlen,
 	for (j = 0; j < 16; j++)
 		padded_buffer[j] = 0x00;
 	for (j = 0; j < 8; j++)
-		padded_buffer[j] = message[j+hdrlen+8+plen-8];
+		padded_buffer[j] = message[j + hdrlen + 8 + plen - 8];
 
 	aes128k128d(key, ctr_preload, aes_out);
 	bitwise_xor(aes_out, padded_buffer, chain_buffer);
@@ -1542,12 +1550,16 @@ static int aes_decipher(u8 *key, uint	hdrlen,
 
 	/* compare the mic */
 	for (i = 0; i < 8; i++) {
-		if (pframe[hdrlen+8+plen-8+i] != message[hdrlen+8+plen-8+i]) {
+		if (pframe[hdrlen + 8 + plen - 8 + i] != message[hdrlen + 8 + plen - 8 + i]) {
 			RT_TRACE(_module_rtl871x_security_c_, _drv_err_,
-				 ("aes_decipher:mic check error mic[%d]: pframe(%x) != message(%x)\n",
-				 i, pframe[hdrlen+8+plen-8+i], message[hdrlen+8+plen-8+i]));
-			DBG_8723A("aes_decipher:mic check error mic[%d]: pframe(%x) != message(%x)\n",
-				  i, pframe[hdrlen+8+plen-8+i], message[hdrlen+8+plen-8+i]);
+				 "%s:mic check error mic[%d]: pframe(%x) != message(%x)\n",
+				 __func__, i,
+				 pframe[hdrlen + 8 + plen - 8 + i],
+				 message[hdrlen + 8 + plen - 8 + i]);
+			DBG_8723A("%s:mic check error mic[%d]: pframe(%x) != message(%x)\n",
+				  __func__, i,
+				  pframe[hdrlen + 8 + plen - 8 + i],
+				  message[hdrlen + 8 + plen - 8 + i]);
 			res = _FAIL;
 		}
 	}
@@ -1562,7 +1574,7 @@ int rtw_aes_decrypt23a(struct rtw_adapter *padapter,
 	struct security_priv *psecuritypriv = &padapter->securitypriv;
 	struct sk_buff *skb = precvframe->pkt;
 	int length;
-	u8 *pframe, *prwskey;	/*  *payload,*iv */
+	u8 *pframe, *prwskey;
 	int res = _SUCCESS;
 
 	pframe = skb->data;
@@ -1573,17 +1585,18 @@ int rtw_aes_decrypt23a(struct rtw_adapter *padapter,
 	stainfo = rtw_get_stainfo23a(&padapter->stapriv, &prxattrib->ta[0]);
 	if (!stainfo) {
 		RT_TRACE(_module_rtl871x_security_c_, _drv_err_,
-			 ("rtw_aes_encrypt23a: stainfo == NULL!!!\n"));
+			 "%s: stainfo == NULL!!!\n", __func__);
 		res = _FAIL;
 		goto exit;
 	}
 
 	RT_TRACE(_module_rtl871x_security_c_, _drv_err_,
-		 ("rtw_aes_decrypt23a: stainfo!= NULL!!!\n"));
+		 "%s: stainfo!= NULL!!!\n", __func__);
 
 	if (is_multicast_ether_addr(prxattrib->ra)) {
-		/* in concurrent we should use sw decrypt in group key,
-		   so we remove this message */
+		/* in concurrent we should use sw decrypt in
+		 * group key, so we remove this message
+		 */
 		if (!psecuritypriv->binstallGrpkey) {
 			res = _FAIL;
 			DBG_8723A("%s:rx bc/mc packets, but didn't install "
@@ -1613,9 +1626,10 @@ void rtw_use_tkipkey_handler23a(void *FunctionContext)
 {
 	struct rtw_adapter *padapter = (struct rtw_adapter *)FunctionContext;
 
-	RT_TRACE(_module_rtl871x_security_c_, _drv_err_, ("^^^rtw_use_tkipkey_handler23a ^^^\n"));
+	RT_TRACE(_module_rtl871x_security_c_, _drv_err_,
+		 "^^^%s ^^^\n", __func__);
 	padapter->securitypriv.busetkipkey = 1;
 	RT_TRACE(_module_rtl871x_security_c_, _drv_err_,
-		 ("^^^rtw_use_tkipkey_handler23a padapter->securitypriv.busetkipkey =%d^^^\n",
-		 padapter->securitypriv.busetkipkey));
+		 "^^^%s padapter->securitypriv.busetkipkey =%d^^^\n",
+		 __func__, padapter->securitypriv.busetkipkey);
 }
diff --git a/drivers/staging/rtl8723au/core/rtw_sta_mgt.c b/drivers/staging/rtl8723au/core/rtw_sta_mgt.c
index d17998da8600..b06bff74502a 100644
--- a/drivers/staging/rtl8723au/core/rtw_sta_mgt.c
+++ b/drivers/staging/rtl8723au/core/rtw_sta_mgt.c
@@ -136,10 +136,10 @@ rtw_alloc_stainfo23a(struct sta_priv *pstapriv, const u8 *hwaddr, gfp_t gfp)
 	index = wifi_mac_hash(hwaddr);
 
 	RT_TRACE(_module_rtl871x_sta_mgt_c_, _drv_info_,
-		 ("rtw_alloc_stainfo23a: index  = %x", index));
+		 "rtw_alloc_stainfo23a: index  = %x\n", index);
 	if (index >= NUM_STA) {
 		RT_TRACE(_module_rtl871x_sta_mgt_c_, _drv_err_,
-			 ("ERROR => rtw_alloc_stainfo23a: index >= NUM_STA"));
+			 "ERROR => rtw_alloc_stainfo23a: index >= NUM_STA\n");
 		psta = NULL;
 		goto exit;
 	}
@@ -160,8 +160,8 @@ rtw_alloc_stainfo23a(struct sta_priv *pstapriv, const u8 *hwaddr, gfp_t gfp)
 			&wRxSeqInitialValue, 2);
 
 	RT_TRACE(_module_rtl871x_sta_mgt_c_, _drv_info_,
-		 ("alloc number_%d stainfo  with hwaddr = %pM\n",
-		 pstapriv->asoc_sta_count, hwaddr));
+		 "alloc number_%d stainfo  with hwaddr = %pM\n",
+		 pstapriv->asoc_sta_count, hwaddr);
 
 	init_addba_retry_timer23a(psta);
 
@@ -249,10 +249,8 @@ int rtw_free_stainfo23a(struct rtw_adapter *padapter, struct sta_info *psta)
 
 	list_del_init(&psta->hash_list);
 	RT_TRACE(_module_rtl871x_sta_mgt_c_, _drv_err_,
-		("\n free number_%d stainfo  with hwaddr = 0x%.2x 0x%.2x 0x%.2x 0x%.2x 0x%.2x 0x%.2x\n",
-			pstapriv->asoc_sta_count, psta->hwaddr[0],
-			psta->hwaddr[1], psta->hwaddr[2], psta->hwaddr[3],
-			psta->hwaddr[4], psta->hwaddr[5]));
+		 "free number_%d stainfo  with hwaddr = %pM\n",
+		 pstapriv->asoc_sta_count, psta->hwaddr);
 	pstapriv->asoc_sta_count--;
 
 	/*  re-init sta_info; 20061114  will be init in alloc_stainfo */
@@ -397,7 +395,7 @@ int rtw_init_bcmc_stainfo23a(struct rtw_adapter *padapter)
 	if (psta == NULL) {
 		res = _FAIL;
 		RT_TRACE(_module_rtl871x_sta_mgt_c_, _drv_err_,
-			 ("rtw_alloc_stainfo23a fail"));
+			 "rtw_alloc_stainfo23a fail\n");
 		return res;
 	}
 	/*  default broadcast & multicast use macid 1 */
diff --git a/drivers/staging/rtl8723au/core/rtw_wlan_util.c b/drivers/staging/rtl8723au/core/rtw_wlan_util.c
index 69d9e0f17fd8..5280338aa387 100644
--- a/drivers/staging/rtl8723au/core/rtw_wlan_util.c
+++ b/drivers/staging/rtl8723au/core/rtw_wlan_util.c
@@ -876,9 +876,9 @@ int rtw_check_bcn_info23a(struct rtw_adapter *Adapter,
 	}
 
 	if (!ether_addr_equal(cur_network->network.MacAddress, mgmt->bssid)) {
-		DBG_8723A("%s: linked but recv other bssid bcn"
-			  MAC_FMT MAC_FMT "\n", __func__, MAC_ARG(mgmt->bssid),
-			  MAC_ARG(cur_network->network.MacAddress));
+		DBG_8723A("%s: linked but recv other bssid bcn %pM %pM\n",
+			  __func__, mgmt->bssid,
+			  cur_network->network.MacAddress);
 		return _FAIL;
 	}
 
@@ -926,10 +926,9 @@ int rtw_check_bcn_info23a(struct rtw_adapter *Adapter,
 	}
 
 	RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_,
-		 ("%s bssid.Ssid.Ssid:%s bssid.Ssid.SsidLength:%d "
-		  "cur_network->network.Ssid.Ssid:%s len:%d\n", __func__,
-		  ssid, ssid_len, cur_network->network.Ssid.ssid,
-		  cur_network->network.Ssid.ssid_len));
+		 "%s bssid.Ssid.Ssid:%s bssid.Ssid.SsidLength:%d cur_network->network.Ssid.Ssid:%s len:%d\n",
+		 __func__, ssid, ssid_len, cur_network->network.Ssid.ssid,
+		 cur_network->network.Ssid.ssid_len);
 
 	if (ssid_len != cur_network->network.Ssid.ssid_len || ssid_len > 32 ||
 	    (ssid_len &&
@@ -947,8 +946,8 @@ int rtw_check_bcn_info23a(struct rtw_adapter *Adapter,
 		privacy = 0;
 
 	RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_,
-		 ("%s(): cur_network->network.Privacy is %d, bssid.Privacy "
-		  "is %d\n", __func__, cur_network->network.Privacy, privacy));
+		 "%s(): cur_network->network.Privacy is %d, bssid.Privacy is %d\n",
+		 __func__, cur_network->network.Privacy, privacy);
 	if (cur_network->network.Privacy != privacy) {
 		DBG_8723A("%s(), privacy is not match return FAIL\n", __func__);
 		goto _mismatch;
@@ -962,10 +961,9 @@ int rtw_check_bcn_info23a(struct rtw_adapter *Adapter,
 						 &pairwise_cipher, &is_8021x);
 			if (r == _SUCCESS)
 				RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_,
-					 ("%s pnetwork->pairwise_cipher: %d, "
-					  "pnetwork->group_cipher: %d, is_802x "
-					  ": %d\n", __func__, pairwise_cipher,
-					  group_cipher, is_8021x));
+					 "%s pnetwork->pairwise_cipher: %d, pnetwork->group_cipher: %d, is_802x : %d\n",
+					 __func__, pairwise_cipher,
+					 group_cipher, is_8021x);
 			}
 	} else {
 		p = cfg80211_find_vendor_ie(WLAN_OUI_MICROSOFT,
@@ -977,10 +975,9 @@ int rtw_check_bcn_info23a(struct rtw_adapter *Adapter,
 						&pairwise_cipher, &is_8021x);
 			if (r == _SUCCESS)
 				RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_,
-					 ("%s pnetwork->pairwise_cipher: %d, "
-					  "group_cipher is %d, is_8021x is "
-					  "%d\n", __func__, pairwise_cipher,
-					  group_cipher, is_8021x));
+					 "%s pnetwork->pairwise_cipher: %d, group_cipher is %d, is_8021x is %d\n",
+					 __func__, pairwise_cipher,
+					 group_cipher, is_8021x);
 		} else {
 			if (privacy)
 				crypto = ENCRYP_PROTOCOL_WEP;
@@ -996,8 +993,8 @@ int rtw_check_bcn_info23a(struct rtw_adapter *Adapter,
 
 	if (crypto == ENCRYP_PROTOCOL_WPA || crypto == ENCRYP_PROTOCOL_WPA2) {
 		RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_,
-			 ("%s cur_network->group_cipher is %d: %d\n", __func__,
-			  cur_network->BcnInfo.group_cipher, group_cipher));
+			 "%s cur_network->group_cipher is %d: %d\n", __func__,
+			 cur_network->BcnInfo.group_cipher, group_cipher);
 		if (pairwise_cipher != cur_network->BcnInfo.pairwise_cipher ||
 		    group_cipher != cur_network->BcnInfo.group_cipher) {
 			DBG_8723A("%s pairwise_cipher(%x:%x) or group_cipher "
diff --git a/drivers/staging/rtl8723au/core/rtw_xmit.c b/drivers/staging/rtl8723au/core/rtw_xmit.c
index 1c82dffcf596..a4b6bb6c79a9 100644
--- a/drivers/staging/rtl8723au/core/rtw_xmit.c
+++ b/drivers/staging/rtl8723au/core/rtw_xmit.c
@@ -463,9 +463,7 @@ static int update_attrib(struct rtw_adapter *padapter,
 					/*  67 : UDP BOOTP server */
 					RT_TRACE(_module_rtl871x_xmit_c_,
 						 _drv_err_,
-						 ("======================"
-						  "update_attrib: get DHCP "
-						  "Packet\n"));
+						 "======================update_attrib: get DHCP Packet\n");
 					pattrib->dhcp_pkt = 1;
 				}
 			}
@@ -493,8 +491,8 @@ static int update_attrib(struct rtw_adapter *padapter,
 		psta = rtw_get_stainfo23a(pstapriv, pattrib->ra);
 		if (psta == NULL) { /*  if we cannot get psta => drrp the pkt */
 			RT_TRACE(_module_rtl871x_xmit_c_, _drv_alert_,
-				 ("\nupdate_attrib => get sta_info fail, ra:"
-				  MAC_FMT"\n", MAC_ARG(pattrib->ra)));
+				 "update_attrib => get sta_info fail, ra:%pM\n",
+				 pattrib->ra);
 			res = _FAIL;
 			goto exit;
 		} else if (check_fwstate(pmlmepriv, WIFI_AP_STATE) &&
@@ -511,8 +509,8 @@ static int update_attrib(struct rtw_adapter *padapter,
 	} else {
 		/*  if we cannot get psta => drop the pkt */
 		RT_TRACE(_module_rtl871x_xmit_c_, _drv_alert_,
-			 ("\nupdate_attrib => get sta_info fail, ra:" MAC_FMT
-			  "\n", MAC_ARG(pattrib->ra)));
+			 "update_attrib => get sta_info fail, ra:%pM\n",
+			 pattrib->ra);
 		res = _FAIL;
 		goto exit;
 	}
@@ -544,16 +542,15 @@ static int update_attrib(struct rtw_adapter *padapter,
 
 	if (psta->ieee8021x_blocked == true) {
 		RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_,
-			 ("\n psta->ieee8021x_blocked == true\n"));
+			 "psta->ieee8021x_blocked == true\n");
 
 		pattrib->encrypt = 0;
 
 		if ((pattrib->ether_type != ETH_P_PAE) &&
 		    !check_fwstate(pmlmepriv, WIFI_MP_STATE)) {
 			RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_,
-				 ("\npsta->ieee8021x_blocked == true,  "
-				  "pattrib->ether_type(%.4x) != 0x888e\n",
-				  pattrib->ether_type));
+				 "psta->ieee8021x_blocked == true,  pattrib->ether_type(%.4x) != 0x888e\n",
+				 pattrib->ether_type);
 			res = _FAIL;
 			goto exit;
 		}
@@ -594,9 +591,8 @@ static int update_attrib(struct rtw_adapter *padapter,
 
 		if (!padapter->securitypriv.busetkipkey) {
 			RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_,
-				 ("\npadapter->securitypriv.busetkip"
-				  "key(%d) == false drop packet\n",
-				  padapter->securitypriv.busetkipkey));
+				 "padapter->securitypriv.busetkipkey(%d) == false drop packet\n",
+				 padapter->securitypriv.busetkipkey);
 			res = _FAIL;
 			goto exit;
 		}
@@ -604,8 +600,8 @@ static int update_attrib(struct rtw_adapter *padapter,
 		break;
 	case WLAN_CIPHER_SUITE_CCMP:
 		RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_,
-			 ("pattrib->encrypt =%d (WLAN_CIPHER_SUITE_CCMP)\n",
-			  pattrib->encrypt));
+			 "pattrib->encrypt =%d (WLAN_CIPHER_SUITE_CCMP)\n",
+			 pattrib->encrypt);
 		pattrib->iv_len = IEEE80211_CCMP_HDR_LEN;
 		pattrib->icv_len = IEEE80211_CCMP_MIC_LEN;
 		break;
@@ -617,17 +613,17 @@ static int update_attrib(struct rtw_adapter *padapter,
 	}
 
 	RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_,
-		 ("update_attrib: encrypt =%d\n", pattrib->encrypt));
+		 "update_attrib: encrypt =%d\n", pattrib->encrypt);
 
 	if (pattrib->encrypt && !psecuritypriv->hw_decrypted) {
 		pattrib->bswenc = true;
 		RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_,
-			 ("update_attrib: encrypt =%d bswenc = true\n",
-			  pattrib->encrypt));
+			 "update_attrib: encrypt =%d bswenc = true\n",
+			 pattrib->encrypt);
 	} else {
 		pattrib->bswenc = false;
 		RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_,
-			 ("update_attrib: bswenc = false\n"));
+			 "update_attrib: bswenc = false\n");
 	}
 	update_attrib_phy_info(pattrib, psta);
 
@@ -726,20 +722,18 @@ static int xmitframe_addmic(struct rtw_adapter *padapter,
 			     curfragnum++) {
 				payload = PTR_ALIGN(payload, 4);
 				RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_,
-					 ("=== curfragnum =%d, pframe = 0x%.2x, "
-					  "0x%.2x, 0x%.2x, 0x%.2x, 0x%.2x, 0x"
-					  "%.2x, 0x%.2x, 0x%.2x,!!!\n",
-					  curfragnum, *payload, *(payload + 1),
-					  *(payload + 2), *(payload + 3),
-					  *(payload + 4), *(payload + 5),
-					  *(payload + 6), *(payload + 7)));
+					 "=== curfragnum =%d, pframe = 0x%.2x, 0x%.2x, 0x%.2x, 0x%.2x, 0x%.2x, 0x%.2x, 0x%.2x, 0x%.2x,!!!\n",
+					 curfragnum, *payload, *(payload + 1),
+					 *(payload + 2), *(payload + 3),
+					 *(payload + 4), *(payload + 5),
+					 *(payload + 6), *(payload + 7));
 
 				payload = payload + pattrib->hdrlen +
 					pattrib->iv_len;
 				RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_,
-					 ("curfragnum =%d pattrib->hdrlen =%d "
-					  "pattrib->iv_len =%d", curfragnum,
-					  pattrib->hdrlen, pattrib->iv_len));
+					 "curfragnum =%d pattrib->hdrlen =%d pattrib->iv_len =%d\n",
+					 curfragnum,
+					 pattrib->hdrlen, pattrib->iv_len);
 				if ((curfragnum + 1) == pattrib->nr_frags) {
 					length = pattrib->last_txcmdsz -
 						pattrib->hdrlen -
@@ -761,25 +755,21 @@ static int xmitframe_addmic(struct rtw_adapter *padapter,
 						pattrib->icv_len;
 					RT_TRACE(_module_rtl871x_xmit_c_,
 						 _drv_err_,
-						 ("curfragnum =%d length =%d "
-						  "pattrib->icv_len =%d",
-						  curfragnum, length,
-						  pattrib->icv_len));
+						 "curfragnum =%d length =%d pattrib->icv_len =%d\n",
+						 curfragnum, length,
+						 pattrib->icv_len);
 				}
 			}
 			rtw_secgetmic23a(&micdata, &mic[0]);
 			RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_,
-				 ("xmitframe_addmic: before add mic code!!\n"));
+				 "xmitframe_addmic: before add mic code!!\n");
 			RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_,
-				 ("xmitframe_addmic: pattrib->last_txcmdsz ="
-				  "%d!!!\n", pattrib->last_txcmdsz));
+				 "xmitframe_addmic: pattrib->last_txcmdsz =%d!!!\n",
+				 pattrib->last_txcmdsz);
 			RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_,
-				 ("xmitframe_addmic: mic[0]= 0x%.2x , mic[1]="
-				  "0x%.2x , mic[2]= 0x%.2x , mic[3]= 0x%.2x\n"
-				  "mic[4]= 0x%.2x , mic[5]= 0x%.2x , mic[6]= 0x%.2x "
-				  ", mic[7]= 0x%.2x !!!!\n", mic[0], mic[1],
-				  mic[2], mic[3], mic[4], mic[5], mic[6],
-				  mic[7]));
+				 "xmitframe_addmic: mic[0]= 0x%.2x , mic[1]=0x%.2x , mic[2]= 0x%.2x , mic[3]= 0x%.2x\nmic[4]= 0x%.2x , mic[5]= 0x%.2x , mic[6]= 0x%.2x , mic[7]= 0x%.2x !!!!\n",
+				 mic[0], mic[1], mic[2], mic[3],
+				 mic[4], mic[5], mic[6], mic[7]);
 			/* add mic code  and add the mic code length
 			   in last_txcmdsz */
 
@@ -787,25 +777,24 @@ static int xmitframe_addmic(struct rtw_adapter *padapter,
 			pattrib->last_txcmdsz += 8;
 
 			RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_,
-				 ("\n ======== last pkt ========\n"));
+				 "======== last pkt ========\n");
 			payload = payload - pattrib->last_txcmdsz + 8;
 			for (curfragnum = 0; curfragnum < pattrib->last_txcmdsz;
-			     curfragnum = curfragnum + 8)
+			     curfragnum = curfragnum + 8) {
 				RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_,
-					 (" %.2x,  %.2x,  %.2x,  %.2x,  %.2x, "
-					  " %.2x,  %.2x,  %.2x ",
-					  *(payload + curfragnum),
-					  *(payload + curfragnum + 1),
-					  *(payload + curfragnum + 2),
-					  *(payload + curfragnum + 3),
-					  *(payload + curfragnum + 4),
-					  *(payload + curfragnum + 5),
-					  *(payload + curfragnum + 6),
-					  *(payload + curfragnum + 7)));
-			} else {
-				RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_,
-					 ("xmitframe_addmic: rtw_get_stainfo23a =="
-					  "NULL!!!\n"));
+					 "%.2x,  %.2x,  %.2x,  %.2x,  %.2x,  %.2x,  %.2x,  %.2x\n",
+					 *(payload + curfragnum),
+					 *(payload + curfragnum + 1),
+					 *(payload + curfragnum + 2),
+					 *(payload + curfragnum + 3),
+					 *(payload + curfragnum + 4),
+					 *(payload + curfragnum + 5),
+					 *(payload + curfragnum + 6),
+					 *(payload + curfragnum + 7));
+			}
+		} else {
+			RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_,
+				 "xmitframe_addmic: rtw_get_stainfo23a ==NULL!!!\n");
 		}
 	}
 
@@ -821,7 +810,7 @@ static int xmitframe_swencrypt(struct rtw_adapter *padapter,
 	if (pattrib->bswenc) {
 		/* DBG_8723A("start xmitframe_swencrypt\n"); */
 		RT_TRACE(_module_rtl871x_xmit_c_, _drv_alert_,
-			 ("### xmitframe_swencrypt\n"));
+			 "### xmitframe_swencrypt\n");
 		switch (pattrib->encrypt) {
 		case WLAN_CIPHER_SUITE_WEP40:
 		case WLAN_CIPHER_SUITE_WEP104:
@@ -839,7 +828,7 @@ static int xmitframe_swencrypt(struct rtw_adapter *padapter,
 
 	} else {
 		RT_TRACE(_module_rtl871x_xmit_c_, _drv_notice_,
-			 ("### xmitframe_hwencrypt\n"));
+			 "### xmitframe_hwencrypt\n");
 	}
 
 	return _SUCCESS;
@@ -916,7 +905,9 @@ static int rtw_make_wlanhdr(struct rtw_adapter *padapter, u8 *hdr,
 				qos_option = true;
 		}
 		else {
-			RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("fw_state:%x is not allowed to xmit frame\n", get_fwstate(pmlmepriv)));
+			RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_,
+				 "fw_state:%x is not allowed to xmit frame\n",
+				 get_fwstate(pmlmepriv));
 			res = _FAIL;
 			goto exit;
 		}
@@ -1117,7 +1108,7 @@ int rtw_xmitframe_coalesce23a(struct rtw_adapter *padapter, struct sk_buff *skb,
 
 	if (rtw_make_wlanhdr(padapter, mem_start, pattrib) == _FAIL) {
 		RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_,
-			 ("%s: rtw_make_wlanhdr fail; drop pkt\n", __func__));
+			 "%s: rtw_make_wlanhdr fail; drop pkt\n", __func__);
 		res = _FAIL;
 		goto exit;
 	}
@@ -1172,11 +1163,10 @@ int rtw_xmitframe_coalesce23a(struct rtw_adapter *padapter, struct sk_buff *skb,
 			memcpy(pframe, pattrib->iv, pattrib->iv_len);
 
 			RT_TRACE(_module_rtl871x_xmit_c_, _drv_notice_,
-				 ("rtw_xmiaframe_coalesce23a: keyid =%d pattrib"
-				  "->iv[3]=%.2x pframe =%.2x %.2x %.2x %.2x\n",
-				  padapter->securitypriv.dot11PrivacyKeyIndex,
-				  pattrib->iv[3], *pframe, *(pframe+1),
-				  *(pframe+2), *(pframe+3)));
+				 "rtw_xmiaframe_coalesce23a: keyid =%d pattrib->iv[3]=%.2x pframe =%.2x %.2x %.2x %.2x\n",
+				 padapter->securitypriv.dot11PrivacyKeyIndex,
+				 pattrib->iv[3], *pframe, *(pframe+1),
+				 *(pframe+2), *(pframe+3));
 			pframe += pattrib->iv_len;
 			mpdu_len -= pattrib->iv_len;
 		}
@@ -1223,8 +1213,8 @@ int rtw_xmitframe_coalesce23a(struct rtw_adapter *padapter, struct sk_buff *skb,
 			break;
 		} else {
 			RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_,
-				 ("%s: There're still something in packet!\n",
-				  __func__));
+				 "%s: There're still something in packet!\n",
+				 __func__);
 		}
 		hdr->frame_control |= cpu_to_le16(IEEE80211_FCTL_MOREFRAGS);
 
@@ -1234,7 +1224,7 @@ int rtw_xmitframe_coalesce23a(struct rtw_adapter *padapter, struct sk_buff *skb,
 
 	if (xmitframe_addmic(padapter, pxmitframe) == _FAIL) {
 		RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_,
-			 ("xmitframe_addmic(padapter, pxmitframe) == _FAIL\n"));
+			 "xmitframe_addmic(padapter, pxmitframe) == _FAIL\n");
 		DBG_8723A("xmitframe_addmic(padapter, pxmitframe) == _FAIL\n");
 		res = _FAIL;
 		goto exit;
@@ -1462,8 +1452,8 @@ static struct xmit_frame *rtw_alloc_xmitframe(struct xmit_priv *pxmitpriv)
 
 	if (list_empty(&pfree_xmit_queue->queue)) {
 		RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_,
-			 ("rtw_alloc_xmitframe:%d\n",
-			  pxmitpriv->free_xmitframe_cnt));
+			 "rtw_alloc_xmitframe:%d\n",
+			 pxmitpriv->free_xmitframe_cnt);
 		pxframe =  NULL;
 	} else {
 		phead = get_list_head(pfree_xmit_queue);
@@ -1475,8 +1465,8 @@ static struct xmit_frame *rtw_alloc_xmitframe(struct xmit_priv *pxmitpriv)
 		list_del_init(&pxframe->list);
 		pxmitpriv->free_xmitframe_cnt--;
 		RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_,
-			 ("rtw_alloc_xmitframe():free_xmitframe_cnt =%d\n",
-			  pxmitpriv->free_xmitframe_cnt));
+			 "rtw_alloc_xmitframe():free_xmitframe_cnt =%d\n",
+			 pxmitpriv->free_xmitframe_cnt);
 	}
 
 	spin_unlock_bh(&pfree_xmit_queue->lock);
@@ -1495,7 +1485,9 @@ struct xmit_frame *rtw_alloc_xmitframe23a_ext(struct xmit_priv *pxmitpriv)
 	spin_lock_bh(&queue->lock);
 
 	if (list_empty(&queue->queue)) {
-		RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_, ("rtw_alloc_xmitframe23a_ext:%d\n", pxmitpriv->free_xframe_ext_cnt));
+		RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_,
+			 "rtw_alloc_xmitframe23a_ext:%d\n",
+			 pxmitpriv->free_xframe_ext_cnt);
 		pxframe =  NULL;
 	} else {
 		phead = get_list_head(queue);
@@ -1504,7 +1496,9 @@ struct xmit_frame *rtw_alloc_xmitframe23a_ext(struct xmit_priv *pxmitpriv)
 
 		list_del_init(&pxframe->list);
 		pxmitpriv->free_xframe_ext_cnt--;
-		RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_, ("rtw_alloc_xmitframe23a_ext():free_xmitframe_cnt =%d\n", pxmitpriv->free_xframe_ext_cnt));
+		RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_,
+			 "rtw_alloc_xmitframe23a_ext():free_xmitframe_cnt =%d\n",
+			 pxmitpriv->free_xframe_ext_cnt);
 	}
 
 	spin_unlock_bh(&queue->lock);
@@ -1521,7 +1515,8 @@ s32 rtw_free_xmitframe23a(struct xmit_priv *pxmitpriv, struct xmit_frame *pxmitf
 	struct sk_buff *pndis_pkt = NULL;
 
 	if (pxmitframe == NULL) {
-		RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("====== rtw_free_xmitframe23a():pxmitframe == NULL!!!!!!!!!!\n"));
+		RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_,
+			 "====== rtw_free_xmitframe23a():pxmitframe == NULL!!!!!!!!!!\n");
 		goto exit;
 	}
 
@@ -1543,10 +1538,14 @@ s32 rtw_free_xmitframe23a(struct xmit_priv *pxmitpriv, struct xmit_frame *pxmitf
 	list_add_tail(&pxmitframe->list, get_list_head(queue));
 	if (pxmitframe->ext_tag == 0) {
 		pxmitpriv->free_xmitframe_cnt++;
-		RT_TRACE(_module_rtl871x_xmit_c_, _drv_debug_, ("rtw_free_xmitframe23a():free_xmitframe_cnt =%d\n", pxmitpriv->free_xmitframe_cnt));
+		RT_TRACE(_module_rtl871x_xmit_c_, _drv_debug_,
+			 "rtw_free_xmitframe23a():free_xmitframe_cnt =%d\n",
+			 pxmitpriv->free_xmitframe_cnt);
 	} else if (pxmitframe->ext_tag == 1) {
 		pxmitpriv->free_xframe_ext_cnt++;
-		RT_TRACE(_module_rtl871x_xmit_c_, _drv_debug_, ("rtw_free_xmitframe23a():free_xframe_ext_cnt =%d\n", pxmitpriv->free_xframe_ext_cnt));
+		RT_TRACE(_module_rtl871x_xmit_c_, _drv_debug_,
+			 "rtw_free_xmitframe23a():free_xframe_ext_cnt =%d\n",
+			 pxmitpriv->free_xframe_ext_cnt);
 	}
 
 	spin_unlock_bh(&queue->lock);
@@ -1585,8 +1584,7 @@ int rtw_xmitframe_enqueue23a(struct rtw_adapter *padapter,
 {
 	if (rtw_xmit23a_classifier(padapter, pxmitframe) == _FAIL) {
 		RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_,
-			 ("rtw_xmitframe_enqueue23a: drop xmit pkt for "
-			  "classifier fail\n"));
+			 "rtw_xmitframe_enqueue23a: drop xmit pkt for classifier fail\n");
 		return _FAIL;
 	}
 
@@ -1675,26 +1673,30 @@ struct tx_servq *rtw_get_sta_pending23a(struct rtw_adapter *padapter, struct sta
 	case 2:
 		ptxservq = &psta->sta_xmitpriv.bk_q;
 		*(ac) = 3;
-		RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_, ("rtw_get_sta_pending23a : BK\n"));
+		RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_,
+			 "rtw_get_sta_pending23a : BK\n");
 		break;
 	case 4:
 	case 5:
 		ptxservq = &psta->sta_xmitpriv.vi_q;
 		*(ac) = 1;
-		RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_, ("rtw_get_sta_pending23a : VI\n"));
+		RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_,
+			 "rtw_get_sta_pending23a : VI\n");
 		break;
 	case 6:
 	case 7:
 		ptxservq = &psta->sta_xmitpriv.vo_q;
 		*(ac) = 0;
-		RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_, ("rtw_get_sta_pending23a : VO\n"));
+		RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_,
+			 "rtw_get_sta_pending23a : VO\n");
 		break;
 	case 0:
 	case 3:
 	default:
 		ptxservq = &psta->sta_xmitpriv.be_q;
 		*(ac) = 2;
-		RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_, ("rtw_get_sta_pending23a : BE\n"));
+		RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_,
+			 "rtw_get_sta_pending23a : BE\n");
 		break;
 	}
 	return ptxservq;
@@ -1725,7 +1727,7 @@ int rtw_xmit23a_classifier(struct rtw_adapter *padapter,
 		res = _FAIL;
 		DBG_8723A("rtw_xmit23a_classifier: psta == NULL\n");
 		RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_,
-			 ("rtw_xmit23a_classifier: psta == NULL\n"));
+			 "rtw_xmit23a_classifier: psta == NULL\n");
 		goto exit;
 	}
 	if (!(psta->state & _FW_LINKED)) {
@@ -1876,14 +1878,15 @@ int rtw_xmit23a(struct rtw_adapter *padapter, struct sk_buff *skb)
 
 	if (pxmitframe == NULL) {
 		RT_TRACE(_module_xmit_osdep_c_, _drv_err_,
-			 ("rtw_xmit23a: no more pxmitframe\n"));
+			 "rtw_xmit23a: no more pxmitframe\n");
 		return -1;
 	}
 
 	res = update_attrib(padapter, skb, &pxmitframe->attrib);
 
 	if (res == _FAIL) {
-		RT_TRACE(_module_xmit_osdep_c_, _drv_err_, ("rtw_xmit23a: update attrib fail\n"));
+		RT_TRACE(_module_xmit_osdep_c_, _drv_err_,
+			 "rtw_xmit23a: update attrib fail\n");
 		rtw_free_xmitframe23a(pxmitpriv, pxmitframe);
 		return -1;
 	}
diff --git a/drivers/staging/rtl8723au/hal/HalDMOutSrc8723A_CE.c b/drivers/staging/rtl8723au/hal/HalDMOutSrc8723A_CE.c
index 179a1ba03029..3f9ec9e00e16 100644
--- a/drivers/staging/rtl8723au/hal/HalDMOutSrc8723A_CE.c
+++ b/drivers/staging/rtl8723au/hal/HalDMOutSrc8723A_CE.c
@@ -23,9 +23,8 @@
 #define		DPK_DELTA_MAPPING_NUM	13
 #define		index_mapping_HP_NUM	15
 /* 091212 chiyokolin */
-static	void
-odm_TXPowerTrackingCallback_ThermalMeter_92C(
-	struct rtw_adapter *Adapter)
+static void
+odm_TXPowerTrackingCallback_ThermalMeter_92C(struct rtw_adapter *Adapter)
 {
 	struct hal_data_8723a *pHalData = GET_HAL_DATA(Adapter);
 	struct dm_priv *pdmpriv = &pHalData->dmpriv;
@@ -35,7 +34,6 @@ odm_TXPowerTrackingCallback_ThermalMeter_92C(
 	s8 OFDM_index[2], CCK_index = 0, OFDM_index_old[2] = {0};
 	s8 CCK_index_old = 0;
 	int i = 0;
-	bool is2T = IS_92C_SERIAL(pHalData->VersionID);
 	u8 OFDM_min_index = 6, rf; /* OFDM BB Swing should be less than +3.0dB*/
 	u8 ThermalValue_HP_count = 0;
 	u32 ThermalValue_HP = 0;
@@ -60,15 +58,15 @@ odm_TXPowerTrackingCallback_ThermalMeter_92C(
 	rtl8723a_phy_ap_calibrate(Adapter, (ThermalValue -
 				  pHalData->EEPROMThermalMeter));
 
-	if (is2T)
+	if (pHalData->rf_type == RF_2T2R)
 		rf = 2;
 	else
 		rf = 1;
 
 	if (ThermalValue) {
 		/* Query OFDM path A default setting	 */
-		ele_D = PHY_QueryBBReg(Adapter, rOFDM0_XATxIQImbalance,
-				       bMaskDWord)&bMaskOFDM_D;
+		ele_D = rtl8723au_read32(Adapter, rOFDM0_XATxIQImbalance) &
+			bMaskOFDM_D;
 		for (i = 0; i < OFDM_TABLE_SIZE_92C; i++) {
 			/* find the index */
 			if (ele_D == (OFDMSwingTable23A[i]&bMaskOFDM_D)) {
@@ -78,9 +76,10 @@ odm_TXPowerTrackingCallback_ThermalMeter_92C(
 		}
 
 		/* Query OFDM path B default setting  */
-		if (is2T) {
-			ele_D = PHY_QueryBBReg(Adapter, rOFDM0_XBTxIQImbalance,
-					       bMaskDWord)&bMaskOFDM_D;
+		if (pHalData->rf_type == RF_2T2R) {
+			ele_D = rtl8723au_read32(Adapter,
+						 rOFDM0_XBTxIQImbalance);
+			ele_D &= bMaskOFDM_D;
 			for (i = 0; i < OFDM_TABLE_SIZE_92C; i++) {	/* find the index  */
 				if (ele_D == (OFDMSwingTable23A[i]&bMaskOFDM_D)) {
 					OFDM_index_old[1] = (u8)i;
@@ -90,8 +89,7 @@ odm_TXPowerTrackingCallback_ThermalMeter_92C(
 		}
 
 		/* Query CCK default setting From 0xa24 */
-		TempCCk = PHY_QueryBBReg(Adapter, rCCK0_TxFilter2,
-					 bMaskDWord)&bMaskCCK;
+		TempCCk = rtl8723au_read32(Adapter, rCCK0_TxFilter2) & bMaskCCK;
 		for (i = 0 ; i < CCK_TABLE_SIZE ; i++) {
 			if (pdmpriv->bCCKinCH14) {
 				if (!memcmp(&TempCCk,
@@ -224,12 +222,13 @@ odm_TXPowerTrackingCallback_ThermalMeter_92C(
 			}
 
 			if (CCK_index > (CCK_TABLE_SIZE-1))
-				CCK_index = (CCK_TABLE_SIZE-1);
+				CCK_index = CCK_TABLE_SIZE-1;
 			else if (CCK_index < 0)
 				CCK_index = 0;
 		}
 
-		if (pdmpriv->TxPowerTrackControl && (delta != 0 || delta_HP != 0)) {
+		if (pdmpriv->TxPowerTrackControl &&
+		    (delta != 0 || delta_HP != 0)) {
 			/* Adujst OFDM Ant_A according to IQK result */
 			ele_D = (OFDMSwingTable23A[OFDM_index[0]] & 0xFFC00000)>>22;
 			X = pdmpriv->RegE94;
@@ -247,7 +246,9 @@ odm_TXPowerTrackingCallback_ThermalMeter_92C(
 
 				/* write new elements A, C, D to regC80 and regC94, element B is always 0 */
 				value32 = (ele_D<<22)|((ele_C&0x3F)<<16)|ele_A;
-				PHY_SetBBReg(Adapter, rOFDM0_XATxIQImbalance, bMaskDWord, value32);
+				rtl8723au_write32(Adapter,
+						  rOFDM0_XATxIQImbalance,
+						  value32);
 
 				value32 = (ele_C&0x000003C0)>>6;
 				PHY_SetBBReg(Adapter, rOFDM0_XCTxAFE, bMaskH4Bits, value32);
@@ -260,9 +261,9 @@ odm_TXPowerTrackingCallback_ThermalMeter_92C(
 				PHY_SetBBReg(Adapter, rOFDM0_ECCAThreshold,
 					     BIT(29), value32);
 			} else {
-				PHY_SetBBReg(Adapter, rOFDM0_XATxIQImbalance,
-					     bMaskDWord,
-					     OFDMSwingTable23A[OFDM_index[0]]);
+				rtl8723au_write32(Adapter,
+						  rOFDM0_XATxIQImbalance,
+						  OFDMSwingTable23A[OFDM_index[0]]);
 				PHY_SetBBReg(Adapter, rOFDM0_XCTxAFE,
 					     bMaskH4Bits, 0x00);
 				PHY_SetBBReg(Adapter, rOFDM0_ECCAThreshold,
@@ -290,7 +291,7 @@ odm_TXPowerTrackingCallback_ThermalMeter_92C(
 				rtl8723au_write8(Adapter, 0xa29, CCKSwingTable_Ch1423A[CCK_index][7]);
 			}
 
-			if (is2T) {
+			if (pHalData->rf_type == RF_2T2R) {
 				ele_D = (OFDMSwingTable23A[(u8)OFDM_index[1]] & 0xFFC00000)>>22;
 
 				/* new element A = element D x X */
@@ -309,7 +310,7 @@ odm_TXPowerTrackingCallback_ThermalMeter_92C(
 
 					/* write new elements A, C, D to regC88 and regC9C, element B is always 0 */
 					value32 = (ele_D<<22)|((ele_C&0x3F)<<16) | ele_A;
-					PHY_SetBBReg(Adapter, rOFDM0_XBTxIQImbalance, bMaskDWord, value32);
+					rtl8723au_write32(Adapter, rOFDM0_XBTxIQImbalance, value32);
 
 					value32 = (ele_C&0x000003C0)>>6;
 					PHY_SetBBReg(Adapter, rOFDM0_XDTxAFE, bMaskH4Bits, value32);
@@ -324,10 +325,9 @@ odm_TXPowerTrackingCallback_ThermalMeter_92C(
 						     rOFDM0_ECCAThreshold,
 						     BIT(25), value32);
 				} else {
-					PHY_SetBBReg(Adapter,
-						     rOFDM0_XBTxIQImbalance,
-						     bMaskDWord,
-						     OFDMSwingTable23A[OFDM_index[1]]);
+					rtl8723au_write32(Adapter,
+							  rOFDM0_XBTxIQImbalance,
+							  OFDMSwingTable23A[OFDM_index[1]]);
 					PHY_SetBBReg(Adapter,
 						     rOFDM0_XDTxAFE,
 						     bMaskH4Bits, 0x00);
@@ -361,14 +361,10 @@ static void ODM_TXPowerTracking92CDirectCall(struct rtw_adapter *Adapter)
 	odm_TXPowerTrackingCallback_ThermalMeter_92C(Adapter);
 }
 
-static void odm_CheckTXPowerTracking_ThermalMeter(struct rtw_adapter *Adapter)
+void rtl8723a_odm_check_tx_power_tracking(struct rtw_adapter *Adapter)
 {
 	struct hal_data_8723a *pHalData = GET_HAL_DATA(Adapter);
 	struct dm_priv *pdmpriv = &pHalData->dmpriv;
-	struct dm_odm_t *podmpriv = &pHalData->odmpriv;
-
-	if (!(podmpriv->SupportAbility & ODM_RF_TX_PWR_TRACK))
-		return;
 
 	if (!pdmpriv->TM_Trigger) {		/* at least delay 1 sec */
 		PHY_SetRFReg(Adapter, RF_PATH_A, RF_T_METER, bRFRegOffsetMask, 0x60);
@@ -381,11 +377,6 @@ static void odm_CheckTXPowerTracking_ThermalMeter(struct rtw_adapter *Adapter)
 	}
 }
 
-void rtl8723a_odm_check_tx_power_tracking(struct rtw_adapter *Adapter)
-{
-	odm_CheckTXPowerTracking_ThermalMeter(Adapter);
-}
-
 /*	IQK */
 #define MAX_TOLERANCE		5
 #define IQK_DELAY_TIME		1	/* ms */
@@ -397,36 +388,37 @@ static u8 _PHY_PathA_IQK(struct rtw_adapter *pAdapter, bool configPathB)
 	struct hal_data_8723a *pHalData = GET_HAL_DATA(pAdapter);
 
 	/* path-A IQK setting */
-	PHY_SetBBReg(pAdapter, rTx_IQK_Tone_A, bMaskDWord, 0x10008c1f);
-	PHY_SetBBReg(pAdapter, rRx_IQK_Tone_A, bMaskDWord, 0x10008c1f);
-	PHY_SetBBReg(pAdapter, rTx_IQK_PI_A, bMaskDWord, 0x82140102);
+	rtl8723au_write32(pAdapter, rTx_IQK_Tone_A, 0x10008c1f);
+	rtl8723au_write32(pAdapter, rRx_IQK_Tone_A, 0x10008c1f);
+	rtl8723au_write32(pAdapter, rTx_IQK_PI_A, 0x82140102);
 
-	PHY_SetBBReg(pAdapter, rRx_IQK_PI_A, bMaskDWord, configPathB ? 0x28160202 :
+	rtl8723au_write32(pAdapter, rRx_IQK_PI_A, configPathB ? 0x28160202 :
 		IS_81xxC_VENDOR_UMC_B_CUT(pHalData->VersionID)?0x28160202:0x28160502);
 
 	/* path-B IQK setting */
 	if (configPathB) {
-		PHY_SetBBReg(pAdapter, rTx_IQK_Tone_B, bMaskDWord, 0x10008c22);
-		PHY_SetBBReg(pAdapter, rRx_IQK_Tone_B, bMaskDWord, 0x10008c22);
-		PHY_SetBBReg(pAdapter, rTx_IQK_PI_B, bMaskDWord, 0x82140102);
-		PHY_SetBBReg(pAdapter, rRx_IQK_PI_B, bMaskDWord, 0x28160202);
+		rtl8723au_write32(pAdapter, rTx_IQK_Tone_B, 0x10008c22);
+		rtl8723au_write32(pAdapter, rRx_IQK_Tone_B, 0x10008c22);
+		rtl8723au_write32(pAdapter, rTx_IQK_PI_B, 0x82140102);
+		rtl8723au_write32(pAdapter, rRx_IQK_PI_B, 0x28160202);
 	}
 
 	/* LO calibration setting */
-	PHY_SetBBReg(pAdapter, rIQK_AGC_Rsp, bMaskDWord, 0x001028d1);
+	rtl8723au_write32(pAdapter, rIQK_AGC_Rsp, 0x001028d1);
 
 	/* One shot, path A LOK & IQK */
-	PHY_SetBBReg(pAdapter, rIQK_AGC_Pts, bMaskDWord, 0xf9000000);
-	PHY_SetBBReg(pAdapter, rIQK_AGC_Pts, bMaskDWord, 0xf8000000);
+	rtl8723au_write32(pAdapter, rIQK_AGC_Pts, 0xf9000000);
+	rtl8723au_write32(pAdapter, rIQK_AGC_Pts, 0xf8000000);
 
 	/*  delay x ms */
-	udelay(IQK_DELAY_TIME*1000);/* PlatformStallExecution(IQK_DELAY_TIME*1000); */
+	/* PlatformStallExecution(IQK_DELAY_TIME*1000); */
+	udelay(IQK_DELAY_TIME*1000);
 
 	/*  Check failed */
-	regEAC = PHY_QueryBBReg(pAdapter, rRx_Power_After_IQK_A_2, bMaskDWord);
-	regE94 = PHY_QueryBBReg(pAdapter, rTx_Power_Before_IQK_A, bMaskDWord);
-	regE9C = PHY_QueryBBReg(pAdapter, rTx_Power_After_IQK_A, bMaskDWord);
-	regEA4 = PHY_QueryBBReg(pAdapter, rRx_Power_Before_IQK_A_2, bMaskDWord);
+	regEAC = rtl8723au_read32(pAdapter, rRx_Power_After_IQK_A_2);
+	regE94 = rtl8723au_read32(pAdapter, rTx_Power_Before_IQK_A);
+	regE9C = rtl8723au_read32(pAdapter, rTx_Power_After_IQK_A);
+	regEA4 = rtl8723au_read32(pAdapter, rRx_Power_Before_IQK_A_2);
 
 	if (!(regEAC & BIT(28)) &&
 	    (((regE94 & 0x03FF0000)>>16) != 0x142) &&
@@ -435,7 +427,7 @@ static u8 _PHY_PathA_IQK(struct rtw_adapter *pAdapter, bool configPathB)
 	else			/* if Tx not OK, ignore Rx */
 		return result;
 
-	if (!(regEAC & BIT(27)) &&		/* if Tx is OK, check whether Rx is OK */
+	if (!(regEAC & BIT(27)) && /* if Tx is OK, check whether Rx is OK */
 	    (((regEA4 & 0x03FF0000)>>16) != 0x132) &&
 	    (((regEAC & 0x03FF0000)>>16) != 0x36))
 		result |= 0x02;
@@ -450,18 +442,18 @@ static u8 _PHY_PathB_IQK(struct rtw_adapter *pAdapter)
 	u8 result = 0x00;
 
 	/* One shot, path B LOK & IQK */
-	PHY_SetBBReg(pAdapter, rIQK_AGC_Cont, bMaskDWord, 0x00000002);
-	PHY_SetBBReg(pAdapter, rIQK_AGC_Cont, bMaskDWord, 0x00000000);
+	rtl8723au_write32(pAdapter, rIQK_AGC_Cont, 0x00000002);
+	rtl8723au_write32(pAdapter, rIQK_AGC_Cont, 0x00000000);
 
 	/*  delay x ms */
 	udelay(IQK_DELAY_TIME*1000);
 
 	/*  Check failed */
-	regEAC = PHY_QueryBBReg(pAdapter, rRx_Power_After_IQK_A_2, bMaskDWord);
-	regEB4 = PHY_QueryBBReg(pAdapter, rTx_Power_Before_IQK_B, bMaskDWord);
-	regEBC = PHY_QueryBBReg(pAdapter, rTx_Power_After_IQK_B, bMaskDWord);
-	regEC4 = PHY_QueryBBReg(pAdapter, rRx_Power_Before_IQK_B_2, bMaskDWord);
-	regECC = PHY_QueryBBReg(pAdapter, rRx_Power_After_IQK_B_2, bMaskDWord);
+	regEAC = rtl8723au_read32(pAdapter, rRx_Power_After_IQK_A_2);
+	regEB4 = rtl8723au_read32(pAdapter, rTx_Power_Before_IQK_B);
+	regEBC = rtl8723au_read32(pAdapter, rTx_Power_After_IQK_B);
+	regEC4 = rtl8723au_read32(pAdapter, rRx_Power_Before_IQK_B_2);
+	regECC = rtl8723au_read32(pAdapter, rRx_Power_After_IQK_B_2);
 
 	if (!(regEAC & BIT(31)) &&
 	    (((regEB4 & 0x03FF0000)>>16) != 0x142) &&
@@ -494,22 +486,27 @@ static void _PHY_PathAFillIQKMatrix(struct rtw_adapter *pAdapter,
 	if (final_candidate == 0xFF) {
 		return;
 	} else if (bIQKOK) {
-		Oldval_0 = (PHY_QueryBBReg(pAdapter, rOFDM0_XATxIQImbalance, bMaskDWord) >> 22) & 0x3FF;
+		Oldval_0 = rtl8723au_read32(pAdapter, rOFDM0_XATxIQImbalance);
+		Oldval_0 = (Oldval_0 >> 22) & 0x3FF;
 
 		X = result[final_candidate][0];
 		if ((X & 0x00000200) != 0)
 			X = X | 0xFFFFFC00;
 		TX0_A = (X * Oldval_0) >> 8;
 		PHY_SetBBReg(pAdapter, rOFDM0_XATxIQImbalance, 0x3FF, TX0_A);
-		PHY_SetBBReg(pAdapter, rOFDM0_ECCAThreshold, BIT(31), ((X * Oldval_0>>7) & 0x1));
+		PHY_SetBBReg(pAdapter, rOFDM0_ECCAThreshold, BIT(31),
+			     ((X * Oldval_0>>7) & 0x1));
 
 		Y = result[final_candidate][1];
 		if ((Y & 0x00000200) != 0)
 			Y = Y | 0xFFFFFC00;
 		TX0_C = (Y * Oldval_0) >> 8;
-		PHY_SetBBReg(pAdapter, rOFDM0_XCTxAFE, 0xF0000000, ((TX0_C&0x3C0)>>6));
-		PHY_SetBBReg(pAdapter, rOFDM0_XATxIQImbalance, 0x003F0000, (TX0_C&0x3F));
-		PHY_SetBBReg(pAdapter, rOFDM0_ECCAThreshold, BIT(29), ((Y * Oldval_0>>7) & 0x1));
+		PHY_SetBBReg(pAdapter, rOFDM0_XCTxAFE, 0xF0000000,
+			     ((TX0_C&0x3C0)>>6));
+		PHY_SetBBReg(pAdapter, rOFDM0_XATxIQImbalance, 0x003F0000,
+			     (TX0_C&0x3F));
+		PHY_SetBBReg(pAdapter, rOFDM0_ECCAThreshold, BIT(29),
+			     ((Y * Oldval_0>>7) & 0x1));
 
 		if (bTxOnly) {
 			DBG_8723A("_PHY_PathAFillIQKMatrix only Tx OK\n");
@@ -537,22 +534,27 @@ static void _PHY_PathBFillIQKMatrix(struct rtw_adapter *pAdapter, bool bIQKOK, i
 	if (final_candidate == 0xFF) {
 		return;
 	} else if (bIQKOK) {
-		Oldval_1 = (PHY_QueryBBReg(pAdapter, rOFDM0_XBTxIQImbalance, bMaskDWord) >> 22) & 0x3FF;
+		Oldval_1 = rtl8723au_read32(pAdapter, rOFDM0_XBTxIQImbalance);
+		Oldval_1 = (Oldval_1 >> 22) & 0x3FF;
 
 		X = result[final_candidate][4];
 		if ((X & 0x00000200) != 0)
 			X = X | 0xFFFFFC00;
 		TX1_A = (X * Oldval_1) >> 8;
 		PHY_SetBBReg(pAdapter, rOFDM0_XBTxIQImbalance, 0x3FF, TX1_A);
-		PHY_SetBBReg(pAdapter, rOFDM0_ECCAThreshold, BIT(27), ((X * Oldval_1>>7) & 0x1));
+		PHY_SetBBReg(pAdapter, rOFDM0_ECCAThreshold, BIT(27),
+			     ((X * Oldval_1 >> 7) & 0x1));
 
 		Y = result[final_candidate][5];
 		if ((Y & 0x00000200) != 0)
 			Y = Y | 0xFFFFFC00;
 		TX1_C = (Y * Oldval_1) >> 8;
-		PHY_SetBBReg(pAdapter, rOFDM0_XDTxAFE, 0xF0000000, ((TX1_C&0x3C0)>>6));
-		PHY_SetBBReg(pAdapter, rOFDM0_XBTxIQImbalance, 0x003F0000, (TX1_C&0x3F));
-		PHY_SetBBReg(pAdapter, rOFDM0_ECCAThreshold, BIT(25), ((Y * Oldval_1>>7) & 0x1));
+		PHY_SetBBReg(pAdapter, rOFDM0_XDTxAFE, 0xF0000000,
+			     ((TX1_C & 0x3C0) >> 6));
+		PHY_SetBBReg(pAdapter, rOFDM0_XBTxIQImbalance, 0x003F0000,
+			     (TX1_C & 0x3F));
+		PHY_SetBBReg(pAdapter, rOFDM0_ECCAThreshold, BIT(25),
+			     ((Y * Oldval_1 >> 7) & 0x1));
 
 		if (bTxOnly)
 			return;
@@ -573,11 +575,12 @@ static void _PHY_SaveADDARegisters(struct rtw_adapter *pAdapter, u32 *ADDAReg, u
 	u32 i;
 
 	for (i = 0 ; i < RegisterNum ; i++) {
-		ADDABackup[i] = PHY_QueryBBReg(pAdapter, ADDAReg[i], bMaskDWord);
+		ADDABackup[i] = rtl8723au_read32(pAdapter, ADDAReg[i]);
 	}
 }
 
-static void _PHY_SaveMACRegisters(struct rtw_adapter *pAdapter, u32 *MACReg, u32 *MACBackup)
+static void _PHY_SaveMACRegisters(struct rtw_adapter *pAdapter, u32 *MACReg,
+				  u32 *MACBackup)
 {
 	u32 i;
 
@@ -587,16 +590,19 @@ static void _PHY_SaveMACRegisters(struct rtw_adapter *pAdapter, u32 *MACReg, u32
 	MACBackup[i] = rtl8723au_read32(pAdapter, MACReg[i]);
 }
 
-static void _PHY_ReloadADDARegisters(struct rtw_adapter *pAdapter, u32 *ADDAReg, u32 *ADDABackup, u32 RegiesterNum)
+static void _PHY_ReloadADDARegisters(struct rtw_adapter *pAdapter,
+				     u32 *ADDAReg, u32 *ADDABackup,
+				     u32 RegiesterNum)
 {
 	u32 i;
 
 	for (i = 0 ; i < RegiesterNum ; i++) {
-		PHY_SetBBReg(pAdapter, ADDAReg[i], bMaskDWord, ADDABackup[i]);
+		rtl8723au_write32(pAdapter, ADDAReg[i], ADDABackup[i]);
 	}
 }
 
-static void _PHY_ReloadMACRegisters(struct rtw_adapter *pAdapter, u32 *MACReg, u32 *MACBackup)
+static void _PHY_ReloadMACRegisters(struct rtw_adapter *pAdapter,
+				    u32 *MACReg, u32 *MACBackup)
 {
 	u32 i;
 
@@ -606,7 +612,8 @@ static void _PHY_ReloadMACRegisters(struct rtw_adapter *pAdapter, u32 *MACReg, u
 	rtl8723au_write32(pAdapter, MACReg[i], MACBackup[i]);
 }
 
-static void _PHY_PathADDAOn(struct rtw_adapter *pAdapter, u32 *ADDAReg, bool isPathAOn, bool is2T)
+static void _PHY_PathADDAOn(struct rtw_adapter *pAdapter, u32 *ADDAReg,
+			    bool isPathAOn, bool is2T)
 {
 	u32 pathOn;
 	u32 i;
@@ -614,16 +621,17 @@ static void _PHY_PathADDAOn(struct rtw_adapter *pAdapter, u32 *ADDAReg, bool isP
 	pathOn = isPathAOn ? 0x04db25a4 : 0x0b1b25a4;
 	if (!is2T) {
 		pathOn = 0x0bdb25a0;
-		PHY_SetBBReg(pAdapter, ADDAReg[0], bMaskDWord, 0x0b1b25a0);
+		rtl8723au_write32(pAdapter, ADDAReg[0], 0x0b1b25a0);
 	} else {
-		PHY_SetBBReg(pAdapter, ADDAReg[0], bMaskDWord, pathOn);
+		rtl8723au_write32(pAdapter, ADDAReg[0], pathOn);
 	}
 
 	for (i = 1 ; i < IQK_ADDA_REG_NUM ; i++)
-		PHY_SetBBReg(pAdapter, ADDAReg[i], bMaskDWord, pathOn);
+		rtl8723au_write32(pAdapter, ADDAReg[i], pathOn);
 }
 
-static void _PHY_MACSettingCalibration(struct rtw_adapter *pAdapter, u32 *MACReg, u32 *MACBackup)
+static void _PHY_MACSettingCalibration(struct rtw_adapter *pAdapter,
+				       u32 *MACReg, u32 *MACBackup)
 {
 	u32 i = 0;
 
@@ -638,9 +646,9 @@ static void _PHY_MACSettingCalibration(struct rtw_adapter *pAdapter, u32 *MACReg
 
 static void _PHY_PathAStandBy(struct rtw_adapter *pAdapter)
 {
-	PHY_SetBBReg(pAdapter, rFPGA0_IQK, bMaskDWord, 0x0);
-	PHY_SetBBReg(pAdapter, 0x840, bMaskDWord, 0x00010000);
-	PHY_SetBBReg(pAdapter, rFPGA0_IQK, bMaskDWord, 0x80800000);
+	rtl8723au_write32(pAdapter, rFPGA0_IQK, 0x0);
+	rtl8723au_write32(pAdapter, 0x840, 0x00010000);
+	rtl8723au_write32(pAdapter, rFPGA0_IQK, 0x80800000);
 }
 
 static void _PHY_PIModeSwitch(struct rtw_adapter *pAdapter, bool PIMode)
@@ -648,8 +656,8 @@ static void _PHY_PIModeSwitch(struct rtw_adapter *pAdapter, bool PIMode)
 	u32 mode;
 
 	mode = PIMode ? 0x01000100 : 0x01000000;
-	PHY_SetBBReg(pAdapter, 0x820, bMaskDWord, mode);
-	PHY_SetBBReg(pAdapter, 0x828, bMaskDWord, mode);
+	rtl8723au_write32(pAdapter, 0x820, mode);
+	rtl8723au_write32(pAdapter, 0x828, mode);
 }
 
 /*
@@ -660,9 +668,9 @@ static bool _PHY_SimularityCompare(struct rtw_adapter *pAdapter, int result[][8]
 	u32 i, j, diff, SimularityBitMap, bound = 0;
 	struct hal_data_8723a *pHalData = GET_HAL_DATA(pAdapter);
 	u8 final_candidate[2] = {0xFF, 0xFF};	/* for path A and path B */
-	bool bResult = true, is2T = IS_92C_SERIAL(pHalData->VersionID);
+	bool bResult = true;
 
-	if (is2T)
+	if (pHalData->rf_type == RF_2T2R)
 		bound = 8;
 	else
 		bound = 4;
@@ -699,7 +707,7 @@ static bool _PHY_SimularityCompare(struct rtw_adapter *pAdapter, int result[][8]
 		for (i = 0; i < 4; i++)
 			result[3][i] = result[c1][i];
 		return false;
-	} else if (!(SimularityBitMap & 0xF0) && is2T) {
+	} else if (!(SimularityBitMap & 0xF0) && pHalData->rf_type == RF_2T2R) {
 		/* path B OK */
 		for (i = 4; i < 8; i++)
 			result[3][i] = result[c1][i];
@@ -746,7 +754,7 @@ static void _PHY_IQCalibrate(struct rtw_adapter *pAdapter, int result[][8], u8 t
 	u32 bbvalue;
 
 	if (t == 0) {
-		bbvalue = PHY_QueryBBReg(pAdapter, rFPGA0_RFMOD, bMaskDWord);
+		bbvalue = rtl8723au_read32(pAdapter, rFPGA0_RFMOD);
 
 		/*  Save ADDA parameters, turn Path A ADDA on */
 		_PHY_SaveADDARegisters(pAdapter, ADDA_REG, pdmpriv->ADDA_backup, IQK_ADDA_REG_NUM);
@@ -766,48 +774,50 @@ static void _PHY_IQCalibrate(struct rtw_adapter *pAdapter, int result[][8], u8 t
 	}
 
 	PHY_SetBBReg(pAdapter, rFPGA0_RFMOD, BIT(24), 0x00);
-	PHY_SetBBReg(pAdapter, rOFDM0_TRxPathEnable, bMaskDWord, 0x03a05600);
-	PHY_SetBBReg(pAdapter, rOFDM0_TRMuxPar, bMaskDWord, 0x000800e4);
-	PHY_SetBBReg(pAdapter, rFPGA0_XCD_RFInterfaceSW, bMaskDWord, 0x22204000);
+	rtl8723au_write32(pAdapter, rOFDM0_TRxPathEnable, 0x03a05600);
+	rtl8723au_write32(pAdapter, rOFDM0_TRMuxPar, 0x000800e4);
+	rtl8723au_write32(pAdapter, rFPGA0_XCD_RFInterfaceSW, 0x22204000);
 	PHY_SetBBReg(pAdapter, rFPGA0_XAB_RFInterfaceSW, BIT(10), 0x01);
 	PHY_SetBBReg(pAdapter, rFPGA0_XAB_RFInterfaceSW, BIT(26), 0x01);
 	PHY_SetBBReg(pAdapter, rFPGA0_XA_RFInterfaceOE, BIT(10), 0x00);
 	PHY_SetBBReg(pAdapter, rFPGA0_XB_RFInterfaceOE, BIT(10), 0x00);
 
 	if (is2T) {
-		PHY_SetBBReg(pAdapter, rFPGA0_XA_LSSIParameter, bMaskDWord, 0x00010000);
-		PHY_SetBBReg(pAdapter, rFPGA0_XB_LSSIParameter, bMaskDWord, 0x00010000);
+		rtl8723au_write32(pAdapter,
+				  rFPGA0_XA_LSSIParameter, 0x00010000);
+		rtl8723au_write32(pAdapter,
+				  rFPGA0_XB_LSSIParameter, 0x00010000);
 	}
 
 	/* MAC settings */
 	_PHY_MACSettingCalibration(pAdapter, IQK_MAC_REG, pdmpriv->IQK_MAC_backup);
 
 	/* Page B init */
-	PHY_SetBBReg(pAdapter, rConfig_AntA, bMaskDWord, 0x00080000);
+	rtl8723au_write32(pAdapter, rConfig_AntA, 0x00080000);
 
 	if (is2T)
-		PHY_SetBBReg(pAdapter, rConfig_AntB, bMaskDWord, 0x00080000);
+		rtl8723au_write32(pAdapter, rConfig_AntB, 0x00080000);
 
 	/*  IQ calibration setting */
-	PHY_SetBBReg(pAdapter, rFPGA0_IQK, bMaskDWord, 0x80800000);
-	PHY_SetBBReg(pAdapter, rTx_IQK, bMaskDWord, 0x01007c00);
-	PHY_SetBBReg(pAdapter, rRx_IQK, bMaskDWord, 0x01004800);
+	rtl8723au_write32(pAdapter, rFPGA0_IQK, 0x80800000);
+	rtl8723au_write32(pAdapter, rTx_IQK, 0x01007c00);
+	rtl8723au_write32(pAdapter, rRx_IQK, 0x01004800);
 
 	for (i = 0 ; i < retryCount ; i++) {
 		PathAOK = _PHY_PathA_IQK(pAdapter, is2T);
 		if (PathAOK == 0x03) {
 				DBG_8723A("Path A IQK Success!!\n");
-				result[t][0] = (PHY_QueryBBReg(pAdapter, rTx_Power_Before_IQK_A, bMaskDWord)&0x3FF0000)>>16;
-				result[t][1] = (PHY_QueryBBReg(pAdapter, rTx_Power_After_IQK_A, bMaskDWord)&0x3FF0000)>>16;
-				result[t][2] = (PHY_QueryBBReg(pAdapter, rRx_Power_Before_IQK_A_2, bMaskDWord)&0x3FF0000)>>16;
-				result[t][3] = (PHY_QueryBBReg(pAdapter, rRx_Power_After_IQK_A_2, bMaskDWord)&0x3FF0000)>>16;
+				result[t][0] = (rtl8723au_read32(pAdapter, rTx_Power_Before_IQK_A)&0x3FF0000)>>16;
+				result[t][1] = (rtl8723au_read32(pAdapter, rTx_Power_After_IQK_A)&0x3FF0000)>>16;
+				result[t][2] = (rtl8723au_read32(pAdapter, rRx_Power_Before_IQK_A_2)&0x3FF0000)>>16;
+				result[t][3] = (rtl8723au_read32(pAdapter, rRx_Power_After_IQK_A_2)&0x3FF0000)>>16;
 			break;
 		} else if (i == (retryCount-1) && PathAOK == 0x01) {
 			/* Tx IQK OK */
 			DBG_8723A("Path A IQK Only  Tx Success!!\n");
 
-			result[t][0] = (PHY_QueryBBReg(pAdapter, rTx_Power_Before_IQK_A, bMaskDWord)&0x3FF0000)>>16;
-			result[t][1] = (PHY_QueryBBReg(pAdapter, rTx_Power_After_IQK_A, bMaskDWord)&0x3FF0000)>>16;
+			result[t][0] = (rtl8723au_read32(pAdapter, rTx_Power_Before_IQK_A)&0x3FF0000)>>16;
+			result[t][1] = (rtl8723au_read32(pAdapter, rTx_Power_After_IQK_A)&0x3FF0000)>>16;
 		}
 	}
 
@@ -825,16 +835,16 @@ static void _PHY_IQCalibrate(struct rtw_adapter *pAdapter, int result[][8], u8 t
 			PathBOK = _PHY_PathB_IQK(pAdapter);
 			if (PathBOK == 0x03) {
 				DBG_8723A("Path B IQK Success!!\n");
-				result[t][4] = (PHY_QueryBBReg(pAdapter, rTx_Power_Before_IQK_B, bMaskDWord)&0x3FF0000)>>16;
-				result[t][5] = (PHY_QueryBBReg(pAdapter, rTx_Power_After_IQK_B, bMaskDWord)&0x3FF0000)>>16;
-				result[t][6] = (PHY_QueryBBReg(pAdapter, rRx_Power_Before_IQK_B_2, bMaskDWord)&0x3FF0000)>>16;
-				result[t][7] = (PHY_QueryBBReg(pAdapter, rRx_Power_After_IQK_B_2, bMaskDWord)&0x3FF0000)>>16;
+				result[t][4] = (rtl8723au_read32(pAdapter, rTx_Power_Before_IQK_B)&0x3FF0000)>>16;
+				result[t][5] = (rtl8723au_read32(pAdapter, rTx_Power_After_IQK_B)&0x3FF0000)>>16;
+				result[t][6] = (rtl8723au_read32(pAdapter, rRx_Power_Before_IQK_B_2)&0x3FF0000)>>16;
+				result[t][7] = (rtl8723au_read32(pAdapter, rRx_Power_After_IQK_B_2)&0x3FF0000)>>16;
 				break;
 			} else if (i == (retryCount - 1) && PathBOK == 0x01) {
 				/* Tx IQK OK */
 				DBG_8723A("Path B Only Tx IQK Success!!\n");
-				result[t][4] = (PHY_QueryBBReg(pAdapter, rTx_Power_Before_IQK_B, bMaskDWord)&0x3FF0000)>>16;
-				result[t][5] = (PHY_QueryBBReg(pAdapter, rTx_Power_After_IQK_B, bMaskDWord)&0x3FF0000)>>16;
+				result[t][4] = (rtl8723au_read32(pAdapter, rTx_Power_Before_IQK_B)&0x3FF0000)>>16;
+				result[t][5] = (rtl8723au_read32(pAdapter, rTx_Power_After_IQK_B)&0x3FF0000)>>16;
 			}
 		}
 
@@ -844,7 +854,7 @@ static void _PHY_IQCalibrate(struct rtw_adapter *pAdapter, int result[][8], u8 t
 	}
 
 	/* Back to BB mode, load original value */
-	PHY_SetBBReg(pAdapter, rFPGA0_IQK, bMaskDWord, 0);
+	rtl8723au_write32(pAdapter, rFPGA0_IQK, 0);
 
 	if (t != 0) {
 		if (!pdmpriv->bRfPiEnable) {
@@ -862,14 +872,16 @@ static void _PHY_IQCalibrate(struct rtw_adapter *pAdapter, int result[][8], u8 t
 		_PHY_ReloadADDARegisters(pAdapter, IQK_BB_REG_92C, pdmpriv->IQK_BB_backup, IQK_BB_REG_NUM);
 
 		/*  Restore RX initial gain */
-		PHY_SetBBReg(pAdapter, rFPGA0_XA_LSSIParameter, bMaskDWord, 0x00032ed3);
+		rtl8723au_write32(pAdapter,
+				  rFPGA0_XA_LSSIParameter, 0x00032ed3);
 		if (is2T) {
-			PHY_SetBBReg(pAdapter, rFPGA0_XB_LSSIParameter, bMaskDWord, 0x00032ed3);
+			rtl8723au_write32(pAdapter,
+					  rFPGA0_XB_LSSIParameter, 0x00032ed3);
 		}
 
 		/* load 0xe30 IQC default value */
-		PHY_SetBBReg(pAdapter, rTx_IQK_Tone_A, bMaskDWord, 0x01008c00);
-		PHY_SetBBReg(pAdapter, rRx_IQK_Tone_A, bMaskDWord, 0x01008c00);
+		rtl8723au_write32(pAdapter, rTx_IQK_Tone_A, 0x01008c00);
+		rtl8723au_write32(pAdapter, rRx_IQK_Tone_A, 0x01008c00);
 
 	}
 }
@@ -980,12 +992,10 @@ void rtl8723a_phy_iq_calibrate(struct rtw_adapter *pAdapter, bool bReCovery)
 	is13simular = false;
 
 	for (i = 0; i < 3; i++) {
-		if (IS_92C_SERIAL(pHalData->VersionID)) {
-			 _PHY_IQCalibrate(pAdapter, result, i, true);
-		} else {
-			/*  For 88C 1T1R */
+		if (pHalData->rf_type == RF_2T2R)
+			_PHY_IQCalibrate(pAdapter, result, i, true);
+		else /*  For 88C 1T1R */
 			_PHY_IQCalibrate(pAdapter, result, i, false);
-		}
 
 		if (i == 1) {
 			is12simular = _PHY_SimularityCompare(pAdapter, result, 0, 1);
@@ -1053,9 +1063,10 @@ void rtl8723a_phy_iq_calibrate(struct rtw_adapter *pAdapter, bool bReCovery)
 	if ((RegE94 != 0)/*&&(RegEA4 != 0)*/)
 		_PHY_PathAFillIQKMatrix(pAdapter, bPathAOK, result, final_candidate, (RegEA4 == 0));
 
-	if (IS_92C_SERIAL(pHalData->VersionID)) {
+	if (pHalData->rf_type == RF_2T2R) {
 		if ((RegEB4 != 0)/*&&(RegEC4 != 0)*/)
-		_PHY_PathBFillIQKMatrix(pAdapter, bPathBOK, result, final_candidate, (RegEC4 == 0));
+			_PHY_PathBFillIQKMatrix(pAdapter, bPathBOK, result,
+						final_candidate, (RegEC4 == 0));
 	}
 
 	_PHY_SaveADDARegisters(pAdapter, IQK_BB_REG_92C, pdmpriv->IQK_BB_backup_recover, 9);
@@ -1074,12 +1085,10 @@ void rtl8723a_phy_lc_calibrate(struct rtw_adapter *pAdapter)
 	if (pmlmeext->sitesurvey_res.state == SCAN_PROCESS)
 		return;
 
-	if (IS_92C_SERIAL(pHalData->VersionID)) {
+	if (pHalData->rf_type == RF_2T2R)
 		_PHY_LCCalibrate(pAdapter, true);
-	} else {
-		/*  For 88C 1T1R */
+	else	/*  For 88C 1T1R */
 		_PHY_LCCalibrate(pAdapter, false);
-	}
 }
 
 void
diff --git a/drivers/staging/rtl8723au/hal/HalHWImg8723A_BB.c b/drivers/staging/rtl8723au/hal/HalHWImg8723A_BB.c
index 9d4f6bed4269..e8cab9e97385 100644
--- a/drivers/staging/rtl8723au/hal/HalHWImg8723A_BB.c
+++ b/drivers/staging/rtl8723au/hal/HalHWImg8723A_BB.c
@@ -30,12 +30,12 @@ static bool CheckCondition(const u32  Condition, const u32  Hex)
 		return false;
 
 	cond = Condition & 0x0000FF00;
-	cond = cond >> 8;
+	cond >>= 8;
 	if ((_interface & cond) == 0 && cond != 0x07)
 		return false;
 
 	cond = Condition & 0x00FF0000;
-	cond = cond >> 16;
+	cond >>= 16;
 	if ((_platform & cond) == 0 && cond != 0x0F)
 		return false;
 	return true;
@@ -215,17 +215,15 @@ static u32 Array_AGC_TAB_1T_8723A[] = {
 
 void ODM_ReadAndConfig_AGC_TAB_1T_8723A(struct dm_odm_t *pDM_Odm)
 {
-
 	u32 hex;
 	u32 i;
 	u8 platform = 0x04;
-	u8 interfaceValue   = pDM_Odm->SupportInterface;
 	u8 board = pDM_Odm->BoardType;
 	u32 ArrayLen = sizeof(Array_AGC_TAB_1T_8723A)/sizeof(u32);
 	u32 *Array = Array_AGC_TAB_1T_8723A;
 
 	hex = board;
-	hex += interfaceValue << 8;
+	hex += ODM_ITRF_USB << 8;
 	hex += platform << 16;
 	hex += 0xFF000000;
 	for (i = 0; i < ArrayLen; i += 2) {
@@ -234,11 +232,11 @@ void ODM_ReadAndConfig_AGC_TAB_1T_8723A(struct dm_odm_t *pDM_Odm)
 
 		/*  This (offset, data) pair meets the condition. */
 		if (v1 < 0xCDCDCDCD) {
-			odm_ConfigBB_AGC_8723A(pDM_Odm, v1, bMaskDWord, v2);
+			odm_ConfigBB_AGC_8723A(pDM_Odm, v1, v2);
 			continue;
 		} else {
 			if (!CheckCondition(Array[i], hex)) {
-				/*  Discard the following (offset, data) pairs. */
+				/* Discard the following (offset, data) pairs */
 				READ_NEXT_PAIR(v1, v2, i);
 				while (v2 != 0xDEAD &&
 				       v2 != 0xCDEF &&
@@ -246,12 +244,13 @@ void ODM_ReadAndConfig_AGC_TAB_1T_8723A(struct dm_odm_t *pDM_Odm)
 					READ_NEXT_PAIR(v1, v2, i);
 				i -= 2; /*  prevent from for-loop += 2 */
 			} else {
-				/*  Configure matched pairs and skip to end of if-else. */
+				/*  Configure matched pairs and skip to
+				    end of if-else. */
 				READ_NEXT_PAIR(v1, v2, i);
 				while (v2 != 0xDEAD &&
 				       v2 != 0xCDEF &&
 				       v2 != 0xCDCD && i < ArrayLen - 2) {
-					odm_ConfigBB_AGC_8723A(pDM_Odm, v1, bMaskDWord, v2);
+					odm_ConfigBB_AGC_8723A(pDM_Odm, v1, v2);
 					READ_NEXT_PAIR(v1, v2, i);
 				}
 				while (v2 != 0xDEAD && i < ArrayLen - 2)
@@ -467,13 +466,12 @@ void ODM_ReadAndConfig_PHY_REG_1T_8723A(struct dm_odm_t *pDM_Odm)
 	u32 hex = 0;
 	u32 i = 0;
 	u8  platform = 0x04;
-	u8  interfaceValue = pDM_Odm->SupportInterface;
 	u8  board = pDM_Odm->BoardType;
 	u32 ArrayLen = sizeof(Array_PHY_REG_1T_8723A)/sizeof(u32);
 	u32 *Array = Array_PHY_REG_1T_8723A;
 
 	hex += board;
-	hex += interfaceValue << 8;
+	hex += ODM_ITRF_USB << 8;
 	hex += platform << 16;
 	hex += 0xFF000000;
 	for (i = 0; i < ArrayLen; i += 2) {
@@ -482,11 +480,11 @@ void ODM_ReadAndConfig_PHY_REG_1T_8723A(struct dm_odm_t *pDM_Odm)
 
 		/*  This (offset, data) pair meets the condition. */
 		if (v1 < 0xCDCDCDCD) {
-			odm_ConfigBB_PHY_8723A(pDM_Odm, v1, bMaskDWord, v2);
+			odm_ConfigBB_PHY_8723A(pDM_Odm, v1, v2);
 			continue;
 		} else {
 			if (!CheckCondition(Array[i], hex)) {
-				/*  Discard the following (offset, data) pairs. */
+				/* Discard the following (offset, data) pairs */
 				READ_NEXT_PAIR(v1, v2, i);
 				while (v2 != 0xDEAD &&
 				       v2 != 0xCDEF &&
@@ -494,12 +492,13 @@ void ODM_ReadAndConfig_PHY_REG_1T_8723A(struct dm_odm_t *pDM_Odm)
 					READ_NEXT_PAIR(v1, v2, i);
 				i -= 2; /*  prevent from for-loop += 2 */
 			} else {
-				/*  Configure matched pairs and skip to end of if-else. */
+				/*  Configure matched pairs and skip to
+				    end of if-else. */
 				READ_NEXT_PAIR(v1, v2, i);
 				while (v2 != 0xDEAD &&
 				       v2 != 0xCDEF &&
 				       v2 != 0xCDCD && i < ArrayLen - 2) {
-					odm_ConfigBB_PHY_8723A(pDM_Odm, v1, bMaskDWord, v2);
+					odm_ConfigBB_PHY_8723A(pDM_Odm, v1, v2);
 					READ_NEXT_PAIR(v1, v2, i);
 				}
 				while (v2 != 0xDEAD && i < ArrayLen - 2)
@@ -520,16 +519,15 @@ static u32 Array_PHY_REG_MP_8723A[] = {
 
 void ODM_ReadAndConfig_PHY_REG_MP_8723A(struct dm_odm_t *pDM_Odm)
 {
-	u32     hex         = 0;
-	u32     i           = 0;
-	u8     platform    = 0x04;
-	u8     interfaceValue   = pDM_Odm->SupportInterface;
-	u8     board       = pDM_Odm->BoardType;
-	u32     ArrayLen    = sizeof(Array_PHY_REG_MP_8723A)/sizeof(u32);
-	u32 *Array       = Array_PHY_REG_MP_8723A;
+	u32 hex = 0;
+	u32 i;
+	u8 platform = 0x04;
+	u8 board = pDM_Odm->BoardType;
+	u32 ArrayLen = sizeof(Array_PHY_REG_MP_8723A)/sizeof(u32);
+	u32 *Array = Array_PHY_REG_MP_8723A;
 
 	hex += board;
-	hex += interfaceValue << 8;
+	hex += ODM_ITRF_USB << 8;
 	hex += platform << 16;
 	hex += 0xFF000000;
 	for (i = 0; i < ArrayLen; i += 2) {
@@ -538,11 +536,11 @@ void ODM_ReadAndConfig_PHY_REG_MP_8723A(struct dm_odm_t *pDM_Odm)
 
 		/*  This (offset, data) pair meets the condition. */
 		if (v1 < 0xCDCDCDCD) {
-			odm_ConfigBB_PHY_8723A(pDM_Odm, v1, bMaskDWord, v2);
+			odm_ConfigBB_PHY_8723A(pDM_Odm, v1, v2);
 			continue;
 		} else {
 			if (!CheckCondition(Array[i], hex)) {
-				/* Discard the following (offset, data) pairs. */
+				/* Discard the following (offset, data) pairs */
 				READ_NEXT_PAIR(v1, v2, i);
 				while (v2 != 0xDEAD &&
 				       v2 != 0xCDEF &&
@@ -550,12 +548,13 @@ void ODM_ReadAndConfig_PHY_REG_MP_8723A(struct dm_odm_t *pDM_Odm)
 					READ_NEXT_PAIR(v1, v2, i);
 				i -= 2; /*  prevent from for-loop += 2 */
 			} else {
-				/* Configure matched pairs and skip to end of if-else. */
+				/* Configure matched pairs and skip to
+				   end of if-else. */
 				READ_NEXT_PAIR(v1, v2, i);
 				while (v2 != 0xDEAD &&
 				       v2 != 0xCDEF &&
 				       v2 != 0xCDCD && i < ArrayLen - 2) {
-					odm_ConfigBB_PHY_8723A(pDM_Odm, v1, bMaskDWord, v2);
+					odm_ConfigBB_PHY_8723A(pDM_Odm, v1, v2);
 					READ_NEXT_PAIR(v1, v2, i);
 				}
 				while (v2 != 0xDEAD && i < ArrayLen - 2)
diff --git a/drivers/staging/rtl8723au/hal/HalHWImg8723A_MAC.c b/drivers/staging/rtl8723au/hal/HalHWImg8723A_MAC.c
index 12071453be97..93b2d183d694 100644
--- a/drivers/staging/rtl8723au/hal/HalHWImg8723A_MAC.c
+++ b/drivers/staging/rtl8723au/hal/HalHWImg8723A_MAC.c
@@ -30,12 +30,12 @@ static bool CheckCondition(const u32  Condition, const u32  Hex)
 		return false;
 
 	cond = Condition & 0x0000FF00;
-	cond = cond >> 8;
+	cond >>= 8;
 	if ((_interface & cond) == 0 && cond != 0x07)
 		return false;
 
 	cond = Condition & 0x00FF0000;
-	cond = cond >> 16;
+	cond >>= 16;
 	if ((_platform & cond) == 0 && cond != 0x0F)
 		return false;
 	return true;
@@ -144,13 +144,12 @@ void ODM_ReadAndConfig_MAC_REG_8723A(struct dm_odm_t *pDM_Odm)
 	u32     hex         = 0;
 	u32     i           = 0;
 	u8     platform    = 0x04;
-	u8     interfaceValue   = pDM_Odm->SupportInterface;
 	u8     board       = pDM_Odm->BoardType;
 	u32     ArrayLen    = sizeof(Array_MAC_REG_8723A)/sizeof(u32);
 	u32 *Array       = Array_MAC_REG_8723A;
 
 	hex += board;
-	hex += interfaceValue << 8;
+	hex += ODM_ITRF_USB << 8;
 	hex += platform << 16;
 	hex += 0xFF000000;
 	for (i = 0; i < ArrayLen; i += 2) {
diff --git a/drivers/staging/rtl8723au/hal/HalHWImg8723A_RF.c b/drivers/staging/rtl8723au/hal/HalHWImg8723A_RF.c
index 00480f5fcdab..dbf571e8b908 100644
--- a/drivers/staging/rtl8723au/hal/HalHWImg8723A_RF.c
+++ b/drivers/staging/rtl8723au/hal/HalHWImg8723A_RF.c
@@ -30,12 +30,12 @@ static bool CheckCondition(const u32  Condition, const u32  Hex)
 		return false;
 
 	cond = Condition & 0x0000FF00;
-	cond = cond >> 8;
+	cond >>= 8;
 	if ((_interface & cond) == 0 && cond != 0x07)
 		return false;
 
 	cond = Condition & 0x00FF0000;
-	cond = cond >> 16;
+	cond >>= 16;
 	if ((_platform & cond) == 0 && cond != 0x0F)
 		return false;
 	return true;
@@ -214,13 +214,12 @@ void ODM_ReadAndConfig_RadioA_1T_8723A(struct dm_odm_t *pDM_Odm)
 	u32     hex         = 0;
 	u32     i           = 0;
 	u8     platform    = 0x04;
-	u8     interfaceValue   = pDM_Odm->SupportInterface;
 	u8     board       = pDM_Odm->BoardType;
 	u32     ArrayLen    = sizeof(Array_RadioA_1T_8723A)/sizeof(u32);
 	u32 *Array = Array_RadioA_1T_8723A;
 
 	hex += board;
-	hex += interfaceValue << 8;
+	hex += ODM_ITRF_USB << 8;
 	hex += platform << 16;
 	hex += 0xFF000000;
 
diff --git a/drivers/staging/rtl8723au/hal/HalPwrSeqCmd.c b/drivers/staging/rtl8723au/hal/HalPwrSeqCmd.c
index 33777d2852f4..ae090ab11585 100644
--- a/drivers/staging/rtl8723au/hal/HalPwrSeqCmd.c
+++ b/drivers/staging/rtl8723au/hal/HalPwrSeqCmd.c
@@ -59,17 +59,15 @@ u8 HalPwrSeqCmdParsing23a(struct rtw_adapter *padapter, u8 CutVersion,
 		PwrCfgCmd = PwrSeqCmd[AryIdx];
 
 		RT_TRACE(_module_hal_init_c_, _drv_info_,
-			 ("HalPwrSeqCmdParsing23a: offset(%#x) cut_msk(%#x) "
-			  "fab_msk(%#x) interface_msk(%#x) base(%#x) cmd(%#x) "
-			  "msk(%#x) value(%#x)\n",
-			  GET_PWR_CFG_OFFSET(PwrCfgCmd),
-			  GET_PWR_CFG_CUT_MASK(PwrCfgCmd),
-			  GET_PWR_CFG_FAB_MASK(PwrCfgCmd),
-			  GET_PWR_CFG_INTF_MASK(PwrCfgCmd),
-			  GET_PWR_CFG_BASE(PwrCfgCmd),
-			  GET_PWR_CFG_CMD(PwrCfgCmd),
-			  GET_PWR_CFG_MASK(PwrCfgCmd),
-			  GET_PWR_CFG_VALUE(PwrCfgCmd)));
+			 "HalPwrSeqCmdParsing23a: offset(%#x) cut_msk(%#x) fab_msk(%#x) interface_msk(%#x) base(%#x) cmd(%#x) msk(%#x) value(%#x)\n",
+			 GET_PWR_CFG_OFFSET(PwrCfgCmd),
+			 GET_PWR_CFG_CUT_MASK(PwrCfgCmd),
+			 GET_PWR_CFG_FAB_MASK(PwrCfgCmd),
+			 GET_PWR_CFG_INTF_MASK(PwrCfgCmd),
+			 GET_PWR_CFG_BASE(PwrCfgCmd),
+			 GET_PWR_CFG_CMD(PwrCfgCmd),
+			 GET_PWR_CFG_MASK(PwrCfgCmd),
+			 GET_PWR_CFG_VALUE(PwrCfgCmd));
 
 		/* 2 Only Handle the command whose FAB, CUT, and Interface are
 		   matched */
@@ -79,14 +77,12 @@ u8 HalPwrSeqCmdParsing23a(struct rtw_adapter *padapter, u8 CutVersion,
 			switch (GET_PWR_CFG_CMD(PwrCfgCmd)) {
 			case PWR_CMD_READ:
 				RT_TRACE(_module_hal_init_c_, _drv_info_,
-					 ("HalPwrSeqCmdParsing23a: "
-					  "PWR_CMD_READ\n"));
+					 "HalPwrSeqCmdParsing23a: PWR_CMD_READ\n");
 				break;
 
 			case PWR_CMD_WRITE:
 				RT_TRACE(_module_hal_init_c_, _drv_info_,
-					 ("HalPwrSeqCmdParsing23a: "
-					  "PWR_CMD_WRITE\n"));
+					 "HalPwrSeqCmdParsing23a: PWR_CMD_WRITE\n");
 				offset = GET_PWR_CFG_OFFSET(PwrCfgCmd);
 
 				/*  Read the value from system register */
@@ -102,8 +98,7 @@ u8 HalPwrSeqCmdParsing23a(struct rtw_adapter *padapter, u8 CutVersion,
 
 			case PWR_CMD_POLLING:
 				RT_TRACE(_module_hal_init_c_, _drv_info_,
-					 ("HalPwrSeqCmdParsing23a: "
-					  "PWR_CMD_POLLING\n"));
+					 "HalPwrSeqCmdParsing23a: PWR_CMD_POLLING\n");
 
 				bPollingBit = false;
 				offset = GET_PWR_CFG_OFFSET(PwrCfgCmd);
@@ -131,8 +126,7 @@ u8 HalPwrSeqCmdParsing23a(struct rtw_adapter *padapter, u8 CutVersion,
 
 			case PWR_CMD_DELAY:
 				RT_TRACE(_module_hal_init_c_, _drv_info_,
-					 ("HalPwrSeqCmdParsing23a: "
-					  "PWR_CMD_DELAY\n"));
+					 "HalPwrSeqCmdParsing23a: PWR_CMD_DELAY\n");
 				if (GET_PWR_CFG_VALUE(PwrCfgCmd) ==
 				    PWRSEQ_DELAY_US)
 					udelay(GET_PWR_CFG_OFFSET(PwrCfgCmd));
@@ -145,15 +139,12 @@ u8 HalPwrSeqCmdParsing23a(struct rtw_adapter *padapter, u8 CutVersion,
 				/*  When this command is parsed, end
 				    the process */
 				RT_TRACE(_module_hal_init_c_, _drv_info_,
-					 ("HalPwrSeqCmdParsing23a: "
-					  "PWR_CMD_END\n"));
+					 "HalPwrSeqCmdParsing23a: PWR_CMD_END\n");
 				return true;
-				break;
 
 			default:
 				RT_TRACE(_module_hal_init_c_, _drv_err_,
-					 ("HalPwrSeqCmdParsing23a: "
-					  "Unknown CMD!!\n"));
+					 "HalPwrSeqCmdParsing23a: Unknown CMD!!\n");
 				break;
 			}
 		}
diff --git a/drivers/staging/rtl8723au/hal/hal_com.c b/drivers/staging/rtl8723au/hal/hal_com.c
index bf4cae20bd12..530db57e8842 100644
--- a/drivers/staging/rtl8723au/hal/hal_com.c
+++ b/drivers/staging/rtl8723au/hal/hal_com.c
@@ -22,46 +22,6 @@
 
 #define _HAL_INIT_C_
 
-void dump_chip_info23a(struct hal_version ChipVersion)
-{
-	int cnt = 0;
-	u8 buf[128];
-
-	cnt += sprintf((buf + cnt), "Chip Version Info: CHIP_8723A_");
-
-	cnt += sprintf((buf + cnt), "%s_", IS_NORMAL_CHIP(ChipVersion) ?
-		       "Normal_Chip" : "Test_Chip");
-	cnt += sprintf((buf + cnt), "%s_",
-		       IS_CHIP_VENDOR_TSMC(ChipVersion) ? "TSMC" : "UMC");
-	if (IS_A_CUT(ChipVersion))
-		cnt += sprintf((buf + cnt), "A_CUT_");
-	else if (IS_B_CUT(ChipVersion))
-		cnt += sprintf((buf + cnt), "B_CUT_");
-	else if (IS_C_CUT(ChipVersion))
-		cnt += sprintf((buf + cnt), "C_CUT_");
-	else if (IS_D_CUT(ChipVersion))
-		cnt += sprintf((buf + cnt), "D_CUT_");
-	else if (IS_E_CUT(ChipVersion))
-		cnt += sprintf((buf + cnt), "E_CUT_");
-	else
-		cnt += sprintf((buf + cnt), "UNKNOWN_CUT(%d)_",
-			       ChipVersion.CUTVersion);
-
-	if (IS_1T1R(ChipVersion))
-		cnt += sprintf((buf + cnt), "1T1R_");
-	else if (IS_1T2R(ChipVersion))
-		cnt += sprintf((buf + cnt), "1T2R_");
-	else if (IS_2T2R(ChipVersion))
-		cnt += sprintf((buf + cnt), "2T2R_");
-	else
-		cnt += sprintf((buf + cnt), "UNKNOWN_RFTYPE(%d)_",
-			       ChipVersion.RFType);
-
-	cnt += sprintf((buf + cnt), "RomVer(%d)\n", ChipVersion.ROMVer);
-
-	DBG_8723A("%s", buf);
-}
-
 #define	EEPROM_CHANNEL_PLAN_BY_HW_MASK	0x80
 
 /* return the final channel plan decision */
@@ -231,13 +191,11 @@ void HalSetBrateCfg23a(struct rtw_adapter *padapter, u8 *mBratesOS)
 	rate_index = 0;
 	/*  Set RTS initial rate */
 	while (brate_cfg > 0x1) {
-		brate_cfg = (brate_cfg >> 1);
+		brate_cfg >>= 1;
 		rate_index++;
 	}
 		/*  Ziv - Check */
 	rtl8723au_write8(padapter, REG_INIRTS_RATE_SEL, rate_index);
-
-	return;
 }
 
 static void _OneOutPipeMapping(struct rtw_adapter *pAdapter)
@@ -437,9 +395,6 @@ rtl8723a_set_ampdu_min_space(struct rtw_adapter *padapter, u8 MinSpacingToSet)
 		if (MinSpacingToSet < SecMinSpace)
 			MinSpacingToSet = SecMinSpace;
 
-		/* RT_TRACE(COMP_MLME, DBG_LOUD,
-		   ("Set HW_VAR_AMPDU_MIN_SPACE: %#x\n",
-		   padapter->MgntInfo.MinSpaceCfg)); */
 		MinSpacingToSet |=
 			rtl8723au_read8(padapter, REG_AMPDU_MIN_SPACE) & 0xf8;
 		rtl8723au_write8(padapter, REG_AMPDU_MIN_SPACE,
@@ -479,9 +434,6 @@ void rtl8723a_set_ampdu_factor(struct rtw_adapter *padapter, u8 FactorToSet)
 			rtl8723au_write8(padapter, REG_AGGLEN_LMT + index,
 					 pRegToSet[index]);
 		}
-
-		/* RT_TRACE(COMP_MLME, DBG_LOUD,
-		   ("Set HW_VAR_AMPDU_FACTOR: %#x\n", FactorToSet)); */
 	}
 }
 
@@ -666,14 +618,8 @@ void rtl8723a_cam_empty_entry(struct rtw_adapter *padapter, u8 ucIndex)
 		/*  write content 0 is equall to mark invalid */
 		/* delay_ms(40); */
 		rtl8723au_write32(padapter, WCAMI, ulContent);
-		/* RT_TRACE(COMP_SEC, DBG_LOUD,
-		   ("rtl8723a_cam_empty_entry(): WRITE A4: %lx\n",
-		   ulContent));*/
 		/* delay_ms(40); */
 		rtl8723au_write32(padapter, REG_CAMCMD, ulCommand);
-		/* RT_TRACE(COMP_SEC, DBG_LOUD,
-		   ("rtl8723a_cam_empty_entry(): WRITE A0: %lx\n",
-		   ulCommand));*/
 	}
 }
 
diff --git a/drivers/staging/rtl8723au/hal/odm.c b/drivers/staging/rtl8723au/hal/odm.c
index 5269b46445f4..ec543cfe1b45 100644
--- a/drivers/staging/rtl8723au/hal/odm.c
+++ b/drivers/staging/rtl8723au/hal/odm.c
@@ -189,25 +189,16 @@ void odm_DynamicBBPowerSaving23a(struct dm_odm_t *pDM_Odm);
 
 /* END---------BB POWER SAVE----------------------- */
 
-void odm_RefreshRateAdaptiveMask23aCE23a(struct dm_odm_t *pDM_Odm);
-
 void odm_DynamicTxPower23aInit(struct dm_odm_t *pDM_Odm);
 
-void odm_RSSIMonitorCheck23aCE(struct dm_odm_t *pDM_Odm);
-void odm_RSSIMonitorCheck23a(struct dm_odm_t *pDM_Odm);
+static void odm_RSSIMonitorCheck(struct dm_odm_t *pDM_Odm);
 void odm_DynamicTxPower23a(struct dm_odm_t *pDM_Odm);
 
-void odm_RefreshRateAdaptiveMask23a(struct dm_odm_t *pDM_Odm);
-
-void ODM_TXPowerTrackingCheck23a(struct dm_odm_t *pDM_Odm);
+static void odm_RefreshRateAdaptiveMask(struct dm_odm_t *pDM_Odm);
 
 void odm_RateAdaptiveMaskInit23a(struct dm_odm_t *pDM_Odm);
 
-void odm_TXPowerTrackingThermalMeterInit23a(struct dm_odm_t *pDM_Odm);
-
-void odm_TXPowerTrackingInit23a(struct dm_odm_t *pDM_Odm);
-
-void odm_TXPowerTrackingCheckCE23a(struct dm_odm_t *pDM_Odm);
+static void odm_TXPowerTrackingInit(struct dm_odm_t *pDM_Odm);
 
 static void odm_EdcaTurboCheck23a(struct dm_odm_t *pDM_Odm);
 static void ODM_EdcaTurboInit23a(struct dm_odm_t *pDM_Odm);
@@ -216,16 +207,16 @@ static void ODM_EdcaTurboInit23a(struct dm_odm_t *pDM_Odm);
 #define	RxDefaultAnt2		0x569a
 
 bool odm_StaDefAntSel(struct dm_odm_t *pDM_Odm,
- u32 OFDM_Ant1_Cnt,
- u32 OFDM_Ant2_Cnt,
- u32 CCK_Ant1_Cnt,
- u32 CCK_Ant2_Cnt,
- u8 *pDefAnt
+	u32 OFDM_Ant1_Cnt,
+	u32 OFDM_Ant2_Cnt,
+	u32 CCK_Ant1_Cnt,
+	u32 CCK_Ant2_Cnt,
+	u8 *pDefAnt
 	);
 
 void odm_SetRxIdleAnt(struct dm_odm_t *pDM_Odm,
 	u8 Ant,
-   bool   bDualPath
+	bool   bDualPath
 );
 
 /* 3 Export Interface */
@@ -241,7 +232,7 @@ void ODM23a_DMInit(struct dm_odm_t *pDM_Odm)
 
 	odm23a_DynBBPSInit(pDM_Odm);
 	odm_DynamicTxPower23aInit(pDM_Odm);
-	odm_TXPowerTrackingInit23a(pDM_Odm);
+	odm_TXPowerTrackingInit(pDM_Odm);
 	ODM_EdcaTurboInit23a(pDM_Odm);
 }
 
@@ -258,7 +249,7 @@ void ODM_DMWatchdog23a(struct rtw_adapter *adapter)
 	odm_CmnInfoUpdate_Debug23a(pDM_Odm);
 	odm_CommonInfoSelfUpdate(pHalData);
 	odm_FalseAlarmCounterStatistics23a(pDM_Odm);
-	odm_RSSIMonitorCheck23a(pDM_Odm);
+	odm_RSSIMonitorCheck(pDM_Odm);
 
 	/* 8723A or 8189ES platform */
 	/* NeilChen--2012--08--24-- */
@@ -277,14 +268,11 @@ void ODM_DMWatchdog23a(struct rtw_adapter *adapter)
 	if (pwrctrlpriv->bpower_saving)
 		return;
 
-	odm_RefreshRateAdaptiveMask23a(pDM_Odm);
+	odm_RefreshRateAdaptiveMask(pDM_Odm);
 
 	odm_DynamicBBPowerSaving23a(pDM_Odm);
 
-	ODM_TXPowerTrackingCheck23a(pDM_Odm);
 	odm_EdcaTurboCheck23a(pDM_Odm);
-
-	odm_dtc(pDM_Odm);
 }
 
 /*  */
@@ -302,11 +290,6 @@ void ODM_CmnInfoInit23a(struct dm_odm_t *pDM_Odm,
 	/*  */
 	switch	(CmnInfo) {
 	/*  Fixed ODM value. */
-	case	ODM_CMNINFO_PLATFORM:
-		break;
-	case	ODM_CMNINFO_INTERFACE:
-		pDM_Odm->SupportInterface = (u8)Value;
-		break;
 	case	ODM_CMNINFO_MP_TEST_CHIP:
 		pDM_Odm->bIsMPChip = (u8)Value;
 		break;
@@ -319,9 +302,6 @@ void ODM_CmnInfoInit23a(struct dm_odm_t *pDM_Odm,
 	case	ODM_CMNINFO_FAB_VER:
 		pDM_Odm->FabVersion = (u8)Value;
 		break;
-	case	ODM_CMNINFO_RF_TYPE:
-		pDM_Odm->RFType = (u8)Value;
-		break;
 	case	ODM_CMNINFO_BOARD_TYPE:
 		pDM_Odm->BoardType = (u8)Value;
 		break;
@@ -334,9 +314,6 @@ void ODM_CmnInfoInit23a(struct dm_odm_t *pDM_Odm,
 	case	ODM_CMNINFO_EXT_TRSW:
 		pDM_Odm->ExtTRSW = (u8)Value;
 		break;
-	case	ODM_CMNINFO_PATCH_ID:
-		pDM_Odm->PatchID = (u8)Value;
-		break;
 	case	ODM_CMNINFO_BINHCT_TEST:
 		pDM_Odm->bInHctTest = (bool)Value;
 		break;
@@ -351,15 +328,6 @@ void ODM_CmnInfoInit23a(struct dm_odm_t *pDM_Odm,
 		/* do nothing */
 		break;
 	}
-
-	/*  */
-	/*  Tx power tracking BB swing table. */
-	/*  The base index = 12. +((12-n)/2)dB 13~?? = decrease tx pwr by -((n-12)/2)dB */
-	/*  */
-	pDM_Odm->BbSwingIdxOfdm			= 12; /*  Set defalut value as index 12. */
-	pDM_Odm->BbSwingIdxOfdmCurrent	= 12;
-	pDM_Odm->BbSwingFlagOfdm		= false;
-
 }
 
 void ODM_CmnInfoPtrArrayHook23a(struct dm_odm_t *pDM_Odm, enum odm_cmninfo CmnInfo,
@@ -383,9 +351,6 @@ void ODM_CmnInfoUpdate23a(struct dm_odm_t *pDM_Odm, u32 CmnInfo, u64 Value)
 {
 	/*  This init variable may be changed in run time. */
 	switch	(CmnInfo) {
-	case	ODM_CMNINFO_RF_TYPE:
-		pDM_Odm->RFType = (u8)Value;
-		break;
 	case	ODM_CMNINFO_WIFI_DIRECT:
 		pDM_Odm->bWIFI_Direct = (bool)Value;
 		break;
@@ -414,13 +379,18 @@ void ODM_CmnInfoUpdate23a(struct dm_odm_t *pDM_Odm, u32 CmnInfo, u64 Value)
 
 }
 
-void odm_CommonInfoSelfInit23a(struct dm_odm_t *pDM_Odm
-	)
+void odm_CommonInfoSelfInit23a(struct dm_odm_t *pDM_Odm)
 {
-	pDM_Odm->bCckHighPower =
-		(bool) ODM_GetBBReg(pDM_Odm, rFPGA0_XA_HSSIParameter2, BIT(9));
+	u32 val32;
+
+	val32 = rtl8723au_read32(pDM_Odm->Adapter, rFPGA0_XA_HSSIParameter2);
+	if (val32 & BIT(9))
+		pDM_Odm->bCckHighPower = true;
+	else
+		pDM_Odm->bCckHighPower = false;
+		
 	pDM_Odm->RFPathRxEnable =
-		(u8) ODM_GetBBReg(pDM_Odm, rOFDM0_TRxPathEnable, 0x0F);
+		rtl8723au_read32(pDM_Odm->Adapter, rOFDM0_TRxPathEnable) & 0x0F;
 
 	ODM_InitDebugSetting23a(pDM_Odm);
 }
@@ -432,15 +402,6 @@ static void odm_CommonInfoSelfUpdate(struct hal_data_8723a *pHalData)
 	u8 EntryCnt = 0;
 	u8 i;
 
-	if (pHalData->CurrentChannelBW == HT_CHANNEL_WIDTH_40) {
-		if (pHalData->nCur40MhzPrimeSC == 1)
-			pDM_Odm->ControlChannel = pHalData->CurrentChannel - 2;
-		else if (pHalData->nCur40MhzPrimeSC == 2)
-			pDM_Odm->ControlChannel = pHalData->CurrentChannel + 2;
-	} else {
-		pDM_Odm->ControlChannel = pHalData->CurrentChannel;
-	}
-
 	for (i = 0; i < ODM_ASSOCIATE_ENTRY_NUM; i++) {
 		pEntry = pDM_Odm->pODM_StaInfo[i];
 		if (pEntry)
@@ -456,16 +417,13 @@ void odm_CmnInfoInit_Debug23a(struct dm_odm_t *pDM_Odm)
 {
 	ODM_RT_TRACE(pDM_Odm, ODM_COMP_COMMON, ODM_DBG_LOUD, ("odm_CmnInfoInit_Debug23a ==>\n"));
 	ODM_RT_TRACE(pDM_Odm, ODM_COMP_COMMON, ODM_DBG_LOUD, ("SupportAbility = 0x%x\n", pDM_Odm->SupportAbility));
-	ODM_RT_TRACE(pDM_Odm, ODM_COMP_COMMON, ODM_DBG_LOUD, ("SupportInterface =%d\n", pDM_Odm->SupportInterface));
 	ODM_RT_TRACE(pDM_Odm, ODM_COMP_COMMON, ODM_DBG_LOUD, ("SupportICType = 0x%x\n", pDM_Odm->SupportICType));
 	ODM_RT_TRACE(pDM_Odm, ODM_COMP_COMMON, ODM_DBG_LOUD, ("CutVersion =%d\n", pDM_Odm->CutVersion));
 	ODM_RT_TRACE(pDM_Odm, ODM_COMP_COMMON, ODM_DBG_LOUD, ("FabVersion =%d\n", pDM_Odm->FabVersion));
-	ODM_RT_TRACE(pDM_Odm, ODM_COMP_COMMON, ODM_DBG_LOUD, ("RFType =%d\n", pDM_Odm->RFType));
 	ODM_RT_TRACE(pDM_Odm, ODM_COMP_COMMON, ODM_DBG_LOUD, ("BoardType =%d\n", pDM_Odm->BoardType));
 	ODM_RT_TRACE(pDM_Odm, ODM_COMP_COMMON, ODM_DBG_LOUD, ("ExtLNA =%d\n", pDM_Odm->ExtLNA));
 	ODM_RT_TRACE(pDM_Odm, ODM_COMP_COMMON, ODM_DBG_LOUD, ("ExtPA =%d\n", pDM_Odm->ExtPA));
 	ODM_RT_TRACE(pDM_Odm, ODM_COMP_COMMON, ODM_DBG_LOUD, ("ExtTRSW =%d\n", pDM_Odm->ExtTRSW));
-	ODM_RT_TRACE(pDM_Odm, ODM_COMP_COMMON, ODM_DBG_LOUD, ("PatchID =%d\n", pDM_Odm->PatchID));
 	ODM_RT_TRACE(pDM_Odm, ODM_COMP_COMMON, ODM_DBG_LOUD, ("bInHctTest =%d\n", pDM_Odm->bInHctTest));
 	ODM_RT_TRACE(pDM_Odm, ODM_COMP_COMMON, ODM_DBG_LOUD, ("bWIFITest =%d\n", pDM_Odm->bWIFITest));
 	ODM_RT_TRACE(pDM_Odm, ODM_COMP_COMMON, ODM_DBG_LOUD, ("bDualMacSmartConcurrent =%d\n", pDM_Odm->bDualMacSmartConcurrent));
@@ -481,18 +439,19 @@ void odm_CmnInfoUpdate_Debug23a(struct dm_odm_t *pDM_Odm)
 	ODM_RT_TRACE(pDM_Odm, ODM_COMP_COMMON, ODM_DBG_LOUD, ("RSSI_Min =%d\n", pDM_Odm->RSSI_Min));
 }
 
-void ODM_Write_DIG23a(struct dm_odm_t *pDM_Odm,
-	u8 CurrentIGI
-	)
+void ODM_Write_DIG23a(struct dm_odm_t *pDM_Odm,	u8 CurrentIGI)
 {
+	struct rtw_adapter *adapter = pDM_Odm->Adapter;
 	struct dig_t *pDM_DigTable = &pDM_Odm->DM_DigTable;
-
-	ODM_RT_TRACE(pDM_Odm, ODM_COMP_DIG, ODM_DBG_LOUD, ("ODM_REG(IGI_A, pDM_Odm) = 0x%x, ODM_BIT(IGI, pDM_Odm) = 0x%x \n",
-		ODM_REG(IGI_A, pDM_Odm), ODM_BIT(IGI, pDM_Odm)));
+	u32 val32;
 
 	if (pDM_DigTable->CurIGValue != CurrentIGI) {
-		ODM_SetBBReg(pDM_Odm, ODM_REG(IGI_A, pDM_Odm), ODM_BIT(IGI, pDM_Odm), CurrentIGI);
-		ODM_RT_TRACE(pDM_Odm, ODM_COMP_DIG, ODM_DBG_LOUD, ("CurrentIGI(0x%02x). \n", CurrentIGI));
+		val32 = rtl8723au_read32(adapter, ODM_REG_IGI_A_11N);
+		val32 &= ~ODM_BIT_IGI_11N;
+		val32 |= CurrentIGI;
+		rtl8723au_write32(adapter, ODM_REG_IGI_A_11N, val32);
+		ODM_RT_TRACE(pDM_Odm, ODM_COMP_DIG, ODM_DBG_LOUD,
+			     ("CurrentIGI(0x%02x). \n", CurrentIGI));
 		pDM_DigTable->CurIGValue = CurrentIGI;
 	}
 	ODM_RT_TRACE(pDM_Odm, ODM_COMP_DIG, ODM_DBG_LOUD,
@@ -515,11 +474,10 @@ void odm_DIG23abyRSSI_LPS(struct dm_odm_t *pDM_Odm)
 	CurrentIGI = CurrentIGI+RSSI_OFFSET_DIG;
 	bFwCurrentInPSMode = pAdapter->pwrctrlpriv.bFwCurrentInPSMode;
 
-	/* ODM_RT_TRACE(pDM_Odm, ODM_COMP_DIG_LPS, ODM_DBG_LOUD, ("odm_DIG23a() ==>\n")); */
-
 	/*  Using FW PS mode to make IGI */
 	if (bFwCurrentInPSMode) {
-		ODM_RT_TRACE(pDM_Odm, ODM_COMP_DIG, ODM_DBG_LOUD, ("---Neil---odm_DIG23a is in LPS mode\n"));
+		ODM_RT_TRACE(pDM_Odm, ODM_COMP_DIG, ODM_DBG_LOUD,
+			     ("---Neil---odm_DIG23a is in LPS mode\n"));
 		/* Adjust by  FA in LPS MODE */
 		if (pFalseAlmCnt->Cnt_all > DM_DIG_FA_TH2_LPS)
 			CurrentIGI = CurrentIGI+2;
@@ -545,15 +503,17 @@ void odm_DIG23abyRSSI_LPS(struct dm_odm_t *pDM_Odm)
 	 else if (CurrentIGI < RSSI_Lower)
 		CurrentIGI = RSSI_Lower;
 
-	ODM_Write_DIG23a(pDM_Odm, CurrentIGI);/* ODM_Write_DIG23a(pDM_Odm, pDM_DigTable->CurIGValue); */
-
+	ODM_Write_DIG23a(pDM_Odm, CurrentIGI);
 }
 
 void odm_DIG23aInit(struct dm_odm_t *pDM_Odm)
 {
 	struct dig_t *pDM_DigTable = &pDM_Odm->DM_DigTable;
+	u32 val32;
+
+	val32 = rtl8723au_read32(pDM_Odm->Adapter, ODM_REG_IGI_A_11N);
+	pDM_DigTable->CurIGValue = val32 & ODM_BIT_IGI_11N;
 
-	pDM_DigTable->CurIGValue = (u8) ODM_GetBBReg(pDM_Odm, ODM_REG(IGI_A, pDM_Odm), ODM_BIT(IGI, pDM_Odm));
 	pDM_DigTable->RssiLowThresh	= DM_DIG_THRESH_LOW;
 	pDM_DigTable->RssiHighThresh	= DM_DIG_THRESH_HIGH;
 	pDM_DigTable->FALowThresh	= DM_FALSEALARM_THRESH_LOW;
@@ -591,26 +551,22 @@ void odm_DIG23a(struct rtw_adapter *adapter)
 	u8 dm_dig_max, dm_dig_min;
 	u8 CurrentIGI = pDM_DigTable->CurIGValue;
 
-	ODM_RT_TRACE(pDM_Odm, ODM_COMP_DIG, ODM_DBG_LOUD, ("odm_DIG23a() ==>\n"));
-	/* if (!(pDM_Odm->SupportAbility & (ODM_BB_DIG|ODM_BB_FA_CNT))) */
-	if ((!(pDM_Odm->SupportAbility&ODM_BB_DIG)) || (!(pDM_Odm->SupportAbility&ODM_BB_FA_CNT))) {
-		ODM_RT_TRACE(pDM_Odm, ODM_COMP_DIG, ODM_DBG_LOUD,
-			     ("odm_DIG23a() Return: SupportAbility ODM_BB_DIG or ODM_BB_FA_CNT is disabled\n"));
-		return;
-	}
-
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_DIG, ODM_DBG_LOUD,
+		     ("odm_DIG23a() ==>\n"));
 	if (adapter->mlmepriv.bScanInProcess) {
-		ODM_RT_TRACE(pDM_Odm, ODM_COMP_DIG, ODM_DBG_LOUD, ("odm_DIG23a() Return: In Scan Progress \n"));
+		ODM_RT_TRACE(pDM_Odm, ODM_COMP_DIG, ODM_DBG_LOUD,
+			     ("odm_DIG23a() Return: In Scan Progress \n"));
 		return;
 	}
 
 	DIG_Dynamic_MIN = pDM_DigTable->DIG_Dynamic_MIN_0;
 	FirstConnect = (pDM_Odm->bLinked) && (!pDM_DigTable->bMediaConnect_0);
-	FirstDisConnect = (!pDM_Odm->bLinked) && (pDM_DigTable->bMediaConnect_0);
+	FirstDisConnect = (!pDM_Odm->bLinked) &&
+		(pDM_DigTable->bMediaConnect_0);
 
 	/* 1 Boundary Decision */
 	if ((pDM_Odm->SupportICType & ODM_RTL8723A) &&
-	    ((pDM_Odm->BoardType == ODM_BOARD_HIGHPWR) || pDM_Odm->ExtLNA)) {
+	    (pDM_Odm->BoardType == ODM_BOARD_HIGHPWR || pDM_Odm->ExtLNA)) {
 		dm_dig_max = DM_DIG_MAX_NIC_HP;
 		dm_dig_min = DM_DIG_MIN_NIC_HP;
 		DIG_MaxOfMin = DM_DIG_MAX_AP_HP;
@@ -764,31 +720,29 @@ void odm_DIG23a(struct rtw_adapter *adapter)
 
 void odm_FalseAlarmCounterStatistics23a(struct dm_odm_t *pDM_Odm)
 {
-	u32 ret_value;
+	struct rtw_adapter *adapter = pDM_Odm->Adapter;
 	struct false_alarm_stats *FalseAlmCnt = &pDM_Odm->FalseAlmCnt;
-
-	if (!(pDM_Odm->SupportAbility & ODM_BB_FA_CNT))
-		return;
+	u32 ret_value, val32;
 
 	/* hold ofdm counter */
-	 /* hold page C counter */
-	ODM_SetBBReg(pDM_Odm, ODM_REG_OFDM_FA_HOLDC_11N, BIT(31), 1);
+	/* hold page C counter */
+	val32 = rtl8723au_read32(adapter, ODM_REG_OFDM_FA_HOLDC_11N);
+	val32 |= BIT(31);
+	rtl8723au_write32(adapter, ODM_REG_OFDM_FA_HOLDC_11N, val32);
 	/* hold page D counter */
-	ODM_SetBBReg(pDM_Odm, ODM_REG_OFDM_FA_RSTD_11N, BIT(31), 1);
-	ret_value =
-		ODM_GetBBReg(pDM_Odm, ODM_REG_OFDM_FA_TYPE1_11N, bMaskDWord);
+	val32 = rtl8723au_read32(adapter, ODM_REG_OFDM_FA_RSTD_11N);
+	val32 |= BIT(31);
+	rtl8723au_write32(adapter, ODM_REG_OFDM_FA_RSTD_11N, val32);
+	ret_value = rtl8723au_read32(adapter, ODM_REG_OFDM_FA_TYPE1_11N);
 	FalseAlmCnt->Cnt_Fast_Fsync = (ret_value&0xffff);
-	FalseAlmCnt->Cnt_SB_Search_fail = ((ret_value&0xffff0000)>>16);
-	ret_value =
-		ODM_GetBBReg(pDM_Odm, ODM_REG_OFDM_FA_TYPE2_11N, bMaskDWord);
+	FalseAlmCnt->Cnt_SB_Search_fail = (ret_value & 0xffff0000)>>16;
+	ret_value = rtl8723au_read32(adapter, ODM_REG_OFDM_FA_TYPE2_11N);
 	FalseAlmCnt->Cnt_OFDM_CCA = (ret_value&0xffff);
-	FalseAlmCnt->Cnt_Parity_Fail = ((ret_value&0xffff0000)>>16);
-	ret_value =
-		ODM_GetBBReg(pDM_Odm, ODM_REG_OFDM_FA_TYPE3_11N, bMaskDWord);
+	FalseAlmCnt->Cnt_Parity_Fail = (ret_value & 0xffff0000)>>16;
+	ret_value = rtl8723au_read32(adapter, ODM_REG_OFDM_FA_TYPE3_11N);
 	FalseAlmCnt->Cnt_Rate_Illegal = (ret_value&0xffff);
-	FalseAlmCnt->Cnt_Crc8_fail = ((ret_value&0xffff0000)>>16);
-	ret_value =
-		ODM_GetBBReg(pDM_Odm, ODM_REG_OFDM_FA_TYPE4_11N, bMaskDWord);
+	FalseAlmCnt->Cnt_Crc8_fail = (ret_value & 0xffff0000)>>16;
+	ret_value = rtl8723au_read32(adapter, ODM_REG_OFDM_FA_TYPE4_11N);
 	FalseAlmCnt->Cnt_Mcs_fail = (ret_value&0xffff);
 
 	FalseAlmCnt->Cnt_Ofdm_fail = FalseAlmCnt->Cnt_Parity_Fail +
@@ -798,15 +752,16 @@ void odm_FalseAlarmCounterStatistics23a(struct dm_odm_t *pDM_Odm)
 		FalseAlmCnt->Cnt_Fast_Fsync +
 		FalseAlmCnt->Cnt_SB_Search_fail;
 	/* hold cck counter */
-	ODM_SetBBReg(pDM_Odm, ODM_REG_CCK_FA_RST_11N, BIT(12), 1);
-	ODM_SetBBReg(pDM_Odm, ODM_REG_CCK_FA_RST_11N, BIT(14), 1);
+	val32 = rtl8723au_read32(adapter, ODM_REG_CCK_FA_RST_11N);
+	val32 |= (BIT(12) | BIT(14));
+	rtl8723au_write32(adapter, ODM_REG_CCK_FA_RST_11N, val32);
 
-	ret_value = ODM_GetBBReg(pDM_Odm, ODM_REG_CCK_FA_LSB_11N, bMaskByte0);
+	ret_value = rtl8723au_read32(adapter, ODM_REG_CCK_FA_LSB_11N) & 0xff;
 	FalseAlmCnt->Cnt_Cck_fail = ret_value;
-	ret_value = ODM_GetBBReg(pDM_Odm, ODM_REG_CCK_FA_MSB_11N, bMaskByte3);
-	FalseAlmCnt->Cnt_Cck_fail +=  (ret_value & 0xff) << 8;
+	ret_value = rtl8723au_read32(adapter, ODM_REG_CCK_FA_MSB_11N) >> 16;
+	FalseAlmCnt->Cnt_Cck_fail += (ret_value & 0xff00);
 
-	ret_value = ODM_GetBBReg(pDM_Odm, ODM_REG_CCK_CCA_CNT_11N, bMaskDWord);
+	ret_value = rtl8723au_read32(adapter, ODM_REG_CCK_CCA_CNT_11N);
 	FalseAlmCnt->Cnt_CCK_CCA =
 		((ret_value&0xFF)<<8) | ((ret_value&0xFF00)>>8);
 
@@ -823,26 +778,39 @@ void odm_FalseAlarmCounterStatistics23a(struct dm_odm_t *pDM_Odm)
 
 	if (pDM_Odm->SupportICType >= ODM_RTL8723A) {
 		/* reset false alarm counter registers */
-		ODM_SetBBReg(pDM_Odm, ODM_REG_OFDM_FA_RSTC_11N, BIT(31), 1);
-		ODM_SetBBReg(pDM_Odm, ODM_REG_OFDM_FA_RSTC_11N, BIT(31), 0);
-		ODM_SetBBReg(pDM_Odm, ODM_REG_OFDM_FA_RSTD_11N, BIT(27), 1);
-		ODM_SetBBReg(pDM_Odm, ODM_REG_OFDM_FA_RSTD_11N, BIT(27), 0);
+		val32 = rtl8723au_read32(adapter, ODM_REG_OFDM_FA_RSTC_11N);
+		val32 |= BIT(31);
+		rtl8723au_write32(adapter, ODM_REG_OFDM_FA_RSTC_11N, val32);
+		val32 = rtl8723au_read32(adapter, ODM_REG_OFDM_FA_RSTC_11N);
+		val32 &= ~BIT(31);
+		rtl8723au_write32(adapter, ODM_REG_OFDM_FA_RSTC_11N, val32);
+
+		val32 = rtl8723au_read32(adapter, ODM_REG_OFDM_FA_RSTD_11N);
+		val32 |= BIT(27);
+		rtl8723au_write32(adapter, ODM_REG_OFDM_FA_RSTD_11N, val32);
+		val32 = rtl8723au_read32(adapter, ODM_REG_OFDM_FA_RSTD_11N);
+		val32 &= ~BIT(27);
+		rtl8723au_write32(adapter, ODM_REG_OFDM_FA_RSTD_11N, val32);
+
 		/* update ofdm counter */
 		 /* update page C counter */
-		ODM_SetBBReg(pDM_Odm, ODM_REG_OFDM_FA_HOLDC_11N, BIT(31), 0);
+		val32 = rtl8723au_read32(adapter, ODM_REG_OFDM_FA_HOLDC_11N);
+		val32 &= ~BIT(31);
+		rtl8723au_write32(adapter, ODM_REG_OFDM_FA_HOLDC_11N, val32);
+
 		 /* update page D counter */
-		ODM_SetBBReg(pDM_Odm, ODM_REG_OFDM_FA_RSTD_11N, BIT(31), 0);
+		val32 = rtl8723au_read32(adapter, ODM_REG_OFDM_FA_RSTD_11N);
+		val32 &= ~BIT(31);
+		rtl8723au_write32(adapter, ODM_REG_OFDM_FA_RSTD_11N, val32);
 
 		/* reset CCK CCA counter */
-		ODM_SetBBReg(pDM_Odm, ODM_REG_CCK_FA_RST_11N,
-			     BIT(13) | BIT(12), 0);
-		ODM_SetBBReg(pDM_Odm, ODM_REG_CCK_FA_RST_11N,
-			     BIT(13) | BIT(12), 2);
-		/* reset CCK FA counter */
-		ODM_SetBBReg(pDM_Odm, ODM_REG_CCK_FA_RST_11N,
-			     BIT(15) | BIT(14), 0);
-		ODM_SetBBReg(pDM_Odm, ODM_REG_CCK_FA_RST_11N,
-			     BIT(15) | BIT(14), 2);
+		val32 = rtl8723au_read32(adapter, ODM_REG_CCK_FA_RST_11N);
+		val32 &= ~(BIT(12) | BIT(13) | BIT(14) | BIT(15));
+		rtl8723au_write32(adapter, ODM_REG_CCK_FA_RST_11N, val32);
+
+		val32 = rtl8723au_read32(adapter, ODM_REG_CCK_FA_RST_11N);
+		val32 |= (BIT(13) | BIT(15));
+		rtl8723au_write32(adapter, ODM_REG_CCK_FA_RST_11N, val32);
 	}
 
 	ODM_RT_TRACE(pDM_Odm, ODM_COMP_FA_CNT, ODM_DBG_LOUD,
@@ -859,9 +827,12 @@ void odm_FalseAlarmCounterStatistics23a(struct dm_odm_t *pDM_Odm)
 		     ("Cnt_Crc8_fail =%d, Cnt_Mcs_fail =%d\n",
 		      FalseAlmCnt->Cnt_Crc8_fail, FalseAlmCnt->Cnt_Mcs_fail));
 
-	ODM_RT_TRACE(pDM_Odm, ODM_COMP_FA_CNT, ODM_DBG_LOUD, ("Cnt_Cck_fail =%d\n", FalseAlmCnt->Cnt_Cck_fail));
-	ODM_RT_TRACE(pDM_Odm, ODM_COMP_FA_CNT, ODM_DBG_LOUD, ("Cnt_Ofdm_fail =%d\n", FalseAlmCnt->Cnt_Ofdm_fail));
-	ODM_RT_TRACE(pDM_Odm, ODM_COMP_FA_CNT, ODM_DBG_LOUD, ("Total False Alarm =%d\n", FalseAlmCnt->Cnt_all));
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_FA_CNT, ODM_DBG_LOUD,
+		     ("Cnt_Cck_fail =%d\n", FalseAlmCnt->Cnt_Cck_fail));
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_FA_CNT, ODM_DBG_LOUD,
+		     ("Cnt_Ofdm_fail =%d\n", FalseAlmCnt->Cnt_Ofdm_fail));
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_FA_CNT, ODM_DBG_LOUD,
+		     ("Total False Alarm =%d\n", FalseAlmCnt->Cnt_all));
 }
 
 /* 3 ============================================================ */
@@ -873,16 +844,13 @@ void odm_CCKPacketDetectionThresh23a(struct dm_odm_t *pDM_Odm)
 	struct false_alarm_stats *FalseAlmCnt = &pDM_Odm->FalseAlmCnt;
 	u8 CurCCK_CCAThres;
 
-	if (!(pDM_Odm->SupportAbility & (ODM_BB_CCK_PD|ODM_BB_FA_CNT)))
-		return;
-
 	if (pDM_Odm->ExtLNA)
 		return;
 
 	if (pDM_Odm->bLinked) {
 		if (pDM_Odm->RSSI_Min > 25) {
 			CurCCK_CCAThres = 0xcd;
-		} else if ((pDM_Odm->RSSI_Min <= 25) && (pDM_Odm->RSSI_Min > 10)) {
+		} else if (pDM_Odm->RSSI_Min <= 25 && pDM_Odm->RSSI_Min > 10) {
 			CurCCK_CCAThres = 0x83;
 		} else {
 			if (FalseAlmCnt->Cnt_Cck_fail > 1000)
@@ -905,10 +873,10 @@ void ODM_Write_CCK_CCA_Thres23a(struct dm_odm_t *pDM_Odm, u8 CurCCK_CCAThres)
 	struct dig_t *pDM_DigTable = &pDM_Odm->DM_DigTable;
 
 	if (pDM_DigTable->CurCCK_CCAThres != CurCCK_CCAThres)
-		ODM_Write1Byte(pDM_Odm, ODM_REG(CCK_CCA, pDM_Odm), CurCCK_CCAThres);
+		rtl8723au_write8(pDM_Odm->Adapter, ODM_REG(CCK_CCA, pDM_Odm),
+				 CurCCK_CCAThres);
 	pDM_DigTable->PreCCK_CCAThres = pDM_DigTable->CurCCK_CCAThres;
 	pDM_DigTable->CurCCK_CCAThres = CurCCK_CCAThres;
-
 }
 
 /* 3 ============================================================ */
@@ -934,20 +902,19 @@ void odm_DynamicBBPowerSaving23a(struct dm_odm_t *pDM_Odm)
 void ODM_RF_Saving23a(struct dm_odm_t *pDM_Odm, u8 bForceInNormal)
 {
 	struct dynamic_pwr_sav *pDM_PSTable = &pDM_Odm->DM_PSTable;
+	struct rtw_adapter *adapter = pDM_Odm->Adapter;
+	u32 val32;
 	u8 Rssi_Up_bound = 30;
 	u8 Rssi_Low_bound = 25;
-	if (pDM_Odm->PatchID == 40) { /* RT_CID_819x_FUNAI_TV */
-		Rssi_Up_bound = 50;
-		Rssi_Low_bound = 45;
-	}
 	if (pDM_PSTable->initialize == 0) {
 
-		pDM_PSTable->Reg874 = (ODM_GetBBReg(pDM_Odm, 0x874, bMaskDWord)&0x1CC000)>>14;
+		pDM_PSTable->Reg874 =
+			rtl8723au_read32(adapter, 0x874) & 0x1CC000;
 		pDM_PSTable->RegC70 =
-			(ODM_GetBBReg(pDM_Odm, 0xc70, bMaskDWord) & BIT(3)) >>3;
-		pDM_PSTable->Reg85C = (ODM_GetBBReg(pDM_Odm, 0x85c, bMaskDWord)&0xFF000000)>>24;
-		pDM_PSTable->RegA74 = (ODM_GetBBReg(pDM_Odm, 0xa74, bMaskDWord)&0xF000)>>12;
-		/* Reg818 = PHY_QueryBBReg(pAdapter, 0x818, bMaskDWord); */
+			rtl8723au_read32(adapter, 0xc70) & BIT(3);
+		pDM_PSTable->Reg85C =
+			rtl8723au_read32(adapter, 0x85c) & 0xFF000000;
+		pDM_PSTable->RegA74 = rtl8723au_read32(adapter, 0xa74) & 0xF000;
 		pDM_PSTable->initialize = 1;
 	}
 
@@ -973,26 +940,74 @@ void ODM_RF_Saving23a(struct dm_odm_t *pDM_Odm, u8 bForceInNormal)
 
 	if (pDM_PSTable->PreRFState != pDM_PSTable->CurRFState) {
 		if (pDM_PSTable->CurRFState == RF_Save) {
-			/*  <tynli_note> 8723 RSSI report will be wrong. Set 0x874[5]= 1 when enter BB power saving mode. */
+			/*  <tynli_note> 8723 RSSI report will be wrong.
+			 * Set 0x874[5]= 1 when enter BB power saving mode. */
 			/*  Suggested by SD3 Yu-Nan. 2011.01.20. */
-			if (pDM_Odm->SupportICType == ODM_RTL8723A)
-				ODM_SetBBReg(pDM_Odm, 0x874, BIT(5), 0x1); /* Reg874[5]= 1b'1 */
-			ODM_SetBBReg(pDM_Odm, 0x874, 0x1C0000, 0x2); /* Reg874[20:18]= 3'b010 */
-			ODM_SetBBReg(pDM_Odm, 0xc70, BIT(3), 0); /* RegC70[3]= 1'b0 */
-			ODM_SetBBReg(pDM_Odm, 0x85c, 0xFF000000, 0x63); /* Reg85C[31:24]= 0x63 */
-			ODM_SetBBReg(pDM_Odm, 0x874, 0xC000, 0x2); /* Reg874[15:14]= 2'b10 */
-			ODM_SetBBReg(pDM_Odm, 0xa74, 0xF000, 0x3); /* RegA75[7:4]= 0x3 */
-			ODM_SetBBReg(pDM_Odm, 0x818, BIT(28), 0x0); /* Reg818[28]= 1'b0 */
-			ODM_SetBBReg(pDM_Odm, 0x818, BIT(28), 0x1); /* Reg818[28]= 1'b1 */
+			/* Reg874[5]= 1b'1 */
+			if (pDM_Odm->SupportICType == ODM_RTL8723A) {
+				val32 = rtl8723au_read32(adapter, 0x874);
+				val32 |= BIT(5);
+				rtl8723au_write32(adapter, 0x874, val32);
+			}
+			/* Reg874[20:18]= 3'b010 */
+			val32 = rtl8723au_read32(adapter, 0x874);
+			val32 &= ~(BIT(18) | BIT(20));
+			val32 |= BIT(19);
+			rtl8723au_write32(adapter, 0x874, val32);
+			/* RegC70[3]= 1'b0 */
+			val32 = rtl8723au_read32(adapter, 0xc70);
+			val32 &= ~BIT(3);
+			rtl8723au_write32(adapter, 0xc70, val32);
+			/* Reg85C[31:24]= 0x63 */
+			val32 = rtl8723au_read32(adapter, 0x85c);
+			val32 &= 0x00ffffff;
+			val32 |= 0x63000000;
+			rtl8723au_write32(adapter, 0x85c, val32);
+			/* Reg874[15:14]= 2'b10 */
+			val32 = rtl8723au_read32(adapter, 0x874);
+			val32 &= ~BIT(14);
+			val32 |= BIT(15);
+			rtl8723au_write32(adapter, 0x874, val32);
+			/* RegA75[7:4]= 0x3 */
+			val32 = rtl8723au_read32(adapter, 0xa74);
+			val32 &= ~(BIT(14) | BIT(15));
+			val32 |= (BIT(12) | BIT(13));
+			rtl8723au_write32(adapter, 0xa74, val32);
+			/* Reg818[28]= 1'b0 */
+			val32 = rtl8723au_read32(adapter, 0x818);
+			val32 &= ~BIT(28);
+			rtl8723au_write32(adapter, 0x818, val32);
+			/* Reg818[28]= 1'b1 */
+			val32 = rtl8723au_read32(adapter, 0x818);
+			val32 |= BIT(28);
+			rtl8723au_write32(adapter, 0x818, val32);
 		} else {
-			ODM_SetBBReg(pDM_Odm, 0x874, 0x1CC000, pDM_PSTable->Reg874);
-			ODM_SetBBReg(pDM_Odm, 0xc70, BIT(3), pDM_PSTable->RegC70);
-			ODM_SetBBReg(pDM_Odm, 0x85c, 0xFF000000, pDM_PSTable->Reg85C);
-			ODM_SetBBReg(pDM_Odm, 0xa74, 0xF000, pDM_PSTable->RegA74);
-			ODM_SetBBReg(pDM_Odm, 0x818, BIT(28), 0x0);
-
-			if (pDM_Odm->SupportICType == ODM_RTL8723A)
-				ODM_SetBBReg(pDM_Odm, 0x874, BIT(5), 0x0); /* Reg874[5]= 1b'0 */
+			val32 = rtl8723au_read32(adapter, 0x874);
+			val32 |= pDM_PSTable->Reg874;
+			rtl8723au_write32(adapter, 0x874, val32);
+		
+			val32 = rtl8723au_read32(adapter, 0xc70);
+			val32 |= pDM_PSTable->RegC70;
+			rtl8723au_write32(adapter, 0xc70, val32);
+
+			val32 = rtl8723au_read32(adapter, 0x85c);
+			val32 |= pDM_PSTable->Reg85C;
+			rtl8723au_write32(adapter, 0x85c, val32);
+
+			val32 = rtl8723au_read32(adapter, 0xa74);
+			val32 |= pDM_PSTable->RegA74;
+			rtl8723au_write32(adapter, 0xa74, val32);
+
+			val32 = rtl8723au_read32(adapter, 0x818);
+			val32 &= ~BIT(28);
+			rtl8723au_write32(adapter, 0x818, val32);
+
+			/* Reg874[5]= 1b'0 */
+			if (pDM_Odm->SupportICType == ODM_RTL8723A) {
+				val32 = rtl8723au_read32(adapter, 0x874);
+				val32 &= ~BIT(5);
+				rtl8723au_write32(adapter, 0x874, val32);
+			}
 		}
 		pDM_PSTable->PreRFState = pDM_PSTable->CurRFState;
 	}
@@ -1010,10 +1025,6 @@ void odm_RateAdaptiveMaskInit23a(struct dm_odm_t *pDM_Odm)
 	struct odm_rate_adapt *pOdmRA = &pDM_Odm->RateAdaptive;
 
 	pOdmRA->Type = DM_Type_ByDriver;
-	if (pOdmRA->Type == DM_Type_ByDriver)
-		pDM_Odm->bUseRAMask = true;
-	else
-		pDM_Odm->bUseRAMask = false;
 
 	pOdmRA->RATRState = DM_RATR_STA_INIT;
 	pOdmRA->HighRSSIThresh = 50;
@@ -1057,7 +1068,8 @@ u32 ODM_Get_Rate_Bitmap23a(struct hal_data_8723a *pHalData, u32 macid,
 		break;
 	case (ODM_WM_B|ODM_WM_G|ODM_WM_N24G):
 	case (ODM_WM_A|ODM_WM_B|ODM_WM_G|ODM_WM_N24G):
-		if (pDM_Odm->RFType == ODM_1T2R || pDM_Odm->RFType == ODM_1T1R) {
+		if (pHalData->rf_type == RF_1T2R ||
+		    pHalData->rf_type == RF_1T1R) {
 			if (rssi_level == DM_RATR_STA_HIGH) {
 				rate_bitmap = 0x000f0000;
 			} else if (rssi_level == DM_RATR_STA_MIDDLE) {
@@ -1086,22 +1098,22 @@ u32 ODM_Get_Rate_Bitmap23a(struct hal_data_8723a *pHalData, u32 macid,
 	default:
 		/* case WIRELESS_11_24N: */
 		/* case WIRELESS_11_5N: */
-		if (pDM_Odm->RFType == RF_1T2R)
+		if (pHalData->rf_type == RF_1T2R)
 			rate_bitmap = 0x000fffff;
 		else
 			rate_bitmap = 0x0fffffff;
 		break;
 	}
 
-	/* printk("%s ==> rssi_level:0x%02x, WirelessMode:0x%02x, rate_bitmap:0x%08x \n", __func__, rssi_level, WirelessMode, rate_bitmap); */
-	ODM_RT_TRACE(pDM_Odm, ODM_COMP_RA_MASK, ODM_DBG_LOUD, (" ==> rssi_level:0x%02x, WirelessMode:0x%02x, rate_bitmap:0x%08x \n", rssi_level, WirelessMode, rate_bitmap));
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_RA_MASK, ODM_DBG_LOUD,
+	(" ==> rssi_level:0x%02x, WirelessMode:0x%02x, rate_bitmap:0x%08x \n",
+	 rssi_level, WirelessMode, rate_bitmap));
 
 	return rate_bitmap;
-
 }
 
 /*-----------------------------------------------------------------------------
- * Function:	odm_RefreshRateAdaptiveMask23a()
+ * Function:	odm_RefreshRateAdaptiveMask()
  *
  * Overview:	Update rate table mask according to rssi
  *
@@ -1116,51 +1128,35 @@ u32 ODM_Get_Rate_Bitmap23a(struct hal_data_8723a *pHalData, u32 macid,
  *05/27/2009	hpfan	Create Version 0.
  *
  *---------------------------------------------------------------------------*/
-void odm_RefreshRateAdaptiveMask23a(struct dm_odm_t *pDM_Odm)
-{
-	if (!(pDM_Odm->SupportAbility & ODM_BB_RA_MASK))
-		return;
-	/*  */
-	/*  2011/09/29 MH In HW integration first stage, we provide 4 different handle to operate */
-	/*  at the same time. In the stage2/3, we need to prive universal interface and merge all */
-	/*  HW dynamic mechanism. */
-	/*  */
-	odm_RefreshRateAdaptiveMask23aCE23a(pDM_Odm);
-}
-
-void odm_RefreshRateAdaptiveMask23aCE23a(struct dm_odm_t *pDM_Odm)
+static void odm_RefreshRateAdaptiveMask(struct dm_odm_t *pDM_Odm)
 {
+	struct rtw_adapter *pAdapter = pDM_Odm->Adapter;
+	u32 smoothed;
 	u8 i;
-	struct rtw_adapter *pAdapter	 =  pDM_Odm->Adapter;
 
 	if (pAdapter->bDriverStopped) {
 		ODM_RT_TRACE(pDM_Odm, ODM_COMP_RA_MASK, ODM_DBG_TRACE,
-			     ("<---- odm_RefreshRateAdaptiveMask23a(): driver is going to unload\n"));
+			     ("<---- %s: driver is going to unload\n",
+			      __func__));
 		return;
 	}
 
-	if (!pDM_Odm->bUseRAMask) {
-		ODM_RT_TRACE(pDM_Odm, ODM_COMP_RA_MASK, ODM_DBG_LOUD,
-			     ("<---- odm_RefreshRateAdaptiveMask23a(): driver does not control rate adaptive mask\n"));
-		return;
-	}
-
-	/* printk("==> %s \n", __func__); */
-
 	for (i = 0; i < ODM_ASSOCIATE_ENTRY_NUM; i++) {
 		struct sta_info *pstat = pDM_Odm->pODM_StaInfo[i];
 		if (pstat) {
-			if (ODM_RAStateCheck23a(pDM_Odm, pstat->rssi_stat.UndecoratedSmoothedPWDB, false, &pstat->rssi_level)) {
-				ODM_RT_TRACE(pDM_Odm, ODM_COMP_RA_MASK, ODM_DBG_LOUD,
+			smoothed = pstat->rssi_stat.UndecoratedSmoothedPWDB;
+			if (ODM_RAStateCheck23a(pDM_Odm, smoothed, false,
+						&pstat->rssi_level)) {
+				ODM_RT_TRACE(pDM_Odm, ODM_COMP_RA_MASK,
+					     ODM_DBG_LOUD,
 					     ("RSSI:%d, RSSI_LEVEL:%d\n",
-					     pstat->rssi_stat.UndecoratedSmoothedPWDB,
-					     pstat->rssi_level));
-				rtw_hal_update_ra_mask23a(pstat, pstat->rssi_level);
+					      smoothed,
+					      pstat->rssi_level));
+				rtw_hal_update_ra_mask23a(pstat,
+							  pstat->rssi_level);
 			}
-
 		}
 	}
-
 }
 
 /*  Return Value: bool */
@@ -1189,7 +1185,8 @@ bool ODM_RAStateCheck23a(struct dm_odm_t *pDM_Odm, s32 RSSI, bool bForceUpdate,
 		LowRSSIThreshForRA += GoUpGap;
 		break;
 	default:
-		ODM_RT_ASSERT(pDM_Odm, false, ("wrong rssi level setting %d !", *pRATRState));
+		ODM_RT_ASSERT(pDM_Odm, false, ("wrong rssi level setting %d !",
+					       *pRATRState));
 		break;
 	}
 
@@ -1227,24 +1224,8 @@ void odm_DynamicTxPower23aInit(struct dm_odm_t *pDM_Odm)
 	pdmpriv->DynamicTxHighPowerLvl = TxHighPwrLevel_Normal;
 }
 
-void odm_RSSIMonitorCheck23a(struct dm_odm_t *pDM_Odm)
-{
-	/*  For AP/ADSL use struct rtl8723a_priv * */
-	/*  For CE/NIC use struct rtw_adapter * */
-
-	if (!(pDM_Odm->SupportAbility & ODM_BB_RSSI_MONITOR))
-		return;
-
-	/*  2011/09/29 MH In HW integration first stage, we provide 4 different handle to operate */
-	/*  at the same time. In the stage2/3, we need to prive universal interface and merge all */
-	/*  HW dynamic mechanism. */
-	odm_RSSIMonitorCheck23aCE(pDM_Odm);
-}	/*  odm_RSSIMonitorCheck23a */
-
 static void
-FindMinimumRSSI(
-	struct rtw_adapter *pAdapter
-	)
+FindMinimumRSSI(struct rtw_adapter *pAdapter)
 {
 	struct hal_data_8723a *pHalData = GET_HAL_DATA(pAdapter);
 	struct dm_priv *pdmpriv = &pHalData->dmpriv;
@@ -1252,21 +1233,22 @@ FindMinimumRSSI(
 
 	/* 1 1.Determine the minimum RSSI */
 
-	if ((!pDM_Odm->bLinked) &&
-	    (pdmpriv->EntryMinUndecoratedSmoothedPWDB == 0))
+	if (!pDM_Odm->bLinked && !pdmpriv->EntryMinUndecoratedSmoothedPWDB)
 		pdmpriv->MinUndecoratedPWDBForDM = 0;
 	else
-		pdmpriv->MinUndecoratedPWDBForDM = pdmpriv->EntryMinUndecoratedSmoothedPWDB;
+		pdmpriv->MinUndecoratedPWDBForDM =
+			pdmpriv->EntryMinUndecoratedSmoothedPWDB;
 }
 
-void odm_RSSIMonitorCheck23aCE(struct dm_odm_t *pDM_Odm)
+static void odm_RSSIMonitorCheck(struct dm_odm_t *pDM_Odm)
 {
 	struct rtw_adapter *Adapter = pDM_Odm->Adapter;
 	struct hal_data_8723a *pHalData = GET_HAL_DATA(Adapter);
 	struct dm_priv *pdmpriv = &pHalData->dmpriv;
-	int	i;
-	int	tmpEntryMaxPWDB = 0, tmpEntryMinPWDB = 0xff;
+	int i;
+	int MaxDB = 0, MinDB = 0xff;
 	u8 sta_cnt = 0;
+	u32 tmpdb;
 	u32 PWDB_rssi[NUM_STA] = {0};/* 0~15]:MACID, [16~31]:PWDB_rssi */
 	struct sta_info *psta;
 
@@ -1276,37 +1258,36 @@ void odm_RSSIMonitorCheck23aCE(struct dm_odm_t *pDM_Odm)
 	for (i = 0; i < ODM_ASSOCIATE_ENTRY_NUM; i++) {
 		psta = pDM_Odm->pODM_StaInfo[i];
 		if (psta) {
-			if (psta->rssi_stat.UndecoratedSmoothedPWDB < tmpEntryMinPWDB)
-				tmpEntryMinPWDB = psta->rssi_stat.UndecoratedSmoothedPWDB;
+			if (psta->rssi_stat.UndecoratedSmoothedPWDB < MinDB)
+				MinDB = psta->rssi_stat.UndecoratedSmoothedPWDB;
 
-			if (psta->rssi_stat.UndecoratedSmoothedPWDB > tmpEntryMaxPWDB)
-				tmpEntryMaxPWDB = psta->rssi_stat.UndecoratedSmoothedPWDB;
+			if (psta->rssi_stat.UndecoratedSmoothedPWDB > MaxDB)
+				MaxDB = psta->rssi_stat.UndecoratedSmoothedPWDB;
 
-			if (psta->rssi_stat.UndecoratedSmoothedPWDB != (-1))
-				PWDB_rssi[sta_cnt++] = (psta->mac_id | (psta->rssi_stat.UndecoratedSmoothedPWDB<<16));
+			if (psta->rssi_stat.UndecoratedSmoothedPWDB != -1) {
+				tmpdb = psta->rssi_stat.UndecoratedSmoothedPWDB;
+				PWDB_rssi[sta_cnt++] = psta->mac_id |
+					(tmpdb << 16);
+			}
 		}
 	}
 
 	for (i = 0; i < sta_cnt; i++) {
-		if (PWDB_rssi[i] != (0)) {
-			if (pHalData->fw_ractrl) /*  Report every sta's RSSI to FW */
-				rtl8723a_set_rssi_cmd(Adapter, (u8 *)&PWDB_rssi[i]);
-		}
+		if (PWDB_rssi[i] != (0))
+			rtl8723a_set_rssi_cmd(Adapter, (u8 *)&PWDB_rssi[i]);
 	}
 
-	if (tmpEntryMaxPWDB != 0)	/*  If associated entry is found */
-		pdmpriv->EntryMaxUndecoratedSmoothedPWDB = tmpEntryMaxPWDB;
-	else
-		pdmpriv->EntryMaxUndecoratedSmoothedPWDB = 0;
+	pdmpriv->EntryMaxUndecoratedSmoothedPWDB = MaxDB;
 
-	if (tmpEntryMinPWDB != 0xff) /*  If associated entry is found */
-		pdmpriv->EntryMinUndecoratedSmoothedPWDB = tmpEntryMinPWDB;
+	if (MinDB != 0xff) /*  If associated entry is found */
+		pdmpriv->EntryMinUndecoratedSmoothedPWDB = MinDB;
 	else
 		pdmpriv->EntryMinUndecoratedSmoothedPWDB = 0;
 
 	FindMinimumRSSI(Adapter);/* get pdmpriv->MinUndecoratedPWDBForDM */
 
-	ODM_CmnInfoUpdate23a(&pHalData->odmpriv, ODM_CMNINFO_RSSI_MIN, pdmpriv->MinUndecoratedPWDBForDM);
+	ODM_CmnInfoUpdate23a(&pHalData->odmpriv, ODM_CMNINFO_RSSI_MIN,
+			     pdmpriv->MinUndecoratedPWDBForDM);
 }
 
 /* endif */
@@ -1314,12 +1295,7 @@ void odm_RSSIMonitorCheck23aCE(struct dm_odm_t *pDM_Odm)
 /* 3 Tx Power Tracking */
 /* 3 ============================================================ */
 
-void odm_TXPowerTrackingInit23a(struct dm_odm_t *pDM_Odm)
-{
-	odm_TXPowerTrackingThermalMeterInit23a(pDM_Odm);
-}
-
-void odm_TXPowerTrackingThermalMeterInit23a(struct dm_odm_t *pDM_Odm)
+static void odm_TXPowerTrackingInit(struct dm_odm_t *pDM_Odm)
 {
 	struct rtw_adapter *Adapter = pDM_Odm->Adapter;
 	struct hal_data_8723a *pHalData = GET_HAL_DATA(Adapter);
@@ -1329,42 +1305,33 @@ void odm_TXPowerTrackingThermalMeterInit23a(struct dm_odm_t *pDM_Odm)
 	pdmpriv->TXPowercount = 0;
 	pdmpriv->bTXPowerTrackingInit = false;
 	pdmpriv->TxPowerTrackControl = true;
-	MSG_8723A("pdmpriv->TxPowerTrackControl = %d\n", pdmpriv->TxPowerTrackControl);
+	MSG_8723A("pdmpriv->TxPowerTrackControl = %d\n",
+		  pdmpriv->TxPowerTrackControl);
 
 	pDM_Odm->RFCalibrateInfo.TxPowerTrackControl = true;
 }
 
-void ODM_TXPowerTrackingCheck23a(struct dm_odm_t *pDM_Odm)
-{
-	/*  For AP/ADSL use struct rtl8723a_priv * */
-	/*  For CE/NIC use struct rtw_adapter * */
-
-	/*  2011/09/29 MH In HW integration first stage, we provide 4 different handle to operate */
-	/*  at the same time. In the stage2/3, we need to prive universal interface and merge all */
-	/*  HW dynamic mechanism. */
-	odm_TXPowerTrackingCheckCE23a(pDM_Odm);
-}
-
-void odm_TXPowerTrackingCheckCE23a(struct dm_odm_t *pDM_Odm)
-{
-}
-
 /* EDCA Turbo */
 static void ODM_EdcaTurboInit23a(struct dm_odm_t *pDM_Odm)
 {
-
 	struct rtw_adapter *Adapter = pDM_Odm->Adapter;
 
 	pDM_Odm->DM_EDCA_Table.bCurrentTurboEDCA = false;
-	pDM_Odm->DM_EDCA_Table.bIsCurRDLState = false;
 	Adapter->recvpriv.bIsAnyNonBEPkts = false;
 
-	ODM_RT_TRACE(pDM_Odm, ODM_COMP_EDCA_TURBO, ODM_DBG_LOUD, ("Orginial VO PARAM: 0x%x\n", ODM_Read4Byte(pDM_Odm, ODM_EDCA_VO_PARAM)));
-	ODM_RT_TRACE(pDM_Odm, ODM_COMP_EDCA_TURBO, ODM_DBG_LOUD, ("Orginial VI PARAM: 0x%x\n", ODM_Read4Byte(pDM_Odm, ODM_EDCA_VI_PARAM)));
-	ODM_RT_TRACE(pDM_Odm, ODM_COMP_EDCA_TURBO, ODM_DBG_LOUD, ("Orginial BE PARAM: 0x%x\n", ODM_Read4Byte(pDM_Odm, ODM_EDCA_BE_PARAM)));
-	ODM_RT_TRACE(pDM_Odm, ODM_COMP_EDCA_TURBO, ODM_DBG_LOUD, ("Orginial BK PARAM: 0x%x\n", ODM_Read4Byte(pDM_Odm, ODM_EDCA_BK_PARAM)));
-
-}	/*  ODM_InitEdcaTurbo */
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_EDCA_TURBO, ODM_DBG_LOUD,
+		     ("Orginial VO PARAM: 0x%x\n",
+		      rtl8723au_read32(Adapter, ODM_EDCA_VO_PARAM)));
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_EDCA_TURBO, ODM_DBG_LOUD,
+		     ("Orginial VI PARAM: 0x%x\n",
+		      rtl8723au_read32(Adapter, ODM_EDCA_VI_PARAM)));
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_EDCA_TURBO, ODM_DBG_LOUD,
+		     ("Orginial BE PARAM: 0x%x\n",
+		      rtl8723au_read32(Adapter, ODM_EDCA_BE_PARAM)));
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_EDCA_TURBO, ODM_DBG_LOUD,
+		     ("Orginial BK PARAM: 0x%x\n",
+		      rtl8723au_read32(Adapter, ODM_EDCA_BK_PARAM)));
+}
 
 static void odm_EdcaTurboCheck23a(struct dm_odm_t *pDM_Odm)
 {
@@ -1377,19 +1344,18 @@ static void odm_EdcaTurboCheck23a(struct dm_odm_t *pDM_Odm)
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 	u32 trafficIndex;
 	u32 edca_param;
-	u64 cur_tx_bytes = 0;
-	u64 cur_rx_bytes = 0;
-	u8 bbtchange = false;
+	u64 cur_tx_bytes;
+	u64 cur_rx_bytes;
 
 	/*  For AP/ADSL use struct rtl8723a_priv * */
 	/*  For CE/NIC use struct rtw_adapter * */
 
-	/*  2011/09/29 MH In HW integration first stage, we provide 4 different handle to operate */
-	/*  at the same time. In the stage2/3, we need to prive universal interface and merge all */
-	/*  HW dynamic mechanism. */
-
-	if (!(pDM_Odm->SupportAbility & ODM_MAC_EDCA_TURBO))
-		return;
+	/*
+	 * 2011/09/29 MH In HW integration first stage, we provide 4
+	 * different handle to operate at the same time. In the stage2/3,
+	 * we need to prive universal interface and merge all HW dynamic
+	 * mechanism.
+	 */
 
 	if ((pregpriv->wifi_spec == 1))/*  (pmlmeinfo->HT_enable == 0)) */
 		goto dm_CheckEdcaTurbo_EXIT;
@@ -1401,7 +1367,7 @@ static void odm_EdcaTurboCheck23a(struct dm_odm_t *pDM_Odm)
 		goto dm_CheckEdcaTurbo_EXIT;
 
 	/*  Check if the status needs to be changed. */
-	if ((bbtchange) || (!precvpriv->bIsAnyNonBEPkts)) {
+	if (!precvpriv->bIsAnyNonBEPkts) {
 		cur_tx_bytes = pxmitpriv->tx_bytes - pxmitpriv->last_tx_bytes;
 		cur_rx_bytes = precvpriv->rx_bytes - precvpriv->last_rx_bytes;
 
@@ -1454,29 +1420,37 @@ dm_CheckEdcaTurbo_EXIT:
 	precvpriv->last_rx_bytes = precvpriv->rx_bytes;
 }
 
-u32 GetPSDData(struct dm_odm_t *pDM_Odm, unsigned int point, u8 initial_gain_psd)
+u32 GetPSDData(struct dm_odm_t *pDM_Odm, unsigned int point,
+	       u8 initial_gain_psd)
 {
-	u32 psd_report;
+	struct rtw_adapter *adapter = pDM_Odm->Adapter;
+	u32 psd_report, val32;
 
 	/* Set DCO frequency index, offset = (40MHz/SamplePts)*point */
-	ODM_SetBBReg(pDM_Odm, 0x808, 0x3FF, point);
+	val32 = rtl8723au_read32(adapter, 0x808);
+	val32 &= ~0x3ff;
+	val32 |= (point & 0x3ff);
+	rtl8723au_write32(adapter, 0x808, val32);
 
 	/* Start PSD calculation, Reg808[22]= 0->1 */
-	ODM_SetBBReg(pDM_Odm, 0x808, BIT(22), 1);
+	val32 = rtl8723au_read32(adapter, 0x808);
+	val32 |= BIT(22);
+	rtl8723au_write32(adapter, 0x808, val32);
 	/* Need to wait for HW PSD report */
 	udelay(30);
-	ODM_SetBBReg(pDM_Odm, 0x808, BIT(22), 0);
+	val32 = rtl8723au_read32(adapter, 0x808);
+	val32 &= ~BIT(22);
+	rtl8723au_write32(adapter, 0x808, val32);
 	/* Read PSD report, Reg8B4[15:0] */
-	psd_report = ODM_GetBBReg(pDM_Odm, 0x8B4, bMaskDWord) & 0x0000FFFF;
+	psd_report = rtl8723au_read32(adapter, 0x8B4) & 0x0000FFFF;
 
-	psd_report = (u32)(ConvertTo_dB23a(psd_report))+(u32)(initial_gain_psd-0x1c);
+	psd_report = (u32)(ConvertTo_dB23a(psd_report)) +
+		(u32)(initial_gain_psd-0x1c);
 
 	return psd_report;
 }
 
-u32
-ConvertTo_dB23a(
-	u32 Value)
+u32 ConvertTo_dB23a(u32 Value)
 {
 	u8 i;
 	u8 j;
@@ -1504,7 +1478,8 @@ ConvertTo_dB23a(
 
 /*  */
 /*  Description: */
-/*Set Single/Dual Antenna default setting for products that do not do detection in advance. */
+/* Set Single/Dual Antenna default setting for products that do not
+ * do detection in advance. */
 /*  */
 /*  Added by Joseph, 2012.03.22 */
 /*  */
@@ -1518,18 +1493,13 @@ void ODM_SingleDualAntennaDefaultSetting(struct dm_odm_t *pDM_Odm)
 
 /* 2 8723A ANT DETECT */
 
-static void odm_PHY_SaveAFERegisters(
-	struct dm_odm_t *pDM_Odm,
-	u32 *AFEReg,
-	u32 *AFEBackup,
-	u32 RegisterNum
-	)
+static void odm_PHY_SaveAFERegisters(struct dm_odm_t *pDM_Odm, u32 *AFEReg,
+				     u32 *AFEBackup, u32 RegisterNum)
 {
 	u32 i;
 
-	/* RTPRINT(FINIT, INIT_IQK, ("Save ADDA parameters.\n")); */
 	for (i = 0 ; i < RegisterNum ; i++)
-		AFEBackup[i] = ODM_GetBBReg(pDM_Odm, AFEReg[i], bMaskDWord);
+		AFEBackup[i] = rtl8723au_read32(pDM_Odm->Adapter, AFEReg[i]);
 }
 
 static void odm_PHY_ReloadAFERegisters(struct dm_odm_t *pDM_Odm, u32 *AFEReg,
@@ -1538,7 +1508,7 @@ static void odm_PHY_ReloadAFERegisters(struct dm_odm_t *pDM_Odm, u32 *AFEReg,
 	u32 i;
 
 	for (i = 0 ; i < RegiesterNum; i++)
-		ODM_SetBBReg(pDM_Odm, AFEReg[i], bMaskDWord, AFEBackup[i]);
+		rtl8723au_write32(pDM_Odm->Adapter, AFEReg[i], AFEBackup[i]);
 }
 
 /* 2 8723A ANT DETECT */
@@ -1548,9 +1518,10 @@ static void odm_PHY_ReloadAFERegisters(struct dm_odm_t *pDM_Odm, u32 *AFEReg,
 bool ODM_SingleDualAntennaDetection(struct dm_odm_t *pDM_Odm, u8 mode)
 {
 	struct sw_ant_sw *pDM_SWAT_Table = &pDM_Odm->DM_SWAT_Table;
+	struct rtw_adapter *adapter = pDM_Odm->Adapter;
 	u32 CurrentChannel, RfLoopReg;
 	u8 n;
-	u32 Reg88c, Regc08, Reg874, Regc50;
+	u32 Reg88c, Regc08, Reg874, Regc50, val32;
 	u8 initial_gain = 0x5a;
 	u32 PSD_report_tmp;
 	u32 AntA_report = 0x0, AntB_report = 0x0, AntO_report = 0x0;
@@ -1573,72 +1544,80 @@ bool ODM_SingleDualAntennaDetection(struct dm_odm_t *pDM_Odm, u8 mode)
 		return bResult;
 	/* 1 Backup Current RF/BB Settings */
 
-	CurrentChannel = ODM_GetRFReg(pDM_Odm, RF_PATH_A, ODM_CHANNEL, bRFRegOffsetMask);
+	CurrentChannel = ODM_GetRFReg(pDM_Odm, RF_PATH_A, ODM_CHANNEL,
+				      bRFRegOffsetMask);
 	RfLoopReg = ODM_GetRFReg(pDM_Odm, RF_PATH_A, 0x00, bRFRegOffsetMask);
-	ODM_SetBBReg(pDM_Odm, rFPGA0_XA_RFInterfaceOE, ODM_DPDT, Antenna_A);  /*  change to Antenna A */
+	/*  change to Antenna A */
+	val32 = rtl8723au_read32(adapter, rFPGA0_XA_RFInterfaceOE);
+	val32 &= ~0x300;
+	val32 |= 0x100;		/* Enable antenna A */
+	rtl8723au_write32(adapter, rFPGA0_XA_RFInterfaceOE, val32);
+
 	/*  Step 1: USE IQK to transmitter single tone */
 
 	udelay(10);
 
 	/* Store A Path Register 88c, c08, 874, c50 */
-	Reg88c = ODM_GetBBReg(pDM_Odm, rFPGA0_AnalogParameter4, bMaskDWord);
-	Regc08 = ODM_GetBBReg(pDM_Odm, rOFDM0_TRMuxPar, bMaskDWord);
-	Reg874 = ODM_GetBBReg(pDM_Odm, rFPGA0_XCD_RFInterfaceSW, bMaskDWord);
-	Regc50 = ODM_GetBBReg(pDM_Odm, rOFDM0_XAAGCCore1, bMaskDWord);
+	Reg88c = rtl8723au_read32(adapter, rFPGA0_AnalogParameter4);
+	Regc08 = rtl8723au_read32(adapter, rOFDM0_TRMuxPar);
+	Reg874 = rtl8723au_read32(adapter, rFPGA0_XCD_RFInterfaceSW);
+	Regc50 = rtl8723au_read32(adapter, rOFDM0_XAAGCCore1);
 
 	/*  Store AFE Registers */
 	odm_PHY_SaveAFERegisters(pDM_Odm, AFE_REG_8723A, AFE_Backup, 16);
 
 	/* Set PSD 128 pts */
-	ODM_SetBBReg(pDM_Odm, rFPGA0_PSDFunction, BIT(14) | BIT(15), 0x0);
+	val32 = rtl8723au_read32(adapter, rFPGA0_PSDFunction);
+	val32 &= ~(BIT(14) | BIT(15));
+	rtl8723au_write32(adapter, rFPGA0_PSDFunction, val32);
 
 	/*  To SET CH1 to do */
-	ODM_SetRFReg(pDM_Odm, RF_PATH_A, ODM_CHANNEL, bRFRegOffsetMask, 0x01);     /* Channel 1 */
+	ODM_SetRFReg(pDM_Odm, RF_PATH_A, ODM_CHANNEL, bRFRegOffsetMask, 0x01);
 
 	/*  AFE all on step */
-	ODM_SetBBReg(pDM_Odm, rRx_Wait_CCA, bMaskDWord, 0x6FDB25A4);
-	ODM_SetBBReg(pDM_Odm, rTx_CCK_RFON, bMaskDWord, 0x6FDB25A4);
-	ODM_SetBBReg(pDM_Odm, rTx_CCK_BBON, bMaskDWord, 0x6FDB25A4);
-	ODM_SetBBReg(pDM_Odm, rTx_OFDM_RFON, bMaskDWord, 0x6FDB25A4);
-	ODM_SetBBReg(pDM_Odm, rTx_OFDM_BBON, bMaskDWord, 0x6FDB25A4);
-	ODM_SetBBReg(pDM_Odm, rTx_To_Rx, bMaskDWord, 0x6FDB25A4);
-	ODM_SetBBReg(pDM_Odm, rTx_To_Tx, bMaskDWord, 0x6FDB25A4);
-	ODM_SetBBReg(pDM_Odm, rRx_CCK, bMaskDWord, 0x6FDB25A4);
-	ODM_SetBBReg(pDM_Odm, rRx_OFDM, bMaskDWord, 0x6FDB25A4);
-	ODM_SetBBReg(pDM_Odm, rRx_Wait_RIFS, bMaskDWord, 0x6FDB25A4);
-	ODM_SetBBReg(pDM_Odm, rRx_TO_Rx, bMaskDWord, 0x6FDB25A4);
-	ODM_SetBBReg(pDM_Odm, rStandby, bMaskDWord, 0x6FDB25A4);
-	ODM_SetBBReg(pDM_Odm, rSleep, bMaskDWord, 0x6FDB25A4);
-	ODM_SetBBReg(pDM_Odm, rPMPD_ANAEN, bMaskDWord, 0x6FDB25A4);
-	ODM_SetBBReg(pDM_Odm, rFPGA0_XCD_SwitchControl, bMaskDWord, 0x6FDB25A4);
-	ODM_SetBBReg(pDM_Odm, rBlue_Tooth, bMaskDWord, 0x6FDB25A4);
+	rtl8723au_write32(adapter, rRx_Wait_CCA, 0x6FDB25A4);
+	rtl8723au_write32(adapter, rTx_CCK_RFON, 0x6FDB25A4);
+	rtl8723au_write32(adapter, rTx_CCK_BBON, 0x6FDB25A4);
+	rtl8723au_write32(adapter, rTx_OFDM_RFON, 0x6FDB25A4);
+	rtl8723au_write32(adapter, rTx_OFDM_BBON, 0x6FDB25A4);
+	rtl8723au_write32(adapter, rTx_To_Rx, 0x6FDB25A4);
+	rtl8723au_write32(adapter, rTx_To_Tx, 0x6FDB25A4);
+	rtl8723au_write32(adapter, rRx_CCK, 0x6FDB25A4);
+	rtl8723au_write32(adapter, rRx_OFDM, 0x6FDB25A4);
+	rtl8723au_write32(adapter, rRx_Wait_RIFS, 0x6FDB25A4);
+	rtl8723au_write32(adapter, rRx_TO_Rx, 0x6FDB25A4);
+	rtl8723au_write32(adapter, rStandby, 0x6FDB25A4);
+	rtl8723au_write32(adapter, rSleep, 0x6FDB25A4);
+	rtl8723au_write32(adapter, rPMPD_ANAEN, 0x6FDB25A4);
+	rtl8723au_write32(adapter, rFPGA0_XCD_SwitchControl, 0x6FDB25A4);
+	rtl8723au_write32(adapter, rBlue_Tooth, 0x6FDB25A4);
 
 	/*  3 wire Disable */
-	ODM_SetBBReg(pDM_Odm, rFPGA0_AnalogParameter4, bMaskDWord, 0xCCF000C0);
+	rtl8723au_write32(adapter, rFPGA0_AnalogParameter4, 0xCCF000C0);
 
 	/* BB IQK Setting */
-	ODM_SetBBReg(pDM_Odm, rOFDM0_TRMuxPar, bMaskDWord, 0x000800E4);
-	ODM_SetBBReg(pDM_Odm, rFPGA0_XCD_RFInterfaceSW, bMaskDWord, 0x22208000);
+	rtl8723au_write32(adapter, rOFDM0_TRMuxPar, 0x000800E4);
+	rtl8723au_write32(adapter, rFPGA0_XCD_RFInterfaceSW, 0x22208000);
 
 	/* IQK setting tone@ 4.34Mhz */
-	ODM_SetBBReg(pDM_Odm, rTx_IQK_Tone_A, bMaskDWord, 0x10008C1C);
-	ODM_SetBBReg(pDM_Odm, rTx_IQK, bMaskDWord, 0x01007c00);
+	rtl8723au_write32(adapter, rTx_IQK_Tone_A, 0x10008C1C);
+	rtl8723au_write32(adapter, rTx_IQK, 0x01007c00);
 
 	/* Page B init */
-	ODM_SetBBReg(pDM_Odm, rConfig_AntA, bMaskDWord, 0x00080000);
-	ODM_SetBBReg(pDM_Odm, rConfig_AntA, bMaskDWord, 0x0f600000);
-	ODM_SetBBReg(pDM_Odm, rRx_IQK, bMaskDWord, 0x01004800);
-	ODM_SetBBReg(pDM_Odm, rRx_IQK_Tone_A, bMaskDWord, 0x10008c1f);
-	ODM_SetBBReg(pDM_Odm, rTx_IQK_PI_A, bMaskDWord, 0x82150008);
-	ODM_SetBBReg(pDM_Odm, rRx_IQK_PI_A, bMaskDWord, 0x28150008);
-	ODM_SetBBReg(pDM_Odm, rIQK_AGC_Rsp, bMaskDWord, 0x001028d0);
+	rtl8723au_write32(adapter, rConfig_AntA, 0x00080000);
+	rtl8723au_write32(adapter, rConfig_AntA, 0x0f600000);
+	rtl8723au_write32(adapter, rRx_IQK, 0x01004800);
+	rtl8723au_write32(adapter, rRx_IQK_Tone_A, 0x10008c1f);
+	rtl8723au_write32(adapter, rTx_IQK_PI_A, 0x82150008);
+	rtl8723au_write32(adapter, rRx_IQK_PI_A, 0x28150008);
+	rtl8723au_write32(adapter, rIQK_AGC_Rsp, 0x001028d0);
 
 	/* RF loop Setting */
 	ODM_SetRFReg(pDM_Odm, RF_PATH_A, 0x0, 0xFFFFF, 0x50008);
 
 	/* IQK Single tone start */
-	ODM_SetBBReg(pDM_Odm, rFPGA0_IQK, bMaskDWord, 0x80800000);
-	ODM_SetBBReg(pDM_Odm, rIQK_AGC_Pts, bMaskDWord, 0xf8000000);
+	rtl8723au_write32(adapter, rFPGA0_IQK, 0x80800000);
+	rtl8723au_write32(adapter, rIQK_AGC_Pts, 0xf8000000);
 	udelay(1000);
 	PSD_report_tmp = 0x0;
 
@@ -1650,7 +1629,10 @@ bool ODM_SingleDualAntennaDetection(struct dm_odm_t *pDM_Odm, u8 mode)
 
 	PSD_report_tmp = 0x0;
 
-	ODM_SetBBReg(pDM_Odm, rFPGA0_XA_RFInterfaceOE, 0x300, Antenna_B);  /*  change to Antenna B */
+	val32 = rtl8723au_read32(adapter, rFPGA0_XA_RFInterfaceOE);
+	val32 &= ~0x300;
+	val32 |= 0x200;		/* Enable antenna B */
+	rtl8723au_write32(adapter, rFPGA0_XA_RFInterfaceOE, val32);
 	udelay(10);
 
 	for (n = 0; n < 2; n++) {
@@ -1660,7 +1642,10 @@ bool ODM_SingleDualAntennaDetection(struct dm_odm_t *pDM_Odm, u8 mode)
 	}
 
 	/*  change to open case */
-	ODM_SetBBReg(pDM_Odm, rFPGA0_XA_RFInterfaceOE, 0x300, 0);  /*  change to Ant A and B all open case */
+	/*  change to Ant A and B all open case */
+	val32 = rtl8723au_read32(adapter, rFPGA0_XA_RFInterfaceOE);
+	val32 &= ~0x300;
+	rtl8723au_write32(adapter, rFPGA0_XA_RFInterfaceOE, val32);
 	udelay(10);
 
 	for (n = 0; n < 2; n++) {
@@ -1670,25 +1655,36 @@ bool ODM_SingleDualAntennaDetection(struct dm_odm_t *pDM_Odm, u8 mode)
 	}
 
 	/* Close IQK Single Tone function */
-	ODM_SetBBReg(pDM_Odm, rFPGA0_IQK, bMaskDWord, 0x00000000);
+	rtl8723au_write32(adapter, rFPGA0_IQK, 0x00000000);
 	PSD_report_tmp = 0x0;
 
 	/* 1 Return to antanna A */
-	ODM_SetBBReg(pDM_Odm, rFPGA0_XA_RFInterfaceOE, 0x300, Antenna_A);
-	ODM_SetBBReg(pDM_Odm, rFPGA0_AnalogParameter4, bMaskDWord, Reg88c);
-	ODM_SetBBReg(pDM_Odm, rOFDM0_TRMuxPar, bMaskDWord, Regc08);
-	ODM_SetBBReg(pDM_Odm, rFPGA0_XCD_RFInterfaceSW, bMaskDWord, Reg874);
-	ODM_SetBBReg(pDM_Odm, rOFDM0_XAAGCCore1, 0x7F, 0x40);
-	ODM_SetBBReg(pDM_Odm, rOFDM0_XAAGCCore1, bMaskDWord, Regc50);
-	ODM_SetRFReg(pDM_Odm, RF_PATH_A, RF_CHNLBW, bRFRegOffsetMask, CurrentChannel);
+	val32 = rtl8723au_read32(adapter, rFPGA0_XA_RFInterfaceOE);
+	val32 &= ~0x300;
+	val32 |= 0x100;		/* Enable antenna A */
+	rtl8723au_write32(adapter, rFPGA0_XA_RFInterfaceOE, val32);
+	rtl8723au_write32(adapter, rFPGA0_AnalogParameter4, Reg88c);
+	rtl8723au_write32(adapter, rOFDM0_TRMuxPar, Regc08);
+	rtl8723au_write32(adapter, rFPGA0_XCD_RFInterfaceSW, Reg874);
+	val32 = rtl8723au_read32(adapter, rOFDM0_XAAGCCore1);
+	val32 &= ~0x7f;
+	val32 |= 0x40;
+	rtl8723au_write32(adapter, rOFDM0_XAAGCCore1, val32);
+
+	rtl8723au_write32(adapter, rOFDM0_XAAGCCore1, Regc50);
+	ODM_SetRFReg(pDM_Odm, RF_PATH_A, RF_CHNLBW, bRFRegOffsetMask,
+		     CurrentChannel);
 	ODM_SetRFReg(pDM_Odm, RF_PATH_A, 0x00, bRFRegOffsetMask, RfLoopReg);
 
 	/* Reload AFE Registers */
 	odm_PHY_ReloadAFERegisters(pDM_Odm, AFE_REG_8723A, AFE_Backup, 16);
 
-	ODM_RT_TRACE(pDM_Odm, ODM_COMP_ANT_DIV, ODM_DBG_LOUD, ("psd_report_A[%d]= %d \n", 2416, AntA_report));
-	ODM_RT_TRACE(pDM_Odm, ODM_COMP_ANT_DIV, ODM_DBG_LOUD, ("psd_report_B[%d]= %d \n", 2416, AntB_report));
-	ODM_RT_TRACE(pDM_Odm, ODM_COMP_ANT_DIV, ODM_DBG_LOUD, ("psd_report_O[%d]= %d \n", 2416, AntO_report));
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_ANT_DIV, ODM_DBG_LOUD,
+		     ("psd_report_A[%d]= %d \n", 2416, AntA_report));
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_ANT_DIV, ODM_DBG_LOUD,
+		     ("psd_report_B[%d]= %d \n", 2416, AntB_report));
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_ANT_DIV, ODM_DBG_LOUD,
+		     ("psd_report_O[%d]= %d \n", 2416, AntO_report));
 
 	/* 2 Test Ant B based on Ant A is ON */
 	if (mode == ANTTESTB) {
@@ -1710,30 +1706,33 @@ bool ODM_SingleDualAntennaDetection(struct dm_odm_t *pDM_Odm, u8 mode)
 		if ((AntO_report >= 100) & (AntO_report < 118)) {
 			if (AntA_report > (AntO_report+1)) {
 				pDM_SWAT_Table->ANTA_ON = false;
-				ODM_RT_TRACE(pDM_Odm, ODM_COMP_ANT_DIV, ODM_DBG_LOUD, ("Ant A is OFF"));
+				ODM_RT_TRACE(pDM_Odm, ODM_COMP_ANT_DIV,
+					     ODM_DBG_LOUD, ("Ant A is OFF"));
 			} else {
 				pDM_SWAT_Table->ANTA_ON = true;
-				ODM_RT_TRACE(pDM_Odm, ODM_COMP_ANT_DIV, ODM_DBG_LOUD, ("Ant A is ON"));
+				ODM_RT_TRACE(pDM_Odm, ODM_COMP_ANT_DIV,
+					     ODM_DBG_LOUD, ("Ant A is ON"));
 			}
 
 			if (AntB_report > (AntO_report+2)) {
 				pDM_SWAT_Table->ANTB_ON = false;
-				ODM_RT_TRACE(pDM_Odm, ODM_COMP_ANT_DIV, ODM_DBG_LOUD, ("Ant B is OFF"));
+				ODM_RT_TRACE(pDM_Odm, ODM_COMP_ANT_DIV,
+					     ODM_DBG_LOUD, ("Ant B is OFF"));
 			} else {
 				pDM_SWAT_Table->ANTB_ON = true;
-				ODM_RT_TRACE(pDM_Odm, ODM_COMP_ANT_DIV, ODM_DBG_LOUD, ("Ant B is ON"));
+				ODM_RT_TRACE(pDM_Odm, ODM_COMP_ANT_DIV,
+					     ODM_DBG_LOUD, ("Ant B is ON"));
 			}
 		}
 	} else {
-		ODM_RT_TRACE(pDM_Odm, ODM_COMP_ANT_DIV, ODM_DBG_LOUD, ("ODM_SingleDualAntennaDetection(): Need to check again\n"));
-		pDM_SWAT_Table->ANTA_ON = true; /*  Set Antenna A on as default */
-		pDM_SWAT_Table->ANTB_ON = false; /*  Set Antenna B off as default */
+		ODM_RT_TRACE(pDM_Odm, ODM_COMP_ANT_DIV, ODM_DBG_LOUD,
+		("ODM_SingleDualAntennaDetection(): Need to check again\n"));
+		/*  Set Antenna A on as default */
+		pDM_SWAT_Table->ANTA_ON = true;
+		/*  Set Antenna B off as default */
+		pDM_SWAT_Table->ANTB_ON = false;
 		bResult = false;
 	}
-	return bResult;
-}
 
-/* Justin: According to the current RRSI to adjust Response Frame TX power, 2012/11/05 */
-void odm_dtc(struct dm_odm_t *pDM_Odm)
-{
+	return bResult;
 }
diff --git a/drivers/staging/rtl8723au/hal/odm_HWConfig.c b/drivers/staging/rtl8723au/hal/odm_HWConfig.c
index 33aafa01f900..7b9799e3dbda 100644
--- a/drivers/staging/rtl8723au/hal/odm_HWConfig.c
+++ b/drivers/staging/rtl8723au/hal/odm_HWConfig.c
@@ -33,24 +33,23 @@ static s32 odm_SignalScaleMapping_92CSeries(struct dm_odm_t *pDM_Odm, s32 CurrSi
 {
 	s32 RetSig = 0;
 
-	if ((pDM_Odm->SupportInterface  == ODM_ITRF_USB) || (pDM_Odm->SupportInterface  == ODM_ITRF_SDIO)) {
-		if (CurrSig >= 51 && CurrSig <= 100)
-			RetSig = 100;
-		else if (CurrSig >= 41 && CurrSig <= 50)
-			RetSig = 80 + ((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 >= 10 && CurrSig <= 20)
-			RetSig = 42 + (((CurrSig - 10) * 2) / 3);
-		else if (CurrSig >= 5 && CurrSig <= 9)
-			RetSig = 22 + (((CurrSig - 5) * 3) / 2);
-		else if (CurrSig >= 1 && CurrSig <= 4)
-			RetSig = 6 + (((CurrSig - 1) * 3) / 2);
-		else
-			RetSig = CurrSig;
-	}
+	if (CurrSig >= 51 && CurrSig <= 100)
+		RetSig = 100;
+	else if (CurrSig >= 41 && CurrSig <= 50)
+		RetSig = 80 + ((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 >= 10 && CurrSig <= 20)
+		RetSig = 42 + (((CurrSig - 10) * 2) / 3);
+	else if (CurrSig >= 5 && CurrSig <= 9)
+		RetSig = 22 + (((CurrSig - 5) * 3) / 2);
+	else if (CurrSig >= 1 && CurrSig <= 4)
+		RetSig = 6 + (((CurrSig - 1) * 3) / 2);
+	else
+		RetSig = CurrSig;
+
 	return RetSig;
 }
 
diff --git a/drivers/staging/rtl8723au/hal/odm_RegConfig8723A.c b/drivers/staging/rtl8723au/hal/odm_RegConfig8723A.c
index 88e0126e855a..342dec3e939f 100644
--- a/drivers/staging/rtl8723au/hal/odm_RegConfig8723A.c
+++ b/drivers/staging/rtl8723au/hal/odm_RegConfig8723A.c
@@ -14,6 +14,7 @@
  ******************************************************************************/
 
 #include "odm_precomp.h"
+#include "usb_ops_linux.h"
 
 void
 odm_ConfigRFReg_8723A(
@@ -43,62 +44,45 @@ odm_ConfigRFReg_8723A(
 	}
 }
 
-void odm_ConfigMAC_8723A(struct dm_odm_t *pDM_Odm,
-	u32		Addr,
-	u8		Data
-	)
+void odm_ConfigMAC_8723A(struct dm_odm_t *pDM_Odm, u32 addr, u8	data)
 {
-	ODM_Write1Byte(pDM_Odm, Addr, Data);
+	rtl8723au_write8(pDM_Odm->Adapter, addr, data);
 	ODM_RT_TRACE(pDM_Odm, ODM_COMP_INIT, ODM_DBG_LOUD,
-		     ("===> ODM_ConfigMACWithHeaderFile23a: [MAC_REG] %08X %08X\n",
-		     Addr, Data));
+		     ("===> %s: [MAC_REG] %08X %08X\n", __func__, addr, data));
 }
 
-void
-odm_ConfigBB_AGC_8723A(
-	struct dm_odm_t *pDM_Odm,
-	u32		Addr,
-	u32		Bitmask,
-	u32		Data
-  )
+void odm_ConfigBB_AGC_8723A(struct dm_odm_t *pDM_Odm, u32 addr, u32 data)
 {
-	ODM_SetBBReg(pDM_Odm, Addr, Bitmask, Data);
+	rtl8723au_write32(pDM_Odm->Adapter, addr, data);
 	/*  Add 1us delay between BB/RF register setting. */
 	udelay(1);
 
 	ODM_RT_TRACE(pDM_Odm, ODM_COMP_INIT, ODM_DBG_LOUD,
-		     ("===> ODM_ConfigBBWithHeaderFile23a: [AGC_TAB] %08X %08X\n",
-		     Addr, Data));
+		     ("===> %s: [AGC_TAB] %08X %08X\n", __func__, addr, data));
 }
 
 void
-odm_ConfigBB_PHY_8723A(
-	struct dm_odm_t *pDM_Odm,
-	u32		Addr,
-	u32		Bitmask,
-	u32		Data
-  )
+odm_ConfigBB_PHY_8723A(struct dm_odm_t *pDM_Odm, u32 addr, u32 data)
 {
-	if (Addr == 0xfe)
+	if (addr == 0xfe)
 		msleep(50);
-	else if (Addr == 0xfd)
+	else if (addr == 0xfd)
 		mdelay(5);
-	else if (Addr == 0xfc)
+	else if (addr == 0xfc)
 		mdelay(1);
-	else if (Addr == 0xfb)
+	else if (addr == 0xfb)
 		udelay(50);
-	else if (Addr == 0xfa)
+	else if (addr == 0xfa)
 		udelay(5);
-	else if (Addr == 0xf9)
+	else if (addr == 0xf9)
 		udelay(1);
-	else if (Addr == 0xa24)
-		pDM_Odm->RFCalibrateInfo.RegA24 = Data;
-	ODM_SetBBReg(pDM_Odm, Addr, Bitmask, Data);
+	else if (addr == 0xa24)
+		pDM_Odm->RFCalibrateInfo.RegA24 = data;
+	rtl8723au_write32(pDM_Odm->Adapter, addr, data);
 
 	/*  Add 1us delay between BB/RF register setting. */
 	udelay(1);
 
 	ODM_RT_TRACE(pDM_Odm, ODM_COMP_INIT, ODM_DBG_LOUD,
-		     ("===> ODM_ConfigBBWithHeaderFile23a: [PHY_REG] %08X %08X\n",
-		     Addr, Data));
+		     ("===> %s: [PHY_REG] %08X %08X\n", __func__, addr, data));
 }
diff --git a/drivers/staging/rtl8723au/hal/odm_debug.c b/drivers/staging/rtl8723au/hal/odm_debug.c
index c912ab89bc3e..cb2bdda6b0eb 100644
--- a/drivers/staging/rtl8723au/hal/odm_debug.c
+++ b/drivers/staging/rtl8723au/hal/odm_debug.c
@@ -22,3 +22,18 @@ void ODM_InitDebugSetting23a(struct dm_odm_t *pDM_Odm)
 }
 
 u32 GlobalDebugLevel23A;
+
+void rt_trace(int comp, int level, const char *fmt, ...)
+{
+	struct va_format vaf;
+	va_list args;
+
+	va_start(args, fmt);
+
+	vaf.fmt = fmt;
+	vaf.va = &args;
+
+	pr_info(DRIVER_PREFIX " [0x%08x,%d] %pV", comp, level, &vaf);
+
+	va_end(args);
+}
diff --git a/drivers/staging/rtl8723au/hal/odm_interface.c b/drivers/staging/rtl8723au/hal/odm_interface.c
index f03f6d4a3888..d8f67902708e 100644
--- a/drivers/staging/rtl8723au/hal/odm_interface.c
+++ b/drivers/staging/rtl8723au/hal/odm_interface.c
@@ -23,96 +23,6 @@
 /*  */
 #include <usb_ops_linux.h>
 
-u8 ODM_Read1Byte(struct dm_odm_t *pDM_Odm,
-	u32			RegAddr
-	)
-{
-	struct rtw_adapter *Adapter = pDM_Odm->Adapter;
-
-	return rtl8723au_read8(Adapter, RegAddr);
-}
-
-u16 ODM_Read2Byte(struct dm_odm_t *pDM_Odm, u32 RegAddr)
-{
-	struct rtw_adapter *Adapter = pDM_Odm->Adapter;
-
-	return rtl8723au_read16(Adapter, RegAddr);
-}
-
-u32 ODM_Read4Byte(struct dm_odm_t *pDM_Odm, u32 RegAddr)
-{
-	struct rtw_adapter *Adapter = pDM_Odm->Adapter;
-
-	return rtl8723au_read32(Adapter, RegAddr);
-}
-
-void ODM_Write1Byte(struct dm_odm_t *pDM_Odm, u32 RegAddr, u8 Data)
-{
-	struct rtw_adapter *Adapter = pDM_Odm->Adapter;
-
-	rtl8723au_write8(Adapter, RegAddr, Data);
-}
-
-void ODM_Write2Byte(struct dm_odm_t *pDM_Odm, u32 RegAddr, u16 Data)
-{
-	struct rtw_adapter *Adapter = pDM_Odm->Adapter;
-
-	rtl8723au_write16(Adapter, RegAddr, Data);
-}
-
-void ODM_Write4Byte(struct dm_odm_t *pDM_Odm, u32 RegAddr, u32 Data)
-{
-	struct rtw_adapter *Adapter = pDM_Odm->Adapter;
-
-	rtl8723au_write32(Adapter, RegAddr, Data);
-}
-
-void ODM_SetMACReg(
-	struct dm_odm_t *pDM_Odm,
-	u32		RegAddr,
-	u32		BitMask,
-	u32		Data
-	)
-{
-	struct rtw_adapter *Adapter = pDM_Odm->Adapter;
-
-	PHY_SetBBReg(Adapter, RegAddr, BitMask, Data);
-}
-
-u32 ODM_GetMACReg(
-	struct dm_odm_t *pDM_Odm,
-	u32		RegAddr,
-	u32		BitMask
-	)
-{
-	struct rtw_adapter *Adapter = pDM_Odm->Adapter;
-
-	return PHY_QueryBBReg(Adapter, RegAddr, BitMask);
-}
-
-void ODM_SetBBReg(
-	struct dm_odm_t *pDM_Odm,
-	u32		RegAddr,
-	u32		BitMask,
-	u32		Data
-	)
-{
-	struct rtw_adapter *Adapter = pDM_Odm->Adapter;
-
-	PHY_SetBBReg(Adapter, RegAddr, BitMask, Data);
-}
-
-u32 ODM_GetBBReg(
-	struct dm_odm_t *pDM_Odm,
-	u32		RegAddr,
-	u32		BitMask
-	)
-{
-	struct rtw_adapter *Adapter = pDM_Odm->Adapter;
-
-	return PHY_QueryBBReg(Adapter, RegAddr, BitMask);
-}
-
 void ODM_SetRFReg(
 	struct dm_odm_t *pDM_Odm,
 	enum RF_RADIO_PATH	eRFPath,
diff --git a/drivers/staging/rtl8723au/hal/rtl8723a_bt-coexist.c b/drivers/staging/rtl8723au/hal/rtl8723a_bt-coexist.c
index 73cfddd6df9a..cf15f80836ba 100644
--- a/drivers/staging/rtl8723au/hal/rtl8723a_bt-coexist.c
+++ b/drivers/staging/rtl8723au/hal/rtl8723a_bt-coexist.c
@@ -1554,7 +1554,8 @@ static void bthci_ResetBtSec(struct rtw_adapter *padapter, struct bt_security *p
 		pBtSec->bUsedHwEncrypt = true;
 	else
 		pBtSec->bUsedHwEncrypt = false;
-	RT_TRACE(_module_rtl871x_security_c_, _drv_info_, ("%s: bUsedHwEncrypt =%d\n", __func__, pBtSec->bUsedHwEncrypt));
+	RT_TRACE(_module_rtl871x_security_c_, _drv_info_,
+		 "%s: bUsedHwEncrypt =%d\n", __func__, pBtSec->bUsedHwEncrypt);
 
 	pBtSec->RSNIE.Octet = pBtSec->RSNIEBuf;
 }
@@ -3208,7 +3209,7 @@ bthci_CmdDisconnectPhysicalLink(struct rtw_adapter *padapter,
 	pBtDbg->dbgHciInfo.hciCmdCntDisconnectPhyLink++;
 
 	PLH = *((u8 *)pHciCmd->Data);
-	PhysLinkDisconnectReason = (*((u8 *)pHciCmd->Data+1));
+	PhysLinkDisconnectReason = *((u8 *)pHciCmd->Data+1);
 	RTPRINT(FIOCTL, IOCTL_BT_HCICMD, ("HCI_DISCONNECT_PHYSICAL_LINK  PhyHandle = 0x%x, Reason = 0x%x\n",
 		PLH, PhysLinkDisconnectReason));
 
@@ -4518,8 +4519,8 @@ bthci_StateConnecting(struct rtw_adapter *padapter,
 		RTPRINT(FIOCTL, IOCTL_STATE, ("STATE_CMD_MAC_CONNECT_COMPLETE\n"));
 
 		if (pBTInfo->BtAsocEntry[EntryNum].AMPRole == AMP_BTAP_JOINER) {
-			RT_TRACE(_module_rtl871x_security_c_,
-				 _drv_info_, ("StateConnecting \n"));
+			RT_TRACE(_module_rtl871x_security_c_, _drv_info_,
+				 "StateConnecting\n");
 		}
 		break;
 	case STATE_CMD_DISCONNECT_PHY_LINK:
@@ -5796,7 +5797,7 @@ static void
 btdm_1AntUpdateHalRAMask(struct rtw_adapter *padapter, u32 mac_id, u32 filter)
 {
 	u8 init_rate = 0;
-	u8 raid;
+	u8 raid, arg;
 	u32 mask;
 	u8 shortGIrate = false;
 	int supportRateNum = 0;
@@ -5860,26 +5861,16 @@ btdm_1AntUpdateHalRAMask(struct rtw_adapter *padapter, u32 mac_id, u32 filter)
 	mask &= ~filter;
 	init_rate = get_highest_rate_idx23a(mask)&0x3f;
 
-	if (pHalData->fw_ractrl) {
-		u8 arg = 0;
+	arg = mac_id&0x1f;/* MACID */
+	arg |= BIT(7);
+	if (true == shortGIrate)
+		arg |= BIT(5);
 
-		arg = mac_id&0x1f;/* MACID */
-		arg |= BIT(7);
-		if (true == shortGIrate)
-			arg |= BIT(5);
-
-		RTPRINT(FBT, BT_TRACE,
-			("[BTCoex], Update FW RAID entry, MASK = 0x%08x, "
-			 "arg = 0x%02x\n", mask, arg));
-
-		rtl8723a_set_raid_cmd(padapter, mask, arg);
-	} else {
-		if (shortGIrate)
-			init_rate |= BIT(6);
+	RTPRINT(FBT, BT_TRACE,
+		("[BTCoex], Update FW RAID entry, MASK = 0x%08x, "
+		 "arg = 0x%02x\n", mask, arg));
 
-		rtl8723au_write8(padapter, REG_INIDATA_RATE_SEL + mac_id,
-				 init_rate);
-	}
+	rtl8723a_set_raid_cmd(padapter, mask, arg);
 
 	psta->init_rate = init_rate;
 	pdmpriv->INIDATA_RATE[mac_id] = init_rate;
@@ -11206,15 +11197,17 @@ void rtl8723a_BT_init_hal_vars(struct rtw_adapter *padapter)
 	pHalData->bt_coexist.bt_radiosharedtype = pHalData->EEPROMBluetoothRadioShared;
 
 	RT_TRACE(_module_hal_init_c_, _drv_info_,
-		 ("BT Coexistance = 0x%x\n", rtl8723a_BT_coexist(padapter)));
+		 "BT Coexistance = 0x%x\n", rtl8723a_BT_coexist(padapter));
 
 	if (rtl8723a_BT_coexist(padapter)) {
 		if (pHalData->bt_coexist.BT_Ant_Num == Ant_x2) {
 			BTDM_SetBtCoexCurrAntNum(padapter, 2);
-			RT_TRACE(_module_hal_init_c_, _drv_info_, ("BlueTooth BT_Ant_Num = Antx2\n"));
+			RT_TRACE(_module_hal_init_c_, _drv_info_,
+				 "BlueTooth BT_Ant_Num = Antx2\n");
 		} else if (pHalData->bt_coexist.BT_Ant_Num == Ant_x1) {
 			BTDM_SetBtCoexCurrAntNum(padapter, 1);
-			RT_TRACE(_module_hal_init_c_, _drv_info_, ("BlueTooth BT_Ant_Num = Antx1\n"));
+			RT_TRACE(_module_hal_init_c_, _drv_info_,
+				 "BlueTooth BT_Ant_Num = Antx1\n");
 		}
 		pHalData->bt_coexist.bBTBusyTraffic = false;
 		pHalData->bt_coexist.bBTTrafficModeSet = false;
@@ -11223,8 +11216,8 @@ void rtl8723a_BT_init_hal_vars(struct rtw_adapter *padapter)
 		pHalData->bt_coexist.PreviousState = 0;
 
 		RT_TRACE(_module_hal_init_c_, _drv_info_,
-			 ("bt_radiosharedType = 0x%x\n",
-			 pHalData->bt_coexist.bt_radiosharedtype));
+			 "bt_radiosharedType = 0x%x\n",
+			 pHalData->bt_coexist.bt_radiosharedtype);
 	}
 }
 
diff --git a/drivers/staging/rtl8723au/hal/rtl8723a_cmd.c b/drivers/staging/rtl8723au/hal/rtl8723a_cmd.c
index 7b56411cc3c8..11e1108d0c56 100644
--- a/drivers/staging/rtl8723au/hal/rtl8723a_cmd.c
+++ b/drivers/staging/rtl8723au/hal/rtl8723a_cmd.c
@@ -142,32 +142,18 @@ int rtl8723a_set_raid_cmd(struct rtw_adapter *padapter, u32 mask, u8 arg)
 /* arg[5] = Short GI */
 void rtl8723a_add_rateatid(struct rtw_adapter *pAdapter, u32 bitmap, u8 arg, u8 rssi_level)
 {
-	struct hal_data_8723a	*pHalData = GET_HAL_DATA(pAdapter);
-	u8 macid = arg&0x1f;
-	u8 raid = (bitmap>>28) & 0x0f;
+	struct hal_data_8723a *pHalData = GET_HAL_DATA(pAdapter);
+	u8 macid = arg & 0x1f;
+	u32 raid = bitmap & 0xf0000000;
 
 	bitmap &= 0x0fffffff;
 	if (rssi_level != DM_RATR_STA_INIT)
 		bitmap = ODM_Get_Rate_Bitmap23a(pHalData, macid, bitmap,
 						rssi_level);
 
-	bitmap |= ((raid<<28)&0xf0000000);
+	bitmap |= raid;
 
-	if (pHalData->fw_ractrl == true) {
-		rtl8723a_set_raid_cmd(pAdapter, bitmap, arg);
-	} else {
-		u8 init_rate, shortGIrate = false;
-
-		init_rate = get_highest_rate_idx23a(bitmap&0x0fffffff)&0x3f;
-
-		shortGIrate = (arg&BIT(5)) ? true:false;
-
-		if (shortGIrate == true)
-			init_rate |= BIT(6);
-
-		rtl8723au_write8(pAdapter, REG_INIDATA_RATE_SEL + macid,
-				 init_rate);
-	}
+	rtl8723a_set_raid_cmd(pAdapter, bitmap, arg);
 }
 
 void rtl8723a_set_FwPwrMode_cmd(struct rtw_adapter *padapter, u8 Mode)
@@ -183,10 +169,8 @@ void rtl8723a_set_FwPwrMode_cmd(struct rtw_adapter *padapter, u8 Mode)
 	    prevent conficting setting in Fw power */
 	/*  saving sequence. 2010.06.07. Added by tynli.
 	    Suggested by SD3 yschang. */
-	if ((Mode != PS_MODE_ACTIVE) &&
-	    (!IS_92C_SERIAL(pHalData->VersionID))) {
+	if (Mode != PS_MODE_ACTIVE && pHalData->rf_type != RF_2T2R)
 		ODM_RF_Saving23a(&pHalData->odmpriv, true);
-	}
 
 	H2CSetPwrMode.Mode = Mode;
 	H2CSetPwrMode.SmartPS = pwrpriv->smart_ps;
diff --git a/drivers/staging/rtl8723au/hal/rtl8723a_dm.c b/drivers/staging/rtl8723au/hal/rtl8723a_dm.c
index fa826b068d11..1e831f2d1caf 100644
--- a/drivers/staging/rtl8723au/hal/rtl8723a_dm.c
+++ b/drivers/staging/rtl8723au/hal/rtl8723a_dm.c
@@ -94,8 +94,6 @@ void rtl8723a_init_dm_priv(struct rtw_adapter *Adapter)
 	memset(pDM_Odm, 0, sizeof(*pDM_Odm));
 
 	pDM_Odm->Adapter = Adapter;
-	ODM_CmnInfoInit23a(pDM_Odm, ODM_CMNINFO_PLATFORM, 0x04);
-	ODM_CmnInfoInit23a(pDM_Odm, ODM_CMNINFO_INTERFACE, RTW_USB);/* RTL871X_HCI_TYPE */
 
 	ODM_CmnInfoInit23a(pDM_Odm, ODM_CMNINFO_IC_TYPE, ODM_RTL8723A);
 
@@ -119,15 +117,7 @@ void rtl8723a_init_dm_priv(struct rtw_adapter *Adapter)
 		ODM_CmnInfoInit23a(pDM_Odm, ODM_CMNINFO_EXT_LNA, true);
 		ODM_CmnInfoInit23a(pDM_Odm, ODM_CMNINFO_EXT_PA, true);
 	}
-	ODM_CmnInfoInit23a(pDM_Odm, ODM_CMNINFO_PATCH_ID, pHalData->CustomerID);
 	ODM_CmnInfoInit23a(pDM_Odm, ODM_CMNINFO_BWIFI_TEST, Adapter->registrypriv.wifi_spec);
-
-	if (pHalData->rf_type == RF_1T1R)
-		ODM_CmnInfoUpdate23a(pDM_Odm, ODM_CMNINFO_RF_TYPE, ODM_1T1R);
-	else if (pHalData->rf_type == RF_2T2R)
-		ODM_CmnInfoUpdate23a(pDM_Odm, ODM_CMNINFO_RF_TYPE, ODM_2T2R);
-	else if (pHalData->rf_type == RF_1T2R)
-		ODM_CmnInfoUpdate23a(pDM_Odm, ODM_CMNINFO_RF_TYPE, ODM_1T2R);
 }
 
 static void Update_ODM_ComInfo_8723a(struct rtw_adapter *Adapter)
@@ -136,16 +126,7 @@ static void Update_ODM_ComInfo_8723a(struct rtw_adapter *Adapter)
 	struct dm_odm_t *pDM_Odm = &pHalData->odmpriv;
 	struct dm_priv	*pdmpriv = &pHalData->dmpriv;
 	int i;
-	pdmpriv->InitODMFlag =	ODM_BB_DIG		|
-				ODM_BB_RA_MASK		|
-				ODM_BB_DYNAMIC_TXPWR	|
-				ODM_BB_FA_CNT		|
-				ODM_BB_RSSI_MONITOR	|
-				ODM_BB_CCK_PD		|
-				ODM_BB_PWR_SAVE		|
-				ODM_MAC_EDCA_TURBO	|
-				ODM_RF_TX_PWR_TRACK	|
-				ODM_RF_CALIBRATION;
+	pdmpriv->InitODMFlag = 0;
 	/*  Pointer reference */
 	rtl8723a_odm_support_ability_set(Adapter, DYNAMIC_ALL_FUNC_ENABLE);
 
diff --git a/drivers/staging/rtl8723au/hal/rtl8723a_hal_init.c b/drivers/staging/rtl8723au/hal/rtl8723a_hal_init.c
index a5eadd4e2580..04d01833dc30 100644
--- a/drivers/staging/rtl8723au/hal/rtl8723a_hal_init.c
+++ b/drivers/staging/rtl8723au/hal/rtl8723a_hal_init.c
@@ -95,7 +95,7 @@ static int _WriteFW(struct rtw_adapter *padapter, void *buffer, u32 size)
 			goto exit;
 	}
 	RT_TRACE(_module_hal_init_c_, _drv_info_,
-		 ("_WriteFW Done- for Normal chip.\n"));
+		 "_WriteFW Done- for Normal chip.\n");
 
 exit:
 	return ret;
@@ -115,13 +115,13 @@ static int _FWFreeToGo(struct rtw_adapter *padapter)
 
 	if (counter >= POLLING_READY_TIMEOUT_COUNT) {
 		RT_TRACE(_module_hal_init_c_, _drv_err_,
-			 ("%s: chksum report fail! REG_MCUFWDL:0x%08x\n",
-			  __func__, value32));
+			 "%s: chksum report fail! REG_MCUFWDL:0x%08x\n",
+			 __func__, value32);
 		return _FAIL;
 	}
 	RT_TRACE(_module_hal_init_c_, _drv_info_,
-		 ("%s: Checksum report OK! REG_MCUFWDL:0x%08x\n", __func__,
-		  value32));
+		 "%s: Checksum report OK! REG_MCUFWDL:0x%08x\n", __func__,
+		 value32);
 
 	value32 = rtl8723au_read32(padapter, REG_MCUFWDL);
 	value32 |= MCUFWDL_RDY;
@@ -134,17 +134,16 @@ static int _FWFreeToGo(struct rtw_adapter *padapter)
 		value32 = rtl8723au_read32(padapter, REG_MCUFWDL);
 		if (value32 & WINTINI_RDY) {
 			RT_TRACE(_module_hal_init_c_, _drv_info_,
-				 ("%s: Polling FW ready success!! "
-				  "REG_MCUFWDL:0x%08x\n",
-				  __func__, value32));
+				 "%s: Polling FW ready success!! REG_MCUFWDL:0x%08x\n",
+				 __func__, value32);
 			return _SUCCESS;
 		}
 		udelay(5);
 	} while (counter++ < POLLING_READY_TIMEOUT_COUNT);
 
 	RT_TRACE(_module_hal_init_c_, _drv_err_,
-		 ("%s: Polling FW ready fail!! REG_MCUFWDL:0x%08x\n",
-		  __func__, value32));
+		 "%s: Polling FW ready fail!! REG_MCUFWDL:0x%08x\n",
+		 __func__, value32);
 	return _FAIL;
 }
 
@@ -173,8 +172,8 @@ void rtl8723a_FirmwareSelfReset(struct rtw_adapter *padapter)
 			u1bTmp = rtl8723au_read8(padapter, REG_SYS_FUNC_EN + 1);
 		}
 		RT_TRACE(_module_hal_init_c_, _drv_info_,
-			 ("-%s: 8051 reset success (%d)\n", __func__,
-			  Delay));
+			 "-%s: 8051 reset success (%d)\n", __func__,
+			 Delay);
 
 		if ((Delay == 0)) {
 			/* force firmware reset */
@@ -206,13 +205,12 @@ int rtl8723a_FirmwareDownload(struct rtw_adapter *padapter)
 	int fw_size;
 	static int log_version;
 
-	RT_TRACE(_module_hal_init_c_, _drv_info_, ("+%s\n", __func__));
+	RT_TRACE(_module_hal_init_c_, _drv_info_, "+%s\n", __func__);
 
 	if (IS_8723A_A_CUT(pHalData->VersionID)) {
 		fw_name = "rtlwifi/rtl8723aufw_A.bin";
 		RT_TRACE(_module_hal_init_c_, _drv_info_,
-			 ("rtl8723a_FirmwareDownload: R8723FwImageArray_UMC "
-			  "for RTL8723A A CUT\n"));
+			 "rtl8723a_FirmwareDownload: R8723FwImageArray_UMC for RTL8723A A CUT\n");
 	} else if (IS_8723A_B_CUT(pHalData->VersionID)) {
 		/*  WLAN Fw. */
 		if (padapter->registrypriv.wifi_spec == 1) {
@@ -234,7 +232,7 @@ int rtl8723a_FirmwareDownload(struct rtw_adapter *padapter)
 		/*  <Roger_TODO> We should download proper RAM Code here
 		    to match the ROM code. */
 		RT_TRACE(_module_hal_init_c_, _drv_err_,
-			 ("%s: unknow version!\n", __func__));
+			 "%s: unknown version!\n", __func__);
 		rtStatus = _FAIL;
 		goto Exit;
 	}
@@ -319,11 +317,11 @@ int rtl8723a_FirmwareDownload(struct rtw_adapter *padapter)
 	rtStatus = _FWFreeToGo(padapter);
 	if (_SUCCESS != rtStatus) {
 		RT_TRACE(_module_hal_init_c_, _drv_err_,
-			 ("DL Firmware failed!\n"));
+			 "DL Firmware failed!\n");
 		goto Exit;
 	}
 	RT_TRACE(_module_hal_init_c_, _drv_info_,
-		 ("Firmware is ready to run!\n"));
+		 "Firmware is ready to run!\n");
 
 Exit:
 	kfree(firmware_buf);
@@ -424,15 +422,14 @@ hal_ReadEFuse_WiFi(struct rtw_adapter *padapter,
 			offset = GET_HDR_OFFSET_2_0(efuseHeader);
 
 			ReadEFuseByte23a(padapter, eFuse_Addr++, &efuseExtHdr);
-			if (ALL_WORDS_DISABLED(efuseExtHdr)) {
+			if (ALL_WORDS_DISABLED(efuseExtHdr))
 				continue;
-			}
 
 			offset |= ((efuseExtHdr & 0xF0) >> 1);
-			wden = (efuseExtHdr & 0x0F);
+			wden = efuseExtHdr & 0x0F;
 		} else {
-			offset = ((efuseHeader >> 4) & 0x0f);
-			wden = (efuseHeader & 0x0f);
+			offset = (efuseHeader >> 4) & 0x0f;
+			wden = efuseHeader & 0x0f;
 		}
 
 		if (offset < EFUSE_MAX_SECTION_8723A) {
@@ -524,15 +521,14 @@ hal_ReadEFuse_BT(struct rtw_adapter *padapter,
 
 				ReadEFuseByte23a(padapter, eFuse_Addr++,
 					      &efuseExtHdr);
-				if (ALL_WORDS_DISABLED(efuseExtHdr)) {
+				if (ALL_WORDS_DISABLED(efuseExtHdr))
 					continue;
-				}
 
 				offset |= ((efuseExtHdr & 0xF0) >> 1);
-				wden = (efuseExtHdr & 0x0F);
+				wden = efuseExtHdr & 0x0F;
 			} else {
-				offset = ((efuseHeader >> 4) & 0x0f);
-				wden = (efuseHeader & 0x0f);
+				offset = (efuseHeader >> 4) & 0x0f;
+				wden = efuseHeader & 0x0f;
 			}
 
 			if (offset < EFUSE_BT_MAX_SECTION) {
@@ -630,9 +626,8 @@ u16 rtl8723a_EfuseGetCurrentSize_WiFi(struct rtw_adapter *padapter)
 			hoffset = GET_HDR_OFFSET_2_0(efuse_data);
 			efuse_addr++;
 			efuse_OneByteRead23a(padapter, efuse_addr, &efuse_data);
-			if (ALL_WORDS_DISABLED(efuse_data)) {
+			if (ALL_WORDS_DISABLED(efuse_data))
 				continue;
-			}
 
 			hoffset |= ((efuse_data & 0xF0) >> 1);
 			hworden = efuse_data & 0x0F;
@@ -721,9 +716,8 @@ u16 rtl8723a_EfuseGetCurrentSize_BT(struct rtw_adapter *padapter)
 		}
 
 		/*  Check if we need to check next bank efuse */
-		if (efuse_addr < retU2) {
+		if (efuse_addr < retU2)
 			break;	/*  don't need to check next bank. */
-		}
 	}
 
 	retU2 = ((bank - 1) * EFUSE_BT_REAL_BANK_CONTENT_LEN) + efuse_addr;
@@ -744,7 +738,7 @@ void rtl8723a_read_chip_version(struct rtw_adapter *padapter)
 	value32 = rtl8723au_read32(padapter, REG_SYS_CFG);
 	ChipVersion.ICType = CHIP_8723A;
 	ChipVersion.ChipType = ((value32 & RTL_ID) ? TEST_CHIP : NORMAL_CHIP);
-	ChipVersion.RFType = RF_TYPE_1T1R;
+	pHalData->rf_type = RF_1T1R;
 	ChipVersion.VendorType =
 		((value32 & VENDOR_ID) ? CHIP_VENDOR_UMC : CHIP_VENDOR_TSMC);
 	ChipVersion.CUTVersion = (value32 & CHIP_VER_RTL_MASK) >> CHIP_VER_RTL_SHIFT;	/*  IC version (CUT) */
@@ -755,7 +749,7 @@ void rtl8723a_read_chip_version(struct rtw_adapter *padapter)
 
 	value32 = rtl8723au_read32(padapter, REG_GPIO_OUTSTS);
 	/*  ROM code version. */
-	ChipVersion.ROMVer = ((value32 & RF_RL_ID) >> 20);
+	ChipVersion.ROMVer = (value32 & RF_RL_ID) >> 20;
 
 	/*  For multi-function consideration. Added by Roger, 2010.10.06. */
 	pHalData->MultiFunc = RT_MULTI_FUNC_NONE;
@@ -768,16 +762,8 @@ void rtl8723a_read_chip_version(struct rtw_adapter *padapter)
 	pHalData->PolarityCtl =
 		((value32 & WL_HWPDN_SL) ? RT_POLARITY_HIGH_ACT :
 		 RT_POLARITY_LOW_ACT);
-	dump_chip_info23a(ChipVersion);
 	pHalData->VersionID = ChipVersion;
 
-	if (IS_1T2R(ChipVersion))
-		pHalData->rf_type = RF_1T2R;
-	else if (IS_2T2R(ChipVersion))
-		pHalData->rf_type = RF_2T2R;
-	else
-		pHalData->rf_type = RF_1T1R;
-
 	MSG_8723A("RF_Type is %x!!\n", pHalData->rf_type);
 }
 
@@ -831,7 +817,7 @@ static void ResumeTxBeacon(struct rtw_adapter *padapter)
 	    we record the value */
 	/*  which should be read from register to a global variable. */
 
-	RT_TRACE(_module_hci_hal_init_c_, _drv_info_, ("+ResumeTxBeacon\n"));
+	RT_TRACE(_module_hci_hal_init_c_, _drv_info_, "+ResumeTxBeacon\n");
 
 	pHalData->RegFwHwTxQCtrl |= BIT(6);
 	rtl8723au_write8(padapter, REG_FWHW_TXQ_CTRL + 2,
@@ -849,7 +835,7 @@ static void StopTxBeacon(struct rtw_adapter *padapter)
 	    we record the value */
 	/*  which should be read from register to a global variable. */
 
-	RT_TRACE(_module_hci_hal_init_c_, _drv_info_, ("+StopTxBeacon\n"));
+	RT_TRACE(_module_hci_hal_init_c_, _drv_info_, "+StopTxBeacon\n");
 
 	pHalData->RegFwHwTxQCtrl &= ~BIT(6);
 	rtl8723au_write8(padapter, REG_FWHW_TXQ_CTRL + 2,
@@ -995,7 +981,7 @@ int c2h_handler_8723a(struct rtw_adapter *padapter, struct c2h_evt_hdr *c2h_evt)
 	switch (c2h_evt->id) {
 	case C2H_DBG:
 		RT_TRACE(_module_hal_init_c_, _drv_info_,
-			 ("C2HCommandHandler: %s\n", c2h_evt->payload));
+			 "C2HCommandHandler: %s\n", c2h_evt->payload);
 		break;
 
 	case C2H_CCX_TX_RPT:
@@ -1005,22 +991,22 @@ int c2h_handler_8723a(struct rtw_adapter *padapter, struct c2h_evt_hdr *c2h_evt)
 		break;
 	case C2H_HW_INFO_EXCH:
 		RT_TRACE(_module_hal_init_c_, _drv_info_,
-			 ("[BT], C2H_HW_INFO_EXCH\n"));
+			 "[BT], C2H_HW_INFO_EXCH\n");
 		for (i = 0; i < c2h_evt->plen; i++) {
 			RT_TRACE(_module_hal_init_c_, _drv_info_,
-				 ("[BT], tmpBuf[%d]= 0x%x\n", i,
-				  c2h_evt->payload[i]));
+				 "[BT], tmpBuf[%d]= 0x%x\n", i,
+				 c2h_evt->payload[i]);
 		}
 		break;
 
 	case C2H_C2H_H2C_TEST:
 		RT_TRACE(_module_hal_init_c_, _drv_info_,
-			 ("[BT], C2H_H2C_TEST\n"));
+			 "[BT], C2H_H2C_TEST\n");
 		RT_TRACE(_module_hal_init_c_, _drv_info_,
-			 ("[BT], tmpBuf[0]/[1]/[2]/[3]/[4]= 0x%x/ 0x%x/ "
-			  "0x%x/ 0x%x/ 0x%x\n", c2h_evt->payload[0],
-			  c2h_evt->payload[1], c2h_evt->payload[2],
-			  c2h_evt->payload[3], c2h_evt->payload[4]));
+			 "[BT], tmpBuf[0]/[1]/[2]/[3]/[4]= 0x%x/ 0x%x/ 0x%x/ 0x%x/ 0x%x\n",
+			 c2h_evt->payload[0],
+			 c2h_evt->payload[1], c2h_evt->payload[2],
+			 c2h_evt->payload[3], c2h_evt->payload[4]);
 		break;
 
 	case C2H_BT_INFO:
@@ -1095,7 +1081,6 @@ void rtl8723a_init_default_value(struct rtw_adapter *padapter)
 	pdmpriv = &pHalData->dmpriv;
 
 	/*  init default value */
-	pHalData->fw_ractrl = false;
 	pHalData->bIQKInitialized = false;
 	if (!padapter->pwrctrlpriv.bkeepfwalive)
 		pHalData->LastHMEBoxNum = 0;
@@ -1149,14 +1134,13 @@ static int _LLTWrite(struct rtw_adapter *padapter, u32 address, u32 data)
 	/* polling */
 	do {
 		value = rtl8723au_read32(padapter, LLTReg);
-		if (_LLT_NO_ACTIVE == _LLT_OP_VALUE(value)) {
+		if (_LLT_NO_ACTIVE == _LLT_OP_VALUE(value))
 			break;
-		}
 
 		if (count > POLLING_LLT_THRESHOLD) {
 			RT_TRACE(_module_hal_init_c_, _drv_err_,
-				 ("Failed to polling write LLT done at "
-				  "address %d!\n", address));
+				 "Failed to polling write LLT done at address %d!\n",
+				 address);
 			status = _FAIL;
 			break;
 		}
@@ -1174,16 +1158,14 @@ int InitLLTTable23a(struct rtw_adapter *padapter, u32 boundary)
 
 	for (i = 0; i < (txpktbuf_bndy - 1); i++) {
 		status = _LLTWrite(padapter, i, i + 1);
-		if (status != _SUCCESS) {
+		if (status != _SUCCESS)
 			return status;
-		}
 	}
 
 	/*  end of list */
 	status = _LLTWrite(padapter, (txpktbuf_bndy - 1), 0xFF);
-	if (status != _SUCCESS) {
+	if (status != _SUCCESS)
 		return status;
-	}
 
 	/*  Make the other pages as ring buffer */
 	/*  This ring buffer is used as beacon buffer if we config this
@@ -1191,16 +1173,14 @@ int InitLLTTable23a(struct rtw_adapter *padapter, u32 boundary)
 	/*  Otherwise used as local loopback buffer. */
 	for (i = txpktbuf_bndy; i < Last_Entry_Of_TxPktBuf; i++) {
 		status = _LLTWrite(padapter, i, (i + 1));
-		if (_SUCCESS != status) {
+		if (_SUCCESS != status)
 			return status;
-		}
 	}
 
 	/*  Let last entry point to the start entry of ring buffer */
 	status = _LLTWrite(padapter, Last_Entry_Of_TxPktBuf, txpktbuf_bndy);
-	if (status != _SUCCESS) {
+	if (status != _SUCCESS)
 		return status;
-	}
 
 	return status;
 }
@@ -1272,8 +1252,6 @@ e.	SYS_FUNC_EN 0x02[7:0] = 0x14		reset BB state machine
 
 	/*  2010/08/12 MH We need to set BB/GLBAL reset to save power
 	    for SS mode. */
-
-/*	RT_TRACE(COMP_INIT, DBG_LOUD, ("======> RF off and reset BB.\n")); */
 }
 
 static void _ResetDigitalProcedure1_92C(struct rtw_adapter *padapter,
@@ -1402,8 +1380,6 @@ static void _DisableAnalog(struct rtw_adapter *padapter, bool bWithoutHWSM)
 		value8 = rtl8723au_read8(padapter, REG_LDOV12D_CTRL);
 		value8 &= ~LDV12_EN;
 		rtl8723au_write8(padapter, REG_LDOV12D_CTRL, value8);
-/*		RT_TRACE(COMP_INIT, DBG_LOUD,
-		(" REG_LDOV12D_CTRL Reg0x21:0x%02x.\n", value8)); */
 	}
 
 	/*****************************
@@ -1435,9 +1411,9 @@ static void _DisableAnalog(struct rtw_adapter *padapter, bool bWithoutHWSM)
 /*  HW Auto state machine */
 int CardDisableHWSM(struct rtw_adapter *padapter, u8 resetMCU)
 {
-	if (padapter->bSurpriseRemoved) {
+	if (padapter->bSurpriseRemoved)
 		return _SUCCESS;
-	}
+
 	/*  RF Off Sequence ==== */
 	_DisableRFAFEAndResetBB8192C(padapter);
 
@@ -1451,7 +1427,7 @@ int CardDisableHWSM(struct rtw_adapter *padapter, u8 resetMCU)
 	_DisableAnalog(padapter, false);
 
 	RT_TRACE(_module_hci_hal_init_c_, _drv_info_,
-		 ("======> Card disable finished.\n"));
+		 "======> Card disable finished.\n");
 
 	return _SUCCESS;
 }
@@ -1459,9 +1435,8 @@ int CardDisableHWSM(struct rtw_adapter *padapter, u8 resetMCU)
 /*  without HW Auto state machine */
 int CardDisableWithoutHWSM(struct rtw_adapter *padapter)
 {
-	if (padapter->bSurpriseRemoved) {
+	if (padapter->bSurpriseRemoved)
 		return _SUCCESS;
-	}
 
 	/*  RF Off Sequence ==== */
 	_DisableRFAFEAndResetBB8192C(padapter);
@@ -1478,8 +1453,6 @@ int CardDisableWithoutHWSM(struct rtw_adapter *padapter)
 	/*   ==== Disable analog sequence === */
 	_DisableAnalog(padapter, true);
 
-	/* RT_TRACE(COMP_INIT, DBG_LOUD,
-	   ("<====== Card Disable Without HWSM .\n")); */
 	return _SUCCESS;
 }
 
@@ -1496,7 +1469,7 @@ void Hal_InitPGData(struct rtw_adapter *padapter, u8 *PROMContent)
 		}
 	} else {
 		RT_TRACE(_module_hci_hal_init_c_, _drv_notice_,
-			 ("AutoLoad Fail reported from CR9346!!\n"));
+			 "AutoLoad Fail reported from CR9346!!\n");
 		/* update to default value 0xFF */
 		if (!pEEPROM->EepromOrEfuse)
 			EFUSE_ShadowMapUpdate23a(padapter, EFUSE_WIFI);
@@ -1521,7 +1494,7 @@ void Hal_EfuseParseIDCode(struct rtw_adapter *padapter, u8 *hwinfo)
 	}
 
 	RT_TRACE(_module_hal_init_c_, _drv_info_,
-		 ("EEPROM ID = 0x%04x\n", EEPROMId));
+		 "EEPROM ID = 0x%04x\n", EEPROMId);
 }
 
 static void Hal_EEValueCheck(u8 EEType, void *pInValue, void *pOutValue)
@@ -1536,9 +1509,8 @@ static void Hal_EEValueCheck(u8 EEType, void *pInValue, void *pOutValue)
 			*pOut = *pIn;
 		else {
 			RT_TRACE(_module_hci_hal_init_c_, _drv_err_,
-				 ("EETYPE_TX_PWR, value =%d is invalid, set "
-				  "to default = 0x%x\n",
-				  *pIn, EEPROM_Default_TxPowerLevel));
+				 "EETYPE_TX_PWR, value =%d is invalid, set to default = 0x%x\n",
+				 *pIn, EEPROM_Default_TxPowerLevel);
 			*pOut = EEPROM_Default_TxPowerLevel;
 		}
 	}
@@ -1676,35 +1648,34 @@ Hal_EfuseParsetxpowerinfo_8723A(struct rtw_adapter *padapter,
 	for (rfPath = 0; rfPath < RF_PATH_MAX; rfPath++) {
 		for (ch = 0; ch < CHANNEL_MAX_NUMBER; ch++) {
 			RT_TRACE(_module_hci_hal_init_c_, _drv_info_,
-				 ("RF(%u)-Ch(%u) [CCK / HT40_1S / HT40_2S] = "
-				  "[0x%x / 0x%x / 0x%x]\n",
-				  rfPath, ch,
-				  pHalData->TxPwrLevelCck[rfPath][ch],
-				  pHalData->TxPwrLevelHT40_1S[rfPath][ch],
-				  pHalData->TxPwrLevelHT40_2S[rfPath][ch]));
+				 "RF(%u)-Ch(%u) [CCK / HT40_1S / HT40_2S] = [0x%x / 0x%x / 0x%x]\n",
+				 rfPath, ch,
+				 pHalData->TxPwrLevelCck[rfPath][ch],
+				 pHalData->TxPwrLevelHT40_1S[rfPath][ch],
+				 pHalData->TxPwrLevelHT40_2S[rfPath][ch]);
 
 		}
 	}
 	for (ch = 0; ch < CHANNEL_MAX_NUMBER; ch++) {
 		RT_TRACE(_module_hci_hal_init_c_, _drv_info_,
-			 ("RF-A Ht20 to HT40 Diff[%u] = 0x%x(%d)\n", ch,
-			  pHalData->TxPwrHt20Diff[RF_PATH_A][ch],
-			  pHalData->TxPwrHt20Diff[RF_PATH_A][ch]));
+			 "RF-A Ht20 to HT40 Diff[%u] = 0x%x(%d)\n", ch,
+			 pHalData->TxPwrHt20Diff[RF_PATH_A][ch],
+			 pHalData->TxPwrHt20Diff[RF_PATH_A][ch]);
 	}
 	for (ch = 0; ch < CHANNEL_MAX_NUMBER; ch++)
 		RT_TRACE(_module_hci_hal_init_c_, _drv_info_,
-			 ("RF-A Legacy to Ht40 Diff[%u] = 0x%x\n", ch,
-			  pHalData->TxPwrLegacyHtDiff[RF_PATH_A][ch]));
+			 "RF-A Legacy to Ht40 Diff[%u] = 0x%x\n", ch,
+			 pHalData->TxPwrLegacyHtDiff[RF_PATH_A][ch]);
 	for (ch = 0; ch < CHANNEL_MAX_NUMBER; ch++) {
 		RT_TRACE(_module_hci_hal_init_c_, _drv_info_,
-			 ("RF-B Ht20 to HT40 Diff[%u] = 0x%x(%d)\n", ch,
-			  pHalData->TxPwrHt20Diff[RF_PATH_B][ch],
-			  pHalData->TxPwrHt20Diff[RF_PATH_B][ch]));
+			 "RF-B Ht20 to HT40 Diff[%u] = 0x%x(%d)\n", ch,
+			 pHalData->TxPwrHt20Diff[RF_PATH_B][ch],
+			 pHalData->TxPwrHt20Diff[RF_PATH_B][ch]);
 	}
 	for (ch = 0; ch < CHANNEL_MAX_NUMBER; ch++)
 		RT_TRACE(_module_hci_hal_init_c_, _drv_info_,
-			 ("RF-B Legacy to HT40 Diff[%u] = 0x%x\n", ch,
-			  pHalData->TxPwrLegacyHtDiff[RF_PATH_B][ch]));
+			 "RF-B Legacy to HT40 Diff[%u] = 0x%x\n", ch,
+			 pHalData->TxPwrLegacyHtDiff[RF_PATH_B][ch]);
 	if (!AutoLoadFail) {
 		struct registry_priv *registry_par = &padapter->registrypriv;
 		if (registry_par->regulatory_tid == 0xff) {
@@ -1721,7 +1692,7 @@ Hal_EfuseParsetxpowerinfo_8723A(struct rtw_adapter *padapter,
 		pHalData->EEPROMRegulatory = 0;
 	}
 	RT_TRACE(_module_hci_hal_init_c_, _drv_info_,
-		 ("EEPROMRegulatory = 0x%x\n", pHalData->EEPROMRegulatory));
+		 "EEPROMRegulatory = 0x%x\n", pHalData->EEPROMRegulatory);
 
 	if (!AutoLoadFail)
 		pHalData->bTXPowerDataReadFromEEPORM = true;
@@ -1747,8 +1718,8 @@ Hal_EfuseParseBTCoexistInfo_8723A(struct rtw_adapter *padapter,
 		/*  eeprom spec */
 		tempval = hwinfo[RF_OPTION4_8723A];
 		pHalData->EEPROMBluetoothAntNum = (tempval & 0x1);
-		pHalData->EEPROMBluetoothAntIsolation = ((tempval & 0x10) >> 4);
-		pHalData->EEPROMBluetoothRadioShared = ((tempval & 0x20) >> 5);
+		pHalData->EEPROMBluetoothAntIsolation = (tempval & 0x10) >> 4;
+		pHalData->EEPROMBluetoothRadioShared = (tempval & 0x20) >> 5;
 	} else {
 		pHalData->EEPROMBluetoothCoexist = 0;
 		pHalData->EEPROMBluetoothType = BT_RTL8723A;
@@ -1771,8 +1742,8 @@ Hal_EfuseParseEEPROMVer(struct rtw_adapter *padapter,
 	else
 		pHalData->EEPROMVersion = 1;
 	RT_TRACE(_module_hci_hal_init_c_, _drv_info_,
-		 ("Hal_EfuseParseEEPROMVer(), EEVer = %d\n",
-		  pHalData->EEPROMVersion));
+		 "Hal_EfuseParseEEPROMVer(), EEVer = %d\n",
+		 pHalData->EEPROMVersion);
 }
 
 void
@@ -1805,10 +1776,10 @@ Hal_EfuseParseCustomerID(struct rtw_adapter *padapter,
 		pHalData->EEPROMSubCustomerID = 0;
 	}
 	RT_TRACE(_module_hci_hal_init_c_, _drv_info_,
-		 ("EEPROM Customer ID: 0x%2x\n", pHalData->EEPROMCustomerID));
+		 "EEPROM Customer ID: 0x%2x\n", pHalData->EEPROMCustomerID);
 	RT_TRACE(_module_hci_hal_init_c_, _drv_info_,
-		 ("EEPROM SubCustomer ID: 0x%02x\n",
-		  pHalData->EEPROMSubCustomerID));
+		 "EEPROM SubCustomer ID: 0x%02x\n",
+		 pHalData->EEPROMSubCustomerID);
 }
 
 void
@@ -1837,8 +1808,8 @@ Hal_EfuseParseXtal_8723A(struct rtw_adapter *pAdapter,
 		pHalData->CrystalCap = EEPROM_Default_CrystalCap_8723A;
 	}
 	RT_TRACE(_module_hci_hal_init_c_, _drv_info_,
-		 ("%s: CrystalCap = 0x%2x\n", __func__,
-		  pHalData->CrystalCap));
+		 "%s: CrystalCap = 0x%2x\n", __func__,
+		 pHalData->CrystalCap);
 }
 
 void
@@ -1875,9 +1846,8 @@ static void rtl8723a_cal_txdesc_chksum(struct tx_desc *ptxdesc)
 	/*  Clear first */
 	ptxdesc->txdw7 &= cpu_to_le32(0xffff0000);
 
-	for (index = 0; index < count; index++) {
+	for (index = 0; index < count; index++)
 		checksum ^= le16_to_cpu(*(usPtr + index));
-	}
 
 	ptxdesc->txdw7 |= cpu_to_le32(checksum & 0x0000ffff);
 }
@@ -1925,9 +1895,8 @@ void rtl8723a_fill_fake_txdesc(struct rtw_adapter *padapter, u8 *pDesc,
 		ptxdesc->txdw3 |= cpu_to_le32((8 << 28));
 	}
 
-	if (true == IsBTQosNull) {
+	if (true == IsBTQosNull)
 		ptxdesc->txdw2 |= cpu_to_le32(BIT(23));	/*  BT NULL */
-	}
 
 	/* offset 16 */
 	ptxdesc->txdw4 |= cpu_to_le32(BIT(8));	/* driver uses rate */
diff --git a/drivers/staging/rtl8723au/hal/rtl8723a_phycfg.c b/drivers/staging/rtl8723au/hal/rtl8723a_phycfg.c
index 19dc5e3b2e2e..46a30659c96f 100644
--- a/drivers/staging/rtl8723au/hal/rtl8723a_phycfg.c
+++ b/drivers/staging/rtl8723au/hal/rtl8723a_phycfg.c
@@ -121,18 +121,15 @@ PHY_SetBBReg(struct rtw_adapter *Adapter, u32 RegAddr, u32 BitMask, u32	Data)
 {
 	u32 OriginalValue, BitShift;
 
-	/* RT_TRACE(COMP_RF, DBG_TRACE, ("--->PHY_SetBBReg(): RegAddr(%#lx), BitMask(%#lx), Data(%#lx)\n", RegAddr, BitMask, Data)); */
-
 	if (BitMask != bMaskDWord) {/* if not "double word" write */
 		OriginalValue = rtl8723au_read32(Adapter, RegAddr);
 		BitShift = phy_CalculateBitShift(BitMask);
-		Data = ((OriginalValue & (~BitMask)) | (Data << BitShift));
+		Data = (OriginalValue & (~BitMask)) | (Data << BitShift);
 	}
 
 	rtl8723au_write32(Adapter, RegAddr, Data);
 
 	/* RTPRINT(FPHY, PHY_BBW, ("BBW MASK = 0x%lx Addr[0x%lx]= 0x%lx\n", BitMask, RegAddr, Data)); */
-	/* RT_TRACE(COMP_RF, DBG_TRACE, ("<---PHY_SetBBReg(): RegAddr(%#lx), BitMask(%#lx), Data(%#lx)\n", RegAddr, BitMask, Data)); */
 }
 
 /*  */
@@ -190,25 +187,24 @@ phy_RFSerialRead(struct rtw_adapter *Adapter, enum RF_RADIO_PATH eRFPath,
 	/*  For 92S LSSI Read RFLSSIRead */
 	/*  For RF A/B write 0x824/82c(does not work in the future) */
 	/*  We must use 0x824 for RF A and B to execute read trigger */
-	tmplong = PHY_QueryBBReg(Adapter, rFPGA0_XA_HSSIParameter2, bMaskDWord);
+	tmplong = rtl8723au_read32(Adapter, rFPGA0_XA_HSSIParameter2);
 	if (eRFPath == RF_PATH_A)
 		tmplong2 = tmplong;
 	else
-		tmplong2 = PHY_QueryBBReg(Adapter, pPhyReg->rfHSSIPara2,
-					  bMaskDWord);
+		tmplong2 = rtl8723au_read32(Adapter, pPhyReg->rfHSSIPara2);
 
 	tmplong2 = (tmplong2 & ~bLSSIReadAddress) |
 		(NewOffset << 23) | bLSSIReadEdge;	/* T65 RF */
 
-	PHY_SetBBReg(Adapter, rFPGA0_XA_HSSIParameter2,
-		     bMaskDWord, tmplong & (~bLSSIReadEdge));
+	rtl8723au_write32(Adapter, rFPGA0_XA_HSSIParameter2,
+			  tmplong & (~bLSSIReadEdge));
 	udelay(10);/*  PlatformStallExecution(10); */
 
-	PHY_SetBBReg(Adapter, pPhyReg->rfHSSIPara2, bMaskDWord, tmplong2);
+	rtl8723au_write32(Adapter, pPhyReg->rfHSSIPara2, tmplong2);
 	udelay(100);/* PlatformStallExecution(100); */
 
-	PHY_SetBBReg(Adapter, rFPGA0_XA_HSSIParameter2, bMaskDWord,
-		     tmplong | bLSSIReadEdge);
+	rtl8723au_write32(Adapter, rFPGA0_XA_HSSIParameter2,
+			  tmplong | bLSSIReadEdge);
 	udelay(10);/* PlatformStallExecution(10); */
 
 	if (eRFPath == RF_PATH_A)
@@ -319,9 +315,7 @@ phy_RFSerialWrite(struct rtw_adapter *Adapter, enum RF_RADIO_PATH eRFPath,
 	/*  */
 	/*  Write Operation */
 	/*  */
-	PHY_SetBBReg(Adapter, pPhyReg->rf3wireOffset, bMaskDWord, DataAndAddr);
-	/* RTPRINT(FPHY, PHY_RFW, ("RFW-%d Addr[0x%lx]= 0x%lx\n", eRFPath, pPhyReg->rf3wireOffset, DataAndAddr)); */
-
+	rtl8723au_write32(Adapter, pPhyReg->rf3wireOffset, DataAndAddr);
 }
 
 /**
@@ -392,7 +386,7 @@ PHY_SetRFReg(struct rtw_adapter *Adapter, enum RF_RADIO_PATH eRFPath,
 	if (BitMask != bRFRegOffsetMask) {
 		Original_Value = phy_RFSerialRead(Adapter, eRFPath, RegAddr);
 		BitShift =  phy_CalculateBitShift(BitMask);
-		Data = ((Original_Value & (~BitMask)) | (Data << BitShift));
+		Data = (Original_Value & (~BitMask)) | (Data << BitShift);
 	}
 
 	phy_RFSerialWrite(Adapter, eRFPath, RegAddr, Data);
@@ -419,7 +413,6 @@ PHY_SetRFReg(struct rtw_adapter *Adapter, enum RF_RADIO_PATH eRFPath,
 int PHY_MACConfig8723A(struct rtw_adapter *Adapter)
 {
 	struct hal_data_8723a *pHalData = GET_HAL_DATA(Adapter);
-	bool is92C = IS_92C_SERIAL(pHalData->VersionID);
 
 	/*  */
 	/*  Config MAC */
@@ -427,9 +420,9 @@ int PHY_MACConfig8723A(struct rtw_adapter *Adapter)
 	ODM_ReadAndConfig_MAC_REG_8723A(&pHalData->odmpriv);
 
 	/*  2010.07.13 AMPDU aggregation number 9 */
-	/* rtw_write16(Adapter, REG_MAX_AGGR_NUM, MAX_AGGR_NUM); */
 	rtl8723au_write8(Adapter, REG_MAX_AGGR_NUM, 0x0A);
-	if (is92C && (BOARD_USB_DONGLE == pHalData->BoardType))
+	if (pHalData->rf_type == RF_2T2R &&
+	    BOARD_USB_DONGLE == pHalData->BoardType)
 		rtl8723au_write8(Adapter, 0x40, 0x04);
 
 	return _SUCCESS;
@@ -552,131 +545,51 @@ storePwrIndexDiffRateOffset(struct rtw_adapter *Adapter, u32 RegAddr,
 
 	if (RegAddr == rTxAGC_A_Rate18_06) {
 		pHalData->MCSTxPowerLevelOriginalOffset[pHalData->pwrGroupCnt][0] = Data;
-		/* RT_TRACE(COMP_INIT, DBG_TRACE,
-		   ("MCSTxPowerLevelOriginalOffset[%d][0] = 0x%lx\n",
-		   pHalData->pwrGroupCnt, */
-		/*	pHalData->MCSTxPowerLevelOriginalOffset[
-			pHalData->pwrGroupCnt][0])); */
 	}
 	if (RegAddr == rTxAGC_A_Rate54_24) {
 		pHalData->MCSTxPowerLevelOriginalOffset[pHalData->pwrGroupCnt][1] = Data;
-		/* RT_TRACE(COMP_INIT, DBG_TRACE,
-		   ("MCSTxPowerLevelOriginalOffset[%d][1] = 0x%lx\n",
-		   pHalData->pwrGroupCnt, */
-		/*	pHalData->MCSTxPowerLevelOriginalOffset[
-			pHalData->pwrGroupCnt][1])); */
 	}
 	if (RegAddr == rTxAGC_A_CCK1_Mcs32) {
 		pHalData->MCSTxPowerLevelOriginalOffset[pHalData->pwrGroupCnt][6] = Data;
-		/* RT_TRACE(COMP_INIT, DBG_TRACE,
-		   ("MCSTxPowerLevelOriginalOffset[%d][6] = 0x%lx\n",
-		   pHalData->pwrGroupCnt, */
-		/*	pHalData->MCSTxPowerLevelOriginalOffset[
-			pHalData->pwrGroupCnt][6])); */
 	}
 	if (RegAddr == rTxAGC_B_CCK11_A_CCK2_11 && BitMask == 0xffffff00) {
 		pHalData->MCSTxPowerLevelOriginalOffset[pHalData->pwrGroupCnt][7] = Data;
-		/* RT_TRACE(COMP_INIT, DBG_TRACE,
-		   ("MCSTxPowerLevelOriginalOffset[%d][7] = 0x%lx\n",
-		   pHalData->pwrGroupCnt, */
-		/*	pHalData->MCSTxPowerLevelOriginalOffset[
-			pHalData->pwrGroupCnt][7])); */
 	}
 	if (RegAddr == rTxAGC_A_Mcs03_Mcs00) {
 		pHalData->MCSTxPowerLevelOriginalOffset[pHalData->pwrGroupCnt][2] = Data;
-		/* RT_TRACE(COMP_INIT, DBG_TRACE,
-		   ("MCSTxPowerLevelOriginalOffset[%d][2] = 0x%lx\n",
-		   pHalData->pwrGroupCnt, */
-		/*	pHalData->MCSTxPowerLevelOriginalOffset[
-			pHalData->pwrGroupCnt][2])); */
 	}
 	if (RegAddr == rTxAGC_A_Mcs07_Mcs04) {
 		pHalData->MCSTxPowerLevelOriginalOffset[pHalData->pwrGroupCnt][3] = Data;
-		/* RT_TRACE(COMP_INIT, DBG_TRACE,
-		   ("MCSTxPowerLevelOriginalOffset[%d][3] = 0x%lx\n",
-		   pHalData->pwrGroupCnt, */
-		/*	pHalData->MCSTxPowerLevelOriginalOffset[
-			pHalData->pwrGroupCnt][3])); */
 	}
 	if (RegAddr == rTxAGC_A_Mcs11_Mcs08) {
 		pHalData->MCSTxPowerLevelOriginalOffset[pHalData->pwrGroupCnt][4] = Data;
-		/* RT_TRACE(COMP_INIT, DBG_TRACE,
-		   ("MCSTxPowerLevelOriginalOffset[%d][4] = 0x%lx\n",
-		   pHalData->pwrGroupCnt, */
-		/*	pHalData->MCSTxPowerLevelOriginalOffset[
-			pHalData->pwrGroupCnt][4])); */
 	}
 	if (RegAddr == rTxAGC_A_Mcs15_Mcs12) {
 		pHalData->MCSTxPowerLevelOriginalOffset[pHalData->pwrGroupCnt][5] = Data;
-		/* RT_TRACE(COMP_INIT, DBG_TRACE,
-		   ("MCSTxPowerLevelOriginalOffset[%d][5] = 0x%lx\n",
-		   pHalData->pwrGroupCnt, */
-		/*	pHalData->MCSTxPowerLevelOriginalOffset[
-			pHalData->pwrGroupCnt][5])); */
 	}
 	if (RegAddr == rTxAGC_B_Rate18_06) {
 		pHalData->MCSTxPowerLevelOriginalOffset[pHalData->pwrGroupCnt][8] = Data;
-		/* RT_TRACE(COMP_INIT, DBG_TRACE,
-		   ("MCSTxPowerLevelOriginalOffset[%d][8] = 0x%lx\n",
-		   pHalData->pwrGroupCnt, */
-		/*	pHalData->MCSTxPowerLevelOriginalOffset[
-			pHalData->pwrGroupCnt][8])); */
 	}
 	if (RegAddr == rTxAGC_B_Rate54_24) {
 		pHalData->MCSTxPowerLevelOriginalOffset[pHalData->pwrGroupCnt][9] = Data;
-		/* RT_TRACE(COMP_INIT, DBG_TRACE,
-		   ("MCSTxPowerLevelOriginalOffset[%d][9] = 0x%lx\n",
-		   pHalData->pwrGroupCnt, */
-		/*	pHalData->MCSTxPowerLevelOriginalOffset[
-			pHalData->pwrGroupCnt][9])); */
 	}
 	if (RegAddr == rTxAGC_B_CCK1_55_Mcs32) {
 		pHalData->MCSTxPowerLevelOriginalOffset[pHalData->pwrGroupCnt][14] = Data;
-		/* RT_TRACE(COMP_INIT, DBG_TRACE,
-		   ("MCSTxPowerLevelOriginalOffset[%d][14] = 0x%lx\n",
-		   pHalData->pwrGroupCnt, */
-		/*	pHalData->MCSTxPowerLevelOriginalOffset[
-			pHalData->pwrGroupCnt][14])); */
 	}
 	if (RegAddr == rTxAGC_B_CCK11_A_CCK2_11 && BitMask == 0x000000ff) {
 		pHalData->MCSTxPowerLevelOriginalOffset[pHalData->pwrGroupCnt][15] = Data;
-		/* RT_TRACE(COMP_INIT, DBG_TRACE,
-		   ("MCSTxPowerLevelOriginalOffset[%d][15] = 0x%lx\n",
-		   pHalData->pwrGroupCnt, */
-		/*	pHalData->MCSTxPowerLevelOriginalOffset[
-			pHalData->pwrGroupCnt][15])); */
 	}
 	if (RegAddr == rTxAGC_B_Mcs03_Mcs00) {
 		pHalData->MCSTxPowerLevelOriginalOffset[pHalData->pwrGroupCnt][10] = Data;
-		/* RT_TRACE(COMP_INIT, DBG_TRACE,
-		   ("MCSTxPowerLevelOriginalOffset[%d][10] = 0x%lx\n",
-		   pHalData->pwrGroupCnt, */
-		/*	pHalData->MCSTxPowerLevelOriginalOffset[
-			pHalData->pwrGroupCnt][10])); */
 	}
 	if (RegAddr == rTxAGC_B_Mcs07_Mcs04) {
 		pHalData->MCSTxPowerLevelOriginalOffset[pHalData->pwrGroupCnt][11] = Data;
-		/* RT_TRACE(COMP_INIT, DBG_TRACE,
-		   ("MCSTxPowerLevelOriginalOffset[%d][11] = 0x%lx\n",
-		   pHalData->pwrGroupCnt, */
-		/*	pHalData->MCSTxPowerLevelOriginalOffset[
-			pHalData->pwrGroupCnt][11])); */
 	}
 	if (RegAddr == rTxAGC_B_Mcs11_Mcs08) {
 		pHalData->MCSTxPowerLevelOriginalOffset[pHalData->pwrGroupCnt][12] = Data;
-		/* RT_TRACE(COMP_INIT, DBG_TRACE,
-		   ("MCSTxPowerLevelOriginalOffset[%d][12] = 0x%lx\n",
-		   pHalData->pwrGroupCnt, */
-		/*	pHalData->MCSTxPowerLevelOriginalOffset[
-			pHalData->pwrGroupCnt][12])); */
 	}
 	if (RegAddr == rTxAGC_B_Mcs15_Mcs12) {
 		pHalData->MCSTxPowerLevelOriginalOffset[pHalData->pwrGroupCnt][13] = Data;
-		/* RT_TRACE(COMP_INIT, DBG_TRACE,
-		   ("MCSTxPowerLevelOriginalOffset[%d][13] = 0x%lx\n",
-		   pHalData->pwrGroupCnt, */
-		/*	pHalData->MCSTxPowerLevelOriginalOffset[
-			pHalData->pwrGroupCnt][13])); */
 		pHalData->pwrGroupCnt++;
 	}
 }
@@ -831,7 +744,7 @@ PHY_BBConfig8723A(struct rtw_adapter *Adapter)
 			     (CrystalCap | (CrystalCap << 6)));
 	}
 
-	PHY_SetBBReg(Adapter, REG_LDOA15_CTRL, bMaskDWord, 0x01572505);
+	rtl8723au_write32(Adapter, REG_LDOA15_CTRL, 0x01572505);
 	return rtStatus;
 }
 
@@ -920,10 +833,6 @@ _PHY_SetBWMode23a92C(struct rtw_adapter *Adapter)
 	u8 regBwOpMode;
 	u8 regRRSR_RSC;
 
-	/*  There is no 40MHz mode in RF_8225. */
-	if (pHalData->rf_chip == RF_8225)
-		return;
-
 	if (Adapter->bDriverStopped)
 		return;
 
@@ -982,10 +891,7 @@ _PHY_SetBWMode23a92C(struct rtw_adapter *Adapter)
 		break;
 
 	default:
-		/*RT_TRACE(COMP_DBG, DBG_LOUD,
-		  ("PHY_SetBWMode23aCallback8192C(): unknown Bandwidth: %#X\n" \
-		  , pHalData->CurrentChannelBW));*/
-			break;
+		break;
 	}
 	/* Skip over setting of J-mode in BB register here. Default value
 	   is "None J mode". Emily 20070315 */
@@ -994,41 +900,8 @@ _PHY_SetBWMode23a92C(struct rtw_adapter *Adapter)
 	/* NowL = PlatformEFIORead4Byte(Adapter, TSFR); */
 	/* NowH = PlatformEFIORead4Byte(Adapter, TSFR+4); */
 	/* EndTime = ((u64)NowH << 32) + NowL; */
-	/* RT_TRACE(COMP_SCAN, DBG_LOUD, ("SetBWMode23aCallback8190Pci: time
-	   of SetBWMode23a = %I64d us!\n", (EndTime - BeginTime))); */
-
-	/* 3<3>Set RF related register */
-	switch (pHalData->rf_chip) {
-	case RF_8225:
-		/* PHY_SetRF8225Bandwidth(Adapter,
-		   pHalData->CurrentChannelBW); */
-		break;
-
-	case RF_8256:
-		/*  Please implement this function in Hal8190PciPhy8256.c */
-		/* PHY_SetRF8256Bandwidth(Adapter,
-		   pHalData->CurrentChannelBW); */
-		break;
-
-	case RF_8258:
-		/*  Please implement this function in Hal8190PciPhy8258.c */
-		/*  PHY_SetRF8258Bandwidth(); */
-		break;
-
-	case RF_6052:
-		rtl8723a_phy_rf6052set_bw(Adapter, pHalData->CurrentChannelBW);
-		break;
-
-	default:
-		/* RT_ASSERT(false, ("Unknown RFChipID: %d\n",
-		   pHalData->RFChipID)); */
-		break;
-	}
-
-	/* pHalData->SetBWMode23aInProgress = false; */
 
-	/* RT_TRACE(COMP_SCAN, DBG_LOUD,
-	   ("<== PHY_SetBWMode23aCallback8192C() \n")); */
+	rtl8723a_phy_rf6052set_bw(Adapter, pHalData->CurrentChannelBW);
 }
 
  /*-----------------------------------------------------------------------------
diff --git a/drivers/staging/rtl8723au/hal/rtl8723a_rf6052.c b/drivers/staging/rtl8723au/hal/rtl8723a_rf6052.c
index 1aad4384471c..3e3f18634ffe 100644
--- a/drivers/staging/rtl8723au/hal/rtl8723a_rf6052.c
+++ b/drivers/staging/rtl8723au/hal/rtl8723a_rf6052.c
@@ -267,8 +267,8 @@ getTxPowerWriteValByRegulatory(struct rtw_adapter *Adapter, u8 Channel,
 			break;
 		case 2:	/*  Better regulatory */
 			/*  don't increase any power diff */
-			writeVal = ((index < 2) ? powerBase0[rf] :
-				    powerBase1[rf]);
+			writeVal = (index < 2) ? powerBase0[rf] :
+				    powerBase1[rf];
 			break;
 		case 3:	/*  Customer defined power diff. */
 			chnlGroup = 0;
@@ -353,7 +353,7 @@ static void writeOFDMPowerReg(struct rtw_adapter *Adapter, u8 index,
 		else
 			RegOffset = RegOffset_B[index];
 
-		PHY_SetBBReg(Adapter, RegOffset, bMaskDWord, writeVal);
+		rtl8723au_write32(Adapter, RegOffset, writeVal);
 
 		/*  201005115 Joseph: Set Tx Power diff for Tx power
 		    training mechanism. */
diff --git a/drivers/staging/rtl8723au/hal/rtl8723au_recv.c b/drivers/staging/rtl8723au/hal/rtl8723au_recv.c
index 6075b6dc1bee..0fec84bcb5d9 100644
--- a/drivers/staging/rtl8723au/hal/rtl8723au_recv.c
+++ b/drivers/staging/rtl8723au/hal/rtl8723au_recv.c
@@ -48,7 +48,7 @@ int rtl8723au_init_recv_priv(struct rtw_adapter *padapter)
 	if (!precvpriv->precv_buf) {
 		res = _FAIL;
 		RT_TRACE(_module_rtl871x_recv_c_, _drv_err_,
-			 ("alloc recv_buf fail!\n"));
+			 "alloc recv_buf fail!\n");
 		goto exit;
 	}
 
@@ -194,8 +194,8 @@ void update_recvframe_phyinfo(struct recv_frame *precvframe,
 	bool matchbssid = false;
 	u8 *bssid;
 
-	matchbssid = (!ieee80211_is_ctl(hdr->frame_control) &&
-		      !pattrib->icv_err && !pattrib->crc_err);
+	matchbssid = !ieee80211_is_ctl(hdr->frame_control) &&
+		      !pattrib->icv_err && !pattrib->crc_err;
 
 	if (matchbssid) {
 		switch (hdr->frame_control &
diff --git a/drivers/staging/rtl8723au/hal/rtl8723au_xmit.c b/drivers/staging/rtl8723au/hal/rtl8723au_xmit.c
index 1759487329ab..6bf87fe86644 100644
--- a/drivers/staging/rtl8723au/hal/rtl8723au_xmit.c
+++ b/drivers/staging/rtl8723au/hal/rtl8723au_xmit.c
@@ -42,7 +42,7 @@ static int urb_zero_packet_chk(struct rtw_adapter *padapter, int sz)
 
 static void rtl8192cu_cal_txdesc_chksum(struct tx_desc	*ptxdesc)
 {
-		u16	*usPtr = (u16 *)ptxdesc;
+		__le16	*usPtr = (__le16 *)ptxdesc;
 		u32 count = 16;		/*  (32 bytes / 2 bytes per XOR) => 16 times */
 		u32 index;
 		u16 checksum = 0;
@@ -130,7 +130,7 @@ static void fill_txdesc_phy(struct pkt_attrib *pattrib, __le32 *pdw)
 	}
 }
 
-static s32 update_txdesc(struct xmit_frame *pxmitframe, u8 *pmem, s32 sz, u8 bagg_pkt)
+static s32 update_txdesc(struct xmit_frame *pxmitframe, u8 *pmem, s32 sz)
 {
 	int	pull = 0;
 	uint	qsel;
@@ -143,7 +143,7 @@ static s32 update_txdesc(struct xmit_frame *pxmitframe, u8 *pmem, s32 sz, u8 bag
 	struct mlme_ext_info	*pmlmeinfo = &pmlmeext->mlmext_info;
 	int	bmcst = is_multicast_ether_addr(pattrib->ra);
 
-	if ((!bagg_pkt) && (urb_zero_packet_chk(padapter, sz) == 0)) {
+	if (urb_zero_packet_chk(padapter, sz) == 0) {
 		ptxdesc = (struct tx_desc *)(pmem+PACKET_OFFSET_SZ);
 		pull = 1;
 		pxmitframe->pkt_offset--;
@@ -272,7 +272,8 @@ static s32 update_txdesc(struct xmit_frame *pxmitframe, u8 *pmem, s32 sz, u8 bag
 	if (bmcst)
 		ptxdesc->txdw0 |= cpu_to_le32(BIT(24));
 
-	RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_, ("offset0-txdesc = 0x%x\n", ptxdesc->txdw0));
+	RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_,
+		 "offset0-txdesc = 0x%x\n", ptxdesc->txdw0);
 
 	/* offset 4 */
 	/*  pkt_offset, unit:8 bytes padding */
@@ -303,7 +304,7 @@ static int rtw_dump_xframe(struct rtw_adapter *padapter,
 
 	mem_addr = pxmitframe->buf_addr;
 
-	RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_, ("rtw_dump_xframe()\n"));
+	RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_, "rtw_dump_xframe()\n");
 
 	for (t = 0; t < pattrib->nr_frags; t++) {
 		if (inner_ret != _SUCCESS && ret == _SUCCESS)
@@ -311,7 +312,7 @@ static int rtw_dump_xframe(struct rtw_adapter *padapter,
 
 		if (t != (pattrib->nr_frags - 1)) {
 			RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_,
-				 ("pattrib->nr_frags =%d\n", pattrib->nr_frags));
+				 "pattrib->nr_frags =%d\n", pattrib->nr_frags);
 
 			sz = pxmitpriv->frag_len;
 			sz = sz - 4 - pattrib->icv_len;
@@ -320,7 +321,7 @@ static int rtw_dump_xframe(struct rtw_adapter *padapter,
 			sz = pattrib->last_txcmdsz;
 		}
 
-		pull = update_txdesc(pxmitframe, mem_addr, sz, false);
+		pull = update_txdesc(pxmitframe, mem_addr, sz);
 
 		if (pull) {
 			mem_addr += PACKET_OFFSET_SZ; /* pull txdesc head */
@@ -338,7 +339,7 @@ static int rtw_dump_xframe(struct rtw_adapter *padapter,
 		rtw_count_tx_stats23a(padapter, pxmitframe, sz);
 
 		RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_,
-			 ("rtw_write_port, w_sz =%d\n", w_sz));
+			 "rtw_write_port, w_sz =%d\n", w_sz);
 
 		mem_addr += w_sz;
 
@@ -365,7 +366,7 @@ bool rtl8723au_xmitframe_complete(struct rtw_adapter *padapter,
 	phwxmits = pxmitpriv->hwxmits;
 	hwentry = pxmitpriv->hwxmit_entry;
 
-	RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_, ("xmitframe_complete()\n"));
+	RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_, "xmitframe_complete()\n");
 
 	if (pxmitbuf == NULL) {
 		pxmitbuf = rtw_alloc_xmitbuf23a(pxmitpriv);
@@ -388,7 +389,8 @@ bool rtl8723au_xmitframe_complete(struct rtw_adapter *padapter,
 			rtw_os_xmit_complete23a(padapter, pxmitframe);/* always return ndis_packet after rtw_xmitframe_coalesce23a */
 		}
 
-		RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_, ("xmitframe_complete(): rtw_dump_xframe\n"));
+		RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_,
+			 "xmitframe_complete(): rtw_dump_xframe\n");
 
 		if (res == _SUCCESS) {
 			rtw_dump_xframe(padapter, pxmitframe);
@@ -481,7 +483,7 @@ enqueue:
 
 	if (res != _SUCCESS) {
 		RT_TRACE(_module_xmit_osdep_c_, _drv_err_,
-			 ("pre_xmitframe: enqueue xmitframe fail\n"));
+			 "pre_xmitframe: enqueue xmitframe fail\n");
 		rtw_free_xmitframe23a(pxmitpriv, pxmitframe);
 
 		/*  Trick, make the statistics correct */
diff --git a/drivers/staging/rtl8723au/hal/usb_halinit.c b/drivers/staging/rtl8723au/hal/usb_halinit.c
index adbf1c2dd383..42ae29d26302 100644
--- a/drivers/staging/rtl8723au/hal/usb_halinit.c
+++ b/drivers/staging/rtl8723au/hal/usb_halinit.c
@@ -447,22 +447,8 @@ static void _InitRetryFunction(struct rtw_adapter *Adapter)
 static void _InitRFType(struct rtw_adapter *Adapter)
 {
 	struct hal_data_8723a *pHalData = GET_HAL_DATA(Adapter);
-	bool is92CU = IS_92C_SERIAL(pHalData->VersionID);
 
-	pHalData->rf_chip = RF_6052;
-
-	if (!is92CU) {
-		pHalData->rf_type = RF_1T1R;
-		DBG_8723A("Set RF Chip ID to RF_6052 and RF type to 1T1R.\n");
-		return;
-	}
-
-	/*  TODO: Consider that EEPROM set 92CU to 1T1R later. */
-	/*  Force to overwrite setting according to chip version. Ignore
-	    EEPROM setting. */
-	/* pHalData->RF_Type = is92CU ? RF_2T2R : RF_1T1R; */
-	MSG_8723A("Set RF Chip ID to RF_6052 and RF type to %d.\n",
-		  pHalData->rf_type);
+	pHalData->rf_type = RF_1T1R;
 }
 
 /*  Set CCK and OFDM Block "ON" */
@@ -530,7 +516,7 @@ int rtl8723au_hal_init(struct rtw_adapter *Adapter)
 	/*  Check if MAC has already power on. by tynli. 2011.05.27. */
 	val8 = rtl8723au_read8(Adapter, REG_CR);
 	RT_TRACE(_module_hci_hal_init_c_, _drv_info_,
-		 ("%s: REG_CR 0x100 = 0x%02x\n", __func__, val8));
+		 "%s: REG_CR 0x100 = 0x%02x\n", __func__, val8);
 	/* Fix 92DU-VC S3 hang with the reason is that secondary mac is not
 	   initialized. */
 	/* 0x100 value of first mac is 0xEA while 0x100 value of secondary
@@ -540,13 +526,13 @@ int rtl8723au_hal_init(struct rtw_adapter *Adapter)
 	} else {
 		mac_on = true;
 		RT_TRACE(_module_hci_hal_init_c_, _drv_info_,
-			 ("%s: MAC has already power on\n", __func__));
+			 "%s: MAC has already power on\n", __func__);
 	}
 
 	status = _InitPowerOn(Adapter);
 	if (status == _FAIL) {
 		RT_TRACE(_module_hci_hal_init_c_, _drv_err_,
-			 ("Failed to init power on!\n"));
+			 "Failed to init power on!\n");
 		goto exit;
 	}
 
@@ -561,7 +547,7 @@ int rtl8723au_hal_init(struct rtw_adapter *Adapter)
 		status =  InitLLTTable23a(Adapter, boundary);
 		if (status == _FAIL) {
 			RT_TRACE(_module_hci_hal_init_c_, _drv_err_,
-				 ("Failed to init LLT table\n"));
+				 "Failed to init LLT table\n");
 			goto exit;
 		}
 	}
@@ -572,12 +558,10 @@ int rtl8723au_hal_init(struct rtw_adapter *Adapter)
 	status = rtl8723a_FirmwareDownload(Adapter);
 	if (status != _SUCCESS) {
 		Adapter->bFWReady = false;
-		pHalData->fw_ractrl = false;
 		DBG_8723A("fw download fail!\n");
 		goto exit;
 	} else {
 		Adapter->bFWReady = true;
-		pHalData->fw_ractrl = true;
 		DBG_8723A("fw download ok!\n");
 	}
 
@@ -625,17 +609,22 @@ int rtl8723au_hal_init(struct rtw_adapter *Adapter)
 	}
 
 	/* reducing 80M spur */
-	PHY_SetBBReg(Adapter, REG_AFE_XTAL_CTRL, bMaskDWord, 0x0381808d);
-	PHY_SetBBReg(Adapter, REG_AFE_PLL_CTRL, bMaskDWord, 0xf0ffff83);
-	PHY_SetBBReg(Adapter, REG_AFE_PLL_CTRL, bMaskDWord, 0xf0ffff82);
-	PHY_SetBBReg(Adapter, REG_AFE_PLL_CTRL, bMaskDWord, 0xf0ffff83);
+	rtl8723au_write32(Adapter, REG_AFE_XTAL_CTRL, 0x0381808d);
+	rtl8723au_write32(Adapter, REG_AFE_PLL_CTRL, 0xf0ffff83);
+	rtl8723au_write32(Adapter, REG_AFE_PLL_CTRL, 0xf0ffff82);
+	rtl8723au_write32(Adapter, REG_AFE_PLL_CTRL, 0xf0ffff83);
 
 	/* RFSW Control */
-	PHY_SetBBReg(Adapter, rFPGA0_TxInfo, bMaskDWord, 0x00000003);	/* 0x804[14]= 0 */
-	PHY_SetBBReg(Adapter, rFPGA0_XAB_RFInterfaceSW, bMaskDWord, 0x07000760);	/* 0x870[6:5]= b'11 */
-	PHY_SetBBReg(Adapter, rFPGA0_XA_RFInterfaceOE, bMaskDWord, 0x66F60210); /* 0x860[6:5]= b'00 */
+	/* 0x804[14]= 0 */
+	rtl8723au_write32(Adapter, rFPGA0_TxInfo, 0x00000003);
+	/* 0x870[6:5]= b'11 */
+	rtl8723au_write32(Adapter, rFPGA0_XAB_RFInterfaceSW, 0x07000760);
+	/* 0x860[6:5]= b'00 */
+	rtl8723au_write32(Adapter, rFPGA0_XA_RFInterfaceOE, 0x66F60210);
 
-	RT_TRACE(_module_hci_hal_init_c_, _drv_info_, ("%s: 0x870 = value 0x%x\n", __func__, PHY_QueryBBReg(Adapter, 0x870, bMaskDWord)));
+	RT_TRACE(_module_hci_hal_init_c_, _drv_info_,
+		 "%s: 0x870 = value 0x%x\n", __func__,
+		 rtl8723au_read32(Adapter, 0x870));
 
 	/*  */
 	/*  Joseph Note: Keep RfRegChnlVal for later use. */
@@ -747,15 +736,16 @@ int rtl8723au_hal_init(struct rtw_adapter *Adapter)
 
 	rtl8723a_InitHalDm(Adapter);
 
-	val8 = ((WiFiNavUpperUs + HAL_8723A_NAV_UPPER_UNIT - 1) /
-		HAL_8723A_NAV_UPPER_UNIT);
+	val8 = (WiFiNavUpperUs + HAL_8723A_NAV_UPPER_UNIT - 1) /
+		HAL_8723A_NAV_UPPER_UNIT;
 	rtl8723au_write8(Adapter, REG_NAV_UPPER, val8);
 
 	/*  2011/03/09 MH debug only, UMC-B cut pass 2500 S5 test, but we need to fin root cause. */
 	if (((rtl8723au_read32(Adapter, rFPGA0_RFMOD) & 0xFF000000) !=
 	     0x83000000)) {
 		PHY_SetBBReg(Adapter, rFPGA0_RFMOD, BIT(24), 1);
-		RT_TRACE(_module_hci_hal_init_c_, _drv_err_, ("%s: IQK fail recorver\n", __func__));
+		RT_TRACE(_module_hci_hal_init_c_, _drv_err_,
+			 "%s: IQK fail recover\n", __func__);
 	}
 
 	/* ack for xmit mgmt frames. */
@@ -806,19 +796,18 @@ static void phy_SsPwrSwitch92CU(struct rtw_adapter *Adapter,
 
 		/* AFE */
 		if (pHalData->rf_type ==  RF_2T2R)
-			PHY_SetBBReg(Adapter, rRx_Wait_CCA, bMaskDWord,
-				     0x63DB25A0);
+			rtl8723au_write32(Adapter, rRx_Wait_CCA, 0x63DB25A0);
 		else if (pHalData->rf_type ==  RF_1T1R)
-			PHY_SetBBReg(Adapter, rRx_Wait_CCA, bMaskDWord,
-				     0x631B25A0);
+			rtl8723au_write32(Adapter, rRx_Wait_CCA, 0x631B25A0);
 
 		/*  4. issue 3-wire command that RF set to Rx idle
 		    mode. This is used to re-write the RX idle mode. */
 		/*  We can only prvide a usual value instead and then
 		    HW will modify the value by itself. */
-		PHY_SetRFReg(Adapter, RF_PATH_A, 0, bRFRegOffsetMask, 0x32D95);
+		PHY_SetRFReg(Adapter, RF_PATH_A, RF_AC,
+			     bRFRegOffsetMask, 0x32D95);
 		if (pHalData->rf_type ==  RF_2T2R) {
-			PHY_SetRFReg(Adapter, RF_PATH_B, 0,
+			PHY_SetRFReg(Adapter, RF_PATH_B, RF_AC,
 				     bRFRegOffsetMask, 0x32D95);
 		}
 		break;
@@ -829,7 +818,7 @@ static void phy_SsPwrSwitch92CU(struct rtw_adapter *Adapter,
 		else
 			sps0 &= ~(BIT(0) | BIT(3));
 
-		RT_TRACE(_module_hal_init_c_, _drv_err_, ("SS LVL1\n"));
+		RT_TRACE(_module_hal_init_c_, _drv_err_, "SS LVL1\n");
 		/*  Disable RF and BB only for SelectSuspend. */
 
 		/*  1. Set BB/RF to shutdown. */
@@ -840,13 +829,11 @@ static void phy_SsPwrSwitch92CU(struct rtw_adapter *Adapter,
 						for packet detection */
 		/*	(4) Reg800[1] = 1	enable preamble power saving */
 		Adapter->pwrctrlpriv.PS_BBRegBackup[PSBBREG_RF0] =
-			PHY_QueryBBReg(Adapter, rFPGA0_XAB_RFParameter,
-				       bMaskDWord);
+			rtl8723au_read32(Adapter, rFPGA0_XAB_RFParameter);
 		Adapter->pwrctrlpriv.PS_BBRegBackup[PSBBREG_RF1] =
-			PHY_QueryBBReg(Adapter, rOFDM0_TRxPathEnable,
-				       bMaskDWord);
+			rtl8723au_read32(Adapter, rOFDM0_TRxPathEnable);
 		Adapter->pwrctrlpriv.PS_BBRegBackup[PSBBREG_RF2] =
-			PHY_QueryBBReg(Adapter, rFPGA0_RFMOD, bMaskDWord);
+			rtl8723au_read32(Adapter, rFPGA0_RFMOD);
 		if (pHalData->rf_type ==  RF_2T2R) {
 			PHY_SetBBReg(Adapter, rFPGA0_XAB_RFParameter,
 				     0x380038, 0);
@@ -858,18 +845,16 @@ static void phy_SsPwrSwitch92CU(struct rtw_adapter *Adapter,
 
 		/*  2 .AFE control register to power down. bit[30:22] */
 		Adapter->pwrctrlpriv.PS_BBRegBackup[PSBBREG_AFE0] =
-			PHY_QueryBBReg(Adapter, rRx_Wait_CCA, bMaskDWord);
+			rtl8723au_read32(Adapter, rRx_Wait_CCA);
 		if (pHalData->rf_type ==  RF_2T2R)
-			PHY_SetBBReg(Adapter, rRx_Wait_CCA, bMaskDWord,
-				     0x00DB25A0);
+			rtl8723au_write32(Adapter, rRx_Wait_CCA, 0x00DB25A0);
 		else if (pHalData->rf_type ==  RF_1T1R)
-			PHY_SetBBReg(Adapter, rRx_Wait_CCA, bMaskDWord,
-				     0x001B25A0);
+			rtl8723au_write32(Adapter, rRx_Wait_CCA, 0x001B25A0);
 
 		/*  3. issue 3-wire command that RF set to power down.*/
-		PHY_SetRFReg(Adapter, RF_PATH_A, 0, bRFRegOffsetMask, 0);
+		PHY_SetRFReg(Adapter, RF_PATH_A, RF_AC, bRFRegOffsetMask, 0);
 		if (pHalData->rf_type ==  RF_2T2R)
-			PHY_SetRFReg(Adapter, RF_PATH_B, 0,
+			PHY_SetRFReg(Adapter, RF_PATH_B, RF_AC,
 				     bRFRegOffsetMask, 0);
 
 		/*  4. Force PFM , disable SPS18_LDO_Marco_Block */
@@ -949,14 +934,14 @@ int rtl8723au_inirp_init(struct rtw_adapter *Adapter)
 
 	status = _SUCCESS;
 
-	RT_TRACE(_module_hci_hal_init_c_, _drv_info_, ("===> usb_inirp_init\n"));
+	RT_TRACE(_module_hci_hal_init_c_, _drv_info_, "===> usb_inirp_init\n");
 
 	/* issue Rx irp to receive data */
 	precvbuf = (struct recv_buf *)precvpriv->precv_buf;
 	for (i = 0; i < NR_RECVBUFF; i++) {
 		if (rtl8723au_read_port(Adapter, 0, precvbuf) == _FAIL) {
 			RT_TRACE(_module_hci_hal_init_c_, _drv_err_,
-				 ("usb_rx_init: usb_read_port error\n"));
+				 "usb_rx_init: usb_read_port error\n");
 			status = _FAIL;
 			goto exit;
 		}
@@ -964,7 +949,7 @@ int rtl8723au_inirp_init(struct rtw_adapter *Adapter)
 	}
 	if (rtl8723au_read_interrupt(Adapter) == _FAIL) {
 		RT_TRACE(_module_hci_hal_init_c_, _drv_err_,
-			 ("%s: usb_read_interrupt error\n", __func__));
+			 "%s: usb_read_interrupt error\n", __func__);
 		status = _FAIL;
 	}
 	pHalData->IntrMask[0] = rtl8723au_read32(Adapter, REG_USB_HIMR);
@@ -973,7 +958,7 @@ int rtl8723au_inirp_init(struct rtw_adapter *Adapter)
 	rtl8723au_write32(Adapter, REG_USB_HIMR, pHalData->IntrMask[0]);
 exit:
 	RT_TRACE(_module_hci_hal_init_c_, _drv_info_,
-		 ("<=== usb_inirp_init\n"));
+		 "<=== usb_inirp_init\n");
 	return status;
 }
 
@@ -982,7 +967,7 @@ int rtl8723au_inirp_deinit(struct rtw_adapter *Adapter)
 	struct hal_data_8723a	*pHalData = GET_HAL_DATA(Adapter);
 
 	RT_TRACE(_module_hci_hal_init_c_, _drv_info_,
-		 ("\n ===> usb_rx_deinit\n"));
+		 "===> usb_rx_deinit\n");
 	rtl8723au_read_port_cancel(Adapter);
 	pHalData->IntrMask[0] = rtl8723au_read32(Adapter, REG_USB_HIMR);
 	MSG_8723A("%s pHalData->IntrMask = 0x%04x\n", __func__,
@@ -990,7 +975,7 @@ int rtl8723au_inirp_deinit(struct rtw_adapter *Adapter)
 	pHalData->IntrMask[0] = 0x0;
 	rtl8723au_write32(Adapter, REG_USB_HIMR, pHalData->IntrMask[0]);
 	RT_TRACE(_module_hci_hal_init_c_, _drv_info_,
-		 ("\n <=== usb_rx_deinit\n"));
+		 "<=== usb_rx_deinit\n");
 	return _SUCCESS;
 }
 
@@ -1037,11 +1022,10 @@ static void Hal_EfuseParseMACAddr_8723AU(struct rtw_adapter *padapter,
 	}
 
 	RT_TRACE(_module_hci_hal_init_c_, _drv_notice_,
-		 ("Hal_EfuseParseMACAddr_8723AU: Permanent Address =%02x:%02x:"
-		  "%02x:%02x:%02x:%02x\n",
-		  pEEPROM->mac_addr[0], pEEPROM->mac_addr[1],
-		  pEEPROM->mac_addr[2], pEEPROM->mac_addr[3],
-		  pEEPROM->mac_addr[4], pEEPROM->mac_addr[5]));
+		 "Hal_EfuseParseMACAddr_8723AU: Permanent Address =%02x:%02x:%02x:%02x:%02x:%02x\n",
+		 pEEPROM->mac_addr[0], pEEPROM->mac_addr[1],
+		 pEEPROM->mac_addr[2], pEEPROM->mac_addr[3],
+		 pEEPROM->mac_addr[4], pEEPROM->mac_addr[5]);
 }
 
 static void readAdapterInfo(struct rtw_adapter *padapter)
@@ -1102,13 +1086,6 @@ static void _ReadPROMContent(struct rtw_adapter *Adapter)
 	readAdapterInfo(Adapter);
 }
 
-static void _ReadRFType(struct rtw_adapter *Adapter)
-{
-	struct hal_data_8723a *pHalData = GET_HAL_DATA(Adapter);
-
-	pHalData->rf_chip = RF_6052;
-}
-
 /*  */
 /*	Description: */
 /*		We should set Efuse cell selection to WiFi cell in default. */
@@ -1138,12 +1115,8 @@ void rtl8723a_read_adapter_info(struct rtw_adapter *Adapter)
 
 	hal_EfuseCellSel(Adapter);
 
-	_ReadRFType(Adapter);/* rf_chip -> _InitRFType() */
 	_ReadPROMContent(Adapter);
 
-	/* MSG_8723A("%s()(done), rf_chip = 0x%x, rf_type = 0x%x\n",
-	   __func__, pHalData->rf_chip, pHalData->rf_type); */
-
 	MSG_8723A("<==== _ReadAdapterInfo8723AU in %d ms\n",
 		  jiffies_to_msecs(jiffies - start));
 }
@@ -1192,8 +1165,6 @@ int GetHalDefVar8192CUsb(struct rtw_adapter *Adapter,
 	}
 		break;
 	default:
-		/* RT_TRACE(COMP_INIT, DBG_WARNING, ("GetHalDefVar8192CUsb(): "
-		   "Unkown variable: %d!\n", eVariable)); */
 		bResult = _FAIL;
 		break;
 	}
@@ -1211,7 +1182,7 @@ void rtl8723a_update_ramask(struct rtw_adapter *padapter,
 	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 	struct wlan_bssid_ex *cur_network = &pmlmeinfo->network;
-	u8 init_rate, networkType, raid;
+	u8 init_rate, networkType, raid, arg;
 	u32 mask, rate_bitmap;
 	u8 shortGIrate = false;
 	int supportRateNum;
@@ -1283,27 +1254,15 @@ void rtl8723a_update_ramask(struct rtw_adapter *padapter,
 
 	init_rate = get_highest_rate_idx23a(mask) & 0x3f;
 
-	if (pHalData->fw_ractrl == true) {
-		u8 arg = 0;
-
-		arg = mac_id & 0x1f;/* MACID */
+	arg = mac_id & 0x1f;/* MACID */
+	arg |= BIT(7);
 
-		arg |= BIT(7);
+	if (shortGIrate == true)
+		arg |= BIT(5);
 
-		if (shortGIrate == true)
-			arg |= BIT(5);
+	DBG_8723A("update raid entry, mask = 0x%x, arg = 0x%x\n", mask, arg);
 
-		DBG_8723A("update raid entry, mask = 0x%x, arg = 0x%x\n",
-			  mask, arg);
-
-		rtl8723a_set_raid_cmd(padapter, mask, arg);
-	} else {
-		if (shortGIrate == true)
-			init_rate |= BIT(6);
-
-		rtl8723au_write8(padapter, (REG_INIDATA_RATE_SEL + mac_id),
-				 init_rate);
-	}
+	rtl8723a_set_raid_cmd(padapter, mask, arg);
 
 	/* set ra_id */
 	psta->raid = raid;
diff --git a/drivers/staging/rtl8723au/hal/usb_ops_linux.c b/drivers/staging/rtl8723au/hal/usb_ops_linux.c
index a6d16adce107..371e6b373420 100644
--- a/drivers/staging/rtl8723au/hal/usb_ops_linux.c
+++ b/drivers/staging/rtl8723au/hal/usb_ops_linux.c
@@ -297,14 +297,12 @@ urb_submit:
 		case -ENODEV:
 		case -ESHUTDOWN:
 			RT_TRACE(_module_hci_ops_os_c_, _drv_err_,
-				 ("usb_read_port_complete:bSurpriseRemoved ="
-				  "true\n"));
+				 "usb_read_port_complete:bSurpriseRemoved =true\n");
 			/* Fall Through here */
 		case -ENOENT:
 			padapter->bDriverStopped = true;
 			RT_TRACE(_module_hci_ops_os_c_, _drv_err_,
-				 ("usb_read_port_complete:bDriverStopped ="
-				  "true\n"));
+				 "usb_read_port_complete:bDriverStopped =true\n");
 			break;
 		case -EPROTO:
 			break;
@@ -367,16 +365,16 @@ static int recvbuf2recvframe(struct rtw_adapter *padapter, struct sk_buff *pskb)
 
 	do {
 		RT_TRACE(_module_rtl871x_recv_c_, _drv_info_,
-			 ("recvbuf2recvframe: rxdesc = offsset 0:0x%08x, "
-			  "4:0x%08x, 8:0x%08x, C:0x%08x\n", prxstat->rxdw0,
-			  prxstat->rxdw1, prxstat->rxdw2, prxstat->rxdw4));
+			 "recvbuf2recvframe: rxdesc = offsset 0:0x%08x, 4:0x%08x, 8:0x%08x, C:0x%08x\n",
+			 prxstat->rxdw0, prxstat->rxdw1,
+			 prxstat->rxdw2, prxstat->rxdw4);
 
 		prxstat = (struct recv_stat *)pbuf;
 
 		precvframe = rtw_alloc_recvframe23a(pfree_recv_queue);
 		if (!precvframe) {
 			RT_TRACE(_module_rtl871x_recv_c_, _drv_err_,
-				 ("recvbuf2recvframe: precvframe == NULL\n"));
+				 "recvbuf2recvframe: precvframe == NULL\n");
 			DBG_8723A("%s()-%d: rtw_alloc_recvframe23a() failed! RX "
 				  "Drop!\n", __func__, __LINE__);
 			goto _exit_recvbuf2recvframe;
@@ -400,7 +398,7 @@ static int recvbuf2recvframe(struct rtw_adapter *padapter, struct sk_buff *pskb)
 
 		if (pattrib->pkt_len <= 0 || pkt_offset > transfer_len) {
 			RT_TRACE(_module_rtl871x_recv_c_, _drv_info_,
-				 ("recvbuf2recvframe: pkt_len<= 0\n"));
+				 "recvbuf2recvframe: pkt_len<= 0\n");
 			DBG_8723A("%s()-%d: RX Warning!\n",
 				  __func__, __LINE__);
 			rtw_free_recvframe23a(precvframe);
@@ -471,8 +469,7 @@ static int recvbuf2recvframe(struct rtw_adapter *padapter, struct sk_buff *pskb)
 
 		if (rtw_recv_entry23a(precvframe) != _SUCCESS)
 			RT_TRACE(_module_rtl871x_recv_c_, _drv_err_,
-				 ("recvbuf2recvframe: rtw_recv_entry23a"
-				  "(precvframe) != _SUCCESS\n"));
+				 "recvbuf2recvframe: rtw_recv_entry23a(precvframe) != _SUCCESS\n");
 
 		pkt_cnt--;
 		transfer_len -= pkt_offset;
@@ -520,16 +517,15 @@ static void usb_read_port_complete(struct urb *purb)
 	struct recv_priv *precvpriv = &padapter->recvpriv;
 
 	RT_TRACE(_module_hci_ops_os_c_, _drv_err_,
-		 ("usb_read_port_complete!!!\n"));
+		 "usb_read_port_complete!!!\n");
 
 	precvpriv->rx_pending_cnt--;
 
 	if (padapter->bSurpriseRemoved || padapter->bDriverStopped ||
 	    padapter->bReadPortCancel) {
 		RT_TRACE(_module_hci_ops_os_c_, _drv_err_,
-			 ("usb_read_port_complete:bDriverStopped(%d) OR "
-			  "bSurpriseRemoved(%d)\n", padapter->bDriverStopped,
-			  padapter->bSurpriseRemoved));
+			 "usb_read_port_complete:bDriverStopped(%d) OR bSurpriseRemoved(%d)\n",
+			 padapter->bDriverStopped, padapter->bSurpriseRemoved);
 
 		DBG_8723A("%s()-%d: RX Warning! bDriverStopped(%d) OR "
 			  "bSurpriseRemoved(%d) bReadPortCancel(%d)\n",
@@ -542,9 +538,7 @@ static void usb_read_port_complete(struct urb *purb)
 		if (purb->actual_length > MAX_RECVBUF_SZ ||
 		    purb->actual_length < RXDESC_SIZE) {
 			RT_TRACE(_module_hci_ops_os_c_, _drv_err_,
-				 ("usb_read_port_complete: (purb->actual_"
-				  "length > MAX_RECVBUF_SZ) || (purb->actual_"
-				  "length < RXDESC_SIZE)\n"));
+				 "usb_read_port_complete: (purb->actual_length > MAX_RECVBUF_SZ) || (purb->actual_length < RXDESC_SIZE)\n");
 			rtl8723au_read_port(padapter, 0, precvbuf);
 			DBG_8723A("%s()-%d: RX Warning!\n",
 				  __func__, __LINE__);
@@ -564,8 +558,8 @@ static void usb_read_port_complete(struct urb *purb)
 		}
 	} else {
 		RT_TRACE(_module_hci_ops_os_c_, _drv_err_,
-			 ("usb_read_port_complete : purb->status(%d) != 0 \n",
-			  purb->status));
+			 "usb_read_port_complete : purb->status(%d) != 0\n",
+			 purb->status);
 		skb_put(precvbuf->pskb, purb->actual_length);
 		precvbuf->pskb = NULL;
 
@@ -583,14 +577,12 @@ static void usb_read_port_complete(struct urb *purb)
 		case -ENODEV:
 		case -ESHUTDOWN:
 			RT_TRACE(_module_hci_ops_os_c_, _drv_err_,
-				 ("usb_read_port_complete:bSurprise"
-				  "Removed = true\n"));
+				 "usb_read_port_complete:bSurpriseRemoved = true\n");
 			/* Intentional fall through here */
 		case -ENOENT:
 			padapter->bDriverStopped = true;
 			RT_TRACE(_module_hci_ops_os_c_, _drv_err_,
-				 ("usb_read_port_complete:"
-				  "bDriverStopped = true\n"));
+				 "usb_read_port_complete:bDriverStopped = true\n");
 			break;
 		case -EPROTO:
 		case -EOVERFLOW:
@@ -620,14 +612,13 @@ int rtl8723au_read_port(struct rtw_adapter *adapter, u32 cnt,
 
 	if (adapter->bDriverStopped || adapter->bSurpriseRemoved) {
 		RT_TRACE(_module_hci_ops_os_c_, _drv_err_,
-			 ("usb_read_port:(padapter->bDriverStopped ||"
-			  "padapter->bSurpriseRemoved)!!!\n"));
+			 "usb_read_port:(padapter->bDriverStopped ||padapter->bSurpriseRemoved)!!!\n");
 		return _FAIL;
 	}
 
 	if (!precvbuf) {
 		RT_TRACE(_module_hci_ops_os_c_, _drv_err_,
-			 ("usb_read_port:precvbuf == NULL\n"));
+			 "usb_read_port:precvbuf == NULL\n");
 		return _FAIL;
 	}
 
@@ -638,7 +629,8 @@ int rtl8723au_read_port(struct rtw_adapter *adapter, u32 cnt,
 	if (!precvbuf->pskb) {
 		precvbuf->pskb = netdev_alloc_skb(adapter->pnetdev, MAX_RECVBUF_SZ + RECVBUFF_ALIGN_SZ);
 		if (precvbuf->pskb == NULL) {
-			RT_TRACE(_module_hci_ops_os_c_, _drv_err_, ("init_recvbuf(): alloc_skb fail!\n"));
+			RT_TRACE(_module_hci_ops_os_c_, _drv_err_,
+				 "init_recvbuf(): alloc_skb fail!\n");
 			return _FAIL;
 		}
 
@@ -661,8 +653,8 @@ int rtl8723au_read_port(struct rtw_adapter *adapter, u32 cnt,
 	err = usb_submit_urb(purb, GFP_ATOMIC);
 	if ((err) && (err != -EPERM)) {
 		RT_TRACE(_module_hci_ops_os_c_, _drv_err_,
-			 ("cannot submit rx in-token(err = 0x%.8x), URB_STATUS "
-			  "= 0x%.8x", err, purb->status));
+			 "cannot submit rx in-token(err = 0x%.8x), URB_STATUS = 0x%.8x\n",
+			 err, purb->status);
 		DBG_8723A("cannot submit rx in-token(err = 0x%08x), urb_status "
 			  "= %d\n", err, purb->status);
 		ret = _FAIL;
diff --git a/drivers/staging/rtl8723au/include/Hal8723APhyCfg.h b/drivers/staging/rtl8723au/include/Hal8723APhyCfg.h
index 2247d9874719..bcf36579f43a 100644
--- a/drivers/staging/rtl8723au/include/Hal8723APhyCfg.h
+++ b/drivers/staging/rtl8723au/include/Hal8723APhyCfg.h
@@ -16,9 +16,6 @@
 #ifndef __INC_HAL8723PHYCFG_H__
 #define __INC_HAL8723PHYCFG_H__
 
-/*--------------------------Define Parameters-------------------------------*/
-#define MAX_AGGR_NUM	0x0909
-
 /*------------------------------Define structure----------------------------*/
 enum RF_RADIO_PATH {
 	RF_PATH_A = 0,			/* Radio Path A */
@@ -39,15 +36,6 @@ enum WIRELESS_MODE {
 	WIRELESS_MODE_AC	= BIT(6)
 };
 
-/* BB/RF related */
-enum rf_type_8190p {
-	RF_TYPE_MIN,		/*  0 */
-	RF_8225 = 1,		/*  1 11b/g RF for verification only */
-	RF_8256 = 2,		/*  2 11b/g/n */
-	RF_8258 = 3,		/*  3 11a/b/g/n RF */
-	RF_6052 = 4,		/*  4 11b/g/n RF */
-};
-
 struct bb_reg_define {
 	u32 rfintfs;		/*  set software control: */
 				/*		0x870~0x877[8 bytes] */
diff --git a/drivers/staging/rtl8723au/include/HalVerDef.h b/drivers/staging/rtl8723au/include/HalVerDef.h
index 607b71f6e1e4..2a0e4ea7afad 100644
--- a/drivers/staging/rtl8723au/include/HalVerDef.h
+++ b/drivers/staging/rtl8723au/include/HalVerDef.h
@@ -51,30 +51,17 @@ enum hal_vendor {
 	CHIP_VENDOR_UMC		=	1,
 };
 
-enum hal_rf_type {
-	RF_TYPE_1T1R	=	0,
-	RF_TYPE_1T2R	=	1,
-	RF_TYPE_2T2R	=	2,
-	RF_TYPE_2T3R	=	3,
-	RF_TYPE_2T4R	=	4,
-	RF_TYPE_3T3R	=	5,
-	RF_TYPE_3T4R	=	6,
-	RF_TYPE_4T4R	=	7,
-};
-
 struct hal_version {
 	enum hal_ic_type	ICType;
 	enum hal_chip_type	ChipType;
 	enum hal_cut_version	CUTVersion;
 	enum hal_vendor		VendorType;
-	enum hal_rf_type	RFType;
 	u8			ROMVer;
 };
 
 /*  Get element */
 #define GET_CVID_IC_TYPE(version)	((version).ICType)
 #define GET_CVID_CHIP_TYPE(version)	((version).ChipType)
-#define GET_CVID_RF_TYPE(version)	((version).RFType)
 #define GET_CVID_MANUFACTUER(version)	((version).VendorType)
 #define GET_CVID_CUT_VERSION(version)	((version).CUTVersion)
 #define GET_CVID_ROM_VERSION(version)	(((version).ROMVer) & ROM_VERSION_MASK)
@@ -108,17 +95,8 @@ struct hal_version {
 #define IS_CHIP_VENDOR_UMC(version)		\
 	((GET_CVID_MANUFACTUER(version) == CHIP_VENDOR_UMC) ? true : false)
 
-#define IS_1T1R(version)			\
-	((GET_CVID_RF_TYPE(version) == RF_TYPE_1T1R) ? true : false)
-#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)
-
 /* Chip version Macro. -- */
 
-#define IS_92C_SERIAL(version)					\
-	((IS_81XXC(version) && IS_2T2R(version)) ? true : false)
 #define IS_81xxC_VENDOR_UMC_A_CUT(version)			\
 	(IS_81XXC(version)?(IS_CHIP_VENDOR_UMC(version) ?	\
 	(IS_A_CUT(version) ? true : false) : false) : false)
diff --git a/drivers/staging/rtl8723au/include/hal_com.h b/drivers/staging/rtl8723au/include/hal_com.h
index 7c31865e9865..9c50320b2100 100644
--- a/drivers/staging/rtl8723au/include/hal_com.h
+++ b/drivers/staging/rtl8723au/include/hal_com.h
@@ -65,36 +65,6 @@
 #define RATE_36M				BIT(9)
 #define RATE_48M				BIT(10)
 #define RATE_54M				BIT(11)
-/* MCS 1 Spatial Stream */
-#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)
-/* MCS 2 Spatial Stream */
-#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)
-
-/*  ALL CCK Rate */
-#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)
 
 /*------------------------------ Tx Desc definition Macro ------------------------*/
 /* pragma mark -- Tx Desc related definition. -- */
@@ -146,7 +116,6 @@
 #define REG_NOA_DESC_COUNT			0x05EC
 
 #include "HalVerDef.h"
-void dump_chip_info23a(struct hal_version	ChipVersion);
 
 
 u8	/* return the final channel plan decision */
diff --git a/drivers/staging/rtl8723au/include/hal_intf.h b/drivers/staging/rtl8723au/include/hal_intf.h
index 404acb52352d..b924d47fcfbc 100644
--- a/drivers/staging/rtl8723au/include/hal_intf.h
+++ b/drivers/staging/rtl8723au/include/hal_intf.h
@@ -18,13 +18,6 @@
 #include <osdep_service.h>
 #include <drv_types.h>
 
-enum RTL871X_HCI_TYPE {
-	RTW_PCIE	= BIT(0),
-	RTW_USB		= BIT(1),
-	RTW_SDIO	= BIT(2),
-	RTW_GSPI	= BIT(3),
-};
-
 enum _CHIP_TYPE {
 	NULL_CHIP_TYPE,
 	RTL8712_8188S_8191S_8192S,
diff --git a/drivers/staging/rtl8723au/include/ieee80211.h b/drivers/staging/rtl8723au/include/ieee80211.h
index cb23cd0349b4..3aa40a32555e 100644
--- a/drivers/staging/rtl8723au/include/ieee80211.h
+++ b/drivers/staging/rtl8723au/include/ieee80211.h
@@ -171,20 +171,6 @@ struct ieee80211_snap_hdr {
 #define WLAN_REASON_JOIN_WRONG_CHANNEL       65534
 #define WLAN_REASON_EXPIRATION_CHK 65535
 
-
-#define IEEE80211_STATMASK_SIGNAL (1<<0)
-#define IEEE80211_STATMASK_RSSI (1<<1)
-#define IEEE80211_STATMASK_NOISE (1<<2)
-#define IEEE80211_STATMASK_RATE (1<<3)
-#define IEEE80211_STATMASK_WEMASK 0x7
-
-
-#define IEEE80211_CCK_MODULATION    (1<<0)
-#define IEEE80211_OFDM_MODULATION   (1<<1)
-
-#define IEEE80211_24GHZ_BAND     (1<<0)
-#define IEEE80211_52GHZ_BAND     (1<<1)
-
 #define IEEE80211_CCK_RATE_LEN			4
 #define IEEE80211_NUM_OFDM_RATESLEN	8
 
@@ -265,9 +251,6 @@ join_res:
 > 0: TID
 */
 
-#define MAC_FMT "%02x:%02x:%02x:%02x:%02x:%02x"
-#define MAC_ARG(x) ((u8*)(x))[0],((u8*)(x))[1],((u8*)(x))[2],((u8*)(x))[3],((u8*)(x))[4],((u8*)(x))[5]
-
 #define MAXTID	16
 
 #define WME_OUI_TYPE 2
diff --git a/drivers/staging/rtl8723au/include/odm.h b/drivers/staging/rtl8723au/include/odm.h
index 5a0561e092ac..24f2f28c473f 100644
--- a/drivers/staging/rtl8723au/include/odm.h
+++ b/drivers/staging/rtl8723au/include/odm.h
@@ -158,27 +158,6 @@ struct false_alarm_stats {
 	u32	Cnt_BW_LSC;	/* Gary */
 };
 
-struct pri_cca {
-	u8		PriCCA_flag;
-	u8		intf_flag;
-	u8		intf_type;
-	u8		DupRTS_flag;
-	u8		Monitor_flag;
-};
-
-struct rx_hp {
-	u8		RXHP_flag;
-	u8		PSD_func_trigger;
-	u8		PSD_bitmap_RXHP[80];
-	u8		Pre_IGI;
-	u8		Cur_IGI;
-	u8		Pre_pw_th;
-	u8		Cur_pw_th;
-	bool		First_time_enter;
-	bool		RXHP_enable;
-	u8		TP_Mode;
-};
-
 #define ASSOCIATE_ENTRY_NUM					32 /*  Max size of AsocEntry[]. */
 #define	ODM_ASSOCIATE_ENTRY_NUM				ASSOCIATE_ENTRY_NUM
 
@@ -227,7 +206,6 @@ struct sw_ant_sw {
 
 struct edca_turbo {
 	bool bCurrentTurboEDCA;
-	bool bIsCurRDLState;
 	u32	prv_traffic_idx; /*  edca turbo */
 };
 
@@ -298,18 +276,14 @@ enum odm_cmninfo {
 	/*  Fixed value: */
 	/*  */
 
-	ODM_CMNINFO_PLATFORM = 0,
-	ODM_CMNINFO_INTERFACE,				/*  enum odm_interface_def */
-	ODM_CMNINFO_MP_TEST_CHIP,
-	ODM_CMNINFO_IC_TYPE,					/*  enum odm_ic_type_def */
-	ODM_CMNINFO_CUT_VER,					/*  enum odm_cut_version */
-	ODM_CMNINFO_FAB_VER,					/*  enum odm_fab_version */
-	ODM_CMNINFO_RF_TYPE,					/*  enum rf_path_def or enum odm_rf_type? */
-	ODM_CMNINFO_BOARD_TYPE,				/*  enum odm_board_type */
-	ODM_CMNINFO_EXT_LNA,					/*  true */
+	ODM_CMNINFO_MP_TEST_CHIP = 2,
+	ODM_CMNINFO_IC_TYPE,			/*  enum odm_ic_type_def */
+	ODM_CMNINFO_CUT_VER,			/*  enum odm_cut_version */
+	ODM_CMNINFO_FAB_VER,			/*  enum odm_fab_version */
+	ODM_CMNINFO_BOARD_TYPE,			/*  enum odm_board_type */
+	ODM_CMNINFO_EXT_LNA,			/*  true */
 	ODM_CMNINFO_EXT_PA,
 	ODM_CMNINFO_EXT_TRSW,
-	ODM_CMNINFO_PATCH_ID,				/* CUSTOMER ID */
 	ODM_CMNINFO_BINHCT_TEST,
 	ODM_CMNINFO_BWIFI_TEST,
 	ODM_CMNINFO_SMART_CONCURRENT,
@@ -348,29 +322,7 @@ enum odm_cmninfo {
 /*  Define ODM support ability.  ODM_CMNINFO_ABILITY */
 enum {
 	/*  BB ODM section BIT 0-15 */
-	ODM_BB_DIG				= BIT(0),
-	ODM_BB_RA_MASK				= BIT(1),
-	ODM_BB_DYNAMIC_TXPWR			= BIT(2),
-	ODM_BB_FA_CNT				= BIT(3),
-	ODM_BB_RSSI_MONITOR			= BIT(4),
-	ODM_BB_CCK_PD				= BIT(5),
 	ODM_BB_ANT_DIV				= BIT(6),
-	ODM_BB_PWR_SAVE				= BIT(7),
-	ODM_BB_PWR_TRAIN			= BIT(8),
-	ODM_BB_RATE_ADAPTIVE			= BIT(9),
-	ODM_BB_PATH_DIV				= BIT(10),
-	ODM_BB_PSD				= BIT(11),
-	ODM_BB_RXHP				= BIT(12),
-
-	/*  MAC DM section BIT 16-23 */
-	ODM_MAC_EDCA_TURBO			= BIT(16),
-	ODM_MAC_EARLY_MODE			= BIT(17),
-
-	/*  RF ODM section BIT 24-31 */
-	ODM_RF_TX_PWR_TRACK			= BIT(24),
-	ODM_RF_RX_GAIN_TRACK			= BIT(25),
-	ODM_RF_CALIBRATION			= BIT(26),
-
 };
 
 /*	ODM_CMNINFO_INTERFACE */
@@ -409,7 +361,6 @@ enum odm_fab_version {
 	ODM_UMC		=	1,
 };
 
-/*  ODM_CMNINFO_RF_TYPE */
 /*  For example 1T2R (A+AB = BIT0|BIT4|BIT5) */
 enum rf_path_def {
 	ODM_RF_TX_A	=	BIT(0),
@@ -422,18 +373,6 @@ enum rf_path_def {
 	ODM_RF_RX_D	=	BIT(7),
 };
 
-
-enum odm_rf_type {
-	ODM_1T1R	=	0,
-	ODM_1T2R	=	1,
-	ODM_2T2R	=	2,
-	ODM_2T3R	=	3,
-	ODM_2T4R	=	4,
-	ODM_3T3R	=	5,
-	ODM_3T4R	=	6,
-	ODM_4T4R	=	7,
-};
-
 /*  ODM Dynamic common info value definition */
 
 enum odm_mac_phy_mode {
@@ -587,33 +526,6 @@ struct odm_rf_cal_t {
 	u8	bDPPathBOK;
 };
 
-/*  ODM Dynamic common info value definition */
-struct odm_fat_t {
-	u8	Bssid[6];
-	u8	antsel_rx_keep_0;
-	u8	antsel_rx_keep_1;
-	u8	antsel_rx_keep_2;
-	u32	antSumRSSI[7];
-	u32	antRSSIcnt[7];
-	u32	antAveRSSI[7];
-	u8	FAT_State;
-	u32	TrainIdx;
-	u8	antsel_a[ODM_ASSOCIATE_ENTRY_NUM];
-	u8	antsel_b[ODM_ASSOCIATE_ENTRY_NUM];
-	u8	antsel_c[ODM_ASSOCIATE_ENTRY_NUM];
-	u32	MainAnt_Sum[ODM_ASSOCIATE_ENTRY_NUM];
-	u32	AuxAnt_Sum[ODM_ASSOCIATE_ENTRY_NUM];
-	u32	MainAnt_Cnt[ODM_ASSOCIATE_ENTRY_NUM];
-	u32	AuxAnt_Cnt[ODM_ASSOCIATE_ENTRY_NUM];
-	u8	RxIdleAnt;
-	bool	bBecomeLinked;
-};
-
-enum fat_state {
-	FAT_NORMAL_STATE		= 0,
-	FAT_TRAINING_STATE		= 1,
-};
-
 enum ant_dif_type {
 	NO_ANTDIV			= 0xFF,
 	CG_TRX_HW_ANTDIV		= 0x01,
@@ -636,7 +548,6 @@ struct dm_odm_t {
 /*  ODM HANDLE, DRIVER NEEDS NOT TO HOOK------ */
 	bool			bCckHighPower;
 	u8			RFPathRxEnable;		/*  ODM_CMNINFO_RFPATH_ENABLE */
-	u8			ControlChannel;
 /*  ODM HANDLE, DRIVER NEEDS NOT TO HOOK------ */
 
 /* 1  COMMON INFORMATION */
@@ -645,16 +556,12 @@ struct dm_odm_t {
 /* HOOK BEFORE REG INIT----------- */
 	/*  ODM Support Ability DIG/RATR/TX_PWR_TRACK/ ¡K¡K = 1/2/3/¡K */
 	u32			SupportAbility;
-	/*  ODM PCIE/USB/SDIO/GSPI = 0/1/2/3 */
-	u8			SupportInterface;
 	/*  ODM composite or independent. Bit oriented/ 92C+92D+ .... or any other type = 1/2/3/... */
 	u32			SupportICType;
 	/*  Cut Version TestChip/A-cut/B-cut... = 0/1/2/3/... */
 	u8			CutVersion;
 	/*  Fab Version TSMC/UMC = 0/1 */
 	u8			FabVersion;
-	/*  RF Type 4T4R/3T3R/2T2R/1T2R/1T1R/... */
-	u8			RFType;
 	/*  Board Type Normal/HighPower/MiniCard/SLIM/Combo/... = 0/1/2/3/4/... */
 	u8			BoardType;
 	/*  with external LNA  NO/Yes = 0/1 */
@@ -663,7 +570,6 @@ struct dm_odm_t {
 	u8			ExtPA;
 	/*  with external TRSW  NO/Yes = 0/1 */
 	u8			ExtTRSW;
-	u8			PatchID; /* Customer ID */
 	bool			bInHctTest;
 	bool			bWIFITest;
 
@@ -703,14 +609,6 @@ struct dm_odm_t {
 	/*  2012/01/12 MH For MP, we need to reduce one array pointer for default port.?? */
 	struct sta_info *		pODM_StaInfo[ODM_ASSOCIATE_ENTRY_NUM];
 
-	/*  */
-	/*  2012/02/14 MH Add to share 88E ra with other SW team. */
-	/*  We need to colelct all support abilit to a proper area. */
-	/*  */
-	bool				RaSupport88E;
-
-	/*  Define ........... */
-
 	/*  Latest packet phy info (ODM write) */
 	struct odm_phy_dbg_info	 PhyDbgInfo;
 	/* PHY_INFO_88E		PhyInfo; */
@@ -728,11 +626,8 @@ struct dm_odm_t {
 	/*  */
 	/* ODM Structure */
 	/*  */
-	struct odm_fat_t		DM_FatTable;
 	struct dig_t	DM_DigTable;
 	struct dynamic_pwr_sav		DM_PSTable;
-	struct pri_cca	DM_PriCCA;
-	struct rx_hp		DM_RXHP_Table;
 	struct false_alarm_stats	FalseAlmCnt;
 	struct false_alarm_stats	FlaseAlmCntBuddyAdapter;
 	struct sw_ant_sw		DM_SWAT_Table;
@@ -745,32 +640,11 @@ struct dm_odm_t {
 	/*  */
 
 	/* PSD */
-	bool			bUserAssignLevel;
-	u8			RSSI_BT;			/* come from BT */
-	bool			bPSDinProcess;
-
-	/* for rate adaptive, in fact,  88c/92c fw will handle this */
-	u8			bUseRAMask;
-
+	u8			RSSI_BT;		/* come from BT */
 	struct odm_rate_adapt	RateAdaptive;
 
 
 	struct odm_rf_cal_t	RFCalibrateInfo;
-
-	/*  */
-	/*  TX power tracking */
-	/*  */
-	u8			BbSwingIdxOfdm;
-	u8			BbSwingIdxOfdmCurrent;
-	u8			BbSwingIdxOfdmBase;
-	bool			BbSwingFlagOfdm;
-	u8			BbSwingIdxCck;
-	u8			BbSwingIdxCckCurrent;
-	u8			BbSwingIdxCckBase;
-	bool			BbSwingFlagCck;
-	/*  */
-	/*  ODM system resource. */
-	/*  */
 };	/*  DM_Dynamic_Mechanism_Structure */
 
 enum odm_rf_content {
@@ -983,6 +857,4 @@ void ODM_SingleDualAntennaDefaultSetting(struct dm_odm_t *pDM_Odm);
 
 bool ODM_SingleDualAntennaDetection(struct dm_odm_t *pDM_Odm, u8 mode);
 
-void odm_dtc(struct dm_odm_t *pDM_Odm);
-
 #endif
diff --git a/drivers/staging/rtl8723au/include/odm_RegConfig8723A.h b/drivers/staging/rtl8723au/include/odm_RegConfig8723A.h
index a6cfb6df4cf7..f2a54d829ed5 100644
--- a/drivers/staging/rtl8723au/include/odm_RegConfig8723A.h
+++ b/drivers/staging/rtl8723au/include/odm_RegConfig8723A.h
@@ -20,9 +20,8 @@ void odm_ConfigRFReg_8723A(struct dm_odm_t *pDM_Odm, u32 Addr, u32 Data,
 
 void odm_ConfigMAC_8723A(struct dm_odm_t *pDM_Odm, u32 Addr, u8 Data);
 
-void odm_ConfigBB_AGC_8723A(struct dm_odm_t *pDM_Odm, u32 Addr,
-			    u32 Bitmask, u32 Data);
+void odm_ConfigBB_AGC_8723A(struct dm_odm_t *pDM_Odm, u32 addr, u32 data);
 
-void odm_ConfigBB_PHY_8723A(struct dm_odm_t *pDM_Odm, u32 Addr, u32 Bitmask, u32 Data);
+void odm_ConfigBB_PHY_8723A(struct dm_odm_t *pDM_Odm, u32 addr, u32 data);
 
 #endif /*  end of SUPPORT */
diff --git a/drivers/staging/rtl8723au/include/odm_interface.h b/drivers/staging/rtl8723au/include/odm_interface.h
index ea35070b744f..1d3bf03b59ea 100644
--- a/drivers/staging/rtl8723au/include/odm_interface.h
+++ b/drivers/staging/rtl8723au/include/odm_interface.h
@@ -54,17 +54,6 @@ typedef void (*RT_WORKITEM_CALL_BACK)(struct work_struct *pContext);
 /*  =========== EXtern Function Prototype */
 /*  */
 
-
-u8 ODM_Read1Byte(struct dm_odm_t *pDM_Odm, u32 RegAddr);
-u16 ODM_Read2Byte(struct dm_odm_t *pDM_Odm, u32 RegAddr);
-u32 ODM_Read4Byte(struct dm_odm_t *pDM_Odm, u32 RegAddr);
-void ODM_Write1Byte(struct dm_odm_t *pDM_Odm, u32 RegAddr, u8 Data);
-void ODM_Write2Byte(struct dm_odm_t *pDM_Odm, u32 RegAddr, u16 Data);
-void ODM_Write4Byte(struct dm_odm_t *pDM_Odm, u32 RegAddr, u32 Data);
-void ODM_SetMACReg(struct dm_odm_t *pDM_Odm, u32 RegAddr, u32 BitMask, u32 Data);
-u32 ODM_GetMACReg(struct dm_odm_t *pDM_Odm, u32 RegAddr, u32 BitMask);
-void ODM_SetBBReg(struct dm_odm_t *pDM_Odm, u32 RegAddr, u32 BitMask, u32 Data);
-u32 ODM_GetBBReg(struct dm_odm_t *pDM_Odm, u32 RegAddr, u32 BitMask);
 void ODM_SetRFReg(struct dm_odm_t *pDM_Odm, enum RF_RADIO_PATH eRFPath,
 		  u32 RegAddr, u32 BitMask, u32 Data);
 u32 ODM_GetRFReg(struct dm_odm_t *pDM_Odm, enum RF_RADIO_PATH eRFPath,
diff --git a/drivers/staging/rtl8723au/include/rtl8723a_hal.h b/drivers/staging/rtl8723au/include/rtl8723a_hal.h
index e14633678b52..ad3a442bc000 100644
--- a/drivers/staging/rtl8723au/include/rtl8723a_hal.h
+++ b/drivers/staging/rtl8723au/include/rtl8723a_hal.h
@@ -270,7 +270,6 @@ struct hal_data_8723a {
 	u16	BasicRateSet;
 
 	/* rf_ctrl */
-	u8	rf_chip;
 	u8	rf_type;
 	u8	NumTotalRFPath;
 
@@ -348,7 +347,6 @@ struct hal_data_8723a {
 	/* for host message to fw */
 	u8	LastHMEBoxNum;
 
-	u8	fw_ractrl;
 	u8	RegTxPause;
 	/*  Beacon function related global variable. */
 	u8	RegFwHwTxQCtrl;
diff --git a/drivers/staging/rtl8723au/include/rtw_debug.h b/drivers/staging/rtl8723au/include/rtw_debug.h
index b6b01732a725..159183e9cab0 100644
--- a/drivers/staging/rtl8723au/include/rtw_debug.h
+++ b/drivers/staging/rtl8723au/include/rtw_debug.h
@@ -163,14 +163,13 @@
 
 extern u32 GlobalDebugLevel23A;
 
+__printf(3, 4)
+void rt_trace(int comp, int level, const char *fmt, ...);
 
-#define RT_TRACE(_Comp, _Level, Fmt)					\
+#define RT_TRACE(_Comp, _Level, Fmt, ...)				\
 do {									\
-	if (_Level <= GlobalDebugLevel23A) {				\
-		pr_info("%s [0x%08x,%d]", DRIVER_PREFIX,		\
-			 (unsigned int)_Comp, _Level);			\
-		pr_info Fmt;						\
-	}								\
+	if (_Level <= GlobalDebugLevel23A)				\
+		rt_trace(_Comp, _Level, Fmt, ##__VA_ARGS__);		\
 } while (0)
 
 #define RT_PRINT_DATA(_Comp, _Level, _TitleString, _HexData,		\
diff --git a/drivers/staging/rtl8723au/os_dep/ioctl_cfg80211.c b/drivers/staging/rtl8723au/os_dep/ioctl_cfg80211.c
index 537bd8214efe..b139ed40cb27 100644
--- a/drivers/staging/rtl8723au/os_dep/ioctl_cfg80211.c
+++ b/drivers/staging/rtl8723au/os_dep/ioctl_cfg80211.c
@@ -729,7 +729,7 @@ static int rtw_cfg80211_set_encryption(struct net_device *dev, u8 key_index,
 	if (keyparms->cipher == WLAN_CIPHER_SUITE_WEP40 ||
 	    keyparms->cipher == WLAN_CIPHER_SUITE_WEP104) {
 		RT_TRACE(_module_rtl871x_ioctl_os_c, _drv_err_,
-			 ("wpa_set_encryption, crypt.alg = WEP\n"));
+			 "wpa_set_encryption, crypt.alg = WEP\n");
 		DBG_8723A("wpa_set_encryption, crypt.alg = WEP\n");
 
 		if (psecuritypriv->bWepDefaultKeyIdxSet == 0) {
@@ -1076,8 +1076,7 @@ static int cfg80211_rtw_get_station(struct wiphy *wiphy,
 		ret = -ENOENT;
 		goto exit;
 	}
-	DBG_8723A("%s(%s): mac =" MAC_FMT "\n", __func__, ndev->name,
-		  MAC_ARG(mac));
+	DBG_8723A("%s(%s): mac=%pM\n", __func__, ndev->name, mac);
 
 	/* for infra./P2PClient mode */
 	if (check_fwstate(pmlmepriv, WIFI_STATION_STATE) &&
@@ -1085,8 +1084,8 @@ static int cfg80211_rtw_get_station(struct wiphy *wiphy,
 		struct wlan_network *cur_network = &pmlmepriv->cur_network;
 
 		if (!ether_addr_equal(mac, cur_network->network.MacAddress)) {
-			DBG_8723A("%s, mismatch bssid =" MAC_FMT "\n", __func__,
-				  MAC_ARG(cur_network->network.MacAddress));
+			DBG_8723A("%s, mismatch bssid=%pM\n",
+				  __func__, cur_network->network.MacAddress);
 			ret = -ENOENT;
 			goto exit;
 		}
@@ -1128,14 +1127,14 @@ static int cfg80211_infrastructure_mode(struct rtw_adapter *padapter,
 	old_mode = cur_network->network.ifmode;
 
 	RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_notice_,
-		 ("+%s: old =%d new =%d fw_state = 0x%08x\n", __func__,
-		  old_mode, ifmode, get_fwstate(pmlmepriv)));
+		 "+%s: old =%d new =%d fw_state = 0x%08x\n", __func__,
+		 old_mode, ifmode, get_fwstate(pmlmepriv));
 
 	if (old_mode != ifmode) {
 		spin_lock_bh(&pmlmepriv->lock);
 
 		RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_,
-			 (" change mode!"));
+			 "change mode!\n");
 
 		if (old_mode == NL80211_IFTYPE_AP ||
 		    old_mode == NL80211_IFTYPE_P2P_GO) {
@@ -1195,10 +1194,6 @@ static int cfg80211_infrastructure_mode(struct rtw_adapter *padapter,
 
 		/* SecClearAllKeys(adapter); */
 
-		/* RT_TRACE(COMP_OID_SET, DBG_LOUD,
-		   ("set_infrastructure: fw_state:%x after changing mode\n", */
-		/* get_fwstate(pmlmepriv))); */
-
 		spin_unlock_bh(&pmlmepriv->lock);
 	}
 
@@ -1603,7 +1598,7 @@ static int rtw_cfg80211_set_wpa_ie(struct rtw_adapter *padapter, const u8 *pie,
 			  pie[i + 4], pie[i + 5], pie[i + 6], pie[i + 7]);
 	if (ielen < RSN_HEADER_LEN) {
 		RT_TRACE(_module_rtl871x_ioctl_os_c, _drv_err_,
-			 ("Ie len too short %d\n", (int)ielen));
+			 "Ie len too short %d\n", (int)ielen);
 		ret = -1;
 		goto exit;
 	}
@@ -1726,11 +1721,10 @@ static int rtw_cfg80211_set_wpa_ie(struct rtw_adapter *padapter, const u8 *pie,
 		rtl8723a_off_rcr_am(padapter);
 
 	RT_TRACE(_module_rtl871x_ioctl_os_c, _drv_info_,
-		 ("rtw_set_wpa_ie: pairwise_cipher = 0x%08x padapter->"
-		  "securitypriv.ndisencryptstatus =%d padapter->"
-		  "securitypriv.ndisauthtype =%d\n", pairwise_cipher,
-		  padapter->securitypriv.ndisencryptstatus,
-		  padapter->securitypriv.ndisauthtype));
+		 "rtw_set_wpa_ie: pairwise_cipher = 0x%08x padapter->securitypriv.ndisencryptstatus =%d padapter->securitypriv.ndisauthtype =%d\n",
+		 pairwise_cipher,
+		 padapter->securitypriv.ndisencryptstatus,
+		 padapter->securitypriv.ndisauthtype);
 
 exit:
 	if (ret)
@@ -1746,7 +1740,7 @@ static int rtw_cfg80211_add_wep(struct rtw_adapter *padapter,
 
 	if (keyid >= NUM_WEP_KEYS) {
 		RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_,
-			 ("%s:keyid>4 =>fail\n", __func__));
+			 "%s:keyid>4 =>fail\n", __func__);
 		res = _FAIL;
 		goto exit;
 	}
@@ -1755,45 +1749,45 @@ static int rtw_cfg80211_add_wep(struct rtw_adapter *padapter,
 	case WLAN_KEY_LEN_WEP40:
 		psecuritypriv->dot11PrivacyAlgrthm = WLAN_CIPHER_SUITE_WEP40;
 		RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_,
-			 ("%s:wep->KeyLength = 5\n", __func__));
+			 "%s:wep->KeyLength = 5\n", __func__);
 		break;
 	case WLAN_KEY_LEN_WEP104:
 		psecuritypriv->dot11PrivacyAlgrthm = WLAN_CIPHER_SUITE_WEP104;
 		RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_,
-			 ("%s:wep->KeyLength = 13\n", __func__));
+			 "%s:wep->KeyLength = 13\n", __func__);
 		break;
 	default:
 		psecuritypriv->dot11PrivacyAlgrthm = 0;
 		RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_,
-			 ("%s:wep->KeyLength!= 5 or 13\n", __func__));
+			 "%s:wep->KeyLength!= 5 or 13\n", __func__);
 		res = _FAIL;
 		goto exit;
 	}
 
 	RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_,
-		 ("%s:before memcpy, wep->KeyLength = 0x%x keyid =%x\n",
-		  __func__, wep->keylen, keyid));
+		 "%s:before memcpy, wep->KeyLength = 0x%x keyid =%x\n",
+		 __func__, wep->keylen, keyid);
 
 	memcpy(&psecuritypriv->wep_key[keyid], wep, sizeof(struct rtw_wep_key));
 
 	psecuritypriv->dot11PrivacyKeyIndex = keyid;
 
 	RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_,
-		 ("%s:security key material : "
-		  "%x %x %x %x %x %x %x %x %x %x %x %x %x\n", __func__,
-		  psecuritypriv->wep_key[keyid].key[0],
-		  psecuritypriv->wep_key[keyid].key[1],
-		  psecuritypriv->wep_key[keyid].key[2],
-		  psecuritypriv->wep_key[keyid].key[3],
-		  psecuritypriv->wep_key[keyid].key[4],
-		  psecuritypriv->wep_key[keyid].key[5],
-		  psecuritypriv->wep_key[keyid].key[6],
-		  psecuritypriv->wep_key[keyid].key[7],
-		  psecuritypriv->wep_key[keyid].key[8],
-		  psecuritypriv->wep_key[keyid].key[9],
-		  psecuritypriv->wep_key[keyid].key[10],
-		  psecuritypriv->wep_key[keyid].key[11],
-		  psecuritypriv->wep_key[keyid].key[12]));
+		 "%s:security key material : %x %x %x %x %x %x %x %x %x %x %x %x %x\n",
+		 __func__,
+		 psecuritypriv->wep_key[keyid].key[0],
+		 psecuritypriv->wep_key[keyid].key[1],
+		 psecuritypriv->wep_key[keyid].key[2],
+		 psecuritypriv->wep_key[keyid].key[3],
+		 psecuritypriv->wep_key[keyid].key[4],
+		 psecuritypriv->wep_key[keyid].key[5],
+		 psecuritypriv->wep_key[keyid].key[6],
+		 psecuritypriv->wep_key[keyid].key[7],
+		 psecuritypriv->wep_key[keyid].key[8],
+		 psecuritypriv->wep_key[keyid].key[9],
+		 psecuritypriv->wep_key[keyid].key[10],
+		 psecuritypriv->wep_key[keyid].key[11],
+		 psecuritypriv->wep_key[keyid].key[12]);
 
 	res = rtw_set_key23a(padapter, psecuritypriv, keyid, 1);
 
@@ -1815,7 +1809,7 @@ static int rtw_set_ssid(struct rtw_adapter *padapter,
 
 	if (padapter->hw_init_completed == false) {
 		RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_,
-			 ("set_ssid: hw_init_completed == false =>exit!!!\n"));
+			 "set_ssid: hw_init_completed == false =>exit!!!\n");
 		status = _FAIL;
 		goto exit;
 	}
@@ -1828,7 +1822,7 @@ static int rtw_set_ssid(struct rtw_adapter *padapter,
 
 	if (check_fwstate(pmlmepriv, _FW_LINKED|WIFI_ADHOC_MASTER_STATE)) {
 		RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_,
-			 ("set_ssid: _FW_LINKED||WIFI_ADHOC_MASTER_STATE\n"));
+			 "set_ssid: _FW_LINKED||WIFI_ADHOC_MASTER_STATE\n");
 
 		if (pmlmepriv->assoc_ssid.ssid_len ==
 		    newnetwork->network.Ssid.ssid_len &&
@@ -1837,9 +1831,9 @@ static int rtw_set_ssid(struct rtw_adapter *padapter,
 			    newnetwork->network.Ssid.ssid_len)) {
 			if (!check_fwstate(pmlmepriv, WIFI_STATION_STATE)) {
 				RT_TRACE(_module_rtl871x_ioctl_set_c_,
-					 _drv_err_, ("New SSID is same SSID, "
-						     "fw_state = 0x%08x\n",
-						     get_fwstate(pmlmepriv)));
+					 _drv_err_,
+					 "New SSID is same SSID, fw_state = 0x%08x\n",
+					 get_fwstate(pmlmepriv));
 
 				if (rtw_is_same_ibss23a(padapter, pnetwork)) {
 					/*
@@ -1875,15 +1869,15 @@ static int rtw_set_ssid(struct rtw_adapter *padapter,
 			}
 		} else {
 			RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_,
-				 ("Set SSID not the same ssid\n"));
+				 "Set SSID not the same ssid\n");
 			RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_,
-				 ("set_ssid =[%s] len = 0x%x\n",
-				  newnetwork->network.Ssid.ssid,
-				  newnetwork->network.Ssid.ssid_len));
+				 "set_ssid =[%s] len = 0x%x\n",
+				 newnetwork->network.Ssid.ssid,
+				 newnetwork->network.Ssid.ssid_len);
 			RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_,
-				 ("assoc_ssid =[%s] len = 0x%x\n",
-				  pmlmepriv->assoc_ssid.ssid,
-				  pmlmepriv->assoc_ssid.ssid_len));
+				 "assoc_ssid =[%s] len = 0x%x\n",
+				 pmlmepriv->assoc_ssid.ssid,
+				 pmlmepriv->assoc_ssid.ssid_len);
 
 			rtw_disassoc_cmd23a(padapter, 0, true);
 
@@ -1947,7 +1941,7 @@ release_mlme_lock:
 
 exit:
 	RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_,
-		 ("-%s: status =%d\n", __func__, status));
+		 "-%s: status =%d\n", __func__, status);
 
 	return status;
 }
@@ -1989,7 +1983,7 @@ static int cfg80211_rtw_connect(struct wiphy *wiphy, struct net_device *ndev,
 	DBG_8723A("ssid =%s, len =%zu\n", sme->ssid, sme->ssid_len);
 
 	if (sme->bssid)
-		DBG_8723A("bssid =" MAC_FMT "\n", MAC_ARG(sme->bssid));
+		DBG_8723A("bssid=%pM\n", sme->bssid);
 
 	if (check_fwstate(pmlmepriv, _FW_UNDER_LINKING)) {
 		ret = -EBUSY;
@@ -2523,8 +2517,8 @@ static int rtw_cfg80211_monitor_if_xmit_entry(struct sk_buff *skb,
 
 		mgmt = (struct ieee80211_mgmt *)dot11_hdr;
 
-		DBG_8723A("RTW_Tx:da =" MAC_FMT " via %s(%s)\n",
-			  MAC_ARG(mgmt->da), __func__, ndev->name);
+		DBG_8723A("RTW_Tx:da=%pM via %s(%s)\n",
+			  mgmt->da, __func__, ndev->name);
 		category = mgmt->u.action.category;
 		action = mgmt->u.action.u.wme_action.action_code;
 		DBG_8723A("RTW_Tx:category(%u), action(%u)\n",
@@ -2877,7 +2871,7 @@ static int cfg80211_rtw_del_station(struct wiphy *wiphy,
 		return ret;
 	}
 
-	DBG_8723A("free sta macaddr =" MAC_FMT "\n", MAC_ARG(mac));
+	DBG_8723A("free sta macaddr=%pM\n", mac);
 
 	if (is_broadcast_ether_addr(mac))
 		return -EINVAL;
@@ -3053,8 +3047,7 @@ static int cfg80211_rtw_mgmt_tx(struct wiphy *wiphy, struct wireless_dev *wdev,
 	cfg80211_mgmt_tx_status(padapter->rtw_wdev, *cookie, buf, len, ack,
 				GFP_KERNEL);
 
-	DBG_8723A("RTW_Tx:tx_ch =%d, da =" MAC_FMT "\n", tx_ch,
-		  MAC_ARG(hdr->da));
+	DBG_8723A("RTW_Tx:tx_ch =%d, da =%pM\n", tx_ch, hdr->da);
 	category = hdr->u.action.category;
 	action = hdr->u.action.u.wme_action.action_code;
 	DBG_8723A("RTW_Tx:category(%u), action(%u)\n", category, action);
diff --git a/drivers/staging/rtl8723au/os_dep/os_intfs.c b/drivers/staging/rtl8723au/os_dep/os_intfs.c
index 1b23eb13222b..83696360c293 100644
--- a/drivers/staging/rtl8723au/os_dep/os_intfs.c
+++ b/drivers/staging/rtl8723au/os_dep/os_intfs.c
@@ -34,7 +34,7 @@ MODULE_FIRMWARE("rtlwifi/rtl8723aufw_B.bin");
 MODULE_FIRMWARE("rtlwifi/rtl8723aufw_B_NoBT.bin");
 
 /* module param defaults */
-static int rtw_chip_version = 0x00;
+static int rtw_chip_version;
 static int rtw_rfintfs = HWPI;
 static int rtw_debug = 1;
 
@@ -342,7 +342,7 @@ int rtw_init_netdev23a_name23a(struct net_device *pnetdev, const char *ifname)
 {
 	if (dev_alloc_name(pnetdev, ifname) < 0) {
 		RT_TRACE(_module_os_intfs_c_, _drv_err_,
-			 ("dev_alloc_name, fail!\n"));
+			 "dev_alloc_name, fail!\n");
 	}
 	netif_carrier_off(pnetdev);
 	return 0;
@@ -357,7 +357,7 @@ struct net_device *rtw_init_netdev23a(struct rtw_adapter *old_padapter)
 	struct rtw_adapter *padapter;
 	struct net_device *pnetdev;
 
-	RT_TRACE(_module_os_intfs_c_, _drv_info_, ("+init_net_dev\n"));
+	RT_TRACE(_module_os_intfs_c_, _drv_info_, "+init_net_dev\n");
 
 	pnetdev = alloc_etherdev_mq(sizeof(struct rtw_adapter), 4);
 	if (!pnetdev)
@@ -456,11 +456,11 @@ int rtw_init_drv_sw23a(struct rtw_adapter *padapter)
 {
 	int ret8 = _SUCCESS;
 
-	RT_TRACE(_module_os_intfs_c_, _drv_info_, ("+rtw_init_drv_sw23a\n"));
+	RT_TRACE(_module_os_intfs_c_, _drv_info_, "+rtw_init_drv_sw23a\n");
 
 	if (rtw_init_cmd_priv23a(&padapter->cmdpriv) == _FAIL) {
 		RT_TRACE(_module_os_intfs_c_, _drv_err_,
-			 ("\n Can't init cmd_priv\n"));
+			 "Can't init cmd_priv\n");
 		ret8 = _FAIL;
 		goto exit;
 	}
@@ -469,14 +469,14 @@ int rtw_init_drv_sw23a(struct rtw_adapter *padapter)
 
 	if (rtw_init_evt_priv23a(&padapter->evtpriv) == _FAIL) {
 		RT_TRACE(_module_os_intfs_c_, _drv_err_,
-			 ("\n Can't init evt_priv\n"));
+			 "Can't init evt_priv\n");
 		ret8 = _FAIL;
 		goto exit;
 	}
 
 	if (rtw_init_mlme_priv23a(padapter) == _FAIL) {
 		RT_TRACE(_module_os_intfs_c_, _drv_err_,
-			 ("\n Can't init mlme_priv\n"));
+			 "Can't init mlme_priv\n");
 		ret8 = _FAIL;
 		goto exit;
 	}
@@ -484,7 +484,7 @@ int rtw_init_drv_sw23a(struct rtw_adapter *padapter)
 
 	if (init_mlme_ext_priv23a(padapter) == _FAIL) {
 		RT_TRACE(_module_os_intfs_c_, _drv_err_,
-			 ("\n Can't init mlme_ext_priv\n"));
+			 "Can't init mlme_ext_priv\n");
 		ret8 = _FAIL;
 		goto exit;
 	}
@@ -521,40 +521,40 @@ int rtw_init_drv_sw23a(struct rtw_adapter *padapter)
 
 exit:
 
-	RT_TRACE(_module_os_intfs_c_, _drv_info_, ("-rtw_init_drv_sw23a\n"));
+	RT_TRACE(_module_os_intfs_c_, _drv_info_, "-rtw_init_drv_sw23a\n");
 	return ret8;
 }
 
 void rtw_cancel_all_timer23a(struct rtw_adapter *padapter)
 {
 	RT_TRACE(_module_os_intfs_c_, _drv_info_,
-		 ("+rtw_cancel_all_timer23a\n"));
+		 "+rtw_cancel_all_timer23a\n");
 
 	del_timer_sync(&padapter->mlmepriv.assoc_timer);
 	RT_TRACE(_module_os_intfs_c_, _drv_info_,
-		 ("%s:cancel association timer complete!\n", __func__));
+		 "%s:cancel association timer complete!\n", __func__);
 
 	del_timer_sync(&padapter->mlmepriv.scan_to_timer);
 	RT_TRACE(_module_os_intfs_c_, _drv_info_,
-		 ("%s:cancel scan_to_timer!\n", __func__));
+		 "%s:cancel scan_to_timer!\n", __func__);
 
 	del_timer_sync(&padapter->mlmepriv.dynamic_chk_timer);
 	RT_TRACE(_module_os_intfs_c_, _drv_info_,
-		 ("%s:cancel dynamic_chk_timer!\n", __func__));
+		 "%s:cancel dynamic_chk_timer!\n", __func__);
 
 	del_timer_sync(&padapter->pwrctrlpriv.pwr_state_check_timer);
 
 	del_timer_sync(&padapter->mlmepriv.set_scan_deny_timer);
 	rtw_clear_scan_deny(padapter);
 	RT_TRACE(_module_os_intfs_c_, _drv_info_,
-		 ("%s:cancel set_scan_deny_timer!\n", __func__));
+		 "%s:cancel set_scan_deny_timer!\n", __func__);
 
 	del_timer_sync(&padapter->recvpriv.signal_stat_timer);
 }
 
 int rtw_free_drv_sw23a(struct rtw_adapter *padapter)
 {
-	RT_TRACE(_module_os_intfs_c_, _drv_info_, ("==>rtw_free_drv_sw23a"));
+	RT_TRACE(_module_os_intfs_c_, _drv_info_, "==>rtw_free_drv_sw23a\n");
 
 	free_mlme_ext_priv23a(&padapter->mlmeextpriv);
 
@@ -574,7 +574,7 @@ int rtw_free_drv_sw23a(struct rtw_adapter *padapter)
 	kfree(padapter->HalData);
 	padapter->HalData = NULL;
 
-	RT_TRACE(_module_os_intfs_c_, _drv_info_, ("-rtw_free_drv_sw23a\n"));
+	RT_TRACE(_module_os_intfs_c_, _drv_info_, "-rtw_free_drv_sw23a\n");
 	return _SUCCESS;
 }
 
@@ -594,8 +594,8 @@ static int _rtw_drv_register_netdev(struct rtw_adapter *padapter, char *name)
 		ret = _FAIL;
 		goto error_register_netdev;
 	}
-	DBG_8723A("%s, MAC Address (if%d) = " MAC_FMT "\n", __func__,
-		  (padapter->iface_id + 1), MAC_ARG(pnetdev->dev_addr));
+	DBG_8723A("%s, MAC Address (if%d) = %pM\n",
+		  __func__, padapter->iface_id + 1, pnetdev->dev_addr);
 	return ret;
 
 error_register_netdev:
@@ -647,7 +647,7 @@ int netdev_open23a(struct net_device *pnetdev)
 	int ret = 0;
 	int status;
 
-	RT_TRACE(_module_os_intfs_c_, _drv_info_, ("+871x_drv - dev_open\n"));
+	RT_TRACE(_module_os_intfs_c_, _drv_info_, "+871x_drv - dev_open\n");
 	DBG_8723A("+871x_drv - drv_open, bup =%d\n", padapter->bup);
 
 	mutex_lock(&adapter_to_dvobj(padapter)->hw_init_mutex);
@@ -662,12 +662,11 @@ int netdev_open23a(struct net_device *pnetdev)
 		status = rtl8723au_hal_init(padapter);
 		if (status == _FAIL) {
 			RT_TRACE(_module_os_intfs_c_, _drv_err_,
-				 ("rtl871x_hal_init(): Can't init h/w!\n"));
+				 "rtl871x_hal_init(): Can't init h/w!\n");
 			goto netdev_open23a_error;
 		}
 
-		DBG_8723A("MAC Address = "MAC_FMT"\n",
-			  MAC_ARG(pnetdev->dev_addr));
+		DBG_8723A("MAC Address = %pM\n", pnetdev->dev_addr);
 
 		if (init_hw_mlme_ext23a(padapter) == _FAIL) {
 			DBG_8723A("can't init mlme_ext_priv\n");
@@ -695,7 +694,7 @@ int netdev_open23a(struct net_device *pnetdev)
 	else
 		netif_tx_wake_all_queues(pnetdev);
 
-	RT_TRACE(_module_os_intfs_c_, _drv_info_, ("-871x_drv - dev_open\n"));
+	RT_TRACE(_module_os_intfs_c_, _drv_info_, "-871x_drv - dev_open\n");
 	DBG_8723A("-871x_drv - drv_open, bup =%d\n", padapter->bup);
 exit:
 	mutex_unlock(&adapter_to_dvobj(padapter)->hw_init_mutex);
@@ -708,7 +707,7 @@ netdev_open23a_error:
 	netif_tx_stop_all_queues(pnetdev);
 
 	RT_TRACE(_module_os_intfs_c_, _drv_err_,
-		 ("-871x_drv - dev_open, fail!\n"));
+		 "-871x_drv - dev_open, fail!\n");
 	DBG_8723A("-871x_drv - drv_open fail, bup =%d\n", padapter->bup);
 
 	ret = -1;
@@ -729,7 +728,7 @@ static int ips_netdrv_open(struct rtw_adapter *padapter)
 	status = rtl8723au_hal_init(padapter);
 	if (status == _FAIL) {
 		RT_TRACE(_module_os_intfs_c_, _drv_err_,
-			 ("ips_netdrv_open(): Can't init h/w!\n"));
+			 "ips_netdrv_open(): Can't init h/w!\n");
 		goto netdev_open23a_error;
 	}
 
@@ -807,7 +806,7 @@ static int netdev_close(struct net_device *pnetdev)
 {
 	struct rtw_adapter *padapter = netdev_priv(pnetdev);
 
-	RT_TRACE(_module_os_intfs_c_, _drv_info_, ("+871x_drv - drv_close\n"));
+	RT_TRACE(_module_os_intfs_c_, _drv_info_, "+871x_drv - drv_close\n");
 
 	padapter->net_closed = true;
 
@@ -835,7 +834,7 @@ static int netdev_close(struct net_device *pnetdev)
 
 	rtw_scan_abort23a(padapter);
 
-	RT_TRACE(_module_os_intfs_c_, _drv_info_, ("-871x_drv - drv_close\n"));
+	RT_TRACE(_module_os_intfs_c_, _drv_info_, "-871x_drv - drv_close\n");
 	DBG_8723A("-871x_drv - drv_close, bup =%d\n", padapter->bup);
 
 	return 0;
diff --git a/drivers/staging/rtl8723au/os_dep/recv_linux.c b/drivers/staging/rtl8723au/os_dep/recv_linux.c
index bcd970baf405..084b506ae161 100644
--- a/drivers/staging/rtl8723au/os_dep/recv_linux.c
+++ b/drivers/staging/rtl8723au/os_dep/recv_linux.c
@@ -81,19 +81,19 @@ int rtw_recv_indicatepkt23a(struct rtw_adapter *padapter,
 	skb = precv_frame->pkt;
 	if (!skb) {
 		RT_TRACE(_module_recv_osdep_c_, _drv_err_,
-			 ("rtw_recv_indicatepkt23a():skb == NULL!!!!\n"));
+			 "rtw_recv_indicatepkt23a():skb == NULL!!!!\n");
 		goto _recv_indicatepkt_drop;
 	}
 
 	RT_TRACE(_module_recv_osdep_c_, _drv_info_,
-		 ("rtw_recv_indicatepkt23a():skb != NULL !!!\n"));
+		 "rtw_recv_indicatepkt23a():skb != NULL !!!\n");
 	RT_TRACE(_module_recv_osdep_c_, _drv_info_,
-		 ("rtw_recv_indicatepkt23a():precv_frame->hdr.rx_data =%p\n",
-		  precv_frame->pkt->data));
+		 "rtw_recv_indicatepkt23a():precv_frame->hdr.rx_data =%p\n",
+		 precv_frame->pkt->data);
 	RT_TRACE(_module_recv_osdep_c_, _drv_info_,
-		 ("\n skb->head =%p skb->data =%p skb->tail =%p skb->end =%p skb->len =%d\n",
-		  skb->head, skb->data,
-		  skb_tail_pointer(skb), skb_end_pointer(skb), skb->len));
+		 "skb->head =%p skb->data =%p skb->tail =%p skb->end =%p skb->len =%d\n",
+		 skb->head, skb->data,
+		 skb_tail_pointer(skb), skb_end_pointer(skb), skb->len);
 
 	if (check_fwstate(pmlmepriv, WIFI_AP_STATE) == true) {
 		struct sk_buff *pskb2 = NULL;
@@ -148,7 +148,7 @@ _recv_indicatepkt_end:
 	rtw_free_recvframe23a(precv_frame);
 
 	RT_TRACE(_module_recv_osdep_c_, _drv_info_,
-		 ("\n rtw_recv_indicatepkt23a :after netif_rx!!!!\n"));
+		 "rtw_recv_indicatepkt23a :after netif_rx!!!!\n");
 	return _SUCCESS;
 
 _recv_indicatepkt_drop:
diff --git a/drivers/staging/rtl8723au/os_dep/usb_intf.c b/drivers/staging/rtl8723au/os_dep/usb_intf.c
index 05755b870a5f..27b3a5b7d8d4 100644
--- a/drivers/staging/rtl8723au/os_dep/usb_intf.c
+++ b/drivers/staging/rtl8723au/os_dep/usb_intf.c
@@ -159,7 +159,7 @@ static struct dvobj_priv *usb_dvobj_init(struct usb_interface *usb_intf)
 
 	if (rtw_init_intf_priv(pdvobjpriv) == _FAIL) {
 		RT_TRACE(_module_os_intfs_c_, _drv_err_,
-			 ("\n Can't INIT rtw_init_intf_priv\n"));
+			 "Can't INIT rtw_init_intf_priv\n");
 		goto free_dvobj;
 	}
 	/* 3 misc */
@@ -213,7 +213,7 @@ static void usb_dvobj_deinit(struct usb_interface *usb_intf)
 
 void rtl8723a_usb_intf_stop(struct rtw_adapter *padapter)
 {
-	RT_TRACE(_module_hci_intfs_c_, _drv_err_, ("+usb_intf_stop\n"));
+	RT_TRACE(_module_hci_intfs_c_, _drv_err_, "+usb_intf_stop\n");
 
 	/* disable_hw_interrupt */
 	if (!padapter->bSurpriseRemoved) {
@@ -221,7 +221,7 @@ void rtl8723a_usb_intf_stop(struct rtw_adapter *padapter)
 		 * TODO:
 		 */
 		RT_TRACE(_module_hci_intfs_c_, _drv_err_,
-			 ("SurpriseRemoved == false\n"));
+			 "SurpriseRemoved == false\n");
 	}
 
 	/* cancel in irp */
@@ -231,13 +231,14 @@ void rtl8723a_usb_intf_stop(struct rtw_adapter *padapter)
 	rtl8723au_write_port_cancel(padapter);
 
 	/* todo:cancel other irps */
-	RT_TRACE(_module_hci_intfs_c_, _drv_err_, ("-usb_intf_stop\n"));
+	RT_TRACE(_module_hci_intfs_c_, _drv_err_, "-usb_intf_stop\n");
 }
 
 static void rtw_dev_unload(struct rtw_adapter *padapter)
 {
 	struct submit_ctx *pack_tx_ops = &padapter->xmitpriv.ack_tx_ops;
-	RT_TRACE(_module_hci_intfs_c_, _drv_err_, ("+rtw_dev_unload\n"));
+
+	RT_TRACE(_module_hci_intfs_c_, _drv_err_, "+rtw_dev_unload\n");
 
 	if (padapter->bup) {
 		DBG_8723A("===> rtw_dev_unload\n");
@@ -261,10 +262,10 @@ static void rtw_dev_unload(struct rtw_adapter *padapter)
 		padapter->bup = false;
 	} else {
 		RT_TRACE(_module_hci_intfs_c_, _drv_err_,
-			 ("r871x_dev_unload():padapter->bup == false\n"));
+			 "r871x_dev_unload():padapter->bup == false\n");
 	}
 	DBG_8723A("<=== rtw_dev_unload\n");
-	RT_TRACE(_module_hci_intfs_c_, _drv_err_, ("-rtw_dev_unload\n"));
+	RT_TRACE(_module_hci_intfs_c_, _drv_err_, "-rtw_dev_unload\n");
 }
 
 static int rtw_suspend(struct usb_interface *pusb_intf, pm_message_t message)
@@ -435,7 +436,7 @@ static struct rtw_adapter *rtw_usb_if1_init(struct dvobj_priv *dvobj,
 	/* step 5. */
 	if (rtw_init_drv_sw23a(padapter) == _FAIL) {
 		RT_TRACE(_module_hci_intfs_c_, _drv_err_,
-			 ("Initialize driver software resource Failed!\n"));
+			 "Initialize driver software resource Failed!\n");
 		goto free_hal_data;
 	}
 
@@ -533,13 +534,13 @@ static int rtw_drv_init(struct usb_interface *pusb_intf,
 	struct dvobj_priv *dvobj;
 	int status = _FAIL;
 
-	RT_TRACE(_module_hci_intfs_c_, _drv_err_, ("+rtw_drv_init\n"));
+	RT_TRACE(_module_hci_intfs_c_, _drv_err_, "+rtw_drv_init\n");
 
 	/* Initialize dvobj_priv */
 	dvobj = usb_dvobj_init(pusb_intf);
 	if (!dvobj) {
 		RT_TRACE(_module_hci_intfs_c_, _drv_err_,
-			 ("initialize device object priv Failed!\n"));
+			 "initialize device object priv Failed!\n");
 		goto exit;
 	}
 
@@ -554,7 +555,7 @@ static int rtw_drv_init(struct usb_interface *pusb_intf,
 	if (status != _SUCCESS)
 		goto free_if1;
 	RT_TRACE(_module_hci_intfs_c_, _drv_err_,
-		 ("-871x_drv - drv_init, success!\n"));
+		 "-871x_drv - drv_init, success!\n");
 
 	status = _SUCCESS;
 
@@ -586,7 +587,7 @@ static void rtw_disconnect(struct usb_interface *pusb_intf)
 
 	usb_set_intfdata(pusb_intf, NULL);
 
-	RT_TRACE(_module_hci_intfs_c_, _drv_err_, ("+dev_remove()\n"));
+	RT_TRACE(_module_hci_intfs_c_, _drv_err_, "+dev_remove()\n");
 
 	rtw_pm_set_ips23a(padapter, IPS_NONE);
 	rtw_pm_set_lps23a(padapter, PS_MODE_ACTIVE);
@@ -597,21 +598,19 @@ static void rtw_disconnect(struct usb_interface *pusb_intf)
 
 	usb_dvobj_deinit(pusb_intf);
 
-	RT_TRACE(_module_hci_intfs_c_, _drv_err_, ("-dev_remove()\n"));
+	RT_TRACE(_module_hci_intfs_c_, _drv_err_, "-dev_remove()\n");
 	DBG_8723A("-r871xu_dev_remove, done\n");
-
-	return;
 }
 
 static int __init rtw_drv_entry(void)
 {
-	RT_TRACE(_module_hci_intfs_c_, _drv_err_, ("+rtw_drv_entry\n"));
+	RT_TRACE(_module_hci_intfs_c_, _drv_err_, "+rtw_drv_entry\n");
 	return usb_register(usb_drv);
 }
 
 static void __exit rtw_drv_halt(void)
 {
-	RT_TRACE(_module_hci_intfs_c_, _drv_err_, ("+rtw_drv_halt\n"));
+	RT_TRACE(_module_hci_intfs_c_, _drv_err_, "+rtw_drv_halt\n");
 	DBG_8723A("+rtw_drv_halt\n");
 
 	usb_deregister(usb_drv);
diff --git a/drivers/staging/rtl8723au/os_dep/usb_ops_linux.c b/drivers/staging/rtl8723au/os_dep/usb_ops_linux.c
index 3e19b3b2c1c2..0cdaef0a8c24 100644
--- a/drivers/staging/rtl8723au/os_dep/usb_ops_linux.c
+++ b/drivers/staging/rtl8723au/os_dep/usb_ops_linux.c
@@ -58,9 +58,8 @@ static void usb_write_port23a_complete(struct urb *purb)
 	if (padapter->bSurpriseRemoved || padapter->bDriverStopped ||
 	    padapter->bWritePortCancel) {
 		RT_TRACE(_module_hci_ops_os_c_, _drv_err_,
-			 ("usb_write_port23a_complete:bDriverStopped(%d) OR "
-			  "bSurpriseRemoved(%d)", padapter->bDriverStopped,
-			  padapter->bSurpriseRemoved));
+			 "usb_write_port23a_complete:bDriverStopped(%d) OR bSurpriseRemoved(%d)\n",
+			 padapter->bDriverStopped, padapter->bSurpriseRemoved);
 		DBG_8723A("%s(): TX Warning! bDriverStopped(%d) OR "
 			  "bSurpriseRemoved(%d) bWritePortCancel(%d) "
 			  "pxmitbuf->ext_tag(%x)\n", __func__,
@@ -72,14 +71,14 @@ static void usb_write_port23a_complete(struct urb *purb)
 
 	if (purb->status) {
 		RT_TRACE(_module_hci_ops_os_c_, _drv_err_,
-			 ("usb_write_port23a_complete : purb->status(%d) "
-			  "!= 0\n", purb->status));
+			 "usb_write_port23a_complete : purb->status(%d) != 0\n",
+			 purb->status);
 		DBG_8723A("###=> urb_write_port_complete status(%d)\n",
 			  purb->status);
 		if (purb->status == -EPIPE || purb->status == -EPROTO) {
 		} else if (purb->status == -EINPROGRESS) {
 			RT_TRACE(_module_hci_ops_os_c_, _drv_err_,
-				 ("usb_write_port23a_complete: EINPROGESS\n"));
+				 "usb_write_port23a_complete: EINPROGESS\n");
 			goto check_completion;
 		} else if (purb->status == -ENOENT) {
 			DBG_8723A("%s: -ENOENT\n", __func__);
@@ -89,18 +88,16 @@ static void usb_write_port23a_complete(struct urb *purb)
 			goto check_completion;
 		} else if (purb->status == -ESHUTDOWN) {
 			RT_TRACE(_module_hci_ops_os_c_, _drv_err_,
-				 ("usb_write_port23a_complete: ESHUTDOWN\n"));
+				 "usb_write_port23a_complete: ESHUTDOWN\n");
 			padapter->bDriverStopped = true;
 			RT_TRACE(_module_hci_ops_os_c_, _drv_err_,
-				 ("usb_write_port23a_complete:bDriverStopped "
-				  "= true\n"));
+				 "usb_write_port23a_complete:bDriverStopped = true\n");
 			goto check_completion;
 		} else {
 			padapter->bSurpriseRemoved = true;
 			DBG_8723A("bSurpriseRemoved = true\n");
 			RT_TRACE(_module_hci_ops_os_c_, _drv_err_,
-				 ("usb_write_port23a_complete:bSurpriseRemoved "
-				  "= true\n"));
+				 "usb_write_port23a_complete:bSurpriseRemoved = true\n");
 			goto check_completion;
 		}
 	}
@@ -132,12 +129,12 @@ int rtl8723au_write_port(struct rtw_adapter *padapter, u32 addr, u32 cnt,
 	int status;
 	int ret = _FAIL;
 
-	RT_TRACE(_module_hci_ops_os_c_, _drv_err_, ("+usb_write_port23a\n"));
+	RT_TRACE(_module_hci_ops_os_c_, _drv_err_, "+usb_write_port23a\n");
 
 	if (padapter->bDriverStopped || padapter->bSurpriseRemoved) {
 		RT_TRACE(_module_hci_ops_os_c_, _drv_err_,
-			 ("%s:(padapter->bDriverStopped || "
-			  "padapter->bSurpriseRemoved)!!!\n", __func__));
+			 "%s:(padapter->bDriverStopped || padapter->bSurpriseRemoved)!!!\n",
+			 __func__);
 		rtw23a_sctx_done_err(&pxmitbuf->sctx, RTW_SCTX_DONE_TX_DENY);
 		goto exit;
 	}
@@ -188,8 +185,8 @@ int rtl8723au_write_port(struct rtw_adapter *padapter, u32 addr, u32 cnt,
 				     RTW_SCTX_DONE_WRITE_PORT_ERR);
 		DBG_8723A("usb_write_port23a, status =%d\n", status);
 		RT_TRACE(_module_hci_ops_os_c_, _drv_err_,
-			 ("usb_write_port23a(): usb_submit_urb, status =%x\n",
-			 status));
+			 "usb_write_port23a(): usb_submit_urb, status =%x\n",
+			 status);
 
 		switch (status) {
 		case -ENODEV:
@@ -201,7 +198,7 @@ int rtl8723au_write_port(struct rtw_adapter *padapter, u32 addr, u32 cnt,
 		goto exit;
 	}
 	ret = _SUCCESS;
-	RT_TRACE(_module_hci_ops_os_c_, _drv_err_, ("-usb_write_port23a\n"));
+	RT_TRACE(_module_hci_ops_os_c_, _drv_err_, "-usb_write_port23a\n");
 
 exit:
 	if (ret != _SUCCESS)
diff --git a/drivers/staging/rtl8723au/os_dep/xmit_linux.c b/drivers/staging/rtl8723au/os_dep/xmit_linux.c
index 69f88848715a..9a14074ecec0 100644
--- a/drivers/staging/rtl8723au/os_dep/xmit_linux.c
+++ b/drivers/staging/rtl8723au/os_dep/xmit_linux.c
@@ -123,11 +123,11 @@ int rtw_xmit23a_entry23a(struct sk_buff *skb, struct net_device *pnetdev)
 	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
 	int res = 0;
 
-	RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("+xmit_enry\n"));
+	RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, "+xmit_enry\n");
 
 	if (!rtw_if_up23a(padapter)) {
 		RT_TRACE(_module_xmit_osdep_c_, _drv_err_,
-			 ("rtw_xmit23a_entry23a: rtw_if_up23a fail\n"));
+			 "rtw_xmit23a_entry23a: rtw_if_up23a fail\n");
 		goto drop_packet;
 	}
 
@@ -139,16 +139,16 @@ int rtw_xmit23a_entry23a(struct sk_buff *skb, struct net_device *pnetdev)
 
 	pxmitpriv->tx_pkts++;
 	RT_TRACE(_module_xmit_osdep_c_, _drv_info_,
-		 ("rtw_xmit23a_entry23a: tx_pkts=%d\n",
-		 (u32)pxmitpriv->tx_pkts));
+		 "rtw_xmit23a_entry23a: tx_pkts=%d\n",
+		 (u32)pxmitpriv->tx_pkts);
 	goto exit;
 
 drop_packet:
 	pxmitpriv->tx_drop++;
 	dev_kfree_skb_any(skb);
 	RT_TRACE(_module_xmit_osdep_c_, _drv_notice_,
-		 ("rtw_xmit23a_entry23a: drop, tx_drop=%d\n",
-		 (u32)pxmitpriv->tx_drop));
+		 "rtw_xmit23a_entry23a: drop, tx_drop=%d\n",
+		 (u32)pxmitpriv->tx_drop);
 exit:
 	return 0;
 }
diff --git a/drivers/staging/rts5208/Makefile b/drivers/staging/rts5208/Makefile
index 17b4471c4d6d..f7fd03a94e5f 100644
--- a/drivers/staging/rts5208/Makefile
+++ b/drivers/staging/rts5208/Makefile
@@ -3,4 +3,4 @@ obj-$(CONFIG_RTS5208) := rts5208.o
 ccflags-y := -Idrivers/scsi
 
 rts5208-y := rtsx.o rtsx_chip.o rtsx_transport.o rtsx_scsi.o \
-	rtsx_card.o general.o sd.o xd.o ms.o spi.o
+	rtsx_card.o general.o sd.o xd.o ms.o spi.o trace.o
diff --git a/drivers/staging/rts5208/ms.c b/drivers/staging/rts5208/ms.c
index a47a19135d49..ee818b0dc401 100644
--- a/drivers/staging/rts5208/ms.c
+++ b/drivers/staging/rts5208/ms.c
@@ -26,9 +26,6 @@
 #include <linux/vmalloc.h>
 
 #include "rtsx.h"
-#include "rtsx_transport.h"
-#include "rtsx_scsi.h"
-#include "rtsx_card.h"
 #include "ms.h"
 
 static inline void ms_set_err_code(struct rtsx_chip *chip, u8 err_code)
@@ -47,7 +44,8 @@ static inline int ms_check_err_code(struct rtsx_chip *chip, u8 err_code)
 
 static int ms_parse_err_code(struct rtsx_chip *chip)
 {
-	TRACE_RET(chip, STATUS_FAIL);
+	rtsx_trace(chip);
+	return STATUS_FAIL;
 }
 
 static int ms_transfer_tpc(struct rtsx_chip *chip, u8 trans_mode,
@@ -78,7 +76,8 @@ static int ms_transfer_tpc(struct rtsx_chip *chip, u8 trans_mode,
 	if (retval < 0) {
 		rtsx_clear_ms_error(chip);
 		ms_set_err_code(chip, MS_TO_ERROR);
-		TRACE_RET(chip, ms_parse_err_code(chip));
+		rtsx_trace(chip);
+		return ms_parse_err_code(chip);
 	}
 
 	ptr = rtsx_get_cmd_data(chip) + 1;
@@ -86,13 +85,15 @@ static int ms_transfer_tpc(struct rtsx_chip *chip, u8 trans_mode,
 	if (!(tpc & 0x08)) {		/* Read Packet */
 		if (*ptr & MS_CRC16_ERR) {
 			ms_set_err_code(chip, MS_CRC16_ERROR);
-			TRACE_RET(chip, ms_parse_err_code(chip));
+			rtsx_trace(chip);
+			return ms_parse_err_code(chip);
 		}
 	} else {			/* Write Packet */
 		if (CHK_MSPRO(ms_card) && !(*ptr & 0x80)) {
 			if (*ptr & (MS_INT_ERR | MS_INT_CMDNK)) {
 				ms_set_err_code(chip, MS_CMD_NK);
-				TRACE_RET(chip, ms_parse_err_code(chip));
+				rtsx_trace(chip);
+				return ms_parse_err_code(chip);
 			}
 		}
 	}
@@ -100,22 +101,25 @@ static int ms_transfer_tpc(struct rtsx_chip *chip, u8 trans_mode,
 	if (*ptr & MS_RDY_TIMEOUT) {
 		rtsx_clear_ms_error(chip);
 		ms_set_err_code(chip, MS_TO_ERROR);
-		TRACE_RET(chip, ms_parse_err_code(chip));
+		rtsx_trace(chip);
+		return ms_parse_err_code(chip);
 	}
 
 	return STATUS_SUCCESS;
 }
 
 static int ms_transfer_data(struct rtsx_chip *chip, u8 trans_mode,
-			u8 tpc, u16 sec_cnt, u8 cfg, int mode_2k,
+			u8 tpc, u16 sec_cnt, u8 cfg, bool mode_2k,
 			int use_sg, void *buf, int buf_len)
 {
 	int retval;
 	u8 val, err_code = 0;
 	enum dma_data_direction dir;
 
-	if (!buf || !buf_len)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (!buf || !buf_len) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	if (trans_mode == MS_TM_AUTO_READ) {
 		dir = DMA_FROM_DEVICE;
@@ -124,7 +128,8 @@ static int ms_transfer_data(struct rtsx_chip *chip, u8 trans_mode,
 		dir = DMA_TO_DEVICE;
 		err_code = MS_FLASH_WRITE_ERROR;
 	} else {
-		TRACE_RET(chip, STATUS_FAIL);
+		rtsx_trace(chip);
+		return STATUS_FAIL;
 	}
 
 	rtsx_init_cmd(chip);
@@ -160,12 +165,19 @@ static int ms_transfer_data(struct rtsx_chip *chip, u8 trans_mode,
 		else
 			retval = STATUS_FAIL;
 
-		TRACE_RET(chip, retval);
+		rtsx_trace(chip);
+		return retval;
 	}
 
-	RTSX_READ_REG(chip, MS_TRANS_CFG, &val);
-	if (val & (MS_INT_CMDNK | MS_INT_ERR | MS_CRC16_ERR | MS_RDY_TIMEOUT))
-		TRACE_RET(chip, STATUS_FAIL);
+	retval = rtsx_read_register(chip, MS_TRANS_CFG, &val);
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
+	if (val & (MS_INT_CMDNK | MS_INT_ERR | MS_CRC16_ERR | MS_RDY_TIMEOUT)) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	return STATUS_SUCCESS;
 }
@@ -176,8 +188,10 @@ static int ms_write_bytes(struct rtsx_chip *chip,
 	struct ms_info *ms_card = &(chip->ms_card);
 	int retval, i;
 
-	if (!data || (data_len < cnt))
-		TRACE_RET(chip, STATUS_ERROR);
+	if (!data || (data_len < cnt)) {
+		rtsx_trace(chip);
+		return STATUS_ERROR;
+	}
 
 	rtsx_init_cmd(chip);
 
@@ -211,25 +225,28 @@ static int ms_write_bytes(struct rtsx_chip *chip,
 		if (!(tpc & 0x08)) {
 			if (val & MS_CRC16_ERR) {
 				ms_set_err_code(chip, MS_CRC16_ERROR);
-				TRACE_RET(chip, ms_parse_err_code(chip));
+				rtsx_trace(chip);
+				return ms_parse_err_code(chip);
 			}
 		} else {
 			if (CHK_MSPRO(ms_card) && !(val & 0x80)) {
 				if (val & (MS_INT_ERR | MS_INT_CMDNK)) {
 					ms_set_err_code(chip, MS_CMD_NK);
-					TRACE_RET(chip,
-						ms_parse_err_code(chip));
+					rtsx_trace(chip);
+					return ms_parse_err_code(chip);
 				}
 			}
 		}
 
 		if (val & MS_RDY_TIMEOUT) {
 			ms_set_err_code(chip, MS_TO_ERROR);
-			TRACE_RET(chip, ms_parse_err_code(chip));
+			rtsx_trace(chip);
+			return ms_parse_err_code(chip);
 		}
 
 		ms_set_err_code(chip, MS_TO_ERROR);
-		TRACE_RET(chip, ms_parse_err_code(chip));
+		rtsx_trace(chip);
+		return ms_parse_err_code(chip);
 	}
 
 	return STATUS_SUCCESS;
@@ -242,8 +259,10 @@ static int ms_read_bytes(struct rtsx_chip *chip,
 	int retval, i;
 	u8 *ptr;
 
-	if (!data)
-		TRACE_RET(chip, STATUS_ERROR);
+	if (!data) {
+		rtsx_trace(chip);
+		return STATUS_ERROR;
+	}
 
 	rtsx_init_cmd(chip);
 
@@ -277,25 +296,28 @@ static int ms_read_bytes(struct rtsx_chip *chip,
 		if (!(tpc & 0x08)) {
 			if (val & MS_CRC16_ERR) {
 				ms_set_err_code(chip, MS_CRC16_ERROR);
-				TRACE_RET(chip, ms_parse_err_code(chip));
+				rtsx_trace(chip);
+				return ms_parse_err_code(chip);
 			}
 		} else {
 			if (CHK_MSPRO(ms_card) && !(val & 0x80)) {
 				if (val & (MS_INT_ERR | MS_INT_CMDNK)) {
 					ms_set_err_code(chip, MS_CMD_NK);
-					TRACE_RET(chip,
-						ms_parse_err_code(chip));
+					rtsx_trace(chip);
+					return ms_parse_err_code(chip);
 				}
 			}
 		}
 
 		if (val & MS_RDY_TIMEOUT) {
 			ms_set_err_code(chip, MS_TO_ERROR);
-			TRACE_RET(chip, ms_parse_err_code(chip));
+			rtsx_trace(chip);
+			return ms_parse_err_code(chip);
 		}
 
 		ms_set_err_code(chip, MS_TO_ERROR);
-		TRACE_RET(chip, ms_parse_err_code(chip));
+		rtsx_trace(chip);
+		return ms_parse_err_code(chip);
 	}
 
 	ptr = rtsx_get_cmd_data(chip) + 1;
@@ -331,7 +353,8 @@ static int ms_set_rw_reg_addr(struct rtsx_chip *chip,
 		rtsx_clear_ms_error(chip);
 	}
 
-	TRACE_RET(chip, STATUS_FAIL);
+	rtsx_trace(chip);
+	return STATUS_FAIL;
 }
 
 static int ms_send_cmd(struct rtsx_chip *chip, u8 cmd, u8 cfg)
@@ -369,12 +392,16 @@ static int ms_set_init_para(struct rtsx_chip *chip)
 	}
 
 	retval = switch_clock(chip, ms_card->ms_clock);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	retval = select_card(chip, MS_CARD);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	return STATUS_SUCCESS;
 }
@@ -385,37 +412,87 @@ static int ms_switch_clock(struct rtsx_chip *chip)
 	int retval;
 
 	retval = select_card(chip, MS_CARD);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	retval = switch_clock(chip, ms_card->ms_clock);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	return STATUS_SUCCESS;
 }
 
 static int ms_pull_ctl_disable(struct rtsx_chip *chip)
 {
+	int retval;
+
 	if (CHECK_PID(chip, 0x5208)) {
-		RTSX_WRITE_REG(chip, CARD_PULL_CTL1, 0xFF,
-			MS_D1_PD | MS_D2_PD | MS_CLK_PD | MS_D6_PD);
-		RTSX_WRITE_REG(chip, CARD_PULL_CTL2, 0xFF,
-			MS_D3_PD | MS_D0_PD | MS_BS_PD | XD_D4_PD);
-		RTSX_WRITE_REG(chip, CARD_PULL_CTL3, 0xFF,
-			MS_D7_PD | XD_CE_PD | XD_CLE_PD | XD_CD_PU);
-		RTSX_WRITE_REG(chip, CARD_PULL_CTL4, 0xFF,
-			XD_RDY_PD | SD_D3_PD | SD_D2_PD | XD_ALE_PD);
-		RTSX_WRITE_REG(chip, CARD_PULL_CTL5, 0xFF,
-			MS_INS_PU | SD_WP_PD | SD_CD_PU | SD_CMD_PD);
-		RTSX_WRITE_REG(chip, CARD_PULL_CTL6, 0xFF,
-			MS_D5_PD | MS_D4_PD);
+		retval = rtsx_write_register(chip, CARD_PULL_CTL1, 0xFF,
+					     MS_D1_PD | MS_D2_PD | MS_CLK_PD | MS_D6_PD);
+		if (retval) {
+			rtsx_trace(chip);
+			return retval;
+		}
+		retval = rtsx_write_register(chip, CARD_PULL_CTL2, 0xFF,
+					     MS_D3_PD | MS_D0_PD | MS_BS_PD | XD_D4_PD);
+		if (retval) {
+			rtsx_trace(chip);
+			return retval;
+		}
+		retval = rtsx_write_register(chip, CARD_PULL_CTL3, 0xFF,
+					     MS_D7_PD | XD_CE_PD | XD_CLE_PD | XD_CD_PU);
+		if (retval) {
+			rtsx_trace(chip);
+			return retval;
+		}
+		retval = rtsx_write_register(chip, CARD_PULL_CTL4, 0xFF,
+					     XD_RDY_PD | SD_D3_PD | SD_D2_PD | XD_ALE_PD);
+		if (retval) {
+			rtsx_trace(chip);
+			return retval;
+		}
+		retval = rtsx_write_register(chip, CARD_PULL_CTL5, 0xFF,
+					     MS_INS_PU | SD_WP_PD | SD_CD_PU | SD_CMD_PD);
+		if (retval) {
+			rtsx_trace(chip);
+			return retval;
+		}
+		retval = rtsx_write_register(chip, CARD_PULL_CTL6, 0xFF,
+					     MS_D5_PD | MS_D4_PD);
+		if (retval) {
+			rtsx_trace(chip);
+			return retval;
+		}
 	} else if (CHECK_PID(chip, 0x5288)) {
 		if (CHECK_BARO_PKG(chip, QFN)) {
-			RTSX_WRITE_REG(chip, CARD_PULL_CTL1, 0xFF, 0x55);
-			RTSX_WRITE_REG(chip, CARD_PULL_CTL2, 0xFF, 0x55);
-			RTSX_WRITE_REG(chip, CARD_PULL_CTL3, 0xFF, 0x4B);
-			RTSX_WRITE_REG(chip, CARD_PULL_CTL4, 0xFF, 0x69);
+			retval = rtsx_write_register(chip, CARD_PULL_CTL1,
+						     0xFF, 0x55);
+			if (retval) {
+				rtsx_trace(chip);
+				return retval;
+			}
+			retval = rtsx_write_register(chip, CARD_PULL_CTL2,
+						     0xFF, 0x55);
+			if (retval) {
+				rtsx_trace(chip);
+				return retval;
+			}
+			retval = rtsx_write_register(chip, CARD_PULL_CTL3,
+						     0xFF, 0x4B);
+			if (retval) {
+				rtsx_trace(chip);
+				return retval;
+			}
+			retval = rtsx_write_register(chip, CARD_PULL_CTL4,
+						     0xFF, 0x69);
+			if (retval) {
+				rtsx_trace(chip);
+				return retval;
+			}
 		}
 	}
 
@@ -455,8 +532,10 @@ static int ms_pull_ctl_enable(struct rtsx_chip *chip)
 	}
 
 	retval = rtsx_send_cmd(chip, MS_CARD, 100);
-	if (retval < 0)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval < 0) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	return STATUS_SUCCESS;
 }
@@ -475,29 +554,41 @@ static int ms_prepare_reset(struct rtsx_chip *chip)
 	ms_card->pro_under_formatting = 0;
 
 	retval = ms_power_off_card3v3(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	if (!chip->ft2_fast_mode)
 		wait_timeout(250);
 
 	retval = enable_card_clock(chip, MS_CARD);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	if (chip->asic_code) {
 		retval = ms_pull_ctl_enable(chip);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, STATUS_FAIL);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 	} else {
-		RTSX_WRITE_REG(chip, FPGA_PULL_CTL,
-			FPGA_MS_PULL_CTL_BIT | 0x20, 0);
+		retval = rtsx_write_register(chip, FPGA_PULL_CTL,
+					     FPGA_MS_PULL_CTL_BIT | 0x20, 0);
+		if (retval) {
+			rtsx_trace(chip);
+			return retval;
+		}
 	}
 
 	if (!chip->ft2_fast_mode) {
 		retval = card_power_on(chip, MS_CARD);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, STATUS_FAIL);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 
 		wait_timeout(150);
 
@@ -510,30 +601,52 @@ static int ms_prepare_reset(struct rtsx_chip *chip)
 		if (chip->ocp_stat & oc_mask) {
 			dev_dbg(rtsx_dev(chip), "Over current, OCPSTAT is 0x%x\n",
 				chip->ocp_stat);
-			TRACE_RET(chip, STATUS_FAIL);
+			rtsx_trace(chip);
+			return STATUS_FAIL;
 		}
 #endif
 	}
 
-	RTSX_WRITE_REG(chip, CARD_OE, MS_OUTPUT_EN, MS_OUTPUT_EN);
+	retval = rtsx_write_register(chip, CARD_OE, MS_OUTPUT_EN,
+				     MS_OUTPUT_EN);
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
 
 	if (chip->asic_code) {
-		RTSX_WRITE_REG(chip, MS_CFG, 0xFF,
-			SAMPLE_TIME_RISING | PUSH_TIME_DEFAULT |
-			NO_EXTEND_TOGGLE | MS_BUS_WIDTH_1);
+		retval = rtsx_write_register(chip, MS_CFG, 0xFF,
+					     SAMPLE_TIME_RISING | PUSH_TIME_DEFAULT | NO_EXTEND_TOGGLE | MS_BUS_WIDTH_1);
+		if (retval) {
+			rtsx_trace(chip);
+			return retval;
+		}
 	} else {
-		RTSX_WRITE_REG(chip, MS_CFG, 0xFF,
-			SAMPLE_TIME_FALLING | PUSH_TIME_DEFAULT |
-			NO_EXTEND_TOGGLE | MS_BUS_WIDTH_1);
+		retval = rtsx_write_register(chip, MS_CFG, 0xFF,
+					     SAMPLE_TIME_FALLING | PUSH_TIME_DEFAULT | NO_EXTEND_TOGGLE | MS_BUS_WIDTH_1);
+		if (retval) {
+			rtsx_trace(chip);
+			return retval;
+		}
+	}
+	retval = rtsx_write_register(chip, MS_TRANS_CFG, 0xFF,
+				     NO_WAIT_INT | NO_AUTO_READ_INT_REG);
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
+	retval = rtsx_write_register(chip, CARD_STOP, MS_STOP | MS_CLR_ERR,
+				     MS_STOP | MS_CLR_ERR);
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
 	}
-	RTSX_WRITE_REG(chip, MS_TRANS_CFG,
-		0xFF, NO_WAIT_INT | NO_AUTO_READ_INT_REG);
-	RTSX_WRITE_REG(chip, CARD_STOP,
-		MS_STOP | MS_CLR_ERR, MS_STOP | MS_CLR_ERR);
 
 	retval = ms_set_init_para(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	return STATUS_SUCCESS;
 }
@@ -545,8 +658,10 @@ static int ms_identify_media_type(struct rtsx_chip *chip, int switch_8bit_bus)
 	u8 val;
 
 	retval = ms_set_rw_reg_addr(chip, Pro_StatusReg, 6, SystemParm, 1);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
 		retval = ms_transfer_tpc(chip, MS_TM_READ_BYTES, READ_REG,
@@ -554,29 +669,49 @@ static int ms_identify_media_type(struct rtsx_chip *chip, int switch_8bit_bus)
 		if (retval == STATUS_SUCCESS)
 			break;
 	}
-	if (i == MS_MAX_RETRY_COUNT)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (i == MS_MAX_RETRY_COUNT) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
-	RTSX_READ_REG(chip, PPBUF_BASE2 + 2, &val);
+	retval = rtsx_read_register(chip, PPBUF_BASE2 + 2, &val);
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
 	dev_dbg(rtsx_dev(chip), "Type register: 0x%x\n", val);
 	if (val != 0x01) {
 		if (val != 0x02)
 			ms_card->check_ms_flow = 1;
 
-		TRACE_RET(chip, STATUS_FAIL);
+		rtsx_trace(chip);
+		return STATUS_FAIL;
 	}
 
-	RTSX_READ_REG(chip, PPBUF_BASE2 + 4, &val);
+	retval = rtsx_read_register(chip, PPBUF_BASE2 + 4, &val);
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
 	dev_dbg(rtsx_dev(chip), "Category register: 0x%x\n", val);
 	if (val != 0) {
 		ms_card->check_ms_flow = 1;
-		TRACE_RET(chip, STATUS_FAIL);
+		rtsx_trace(chip);
+		return STATUS_FAIL;
 	}
 
-	RTSX_READ_REG(chip, PPBUF_BASE2 + 5, &val);
+	retval = rtsx_read_register(chip, PPBUF_BASE2 + 5, &val);
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
 	dev_dbg(rtsx_dev(chip), "Class register: 0x%x\n", val);
 	if (val == 0) {
-		RTSX_READ_REG(chip, PPBUF_BASE2, &val);
+		retval = rtsx_read_register(chip, PPBUF_BASE2, &val);
+		if (retval) {
+			rtsx_trace(chip);
+			return retval;
+		}
 		if (val & WRT_PRTCT)
 			chip->card_wp |= MS_CARD;
 		else
@@ -586,12 +721,17 @@ static int ms_identify_media_type(struct rtsx_chip *chip, int switch_8bit_bus)
 		chip->card_wp |= MS_CARD;
 	} else {
 		ms_card->check_ms_flow = 1;
-		TRACE_RET(chip, STATUS_FAIL);
+		rtsx_trace(chip);
+		return STATUS_FAIL;
 	}
 
 	ms_card->ms_type |= TYPE_MSPRO;
 
-	RTSX_READ_REG(chip, PPBUF_BASE2 + 3, &val);
+	retval = rtsx_read_register(chip, PPBUF_BASE2 + 3, &val);
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
 	dev_dbg(rtsx_dev(chip), "IF Mode register: 0x%x\n", val);
 	if (val == 0) {
 		ms_card->ms_type &= 0x0F;
@@ -602,7 +742,8 @@ static int ms_identify_media_type(struct rtsx_chip *chip, int switch_8bit_bus)
 			ms_card->ms_type &= 0x0F;
 
 	} else {
-		TRACE_RET(chip, STATUS_FAIL);
+		rtsx_trace(chip);
+		return STATUS_FAIL;
 	}
 
 	return STATUS_SUCCESS;
@@ -618,7 +759,8 @@ static int ms_confirm_cpu_startup(struct rtsx_chip *chip)
 	do {
 		if (detect_card_cd(chip, MS_CARD) != STATUS_SUCCESS) {
 			ms_set_err_code(chip, MS_NO_CARD);
-			TRACE_RET(chip, STATUS_FAIL);
+			rtsx_trace(chip);
+			return STATUS_FAIL;
 		}
 
 		for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
@@ -627,11 +769,15 @@ static int ms_confirm_cpu_startup(struct rtsx_chip *chip)
 			if (retval == STATUS_SUCCESS)
 				break;
 		}
-		if (i == MS_MAX_RETRY_COUNT)
-			TRACE_RET(chip, STATUS_FAIL);
+		if (i == MS_MAX_RETRY_COUNT) {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 
-		if (k > 100)
-			TRACE_RET(chip, STATUS_FAIL);
+		if (k > 100) {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 
 		k++;
 		wait_timeout(100);
@@ -642,14 +788,18 @@ static int ms_confirm_cpu_startup(struct rtsx_chip *chip)
 		if (retval == STATUS_SUCCESS)
 			break;
 	}
-	if (i == MS_MAX_RETRY_COUNT)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (i == MS_MAX_RETRY_COUNT) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	if (val & INT_REG_ERR) {
 		if (val & INT_REG_CMDNK)
 			chip->card_wp |= (MS_CARD);
-		else
-			TRACE_RET(chip, STATUS_FAIL);
+		else {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 	}
 	/* --  end confirm CPU startup */
 
@@ -669,8 +819,10 @@ static int ms_switch_parallel_bus(struct rtsx_chip *chip)
 		if (retval == STATUS_SUCCESS)
 			break;
 	}
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	return STATUS_SUCCESS;
 }
@@ -689,21 +841,31 @@ static int ms_switch_8bit_bus(struct rtsx_chip *chip)
 		if (retval == STATUS_SUCCESS)
 			break;
 	}
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
-	RTSX_WRITE_REG(chip, MS_CFG, 0x98,
-		MS_BUS_WIDTH_8 | SAMPLE_TIME_FALLING);
+	retval = rtsx_write_register(chip, MS_CFG, 0x98,
+				     MS_BUS_WIDTH_8 | SAMPLE_TIME_FALLING);
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
 	ms_card->ms_type |= MS_8BIT;
 	retval = ms_set_init_para(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
 		retval = ms_transfer_tpc(chip, MS_TM_READ_BYTES, GET_INT,
 					1, NO_WAIT_INT);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, STATUS_FAIL);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 	}
 
 	return STATUS_SUCCESS;
@@ -716,22 +878,29 @@ static int ms_pro_reset_flow(struct rtsx_chip *chip, int switch_8bit_bus)
 
 	for (i = 0; i < 3; i++) {
 		retval = ms_prepare_reset(chip);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, STATUS_FAIL);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 
 		retval = ms_identify_media_type(chip, switch_8bit_bus);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, STATUS_FAIL);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 
 		retval = ms_confirm_cpu_startup(chip);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, STATUS_FAIL);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 
 		retval = ms_switch_parallel_bus(chip);
 		if (retval != STATUS_SUCCESS) {
 			if (detect_card_cd(chip, MS_CARD) != STATUS_SUCCESS) {
 				ms_set_err_code(chip, MS_NO_CARD);
-				TRACE_RET(chip, STATUS_FAIL);
+				rtsx_trace(chip);
+				return STATUS_FAIL;
 			}
 			continue;
 		} else {
@@ -739,23 +908,37 @@ static int ms_pro_reset_flow(struct rtsx_chip *chip, int switch_8bit_bus)
 		}
 	}
 
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	/* Switch MS-PRO into Parallel mode */
-	RTSX_WRITE_REG(chip, MS_CFG, 0x18, MS_BUS_WIDTH_4);
-	RTSX_WRITE_REG(chip, MS_CFG, PUSH_TIME_ODD, PUSH_TIME_ODD);
+	retval = rtsx_write_register(chip, MS_CFG, 0x18, MS_BUS_WIDTH_4);
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
+	retval = rtsx_write_register(chip, MS_CFG, PUSH_TIME_ODD,
+				     PUSH_TIME_ODD);
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
 
 	retval = ms_set_init_para(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	/* If MSPro HG Card, We shall try to switch to 8-bit bus */
 	if (CHK_MSHG(ms_card) && chip->support_ms_8bit && switch_8bit_bus) {
 		retval = ms_switch_8bit_bus(chip);
 		if (retval != STATUS_SUCCESS) {
 			ms_card->switch_8bit_fail = 1;
-			TRACE_RET(chip, STATUS_FAIL);
+			rtsx_trace(chip);
+			return STATUS_FAIL;
 		}
 	}
 
@@ -771,8 +954,10 @@ static int msxc_change_power(struct rtsx_chip *chip, u8 mode)
 	ms_cleanup_work(chip);
 
 	retval = ms_set_rw_reg_addr(chip, 0, 0, Pro_DataCount1, 6);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	buf[0] = 0;
 	buf[1] = mode;
@@ -782,16 +967,26 @@ static int msxc_change_power(struct rtsx_chip *chip, u8 mode)
 	buf[5] = 0;
 
 	retval = ms_write_bytes(chip, PRO_WRITE_REG, 6, NO_WAIT_INT, buf, 6);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	retval = ms_send_cmd(chip, XC_CHG_POWER, WAIT_INT);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
-	RTSX_READ_REG(chip, MS_TRANS_CFG, buf);
-	if (buf[0] & (MS_INT_CMDNK | MS_INT_ERR))
-		TRACE_RET(chip, STATUS_FAIL);
+	retval = rtsx_read_register(chip, MS_TRANS_CFG, buf);
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
+	if (buf[0] & (MS_INT_CMDNK | MS_INT_ERR)) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	return STATUS_SUCCESS;
 }
@@ -813,8 +1008,10 @@ static int ms_read_attribute_info(struct rtsx_chip *chip)
 #endif
 
 	retval = ms_set_rw_reg_addr(chip, Pro_IntReg, 2, Pro_SystemParm, 7);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	if (CHK_MS8BIT(ms_card))
 		data[0] = PARALLEL_8BIT_IF;
@@ -836,12 +1033,16 @@ static int ms_read_attribute_info(struct rtsx_chip *chip)
 		if (retval == STATUS_SUCCESS)
 			break;
 	}
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	buf = kmalloc(64 * 512, GFP_KERNEL);
-	if (buf == NULL)
-		TRACE_RET(chip, STATUS_ERROR);
+	if (buf == NULL) {
+		rtsx_trace(chip);
+		return STATUS_ERROR;
+	}
 
 	for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
 		retval = ms_send_cmd(chip, PRO_READ_ATRB, WAIT_INT);
@@ -851,11 +1052,13 @@ static int ms_read_attribute_info(struct rtsx_chip *chip)
 		retval = rtsx_read_register(chip, MS_TRANS_CFG, &val);
 		if (retval != STATUS_SUCCESS) {
 			kfree(buf);
-			TRACE_RET(chip, STATUS_FAIL);
+			rtsx_trace(chip);
+			return STATUS_FAIL;
 		}
 		if (!(val & MS_INT_BREQ)) {
 			kfree(buf);
-			TRACE_RET(chip, STATUS_FAIL);
+			rtsx_trace(chip);
+			return STATUS_FAIL;
 		}
 		retval = ms_transfer_data(chip, MS_TM_AUTO_READ,
 					PRO_READ_LONG_DATA, 0x40, WAIT_INT,
@@ -867,7 +1070,8 @@ static int ms_read_attribute_info(struct rtsx_chip *chip)
 	}
 	if (retval != STATUS_SUCCESS) {
 		kfree(buf);
-		TRACE_RET(chip, STATUS_FAIL);
+		rtsx_trace(chip);
+		return STATUS_FAIL;
 	}
 
 	i = 0;
@@ -875,7 +1079,8 @@ static int ms_read_attribute_info(struct rtsx_chip *chip)
 		retval = rtsx_read_register(chip, MS_TRANS_CFG, &val);
 		if (retval != STATUS_SUCCESS) {
 			kfree(buf);
-			TRACE_RET(chip, STATUS_FAIL);
+			rtsx_trace(chip);
+			return STATUS_FAIL;
 		}
 
 		if ((val & MS_INT_CED) || !(val & MS_INT_BREQ))
@@ -885,7 +1090,8 @@ static int ms_read_attribute_info(struct rtsx_chip *chip)
 					PRO_READ_LONG_DATA, 0, WAIT_INT);
 		if (retval != STATUS_SUCCESS) {
 			kfree(buf);
-			TRACE_RET(chip, STATUS_FAIL);
+			rtsx_trace(chip);
+			return STATUS_FAIL;
 		}
 
 		i++;
@@ -893,18 +1099,21 @@ static int ms_read_attribute_info(struct rtsx_chip *chip)
 
 	if (retval != STATUS_SUCCESS) {
 		kfree(buf);
-		TRACE_RET(chip, STATUS_FAIL);
+		rtsx_trace(chip);
+		return STATUS_FAIL;
 	}
 
 	if ((buf[0] != 0xa5) && (buf[1] != 0xc3)) {
 		/* Signature code is wrong */
 		kfree(buf);
-		TRACE_RET(chip, STATUS_FAIL);
+		rtsx_trace(chip);
+		return STATUS_FAIL;
 	}
 
 	if ((buf[4] < 1) || (buf[4] > 12)) {
 		kfree(buf);
-		TRACE_RET(chip, STATUS_FAIL);
+		rtsx_trace(chip);
+		return STATUS_FAIL;
 	}
 
 	for (i = 0; i < buf[4]; i++) {
@@ -929,15 +1138,18 @@ static int ms_read_attribute_info(struct rtsx_chip *chip)
 				sys_info_addr, sys_info_size);
 			if (sys_info_size != 96)  {
 				kfree(buf);
-				TRACE_RET(chip, STATUS_FAIL);
+				rtsx_trace(chip);
+				return STATUS_FAIL;
 			}
 			if (sys_info_addr < 0x1A0) {
 				kfree(buf);
-				TRACE_RET(chip, STATUS_FAIL);
+				rtsx_trace(chip);
+				return STATUS_FAIL;
 			}
 			if ((sys_info_size + sys_info_addr) > 0x8000) {
 				kfree(buf);
-				TRACE_RET(chip, STATUS_FAIL);
+				rtsx_trace(chip);
+				return STATUS_FAIL;
 			}
 
 #ifdef SUPPORT_MSXC
@@ -964,15 +1176,18 @@ static int ms_read_attribute_info(struct rtsx_chip *chip)
 				model_name_addr, model_name_size);
 			if (model_name_size != 48)  {
 				kfree(buf);
-				TRACE_RET(chip, STATUS_FAIL);
+				rtsx_trace(chip);
+				return STATUS_FAIL;
 			}
 			if (model_name_addr < 0x1A0) {
 				kfree(buf);
-				TRACE_RET(chip, STATUS_FAIL);
+				rtsx_trace(chip);
+				return STATUS_FAIL;
 			}
 			if ((model_name_size + model_name_addr) > 0x8000) {
 				kfree(buf);
-				TRACE_RET(chip, STATUS_FAIL);
+				rtsx_trace(chip);
+				return STATUS_FAIL;
 			}
 
 			found_model_name = 1;
@@ -985,7 +1200,8 @@ static int ms_read_attribute_info(struct rtsx_chip *chip)
 
 	if (i == buf[4]) {
 		kfree(buf);
-		TRACE_RET(chip, STATUS_FAIL);
+		rtsx_trace(chip);
+		return STATUS_FAIL;
 	}
 
 	class_code =  buf[sys_info_addr + 0];
@@ -1030,15 +1246,21 @@ static int ms_read_attribute_info(struct rtsx_chip *chip)
 
 #ifdef SUPPORT_MSXC
 	if (CHK_MSXC(ms_card)) {
-		if (class_code != 0x03)
-			TRACE_RET(chip, STATUS_FAIL);
+		if (class_code != 0x03) {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 	} else {
-		if (class_code != 0x02)
-			TRACE_RET(chip, STATUS_FAIL);
+		if (class_code != 0x02) {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 	}
 #else
-	if (class_code != 0x02)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (class_code != 0x02) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 #endif
 
 	if (device_type != 0x00) {
@@ -1046,12 +1268,15 @@ static int ms_read_attribute_info(struct rtsx_chip *chip)
 				(device_type == 0x03)) {
 			chip->card_wp |= MS_CARD;
 		} else {
-			TRACE_RET(chip, STATUS_FAIL);
+			rtsx_trace(chip);
+			return STATUS_FAIL;
 		}
 	}
 
-	if (sub_class & 0xC0)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (sub_class & 0xC0) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	dev_dbg(rtsx_dev(chip), "class_code: 0x%x, device_type: 0x%x, sub_class: 0x%x\n",
 		class_code, device_type, sub_class);
@@ -1099,16 +1324,21 @@ Retry:
 	if (retval != STATUS_SUCCESS) {
 		if (ms_card->switch_8bit_fail) {
 			retval = ms_pro_reset_flow(chip, 0);
-			if (retval != STATUS_SUCCESS)
-				TRACE_RET(chip, STATUS_FAIL);
+			if (retval != STATUS_SUCCESS) {
+				rtsx_trace(chip);
+				return STATUS_FAIL;
+			}
 		} else {
-			TRACE_RET(chip, STATUS_FAIL);
+			rtsx_trace(chip);
+			return STATUS_FAIL;
 		}
 	}
 
 	retval = ms_read_attribute_info(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 #ifdef XC_POWERCLASS
 	if (CHK_HG8BIT(ms_card))
@@ -1148,8 +1378,10 @@ Retry:
 
 #ifdef SUPPORT_MAGIC_GATE
 	retval = mg_set_tpc_para_sub(chip, 0, 0);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 #endif
 
 	if (CHK_HG8BIT(ms_card))
@@ -1166,16 +1398,21 @@ static int ms_read_status_reg(struct rtsx_chip *chip)
 	u8 val[2];
 
 	retval = ms_set_rw_reg_addr(chip, StatusReg0, 2, 0, 0);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	retval = ms_read_bytes(chip, READ_REG, 2, NO_WAIT_INT, val, 2);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	if (val[1] & (STS_UCDT | STS_UCEX | STS_UCFG)) {
 		ms_set_err_code(chip, MS_FLASH_READ_ERROR);
-		TRACE_RET(chip, STATUS_FAIL);
+		rtsx_trace(chip);
+		return STATUS_FAIL;
 	}
 
 	return STATUS_SUCCESS;
@@ -1191,8 +1428,10 @@ static int ms_read_extra_data(struct rtsx_chip *chip,
 
 	retval = ms_set_rw_reg_addr(chip, OverwriteFlag, MS_EXTRA_SIZE,
 				SystemParm, 6);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	if (CHK_MS4BIT(ms_card)) {
 		/* Parallel interface */
@@ -1213,8 +1452,10 @@ static int ms_read_extra_data(struct rtsx_chip *chip,
 		if (retval == STATUS_SUCCESS)
 			break;
 	}
-	if (i == MS_MAX_RETRY_COUNT)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (i == MS_MAX_RETRY_COUNT) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	ms_set_err_code(chip, MS_NO_ERROR);
 
@@ -1223,35 +1464,46 @@ static int ms_read_extra_data(struct rtsx_chip *chip,
 		if (retval == STATUS_SUCCESS)
 			break;
 	}
-	if (i == MS_MAX_RETRY_COUNT)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (i == MS_MAX_RETRY_COUNT) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	ms_set_err_code(chip, MS_NO_ERROR);
 	retval = ms_read_bytes(chip, GET_INT, 1, NO_WAIT_INT, &val, 1);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	if (val & INT_REG_CMDNK) {
 		ms_set_err_code(chip, MS_CMD_NK);
-		TRACE_RET(chip, STATUS_FAIL);
+		rtsx_trace(chip);
+		return STATUS_FAIL;
 	}
 	if (val & INT_REG_CED) {
 		if (val & INT_REG_ERR) {
 			retval = ms_read_status_reg(chip);
-			if (retval != STATUS_SUCCESS)
-				TRACE_RET(chip, STATUS_FAIL);
+			if (retval != STATUS_SUCCESS) {
+				rtsx_trace(chip);
+				return STATUS_FAIL;
+			}
 
 			retval = ms_set_rw_reg_addr(chip, OverwriteFlag,
 						MS_EXTRA_SIZE, SystemParm, 6);
-			if (retval != STATUS_SUCCESS)
-				TRACE_RET(chip, STATUS_FAIL);
+			if (retval != STATUS_SUCCESS) {
+				rtsx_trace(chip);
+				return STATUS_FAIL;
+			}
 		}
 	}
 
 	retval = ms_read_bytes(chip, READ_REG, MS_EXTRA_SIZE, NO_WAIT_INT,
 			data, MS_EXTRA_SIZE);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	if (buf && buf_len) {
 		if (buf_len > MS_EXTRA_SIZE)
@@ -1269,13 +1521,17 @@ static int ms_write_extra_data(struct rtsx_chip *chip,
 	int retval, i;
 	u8 val, data[16];
 
-	if (!buf || (buf_len < MS_EXTRA_SIZE))
-		TRACE_RET(chip, STATUS_FAIL);
+	if (!buf || (buf_len < MS_EXTRA_SIZE)) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	retval = ms_set_rw_reg_addr(chip, OverwriteFlag, MS_EXTRA_SIZE,
 				SystemParm, 6 + MS_EXTRA_SIZE);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	if (CHK_MS4BIT(ms_card))
 		data[0] = 0x88;
@@ -1293,26 +1549,34 @@ static int ms_write_extra_data(struct rtsx_chip *chip,
 
 	retval = ms_write_bytes(chip, WRITE_REG, (6 + MS_EXTRA_SIZE),
 				NO_WAIT_INT, data, 16);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	retval = ms_send_cmd(chip, BLOCK_WRITE, WAIT_INT);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	ms_set_err_code(chip, MS_NO_ERROR);
 	retval = ms_read_bytes(chip, GET_INT, 1, NO_WAIT_INT, &val, 1);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	if (val & INT_REG_CMDNK) {
 		ms_set_err_code(chip, MS_CMD_NK);
-		TRACE_RET(chip, STATUS_FAIL);
+		rtsx_trace(chip);
+		return STATUS_FAIL;
 	}
 	if (val & INT_REG_CED) {
 		if (val & INT_REG_ERR) {
 			ms_set_err_code(chip, MS_FLASH_WRITE_ERROR);
-			TRACE_RET(chip, STATUS_FAIL);
+			rtsx_trace(chip);
+			return STATUS_FAIL;
 		}
 	}
 
@@ -1328,8 +1592,10 @@ static int ms_read_page(struct rtsx_chip *chip, u16 block_addr, u8 page_num)
 
 	retval = ms_set_rw_reg_addr(chip, OverwriteFlag, MS_EXTRA_SIZE,
 				SystemParm, 6);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	if (CHK_MS4BIT(ms_card))
 		data[0] = 0x88;
@@ -1343,28 +1609,36 @@ static int ms_read_page(struct rtsx_chip *chip, u16 block_addr, u8 page_num)
 	data[5] = page_num;
 
 	retval = ms_write_bytes(chip, WRITE_REG, 6, NO_WAIT_INT, data, 6);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	retval = ms_send_cmd(chip, BLOCK_READ, WAIT_INT);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	ms_set_err_code(chip, MS_NO_ERROR);
 	retval = ms_read_bytes(chip, GET_INT, 1, NO_WAIT_INT, &val, 1);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	if (val & INT_REG_CMDNK) {
 		ms_set_err_code(chip, MS_CMD_NK);
-		TRACE_RET(chip, STATUS_FAIL);
+		rtsx_trace(chip);
+		return STATUS_FAIL;
 	}
 
 	if (val & INT_REG_CED) {
 		if (val & INT_REG_ERR) {
 			if (!(val & INT_REG_BREQ)) {
 				ms_set_err_code(chip,  MS_FLASH_READ_ERROR);
-				TRACE_RET(chip, STATUS_FAIL);
+				rtsx_trace(chip);
+				return STATUS_FAIL;
 			}
 			retval = ms_read_status_reg(chip);
 			if (retval != STATUS_SUCCESS)
@@ -1373,18 +1647,23 @@ static int ms_read_page(struct rtsx_chip *chip, u16 block_addr, u8 page_num)
 		} else {
 			if (!(val & INT_REG_BREQ)) {
 				ms_set_err_code(chip, MS_BREQ_ERROR);
-				TRACE_RET(chip, STATUS_FAIL);
+				rtsx_trace(chip);
+				return STATUS_FAIL;
 			}
 		}
 	}
 
 	retval = ms_transfer_tpc(chip, MS_TM_NORMAL_READ, READ_PAGE_DATA,
 				0, NO_WAIT_INT);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
-	if (ms_check_err_code(chip, MS_FLASH_WRITE_ERROR))
-		TRACE_RET(chip, STATUS_FAIL);
+	if (ms_check_err_code(chip, MS_FLASH_WRITE_ERROR)) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	return STATUS_SUCCESS;
 }
@@ -1397,13 +1676,17 @@ static int ms_set_bad_block(struct rtsx_chip *chip, u16 phy_blk)
 	u8 val, data[8], extra[MS_EXTRA_SIZE];
 
 	retval = ms_read_extra_data(chip, phy_blk, 0, extra, MS_EXTRA_SIZE);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	retval = ms_set_rw_reg_addr(chip, OverwriteFlag, MS_EXTRA_SIZE,
 				SystemParm, 7);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	ms_set_err_code(chip, MS_NO_ERROR);
 
@@ -1421,27 +1704,35 @@ static int ms_set_bad_block(struct rtsx_chip *chip, u16 phy_blk)
 	data[7] = 0xFF;
 
 	retval = ms_write_bytes(chip, WRITE_REG, 7, NO_WAIT_INT, data, 7);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	retval = ms_send_cmd(chip, BLOCK_WRITE, WAIT_INT);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	ms_set_err_code(chip, MS_NO_ERROR);
 	retval = ms_read_bytes(chip, GET_INT, 1, NO_WAIT_INT, &val, 1);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	if (val & INT_REG_CMDNK) {
 		ms_set_err_code(chip, MS_CMD_NK);
-		TRACE_RET(chip, STATUS_FAIL);
+		rtsx_trace(chip);
+		return STATUS_FAIL;
 	}
 
 	if (val & INT_REG_CED) {
 		if (val & INT_REG_ERR) {
 			ms_set_err_code(chip, MS_FLASH_WRITE_ERROR);
-			TRACE_RET(chip, STATUS_FAIL);
+			rtsx_trace(chip);
+			return STATUS_FAIL;
 		}
 	}
 
@@ -1457,8 +1748,10 @@ static int ms_erase_block(struct rtsx_chip *chip, u16 phy_blk)
 
 	retval = ms_set_rw_reg_addr(chip, OverwriteFlag, MS_EXTRA_SIZE,
 				SystemParm, 6);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	ms_set_err_code(chip, MS_NO_ERROR);
 
@@ -1474,18 +1767,24 @@ static int ms_erase_block(struct rtsx_chip *chip, u16 phy_blk)
 	data[5] = 0;
 
 	retval = ms_write_bytes(chip, WRITE_REG, 6, NO_WAIT_INT, data, 6);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 ERASE_RTY:
 	retval = ms_send_cmd(chip, BLOCK_ERASE, WAIT_INT);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	ms_set_err_code(chip, MS_NO_ERROR);
 	retval = ms_read_bytes(chip, GET_INT, 1, NO_WAIT_INT, &val, 1);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	if (val & INT_REG_CMDNK) {
 		if (i < 3) {
@@ -1495,13 +1794,15 @@ ERASE_RTY:
 
 		ms_set_err_code(chip, MS_CMD_NK);
 		ms_set_bad_block(chip, phy_blk);
-		TRACE_RET(chip, STATUS_FAIL);
+		rtsx_trace(chip);
+		return STATUS_FAIL;
 	}
 
 	if (val & INT_REG_CED) {
 		if (val & INT_REG_ERR) {
 			ms_set_err_code(chip, MS_FLASH_WRITE_ERROR);
-			TRACE_RET(chip, STATUS_FAIL);
+			rtsx_trace(chip);
+			return STATUS_FAIL;
 		}
 	}
 
@@ -1544,13 +1845,16 @@ static int ms_init_page(struct rtsx_chip *chip, u16 phy_blk, u16 log_blk,
 	for (i = start_page; i < end_page; i++) {
 		if (detect_card_cd(chip, MS_CARD) != STATUS_SUCCESS) {
 			ms_set_err_code(chip, MS_NO_CARD);
-			TRACE_RET(chip, STATUS_FAIL);
+			rtsx_trace(chip);
+			return STATUS_FAIL;
 		}
 
 		retval = ms_write_extra_data(chip, phy_blk, i,
 					extra, MS_EXTRA_SIZE);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, STATUS_FAIL);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 	}
 
 	return STATUS_SUCCESS;
@@ -1560,7 +1864,8 @@ static int ms_copy_page(struct rtsx_chip *chip, u16 old_blk, u16 new_blk,
 		u16 log_blk, u8 start_page, u8 end_page)
 {
 	struct ms_info *ms_card = &(chip->ms_card);
-	int retval, rty_cnt, uncorrect_flag = 0;
+	bool uncorrect_flag = false;
+	int retval, rty_cnt;
 	u8 extra[MS_EXTRA_SIZE], val, i, j, data[16];
 
 	dev_dbg(rtsx_dev(chip), "Copy page from 0x%x to 0x%x, logical block is 0x%x\n",
@@ -1569,42 +1874,58 @@ static int ms_copy_page(struct rtsx_chip *chip, u16 old_blk, u16 new_blk,
 		start_page, end_page);
 
 	retval = ms_read_extra_data(chip, new_blk, 0, extra, MS_EXTRA_SIZE);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	retval = ms_read_status_reg(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
-	RTSX_READ_REG(chip, PPBUF_BASE2, &val);
+	retval = rtsx_read_register(chip, PPBUF_BASE2, &val);
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
 
 	if (val & BUF_FULL) {
 		retval = ms_send_cmd(chip, CLEAR_BUF, WAIT_INT);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, STATUS_FAIL);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 
 		retval = ms_read_bytes(chip, GET_INT, 1, NO_WAIT_INT, &val, 1);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, STATUS_FAIL);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 
 		if (!(val & INT_REG_CED)) {
 			ms_set_err_code(chip, MS_FLASH_WRITE_ERROR);
-			TRACE_RET(chip, STATUS_FAIL);
+			rtsx_trace(chip);
+			return STATUS_FAIL;
 		}
 	}
 
 	for (i = start_page; i < end_page; i++) {
 		if (detect_card_cd(chip, MS_CARD) != STATUS_SUCCESS) {
 			ms_set_err_code(chip, MS_NO_CARD);
-			TRACE_RET(chip, STATUS_FAIL);
+			rtsx_trace(chip);
+			return STATUS_FAIL;
 		}
 
 		ms_read_extra_data(chip, old_blk, i, extra, MS_EXTRA_SIZE);
 
 		retval = ms_set_rw_reg_addr(chip, OverwriteFlag,
 					MS_EXTRA_SIZE, SystemParm, 6);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, STATUS_FAIL);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 
 		ms_set_err_code(chip, MS_NO_ERROR);
 
@@ -1621,39 +1942,48 @@ static int ms_copy_page(struct rtsx_chip *chip, u16 old_blk, u16 new_blk,
 
 		retval = ms_write_bytes(chip, WRITE_REG, 6, NO_WAIT_INT,
 					data, 6);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, STATUS_FAIL);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 
 		retval = ms_send_cmd(chip, BLOCK_READ, WAIT_INT);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, STATUS_FAIL);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 
 		ms_set_err_code(chip, MS_NO_ERROR);
 		retval = ms_read_bytes(chip, GET_INT, 1, NO_WAIT_INT, &val, 1);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, STATUS_FAIL);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 
 		if (val & INT_REG_CMDNK) {
 			ms_set_err_code(chip, MS_CMD_NK);
-			TRACE_RET(chip, STATUS_FAIL);
+			rtsx_trace(chip);
+			return STATUS_FAIL;
 		}
 
 		if (val & INT_REG_CED) {
 			if (val & INT_REG_ERR) {
 				retval = ms_read_status_reg(chip);
 				if (retval != STATUS_SUCCESS) {
-					uncorrect_flag = 1;
+					uncorrect_flag = true;
 					dev_dbg(rtsx_dev(chip), "Uncorrectable error\n");
 				} else {
-					uncorrect_flag = 0;
+					uncorrect_flag = false;
 				}
 
 				retval = ms_transfer_tpc(chip,
 							MS_TM_NORMAL_READ,
 							READ_PAGE_DATA,
 							0, NO_WAIT_INT);
-				if (retval != STATUS_SUCCESS)
-					TRACE_RET(chip, STATUS_FAIL);
+				if (retval != STATUS_SUCCESS) {
+					rtsx_trace(chip);
+					return STATUS_FAIL;
+				}
 
 				if (uncorrect_flag) {
 					ms_set_page_status(log_blk, setPS_NG,
@@ -1684,13 +2014,16 @@ static int ms_copy_page(struct rtsx_chip *chip, u16 old_blk, u16 new_blk,
 					if (retval == STATUS_SUCCESS)
 						break;
 				}
-				if (rty_cnt == MS_MAX_RETRY_COUNT)
-					TRACE_RET(chip, STATUS_FAIL);
+				if (rty_cnt == MS_MAX_RETRY_COUNT) {
+					rtsx_trace(chip);
+					return STATUS_FAIL;
+				}
 			}
 
 			if (!(val & INT_REG_BREQ)) {
 				ms_set_err_code(chip, MS_BREQ_ERROR);
-				TRACE_RET(chip, STATUS_FAIL);
+				rtsx_trace(chip);
+				return STATUS_FAIL;
 			}
 		}
 
@@ -1724,35 +2057,45 @@ static int ms_copy_page(struct rtsx_chip *chip, u16 old_blk, u16 new_blk,
 
 		retval = ms_write_bytes(chip, WRITE_REG, (6 + MS_EXTRA_SIZE),
 					NO_WAIT_INT, data, 16);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, STATUS_FAIL);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 
 		retval = ms_send_cmd(chip, BLOCK_WRITE, WAIT_INT);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, STATUS_FAIL);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 
 		ms_set_err_code(chip, MS_NO_ERROR);
 		retval = ms_read_bytes(chip, GET_INT, 1, NO_WAIT_INT, &val, 1);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, STATUS_FAIL);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 
 		if (val & INT_REG_CMDNK) {
 			ms_set_err_code(chip, MS_CMD_NK);
-			TRACE_RET(chip, STATUS_FAIL);
+			rtsx_trace(chip);
+			return STATUS_FAIL;
 		}
 
 		if (val & INT_REG_CED) {
 			if (val & INT_REG_ERR) {
 				ms_set_err_code(chip, MS_FLASH_WRITE_ERROR);
-				TRACE_RET(chip, STATUS_FAIL);
+				rtsx_trace(chip);
+				return STATUS_FAIL;
 			}
 		}
 
 		if (i == 0) {
 			retval = ms_set_rw_reg_addr(chip, OverwriteFlag,
 						MS_EXTRA_SIZE, SystemParm, 7);
-			if (retval != STATUS_SUCCESS)
-				TRACE_RET(chip, STATUS_FAIL);
+			if (retval != STATUS_SUCCESS) {
+				rtsx_trace(chip);
+				return STATUS_FAIL;
+			}
 
 			ms_set_err_code(chip, MS_NO_ERROR);
 
@@ -1771,29 +2114,37 @@ static int ms_copy_page(struct rtsx_chip *chip, u16 old_blk, u16 new_blk,
 
 			retval = ms_write_bytes(chip, WRITE_REG, 7,
 						NO_WAIT_INT, data, 8);
-			if (retval != STATUS_SUCCESS)
-				TRACE_RET(chip, STATUS_FAIL);
+			if (retval != STATUS_SUCCESS) {
+				rtsx_trace(chip);
+				return STATUS_FAIL;
+			}
 
 			retval = ms_send_cmd(chip, BLOCK_WRITE, WAIT_INT);
-			if (retval != STATUS_SUCCESS)
-				TRACE_RET(chip, STATUS_FAIL);
+			if (retval != STATUS_SUCCESS) {
+				rtsx_trace(chip);
+				return STATUS_FAIL;
+			}
 
 			ms_set_err_code(chip, MS_NO_ERROR);
 			retval = ms_read_bytes(chip, GET_INT, 1,
 					NO_WAIT_INT, &val, 1);
-			if (retval != STATUS_SUCCESS)
-				TRACE_RET(chip, STATUS_FAIL);
+			if (retval != STATUS_SUCCESS) {
+				rtsx_trace(chip);
+				return STATUS_FAIL;
+			}
 
 			if (val & INT_REG_CMDNK) {
 				ms_set_err_code(chip, MS_CMD_NK);
-				TRACE_RET(chip, STATUS_FAIL);
+				rtsx_trace(chip);
+				return STATUS_FAIL;
 			}
 
 			if (val & INT_REG_CED) {
 				if (val & INT_REG_ERR) {
 					ms_set_err_code(chip,
 							MS_FLASH_WRITE_ERROR);
-					TRACE_RET(chip, STATUS_FAIL);
+					rtsx_trace(chip);
+					return STATUS_FAIL;
 				}
 			}
 		}
@@ -1814,20 +2165,30 @@ static int reset_ms(struct rtsx_chip *chip)
 #endif
 
 	retval = ms_prepare_reset(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	ms_card->ms_type |= TYPE_MS;
 
 	retval = ms_send_cmd(chip, MS_RESET, NO_WAIT_INT);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	retval = ms_read_status_reg(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
-	RTSX_READ_REG(chip, PPBUF_BASE2, &val);
+	retval = rtsx_read_register(chip, PPBUF_BASE2, &val);
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
 	if (val & WRT_PRTCT)
 		chip->card_wp |= MS_CARD;
 	else
@@ -1840,7 +2201,8 @@ RE_SEARCH:
 	while (i < (MAX_DEFECTIVE_BLOCK + 2)) {
 		if (detect_card_cd(chip, MS_CARD) != STATUS_SUCCESS) {
 			ms_set_err_code(chip, MS_NO_CARD);
-			TRACE_RET(chip, STATUS_FAIL);
+			rtsx_trace(chip);
+			return STATUS_FAIL;
 		}
 
 		retval = ms_read_extra_data(chip, i, 0, extra, MS_EXTRA_SIZE);
@@ -1860,7 +2222,8 @@ RE_SEARCH:
 
 	if (i == (MAX_DEFECTIVE_BLOCK + 2)) {
 		dev_dbg(rtsx_dev(chip), "No boot block found!");
-		TRACE_RET(chip, STATUS_FAIL);
+		rtsx_trace(chip);
+		return STATUS_FAIL;
 	}
 
 	for (j = 0; j < 3; j++) {
@@ -1875,8 +2238,10 @@ RE_SEARCH:
 	}
 
 	retval = ms_read_page(chip, ms_card->boot_block, 0);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	/* Read MS system information as sys_info */
 	rtsx_init_cmd(chip);
@@ -1885,8 +2250,10 @@ RE_SEARCH:
 		rtsx_add_cmd(chip, READ_REG_CMD, PPBUF_BASE2 + 0x1A0 + i, 0, 0);
 
 	retval = rtsx_send_cmd(chip, MS_CARD, 100);
-	if (retval < 0)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval < 0) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	ptr = rtsx_get_cmd_data(chip);
 	memcpy(ms_card->raw_sys_info, ptr, 96);
@@ -1908,8 +2275,10 @@ RE_SEARCH:
 	rtsx_add_cmd(chip, READ_REG_CMD, MS_4bit_Support, 0, 0);
 
 	retval = rtsx_send_cmd(chip, MS_CARD, 100);
-	if (retval < 0)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval < 0) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	ptr = rtsx_get_cmd_data(chip);
 
@@ -1982,19 +2351,36 @@ RE_SEARCH:
 	/* Switch I/F Mode */
 	if (ptr[15]) {
 		retval = ms_set_rw_reg_addr(chip, 0, 0, SystemParm, 1);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, STATUS_FAIL);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 
-		RTSX_WRITE_REG(chip, PPBUF_BASE2, 0xFF, 0x88);
-		RTSX_WRITE_REG(chip, PPBUF_BASE2 + 1, 0xFF, 0);
+		retval = rtsx_write_register(chip, PPBUF_BASE2, 0xFF, 0x88);
+		if (retval) {
+			rtsx_trace(chip);
+			return retval;
+		}
+		retval = rtsx_write_register(chip, PPBUF_BASE2 + 1, 0xFF, 0);
+		if (retval) {
+			rtsx_trace(chip);
+			return retval;
+		}
 
 		retval = ms_transfer_tpc(chip, MS_TM_WRITE_BYTES, WRITE_REG, 1,
 					NO_WAIT_INT);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, STATUS_FAIL);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 
-		RTSX_WRITE_REG(chip, MS_CFG, 0x58 | MS_NO_CHECK_INT,
-			MS_BUS_WIDTH_4 | PUSH_TIME_ODD | MS_NO_CHECK_INT);
+		retval = rtsx_write_register(chip, MS_CFG,
+					     0x58 | MS_NO_CHECK_INT,
+					     MS_BUS_WIDTH_4 | PUSH_TIME_ODD | MS_NO_CHECK_INT);
+		if (retval) {
+			rtsx_trace(chip);
+			return retval;
+		}
 
 		ms_card->ms_type |= MS_4BIT;
 	}
@@ -2020,29 +2406,41 @@ static int ms_init_l2p_tbl(struct rtsx_chip *chip)
 
 	size = ms_card->segment_cnt * sizeof(struct zone_entry);
 	ms_card->segment = vzalloc(size);
-	if (ms_card->segment == NULL)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (ms_card->segment == NULL) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	retval = ms_read_page(chip, ms_card->boot_block, 1);
-	if (retval != STATUS_SUCCESS)
-		TRACE_GOTO(chip, INIT_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		goto INIT_FAIL;
+	}
 
 	reg_addr = PPBUF_BASE2;
 	for (i = 0; i < (((ms_card->total_block >> 9) * 10) + 1); i++) {
+		int block_no;
+
 		retval = rtsx_read_register(chip, reg_addr++, &val1);
-		if (retval != STATUS_SUCCESS)
-			TRACE_GOTO(chip, INIT_FAIL);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			goto INIT_FAIL;
+		}
 
 		retval = rtsx_read_register(chip, reg_addr++, &val2);
-		if (retval != STATUS_SUCCESS)
-			TRACE_GOTO(chip, INIT_FAIL);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			goto INIT_FAIL;
+		}
 
 		defect_block = ((u16)val1 << 8) | val2;
 		if (defect_block == 0xFFFF)
 			break;
 
 		seg_no = defect_block / 512;
-		ms_card->segment[seg_no].defect_list[ms_card->segment[seg_no].disable_count++] = defect_block;
+
+		block_no = ms_card->segment[seg_no].disable_count++;
+		ms_card->segment[seg_no].defect_list[block_no] = defect_block;
 	}
 
 	for (i = 0; i < ms_card->segment_cnt; i++) {
@@ -2187,16 +2585,19 @@ static int ms_build_l2p_tbl(struct rtsx_chip *chip, int seg_no)
 {
 	struct ms_info *ms_card = &(chip->ms_card);
 	struct zone_entry *segment;
-	int retval, table_size, disable_cnt, defect_flag, i;
-	u16 start, end, phy_blk, log_blk, tmp_blk;
+	bool defect_flag;
+	int retval, table_size, disable_cnt, i;
+	u16 start, end, phy_blk, log_blk, tmp_blk, idx;
 	u8 extra[MS_EXTRA_SIZE], us1, us2;
 
 	dev_dbg(rtsx_dev(chip), "ms_build_l2p_tbl: %d\n", seg_no);
 
 	if (ms_card->segment == NULL) {
 		retval = ms_init_l2p_tbl(chip);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			return retval;
+		}
 	}
 
 	if (ms_card->segment[seg_no].build_flag) {
@@ -2214,15 +2615,19 @@ static int ms_build_l2p_tbl(struct rtsx_chip *chip, int seg_no)
 
 	if (segment->l2p_table == NULL) {
 		segment->l2p_table = vmalloc(table_size * 2);
-		if (segment->l2p_table == NULL)
-			TRACE_GOTO(chip, BUILD_FAIL);
+		if (segment->l2p_table == NULL) {
+			rtsx_trace(chip);
+			goto BUILD_FAIL;
+		}
 	}
 	memset((u8 *)(segment->l2p_table), 0xff, table_size * 2);
 
 	if (segment->free_table == NULL) {
 		segment->free_table = vmalloc(MS_FREE_TABLE_CNT * 2);
-		if (segment->free_table == NULL)
-			TRACE_GOTO(chip, BUILD_FAIL);
+		if (segment->free_table == NULL) {
+			rtsx_trace(chip);
+			goto BUILD_FAIL;
+		}
 	}
 	memset((u8 *)(segment->free_table), 0xff, MS_FREE_TABLE_CNT * 2);
 
@@ -2236,10 +2641,10 @@ static int ms_build_l2p_tbl(struct rtsx_chip *chip, int seg_no)
 
 	for (phy_blk = start; phy_blk < end; phy_blk++) {
 		if (disable_cnt) {
-			defect_flag = 0;
+			defect_flag = false;
 			for (i = 0; i < segment->disable_count; i++) {
 				if (phy_blk == segment->defect_list[i]) {
-					defect_flag = 1;
+					defect_flag = true;
 					break;
 				}
 			}
@@ -2299,13 +2704,15 @@ static int ms_build_l2p_tbl(struct rtsx_chip *chip, int seg_no)
 			continue;
 		}
 
-		if (segment->l2p_table[log_blk - ms_start_idx[seg_no]] == 0xFFFF) {
-			segment->l2p_table[log_blk - ms_start_idx[seg_no]] = phy_blk;
+		idx = log_blk - ms_start_idx[seg_no];
+
+		if (segment->l2p_table[idx] == 0xFFFF) {
+			segment->l2p_table[idx] = phy_blk;
 			continue;
 		}
 
 		us1 = extra[0] & 0x10;
-		tmp_blk = segment->l2p_table[log_blk - ms_start_idx[seg_no]];
+		tmp_blk = segment->l2p_table[idx];
 		retval = ms_read_extra_data(chip, tmp_blk, 0,
 					extra, MS_EXTRA_SIZE);
 		if (retval != STATUS_SUCCESS)
@@ -2336,17 +2743,20 @@ static int ms_build_l2p_tbl(struct rtsx_chip *chip, int seg_no)
 
 	for (log_blk = ms_start_idx[seg_no];
 	     log_blk < ms_start_idx[seg_no + 1]; log_blk++) {
-		if (segment->l2p_table[log_blk-ms_start_idx[seg_no]] == 0xFFFF) {
+		idx = log_blk - ms_start_idx[seg_no];
+		if (segment->l2p_table[idx] == 0xFFFF) {
 			phy_blk = ms_get_unused_block(chip, seg_no);
 			if (phy_blk == 0xFFFF) {
 				chip->card_wp |= MS_CARD;
 				return STATUS_SUCCESS;
 			}
 			retval = ms_init_page(chip, phy_blk, log_blk, 0, 1);
-			if (retval != STATUS_SUCCESS)
-				TRACE_GOTO(chip, BUILD_FAIL);
+			if (retval != STATUS_SUCCESS) {
+				rtsx_trace(chip);
+				goto BUILD_FAIL;
+			}
 
-			segment->l2p_table[log_blk-ms_start_idx[seg_no]] = phy_blk;
+			segment->l2p_table[idx] = phy_blk;
 			if (seg_no == ms_card->segment_cnt - 1) {
 				if (segment->unused_blk_cnt < 2) {
 					chip->card_wp |= MS_CARD;
@@ -2374,14 +2784,18 @@ static int ms_build_l2p_tbl(struct rtsx_chip *chip, int seg_no)
 				phy_blk = ms_get_unused_block(chip, 0);
 				retval = ms_copy_page(chip, tmp_blk, phy_blk,
 						log_blk, 0, ms_card->page_off + 1);
-				if (retval != STATUS_SUCCESS)
-					TRACE_RET(chip, STATUS_FAIL);
+				if (retval != STATUS_SUCCESS) {
+					rtsx_trace(chip);
+					return STATUS_FAIL;
+				}
 
 				segment->l2p_table[log_blk] = phy_blk;
 
 				retval = ms_set_bad_block(chip, tmp_blk);
-				if (retval != STATUS_SUCCESS)
-					TRACE_RET(chip, STATUS_FAIL);
+				if (retval != STATUS_SUCCESS) {
+					rtsx_trace(chip);
+					return STATUS_FAIL;
+				}
 			}
 		}
 	}
@@ -2411,12 +2825,16 @@ int reset_ms_card(struct rtsx_chip *chip)
 	memset(ms_card, 0, sizeof(struct ms_info));
 
 	retval = enable_card_clock(chip, MS_CARD);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	retval = select_card(chip, MS_CARD);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	ms_card->ms_type = 0;
 
@@ -2424,24 +2842,31 @@ int reset_ms_card(struct rtsx_chip *chip)
 	if (retval != STATUS_SUCCESS) {
 		if (ms_card->check_ms_flow) {
 			retval = reset_ms(chip);
-			if (retval != STATUS_SUCCESS)
-				TRACE_RET(chip, STATUS_FAIL);
+			if (retval != STATUS_SUCCESS) {
+				rtsx_trace(chip);
+				return STATUS_FAIL;
+			}
 		} else {
-			TRACE_RET(chip, STATUS_FAIL);
+			rtsx_trace(chip);
+			return STATUS_FAIL;
 		}
 	}
 
 	retval = ms_set_init_para(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	if (!CHK_MSPRO(ms_card)) {
 		/* Build table for the last segment,
 		 * to check if L2P table block exists, erasing it
 		 */
 		retval = ms_build_l2p_tbl(chip, ms_card->total_block / 512 - 1);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, STATUS_FAIL);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 	}
 
 	dev_dbg(rtsx_dev(chip), "ms_card->ms_type = 0x%x\n", ms_card->ms_type);
@@ -2470,8 +2895,10 @@ static int mspro_set_rw_cmd(struct rtsx_chip *chip,
 		if (retval == STATUS_SUCCESS)
 			break;
 	}
-	if (i == MS_MAX_RETRY_COUNT)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (i == MS_MAX_RETRY_COUNT) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	return STATUS_SUCCESS;
 }
@@ -2511,8 +2938,10 @@ static inline int ms_auto_tune_clock(struct rtsx_chip *chip)
 	}
 
 	retval = ms_switch_clock(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	return STATUS_SUCCESS;
 }
@@ -2522,7 +2951,8 @@ static int mspro_rw_multi_sector(struct scsi_cmnd *srb,
 				u16 sector_cnt)
 {
 	struct ms_info *ms_card = &(chip->ms_card);
-	int retval, mode_2k = 0;
+	bool mode_2k = false;
+	int retval;
 	u16 count;
 	u8 val, trans_mode, rw_tpc, rw_cmd;
 
@@ -2547,7 +2977,7 @@ static int mspro_rw_multi_sector(struct scsi_cmnd *srb,
 				rw_tpc = PRO_WRITE_QUAD_DATA;
 				rw_cmd = PRO_WRITE_2K_DATA;
 			}
-			mode_2k = 1;
+			mode_2k = true;
 		}
 	} else {
 		if (srb->sc_data_direction == DMA_FROM_DEVICE) {
@@ -2560,15 +2990,21 @@ static int mspro_rw_multi_sector(struct scsi_cmnd *srb,
 	}
 
 	retval = ms_switch_clock(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	if (srb->sc_data_direction == DMA_FROM_DEVICE)
 		trans_mode = MS_TM_AUTO_READ;
 	else
 		trans_mode = MS_TM_AUTO_WRITE;
 
-	RTSX_READ_REG(chip, MS_TRANS_CFG, &val);
+	retval = rtsx_read_register(chip, MS_TRANS_CFG, &val);
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
 
 	if (ms_card->seq_mode) {
 		if ((ms_card->pre_dir != srb->sc_data_direction)
@@ -2581,8 +3017,10 @@ static int mspro_rw_multi_sector(struct scsi_cmnd *srb,
 			ms_card->total_sec_cnt = 0;
 			if (val & MS_INT_BREQ) {
 				retval = ms_send_cmd(chip, PRO_STOP, WAIT_INT);
-				if (retval != STATUS_SUCCESS)
-					TRACE_RET(chip, STATUS_FAIL);
+				if (retval != STATUS_SUCCESS) {
+					rtsx_trace(chip);
+					return STATUS_FAIL;
+				}
 
 				rtsx_write_register(chip, RBCTL, RB_FLUSH, RB_FLUSH);
 			}
@@ -2609,7 +3047,8 @@ static int mspro_rw_multi_sector(struct scsi_cmnd *srb,
 		retval = mspro_set_rw_cmd(chip, start_sector, count, rw_cmd);
 		if (retval != STATUS_SUCCESS) {
 			ms_card->seq_mode = 0;
-			TRACE_RET(chip, STATUS_FAIL);
+			rtsx_trace(chip);
+			return STATUS_FAIL;
 		}
 	}
 
@@ -2624,7 +3063,8 @@ static int mspro_rw_multi_sector(struct scsi_cmnd *srb,
 		if (detect_card_cd(chip, MS_CARD) != STATUS_SUCCESS) {
 			chip->rw_need_retry = 0;
 			dev_dbg(rtsx_dev(chip), "No card exist, exit mspro_rw_multi_sector\n");
-			TRACE_RET(chip, STATUS_FAIL);
+			rtsx_trace(chip);
+			return STATUS_FAIL;
 		}
 
 		if (val & MS_INT_BREQ)
@@ -2636,7 +3076,8 @@ static int mspro_rw_multi_sector(struct scsi_cmnd *srb,
 			ms_auto_tune_clock(chip);
 		}
 
-		TRACE_RET(chip, retval);
+		rtsx_trace(chip);
+		return retval;
 	}
 
 	if (ms_card->seq_mode) {
@@ -2664,13 +3105,15 @@ static int mspro_read_format_progress(struct rtsx_chip *chip,
 	retval = ms_switch_clock(chip);
 	if (retval != STATUS_SUCCESS) {
 		ms_card->format_status = FORMAT_FAIL;
-		TRACE_RET(chip, STATUS_FAIL);
+		rtsx_trace(chip);
+		return STATUS_FAIL;
 	}
 
 	retval = rtsx_read_register(chip, MS_TRANS_CFG, &tmp);
 	if (retval != STATUS_SUCCESS) {
 		ms_card->format_status = FORMAT_FAIL;
-		TRACE_RET(chip, STATUS_FAIL);
+		rtsx_trace(chip);
+		return STATUS_FAIL;
 	}
 
 	if (!(tmp & MS_INT_BREQ)) {
@@ -2679,7 +3122,8 @@ static int mspro_read_format_progress(struct rtsx_chip *chip,
 			return STATUS_SUCCESS;
 		}
 		ms_card->format_status = FORMAT_FAIL;
-		TRACE_RET(chip, STATUS_FAIL);
+		rtsx_trace(chip);
+		return STATUS_FAIL;
 	}
 
 	if (short_data_len >= 256)
@@ -2691,14 +3135,16 @@ static int mspro_read_format_progress(struct rtsx_chip *chip,
 				MS_NO_CHECK_INT);
 	if (retval != STATUS_SUCCESS) {
 		ms_card->format_status = FORMAT_FAIL;
-		TRACE_RET(chip, STATUS_FAIL);
+		rtsx_trace(chip);
+		return STATUS_FAIL;
 	}
 
 	retval = ms_read_bytes(chip, PRO_READ_SHORT_DATA, cnt, WAIT_INT,
 			data, 8);
 	if (retval != STATUS_SUCCESS) {
 		ms_card->format_status = FORMAT_FAIL;
-		TRACE_RET(chip, STATUS_FAIL);
+		rtsx_trace(chip);
+		return STATUS_FAIL;
 	}
 
 	total_progress = (data[0] << 24) | (data[1] << 16) |
@@ -2723,7 +3169,8 @@ static int mspro_read_format_progress(struct rtsx_chip *chip,
 		retval = rtsx_read_register(chip, MS_TRANS_CFG, &tmp);
 		if (retval != STATUS_SUCCESS) {
 			ms_card->format_status = FORMAT_FAIL;
-			TRACE_RET(chip, STATUS_FAIL);
+			rtsx_trace(chip);
+			return STATUS_FAIL;
 		}
 		if (tmp & (MS_INT_CED | MS_INT_CMDNK |
 				MS_INT_BREQ | MS_INT_ERR))
@@ -2735,17 +3182,20 @@ static int mspro_read_format_progress(struct rtsx_chip *chip,
 	retval = rtsx_write_register(chip, MS_CFG, MS_NO_CHECK_INT, 0);
 	if (retval != STATUS_SUCCESS) {
 		ms_card->format_status = FORMAT_FAIL;
-		TRACE_RET(chip, STATUS_FAIL);
+		rtsx_trace(chip);
+		return STATUS_FAIL;
 	}
 
 	if (i == 5000) {
 		ms_card->format_status = FORMAT_FAIL;
-		TRACE_RET(chip, STATUS_FAIL);
+		rtsx_trace(chip);
+		return STATUS_FAIL;
 	}
 
 	if (tmp & (MS_INT_CMDNK | MS_INT_ERR)) {
 		ms_card->format_status = FORMAT_FAIL;
-		TRACE_RET(chip, STATUS_FAIL);
+		rtsx_trace(chip);
+		return STATUS_FAIL;
 	}
 
 	if (tmp & MS_INT_CED) {
@@ -2756,7 +3206,8 @@ static int mspro_read_format_progress(struct rtsx_chip *chip,
 	} else {
 		ms_card->format_status = FORMAT_FAIL;
 		ms_card->pro_under_formatting = 0;
-		TRACE_RET(chip, STATUS_FAIL);
+		rtsx_trace(chip);
+		return STATUS_FAIL;
 	}
 
 	return STATUS_SUCCESS;
@@ -2780,7 +3231,7 @@ void mspro_polling_format_status(struct rtsx_chip *chip)
 }
 
 int mspro_format(struct scsi_cmnd *srb, struct rtsx_chip *chip,
-		int short_data_len, int quick_format)
+		int short_data_len, bool quick_format)
 {
 	struct ms_info *ms_card = &(chip->ms_card);
 	int retval, i;
@@ -2788,12 +3239,16 @@ int mspro_format(struct scsi_cmnd *srb, struct rtsx_chip *chip,
 	u16 para;
 
 	retval = ms_switch_clock(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	retval = ms_set_rw_reg_addr(chip, 0x00, 0x00, Pro_TPCParm, 0x01);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	memset(buf, 0, 2);
 	switch (short_data_len) {
@@ -2818,8 +3273,10 @@ int mspro_format(struct scsi_cmnd *srb, struct rtsx_chip *chip,
 		if (retval == STATUS_SUCCESS)
 			break;
 	}
-	if (i == MS_MAX_RETRY_COUNT)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (i == MS_MAX_RETRY_COUNT) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	if (quick_format)
 		para = 0x0000;
@@ -2827,13 +3284,21 @@ int mspro_format(struct scsi_cmnd *srb, struct rtsx_chip *chip,
 		para = 0x0001;
 
 	retval = mspro_set_rw_cmd(chip, 0, para, PRO_FORMAT);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
-	RTSX_READ_REG(chip, MS_TRANS_CFG, &tmp);
+	retval = rtsx_read_register(chip, MS_TRANS_CFG, &tmp);
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
 
-	if (tmp & (MS_INT_CMDNK | MS_INT_ERR))
-		TRACE_RET(chip, STATUS_FAIL);
+	if (tmp & (MS_INT_CMDNK | MS_INT_ERR)) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	if ((tmp & (MS_INT_BREQ | MS_INT_CED)) == MS_INT_BREQ) {
 		ms_card->pro_under_formatting = 1;
@@ -2850,7 +3315,8 @@ int mspro_format(struct scsi_cmnd *srb, struct rtsx_chip *chip,
 		return STATUS_SUCCESS;
 	}
 
-	TRACE_RET(chip, STATUS_FAIL);
+	rtsx_trace(chip);
+	return STATUS_FAIL;
 }
 
 
@@ -2869,14 +3335,17 @@ static int ms_read_multiple_pages(struct rtsx_chip *chip, u16 phy_blk,
 	if (retval == STATUS_SUCCESS) {
 		if ((extra[1] & 0x30) != 0x30) {
 			ms_set_err_code(chip, MS_FLASH_READ_ERROR);
-			TRACE_RET(chip, STATUS_FAIL);
+			rtsx_trace(chip);
+			return STATUS_FAIL;
 		}
 	}
 
 	retval = ms_set_rw_reg_addr(chip, OverwriteFlag, MS_EXTRA_SIZE,
 				SystemParm, 6);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	if (CHK_MS4BIT(ms_card))
 		data[0] = 0x88;
@@ -2895,14 +3364,18 @@ static int ms_read_multiple_pages(struct rtsx_chip *chip, u16 phy_blk,
 		if (retval == STATUS_SUCCESS)
 			break;
 	}
-	if (i == MS_MAX_RETRY_COUNT)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (i == MS_MAX_RETRY_COUNT) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	ms_set_err_code(chip, MS_NO_ERROR);
 
 	retval = ms_send_cmd(chip, BLOCK_READ, WAIT_INT);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	ptr = buf;
 
@@ -2911,16 +3384,20 @@ static int ms_read_multiple_pages(struct rtsx_chip *chip, u16 phy_blk,
 
 		if (detect_card_cd(chip, MS_CARD) != STATUS_SUCCESS) {
 			ms_set_err_code(chip, MS_NO_CARD);
-			TRACE_RET(chip, STATUS_FAIL);
+			rtsx_trace(chip);
+			return STATUS_FAIL;
 		}
 
 		retval = ms_read_bytes(chip, GET_INT, 1, NO_WAIT_INT, &val, 1);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, STATUS_FAIL);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 
 		if (val & INT_REG_CMDNK) {
 			ms_set_err_code(chip, MS_CMD_NK);
-			TRACE_RET(chip, STATUS_FAIL);
+			rtsx_trace(chip);
+			return STATUS_FAIL;
 		}
 		if (val & INT_REG_ERR) {
 			if (val & INT_REG_BREQ) {
@@ -2933,34 +3410,42 @@ static int ms_read_multiple_pages(struct rtsx_chip *chip, u16 phy_blk,
 								page_addr, extra, MS_EXTRA_SIZE);
 					}
 					ms_set_err_code(chip, MS_FLASH_READ_ERROR);
-					TRACE_RET(chip, STATUS_FAIL);
+					rtsx_trace(chip);
+					return STATUS_FAIL;
 				}
 			} else {
 				ms_set_err_code(chip, MS_FLASH_READ_ERROR);
-				TRACE_RET(chip, STATUS_FAIL);
+				rtsx_trace(chip);
+				return STATUS_FAIL;
 			}
 		} else {
 			if (!(val & INT_REG_BREQ)) {
 				ms_set_err_code(chip, MS_BREQ_ERROR);
-				TRACE_RET(chip, STATUS_FAIL);
+				rtsx_trace(chip);
+				return STATUS_FAIL;
 			}
 		}
 
 		if (page_addr == (end_page - 1)) {
 			if (!(val & INT_REG_CED)) {
 				retval = ms_send_cmd(chip, BLOCK_END, WAIT_INT);
-				if (retval != STATUS_SUCCESS)
-					TRACE_RET(chip, STATUS_FAIL);
+				if (retval != STATUS_SUCCESS) {
+					rtsx_trace(chip);
+					return STATUS_FAIL;
+				}
 			}
 
 			retval = ms_read_bytes(chip, GET_INT, 1, NO_WAIT_INT,
 					&val, 1);
-			if (retval != STATUS_SUCCESS)
-				TRACE_RET(chip, STATUS_FAIL);
+			if (retval != STATUS_SUCCESS) {
+				rtsx_trace(chip);
+				return STATUS_FAIL;
+			}
 
 			if (!(val & INT_REG_CED)) {
 				ms_set_err_code(chip, MS_FLASH_READ_ERROR);
-				TRACE_RET(chip, STATUS_FAIL);
+				rtsx_trace(chip);
+				return STATUS_FAIL;
 			}
 
 			trans_cfg = NO_WAIT_INT;
@@ -2993,19 +3478,22 @@ static int ms_read_multiple_pages(struct rtsx_chip *chip, u16 phy_blk,
 			if (retval == -ETIMEDOUT) {
 				ms_set_err_code(chip, MS_TO_ERROR);
 				rtsx_clear_ms_error(chip);
-				TRACE_RET(chip, STATUS_TIMEDOUT);
+				rtsx_trace(chip);
+				return STATUS_TIMEDOUT;
 			}
 
 			retval = rtsx_read_register(chip, MS_TRANS_CFG, &val);
 			if (retval != STATUS_SUCCESS) {
 				ms_set_err_code(chip, MS_TO_ERROR);
 				rtsx_clear_ms_error(chip);
-				TRACE_RET(chip, STATUS_TIMEDOUT);
+				rtsx_trace(chip);
+				return STATUS_TIMEDOUT;
 			}
 			if (val & (MS_CRC16_ERR | MS_RDY_TIMEOUT)) {
 				ms_set_err_code(chip, MS_CRC16_ERROR);
 				rtsx_clear_ms_error(chip);
-				TRACE_RET(chip, STATUS_FAIL);
+				rtsx_trace(chip);
+				return STATUS_FAIL;
 			}
 		}
 
@@ -3029,8 +3517,10 @@ static int ms_write_multiple_pages(struct rtsx_chip *chip, u16 old_blk,
 	if (!start_page) {
 		retval = ms_set_rw_reg_addr(chip, OverwriteFlag, MS_EXTRA_SIZE,
 					SystemParm, 7);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, STATUS_FAIL);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 
 		if (CHK_MS4BIT(ms_card))
 			data[0] = 0x88;
@@ -3047,24 +3537,32 @@ static int ms_write_multiple_pages(struct rtsx_chip *chip, u16 old_blk,
 
 		retval = ms_write_bytes(chip, WRITE_REG, 7, NO_WAIT_INT,
 					data, 8);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, STATUS_FAIL);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 
 		retval = ms_send_cmd(chip, BLOCK_WRITE, WAIT_INT);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, STATUS_FAIL);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 
 		ms_set_err_code(chip, MS_NO_ERROR);
 		retval = ms_transfer_tpc(chip, MS_TM_READ_BYTES, GET_INT, 1,
 					NO_WAIT_INT);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, STATUS_FAIL);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 	}
 
 	retval = ms_set_rw_reg_addr(chip, OverwriteFlag, MS_EXTRA_SIZE,
 				SystemParm, (6 + MS_EXTRA_SIZE));
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	ms_set_err_code(chip, MS_NO_ERROR);
 
@@ -3096,20 +3594,26 @@ static int ms_write_multiple_pages(struct rtsx_chip *chip, u16 old_blk,
 		if (retval == STATUS_SUCCESS)
 			break;
 	}
-	if (i == MS_MAX_RETRY_COUNT)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (i == MS_MAX_RETRY_COUNT) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
 		retval = ms_send_cmd(chip, BLOCK_WRITE, WAIT_INT);
 		if (retval == STATUS_SUCCESS)
 			break;
 	}
-	if (i == MS_MAX_RETRY_COUNT)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (i == MS_MAX_RETRY_COUNT) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	retval = ms_read_bytes(chip, GET_INT, 1, NO_WAIT_INT, &val, 1);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	ptr = buf;
 	for (page_addr = start_page; page_addr < end_page; page_addr++) {
@@ -3117,20 +3621,24 @@ static int ms_write_multiple_pages(struct rtsx_chip *chip, u16 old_blk,
 
 		if (detect_card_cd(chip, MS_CARD) != STATUS_SUCCESS) {
 			ms_set_err_code(chip, MS_NO_CARD);
-			TRACE_RET(chip, STATUS_FAIL);
+			rtsx_trace(chip);
+			return STATUS_FAIL;
 		}
 
 		if (val & INT_REG_CMDNK) {
 			ms_set_err_code(chip, MS_CMD_NK);
-			TRACE_RET(chip, STATUS_FAIL);
+			rtsx_trace(chip);
+			return STATUS_FAIL;
 		}
 		if (val & INT_REG_ERR) {
 			ms_set_err_code(chip, MS_FLASH_WRITE_ERROR);
-			TRACE_RET(chip, STATUS_FAIL);
+			rtsx_trace(chip);
+			return STATUS_FAIL;
 		}
 		if (!(val & INT_REG_BREQ)) {
 			ms_set_err_code(chip, MS_BREQ_ERROR);
-			TRACE_RET(chip, STATUS_FAIL);
+			rtsx_trace(chip);
+			return STATUS_FAIL;
 		}
 
 		udelay(30);
@@ -3161,34 +3669,43 @@ static int ms_write_multiple_pages(struct rtsx_chip *chip, u16 old_blk,
 			ms_set_err_code(chip, MS_TO_ERROR);
 			rtsx_clear_ms_error(chip);
 
-			if (retval == -ETIMEDOUT)
-				TRACE_RET(chip, STATUS_TIMEDOUT);
-			else
-				TRACE_RET(chip, STATUS_FAIL);
+			if (retval == -ETIMEDOUT) {
+				rtsx_trace(chip);
+				return STATUS_TIMEDOUT;
+			}
+			rtsx_trace(chip);
+			return STATUS_FAIL;
 		}
 
 		retval = ms_read_bytes(chip, GET_INT, 1, NO_WAIT_INT, &val, 1);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, STATUS_FAIL);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 
 		if ((end_page - start_page) == 1) {
 			if (!(val & INT_REG_CED)) {
 				ms_set_err_code(chip, MS_FLASH_WRITE_ERROR);
-				TRACE_RET(chip, STATUS_FAIL);
+				rtsx_trace(chip);
+				return STATUS_FAIL;
 			}
 		} else {
 			if (page_addr == (end_page - 1)) {
 				if (!(val & INT_REG_CED)) {
 					retval = ms_send_cmd(chip, BLOCK_END,
 							WAIT_INT);
-					if (retval != STATUS_SUCCESS)
-						TRACE_RET(chip, STATUS_FAIL);
+					if (retval != STATUS_SUCCESS) {
+						rtsx_trace(chip);
+						return STATUS_FAIL;
+					}
 				}
 
 				retval = ms_read_bytes(chip, GET_INT, 1,
 						NO_WAIT_INT, &val, 1);
-				if (retval != STATUS_SUCCESS)
-					TRACE_RET(chip, STATUS_FAIL);
+				if (retval != STATUS_SUCCESS) {
+					rtsx_trace(chip);
+					return STATUS_FAIL;
+				}
 			}
 
 			if ((page_addr == (end_page - 1)) ||
@@ -3196,7 +3713,8 @@ static int ms_write_multiple_pages(struct rtsx_chip *chip, u16 old_blk,
 				if (!(val & INT_REG_CED)) {
 					ms_set_err_code(chip,
 							MS_FLASH_WRITE_ERROR);
-					TRACE_RET(chip, STATUS_FAIL);
+					rtsx_trace(chip);
+					return STATUS_FAIL;
 				}
 			}
 		}
@@ -3217,8 +3735,10 @@ static int ms_finish_write(struct rtsx_chip *chip, u16 old_blk, u16 new_blk,
 
 	retval = ms_copy_page(chip, old_blk, new_blk, log_blk,
 			page_off, ms_card->page_off + 1);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	seg_no = old_blk >> 9;
 
@@ -3244,8 +3764,10 @@ static int ms_prepare_write(struct rtsx_chip *chip, u16 old_blk, u16 new_blk,
 	if (start_page) {
 		retval = ms_copy_page(chip, old_blk, new_blk, log_blk,
 				0, start_page);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, STATUS_FAIL);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 	}
 
 	return STATUS_SUCCESS;
@@ -3260,8 +3782,10 @@ int ms_delay_write(struct rtsx_chip *chip)
 
 	if (delay_write->delay_write_flag) {
 		retval = ms_set_init_para(chip);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, STATUS_FAIL);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 
 		delay_write->delay_write_flag = 0;
 		retval = ms_finish_write(chip,
@@ -3269,8 +3793,10 @@ int ms_delay_write(struct rtsx_chip *chip)
 					delay_write->new_phyblock,
 					delay_write->logblock,
 					delay_write->pageoff);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, STATUS_FAIL);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 	}
 
 	return STATUS_SUCCESS;
@@ -3309,7 +3835,8 @@ static int ms_rw_multi_sector(struct scsi_cmnd *srb, struct rtsx_chip *chip,
 	retval = ms_switch_clock(chip);
 	if (retval != STATUS_SUCCESS) {
 		ms_rw_fail(srb, chip);
-		TRACE_RET(chip, STATUS_FAIL);
+		rtsx_trace(chip);
+		return STATUS_FAIL;
 	}
 
 	log_blk = (u16)(start_sector >> ms_card->block_shift);
@@ -3325,7 +3852,8 @@ static int ms_rw_multi_sector(struct scsi_cmnd *srb, struct rtsx_chip *chip,
 		if (retval != STATUS_SUCCESS) {
 			chip->card_fail |= MS_CARD;
 			set_sense_type(chip, lun, SENSE_TYPE_MEDIA_NOT_PRESENT);
-			TRACE_RET(chip, STATUS_FAIL);
+			rtsx_trace(chip);
+			return STATUS_FAIL;
 		}
 	}
 
@@ -3342,7 +3870,8 @@ static int ms_rw_multi_sector(struct scsi_cmnd *srb, struct rtsx_chip *chip,
 			if (retval != STATUS_SUCCESS) {
 				set_sense_type(chip, lun,
 					SENSE_TYPE_MEDIA_WRITE_ERR);
-				TRACE_RET(chip, STATUS_FAIL);
+				rtsx_trace(chip);
+				return STATUS_FAIL;
 			}
 			old_blk = delay_write->old_phyblock;
 			new_blk = delay_write->new_phyblock;
@@ -3357,7 +3886,8 @@ static int ms_rw_multi_sector(struct scsi_cmnd *srb, struct rtsx_chip *chip,
 			if (retval != STATUS_SUCCESS) {
 				set_sense_type(chip, lun,
 					SENSE_TYPE_MEDIA_WRITE_ERR);
-				TRACE_RET(chip, STATUS_FAIL);
+				rtsx_trace(chip);
+				return STATUS_FAIL;
 			}
 #endif
 			old_blk = ms_get_l2p_tbl(chip, seg_no,
@@ -3366,7 +3896,8 @@ static int ms_rw_multi_sector(struct scsi_cmnd *srb, struct rtsx_chip *chip,
 			if ((old_blk == 0xFFFF) || (new_blk == 0xFFFF)) {
 				set_sense_type(chip, lun,
 					SENSE_TYPE_MEDIA_WRITE_ERR);
-				TRACE_RET(chip, STATUS_FAIL);
+				rtsx_trace(chip);
+				return STATUS_FAIL;
 			}
 
 			retval = ms_prepare_write(chip, old_blk, new_blk,
@@ -3375,11 +3906,13 @@ static int ms_rw_multi_sector(struct scsi_cmnd *srb, struct rtsx_chip *chip,
 				if (detect_card_cd(chip, MS_CARD) != STATUS_SUCCESS) {
 					set_sense_type(chip, lun,
 						SENSE_TYPE_MEDIA_NOT_PRESENT);
-					TRACE_RET(chip, STATUS_FAIL);
+					rtsx_trace(chip);
+					return STATUS_FAIL;
 				}
 				set_sense_type(chip, lun,
 					SENSE_TYPE_MEDIA_WRITE_ERR);
-				TRACE_RET(chip, STATUS_FAIL);
+				rtsx_trace(chip);
+				return STATUS_FAIL;
 			}
 #ifdef MS_DELAY_WRITE
 		}
@@ -3391,11 +3924,13 @@ static int ms_rw_multi_sector(struct scsi_cmnd *srb, struct rtsx_chip *chip,
 			if (detect_card_cd(chip, MS_CARD) != STATUS_SUCCESS) {
 				set_sense_type(chip, lun,
 					SENSE_TYPE_MEDIA_NOT_PRESENT);
-				TRACE_RET(chip, STATUS_FAIL);
+				rtsx_trace(chip);
+				return STATUS_FAIL;
 			}
 			set_sense_type(chip, lun,
 				SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
-			TRACE_RET(chip, STATUS_FAIL);
+			rtsx_trace(chip);
+			return STATUS_FAIL;
 		}
 #endif
 		old_blk = ms_get_l2p_tbl(chip, seg_no,
@@ -3403,7 +3938,8 @@ static int ms_rw_multi_sector(struct scsi_cmnd *srb, struct rtsx_chip *chip,
 		if (old_blk == 0xFFFF) {
 			set_sense_type(chip, lun,
 				SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
-			TRACE_RET(chip, STATUS_FAIL);
+			rtsx_trace(chip);
+			return STATUS_FAIL;
 		}
 	}
 
@@ -3436,10 +3972,12 @@ static int ms_rw_multi_sector(struct scsi_cmnd *srb, struct rtsx_chip *chip,
 			if (detect_card_cd(chip, MS_CARD) != STATUS_SUCCESS) {
 				set_sense_type(chip, lun,
 					SENSE_TYPE_MEDIA_NOT_PRESENT);
-				TRACE_RET(chip, STATUS_FAIL);
+				rtsx_trace(chip);
+				return STATUS_FAIL;
 			}
 			ms_rw_fail(srb, chip);
-			TRACE_RET(chip, STATUS_FAIL);
+			rtsx_trace(chip);
+			return STATUS_FAIL;
 		}
 
 		if (srb->sc_data_direction == DMA_TO_DEVICE) {
@@ -3475,7 +4013,8 @@ static int ms_rw_multi_sector(struct scsi_cmnd *srb, struct rtsx_chip *chip,
 				chip->card_fail |= MS_CARD;
 				set_sense_type(chip, lun,
 					SENSE_TYPE_MEDIA_NOT_PRESENT);
-				TRACE_RET(chip, STATUS_FAIL);
+				rtsx_trace(chip);
+				return STATUS_FAIL;
 			}
 		}
 
@@ -3483,14 +4022,16 @@ static int ms_rw_multi_sector(struct scsi_cmnd *srb, struct rtsx_chip *chip,
 					log_blk - ms_start_idx[seg_no]);
 		if (old_blk == 0xFFFF) {
 			ms_rw_fail(srb, chip);
-			TRACE_RET(chip, STATUS_FAIL);
+			rtsx_trace(chip);
+			return STATUS_FAIL;
 		}
 
 		if (srb->sc_data_direction == DMA_TO_DEVICE) {
 			new_blk = ms_get_unused_block(chip, seg_no);
 			if (new_blk == 0xFFFF) {
 				ms_rw_fail(srb, chip);
-				TRACE_RET(chip, STATUS_FAIL);
+				rtsx_trace(chip);
+				return STATUS_FAIL;
 			}
 		}
 
@@ -3515,11 +4056,13 @@ static int ms_rw_multi_sector(struct scsi_cmnd *srb, struct rtsx_chip *chip,
 				if (detect_card_cd(chip, MS_CARD) != STATUS_SUCCESS) {
 					set_sense_type(chip, lun,
 						SENSE_TYPE_MEDIA_NOT_PRESENT);
-					TRACE_RET(chip, STATUS_FAIL);
+					rtsx_trace(chip);
+					return STATUS_FAIL;
 				}
 
 				ms_rw_fail(srb, chip);
-				TRACE_RET(chip, STATUS_FAIL);
+				rtsx_trace(chip);
+				return STATUS_FAIL;
 			}
 #endif
 		}
@@ -3581,12 +4124,16 @@ static int ms_poll_int(struct rtsx_chip *chip)
 	rtsx_add_cmd(chip, CHECK_REG_CMD, MS_TRANS_CFG, MS_INT_CED, MS_INT_CED);
 
 	retval = rtsx_send_cmd(chip, MS_CARD, 5000);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	val = *rtsx_get_cmd_data(chip);
-	if (val & MS_INT_ERR)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (val & MS_INT_ERR) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	return STATUS_SUCCESS;
 }
@@ -3649,12 +4196,15 @@ static int mg_send_ex_cmd(struct rtsx_chip *chip, u8 cmd, u8 entry_num)
 		if (retval == STATUS_SUCCESS)
 			break;
 	}
-	if (i == MS_MAX_RETRY_COUNT)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (i == MS_MAX_RETRY_COUNT) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	if (check_ms_err(chip)) {
 		rtsx_clear_ms_error(chip);
-		TRACE_RET(chip, STATUS_FAIL);
+		rtsx_trace(chip);
+		return STATUS_FAIL;
 	}
 
 	return STATUS_SUCCESS;
@@ -3671,8 +4221,10 @@ static int mg_set_tpc_para_sub(struct rtsx_chip *chip, int type,
 	else
 		retval = ms_set_rw_reg_addr(chip, 0, 0, Pro_DataCount1, 6);
 
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	buf[0] = 0;
 	buf[1] = 0;
@@ -3684,8 +4236,10 @@ static int mg_set_tpc_para_sub(struct rtsx_chip *chip, int type,
 	}
 	retval = ms_write_bytes(chip, PRO_WRITE_REG, (type == 0) ? 1 : 6,
 				NO_WAIT_INT, buf, 6);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	return STATUS_SUCCESS;
 }
@@ -3699,19 +4253,23 @@ int mg_set_leaf_id(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 
 	if (scsi_bufflen(srb) < 12) {
 		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-		TRACE_RET(chip, STATUS_FAIL);
+		rtsx_trace(chip);
+		return STATUS_FAIL;
 	}
 
 	ms_cleanup_work(chip);
 
 	retval = ms_switch_clock(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	retval = mg_send_ex_cmd(chip, MG_SET_LID, 0);
 	if (retval != STATUS_SUCCESS) {
 		set_sense_type(chip, lun, SENSE_TYPE_MG_KEY_FAIL_NOT_ESTAB);
-		TRACE_RET(chip, STATUS_FAIL);
+		rtsx_trace(chip);
+		return STATUS_FAIL;
 	}
 
 	memset(buf1, 0, 32);
@@ -3723,12 +4281,14 @@ int mg_set_leaf_id(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 				buf1, 32);
 	if (retval != STATUS_SUCCESS) {
 		set_sense_type(chip, lun, SENSE_TYPE_MG_KEY_FAIL_NOT_ESTAB);
-		TRACE_RET(chip, STATUS_FAIL);
+		rtsx_trace(chip);
+		return STATUS_FAIL;
 	}
 	if (check_ms_err(chip)) {
 		set_sense_type(chip, lun, SENSE_TYPE_MG_KEY_FAIL_NOT_ESTAB);
 		rtsx_clear_ms_error(chip);
-		TRACE_RET(chip, STATUS_FAIL);
+		rtsx_trace(chip);
+		return STATUS_FAIL;
 	}
 
 	return STATUS_SUCCESS;
@@ -3744,12 +4304,16 @@ int mg_get_local_EKB(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 	ms_cleanup_work(chip);
 
 	retval = ms_switch_clock(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	buf = kmalloc(1540, GFP_KERNEL);
-	if (!buf)
-		TRACE_RET(chip, STATUS_ERROR);
+	if (!buf) {
+		rtsx_trace(chip);
+		return STATUS_ERROR;
+	}
 
 	buf[0] = 0x04;
 	buf[1] = 0x1A;
@@ -3759,7 +4323,8 @@ int mg_get_local_EKB(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 	retval = mg_send_ex_cmd(chip, MG_GET_LEKB, 0);
 	if (retval != STATUS_SUCCESS) {
 		set_sense_type(chip, lun, SENSE_TYPE_MG_KEY_FAIL_NOT_AUTHEN);
-		TRACE_GOTO(chip, GetEKBFinish);
+		rtsx_trace(chip);
+		goto GetEKBFinish;
 	}
 
 	retval = ms_transfer_data(chip, MS_TM_AUTO_READ, PRO_READ_LONG_DATA,
@@ -3767,12 +4332,14 @@ int mg_get_local_EKB(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 	if (retval != STATUS_SUCCESS) {
 		set_sense_type(chip, lun, SENSE_TYPE_MG_KEY_FAIL_NOT_AUTHEN);
 		rtsx_clear_ms_error(chip);
-		TRACE_GOTO(chip, GetEKBFinish);
+		rtsx_trace(chip);
+		goto GetEKBFinish;
 	}
 	if (check_ms_err(chip)) {
 		set_sense_type(chip, lun, SENSE_TYPE_MG_KEY_FAIL_NOT_AUTHEN);
 		rtsx_clear_ms_error(chip);
-		TRACE_RET(chip, STATUS_FAIL);
+		rtsx_trace(chip);
+		return STATUS_FAIL;
 	}
 
 	bufflen = min_t(int, 1052, scsi_bufflen(srb));
@@ -3795,25 +4362,30 @@ int mg_chg(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 	ms_cleanup_work(chip);
 
 	retval = ms_switch_clock(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	retval = mg_send_ex_cmd(chip, MG_GET_ID, 0);
 	if (retval != STATUS_SUCCESS) {
 		set_sense_type(chip, lun, SENSE_TYPE_MG_INCOMPATIBLE_MEDIUM);
-		TRACE_RET(chip, STATUS_FAIL);
+		rtsx_trace(chip);
+		return STATUS_FAIL;
 	}
 
 	retval = ms_read_bytes(chip, PRO_READ_SHORT_DATA, 32, WAIT_INT,
 			buf, 32);
 	if (retval != STATUS_SUCCESS) {
 		set_sense_type(chip, lun, SENSE_TYPE_MG_INCOMPATIBLE_MEDIUM);
-		TRACE_RET(chip, STATUS_FAIL);
+		rtsx_trace(chip);
+		return STATUS_FAIL;
 	}
 	if (check_ms_err(chip)) {
 		set_sense_type(chip, lun, SENSE_TYPE_MG_INCOMPATIBLE_MEDIUM);
 		rtsx_clear_ms_error(chip);
-		TRACE_RET(chip, STATUS_FAIL);
+		rtsx_trace(chip);
+		return STATUS_FAIL;
 	}
 
 	memcpy(ms_card->magic_gate_id, buf, 16);
@@ -3822,14 +4394,16 @@ int mg_chg(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 	retval = ms_poll_int(chip);
 	if (retval != STATUS_SUCCESS) {
 		set_sense_type(chip, lun, SENSE_TYPE_MG_INCOMPATIBLE_MEDIUM);
-		TRACE_RET(chip, STATUS_FAIL);
+		rtsx_trace(chip);
+		return STATUS_FAIL;
 	}
 #endif
 
 	retval = mg_send_ex_cmd(chip, MG_SET_RD, 0);
 	if (retval != STATUS_SUCCESS) {
 		set_sense_type(chip, lun, SENSE_TYPE_MG_INCOMPATIBLE_MEDIUM);
-		TRACE_RET(chip, STATUS_FAIL);
+		rtsx_trace(chip);
+		return STATUS_FAIL;
 	}
 
 	bufflen = min_t(int, 12, scsi_bufflen(srb));
@@ -3845,12 +4419,14 @@ int mg_chg(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 				32, WAIT_INT, buf, 32);
 	if (retval != STATUS_SUCCESS) {
 		set_sense_type(chip, lun, SENSE_TYPE_MG_INCOMPATIBLE_MEDIUM);
-		TRACE_RET(chip, STATUS_FAIL);
+		rtsx_trace(chip);
+		return STATUS_FAIL;
 	}
 	if (check_ms_err(chip)) {
 		set_sense_type(chip, lun, SENSE_TYPE_MG_INCOMPATIBLE_MEDIUM);
 		rtsx_clear_ms_error(chip);
-		TRACE_RET(chip, STATUS_FAIL);
+		rtsx_trace(chip);
+		return STATUS_FAIL;
 	}
 
 	ms_card->mg_auth = 0;
@@ -3869,25 +4445,30 @@ int mg_get_rsp_chg(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 	ms_cleanup_work(chip);
 
 	retval = ms_switch_clock(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	retval = mg_send_ex_cmd(chip, MG_MAKE_RMS, 0);
 	if (retval != STATUS_SUCCESS) {
 		set_sense_type(chip, lun, SENSE_TYPE_MG_KEY_FAIL_NOT_AUTHEN);
-		TRACE_RET(chip, STATUS_FAIL);
+		rtsx_trace(chip);
+		return STATUS_FAIL;
 	}
 
 	retval = ms_read_bytes(chip, PRO_READ_SHORT_DATA, 32, WAIT_INT,
 			buf1, 32);
 	if (retval != STATUS_SUCCESS) {
 		set_sense_type(chip, lun, SENSE_TYPE_MG_KEY_FAIL_NOT_AUTHEN);
-		TRACE_RET(chip, STATUS_FAIL);
+		rtsx_trace(chip);
+		return STATUS_FAIL;
 	}
 	if (check_ms_err(chip)) {
 		set_sense_type(chip, lun, SENSE_TYPE_MG_KEY_FAIL_NOT_AUTHEN);
 		rtsx_clear_ms_error(chip);
-		TRACE_RET(chip, STATUS_FAIL);
+		rtsx_trace(chip);
+		return STATUS_FAIL;
 	}
 
 	buf2[0] = 0x00;
@@ -3905,7 +4486,8 @@ int mg_get_rsp_chg(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 	retval = ms_poll_int(chip);
 	if (retval != STATUS_SUCCESS) {
 		set_sense_type(chip, lun, SENSE_TYPE_MG_KEY_FAIL_NOT_AUTHEN);
-		TRACE_RET(chip, STATUS_FAIL);
+		rtsx_trace(chip);
+		return STATUS_FAIL;
 	}
 #endif
 
@@ -3924,13 +4506,16 @@ int mg_rsp(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 	ms_cleanup_work(chip);
 
 	retval = ms_switch_clock(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	retval = mg_send_ex_cmd(chip, MG_MAKE_KSE, 0);
 	if (retval != STATUS_SUCCESS) {
 		set_sense_type(chip, lun, SENSE_TYPE_MG_KEY_FAIL_NOT_AUTHEN);
-		TRACE_RET(chip, STATUS_FAIL);
+		rtsx_trace(chip);
+		return STATUS_FAIL;
 	}
 
 	bufflen = min_t(int, 12, scsi_bufflen(srb));
@@ -3946,12 +4531,14 @@ int mg_rsp(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 				buf, 32);
 	if (retval != STATUS_SUCCESS) {
 		set_sense_type(chip, lun, SENSE_TYPE_MG_KEY_FAIL_NOT_AUTHEN);
-		TRACE_RET(chip, STATUS_FAIL);
+		rtsx_trace(chip);
+		return STATUS_FAIL;
 	}
 	if (check_ms_err(chip)) {
 		set_sense_type(chip, lun, SENSE_TYPE_MG_KEY_FAIL_NOT_AUTHEN);
 		rtsx_clear_ms_error(chip);
-		TRACE_RET(chip, STATUS_FAIL);
+		rtsx_trace(chip);
+		return STATUS_FAIL;
 	}
 
 	ms_card->mg_auth = 1;
@@ -3970,12 +4557,16 @@ int mg_get_ICV(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 	ms_cleanup_work(chip);
 
 	retval = ms_switch_clock(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	buf = kmalloc(1028, GFP_KERNEL);
-	if (!buf)
-		TRACE_RET(chip, STATUS_ERROR);
+	if (!buf) {
+		rtsx_trace(chip);
+		return STATUS_ERROR;
+	}
 
 	buf[0] = 0x04;
 	buf[1] = 0x02;
@@ -3985,7 +4576,8 @@ int mg_get_ICV(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 	retval = mg_send_ex_cmd(chip, MG_GET_IBD, ms_card->mg_entry_num);
 	if (retval != STATUS_SUCCESS) {
 		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
-		TRACE_GOTO(chip, GetICVFinish);
+		rtsx_trace(chip);
+		goto GetICVFinish;
 	}
 
 	retval = ms_transfer_data(chip, MS_TM_AUTO_READ, PRO_READ_LONG_DATA,
@@ -3993,12 +4585,14 @@ int mg_get_ICV(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 	if (retval != STATUS_SUCCESS) {
 		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
 		rtsx_clear_ms_error(chip);
-		TRACE_GOTO(chip, GetICVFinish);
+		rtsx_trace(chip);
+		goto GetICVFinish;
 	}
 	if (check_ms_err(chip)) {
 		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
 		rtsx_clear_ms_error(chip);
-		TRACE_RET(chip, STATUS_FAIL);
+		rtsx_trace(chip);
+		return STATUS_FAIL;
 	}
 
 	bufflen = min_t(int, 1028, scsi_bufflen(srb));
@@ -4023,12 +4617,16 @@ int mg_set_ICV(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 	ms_cleanup_work(chip);
 
 	retval = ms_switch_clock(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	buf = kmalloc(1028, GFP_KERNEL);
-	if (!buf)
-		TRACE_RET(chip, STATUS_ERROR);
+	if (!buf) {
+		rtsx_trace(chip);
+		return STATUS_ERROR;
+	}
 
 	bufflen = min_t(int, 1028, scsi_bufflen(srb));
 	rtsx_stor_get_xfer_buf(buf, bufflen, srb);
@@ -4045,7 +4643,8 @@ int mg_set_ICV(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 		} else {
 			set_sense_type(chip, lun, SENSE_TYPE_MG_WRITE_ERR);
 		}
-		TRACE_GOTO(chip, SetICVFinish);
+		rtsx_trace(chip);
+		goto SetICVFinish;
 	}
 
 #ifdef MG_SET_ICV_SLOW
@@ -4084,7 +4683,8 @@ int mg_set_ICV(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 					SENSE_TYPE_MG_WRITE_ERR);
 			}
 			retval = STATUS_FAIL;
-			TRACE_GOTO(chip, SetICVFinish);
+			rtsx_trace(chip);
+			goto SetICVFinish;
 		}
 	}
 #else
@@ -4102,7 +4702,8 @@ int mg_set_ICV(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 		} else {
 			set_sense_type(chip, lun, SENSE_TYPE_MG_WRITE_ERR);
 		}
-		TRACE_GOTO(chip, SetICVFinish);
+		rtsx_trace(chip);
+		goto SetICVFinish;
 	}
 #endif
 
@@ -4142,22 +4743,37 @@ int ms_power_off_card3v3(struct rtsx_chip *chip)
 	int retval;
 
 	retval = disable_card_clock(chip, MS_CARD);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	if (chip->asic_code) {
 		retval = ms_pull_ctl_disable(chip);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, STATUS_FAIL);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 	} else {
-		RTSX_WRITE_REG(chip, FPGA_PULL_CTL,
-			FPGA_MS_PULL_CTL_BIT | 0x20, FPGA_MS_PULL_CTL_BIT);
+		retval = rtsx_write_register(chip, FPGA_PULL_CTL,
+					     FPGA_MS_PULL_CTL_BIT | 0x20,
+					     FPGA_MS_PULL_CTL_BIT);
+		if (retval) {
+			rtsx_trace(chip);
+			return retval;
+		}
+	}
+	retval = rtsx_write_register(chip, CARD_OE, MS_OUTPUT_EN, 0);
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
 	}
-	RTSX_WRITE_REG(chip, CARD_OE, MS_OUTPUT_EN, 0);
 	if (!chip->ft2_fast_mode) {
 		retval = card_power_off(chip, MS_CARD);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, STATUS_FAIL);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 	}
 
 	return STATUS_SUCCESS;
@@ -4185,8 +4801,10 @@ int release_ms_card(struct rtsx_chip *chip)
 #endif
 
 	retval = ms_power_off_card3v3(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	return STATUS_SUCCESS;
 }
diff --git a/drivers/staging/rts5208/ms.h b/drivers/staging/rts5208/ms.h
index 26c5b03d535e..d919170f2720 100644
--- a/drivers/staging/rts5208/ms.h
+++ b/drivers/staging/rts5208/ms.h
@@ -205,7 +205,7 @@ int reset_ms_card(struct rtsx_chip *chip);
 int ms_rw(struct scsi_cmnd *srb, struct rtsx_chip *chip,
 	u32 start_sector, u16 sector_cnt);
 int mspro_format(struct scsi_cmnd *srb, struct rtsx_chip *chip,
-		int short_data_len, int quick_format);
+		int short_data_len, bool quick_format);
 void ms_free_l2p_tbl(struct rtsx_chip *chip);
 void ms_cleanup_work(struct rtsx_chip *chip);
 int ms_power_off_card3v3(struct rtsx_chip *chip);
diff --git a/drivers/staging/rts5208/rtsx.c b/drivers/staging/rts5208/rtsx.c
index c74f1b8108f6..d64b6ed9c0c9 100644
--- a/drivers/staging/rts5208/rtsx.c
+++ b/drivers/staging/rts5208/rtsx.c
@@ -26,12 +26,6 @@
 #include <linux/workqueue.h>
 
 #include "rtsx.h"
-#include "rtsx_chip.h"
-#include "rtsx_transport.h"
-#include "rtsx_scsi.h"
-#include "rtsx_card.h"
-#include "general.h"
-
 #include "ms.h"
 #include "sd.h"
 #include "xd.h"
@@ -137,8 +131,8 @@ static int queuecommand_lck(struct scsi_cmnd *srb,
 
 	/* check for state-transition errors */
 	if (chip->srb != NULL) {
-		dev_err(&dev->pci->dev, "Error in %s: chip->srb = %p\n",
-			__func__, chip->srb);
+		dev_err(&dev->pci->dev, "Error: chip->srb = %p\n",
+			chip->srb);
 		return SCSI_MLQUEUE_HOST_BUSY;
 	}
 
@@ -1036,7 +1030,7 @@ static const struct pci_device_id rtsx_ids[] = {
 MODULE_DEVICE_TABLE(pci, rtsx_ids);
 
 /* pci_driver definition */
-static struct pci_driver driver = {
+static struct pci_driver rtsx_driver = {
 	.name = CR_DRIVER_NAME,
 	.id_table = rtsx_ids,
 	.probe = rtsx_probe,
@@ -1048,21 +1042,4 @@ static struct pci_driver driver = {
 	.shutdown = rtsx_shutdown,
 };
 
-static int __init rtsx_init(void)
-{
-	pr_info("Initializing Realtek PCIE storage driver...\n");
-
-	return pci_register_driver(&driver);
-}
-
-static void __exit rtsx_exit(void)
-{
-	pr_info("rtsx_exit() called\n");
-
-	pci_unregister_driver(&driver);
-
-	pr_info("%s module exit\n", CR_DRIVER_NAME);
-}
-
-module_init(rtsx_init)
-module_exit(rtsx_exit)
+module_pci_driver(rtsx_driver);
diff --git a/drivers/staging/rts5208/rtsx.h b/drivers/staging/rts5208/rtsx.h
index 9e6ecb7457b5..262441bcfc41 100644
--- a/drivers/staging/rts5208/rtsx.h
+++ b/drivers/staging/rts5208/rtsx.h
@@ -46,9 +46,6 @@
 #include <scsi/scsi_eh.h>
 #include <scsi/scsi_host.h>
 
-#include "trace.h"
-#include "general.h"
-
 #define CR_DRIVER_NAME		"rts5208"
 
 #define pci_get_bus_and_slot(bus, devfn)	\
@@ -181,4 +178,14 @@ enum xfer_buf_dir	{TO_XFER_BUF, FROM_XFER_BUF};
 
 int rtsx_read_pci_cfg_byte(u8 bus, u8 dev, u8 func, u8 offset, u8 *val);
 
+#define _MSG_TRACE
+
+#include "trace.h"
+#include "rtsx_chip.h"
+#include "rtsx_transport.h"
+#include "rtsx_scsi.h"
+#include "rtsx_card.h"
+#include "rtsx_sys.h"
+#include "general.h"
+
 #endif  /* __REALTEK_RTSX_H */
diff --git a/drivers/staging/rts5208/rtsx_card.c b/drivers/staging/rts5208/rtsx_card.c
index b4595ab3f02b..437436f5dbdd 100644
--- a/drivers/staging/rts5208/rtsx_card.c
+++ b/drivers/staging/rts5208/rtsx_card.c
@@ -27,13 +27,6 @@
 #include <linux/kernel.h>
 
 #include "rtsx.h"
-#include "rtsx_transport.h"
-#include "rtsx_scsi.h"
-#include "rtsx_card.h"
-
-#include "rtsx_sys.h"
-#include "general.h"
-
 #include "sd.h"
 #include "xd.h"
 #include "ms.h"
@@ -657,8 +650,10 @@ int switch_ssc_clock(struct rtsx_chip *chip, int clk)
 	dev_dbg(rtsx_dev(chip), "Switch SSC clock to %dMHz (cur_clk = %d)\n",
 		clk, chip->cur_clk);
 
-	if ((clk <= 2) || (N > max_N))
-		TRACE_RET(chip, STATUS_FAIL);
+	if ((clk <= 2) || (N > max_N)) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	mcu_cnt = (u8)(125/clk + 3);
 	if (mcu_cnt > 7)
@@ -697,11 +692,17 @@ int switch_ssc_clock(struct rtsx_chip *chip, int clk)
 	}
 
 	retval = rtsx_send_cmd(chip, 0, WAIT_TIME);
-	if (retval < 0)
-		TRACE_RET(chip, STATUS_ERROR);
+	if (retval < 0) {
+		rtsx_trace(chip);
+		return STATUS_ERROR;
+	}
 
 	udelay(10);
-	RTSX_WRITE_REG(chip, CLK_CTL, CLK_LOW_FREQ, 0);
+	retval = rtsx_write_register(chip, CLK_CTL, CLK_LOW_FREQ, 0);
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
 
 	chip->cur_clk = clk;
 
@@ -710,6 +711,7 @@ int switch_ssc_clock(struct rtsx_chip *chip, int clk)
 
 int switch_normal_clock(struct rtsx_chip *chip, int clk)
 {
+	int retval;
 	u8 sel, div, mcu_cnt;
 	int sd_vpclk_phase_reset = 0;
 
@@ -790,26 +792,62 @@ int switch_normal_clock(struct rtsx_chip *chip, int clk)
 	default:
 		dev_dbg(rtsx_dev(chip), "Try to switch to an illegal clock (%d)\n",
 			clk);
-		TRACE_RET(chip, STATUS_FAIL);
+		rtsx_trace(chip);
+		return STATUS_FAIL;
 	}
 
-	RTSX_WRITE_REG(chip, CLK_CTL, 0xFF, CLK_LOW_FREQ);
+	retval = rtsx_write_register(chip, CLK_CTL, 0xFF, CLK_LOW_FREQ);
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
 	if (sd_vpclk_phase_reset) {
-		RTSX_WRITE_REG(chip, SD_VPCLK0_CTL, PHASE_NOT_RESET, 0);
-		RTSX_WRITE_REG(chip, SD_VPCLK1_CTL, PHASE_NOT_RESET, 0);
+		retval = rtsx_write_register(chip, SD_VPCLK0_CTL,
+					     PHASE_NOT_RESET, 0);
+		if (retval) {
+			rtsx_trace(chip);
+			return retval;
+		}
+		retval = rtsx_write_register(chip, SD_VPCLK1_CTL,
+					     PHASE_NOT_RESET, 0);
+		if (retval) {
+			rtsx_trace(chip);
+			return retval;
+		}
+	}
+	retval = rtsx_write_register(chip, CLK_DIV, 0xFF,
+				     (div << 4) | mcu_cnt);
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
+	retval = rtsx_write_register(chip, CLK_SEL, 0xFF, sel);
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
 	}
-	RTSX_WRITE_REG(chip, CLK_DIV, 0xFF, (div << 4) | mcu_cnt);
-	RTSX_WRITE_REG(chip, CLK_SEL, 0xFF, sel);
 
 	if (sd_vpclk_phase_reset) {
 		udelay(200);
-		RTSX_WRITE_REG(chip, SD_VPCLK0_CTL, PHASE_NOT_RESET,
-				PHASE_NOT_RESET);
-		RTSX_WRITE_REG(chip, SD_VPCLK1_CTL, PHASE_NOT_RESET,
-				PHASE_NOT_RESET);
+		retval = rtsx_write_register(chip, SD_VPCLK0_CTL,
+					     PHASE_NOT_RESET, PHASE_NOT_RESET);
+		if (retval) {
+			rtsx_trace(chip);
+			return retval;
+		}
+		retval = rtsx_write_register(chip, SD_VPCLK1_CTL,
+					     PHASE_NOT_RESET, PHASE_NOT_RESET);
+		if (retval) {
+			rtsx_trace(chip);
+			return retval;
+		}
 		udelay(200);
 	}
-	RTSX_WRITE_REG(chip, CLK_CTL, 0xFF, 0);
+	retval = rtsx_write_register(chip, CLK_CTL, 0xFF, 0);
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
 
 	chip->cur_clk = clk;
 
@@ -844,6 +882,7 @@ void trans_dma_enable(enum dma_data_direction dir, struct rtsx_chip *chip,
 
 int enable_card_clock(struct rtsx_chip *chip, u8 card)
 {
+	int retval;
 	u8 clk_en = 0;
 
 	if (card & XD_CARD)
@@ -853,13 +892,18 @@ int enable_card_clock(struct rtsx_chip *chip, u8 card)
 	if (card & MS_CARD)
 		clk_en |= MS_CLK_EN;
 
-	RTSX_WRITE_REG(chip, CARD_CLK_EN, clk_en, clk_en);
+	retval = rtsx_write_register(chip, CARD_CLK_EN, clk_en, clk_en);
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
 
 	return STATUS_SUCCESS;
 }
 
 int disable_card_clock(struct rtsx_chip *chip, u8 card)
 {
+	int retval;
 	u8 clk_en = 0;
 
 	if (card & XD_CARD)
@@ -869,7 +913,11 @@ int disable_card_clock(struct rtsx_chip *chip, u8 card)
 	if (card & MS_CARD)
 		clk_en |= MS_CLK_EN;
 
-	RTSX_WRITE_REG(chip, CARD_CLK_EN, clk_en, 0);
+	retval = rtsx_write_register(chip, CARD_CLK_EN, clk_en, 0);
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
 
 	return STATUS_SUCCESS;
 }
@@ -893,8 +941,10 @@ int card_power_on(struct rtsx_chip *chip, u8 card)
 	rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_PWR_CTL, mask, val1);
 
 	retval = rtsx_send_cmd(chip, 0, 100);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	udelay(chip->pmos_pwr_on_interval);
 
@@ -902,14 +952,17 @@ int card_power_on(struct rtsx_chip *chip, u8 card)
 	rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_PWR_CTL, mask, val2);
 
 	retval = rtsx_send_cmd(chip, 0, 100);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	return STATUS_SUCCESS;
 }
 
 int card_power_off(struct rtsx_chip *chip, u8 card)
 {
+	int retval;
 	u8 mask, val;
 
 	if (CHECK_LUN_MODE(chip, SD_MS_2LUN) && (card == MS_CARD)) {
@@ -920,7 +973,11 @@ int card_power_off(struct rtsx_chip *chip, u8 card)
 		val = SD_POWER_OFF;
 	}
 
-	RTSX_WRITE_REG(chip, CARD_PWR_CTL, mask, val);
+	retval = rtsx_write_register(chip, CARD_PWR_CTL, mask, val);
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
 
 	return STATUS_SUCCESS;
 }
@@ -932,8 +989,10 @@ int card_rw(struct scsi_cmnd *srb, struct rtsx_chip *chip,
 	unsigned int lun = SCSI_LUN(srb);
 	int i;
 
-	if (chip->rw_card[lun] == NULL)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (chip->rw_card[lun] == NULL) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	for (i = 0; i < 3; i++) {
 		chip->rw_need_retry = 0;
@@ -942,11 +1001,14 @@ int card_rw(struct scsi_cmnd *srb, struct rtsx_chip *chip,
 		if (retval != STATUS_SUCCESS) {
 			if (rtsx_check_chip_exist(chip) != STATUS_SUCCESS) {
 				rtsx_release_chip(chip);
-				TRACE_RET(chip, STATUS_FAIL);
+				rtsx_trace(chip);
+				return STATUS_FAIL;
 			}
 			if (detect_card_cd(chip, chip->cur_card) !=
-							STATUS_SUCCESS)
-				TRACE_RET(chip, STATUS_FAIL);
+							STATUS_SUCCESS) {
+				rtsx_trace(chip);
+				return STATUS_FAIL;
+			}
 
 			if (!chip->rw_need_retry) {
 				dev_dbg(rtsx_dev(chip), "RW fail, but no need to retry\n");
@@ -965,6 +1027,7 @@ int card_rw(struct scsi_cmnd *srb, struct rtsx_chip *chip,
 
 int card_share_mode(struct rtsx_chip *chip, int card)
 {
+	int retval;
 	u8 mask, value;
 
 	if (CHECK_PID(chip, 0x5208)) {
@@ -975,8 +1038,10 @@ int card_share_mode(struct rtsx_chip *chip, int card)
 			value = CARD_SHARE_48_MS;
 		else if (card == XD_CARD)
 			value = CARD_SHARE_48_XD;
-		else
-			TRACE_RET(chip, STATUS_FAIL);
+		else {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 
 	} else if (CHECK_PID(chip, 0x5288)) {
 		mask = 0x03;
@@ -986,14 +1051,21 @@ int card_share_mode(struct rtsx_chip *chip, int card)
 			value = CARD_SHARE_BAROSSA_MS;
 		else if (card == XD_CARD)
 			value = CARD_SHARE_BAROSSA_XD;
-		else
-			TRACE_RET(chip, STATUS_FAIL);
+		else {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 
 	} else {
-		TRACE_RET(chip, STATUS_FAIL);
+		rtsx_trace(chip);
+		return STATUS_FAIL;
 	}
 
-	RTSX_WRITE_REG(chip, CARD_SHARE_MODE, mask, value);
+	retval = rtsx_write_register(chip, CARD_SHARE_MODE, mask, value);
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
 
 	return STATUS_SUCCESS;
 }
@@ -1014,15 +1086,23 @@ int select_card(struct rtsx_chip *chip, int card)
 			mod = XD_MOD_SEL;
 		else if (card == SPI_CARD)
 			mod = SPI_MOD_SEL;
-		else
-			TRACE_RET(chip, STATUS_FAIL);
+		else {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 
-		RTSX_WRITE_REG(chip, CARD_SELECT, 0x07, mod);
+		retval = rtsx_write_register(chip, CARD_SELECT, 0x07, mod);
+		if (retval) {
+			rtsx_trace(chip);
+			return retval;
+		}
 		chip->cur_card = card;
 
 		retval =  card_share_mode(chip, card);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, STATUS_FAIL);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 	}
 
 	return STATUS_SUCCESS;
@@ -1067,12 +1147,15 @@ int detect_card_cd(struct rtsx_chip *chip, int card)
 		card_cd = XD_EXIST;
 	} else {
 		dev_dbg(rtsx_dev(chip), "Wrong card type: 0x%x\n", card);
-		TRACE_RET(chip, STATUS_FAIL);
+		rtsx_trace(chip);
+		return STATUS_FAIL;
 	}
 
 	status = rtsx_readl(chip, RTSX_BIPR);
-	if (!(status & card_cd))
-		TRACE_RET(chip, STATUS_FAIL);
+	if (!(status & card_cd)) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	return STATUS_SUCCESS;
 }
diff --git a/drivers/staging/rts5208/rtsx_card.h b/drivers/staging/rts5208/rtsx_card.h
index 75cf5889bda4..8f2cf9a4ec69 100644
--- a/drivers/staging/rts5208/rtsx_card.h
+++ b/drivers/staging/rts5208/rtsx_card.h
@@ -1061,7 +1061,13 @@ int card_power_off(struct rtsx_chip *chip, u8 card);
 
 static inline int card_power_off_all(struct rtsx_chip *chip)
 {
-	RTSX_WRITE_REG(chip, CARD_PWR_CTL, 0x0F, 0x0F);
+	int retval;
+
+	retval = rtsx_write_register(chip, CARD_PWR_CTL, 0x0F, 0x0F);
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
 
 	return STATUS_SUCCESS;
 }
diff --git a/drivers/staging/rts5208/rtsx_chip.c b/drivers/staging/rts5208/rtsx_chip.c
index 9593d8132938..0c1716ebc827 100644
--- a/drivers/staging/rts5208/rtsx_chip.c
+++ b/drivers/staging/rts5208/rtsx_chip.c
@@ -27,13 +27,6 @@
 #include <linux/vmalloc.h>
 
 #include "rtsx.h"
-#include "rtsx_transport.h"
-#include "rtsx_scsi.h"
-#include "rtsx_card.h"
-#include "rtsx_chip.h"
-#include "rtsx_sys.h"
-#include "general.h"
-
 #include "sd.h"
 #include "xd.h"
 #include "ms.h"
@@ -123,22 +116,46 @@ void rtsx_disable_bus_int(struct rtsx_chip *chip)
 
 static int rtsx_pre_handle_sdio_old(struct rtsx_chip *chip)
 {
+	int retval;
+
 	if (chip->ignore_sd && CHK_SDIO_EXIST(chip)) {
 		if (chip->asic_code) {
-			RTSX_WRITE_REG(chip, CARD_PULL_CTL5, 0xFF,
-				       MS_INS_PU | SD_WP_PU |
-				       SD_CD_PU | SD_CMD_PU);
+			retval = rtsx_write_register(chip, CARD_PULL_CTL5,
+						     0xFF,
+						     MS_INS_PU | SD_WP_PU | SD_CD_PU | SD_CMD_PU);
+			if (retval) {
+				rtsx_trace(chip);
+				return retval;
+			}
 		} else {
-			RTSX_WRITE_REG(chip, FPGA_PULL_CTL, 0xFF,
-				       FPGA_SD_PULL_CTL_EN);
+			retval = rtsx_write_register(chip, FPGA_PULL_CTL,
+						     0xFF,
+						     FPGA_SD_PULL_CTL_EN);
+			if (retval) {
+				rtsx_trace(chip);
+				return retval;
+			}
+		}
+		retval = rtsx_write_register(chip, CARD_SHARE_MODE, 0xFF,
+					     CARD_SHARE_48_SD);
+		if (retval) {
+			rtsx_trace(chip);
+			return retval;
 		}
-		RTSX_WRITE_REG(chip, CARD_SHARE_MODE, 0xFF, CARD_SHARE_48_SD);
 
 		/* Enable SDIO internal clock */
-		RTSX_WRITE_REG(chip, 0xFF2C, 0x01, 0x01);
+		retval = rtsx_write_register(chip, 0xFF2C, 0x01, 0x01);
+		if (retval) {
+			rtsx_trace(chip);
+			return retval;
+		}
 
-		RTSX_WRITE_REG(chip, SDIO_CTRL, 0xFF,
-			       SDIO_BUS_CTRL | SDIO_CD_CTRL);
+		retval = rtsx_write_register(chip, SDIO_CTRL, 0xFF,
+					     SDIO_BUS_CTRL | SDIO_CD_CTRL);
+		if (retval) {
+			rtsx_trace(chip);
+			return retval;
+		}
 
 		chip->sd_int = 1;
 		chip->sd_io = 1;
@@ -153,22 +170,30 @@ static int rtsx_pre_handle_sdio_old(struct rtsx_chip *chip)
 static int rtsx_pre_handle_sdio_new(struct rtsx_chip *chip)
 {
 	u8 tmp;
-	int sw_bypass_sd = 0;
+	bool sw_bypass_sd = false;
 	int retval;
 
 	if (chip->driver_first_load) {
 		if (CHECK_PID(chip, 0x5288)) {
-			RTSX_READ_REG(chip, 0xFE5A, &tmp);
+			retval = rtsx_read_register(chip, 0xFE5A, &tmp);
+			if (retval) {
+				rtsx_trace(chip);
+				return retval;
+			}
 			if (tmp & 0x08)
-				sw_bypass_sd = 1;
+				sw_bypass_sd = true;
 		} else if (CHECK_PID(chip, 0x5208)) {
-			RTSX_READ_REG(chip, 0xFE70, &tmp);
+			retval = rtsx_read_register(chip, 0xFE70, &tmp);
+			if (retval) {
+				rtsx_trace(chip);
+				return retval;
+			}
 			if (tmp & 0x80)
-				sw_bypass_sd = 1;
+				sw_bypass_sd = true;
 		}
 	} else {
 		if (chip->sdio_in_charge)
-			sw_bypass_sd = 1;
+			sw_bypass_sd = true;
 	}
 	dev_dbg(rtsx_dev(chip), "chip->sdio_in_charge = %d\n",
 		chip->sdio_in_charge);
@@ -180,17 +205,37 @@ static int rtsx_pre_handle_sdio_new(struct rtsx_chip *chip)
 	if (sw_bypass_sd) {
 		u8 cd_toggle_mask = 0;
 
-		RTSX_READ_REG(chip, TLPTISTAT, &tmp);
+		retval = rtsx_read_register(chip, TLPTISTAT, &tmp);
+		if (retval) {
+			rtsx_trace(chip);
+			return retval;
+		}
 		cd_toggle_mask = 0x08;
 
 		if (tmp & cd_toggle_mask) {
 			/* Disable sdio_bus_auto_switch */
-			if (CHECK_PID(chip, 0x5288))
-				RTSX_WRITE_REG(chip, 0xFE5A, 0x08, 0x00);
-			else if (CHECK_PID(chip, 0x5208))
-				RTSX_WRITE_REG(chip, 0xFE70, 0x80, 0x00);
+			if (CHECK_PID(chip, 0x5288)) {
+				retval = rtsx_write_register(chip, 0xFE5A,
+							     0x08, 0x00);
+				if (retval) {
+					rtsx_trace(chip);
+					return retval;
+				}
+			} else if (CHECK_PID(chip, 0x5208)) {
+				retval = rtsx_write_register(chip, 0xFE70,
+							     0x80, 0x00);
+				if (retval) {
+					rtsx_trace(chip);
+					return retval;
+				}
+			}
 
-			RTSX_WRITE_REG(chip, TLPTISTAT, 0xFF, tmp);
+			retval = rtsx_write_register(chip, TLPTISTAT, 0xFF,
+						     tmp);
+			if (retval) {
+				rtsx_trace(chip);
+				return retval;
+			}
 
 			chip->need_reset |= SD_CARD;
 		} else {
@@ -198,27 +243,52 @@ static int rtsx_pre_handle_sdio_new(struct rtsx_chip *chip)
 
 			if (chip->asic_code) {
 				retval = sd_pull_ctl_enable(chip);
-				if (retval != STATUS_SUCCESS)
-					TRACE_RET(chip, STATUS_FAIL);
+				if (retval != STATUS_SUCCESS) {
+					rtsx_trace(chip);
+					return STATUS_FAIL;
+				}
 			} else {
-				RTSX_WRITE_REG(chip, FPGA_PULL_CTL,
-					       FPGA_SD_PULL_CTL_BIT | 0x20, 0);
+				retval = rtsx_write_register(chip,
+							     FPGA_PULL_CTL,
+							     FPGA_SD_PULL_CTL_BIT | 0x20,
+							     0);
+				if (retval) {
+					rtsx_trace(chip);
+					return retval;
+				}
 			}
 			retval = card_share_mode(chip, SD_CARD);
-			if (retval != STATUS_SUCCESS)
-				TRACE_RET(chip, STATUS_FAIL);
+			if (retval != STATUS_SUCCESS) {
+				rtsx_trace(chip);
+				return STATUS_FAIL;
+			}
 
 			/* Enable sdio_bus_auto_switch */
-			if (CHECK_PID(chip, 0x5288))
-				RTSX_WRITE_REG(chip, 0xFE5A, 0x08, 0x08);
-			else if (CHECK_PID(chip, 0x5208))
-				RTSX_WRITE_REG(chip, 0xFE70, 0x80, 0x80);
+			if (CHECK_PID(chip, 0x5288)) {
+				retval = rtsx_write_register(chip, 0xFE5A,
+							     0x08, 0x08);
+				if (retval) {
+					rtsx_trace(chip);
+					return retval;
+				}
+			} else if (CHECK_PID(chip, 0x5208)) {
+				retval = rtsx_write_register(chip, 0xFE70,
+							     0x80, 0x80);
+				if (retval) {
+					rtsx_trace(chip);
+					return retval;
+				}
+			}
 
 			chip->chip_insert_with_sdio = 1;
 			chip->sd_io = 1;
 		}
 	} else {
-		RTSX_WRITE_REG(chip, TLPTISTAT, 0x08, 0x08);
+		retval = rtsx_write_register(chip, TLPTISTAT, 0x08, 0x08);
+		if (retval) {
+			rtsx_trace(chip);
+			return retval;
+		}
 
 		chip->need_reset |= SD_CARD;
 	}
@@ -237,25 +307,36 @@ static int rtsx_reset_aspm(struct rtsx_chip *chip)
 
 		ret = rtsx_write_cfg_dw(chip, 2, 0xC0, 0xFF,
 					chip->aspm_l0s_l1_en);
-		if (ret != STATUS_SUCCESS)
-			TRACE_RET(chip, STATUS_FAIL);
+		if (ret != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 
 		return STATUS_SUCCESS;
 	}
 
-	if (CHECK_PID(chip, 0x5208))
-		RTSX_WRITE_REG(chip, ASPM_FORCE_CTL, 0xFF, 0x3F);
+	if (CHECK_PID(chip, 0x5208)) {
+		ret = rtsx_write_register(chip, ASPM_FORCE_CTL, 0xFF, 0x3F);
+		if (ret) {
+			rtsx_trace(chip);
+			return ret;
+		}
+	}
 	ret = rtsx_write_config_byte(chip, LCTLR, chip->aspm_l0s_l1_en);
-	if (ret != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (ret != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	chip->aspm_level[0] = chip->aspm_l0s_l1_en;
 	if (CHK_SDIO_EXIST(chip)) {
 		chip->aspm_level[1] = chip->aspm_l0s_l1_en;
 		ret = rtsx_write_cfg_dw(chip, CHECK_PID(chip, 0x5288) ? 2 : 1,
 					0xC0, 0xFF, chip->aspm_l0s_l1_en);
-		if (ret != STATUS_SUCCESS)
-			TRACE_RET(chip, STATUS_FAIL);
+		if (ret != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 	}
 
 	chip->aspm_enabled = 1;
@@ -273,7 +354,11 @@ static int rtsx_enable_pcie_intr(struct rtsx_chip *chip)
 	}
 
 	if (chip->phy_debug_mode) {
-		RTSX_WRITE_REG(chip, CDRESUMECTL, 0x77, 0);
+		ret = rtsx_write_register(chip, CDRESUMECTL, 0x77, 0);
+		if (ret) {
+			rtsx_trace(chip);
+			return ret;
+		}
 		rtsx_disable_bus_int(chip);
 	} else {
 		rtsx_enable_bus_int(chip);
@@ -283,23 +368,31 @@ static int rtsx_enable_pcie_intr(struct rtsx_chip *chip)
 		u16 reg;
 
 		ret = rtsx_read_phy_register(chip, 0x00, &reg);
-		if (ret != STATUS_SUCCESS)
-			TRACE_RET(chip, STATUS_FAIL);
+		if (ret != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 
 		reg &= 0xFE7F;
 		reg |= 0x80;
 		ret = rtsx_write_phy_register(chip, 0x00, reg);
-		if (ret != STATUS_SUCCESS)
-			TRACE_RET(chip, STATUS_FAIL);
+		if (ret != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 
 		ret = rtsx_read_phy_register(chip, 0x1C, &reg);
-		if (ret != STATUS_SUCCESS)
-			TRACE_RET(chip, STATUS_FAIL);
+		if (ret != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 
 		reg &= 0xFFF7;
 		ret = rtsx_write_phy_register(chip, 0x1C, reg);
-		if (ret != STATUS_SUCCESS)
-			TRACE_RET(chip, STATUS_FAIL);
+		if (ret != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 	}
 
 	if (chip->driver_first_load && (chip->ic_version < IC_VER_C))
@@ -316,47 +409,116 @@ int rtsx_reset_chip(struct rtsx_chip *chip)
 
 	rtsx_disable_aspm(chip);
 
-	RTSX_WRITE_REG(chip, HOST_SLEEP_STATE, 0x03, 0x00);
+	retval = rtsx_write_register(chip, HOST_SLEEP_STATE, 0x03, 0x00);
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
 
 	/* Disable card clock */
-	RTSX_WRITE_REG(chip, CARD_CLK_EN, 0x1E, 0);
+	retval = rtsx_write_register(chip, CARD_CLK_EN, 0x1E, 0);
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
 
 #ifdef SUPPORT_OCP
 	/* SSC power on, OCD power on */
-	if (CHECK_LUN_MODE(chip, SD_MS_2LUN))
-		RTSX_WRITE_REG(chip, FPDCTL, OC_POWER_DOWN, 0);
-	else
-		RTSX_WRITE_REG(chip, FPDCTL, OC_POWER_DOWN, MS_OC_POWER_DOWN);
+	if (CHECK_LUN_MODE(chip, SD_MS_2LUN)) {
+		retval = rtsx_write_register(chip, FPDCTL, OC_POWER_DOWN, 0);
+		if (retval) {
+			rtsx_trace(chip);
+			return retval;
+		}
+	} else {
+		retval = rtsx_write_register(chip, FPDCTL, OC_POWER_DOWN,
+					     MS_OC_POWER_DOWN);
+		if (retval) {
+			rtsx_trace(chip);
+			return retval;
+		}
+	}
 
-	RTSX_WRITE_REG(chip, OCPPARA1, OCP_TIME_MASK, OCP_TIME_800);
-	RTSX_WRITE_REG(chip, OCPPARA2, OCP_THD_MASK, OCP_THD_244_946);
-	RTSX_WRITE_REG(chip, OCPCTL, 0xFF, CARD_OC_INT_EN | CARD_DETECT_EN);
+	retval = rtsx_write_register(chip, OCPPARA1, OCP_TIME_MASK,
+				     OCP_TIME_800);
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
+	retval = rtsx_write_register(chip, OCPPARA2, OCP_THD_MASK,
+				     OCP_THD_244_946);
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
+	retval = rtsx_write_register(chip, OCPCTL, 0xFF,
+				     CARD_OC_INT_EN | CARD_DETECT_EN);
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
 #else
 	/* OC power down */
-	RTSX_WRITE_REG(chip, FPDCTL, OC_POWER_DOWN, OC_POWER_DOWN);
+	retval = rtsx_write_register(chip, FPDCTL, OC_POWER_DOWN,
+				     OC_POWER_DOWN);
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
 #endif
 
-	if (!CHECK_PID(chip, 0x5288))
-		RTSX_WRITE_REG(chip, CARD_GPIO_DIR, 0xFF, 0x03);
+	if (!CHECK_PID(chip, 0x5288)) {
+		retval = rtsx_write_register(chip, CARD_GPIO_DIR, 0xFF, 0x03);
+		if (retval) {
+			rtsx_trace(chip);
+			return retval;
+		}
+	}
 
 	/* Turn off LED */
-	RTSX_WRITE_REG(chip, CARD_GPIO, 0xFF, 0x03);
+	retval = rtsx_write_register(chip, CARD_GPIO, 0xFF, 0x03);
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
 
 	/* Reset delink mode */
-	RTSX_WRITE_REG(chip, CHANGE_LINK_STATE, 0x0A, 0);
+	retval = rtsx_write_register(chip, CHANGE_LINK_STATE, 0x0A, 0);
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
 
 	/* Card driving select */
-	RTSX_WRITE_REG(chip, CARD_DRIVE_SEL, 0xFF, chip->card_drive_sel);
+	retval = rtsx_write_register(chip, CARD_DRIVE_SEL, 0xFF,
+				     chip->card_drive_sel);
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
 
 #ifdef LED_AUTO_BLINK
-	RTSX_WRITE_REG(chip, CARD_AUTO_BLINK, 0xFF,
-		       LED_BLINK_SPEED | BLINK_EN | LED_GPIO0);
+	retval = rtsx_write_register(chip, CARD_AUTO_BLINK, 0xFF,
+				     LED_BLINK_SPEED | BLINK_EN | LED_GPIO0);
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
 #endif
 
 	if (chip->asic_code) {
 		/* Enable SSC Clock */
-		RTSX_WRITE_REG(chip, SSC_CTL1, 0xFF, SSC_8X_EN | SSC_SEL_4M);
-		RTSX_WRITE_REG(chip, SSC_CTL2, 0xFF, 0x12);
+		retval = rtsx_write_register(chip, SSC_CTL1, 0xFF,
+					     SSC_8X_EN | SSC_SEL_4M);
+		if (retval) {
+			rtsx_trace(chip);
+			return retval;
+		}
+		retval = rtsx_write_register(chip, SSC_CTL2, 0xFF, 0x12);
+		if (retval) {
+			rtsx_trace(chip);
+			return retval;
+		}
 	}
 
 	/* Disable cd_pwr_save (u_force_rst_core_en=0, u_cd_rst_core_en=0)
@@ -366,55 +528,84 @@ int rtsx_reset_chip(struct rtsx_chip *chip)
 	      bit[5]    u_mac_phy_rst_n_dbg	rst_value = 1
 	      bit[4]	u_non_sticky_rst_n_dbg	rst_value = 0
 	*/
-	RTSX_WRITE_REG(chip, CHANGE_LINK_STATE, 0x16, 0x10);
+	retval = rtsx_write_register(chip, CHANGE_LINK_STATE, 0x16, 0x10);
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
 
 	/* Enable ASPM */
 	if (chip->aspm_l0s_l1_en) {
 		retval = rtsx_reset_aspm(chip);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, STATUS_FAIL);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 	} else {
 		if (chip->asic_code && CHECK_PID(chip, 0x5208)) {
 			retval = rtsx_write_phy_register(chip, 0x07, 0x0129);
-			if (retval != STATUS_SUCCESS)
-				TRACE_RET(chip, STATUS_FAIL);
+			if (retval != STATUS_SUCCESS) {
+				rtsx_trace(chip);
+				return STATUS_FAIL;
+			}
 		}
 		retval = rtsx_write_config_byte(chip, LCTLR,
 						chip->aspm_l0s_l1_en);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, STATUS_FAIL);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 	}
 
 	retval = rtsx_write_config_byte(chip, 0x81, 1);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	if (CHK_SDIO_EXIST(chip)) {
 		retval = rtsx_write_cfg_dw(chip,
 					   CHECK_PID(chip, 0x5288) ? 2 : 1,
 					   0xC0, 0xFF00, 0x0100);
 
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, STATUS_FAIL);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 	}
 
 	if (CHECK_PID(chip, 0x5288) && !CHK_SDIO_EXIST(chip)) {
 		retval = rtsx_write_cfg_dw(chip, 2, 0xC0, 0xFFFF, 0x0103);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, STATUS_FAIL);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 
 		retval = rtsx_write_cfg_dw(chip, 2, 0x84, 0xFF, 0x03);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, STATUS_FAIL);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 	}
 
-	RTSX_WRITE_REG(chip, IRQSTAT0, LINK_RDY_INT, LINK_RDY_INT);
+	retval = rtsx_write_register(chip, IRQSTAT0, LINK_RDY_INT,
+				     LINK_RDY_INT);
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
 
-	RTSX_WRITE_REG(chip, PERST_GLITCH_WIDTH, 0xFF, 0x80);
+	retval = rtsx_write_register(chip, PERST_GLITCH_WIDTH, 0xFF, 0x80);
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
 
 	retval = rtsx_enable_pcie_intr(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	chip->need_reset = 0;
 
@@ -436,13 +627,19 @@ int rtsx_reset_chip(struct rtsx_chip *chip)
 #else  /* HW_AUTO_SWITCH_SD_BUS */
 		retval = rtsx_pre_handle_sdio_old(chip);
 #endif  /* HW_AUTO_SWITCH_SD_BUS */
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, STATUS_FAIL);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 
 	} else {
 		chip->sd_io = 0;
-		RTSX_WRITE_REG(chip, SDIO_CTRL, SDIO_BUS_CTRL | SDIO_CD_CTRL,
-			       0);
+		retval = rtsx_write_register(chip, SDIO_CTRL,
+					     SDIO_BUS_CTRL | SDIO_CD_CTRL, 0);
+		if (retval) {
+			rtsx_trace(chip);
+			return retval;
+		}
 	}
 
 nextcard:
@@ -450,43 +647,91 @@ nextcard:
 		chip->need_reset |= XD_CARD;
 	if (chip->int_reg & MS_EXIST)
 		chip->need_reset |= MS_CARD;
-	if (chip->int_reg & CARD_EXIST)
-		RTSX_WRITE_REG(chip, SSC_CTL1, SSC_RSTB, SSC_RSTB);
+	if (chip->int_reg & CARD_EXIST) {
+		retval = rtsx_write_register(chip, SSC_CTL1, SSC_RSTB,
+					     SSC_RSTB);
+		if (retval) {
+			rtsx_trace(chip);
+			return retval;
+		}
+	}
 
 	dev_dbg(rtsx_dev(chip), "In %s, chip->need_reset = 0x%x\n", __func__,
 		(unsigned int)(chip->need_reset));
 
-	RTSX_WRITE_REG(chip, RCCTL, 0x01, 0x00);
+	retval = rtsx_write_register(chip, RCCTL, 0x01, 0x00);
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
 
 	if (CHECK_PID(chip, 0x5208) || CHECK_PID(chip, 0x5288)) {
 		/* Turn off main power when entering S3/S4 state */
-		RTSX_WRITE_REG(chip, MAIN_PWR_OFF_CTL, 0x03, 0x03);
+		retval = rtsx_write_register(chip, MAIN_PWR_OFF_CTL, 0x03,
+					     0x03);
+		if (retval) {
+			rtsx_trace(chip);
+			return retval;
+		}
 	}
 
 	if (chip->remote_wakeup_en && !chip->auto_delink_en) {
-		RTSX_WRITE_REG(chip, WAKE_SEL_CTL, 0x07, 0x07);
-		if (chip->aux_pwr_exist)
-			RTSX_WRITE_REG(chip, PME_FORCE_CTL, 0xFF, 0x33);
+		retval = rtsx_write_register(chip, WAKE_SEL_CTL, 0x07, 0x07);
+		if (retval) {
+			rtsx_trace(chip);
+			return retval;
+		}
+		if (chip->aux_pwr_exist) {
+			retval = rtsx_write_register(chip, PME_FORCE_CTL,
+						     0xFF, 0x33);
+			if (retval) {
+				rtsx_trace(chip);
+				return retval;
+			}
+		}
 	} else {
-		RTSX_WRITE_REG(chip, WAKE_SEL_CTL, 0x07, 0x04);
-		RTSX_WRITE_REG(chip, PME_FORCE_CTL, 0xFF, 0x30);
+		retval = rtsx_write_register(chip, WAKE_SEL_CTL, 0x07, 0x04);
+		if (retval) {
+			rtsx_trace(chip);
+			return retval;
+		}
+		retval = rtsx_write_register(chip, PME_FORCE_CTL, 0xFF, 0x30);
+		if (retval) {
+			rtsx_trace(chip);
+			return retval;
+		}
 	}
 
-	if (CHECK_PID(chip, 0x5208) && (chip->ic_version >= IC_VER_D))
-		RTSX_WRITE_REG(chip, PETXCFG, 0x1C, 0x14);
+	if (CHECK_PID(chip, 0x5208) && (chip->ic_version >= IC_VER_D)) {
+		retval = rtsx_write_register(chip, PETXCFG, 0x1C, 0x14);
+		if (retval) {
+			rtsx_trace(chip);
+			return retval;
+		}
+	}
 
 	if (chip->asic_code && CHECK_PID(chip, 0x5208)) {
 		retval = rtsx_clr_phy_reg_bit(chip, 0x1C, 2);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, STATUS_FAIL);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 	}
 
 	if (chip->ft2_fast_mode) {
-		RTSX_WRITE_REG(chip, CARD_PWR_CTL, 0xFF,
-			       MS_PARTIAL_POWER_ON | SD_PARTIAL_POWER_ON);
+		retval = rtsx_write_register(chip, CARD_PWR_CTL, 0xFF,
+					     MS_PARTIAL_POWER_ON | SD_PARTIAL_POWER_ON);
+		if (retval) {
+			rtsx_trace(chip);
+			return retval;
+		}
 		udelay(chip->pmos_pwr_on_interval);
-		RTSX_WRITE_REG(chip, CARD_PWR_CTL, 0xFF,
-			       MS_POWER_ON | SD_POWER_ON);
+		retval = rtsx_write_register(chip, CARD_PWR_CTL, 0xFF,
+					     MS_POWER_ON | SD_POWER_ON);
+		if (retval) {
+			rtsx_trace(chip);
+			return retval;
+		}
 
 		wait_timeout(200);
 	}
@@ -501,13 +746,14 @@ nextcard:
 
 static inline int check_sd_speed_prior(u32 sd_speed_prior)
 {
-	int i, fake_para = 0;
+	bool fake_para = false;
+	int i;
 
 	for (i = 0; i < 4; i++) {
 		u8 tmp = (u8)(sd_speed_prior >> (i*8));
 
 		if ((tmp < 0x01) || (tmp > 0x04)) {
-			fake_para = 1;
+			fake_para = true;
 			break;
 		}
 	}
@@ -517,13 +763,14 @@ static inline int check_sd_speed_prior(u32 sd_speed_prior)
 
 static inline int check_sd_current_prior(u32 sd_current_prior)
 {
-	int i, fake_para = 0;
+	bool fake_para = false;
+	int i;
 
 	for (i = 0; i < 4; i++) {
 		u8 tmp = (u8)(sd_current_prior >> (i*8));
 
 		if (tmp > 0x03) {
-			fake_para = 1;
+			fake_para = true;
 			break;
 		}
 	}
@@ -537,14 +784,24 @@ static int rts5208_init(struct rtsx_chip *chip)
 	u16 reg = 0;
 	u8 val = 0;
 
-	RTSX_WRITE_REG(chip, CLK_SEL, 0x03, 0x03);
-	RTSX_READ_REG(chip, CLK_SEL, &val);
+	retval = rtsx_write_register(chip, CLK_SEL, 0x03, 0x03);
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
+	retval = rtsx_read_register(chip, CLK_SEL, &val);
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
 	chip->asic_code = val == 0 ? 1 : 0;
 
 	if (chip->asic_code) {
 		retval = rtsx_read_phy_register(chip, 0x1C, &reg);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, STATUS_FAIL);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 
 		dev_dbg(rtsx_dev(chip), "Value of phy register 0x1C is 0x%x\n",
 			reg);
@@ -552,16 +809,28 @@ static int rts5208_init(struct rtsx_chip *chip)
 		chip->phy_debug_mode = reg & PHY_DEBUG_MODE ? 1 : 0;
 
 	} else {
-		RTSX_READ_REG(chip, 0xFE80, &val);
+		retval = rtsx_read_register(chip, 0xFE80, &val);
+		if (retval) {
+			rtsx_trace(chip);
+			return retval;
+		}
 		chip->ic_version = val;
 		chip->phy_debug_mode = 0;
 	}
 
-	RTSX_READ_REG(chip, PDINFO, &val);
+	retval = rtsx_read_register(chip, PDINFO, &val);
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
 	dev_dbg(rtsx_dev(chip), "PDINFO: 0x%x\n", val);
 	chip->aux_pwr_exist = val & AUX_PWR_DETECTED ? 1 : 0;
 
-	RTSX_READ_REG(chip, 0xFE50, &val);
+	retval = rtsx_read_register(chip, 0xFE50, &val);
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
 	chip->hw_bypass_sd = val & 0x01 ? 1 : 0;
 
 	rtsx_read_config_byte(chip, 0x0E, &val);
@@ -571,7 +840,11 @@ static int rts5208_init(struct rtsx_chip *chip)
 		CLR_SDIO_EXIST(chip);
 
 	if (chip->use_hw_setting) {
-		RTSX_READ_REG(chip, CHANGE_LINK_STATE, &val);
+		retval = rtsx_read_register(chip, CHANGE_LINK_STATE, &val);
+		if (retval) {
+			rtsx_trace(chip);
+			return retval;
+		}
 		chip->auto_delink_en = val & 0x80 ? 1 : 0;
 	}
 
@@ -584,27 +857,49 @@ static int rts5288_init(struct rtsx_chip *chip)
 	u8 val = 0, max_func;
 	u32 lval = 0;
 
-	RTSX_WRITE_REG(chip, CLK_SEL, 0x03, 0x03);
-	RTSX_READ_REG(chip, CLK_SEL, &val);
+	retval = rtsx_write_register(chip, CLK_SEL, 0x03, 0x03);
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
+	retval = rtsx_read_register(chip, CLK_SEL, &val);
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
 	chip->asic_code = val == 0 ? 1 : 0;
 
 	chip->ic_version = 0;
 	chip->phy_debug_mode = 0;
 
-	RTSX_READ_REG(chip, PDINFO, &val);
+	retval = rtsx_read_register(chip, PDINFO, &val);
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
 	dev_dbg(rtsx_dev(chip), "PDINFO: 0x%x\n", val);
 	chip->aux_pwr_exist = val & AUX_PWR_DETECTED ? 1 : 0;
 
-	RTSX_READ_REG(chip, CARD_SHARE_MODE, &val);
+	retval = rtsx_read_register(chip, CARD_SHARE_MODE, &val);
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
 	dev_dbg(rtsx_dev(chip), "CARD_SHARE_MODE: 0x%x\n", val);
 	chip->baro_pkg = val & 0x04 ? QFN : LQFP;
 
-	RTSX_READ_REG(chip, 0xFE5A, &val);
+	retval = rtsx_read_register(chip, 0xFE5A, &val);
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
 	chip->hw_bypass_sd = val & 0x10 ? 1 : 0;
 
 	retval = rtsx_read_cfg_dw(chip, 0, 0x718, &lval);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	max_func = (u8)((lval >> 29) & 0x07);
 	dev_dbg(rtsx_dev(chip), "Max function number: %d\n", max_func);
@@ -614,7 +909,11 @@ static int rts5288_init(struct rtsx_chip *chip)
 		CLR_SDIO_EXIST(chip);
 
 	if (chip->use_hw_setting) {
-		RTSX_READ_REG(chip, CHANGE_LINK_STATE, &val);
+		retval = rtsx_read_register(chip, CHANGE_LINK_STATE, &val);
+		if (retval) {
+			rtsx_trace(chip);
+			return retval;
+		}
 		chip->auto_delink_en = val & 0x80 ? 1 : 0;
 
 		if (CHECK_BARO_PKG(chip, LQFP))
@@ -693,21 +992,33 @@ int rtsx_init_chip(struct rtsx_chip *chip)
 	if ((chip->mmc_ddr_tx_phase > 31) || (chip->mmc_ddr_tx_phase < 0))
 		chip->mmc_ddr_tx_phase = 0;
 
-	RTSX_WRITE_REG(chip, FPDCTL, SSC_POWER_DOWN, 0);
+	retval = rtsx_write_register(chip, FPDCTL, SSC_POWER_DOWN, 0);
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
 	wait_timeout(200);
-	RTSX_WRITE_REG(chip, CLK_DIV, 0x07, 0x07);
+	retval = rtsx_write_register(chip, CLK_DIV, 0x07, 0x07);
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
 	dev_dbg(rtsx_dev(chip), "chip->use_hw_setting = %d\n",
 		chip->use_hw_setting);
 
 	if (CHECK_PID(chip, 0x5208)) {
 		retval = rts5208_init(chip);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, STATUS_FAIL);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 
 	} else if (CHECK_PID(chip, 0x5288)) {
 		retval = rts5288_init(chip);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, STATUS_FAIL);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 	}
 
 	if (chip->ss_en == 2)
@@ -754,8 +1065,10 @@ int rtsx_init_chip(struct rtsx_chip *chip)
 	}
 
 	retval = rtsx_reset_chip(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	return STATUS_SUCCESS;
 }
@@ -784,31 +1097,31 @@ static inline void rtsx_blink_led(struct rtsx_chip *chip)
 
 static void rtsx_monitor_aspm_config(struct rtsx_chip *chip)
 {
-	int maybe_support_aspm, reg_changed;
+	bool reg_changed, maybe_support_aspm;
 	u32 tmp = 0;
 	u8 reg0 = 0, reg1 = 0;
 
-	maybe_support_aspm = 0;
-	reg_changed = 0;
+	maybe_support_aspm = false;
+	reg_changed = false;
 	rtsx_read_config_byte(chip, LCTLR, &reg0);
 	if (chip->aspm_level[0] != reg0) {
-		reg_changed = 1;
+		reg_changed = true;
 		chip->aspm_level[0] = reg0;
 	}
 	if (CHK_SDIO_EXIST(chip) && !CHK_SDIO_IGNORED(chip)) {
 		rtsx_read_cfg_dw(chip, 1, 0xC0, &tmp);
 		reg1 = (u8)tmp;
 		if (chip->aspm_level[1] != reg1) {
-			reg_changed = 1;
+			reg_changed = true;
 			chip->aspm_level[1] = reg1;
 		}
 
 		if ((reg0 & 0x03) && (reg1 & 0x03))
-			maybe_support_aspm = 1;
+			maybe_support_aspm = true;
 
 	} else {
 		if (reg0 & 0x03)
-			maybe_support_aspm = 1;
+			maybe_support_aspm = true;
 	}
 
 	if (reg_changed) {
@@ -835,7 +1148,7 @@ void rtsx_polling_func(struct rtsx_chip *chip)
 #ifdef SUPPORT_SD_LOCK
 	struct sd_info *sd_card = &chip->sd_card;
 #endif
-	int ss_allowed;
+	bool ss_allowed;
 
 	if (rtsx_chk_stat(chip, RTSX_STAT_SUSPEND))
 		return;
@@ -887,21 +1200,21 @@ void rtsx_polling_func(struct rtsx_chip *chip)
 	rtsx_init_cards(chip);
 
 	if (chip->ss_en) {
-		ss_allowed = 1;
+		ss_allowed = true;
 
 		if (CHECK_PID(chip, 0x5288)) {
-			ss_allowed = 0;
+			ss_allowed = false;
 		} else {
 			if (CHK_SDIO_EXIST(chip) && !CHK_SDIO_IGNORED(chip)) {
 				u32 val;
 
 				rtsx_read_cfg_dw(chip, 1, 0x04, &val);
 				if (val & 0x07)
-					ss_allowed = 0;
+					ss_allowed = false;
 			}
 		}
 	} else {
-		ss_allowed = 0;
+		ss_allowed = false;
 	}
 
 	if (ss_allowed && !chip->sd_io) {
@@ -1153,14 +1466,17 @@ int rtsx_write_register(struct rtsx_chip *chip, u16 addr, u8 mask, u8 data)
 	for (i = 0; i < MAX_RW_REG_CNT; i++) {
 		val = rtsx_readl(chip, RTSX_HAIMR);
 		if ((val & (1 << 31)) == 0) {
-			if (data != (u8)val)
-				TRACE_RET(chip, STATUS_FAIL);
+			if (data != (u8)val) {
+				rtsx_trace(chip);
+				return STATUS_FAIL;
+			}
 
 			return STATUS_SUCCESS;
 		}
 	}
 
-	TRACE_RET(chip, STATUS_TIMEDOUT);
+	rtsx_trace(chip);
+	return STATUS_TIMEDOUT;
 }
 
 int rtsx_read_register(struct rtsx_chip *chip, u16 addr, u8 *data)
@@ -1181,8 +1497,10 @@ int rtsx_read_register(struct rtsx_chip *chip, u16 addr, u8 *data)
 			break;
 	}
 
-	if (i >= MAX_RW_REG_CNT)
-		TRACE_RET(chip, STATUS_TIMEDOUT);
+	if (i >= MAX_RW_REG_CNT) {
+		rtsx_trace(chip);
+		return STATUS_TIMEDOUT;
+	}
 
 	if (data)
 		*data = (u8)(val & 0xFF);
@@ -1193,13 +1511,19 @@ int rtsx_read_register(struct rtsx_chip *chip, u16 addr, u8 *data)
 int rtsx_write_cfg_dw(struct rtsx_chip *chip, u8 func_no, u16 addr, u32 mask,
 		      u32 val)
 {
+	int retval;
 	u8 mode = 0, tmp;
 	int i;
 
 	for (i = 0; i < 4; i++) {
 		if (mask & 0xFF) {
-			RTSX_WRITE_REG(chip, CFGDATA0 + i,
-				       0xFF, (u8)(val & mask & 0xFF));
+			retval = rtsx_write_register(chip, CFGDATA0 + i,
+						     0xFF,
+						     (u8)(val & mask & 0xFF));
+			if (retval) {
+				rtsx_trace(chip);
+				return retval;
+			}
 			mode |= (1 << i);
 		}
 		mask >>= 8;
@@ -1207,14 +1531,31 @@ int rtsx_write_cfg_dw(struct rtsx_chip *chip, u8 func_no, u16 addr, u32 mask,
 	}
 
 	if (mode) {
-		RTSX_WRITE_REG(chip, CFGADDR0, 0xFF, (u8)addr);
-		RTSX_WRITE_REG(chip, CFGADDR1, 0xFF, (u8)(addr >> 8));
+		retval = rtsx_write_register(chip, CFGADDR0, 0xFF, (u8)addr);
+		if (retval) {
+			rtsx_trace(chip);
+			return retval;
+		}
+		retval = rtsx_write_register(chip, CFGADDR1, 0xFF,
+					     (u8)(addr >> 8));
+		if (retval) {
+			rtsx_trace(chip);
+			return retval;
+		}
 
-		RTSX_WRITE_REG(chip, CFGRWCTL, 0xFF,
-			       0x80 | mode | ((func_no & 0x03) << 4));
+		retval = rtsx_write_register(chip, CFGRWCTL, 0xFF,
+					     0x80 | mode | ((func_no & 0x03) << 4));
+		if (retval) {
+			rtsx_trace(chip);
+			return retval;
+		}
 
 		for (i = 0; i < MAX_RW_REG_CNT; i++) {
-			RTSX_READ_REG(chip, CFGRWCTL, &tmp);
+			retval = rtsx_read_register(chip, CFGRWCTL, &tmp);
+			if (retval) {
+				rtsx_trace(chip);
+				return retval;
+			}
 			if ((tmp & 0x80) == 0)
 				break;
 		}
@@ -1225,22 +1566,44 @@ int rtsx_write_cfg_dw(struct rtsx_chip *chip, u8 func_no, u16 addr, u32 mask,
 
 int rtsx_read_cfg_dw(struct rtsx_chip *chip, u8 func_no, u16 addr, u32 *val)
 {
+	int retval;
 	int i;
 	u8 tmp;
 	u32 data = 0;
 
-	RTSX_WRITE_REG(chip, CFGADDR0, 0xFF, (u8)addr);
-	RTSX_WRITE_REG(chip, CFGADDR1, 0xFF, (u8)(addr >> 8));
-	RTSX_WRITE_REG(chip, CFGRWCTL, 0xFF, 0x80 | ((func_no & 0x03) << 4));
+	retval = rtsx_write_register(chip, CFGADDR0, 0xFF, (u8)addr);
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
+	retval = rtsx_write_register(chip, CFGADDR1, 0xFF, (u8)(addr >> 8));
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
+	retval = rtsx_write_register(chip, CFGRWCTL, 0xFF,
+				     0x80 | ((func_no & 0x03) << 4));
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
 
 	for (i = 0; i < MAX_RW_REG_CNT; i++) {
-		RTSX_READ_REG(chip, CFGRWCTL, &tmp);
+		retval = rtsx_read_register(chip, CFGRWCTL, &tmp);
+		if (retval) {
+			rtsx_trace(chip);
+			return retval;
+		}
 		if ((tmp & 0x80) == 0)
 			break;
 	}
 
 	for (i = 0; i < 4; i++) {
-		RTSX_READ_REG(chip, CFGDATA0 + i, &tmp);
+		retval = rtsx_read_register(chip, CFGDATA0 + i, &tmp);
+		if (retval) {
+			rtsx_trace(chip);
+			return retval;
+		}
 		data |= (u32)tmp << (i * 8);
 	}
 
@@ -1259,8 +1622,10 @@ int rtsx_write_cfg_seq(struct rtsx_chip *chip, u8 func, u16 addr, u8 *buf,
 	int dw_len, i, j;
 	int retval;
 
-	if (!buf)
-		TRACE_RET(chip, STATUS_NOMEM);
+	if (!buf) {
+		rtsx_trace(chip);
+		return STATUS_NOMEM;
+	}
 
 	if ((len + offset) % 4)
 		dw_len = (len + offset) / 4 + 1;
@@ -1270,13 +1635,16 @@ int rtsx_write_cfg_seq(struct rtsx_chip *chip, u8 func, u16 addr, u8 *buf,
 	dev_dbg(rtsx_dev(chip), "dw_len = %d\n", dw_len);
 
 	data = vzalloc(dw_len * 4);
-	if (!data)
-		TRACE_RET(chip, STATUS_NOMEM);
+	if (!data) {
+		rtsx_trace(chip);
+		return STATUS_NOMEM;
+	}
 
 	mask = vzalloc(dw_len * 4);
 	if (!mask) {
 		vfree(data);
-		TRACE_RET(chip, STATUS_NOMEM);
+		rtsx_trace(chip);
+		return STATUS_NOMEM;
 	}
 
 	j = 0;
@@ -1300,7 +1668,8 @@ int rtsx_write_cfg_seq(struct rtsx_chip *chip, u8 func, u16 addr, u8 *buf,
 		if (retval != STATUS_SUCCESS) {
 			vfree(data);
 			vfree(mask);
-			TRACE_RET(chip, STATUS_FAIL);
+			rtsx_trace(chip);
+			return STATUS_FAIL;
 		}
 	}
 
@@ -1327,15 +1696,18 @@ int rtsx_read_cfg_seq(struct rtsx_chip *chip, u8 func, u16 addr, u8 *buf,
 	dev_dbg(rtsx_dev(chip), "dw_len = %d\n", dw_len);
 
 	data = vmalloc(dw_len * 4);
-	if (!data)
-		TRACE_RET(chip, STATUS_NOMEM);
+	if (!data) {
+		rtsx_trace(chip);
+		return STATUS_NOMEM;
+	}
 
 	for (i = 0; i < dw_len; i++) {
 		retval = rtsx_read_cfg_dw(chip, func, aligned_addr + i * 4,
 					  data + i);
 		if (retval != STATUS_SUCCESS) {
 			vfree(data);
-			TRACE_RET(chip, STATUS_FAIL);
+			rtsx_trace(chip);
+			return STATUS_FAIL;
 		}
 	}
 
@@ -1358,51 +1730,99 @@ int rtsx_read_cfg_seq(struct rtsx_chip *chip, u8 func, u16 addr, u8 *buf,
 
 int rtsx_write_phy_register(struct rtsx_chip *chip, u8 addr, u16 val)
 {
-	int i, finished = 0;
+	int retval;
+	bool finished = false;
+	int i;
 	u8 tmp;
 
-	RTSX_WRITE_REG(chip, PHYDATA0, 0xFF, (u8)val);
-	RTSX_WRITE_REG(chip, PHYDATA1, 0xFF, (u8)(val >> 8));
-	RTSX_WRITE_REG(chip, PHYADDR, 0xFF, addr);
-	RTSX_WRITE_REG(chip, PHYRWCTL, 0xFF, 0x81);
+	retval = rtsx_write_register(chip, PHYDATA0, 0xFF, (u8)val);
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
+	retval = rtsx_write_register(chip, PHYDATA1, 0xFF, (u8)(val >> 8));
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
+	retval = rtsx_write_register(chip, PHYADDR, 0xFF, addr);
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
+	retval = rtsx_write_register(chip, PHYRWCTL, 0xFF, 0x81);
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
 
 	for (i = 0; i < 100000; i++) {
-		RTSX_READ_REG(chip, PHYRWCTL, &tmp);
+		retval = rtsx_read_register(chip, PHYRWCTL, &tmp);
+		if (retval) {
+			rtsx_trace(chip);
+			return retval;
+		}
 		if (!(tmp & 0x80)) {
-			finished = 1;
+			finished = true;
 			break;
 		}
 	}
 
-	if (!finished)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (!finished) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	return STATUS_SUCCESS;
 }
 
 int rtsx_read_phy_register(struct rtsx_chip *chip, u8 addr, u16 *val)
 {
-	int i, finished = 0;
+	int retval;
+	bool finished = false;
+	int i;
 	u16 data = 0;
 	u8 tmp;
 
-	RTSX_WRITE_REG(chip, PHYADDR, 0xFF, addr);
-	RTSX_WRITE_REG(chip, PHYRWCTL, 0xFF, 0x80);
+	retval = rtsx_write_register(chip, PHYADDR, 0xFF, addr);
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
+	retval = rtsx_write_register(chip, PHYRWCTL, 0xFF, 0x80);
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
 
 	for (i = 0; i < 100000; i++) {
-		RTSX_READ_REG(chip, PHYRWCTL, &tmp);
+		retval = rtsx_read_register(chip, PHYRWCTL, &tmp);
+		if (retval) {
+			rtsx_trace(chip);
+			return retval;
+		}
 		if (!(tmp & 0x80)) {
-			finished = 1;
+			finished = true;
 			break;
 		}
 	}
 
-	if (!finished)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (!finished) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
-	RTSX_READ_REG(chip, PHYDATA0, &tmp);
+	retval = rtsx_read_register(chip, PHYDATA0, &tmp);
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
 	data = tmp;
-	RTSX_READ_REG(chip, PHYDATA1, &tmp);
+	retval = rtsx_read_register(chip, PHYDATA1, &tmp);
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
 	data |= (u16)tmp << 8;
 
 	if (val)
@@ -1413,22 +1833,37 @@ int rtsx_read_phy_register(struct rtsx_chip *chip, u8 addr, u16 *val)
 
 int rtsx_read_efuse(struct rtsx_chip *chip, u8 addr, u8 *val)
 {
+	int retval;
 	int i;
 	u8 data = 0;
 
-	RTSX_WRITE_REG(chip, EFUSE_CTRL, 0xFF, 0x80|addr);
+	retval = rtsx_write_register(chip, EFUSE_CTRL, 0xFF, 0x80 | addr);
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
 
 	for (i = 0; i < 100; i++) {
-		RTSX_READ_REG(chip, EFUSE_CTRL, &data);
+		retval = rtsx_read_register(chip, EFUSE_CTRL, &data);
+		if (retval) {
+			rtsx_trace(chip);
+			return retval;
+		}
 		if (!(data & 0x80))
 			break;
 		udelay(1);
 	}
 
-	if (data & 0x80)
-		TRACE_RET(chip, STATUS_TIMEDOUT);
+	if (data & 0x80) {
+		rtsx_trace(chip);
+		return STATUS_TIMEDOUT;
+	}
 
-	RTSX_READ_REG(chip, EFUSE_DATA, &data);
+	retval = rtsx_read_register(chip, EFUSE_DATA, &data);
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
 	if (val)
 		*val = data;
 
@@ -1437,6 +1872,7 @@ int rtsx_read_efuse(struct rtsx_chip *chip, u8 addr, u8 *val)
 
 int rtsx_write_efuse(struct rtsx_chip *chip, u8 addr, u8 val)
 {
+	int retval;
 	int i, j;
 	u8 data = 0, tmp = 0xFF;
 
@@ -1447,18 +1883,33 @@ int rtsx_write_efuse(struct rtsx_chip *chip, u8 addr, u8 val)
 		tmp &= (~(u8)(1 << i));
 		dev_dbg(rtsx_dev(chip), "Write 0x%x to 0x%x\n", tmp, addr);
 
-		RTSX_WRITE_REG(chip, EFUSE_DATA, 0xFF, tmp);
-		RTSX_WRITE_REG(chip, EFUSE_CTRL, 0xFF, 0xA0|addr);
+		retval = rtsx_write_register(chip, EFUSE_DATA, 0xFF, tmp);
+		if (retval) {
+			rtsx_trace(chip);
+			return retval;
+		}
+		retval = rtsx_write_register(chip, EFUSE_CTRL, 0xFF,
+					     0xA0 | addr);
+		if (retval) {
+			rtsx_trace(chip);
+			return retval;
+		}
 
 		for (j = 0; j < 100; j++) {
-			RTSX_READ_REG(chip, EFUSE_CTRL, &data);
+			retval = rtsx_read_register(chip, EFUSE_CTRL, &data);
+			if (retval) {
+				rtsx_trace(chip);
+				return retval;
+			}
 			if (!(data & 0x80))
 				break;
 			wait_timeout(3);
 		}
 
-		if (data & 0x80)
-			TRACE_RET(chip, STATUS_TIMEDOUT);
+		if (data & 0x80) {
+			rtsx_trace(chip);
+			return STATUS_TIMEDOUT;
+		}
 
 		wait_timeout(5);
 	}
@@ -1472,14 +1923,18 @@ int rtsx_clr_phy_reg_bit(struct rtsx_chip *chip, u8 reg, u8 bit)
 	u16 value;
 
 	retval = rtsx_read_phy_register(chip, reg, &value);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	if (value & (1 << bit)) {
 		value &= ~(1 << bit);
 		retval = rtsx_write_phy_register(chip, reg, value);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, STATUS_FAIL);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 	}
 
 	return STATUS_SUCCESS;
@@ -1491,14 +1946,18 @@ int rtsx_set_phy_reg_bit(struct rtsx_chip *chip, u8 reg, u8 bit)
 	u16 value;
 
 	retval = rtsx_read_phy_register(chip, reg, &value);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	if ((value & (1 << bit)) == 0) {
 		value |= (1 << bit);
 		retval = rtsx_write_phy_register(chip, reg, value);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, STATUS_FAIL);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 	}
 
 	return STATUS_SUCCESS;
@@ -1506,9 +1965,14 @@ int rtsx_set_phy_reg_bit(struct rtsx_chip *chip, u8 reg, u8 bit)
 
 int rtsx_check_link_ready(struct rtsx_chip *chip)
 {
+	int retval;
 	u8 val;
 
-	RTSX_READ_REG(chip, IRQSTAT0, &val);
+	retval = rtsx_read_register(chip, IRQSTAT0, &val);
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
 
 	dev_dbg(rtsx_dev(chip), "IRQSTAT0: 0x%x\n", val);
 	if (val & LINK_RDY_INT) {
@@ -1615,7 +2079,7 @@ void rtsx_exit_ss(struct rtsx_chip *chip)
 int rtsx_pre_handle_interrupt(struct rtsx_chip *chip)
 {
 	u32 status, int_enable;
-	int exit_ss = 0;
+	bool exit_ss = false;
 #ifdef SUPPORT_OCP
 	u32 ocp_int = 0;
 
@@ -1625,7 +2089,7 @@ int rtsx_pre_handle_interrupt(struct rtsx_chip *chip)
 	if (chip->ss_en) {
 		chip->ss_counter = 0;
 		if (rtsx_get_stat(chip) == RTSX_STAT_SS) {
-			exit_ss = 1;
+			exit_ss = true;
 			rtsx_exit_L1(chip);
 			rtsx_set_stat(chip, RTSX_STAT_RUN);
 		}
@@ -1814,8 +2278,10 @@ int rtsx_read_ppbuf(struct rtsx_chip *chip, u8 *buf, int buf_len)
 	u16 reg_addr;
 	u8 *ptr;
 
-	if (!buf)
-		TRACE_RET(chip, STATUS_ERROR);
+	if (!buf) {
+		rtsx_trace(chip);
+		return STATUS_ERROR;
+	}
 
 	ptr = buf;
 	reg_addr = PPBUF_BASE2;
@@ -1826,8 +2292,10 @@ int rtsx_read_ppbuf(struct rtsx_chip *chip, u8 *buf, int buf_len)
 			rtsx_add_cmd(chip, READ_REG_CMD, reg_addr++, 0, 0);
 
 		retval = rtsx_send_cmd(chip, 0, 250);
-		if (retval < 0)
-			TRACE_RET(chip, STATUS_FAIL);
+		if (retval < 0) {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 
 		memcpy(ptr, rtsx_get_cmd_data(chip), 256);
 		ptr += 256;
@@ -1840,8 +2308,10 @@ int rtsx_read_ppbuf(struct rtsx_chip *chip, u8 *buf, int buf_len)
 			rtsx_add_cmd(chip, READ_REG_CMD, reg_addr++, 0, 0);
 
 		retval = rtsx_send_cmd(chip, 0, 250);
-		if (retval < 0)
-			TRACE_RET(chip, STATUS_FAIL);
+		if (retval < 0) {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 	}
 
 	memcpy(ptr, rtsx_get_cmd_data(chip), buf_len%256);
@@ -1856,8 +2326,10 @@ int rtsx_write_ppbuf(struct rtsx_chip *chip, u8 *buf, int buf_len)
 	u16 reg_addr;
 	u8 *ptr;
 
-	if (!buf)
-		TRACE_RET(chip, STATUS_ERROR);
+	if (!buf) {
+		rtsx_trace(chip);
+		return STATUS_ERROR;
+	}
 
 	ptr = buf;
 	reg_addr = PPBUF_BASE2;
@@ -1871,8 +2343,10 @@ int rtsx_write_ppbuf(struct rtsx_chip *chip, u8 *buf, int buf_len)
 		}
 
 		retval = rtsx_send_cmd(chip, 0, 250);
-		if (retval < 0)
-			TRACE_RET(chip, STATUS_FAIL);
+		if (retval < 0) {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 	}
 
 	if (buf_len%256) {
@@ -1885,8 +2359,10 @@ int rtsx_write_ppbuf(struct rtsx_chip *chip, u8 *buf, int buf_len)
 		}
 
 		retval = rtsx_send_cmd(chip, 0, 250);
-		if (retval < 0)
-			TRACE_RET(chip, STATUS_FAIL);
+		if (retval < 0) {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 	}
 
 	return STATUS_SUCCESS;
@@ -1894,8 +2370,10 @@ int rtsx_write_ppbuf(struct rtsx_chip *chip, u8 *buf, int buf_len)
 
 int rtsx_check_chip_exist(struct rtsx_chip *chip)
 {
-	if (rtsx_readl(chip, 0) == 0xFFFFFFFF)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (rtsx_readl(chip, 0) == 0xFFFFFFFF) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	return STATUS_SUCCESS;
 }
@@ -1918,8 +2396,10 @@ int rtsx_force_power_on(struct rtsx_chip *chip, u8 ctl)
 
 	if (mask) {
 		retval = rtsx_write_register(chip, FPDCTL, mask, 0);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, STATUS_FAIL);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 
 		if (CHECK_PID(chip, 0x5288))
 			wait_timeout(200);
@@ -1947,8 +2427,10 @@ int rtsx_force_power_down(struct rtsx_chip *chip, u8 ctl)
 	if (mask) {
 		val = mask;
 		retval = rtsx_write_register(chip, FPDCTL, mask, val);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, STATUS_FAIL);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 	}
 
 	return STATUS_SUCCESS;
diff --git a/drivers/staging/rts5208/rtsx_chip.h b/drivers/staging/rts5208/rtsx_chip.h
index feac98661cec..c295b1eedb44 100644
--- a/drivers/staging/rts5208/rtsx_chip.h
+++ b/drivers/staging/rts5208/rtsx_chip.h
@@ -988,20 +988,4 @@ int rtsx_read_ppbuf(struct rtsx_chip *chip, u8 *buf, int buf_len);
 int rtsx_write_ppbuf(struct rtsx_chip *chip, u8 *buf, int buf_len);
 int rtsx_check_chip_exist(struct rtsx_chip *chip);
 
-#define RTSX_WRITE_REG(chip, addr, mask, data)				\
-	do {								\
-		int retval = rtsx_write_register((chip), (addr), (mask), (data)); \
-		if (retval != STATUS_SUCCESS) {				\
-			TRACE_RET((chip), retval);			\
-		}							\
-	} while (0)
-
-#define RTSX_READ_REG(chip, addr, data)					\
-	do {								\
-		int retval = rtsx_read_register((chip), (addr), (data)); \
-		if (retval != STATUS_SUCCESS) {				\
-			TRACE_RET((chip), retval);			\
-		}							\
-	} while (0)
-
 #endif  /* __REALTEK_RTSX_CHIP_H */
diff --git a/drivers/staging/rts5208/rtsx_scsi.c b/drivers/staging/rts5208/rtsx_scsi.c
index 11610826acf1..8a5d6a8e780f 100644
--- a/drivers/staging/rts5208/rtsx_scsi.c
+++ b/drivers/staging/rts5208/rtsx_scsi.c
@@ -26,11 +26,6 @@
 #include <linux/vmalloc.h>
 
 #include "rtsx.h"
-#include "rtsx_transport.h"
-#include "rtsx_sys.h"
-#include "rtsx_card.h"
-#include "rtsx_chip.h"
-#include "rtsx_scsi.h"
 #include "sd.h"
 #include "ms.h"
 #include "spi.h"
@@ -39,7 +34,8 @@ void scsi_show_command(struct rtsx_chip *chip)
 {
 	struct scsi_cmnd *srb = chip->srb;
 	char *what = NULL;
-	int unknown_cmd = 0, len;
+	bool unknown_cmd = false;
+	int len;
 
 	switch (srb->cmnd[0]) {
 	case TEST_UNIT_READY:
@@ -310,7 +306,8 @@ void scsi_show_command(struct rtsx_chip *chip)
 		what = "Realtek's vendor command";
 		break;
 	default:
-		what = "(unknown command)"; unknown_cmd = 1;
+		what = "(unknown command)";
+		unknown_cmd = true;
 		break;
 	}
 
@@ -485,7 +482,7 @@ static int inquiry(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 	unsigned char sendbytes;
 	unsigned char *buf;
 	u8 card = get_lun_card(chip, lun);
-	int pro_formatter_flag = 0;
+	bool pro_formatter_flag = false;
 	unsigned char inquiry_buf[] = {
 		QULIFIRE|DRCT_ACCESS_DEV,
 		RMB_DISC|0x0D,
@@ -510,8 +507,10 @@ static int inquiry(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 	}
 
 	buf = vmalloc(scsi_bufflen(srb));
-	if (buf == NULL)
-		TRACE_RET(chip, TRANSPORT_ERROR);
+	if (buf == NULL) {
+		rtsx_trace(chip);
+		return TRANSPORT_ERROR;
+	}
 
 #ifdef SUPPORT_MAGIC_GATE
 	if ((chip->mspro_formatter_enable) &&
@@ -519,10 +518,8 @@ static int inquiry(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 #else
 	if (chip->mspro_formatter_enable)
 #endif
-	{
 		if (!card || (card == MS_CARD))
-			pro_formatter_flag = 1;
-	}
+			pro_formatter_flag = true;
 
 	if (pro_formatter_flag) {
 		if (scsi_bufflen(srb) < 56)
@@ -587,12 +584,14 @@ static int start_stop_unit(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 		if (check_card_ready(chip, lun))
 			return TRANSPORT_GOOD;
 		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_NOT_PRESENT);
-		TRACE_RET(chip, TRANSPORT_FAILED);
+		rtsx_trace(chip);
+		return TRANSPORT_FAILED;
 
 		break;
 	}
 
-	TRACE_RET(chip, TRANSPORT_ERROR);
+	rtsx_trace(chip);
+	return TRANSPORT_ERROR;
 }
 
 
@@ -607,7 +606,8 @@ static int allow_medium_removal(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 	if (prevent) {
 		set_sense_type(chip, SCSI_LUN(srb),
 			SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-		TRACE_RET(chip, TRANSPORT_FAILED);
+		rtsx_trace(chip);
+		return TRANSPORT_FAILED;
 	}
 
 	return TRANSPORT_GOOD;
@@ -644,8 +644,10 @@ static int request_sense(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 	}
 
 	buf = vmalloc(scsi_bufflen(srb));
-	if (buf == NULL)
-		TRACE_RET(chip, TRANSPORT_ERROR);
+	if (buf == NULL) {
+		rtsx_trace(chip);
+		return TRANSPORT_ERROR;
+	}
 
 	tmp = (unsigned char *)sense;
 	memcpy(buf, tmp, scsi_bufflen(srb));
@@ -665,7 +667,7 @@ static void ms_mode_sense(struct rtsx_chip *chip, u8 cmd,
 	struct ms_info *ms_card = &(chip->ms_card);
 	int sys_info_offset;
 	int data_size = buf_len;
-	int support_format = 0;
+	bool support_format = false;
 	int i = 0;
 
 	if (cmd == MODE_SENSE) {
@@ -686,10 +688,10 @@ static void ms_mode_sense(struct rtsx_chip *chip, u8 cmd,
 	/* Medium Type Code */
 	if (check_card_ready(chip, lun)) {
 		if (CHK_MSXC(ms_card)) {
-			support_format = 1;
+			support_format = true;
 			buf[i++] = 0x40;
 		} else if (CHK_MSPRO(ms_card)) {
-			support_format = 1;
+			support_format = true;
 			buf[i++] = 0x20;
 		} else {
 			buf[i++] = 0x10;
@@ -757,7 +759,7 @@ static int mode_sense(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 	unsigned int lun = SCSI_LUN(srb);
 	unsigned int dataSize;
 	int status;
-	int pro_formatter_flag;
+	bool pro_formatter_flag;
 	unsigned char pageCode, *buf;
 	u8 card = get_lun_card(chip, lun);
 
@@ -765,32 +767,35 @@ static int mode_sense(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 	if (!check_card_ready(chip, lun)) {
 		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_NOT_PRESENT);
 		scsi_set_resid(srb, scsi_bufflen(srb));
-		TRACE_RET(chip, TRANSPORT_FAILED);
+		rtsx_trace(chip);
+		return TRANSPORT_FAILED;
 	}
 #endif
 
-	pro_formatter_flag = 0;
+	pro_formatter_flag = false;
 	dataSize = 8;
 #ifdef SUPPORT_MAGIC_GATE
 	if ((chip->lun2card[lun] & MS_CARD)) {
 		if (!card || (card == MS_CARD)) {
 			dataSize = 108;
 			if (chip->mspro_formatter_enable)
-				pro_formatter_flag = 1;
+				pro_formatter_flag = true;
 		}
 	}
 #else
 	if (card == MS_CARD) {
 		if (chip->mspro_formatter_enable) {
-			pro_formatter_flag = 1;
+			pro_formatter_flag = true;
 			dataSize = 108;
 		}
 	}
 #endif
 
 	buf = kmalloc(dataSize, GFP_KERNEL);
-	if (buf == NULL)
-		TRACE_RET(chip, TRANSPORT_ERROR);
+	if (buf == NULL) {
+		rtsx_trace(chip);
+		return TRANSPORT_ERROR;
+	}
 
 	pageCode = srb->cmnd[2] & 0x3f;
 
@@ -869,7 +874,8 @@ static int read_write(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 
 	if (!check_card_ready(chip, lun) || (get_card_size(chip, lun) == 0)) {
 		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_NOT_PRESENT);
-		TRACE_RET(chip, TRANSPORT_FAILED);
+		rtsx_trace(chip);
+		return TRANSPORT_FAILED;
 	}
 
 	if (!(CHK_BIT(chip->lun_mc, lun))) {
@@ -885,7 +891,8 @@ static int read_write(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 		 */
 		dev_dbg(rtsx_dev(chip), "SD card being erased!\n");
 		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_READ_FORBIDDEN);
-		TRACE_RET(chip, TRANSPORT_FAILED);
+		rtsx_trace(chip);
+		return TRANSPORT_FAILED;
 	}
 
 	if (get_lun_card(chip, lun) == SD_CARD) {
@@ -893,7 +900,8 @@ static int read_write(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 			dev_dbg(rtsx_dev(chip), "SD card locked!\n");
 			set_sense_type(chip, lun,
 				SENSE_TYPE_MEDIA_READ_FORBIDDEN);
-			TRACE_RET(chip, TRANSPORT_FAILED);
+			rtsx_trace(chip);
+			return TRANSPORT_FAILED;
 		}
 	}
 #endif
@@ -916,7 +924,8 @@ static int read_write(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 		sec_cnt = ((u16)(srb->cmnd[9]) << 8) | srb->cmnd[10];
 	} else {
 		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-		TRACE_RET(chip, TRANSPORT_FAILED);
+		rtsx_trace(chip);
+		return TRANSPORT_FAILED;
 	}
 
 	/* In some test, we will receive a start_sec like 0xFFFFFFFF.
@@ -926,7 +935,8 @@ static int read_write(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 	if ((start_sec > get_card_size(chip, lun)) ||
 			((start_sec + sec_cnt) > get_card_size(chip, lun))) {
 		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_LBA_OVER_RANGE);
-		TRACE_RET(chip, TRANSPORT_FAILED);
+		rtsx_trace(chip);
+		return TRANSPORT_FAILED;
 	}
 
 	if (sec_cnt == 0) {
@@ -942,7 +952,8 @@ static int read_write(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 		else
 			set_sense_type(chip, lun, SENSE_TYPE_MEDIA_WRITE_ERR);
 
-		TRACE_RET(chip, TRANSPORT_FAILED);
+		rtsx_trace(chip);
+		return TRANSPORT_FAILED;
 	}
 
 	if (srb->sc_data_direction == DMA_TO_DEVICE) {
@@ -950,7 +961,8 @@ static int read_write(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 			dev_dbg(rtsx_dev(chip), "Write protected card!\n");
 			set_sense_type(chip, lun,
 				SENSE_TYPE_MEDIA_WRITE_PROTECT);
-			TRACE_RET(chip, TRANSPORT_FAILED);
+			rtsx_trace(chip);
+			return TRANSPORT_FAILED;
 		}
 	}
 
@@ -969,7 +981,8 @@ static int read_write(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 					SENSE_TYPE_MEDIA_WRITE_ERR);
 		}
 		retval = TRANSPORT_FAILED;
-		TRACE_GOTO(chip, Exit);
+		rtsx_trace(chip);
+		goto Exit;
 	} else {
 		chip->rw_fail_cnt[lun] = 0;
 		retval = TRANSPORT_GOOD;
@@ -994,15 +1007,18 @@ static int read_format_capacity(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 	if (!check_card_ready(chip, lun)) {
 		if (!chip->mspro_formatter_enable) {
 			set_sense_type(chip, lun, SENSE_TYPE_MEDIA_NOT_PRESENT);
-			TRACE_RET(chip, TRANSPORT_FAILED);
+			rtsx_trace(chip);
+			return TRANSPORT_FAILED;
 		}
 	}
 
 	buf_len = (scsi_bufflen(srb) > 12) ? 0x14 : 12;
 
 	buf = kmalloc(buf_len, GFP_KERNEL);
-	if (buf == NULL)
-		TRACE_RET(chip, TRANSPORT_ERROR);
+	if (buf == NULL) {
+		rtsx_trace(chip);
+		return TRANSPORT_ERROR;
+	}
 
 	buf[i++] = 0;
 	buf[i++] = 0;
@@ -1067,7 +1083,8 @@ static int read_capacity(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 
 	if (!check_card_ready(chip, lun)) {
 		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_NOT_PRESENT);
-		TRACE_RET(chip, TRANSPORT_FAILED);
+		rtsx_trace(chip);
+		return TRANSPORT_FAILED;
 	}
 
 	if (!(CHK_BIT(chip->lun_mc, lun))) {
@@ -1077,8 +1094,10 @@ static int read_capacity(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 	}
 
 	buf = kmalloc(8, GFP_KERNEL);
-	if (buf == NULL)
-		TRACE_RET(chip, TRANSPORT_ERROR);
+	if (buf == NULL) {
+		rtsx_trace(chip);
+		return TRANSPORT_ERROR;
+	}
 
 	card_size = get_card_size(chip, lun);
 	buf[0] = (unsigned char)((card_size - 1) >> 24);
@@ -1116,15 +1135,18 @@ static int read_eeprom(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 	len = ((u16)srb->cmnd[4] << 8) | srb->cmnd[5];
 
 	buf = vmalloc(len);
-	if (!buf)
-		TRACE_RET(chip, TRANSPORT_ERROR);
+	if (!buf) {
+		rtsx_trace(chip);
+		return TRANSPORT_ERROR;
+	}
 
 	retval = rtsx_force_power_on(chip, SSC_PDCTL);
 	if (retval != STATUS_SUCCESS) {
 		vfree(buf);
 		set_sense_type(chip, SCSI_LUN(srb),
 			SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
-		TRACE_RET(chip, TRANSPORT_FAILED);
+		rtsx_trace(chip);
+		return TRANSPORT_FAILED;
 	}
 
 	for (i = 0; i < len; i++) {
@@ -1133,7 +1155,8 @@ static int read_eeprom(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 			vfree(buf);
 			set_sense_type(chip, SCSI_LUN(srb),
 				SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
-			TRACE_RET(chip, TRANSPORT_FAILED);
+			rtsx_trace(chip);
+			return TRANSPORT_FAILED;
 		}
 	}
 
@@ -1165,7 +1188,8 @@ static int write_eeprom(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 	retval = rtsx_force_power_on(chip, SSC_PDCTL);
 	if (retval != STATUS_SUCCESS) {
 		set_sense_type(chip, SCSI_LUN(srb), SENSE_TYPE_MEDIA_WRITE_ERR);
-		TRACE_RET(chip, TRANSPORT_FAILED);
+		rtsx_trace(chip);
+		return TRANSPORT_FAILED;
 	}
 
 	if (len == 511) {
@@ -1173,14 +1197,17 @@ static int write_eeprom(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 		if (retval != STATUS_SUCCESS) {
 			set_sense_type(chip, SCSI_LUN(srb),
 				SENSE_TYPE_MEDIA_WRITE_ERR);
-			TRACE_RET(chip, TRANSPORT_FAILED);
+			rtsx_trace(chip);
+			return TRANSPORT_FAILED;
 		}
 	} else {
 		len = (unsigned short)min_t(unsigned int, scsi_bufflen(srb),
 					len);
 		buf = vmalloc(len);
-		if (buf == NULL)
-			TRACE_RET(chip, TRANSPORT_ERROR);
+		if (buf == NULL) {
+			rtsx_trace(chip);
+			return TRANSPORT_ERROR;
+		}
 
 		rtsx_stor_get_xfer_buf(buf, len, srb);
 		scsi_set_resid(srb, scsi_bufflen(srb) - len);
@@ -1191,7 +1218,8 @@ static int write_eeprom(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 				vfree(buf);
 				set_sense_type(chip, SCSI_LUN(srb),
 					SENSE_TYPE_MEDIA_WRITE_ERR);
-				TRACE_RET(chip, TRANSPORT_FAILED);
+				rtsx_trace(chip);
+				return TRANSPORT_FAILED;
 			}
 		}
 
@@ -1221,18 +1249,22 @@ static int read_mem(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 	if (addr < 0xFC00) {
 		set_sense_type(chip, SCSI_LUN(srb),
 			SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-		TRACE_RET(chip, TRANSPORT_FAILED);
+		rtsx_trace(chip);
+		return TRANSPORT_FAILED;
 	}
 
 	buf = vmalloc(len);
-	if (!buf)
-		TRACE_RET(chip, TRANSPORT_ERROR);
+	if (!buf) {
+		rtsx_trace(chip);
+		return TRANSPORT_ERROR;
+	}
 
 	retval = rtsx_force_power_on(chip, SSC_PDCTL);
 	if (retval != STATUS_SUCCESS) {
 		vfree(buf);
 		set_sense_type(chip, SCSI_LUN(srb), SENSE_TYPE_MEDIA_WRITE_ERR);
-		TRACE_RET(chip, TRANSPORT_FAILED);
+		rtsx_trace(chip);
+		return TRANSPORT_FAILED;
 	}
 
 	for (i = 0; i < len; i++) {
@@ -1241,7 +1273,8 @@ static int read_mem(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 			vfree(buf);
 			set_sense_type(chip, SCSI_LUN(srb),
 				SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
-			TRACE_RET(chip, TRANSPORT_FAILED);
+			rtsx_trace(chip);
+			return TRANSPORT_FAILED;
 		}
 	}
 
@@ -1274,13 +1307,16 @@ static int write_mem(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 	if (addr < 0xFC00) {
 		set_sense_type(chip, SCSI_LUN(srb),
 			SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-		TRACE_RET(chip, TRANSPORT_FAILED);
+		rtsx_trace(chip);
+		return TRANSPORT_FAILED;
 	}
 
 	len = (unsigned short)min_t(unsigned int, scsi_bufflen(srb), len);
 	buf = vmalloc(len);
-	if (buf == NULL)
-		TRACE_RET(chip, TRANSPORT_ERROR);
+	if (buf == NULL) {
+		rtsx_trace(chip);
+		return TRANSPORT_ERROR;
+	}
 
 	rtsx_stor_get_xfer_buf(buf, len, srb);
 	scsi_set_resid(srb, scsi_bufflen(srb) - len);
@@ -1289,7 +1325,8 @@ static int write_mem(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 	if (retval != STATUS_SUCCESS) {
 		vfree(buf);
 		set_sense_type(chip, SCSI_LUN(srb), SENSE_TYPE_MEDIA_WRITE_ERR);
-		TRACE_RET(chip, TRANSPORT_FAILED);
+		rtsx_trace(chip);
+		return TRANSPORT_FAILED;
 	}
 
 	for (i = 0; i < len; i++) {
@@ -1298,7 +1335,8 @@ static int write_mem(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 			vfree(buf);
 			set_sense_type(chip, SCSI_LUN(srb),
 				SENSE_TYPE_MEDIA_WRITE_ERR);
-			TRACE_RET(chip, TRANSPORT_FAILED);
+			rtsx_trace(chip);
+			return TRANSPORT_FAILED;
 		}
 	}
 
@@ -1314,12 +1352,14 @@ static int get_sd_csd(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 
 	if (!check_card_ready(chip, lun)) {
 		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_NOT_PRESENT);
-		TRACE_RET(chip, TRANSPORT_FAILED);
+		rtsx_trace(chip);
+		return TRANSPORT_FAILED;
 	}
 
 	if (get_lun_card(chip, lun) != SD_CARD) {
 		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
-		TRACE_RET(chip, TRANSPORT_FAILED);
+		rtsx_trace(chip);
+		return TRANSPORT_FAILED;
 	}
 
 	scsi_set_resid(srb, 0);
@@ -1361,14 +1401,17 @@ static int trace_msg_cmd(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 	if ((scsi_bufflen(srb) < buf_len) || (scsi_sglist(srb) == NULL)) {
 		set_sense_type(chip, SCSI_LUN(srb),
 			SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
-		TRACE_RET(chip, TRANSPORT_FAILED);
+		rtsx_trace(chip);
+		return TRANSPORT_FAILED;
 	}
 
 	clear = srb->cmnd[2];
 
 	buf = vmalloc(scsi_bufflen(srb));
-	if (buf == NULL)
-		TRACE_RET(chip, TRANSPORT_ERROR);
+	if (buf == NULL) {
+		rtsx_trace(chip);
+		return TRANSPORT_ERROR;
+	}
 	ptr = buf;
 
 	if (chip->trace_msg[chip->msg_idx].valid)
@@ -1500,7 +1543,8 @@ static int set_variable(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 		default:
 			set_sense_type(chip, lun,
 				SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-			TRACE_RET(chip, TRANSPORT_FAILED);
+			rtsx_trace(chip);
+			return TRANSPORT_FAILED;
 		}
 	} else if (srb->cmnd[3] == 2) {
 		if (srb->cmnd[4]) {
@@ -1523,14 +1567,16 @@ static int set_variable(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 			if (retval != STATUS_SUCCESS) {
 				set_sense_type(chip, SCSI_LUN(srb),
 					SENSE_TYPE_MEDIA_WRITE_ERR);
-				TRACE_RET(chip, TRANSPORT_FAILED);
+				rtsx_trace(chip);
+				return TRANSPORT_FAILED;
 			}
 
 			turn_off_led(chip, LED_GPIO);
 		}
 	} else {
 		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-		TRACE_RET(chip, TRANSPORT_FAILED);
+		rtsx_trace(chip);
+		return TRANSPORT_FAILED;
 	}
 
 	return TRANSPORT_GOOD;
@@ -1562,7 +1608,8 @@ static int get_variable(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 		default:
 			set_sense_type(chip, lun,
 				SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-			TRACE_RET(chip, TRANSPORT_FAILED);
+			rtsx_trace(chip);
+			return TRANSPORT_FAILED;
 		}
 
 		rtsx_stor_set_xfer_buf(&tmp, 1, srb);
@@ -1572,7 +1619,8 @@ static int get_variable(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 		rtsx_stor_set_xfer_buf(&tmp, 1, srb);
 	} else {
 		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-		TRACE_RET(chip, TRANSPORT_FAILED);
+		rtsx_trace(chip);
+		return TRANSPORT_FAILED;
 	}
 
 	return TRANSPORT_GOOD;
@@ -1610,7 +1658,8 @@ static int dma_access_ring_buffer(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 			set_sense_type(chip, lun,
 				SENSE_TYPE_MEDIA_WRITE_ERR);
 
-		TRACE_RET(chip, TRANSPORT_FAILED);
+		rtsx_trace(chip);
+		return TRANSPORT_FAILED;
 	}
 	scsi_set_resid(srb, 0);
 
@@ -1757,7 +1806,8 @@ static int set_chip_mode(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 	if (!CHECK_PID(chip, 0x5208)) {
 		set_sense_type(chip, SCSI_LUN(srb),
 			SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-		TRACE_RET(chip, TRANSPORT_FAILED);
+		rtsx_trace(chip);
+		return TRANSPORT_FAILED;
 	}
 
 	phy_debug_mode = (int)(srb->cmnd[3]);
@@ -1765,35 +1815,47 @@ static int set_chip_mode(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 	if (phy_debug_mode) {
 		chip->phy_debug_mode = 1;
 		retval = rtsx_write_register(chip, CDRESUMECTL, 0x77, 0);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, TRANSPORT_FAILED);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			return TRANSPORT_FAILED;
+		}
 
 		rtsx_disable_bus_int(chip);
 
 		retval = rtsx_read_phy_register(chip, 0x1C, &reg);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, TRANSPORT_FAILED);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			return TRANSPORT_FAILED;
+		}
 
 		reg |= 0x0001;
 		retval = rtsx_write_phy_register(chip, 0x1C, reg);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, TRANSPORT_FAILED);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			return TRANSPORT_FAILED;
+		}
 	} else {
 		chip->phy_debug_mode = 0;
 		retval = rtsx_write_register(chip, CDRESUMECTL, 0x77, 0x77);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, TRANSPORT_FAILED);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			return TRANSPORT_FAILED;
+		}
 
 		rtsx_enable_bus_int(chip);
 
 		retval = rtsx_read_phy_register(chip, 0x1C, &reg);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, TRANSPORT_FAILED);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			return TRANSPORT_FAILED;
+		}
 
 		reg &= 0xFFFE;
 		retval = rtsx_write_phy_register(chip, 0x1C, reg);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, TRANSPORT_FAILED);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			return TRANSPORT_FAILED;
+		}
 	}
 
 	return TRANSPORT_GOOD;
@@ -1824,7 +1886,8 @@ static int rw_mem_cmd_buf(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 		if (cmd_type > 2) {
 			set_sense_type(chip, lun,
 				SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-			TRACE_RET(chip, TRANSPORT_FAILED);
+			rtsx_trace(chip);
+			return TRANSPORT_FAILED;
 		}
 		addr = (srb->cmnd[5] << 8) | srb->cmnd[6];
 		mask = srb->cmnd[7];
@@ -1842,7 +1905,8 @@ static int rw_mem_cmd_buf(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 		if (scsi_bufflen(srb) < 1) {
 			set_sense_type(chip, lun,
 				SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-			TRACE_RET(chip, TRANSPORT_FAILED);
+			rtsx_trace(chip);
+			return TRANSPORT_FAILED;
 		}
 		rtsx_stor_set_xfer_buf(&value, 1, srb);
 		scsi_set_resid(srb, 0);
@@ -1850,12 +1914,14 @@ static int rw_mem_cmd_buf(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 
 	default:
 		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-		TRACE_RET(chip, TRANSPORT_FAILED);
+		rtsx_trace(chip);
+		return TRANSPORT_FAILED;
 	}
 
 	if (retval != STATUS_SUCCESS) {
 		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_WRITE_ERR);
-		TRACE_RET(chip, TRANSPORT_FAILED);
+		rtsx_trace(chip);
+		return TRANSPORT_FAILED;
 	}
 
 	return TRANSPORT_GOOD;
@@ -1902,15 +1968,18 @@ static int read_phy_register(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 
 	if (len) {
 		buf = vmalloc(len);
-		if (!buf)
-			TRACE_RET(chip, TRANSPORT_ERROR);
+		if (!buf) {
+			rtsx_trace(chip);
+			return TRANSPORT_ERROR;
+		}
 
 		retval = rtsx_force_power_on(chip, SSC_PDCTL);
 		if (retval != STATUS_SUCCESS) {
 			vfree(buf);
 			set_sense_type(chip, SCSI_LUN(srb),
 				SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
-			TRACE_RET(chip, TRANSPORT_FAILED);
+			rtsx_trace(chip);
+			return TRANSPORT_FAILED;
 		}
 
 		for (i = 0; i < len / 2; i++) {
@@ -1919,7 +1988,8 @@ static int read_phy_register(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 				vfree(buf);
 				set_sense_type(chip, SCSI_LUN(srb),
 					SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
-				TRACE_RET(chip, TRANSPORT_FAILED);
+				rtsx_trace(chip);
+				return TRANSPORT_FAILED;
 			}
 
 			buf[2*i] = (u8)(val >> 8);
@@ -1963,8 +2033,10 @@ static int write_phy_register(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 					len);
 
 		buf = vmalloc(len);
-		if (buf == NULL)
-			TRACE_RET(chip, TRANSPORT_ERROR);
+		if (buf == NULL) {
+			rtsx_trace(chip);
+			return TRANSPORT_ERROR;
+		}
 
 		rtsx_stor_get_xfer_buf(buf, len, srb);
 		scsi_set_resid(srb, scsi_bufflen(srb) - len);
@@ -1974,7 +2046,8 @@ static int write_phy_register(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 			vfree(buf);
 			set_sense_type(chip, SCSI_LUN(srb),
 				SENSE_TYPE_MEDIA_WRITE_ERR);
-			TRACE_RET(chip, TRANSPORT_FAILED);
+			rtsx_trace(chip);
+			return TRANSPORT_FAILED;
 		}
 
 		for (i = 0; i < len / 2; i++) {
@@ -1984,7 +2057,8 @@ static int write_phy_register(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 				vfree(buf);
 				set_sense_type(chip, SCSI_LUN(srb),
 					SENSE_TYPE_MEDIA_WRITE_ERR);
-				TRACE_RET(chip, TRANSPORT_FAILED);
+				rtsx_trace(chip);
+				return TRANSPORT_FAILED;
 			}
 		}
 
@@ -2011,7 +2085,8 @@ static int erase_eeprom2(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 	retval = rtsx_force_power_on(chip, SSC_PDCTL);
 	if (retval != STATUS_SUCCESS) {
 		set_sense_type(chip, SCSI_LUN(srb), SENSE_TYPE_MEDIA_WRITE_ERR);
-		TRACE_RET(chip, TRANSPORT_FAILED);
+		rtsx_trace(chip);
+		return TRANSPORT_FAILED;
 	}
 
 	mode = srb->cmnd[3];
@@ -2022,19 +2097,22 @@ static int erase_eeprom2(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 		if (retval != STATUS_SUCCESS) {
 			set_sense_type(chip, SCSI_LUN(srb),
 				SENSE_TYPE_MEDIA_WRITE_ERR);
-			TRACE_RET(chip, TRANSPORT_FAILED);
+			rtsx_trace(chip);
+			return TRANSPORT_FAILED;
 		}
 	} else if (mode == 1) {
 		retval = spi_erase_eeprom_byte(chip, addr);
 		if (retval != STATUS_SUCCESS) {
 			set_sense_type(chip, SCSI_LUN(srb),
 				SENSE_TYPE_MEDIA_WRITE_ERR);
-			TRACE_RET(chip, TRANSPORT_FAILED);
+			rtsx_trace(chip);
+			return TRANSPORT_FAILED;
 		}
 	} else {
 		set_sense_type(chip, SCSI_LUN(srb),
 			SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-		TRACE_RET(chip, TRANSPORT_FAILED);
+		rtsx_trace(chip);
+		return TRANSPORT_FAILED;
 	}
 
 	return TRANSPORT_GOOD;
@@ -2058,15 +2136,18 @@ static int read_eeprom2(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 	len = ((u16)srb->cmnd[6] << 8) | srb->cmnd[7];
 
 	buf = vmalloc(len);
-	if (!buf)
-		TRACE_RET(chip, TRANSPORT_ERROR);
+	if (!buf) {
+		rtsx_trace(chip);
+		return TRANSPORT_ERROR;
+	}
 
 	retval = rtsx_force_power_on(chip, SSC_PDCTL);
 	if (retval != STATUS_SUCCESS) {
 		vfree(buf);
 		set_sense_type(chip, SCSI_LUN(srb),
 			SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
-		TRACE_RET(chip, TRANSPORT_FAILED);
+		rtsx_trace(chip);
+		return TRANSPORT_FAILED;
 	}
 
 	for (i = 0; i < len; i++) {
@@ -2075,7 +2156,8 @@ static int read_eeprom2(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 			vfree(buf);
 			set_sense_type(chip, SCSI_LUN(srb),
 				SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
-			TRACE_RET(chip, TRANSPORT_FAILED);
+			rtsx_trace(chip);
+			return TRANSPORT_FAILED;
 		}
 	}
 
@@ -2107,8 +2189,10 @@ static int write_eeprom2(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 
 	len = (unsigned short)min_t(unsigned int, scsi_bufflen(srb), len);
 	buf = vmalloc(len);
-	if (buf == NULL)
-		TRACE_RET(chip, TRANSPORT_ERROR);
+	if (buf == NULL) {
+		rtsx_trace(chip);
+		return TRANSPORT_ERROR;
+	}
 
 	rtsx_stor_get_xfer_buf(buf, len, srb);
 	scsi_set_resid(srb, scsi_bufflen(srb) - len);
@@ -2117,7 +2201,8 @@ static int write_eeprom2(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 	if (retval != STATUS_SUCCESS) {
 		vfree(buf);
 		set_sense_type(chip, SCSI_LUN(srb), SENSE_TYPE_MEDIA_WRITE_ERR);
-		TRACE_RET(chip, TRANSPORT_FAILED);
+		rtsx_trace(chip);
+		return TRANSPORT_FAILED;
 	}
 
 	for (i = 0; i < len; i++) {
@@ -2126,7 +2211,8 @@ static int write_eeprom2(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 			vfree(buf);
 			set_sense_type(chip, SCSI_LUN(srb),
 				SENSE_TYPE_MEDIA_WRITE_ERR);
-			TRACE_RET(chip, TRANSPORT_FAILED);
+			rtsx_trace(chip);
+			return TRANSPORT_FAILED;
 		}
 	}
 
@@ -2153,15 +2239,18 @@ static int read_efuse(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 	len = srb->cmnd[5];
 
 	buf = vmalloc(len);
-	if (!buf)
-		TRACE_RET(chip, TRANSPORT_ERROR);
+	if (!buf) {
+		rtsx_trace(chip);
+		return TRANSPORT_ERROR;
+	}
 
 	retval = rtsx_force_power_on(chip, SSC_PDCTL);
 	if (retval != STATUS_SUCCESS) {
 		vfree(buf);
 		set_sense_type(chip, SCSI_LUN(srb),
 			SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
-		TRACE_RET(chip, TRANSPORT_FAILED);
+		rtsx_trace(chip);
+		return TRANSPORT_FAILED;
 	}
 
 	for (i = 0; i < len; i++) {
@@ -2170,7 +2259,8 @@ static int read_efuse(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 			vfree(buf);
 			set_sense_type(chip, SCSI_LUN(srb),
 				SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
-			TRACE_RET(chip, TRANSPORT_FAILED);
+			rtsx_trace(chip);
+			return TRANSPORT_FAILED;
 		}
 	}
 
@@ -2203,8 +2293,10 @@ static int write_efuse(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 
 	len = (u8)min_t(unsigned int, scsi_bufflen(srb), len);
 	buf = vmalloc(len);
-	if (buf == NULL)
-		TRACE_RET(chip, TRANSPORT_ERROR);
+	if (buf == NULL) {
+		rtsx_trace(chip);
+		return TRANSPORT_ERROR;
+	}
 
 	rtsx_stor_get_xfer_buf(buf, len, srb);
 	scsi_set_resid(srb, scsi_bufflen(srb) - len);
@@ -2212,21 +2304,24 @@ static int write_efuse(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 	retval = rtsx_force_power_on(chip, SSC_PDCTL);
 	if (retval != STATUS_SUCCESS) {
 		vfree(buf);
-		TRACE_RET(chip, TRANSPORT_ERROR);
+		rtsx_trace(chip);
+		return TRANSPORT_ERROR;
 	}
 
 	if (chip->asic_code) {
 		retval = rtsx_read_phy_register(chip, 0x08, &val);
 		if (retval != STATUS_SUCCESS) {
 			vfree(buf);
-			TRACE_RET(chip, TRANSPORT_ERROR);
+			rtsx_trace(chip);
+			return TRANSPORT_ERROR;
 		}
 
 		retval = rtsx_write_register(chip, PWR_GATE_CTRL,
 					LDO3318_PWR_MASK, LDO_OFF);
 		if (retval != STATUS_SUCCESS) {
 			vfree(buf);
-			TRACE_RET(chip, TRANSPORT_ERROR);
+			rtsx_trace(chip);
+			return TRANSPORT_ERROR;
 		}
 
 		wait_timeout(600);
@@ -2235,14 +2330,16 @@ static int write_efuse(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 						0x4C00 | chip->phy_voltage);
 		if (retval != STATUS_SUCCESS) {
 			vfree(buf);
-			TRACE_RET(chip, TRANSPORT_ERROR);
+			rtsx_trace(chip);
+			return TRANSPORT_ERROR;
 		}
 
 		retval = rtsx_write_register(chip, PWR_GATE_CTRL,
 					LDO3318_PWR_MASK, LDO_ON);
 		if (retval != STATUS_SUCCESS) {
 			vfree(buf);
-			TRACE_RET(chip, TRANSPORT_ERROR);
+			rtsx_trace(chip);
+			return TRANSPORT_ERROR;
 		}
 
 		wait_timeout(600);
@@ -2251,7 +2348,8 @@ static int write_efuse(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 	retval = card_power_on(chip, SPI_CARD);
 	if (retval != STATUS_SUCCESS) {
 		vfree(buf);
-		TRACE_RET(chip, TRANSPORT_ERROR);
+		rtsx_trace(chip);
+		return TRANSPORT_ERROR;
 	}
 
 	wait_timeout(50);
@@ -2262,7 +2360,8 @@ static int write_efuse(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 			set_sense_type(chip, SCSI_LUN(srb),
 				SENSE_TYPE_MEDIA_WRITE_ERR);
 			result = TRANSPORT_FAILED;
-			TRACE_GOTO(chip, Exit);
+			rtsx_trace(chip);
+			goto Exit;
 		}
 	}
 
@@ -2270,25 +2369,33 @@ Exit:
 	vfree(buf);
 
 	retval = card_power_off(chip, SPI_CARD);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, TRANSPORT_ERROR);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return TRANSPORT_ERROR;
+	}
 
 	if (chip->asic_code) {
 		retval = rtsx_write_register(chip, PWR_GATE_CTRL,
 					LDO3318_PWR_MASK, LDO_OFF);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, TRANSPORT_ERROR);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			return TRANSPORT_ERROR;
+		}
 
 		wait_timeout(600);
 
 		retval = rtsx_write_phy_register(chip, 0x08, val);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, TRANSPORT_ERROR);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			return TRANSPORT_ERROR;
+		}
 
 		retval = rtsx_write_register(chip, PWR_GATE_CTRL,
 					LDO3318_PWR_MASK, LDO_ON);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, TRANSPORT_ERROR);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			return TRANSPORT_ERROR;
+		}
 	}
 
 	return result;
@@ -2297,7 +2404,8 @@ Exit:
 static int read_cfg_byte(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 {
 	int retval;
-	u8 func, func_max;
+	bool func_max;
+	u8 func;
 	u16 addr, len;
 	u8 *buf;
 
@@ -2317,26 +2425,30 @@ static int read_cfg_byte(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 		__func__, func, addr, len);
 
 	if (CHK_SDIO_EXIST(chip) && !CHK_SDIO_IGNORED(chip))
-		func_max = 1;
+		func_max = true;
 	else
-		func_max = 0;
+		func_max = false;
 
 	if (func > func_max) {
 		set_sense_type(chip, SCSI_LUN(srb),
 			SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-		TRACE_RET(chip, TRANSPORT_FAILED);
+		rtsx_trace(chip);
+		return TRANSPORT_FAILED;
 	}
 
 	buf = vmalloc(len);
-	if (!buf)
-		TRACE_RET(chip, TRANSPORT_ERROR);
+	if (!buf) {
+		rtsx_trace(chip);
+		return TRANSPORT_ERROR;
+	}
 
 	retval = rtsx_read_cfg_seq(chip, func, addr, buf, len);
 	if (retval != STATUS_SUCCESS) {
 		set_sense_type(chip, SCSI_LUN(srb),
 			SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
 		vfree(buf);
-		TRACE_RET(chip, TRANSPORT_FAILED);
+		rtsx_trace(chip);
+		return TRANSPORT_FAILED;
 	}
 
 	len = (u16)min_t(unsigned int, scsi_bufflen(srb), len);
@@ -2351,7 +2463,8 @@ static int read_cfg_byte(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 static int write_cfg_byte(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 {
 	int retval;
-	u8 func, func_max;
+	bool func_max;
+	u8 func;
 	u16 addr, len;
 	u8 *buf;
 
@@ -2371,20 +2484,23 @@ static int write_cfg_byte(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 		__func__, func, addr);
 
 	if (CHK_SDIO_EXIST(chip) && !CHK_SDIO_IGNORED(chip))
-		func_max = 1;
+		func_max = true;
 	else
-		func_max = 0;
+		func_max = false;
 
 	if (func > func_max) {
 		set_sense_type(chip, SCSI_LUN(srb),
 			SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-		TRACE_RET(chip, TRANSPORT_FAILED);
+		rtsx_trace(chip);
+		return TRANSPORT_FAILED;
 	}
 
 	len = (unsigned short)min_t(unsigned int, scsi_bufflen(srb), len);
 	buf = vmalloc(len);
-	if (!buf)
-		TRACE_RET(chip, TRANSPORT_ERROR);
+	if (!buf) {
+		rtsx_trace(chip);
+		return TRANSPORT_ERROR;
+	}
 
 	rtsx_stor_get_xfer_buf(buf, len, srb);
 	scsi_set_resid(srb, scsi_bufflen(srb) - len);
@@ -2393,7 +2509,8 @@ static int write_cfg_byte(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 	if (retval != STATUS_SUCCESS) {
 		set_sense_type(chip, SCSI_LUN(srb), SENSE_TYPE_MEDIA_WRITE_ERR);
 		vfree(buf);
-		TRACE_RET(chip, TRANSPORT_FAILED);
+		rtsx_trace(chip);
+		return TRANSPORT_FAILED;
 	}
 
 	vfree(buf);
@@ -2483,7 +2600,8 @@ static int app_cmd(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 	default:
 		set_sense_type(chip, SCSI_LUN(srb),
 			SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-		TRACE_RET(chip, TRANSPORT_FAILED);
+		rtsx_trace(chip);
+		return TRANSPORT_FAILED;
 	}
 
 	return result;
@@ -2626,7 +2744,8 @@ static int get_card_bus_width(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 
 	if (!check_card_ready(chip, lun)) {
 		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_NOT_PRESENT);
-		TRACE_RET(chip, TRANSPORT_FAILED);
+		rtsx_trace(chip);
+		return TRANSPORT_FAILED;
 	}
 
 	card = get_lun_card(chip, lun);
@@ -2634,7 +2753,8 @@ static int get_card_bus_width(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 		bus_width = chip->card_bus_width[lun];
 	} else {
 		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
-		TRACE_RET(chip, TRANSPORT_FAILED);
+		rtsx_trace(chip);
+		return TRANSPORT_FAILED;
 	}
 
 	scsi_set_resid(srb, 0);
@@ -2651,7 +2771,8 @@ static int spi_vendor_cmd(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 
 	if (CHECK_PID(chip, 0x5208) || CHECK_PID(chip, 0x5288)) {
 		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-		TRACE_RET(chip, TRANSPORT_FAILED);
+		rtsx_trace(chip);
+		return TRANSPORT_FAILED;
 	}
 
 	rtsx_disable_aspm(chip);
@@ -2700,13 +2821,16 @@ static int spi_vendor_cmd(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 		rtsx_write_register(chip, CARD_GPIO_DIR, 0x07, gpio_dir);
 
 		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-		TRACE_RET(chip, TRANSPORT_FAILED);
+		rtsx_trace(chip);
+		return TRANSPORT_FAILED;
 	}
 
 	rtsx_write_register(chip, CARD_GPIO_DIR, 0x07, gpio_dir);
 
-	if (result != STATUS_SUCCESS)
-		TRACE_RET(chip, TRANSPORT_FAILED);
+	if (result != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return TRANSPORT_FAILED;
+	}
 
 	return TRANSPORT_GOOD;
 }
@@ -2765,7 +2889,8 @@ static int vendor_cmnd(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 	default:
 		set_sense_type(chip, SCSI_LUN(srb),
 			SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-		TRACE_RET(chip, TRANSPORT_FAILED);
+		rtsx_trace(chip);
+		return TRANSPORT_FAILED;
 	}
 
 	return result;
@@ -2797,18 +2922,21 @@ static int ms_format_cmnd(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 {
 	struct ms_info *ms_card = &(chip->ms_card);
 	unsigned int lun = SCSI_LUN(srb);
-	int retval, quick_format;
+	bool quick_format;
+	int retval;
 
 	if (get_lun_card(chip, lun) != MS_CARD) {
 		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_LUN_NOT_SUPPORT);
-		TRACE_RET(chip, TRANSPORT_FAILED);
+		rtsx_trace(chip);
+		return TRANSPORT_FAILED;
 	}
 
 	if ((srb->cmnd[3] != 0x4D) || (srb->cmnd[4] != 0x47) ||
 		(srb->cmnd[5] != 0x66) || (srb->cmnd[6] != 0x6D) ||
 		(srb->cmnd[7] != 0x74)) {
 		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-		TRACE_RET(chip, TRANSPORT_FAILED);
+		rtsx_trace(chip);
+		return TRANSPORT_FAILED;
 	}
 
 	rtsx_disable_aspm(chip);
@@ -2820,35 +2948,40 @@ static int ms_format_cmnd(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 		if (!check_card_ready(chip, lun) ||
 				(get_card_size(chip, lun) == 0)) {
 			set_sense_type(chip, lun, SENSE_TYPE_MEDIA_NOT_PRESENT);
-			TRACE_RET(chip, TRANSPORT_FAILED);
+			rtsx_trace(chip);
+			return TRANSPORT_FAILED;
 		}
 	}
 	rtsx_set_stat(chip, RTSX_STAT_RUN);
 
 	if (srb->cmnd[8] & 0x01)
-		quick_format = 0;
+		quick_format = false;
 	else
-		quick_format = 1;
+		quick_format = true;
 
 	if (!(chip->card_ready & MS_CARD)) {
 		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_NOT_PRESENT);
-		TRACE_RET(chip, TRANSPORT_FAILED);
+		rtsx_trace(chip);
+		return TRANSPORT_FAILED;
 	}
 
 	if (chip->card_wp & MS_CARD) {
 		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_WRITE_PROTECT);
-		TRACE_RET(chip, TRANSPORT_FAILED);
+		rtsx_trace(chip);
+		return TRANSPORT_FAILED;
 	}
 
 	if (!CHK_MSPRO(ms_card)) {
 		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_LUN_NOT_SUPPORT);
-		TRACE_RET(chip, TRANSPORT_FAILED);
+		rtsx_trace(chip);
+		return TRANSPORT_FAILED;
 	}
 
 	retval = mspro_format(srb, chip, MS_SHORT_DATA_LEN, quick_format);
 	if (retval != STATUS_SUCCESS) {
 		set_sense_type(chip, lun, SENSE_TYPE_FORMAT_CMD_FAILED);
-		TRACE_RET(chip, TRANSPORT_FAILED);
+		rtsx_trace(chip);
+		return TRANSPORT_FAILED;
 	}
 
 	scsi_set_resid(srb, 0);
@@ -2867,18 +3000,21 @@ static int get_ms_information(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 
 	if (!check_card_ready(chip, lun)) {
 		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_NOT_PRESENT);
-		TRACE_RET(chip, TRANSPORT_FAILED);
+		rtsx_trace(chip);
+		return TRANSPORT_FAILED;
 	}
 	if (get_lun_card(chip, lun) != MS_CARD) {
 		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_LUN_NOT_SUPPORT);
-		TRACE_RET(chip, TRANSPORT_FAILED);
+		rtsx_trace(chip);
+		return TRANSPORT_FAILED;
 	}
 
 	if ((srb->cmnd[2] != 0xB0) || (srb->cmnd[4] != 0x4D) ||
 		(srb->cmnd[5] != 0x53) || (srb->cmnd[6] != 0x49) ||
 		(srb->cmnd[7] != 0x44)) {
 		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-		TRACE_RET(chip, TRANSPORT_FAILED);
+		rtsx_trace(chip);
+		return TRANSPORT_FAILED;
 	}
 
 	dev_info_id = srb->cmnd[3];
@@ -2886,7 +3022,8 @@ static int get_ms_information(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 			(!CHK_MSXC(ms_card) && (dev_info_id == 0x13)) ||
 			!CHK_MSPRO(ms_card)) {
 		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-		TRACE_RET(chip, TRANSPORT_FAILED);
+		rtsx_trace(chip);
+		return TRANSPORT_FAILED;
 	}
 
 	if (dev_info_id == 0x15)
@@ -2895,8 +3032,10 @@ static int get_ms_information(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 		buf_len = data_len = 0x6A;
 
 	buf = kmalloc(buf_len, GFP_KERNEL);
-	if (!buf)
-		TRACE_RET(chip, TRANSPORT_ERROR);
+	if (!buf) {
+		rtsx_trace(chip);
+		return TRANSPORT_ERROR;
+	}
 
 	i = 0;
 	/*  GET Memory Stick Media Information Response Header */
@@ -2983,11 +3122,13 @@ static int sd_extention_cmnd(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 
 	if (!check_card_ready(chip, lun)) {
 		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_NOT_PRESENT);
-		TRACE_RET(chip, TRANSPORT_FAILED);
+		rtsx_trace(chip);
+		return TRANSPORT_FAILED;
 	}
 	if (get_lun_card(chip, lun) != SD_CARD) {
 		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_LUN_NOT_SUPPORT);
-		TRACE_RET(chip, TRANSPORT_FAILED);
+		rtsx_trace(chip);
+		return TRANSPORT_FAILED;
 	}
 
 	switch (srb->cmnd[0]) {
@@ -3017,7 +3158,8 @@ static int sd_extention_cmnd(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 
 	default:
 		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-		TRACE_RET(chip, TRANSPORT_FAILED);
+		rtsx_trace(chip);
+		return TRANSPORT_FAILED;
 	}
 
 	return result;
@@ -3044,21 +3186,25 @@ static int mg_report_key(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 
 	if (!check_card_ready(chip, lun)) {
 		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_NOT_PRESENT);
-		TRACE_RET(chip, TRANSPORT_FAILED);
+		rtsx_trace(chip);
+		return TRANSPORT_FAILED;
 	}
 	if (get_lun_card(chip, lun) != MS_CARD) {
 		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_LUN_NOT_SUPPORT);
-		TRACE_RET(chip, TRANSPORT_FAILED);
+		rtsx_trace(chip);
+		return TRANSPORT_FAILED;
 	}
 
 	if (srb->cmnd[7] != KC_MG_R_PRO) {
 		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-		TRACE_RET(chip, TRANSPORT_FAILED);
+		rtsx_trace(chip);
+		return TRANSPORT_FAILED;
 	}
 
 	if (!CHK_MSPRO(ms_card)) {
 		set_sense_type(chip, lun, SENSE_TYPE_MG_INCOMPATIBLE_MEDIUM);
-		TRACE_RET(chip, TRANSPORT_FAILED);
+		rtsx_trace(chip);
+		return TRANSPORT_FAILED;
 	}
 
 	key_format = srb->cmnd[10] & 0x3F;
@@ -3070,13 +3216,16 @@ static int mg_report_key(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 			(srb->cmnd[8] == 0x04) &&
 			(srb->cmnd[9] == 0x1C)) {
 			retval = mg_get_local_EKB(srb, chip);
-			if (retval != STATUS_SUCCESS)
-				TRACE_RET(chip, TRANSPORT_FAILED);
+			if (retval != STATUS_SUCCESS) {
+				rtsx_trace(chip);
+				return TRANSPORT_FAILED;
+			}
 
 		} else {
 			set_sense_type(chip, lun,
 				SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-			TRACE_RET(chip, TRANSPORT_FAILED);
+			rtsx_trace(chip);
+			return TRANSPORT_FAILED;
 		}
 		break;
 
@@ -3085,13 +3234,16 @@ static int mg_report_key(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 			(srb->cmnd[8] == 0x00) &&
 			(srb->cmnd[9] == 0x24)) {
 			retval = mg_get_rsp_chg(srb, chip);
-			if (retval != STATUS_SUCCESS)
-				TRACE_RET(chip, TRANSPORT_FAILED);
+			if (retval != STATUS_SUCCESS) {
+				rtsx_trace(chip);
+				return TRANSPORT_FAILED;
+			}
 
 		} else {
 			set_sense_type(chip, lun,
 				SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-			TRACE_RET(chip, TRANSPORT_FAILED);
+			rtsx_trace(chip);
+			return TRANSPORT_FAILED;
 		}
 		break;
 
@@ -3105,19 +3257,23 @@ static int mg_report_key(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 			(srb->cmnd[4] == 0x00) &&
 			(srb->cmnd[5] < 32)) {
 			retval = mg_get_ICV(srb, chip);
-			if (retval != STATUS_SUCCESS)
-				TRACE_RET(chip, TRANSPORT_FAILED);
+			if (retval != STATUS_SUCCESS) {
+				rtsx_trace(chip);
+				return TRANSPORT_FAILED;
+			}
 
 		} else {
 			set_sense_type(chip, lun,
 				SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-			TRACE_RET(chip, TRANSPORT_FAILED);
+			rtsx_trace(chip);
+			return TRANSPORT_FAILED;
 		}
 		break;
 
 	default:
 		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-		TRACE_RET(chip, TRANSPORT_FAILED);
+		rtsx_trace(chip);
+		return TRANSPORT_FAILED;
 	}
 
 	scsi_set_resid(srb, 0);
@@ -3143,25 +3299,30 @@ static int mg_send_key(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 
 	if (!check_card_ready(chip, lun)) {
 		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_NOT_PRESENT);
-		TRACE_RET(chip, TRANSPORT_FAILED);
+		rtsx_trace(chip);
+		return TRANSPORT_FAILED;
 	}
 	if (check_card_wp(chip, lun)) {
 		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_WRITE_PROTECT);
-		TRACE_RET(chip, TRANSPORT_FAILED);
+		rtsx_trace(chip);
+		return TRANSPORT_FAILED;
 	}
 	if (get_lun_card(chip, lun) != MS_CARD) {
 		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_LUN_NOT_SUPPORT);
-		TRACE_RET(chip, TRANSPORT_FAILED);
+		rtsx_trace(chip);
+		return TRANSPORT_FAILED;
 	}
 
 	if (srb->cmnd[7] != KC_MG_R_PRO) {
 		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-		TRACE_RET(chip, TRANSPORT_FAILED);
+		rtsx_trace(chip);
+		return TRANSPORT_FAILED;
 	}
 
 	if (!CHK_MSPRO(ms_card)) {
 		set_sense_type(chip, lun, SENSE_TYPE_MG_INCOMPATIBLE_MEDIUM);
-		TRACE_RET(chip, TRANSPORT_FAILED);
+		rtsx_trace(chip);
+		return TRANSPORT_FAILED;
 	}
 
 	key_format = srb->cmnd[10] & 0x3F;
@@ -3173,13 +3334,16 @@ static int mg_send_key(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 			(srb->cmnd[8] == 0x00) &&
 			(srb->cmnd[9] == 0x0C)) {
 			retval = mg_set_leaf_id(srb, chip);
-			if (retval != STATUS_SUCCESS)
-				TRACE_RET(chip, TRANSPORT_FAILED);
+			if (retval != STATUS_SUCCESS) {
+				rtsx_trace(chip);
+				return TRANSPORT_FAILED;
+			}
 
 		} else {
 			set_sense_type(chip, lun,
 				SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-			TRACE_RET(chip, TRANSPORT_FAILED);
+			rtsx_trace(chip);
+			return TRANSPORT_FAILED;
 		}
 		break;
 
@@ -3188,13 +3352,16 @@ static int mg_send_key(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 			(srb->cmnd[8] == 0x00) &&
 			(srb->cmnd[9] == 0x0C)) {
 			retval = mg_chg(srb, chip);
-			if (retval != STATUS_SUCCESS)
-				TRACE_RET(chip, TRANSPORT_FAILED);
+			if (retval != STATUS_SUCCESS) {
+				rtsx_trace(chip);
+				return TRANSPORT_FAILED;
+			}
 
 		} else {
 			set_sense_type(chip, lun,
 				SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-			TRACE_RET(chip, TRANSPORT_FAILED);
+			rtsx_trace(chip);
+			return TRANSPORT_FAILED;
 		}
 		break;
 
@@ -3203,13 +3370,16 @@ static int mg_send_key(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 			(srb->cmnd[8] == 0x00) &&
 			(srb->cmnd[9] == 0x0C)) {
 			retval = mg_rsp(srb, chip);
-			if (retval != STATUS_SUCCESS)
-				TRACE_RET(chip, TRANSPORT_FAILED);
+			if (retval != STATUS_SUCCESS) {
+				rtsx_trace(chip);
+				return TRANSPORT_FAILED;
+			}
 
 		} else {
 			set_sense_type(chip, lun,
 				SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-			TRACE_RET(chip, TRANSPORT_FAILED);
+			rtsx_trace(chip);
+			return TRANSPORT_FAILED;
 		}
 		break;
 
@@ -3223,19 +3393,23 @@ static int mg_send_key(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 			(srb->cmnd[4] == 0x00) &&
 			(srb->cmnd[5] < 32)) {
 			retval = mg_set_ICV(srb, chip);
-			if (retval != STATUS_SUCCESS)
-				TRACE_RET(chip, TRANSPORT_FAILED);
+			if (retval != STATUS_SUCCESS) {
+				rtsx_trace(chip);
+				return TRANSPORT_FAILED;
+			}
 
 		} else {
 			set_sense_type(chip, lun,
 				SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-			TRACE_RET(chip, TRANSPORT_FAILED);
+			rtsx_trace(chip);
+			return TRANSPORT_FAILED;
 		}
 		break;
 
 	default:
 		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-		TRACE_RET(chip, TRANSPORT_FAILED);
+		rtsx_trace(chip);
+		return TRANSPORT_FAILED;
 	}
 
 	scsi_set_resid(srb, 0);
@@ -3264,7 +3438,8 @@ int rtsx_scsi_handler(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 			/* Logical Unit Not Ready Format in Progress */
 			set_sense_data(chip, lun, CUR_ERR,
 				       0x02, 0, 0x04, 0x04, 0, 0);
-			TRACE_RET(chip, TRANSPORT_FAILED);
+			rtsx_trace(chip);
+			return TRANSPORT_FAILED;
 		}
 	}
 #endif
@@ -3276,7 +3451,8 @@ int rtsx_scsi_handler(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 			/* Logical Unit Not Ready Format in Progress */
 			set_sense_data(chip, lun, CUR_ERR, 0x02, 0, 0x04, 0x04,
 					0, (u16)(ms_card->progress));
-			TRACE_RET(chip, TRANSPORT_FAILED);
+			rtsx_trace(chip);
+			return TRANSPORT_FAILED;
 		}
 	}
 
diff --git a/drivers/staging/rts5208/rtsx_transport.c b/drivers/staging/rts5208/rtsx_transport.c
index dab1995d1a6a..f27491e802ed 100644
--- a/drivers/staging/rts5208/rtsx_transport.c
+++ b/drivers/staging/rts5208/rtsx_transport.c
@@ -25,10 +25,6 @@
 #include <linux/sched.h>
 
 #include "rtsx.h"
-#include "rtsx_scsi.h"
-#include "rtsx_transport.h"
-#include "rtsx_chip.h"
-#include "rtsx_card.h"
 
 /***********************************************************************
  * Scatter-gather transfer buffer access routines
@@ -276,7 +272,8 @@ int rtsx_send_cmd(struct rtsx_chip *chip, u8 card, int timeout)
 		dev_dbg(rtsx_dev(chip), "chip->int_reg = 0x%x\n",
 			chip->int_reg);
 		err = -ETIMEDOUT;
-		TRACE_GOTO(chip, finish_send_cmd);
+		rtsx_trace(chip);
+		goto finish_send_cmd;
 	}
 
 	spin_lock_irq(&rtsx->reg_lock);
@@ -642,7 +639,7 @@ static int rtsx_transfer_buf(struct rtsx_chip *chip, u8 card, void *buf,
 	dma_addr_t addr;
 	u8 dir;
 	int err = 0;
-	u32 val = (1 << 31);
+	u32 val = 1 << 31;
 	long timeleft;
 
 	if ((buf == NULL) || (len <= 0))
diff --git a/drivers/staging/rts5208/sd.c b/drivers/staging/rts5208/sd.c
index c28a92773f05..a8d657bb5c1b 100644
--- a/drivers/staging/rts5208/sd.c
+++ b/drivers/staging/rts5208/sd.c
@@ -25,9 +25,6 @@
 #include <linux/sched.h>
 
 #include "rtsx.h"
-#include "rtsx_transport.h"
-#include "rtsx_scsi.h"
-#include "rtsx_card.h"
 #include "sd.h"
 
 #define SD_MAX_RETRY_COUNT	3
@@ -108,13 +105,19 @@ static void sd_init_reg_addr(struct rtsx_chip *chip)
 
 static int sd_check_data0_status(struct rtsx_chip *chip)
 {
+	int retval;
 	u8 stat;
 
-	RTSX_READ_REG(chip, REG_SD_STAT1, &stat);
+	retval = rtsx_read_register(chip, REG_SD_STAT1, &stat);
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
 
 	if (!(stat & SD_DAT0_STATUS)) {
 		sd_set_err_code(chip, SD_BUSY);
-		TRACE_RET(chip, STATUS_FAIL);
+		rtsx_trace(chip);
+		return STATUS_FAIL;
 	}
 
 	return STATUS_SUCCESS;
@@ -187,7 +190,8 @@ RTY_SEND_CMD:
 				retval = sd_check_data0_status(chip);
 				if (retval != STATUS_SUCCESS) {
 					rtsx_clear_sd_error(chip);
-					TRACE_RET(chip, retval);
+					rtsx_trace(chip);
+					return retval;
 				}
 			} else {
 				sd_set_err_code(chip, SD_TO_ERR);
@@ -198,7 +202,8 @@ RTY_SEND_CMD:
 		}
 		rtsx_clear_sd_error(chip);
 
-		TRACE_RET(chip, retval);
+		rtsx_trace(chip);
+		return retval;
 	}
 
 	if (rsp_type == SD_RSP_TYPE_R0)
@@ -208,14 +213,16 @@ RTY_SEND_CMD:
 
 	if ((ptr[0] & 0xC0) != 0) {
 		sd_set_err_code(chip, SD_STS_ERR);
-		TRACE_RET(chip, STATUS_FAIL);
+		rtsx_trace(chip);
+		return STATUS_FAIL;
 	}
 
 	if (!(rsp_type & SD_NO_CHECK_CRC7)) {
 		if (ptr[stat_idx] & SD_CRC7_ERR) {
 			if (cmd_idx == WRITE_MULTIPLE_BLOCK) {
 				sd_set_err_code(chip, SD_CRC_ERR);
-				TRACE_RET(chip, STATUS_FAIL);
+				rtsx_trace(chip);
+				return STATUS_FAIL;
 			}
 			if (rty_cnt < SD_MAX_RETRY_COUNT) {
 				wait_timeout(20);
@@ -223,7 +230,8 @@ RTY_SEND_CMD:
 				goto RTY_SEND_CMD;
 			} else {
 				sd_set_err_code(chip, SD_CRC_ERR);
-				TRACE_RET(chip, STATUS_FAIL);
+				rtsx_trace(chip);
+				return STATUS_FAIL;
 			}
 		}
 	}
@@ -232,8 +240,10 @@ RTY_SEND_CMD:
 		if ((cmd_idx != SEND_RELATIVE_ADDR) &&
 			(cmd_idx != SEND_IF_COND)) {
 			if (cmd_idx != STOP_TRANSMISSION) {
-				if (ptr[1] & 0x80)
-					TRACE_RET(chip, STATUS_FAIL);
+				if (ptr[1] & 0x80) {
+					rtsx_trace(chip);
+					return STATUS_FAIL;
+				}
 			}
 #ifdef SUPPORT_SD_LOCK
 			if (ptr[1] & 0x7D)
@@ -243,17 +253,20 @@ RTY_SEND_CMD:
 			{
 				dev_dbg(rtsx_dev(chip), "ptr[1]: 0x%02x\n",
 					ptr[1]);
-				TRACE_RET(chip, STATUS_FAIL);
+				rtsx_trace(chip);
+				return STATUS_FAIL;
 			}
 			if (ptr[2] & 0xFF) {
 				dev_dbg(rtsx_dev(chip), "ptr[2]: 0x%02x\n",
 					ptr[2]);
-				TRACE_RET(chip, STATUS_FAIL);
+				rtsx_trace(chip);
+				return STATUS_FAIL;
 			}
 			if (ptr[3] & 0x80) {
 				dev_dbg(rtsx_dev(chip), "ptr[3]: 0x%02x\n",
 					ptr[3]);
-				TRACE_RET(chip, STATUS_FAIL);
+				rtsx_trace(chip);
+				return STATUS_FAIL;
 			}
 			if (ptr[3] & 0x01)
 				sd_card->sd_data_buf_ready = 1;
@@ -282,8 +295,10 @@ static int sd_read_data(struct rtsx_chip *chip,
 	if (!buf)
 		buf_len = 0;
 
-	if (buf_len > 512)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (buf_len > 512) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	rtsx_init_cmd(chip);
 
@@ -323,13 +338,16 @@ static int sd_read_data(struct rtsx_chip *chip,
 					    SD_RSP_TYPE_R1, NULL, 0);
 		}
 
-		TRACE_RET(chip, STATUS_FAIL);
+		rtsx_trace(chip);
+		return STATUS_FAIL;
 	}
 
 	if (buf && buf_len) {
 		retval = rtsx_read_ppbuf(chip, buf, buf_len);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, STATUS_FAIL);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 	}
 
 	return STATUS_SUCCESS;
@@ -350,13 +368,16 @@ static int sd_write_data(struct rtsx_chip *chip, u8 trans_mode,
 
 	if (buf_len > 512) {
 		/* This function can't write data more than one page */
-		TRACE_RET(chip, STATUS_FAIL);
+		rtsx_trace(chip);
+		return STATUS_FAIL;
 	}
 
 	if (buf && buf_len) {
 		retval = rtsx_write_ppbuf(chip, buf, buf_len);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, STATUS_FAIL);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 	}
 
 	rtsx_init_cmd(chip);
@@ -395,7 +416,8 @@ static int sd_write_data(struct rtsx_chip *chip, u8 trans_mode,
 				sd_card->sd_addr, SD_RSP_TYPE_R1, NULL, 0);
 		}
 
-		TRACE_RET(chip, STATUS_FAIL);
+		rtsx_trace(chip);
+		return STATUS_FAIL;
 	}
 
 	return STATUS_SUCCESS;
@@ -412,7 +434,8 @@ static int sd_check_csd(struct rtsx_chip *chip, char check_wp)
 	for (i = 0; i < 6; i++) {
 		if (detect_card_cd(chip, SD_CARD) != STATUS_SUCCESS) {
 			sd_set_err_code(chip, SD_NO_CARD);
-			TRACE_RET(chip, STATUS_FAIL);
+			rtsx_trace(chip);
+			return STATUS_FAIL;
 		}
 
 		retval = sd_send_cmd_get_rsp(chip, SEND_CSD, sd_card->sd_addr,
@@ -421,8 +444,10 @@ static int sd_check_csd(struct rtsx_chip *chip, char check_wp)
 			break;
 	}
 
-	if (i == 6)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (i == 6) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	memcpy(sd_card->raw_csd, rsp + 1, 15);
 
@@ -464,10 +489,12 @@ static int sd_check_csd(struct rtsx_chip *chip, char check_wp)
 			else
 				sd_card->sd_clock = CLK_20;
 		} else {
-			TRACE_RET(chip, STATUS_FAIL);
+			rtsx_trace(chip);
+			return STATUS_FAIL;
 		}
 	} else {
-		TRACE_RET(chip, STATUS_FAIL);
+		rtsx_trace(chip);
+		return STATUS_FAIL;
 	}
 
 	if (CHK_MMC_SECTOR_MODE(sd_card)) {
@@ -507,8 +534,8 @@ static int sd_check_csd(struct rtsx_chip *chip, char check_wp)
 
 static int sd_set_sample_push_timing(struct rtsx_chip *chip)
 {
+	int retval;
 	struct sd_info *sd_card = &(chip->sd_card);
-
 	u8 val = 0;
 
 	if ((chip->sd_ctl & SD_PUSH_POINT_CTL_MASK) == SD_PUSH_POINT_DELAY)
@@ -536,7 +563,11 @@ static int sd_set_sample_push_timing(struct rtsx_chip *chip)
 			val |= 0x08;
 	}
 
-	RTSX_WRITE_REG(chip, REG_SD_CFG1, 0x1C, val);
+	retval = rtsx_write_register(chip, REG_SD_CFG1, 0x1C, val);
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
 
 	return STATUS_SUCCESS;
 }
@@ -585,6 +616,7 @@ static void sd_choose_proper_clock(struct rtsx_chip *chip)
 
 static int sd_set_clock_divider(struct rtsx_chip *chip, u8 clk_div)
 {
+	int retval;
 	u8 mask = 0, val = 0;
 
 	mask = 0x60;
@@ -595,7 +627,11 @@ static int sd_set_clock_divider(struct rtsx_chip *chip, u8 clk_div)
 	else if (clk_div == SD_CLK_DIVIDE_256)
 		val = 0x20;
 
-	RTSX_WRITE_REG(chip, REG_SD_CFG1, mask, val);
+	retval = rtsx_write_register(chip, REG_SD_CFG1, mask, val);
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
 
 	return STATUS_SUCCESS;
 }
@@ -606,14 +642,18 @@ static int sd_set_init_para(struct rtsx_chip *chip)
 	int retval;
 
 	retval = sd_set_sample_push_timing(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	sd_choose_proper_clock(chip);
 
 	retval = switch_clock(chip, sd_card->sd_clock);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	return STATUS_SUCCESS;
 }
@@ -636,8 +676,10 @@ int sd_select_card(struct rtsx_chip *chip, int select)
 	}
 
 	retval = sd_send_cmd_get_rsp(chip, cmd_idx, addr, cmd_type, NULL, 0);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	return STATUS_SUCCESS;
 }
@@ -651,8 +693,10 @@ static int sd_update_lock_status(struct rtsx_chip *chip)
 
 	retval = sd_send_cmd_get_rsp(chip, SEND_STATUS, sd_card->sd_addr,
 				SD_RSP_TYPE_R1, rsp, 5);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	if (rsp[1] & 0x02)
 		sd_card->sd_lock_status |= SD_LOCKED;
@@ -662,8 +706,10 @@ static int sd_update_lock_status(struct rtsx_chip *chip)
 	dev_dbg(rtsx_dev(chip), "sd_card->sd_lock_status = 0x%x\n",
 		sd_card->sd_lock_status);
 
-	if (rsp[1] & 0x01)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (rsp[1] & 0x01) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	return STATUS_SUCCESS;
 }
@@ -680,15 +726,18 @@ static int sd_wait_state_data_ready(struct rtsx_chip *chip, u8 state,
 		retval = sd_send_cmd_get_rsp(chip, SEND_STATUS,
 					sd_card->sd_addr, SD_RSP_TYPE_R1, rsp,
 					5);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, STATUS_FAIL);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 
 		if (((rsp[3] & 0x1E) == state) &&
 			((rsp[3] & 0x01) == data_ready))
 			return STATUS_SUCCESS;
 	}
 
-	TRACE_RET(chip, STATUS_FAIL);
+	rtsx_trace(chip);
+	return STATUS_FAIL;
 }
 
 static int sd_change_bank_voltage(struct rtsx_chip *chip, u8 voltage)
@@ -700,24 +749,39 @@ static int sd_change_bank_voltage(struct rtsx_chip *chip, u8 voltage)
 			retval = rtsx_write_phy_register(chip, 0x08,
 							0x4FC0 |
 							chip->phy_voltage);
-			if (retval != STATUS_SUCCESS)
-				TRACE_RET(chip, STATUS_FAIL);
+			if (retval != STATUS_SUCCESS) {
+				rtsx_trace(chip);
+				return STATUS_FAIL;
+			}
 		} else {
-			RTSX_WRITE_REG(chip, SD_PAD_CTL, SD_IO_USING_1V8, 0);
+			retval = rtsx_write_register(chip, SD_PAD_CTL,
+						     SD_IO_USING_1V8, 0);
+			if (retval) {
+				rtsx_trace(chip);
+				return retval;
+			}
 		}
 	} else if (voltage == SD_IO_1V8) {
 		if (chip->asic_code) {
 			retval = rtsx_write_phy_register(chip, 0x08,
 							0x4C40 |
 							chip->phy_voltage);
-			if (retval != STATUS_SUCCESS)
-				TRACE_RET(chip, STATUS_FAIL);
+			if (retval != STATUS_SUCCESS) {
+				rtsx_trace(chip);
+				return STATUS_FAIL;
+			}
 		} else {
-			RTSX_WRITE_REG(chip, SD_PAD_CTL, SD_IO_USING_1V8,
-				SD_IO_USING_1V8);
+			retval = rtsx_write_register(chip, SD_PAD_CTL,
+						     SD_IO_USING_1V8,
+						     SD_IO_USING_1V8);
+			if (retval) {
+				rtsx_trace(chip);
+				return retval;
+			}
 		}
 	} else {
-		TRACE_RET(chip, STATUS_FAIL);
+		rtsx_trace(chip);
+		return STATUS_FAIL;
 	}
 
 	return STATUS_SUCCESS;
@@ -728,33 +792,61 @@ static int sd_voltage_switch(struct rtsx_chip *chip)
 	int retval;
 	u8 stat;
 
-	RTSX_WRITE_REG(chip, SD_BUS_STAT, SD_CLK_TOGGLE_EN | SD_CLK_FORCE_STOP,
-		SD_CLK_TOGGLE_EN);
+	retval = rtsx_write_register(chip, SD_BUS_STAT,
+				     SD_CLK_TOGGLE_EN | SD_CLK_FORCE_STOP,
+				     SD_CLK_TOGGLE_EN);
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
 
 	retval = sd_send_cmd_get_rsp(chip, VOLTAGE_SWITCH, 0, SD_RSP_TYPE_R1,
 				NULL, 0);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	udelay(chip->sd_voltage_switch_delay);
 
-	RTSX_READ_REG(chip, SD_BUS_STAT, &stat);
+	retval = rtsx_read_register(chip, SD_BUS_STAT, &stat);
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
 	if (stat & (SD_CMD_STATUS | SD_DAT3_STATUS | SD_DAT2_STATUS |
 				SD_DAT1_STATUS | SD_DAT0_STATUS)) {
-		TRACE_RET(chip, STATUS_FAIL);
+		rtsx_trace(chip);
+		return STATUS_FAIL;
 	}
 
-	RTSX_WRITE_REG(chip, SD_BUS_STAT, 0xFF, SD_CLK_FORCE_STOP);
+	retval = rtsx_write_register(chip, SD_BUS_STAT, 0xFF,
+				     SD_CLK_FORCE_STOP);
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
 	retval = sd_change_bank_voltage(chip, SD_IO_1V8);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	wait_timeout(50);
 
-	RTSX_WRITE_REG(chip, SD_BUS_STAT, 0xFF, SD_CLK_TOGGLE_EN);
+	retval = rtsx_write_register(chip, SD_BUS_STAT, 0xFF,
+				     SD_CLK_TOGGLE_EN);
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
 	wait_timeout(10);
 
-	RTSX_READ_REG(chip, SD_BUS_STAT, &stat);
+	retval = rtsx_read_register(chip, SD_BUS_STAT, &stat);
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
 	if ((stat & (SD_CMD_STATUS | SD_DAT3_STATUS | SD_DAT2_STATUS |
 				SD_DAT1_STATUS | SD_DAT0_STATUS)) !=
 			(SD_CMD_STATUS | SD_DAT3_STATUS | SD_DAT2_STATUS |
@@ -763,23 +855,48 @@ static int sd_voltage_switch(struct rtsx_chip *chip)
 		rtsx_write_register(chip, SD_BUS_STAT,
 				SD_CLK_TOGGLE_EN | SD_CLK_FORCE_STOP, 0);
 		rtsx_write_register(chip, CARD_CLK_EN, 0xFF, 0);
-		TRACE_RET(chip, STATUS_FAIL);
+		rtsx_trace(chip);
+		return STATUS_FAIL;
 	}
 
-	RTSX_WRITE_REG(chip, SD_BUS_STAT, SD_CLK_TOGGLE_EN | SD_CLK_FORCE_STOP,
-		0);
+	retval = rtsx_write_register(chip, SD_BUS_STAT,
+				     SD_CLK_TOGGLE_EN | SD_CLK_FORCE_STOP, 0);
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
 
 	return STATUS_SUCCESS;
 }
 
 static int sd_reset_dcm(struct rtsx_chip *chip, u8 tune_dir)
 {
+	int retval;
+
 	if (tune_dir == TUNE_RX) {
-		RTSX_WRITE_REG(chip, DCM_DRP_CTL, 0xFF, DCM_RESET | DCM_RX);
-		RTSX_WRITE_REG(chip, DCM_DRP_CTL, 0xFF, DCM_RX);
+		retval = rtsx_write_register(chip, DCM_DRP_CTL, 0xFF,
+					     DCM_RESET | DCM_RX);
+		if (retval) {
+			rtsx_trace(chip);
+			return retval;
+		}
+		retval = rtsx_write_register(chip, DCM_DRP_CTL, 0xFF, DCM_RX);
+		if (retval) {
+			rtsx_trace(chip);
+			return retval;
+		}
 	} else {
-		RTSX_WRITE_REG(chip, DCM_DRP_CTL, 0xFF, DCM_RESET | DCM_TX);
-		RTSX_WRITE_REG(chip, DCM_DRP_CTL, 0xFF, DCM_TX);
+		retval = rtsx_write_register(chip, DCM_DRP_CTL, 0xFF,
+					     DCM_RESET | DCM_TX);
+		if (retval) {
+			rtsx_trace(chip);
+			return retval;
+		}
+		retval = rtsx_write_register(chip, DCM_DRP_CTL, 0xFF, DCM_TX);
+		if (retval) {
+			rtsx_trace(chip);
+			return retval;
+		}
 	}
 
 	return STATUS_SUCCESS;
@@ -791,7 +908,7 @@ static int sd_change_phase(struct rtsx_chip *chip, u8 sample_point, u8 tune_dir)
 	u16 SD_VP_CTL, SD_DCMPS_CTL;
 	u8 val;
 	int retval;
-	int ddr_rx = 0;
+	bool ddr_rx = false;
 
 	dev_dbg(rtsx_dev(chip), "sd_change_phase (sample_point = %d, tune_dir = %d)\n",
 		sample_point, tune_dir);
@@ -800,19 +917,42 @@ static int sd_change_phase(struct rtsx_chip *chip, u8 sample_point, u8 tune_dir)
 		SD_VP_CTL = SD_VPRX_CTL;
 		SD_DCMPS_CTL = SD_DCMPS_RX_CTL;
 		if (CHK_SD_DDR50(sd_card))
-			ddr_rx = 1;
+			ddr_rx = true;
 	} else {
 		SD_VP_CTL = SD_VPTX_CTL;
 		SD_DCMPS_CTL = SD_DCMPS_TX_CTL;
 	}
 
 	if (chip->asic_code) {
-		RTSX_WRITE_REG(chip, CLK_CTL, CHANGE_CLK, CHANGE_CLK);
-		RTSX_WRITE_REG(chip, SD_VP_CTL, 0x1F, sample_point);
-		RTSX_WRITE_REG(chip, SD_VPCLK0_CTL, PHASE_NOT_RESET, 0);
-		RTSX_WRITE_REG(chip, SD_VPCLK0_CTL, PHASE_NOT_RESET,
-			PHASE_NOT_RESET);
-		RTSX_WRITE_REG(chip, CLK_CTL, CHANGE_CLK, 0);
+		retval = rtsx_write_register(chip, CLK_CTL, CHANGE_CLK,
+					     CHANGE_CLK);
+		if (retval) {
+			rtsx_trace(chip);
+			return retval;
+		}
+		retval = rtsx_write_register(chip, SD_VP_CTL, 0x1F,
+					     sample_point);
+		if (retval) {
+			rtsx_trace(chip);
+			return retval;
+		}
+		retval = rtsx_write_register(chip, SD_VPCLK0_CTL,
+					     PHASE_NOT_RESET, 0);
+		if (retval) {
+			rtsx_trace(chip);
+			return retval;
+		}
+		retval = rtsx_write_register(chip, SD_VPCLK0_CTL,
+					     PHASE_NOT_RESET, PHASE_NOT_RESET);
+		if (retval) {
+			rtsx_trace(chip);
+			return retval;
+		}
+		retval = rtsx_write_register(chip, CLK_CTL, CHANGE_CLK, 0);
+		if (retval) {
+			rtsx_trace(chip);
+			return retval;
+		}
 	} else {
 		rtsx_read_register(chip, SD_VP_CTL, &val);
 		dev_dbg(rtsx_dev(chip), "SD_VP_CTL: 0x%x\n", val);
@@ -820,16 +960,34 @@ static int sd_change_phase(struct rtsx_chip *chip, u8 sample_point, u8 tune_dir)
 		dev_dbg(rtsx_dev(chip), "SD_DCMPS_CTL: 0x%x\n", val);
 
 		if (ddr_rx) {
-			RTSX_WRITE_REG(chip, SD_VP_CTL, PHASE_CHANGE,
-				PHASE_CHANGE);
+			retval = rtsx_write_register(chip, SD_VP_CTL,
+						     PHASE_CHANGE,
+						     PHASE_CHANGE);
+			if (retval) {
+				rtsx_trace(chip);
+				return retval;
+			}
 			udelay(50);
-			RTSX_WRITE_REG(chip, SD_VP_CTL, 0xFF,
-				PHASE_CHANGE | PHASE_NOT_RESET | sample_point);
+			retval = rtsx_write_register(chip, SD_VP_CTL, 0xFF,
+						     PHASE_CHANGE | PHASE_NOT_RESET | sample_point);
+			if (retval) {
+				rtsx_trace(chip);
+				return retval;
+			}
 		} else {
-			RTSX_WRITE_REG(chip, CLK_CTL, CHANGE_CLK, CHANGE_CLK);
+			retval = rtsx_write_register(chip, CLK_CTL,
+						     CHANGE_CLK, CHANGE_CLK);
+			if (retval) {
+				rtsx_trace(chip);
+				return retval;
+			}
 			udelay(50);
-			RTSX_WRITE_REG(chip, SD_VP_CTL, 0xFF,
-					PHASE_NOT_RESET | sample_point);
+			retval = rtsx_write_register(chip, SD_VP_CTL, 0xFF,
+						     PHASE_NOT_RESET | sample_point);
+			if (retval) {
+				rtsx_trace(chip);
+				return retval;
+			}
 		}
 		udelay(100);
 
@@ -839,26 +997,52 @@ static int sd_change_phase(struct rtsx_chip *chip, u8 sample_point, u8 tune_dir)
 		rtsx_add_cmd(chip, CHECK_REG_CMD, SD_DCMPS_CTL,
 			DCMPS_CHANGE_DONE, DCMPS_CHANGE_DONE);
 		retval = rtsx_send_cmd(chip, SD_CARD, 100);
-		if (retval != STATUS_SUCCESS)
-			TRACE_GOTO(chip, Fail);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			goto Fail;
+		}
 
 		val = *rtsx_get_cmd_data(chip);
-		if (val & DCMPS_ERROR)
-			TRACE_GOTO(chip, Fail);
+		if (val & DCMPS_ERROR) {
+			rtsx_trace(chip);
+			goto Fail;
+		}
 
-		if ((val & DCMPS_CURRENT_PHASE) != sample_point)
-			TRACE_GOTO(chip, Fail);
+		if ((val & DCMPS_CURRENT_PHASE) != sample_point) {
+			rtsx_trace(chip);
+			goto Fail;
+		}
 
-		RTSX_WRITE_REG(chip, SD_DCMPS_CTL, DCMPS_CHANGE, 0);
-		if (ddr_rx)
-			RTSX_WRITE_REG(chip, SD_VP_CTL, PHASE_CHANGE, 0);
-		else
-			RTSX_WRITE_REG(chip, CLK_CTL, CHANGE_CLK, 0);
+		retval = rtsx_write_register(chip, SD_DCMPS_CTL,
+					     DCMPS_CHANGE, 0);
+		if (retval) {
+			rtsx_trace(chip);
+			return retval;
+		}
+		if (ddr_rx) {
+			retval = rtsx_write_register(chip, SD_VP_CTL,
+						     PHASE_CHANGE, 0);
+			if (retval) {
+				rtsx_trace(chip);
+				return retval;
+			}
+		} else {
+			retval = rtsx_write_register(chip, CLK_CTL,
+						     CHANGE_CLK, 0);
+			if (retval) {
+				rtsx_trace(chip);
+				return retval;
+			}
+		}
 
 		udelay(50);
 	}
 
-	RTSX_WRITE_REG(chip, SD_CFG1, SD_ASYNC_FIFO_NOT_RST, 0);
+	retval = rtsx_write_register(chip, SD_CFG1, SD_ASYNC_FIFO_NOT_RST, 0);
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
 
 	return STATUS_SUCCESS;
 
@@ -883,8 +1067,10 @@ static int sd_check_spec(struct rtsx_chip *chip, u8 bus_width)
 
 	retval = sd_send_cmd_get_rsp(chip, APP_CMD, sd_card->sd_addr,
 				SD_RSP_TYPE_R1, NULL, 0);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	cmd[0] = 0x40 | SEND_SCR;
 	cmd[1] = 0;
@@ -896,13 +1082,16 @@ static int sd_check_spec(struct rtsx_chip *chip, u8 bus_width)
 			buf, 8, 250);
 	if (retval != STATUS_SUCCESS) {
 		rtsx_clear_sd_error(chip);
-		TRACE_RET(chip, STATUS_FAIL);
+		rtsx_trace(chip);
+		return STATUS_FAIL;
 	}
 
 	memcpy(sd_card->raw_scr, buf, 8);
 
-	if ((buf[0] & 0x0F) == 0)
-		TRACE_RET(chip, STATUS_FAIL);
+	if ((buf[0] & 0x0F) == 0) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	return STATUS_SUCCESS;
 }
@@ -944,7 +1133,8 @@ static int sd_query_switch_result(struct rtsx_chip *chip, u8 func_group,
 			break;
 
 		default:
-			TRACE_RET(chip, STATUS_FAIL);
+			rtsx_trace(chip);
+			return STATUS_FAIL;
 		}
 	} else if (func_group == SD_FUNC_GROUP_3) {
 		support_offset = FUNCTION_GROUP3_SUPPORT_OFFSET;
@@ -971,7 +1161,8 @@ static int sd_query_switch_result(struct rtsx_chip *chip, u8 func_group,
 			break;
 
 		default:
-			TRACE_RET(chip, STATUS_FAIL);
+			rtsx_trace(chip);
+			return STATUS_FAIL;
 		}
 	} else if (func_group == SD_FUNC_GROUP_4) {
 		support_offset = FUNCTION_GROUP4_SUPPORT_OFFSET;
@@ -998,23 +1189,27 @@ static int sd_query_switch_result(struct rtsx_chip *chip, u8 func_group,
 			break;
 
 		default:
-			TRACE_RET(chip, STATUS_FAIL);
+			rtsx_trace(chip);
+			return STATUS_FAIL;
 		}
 	} else {
-		TRACE_RET(chip, STATUS_FAIL);
+		rtsx_trace(chip);
+		return STATUS_FAIL;
 	}
 
 	if (func_group == SD_FUNC_GROUP_1) {
 		if (!(buf[support_offset] & support_mask) ||
 			((buf[query_switch_offset] & 0x0F) != query_switch)) {
-			TRACE_RET(chip, STATUS_FAIL);
+			rtsx_trace(chip);
+			return STATUS_FAIL;
 		}
 	}
 
 	/* Check 'Busy Status' */
 	if ((buf[DATA_STRUCTURE_VER_OFFSET] == 0x01) &&
 		    ((buf[check_busy_offset] & switch_busy) == switch_busy)) {
-		TRACE_RET(chip, STATUS_FAIL);
+		rtsx_trace(chip);
+		return STATUS_FAIL;
 	}
 
 	return STATUS_SUCCESS;
@@ -1056,7 +1251,8 @@ static int sd_check_switch_mode(struct rtsx_chip *chip, u8 mode,
 			buf, 64, 250);
 	if (retval != STATUS_SUCCESS) {
 		rtsx_clear_sd_error(chip);
-		TRACE_RET(chip, STATUS_FAIL);
+		rtsx_trace(chip);
+		return STATUS_FAIL;
 	}
 
 	dev_dbg(rtsx_dev(chip), "%*ph\n", 64, buf);
@@ -1083,19 +1279,33 @@ static int sd_check_switch_mode(struct rtsx_chip *chip, u8 mode,
 
 		dev_dbg(rtsx_dev(chip), "Maximum current consumption: %dmA\n",
 			cc);
-		if ((cc == 0) || (cc > 800))
-			TRACE_RET(chip, STATUS_FAIL);
+		if ((cc == 0) || (cc > 800)) {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 
 		retval = sd_query_switch_result(chip, func_group,
 						func_to_switch, buf, 64);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, STATUS_FAIL);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 
 		if ((cc > 400) || (func_to_switch > CURRENT_LIMIT_400)) {
-			RTSX_WRITE_REG(chip, OCPPARA2, SD_OCP_THD_MASK,
-				chip->sd_800mA_ocp_thd);
-			RTSX_WRITE_REG(chip, CARD_PWR_CTL, PMOS_STRG_MASK,
-				PMOS_STRG_800mA);
+			retval = rtsx_write_register(chip, OCPPARA2,
+						     SD_OCP_THD_MASK,
+						     chip->sd_800mA_ocp_thd);
+			if (retval) {
+				rtsx_trace(chip);
+				return retval;
+			}
+			retval = rtsx_write_register(chip, CARD_PWR_CTL,
+						     PMOS_STRG_MASK,
+						     PMOS_STRG_800mA);
+			if (retval) {
+				rtsx_trace(chip);
+				return retval;
+			}
 		}
 	}
 
@@ -1121,12 +1331,13 @@ static int sd_check_switch(struct rtsx_chip *chip,
 {
 	int retval;
 	int i;
-	int switch_good = 0;
+	bool switch_good = false;
 
 	for (i = 0; i < 3; i++) {
 		if (detect_card_cd(chip, SD_CARD) != STATUS_SUCCESS) {
 			sd_set_err_code(chip, SD_NO_CARD);
-			TRACE_RET(chip, STATUS_FAIL);
+			rtsx_trace(chip);
+			return STATUS_FAIL;
 		}
 
 		retval = sd_check_switch_mode(chip, SD_CHECK_MODE, func_group,
@@ -1137,14 +1348,19 @@ static int sd_check_switch(struct rtsx_chip *chip,
 			retval = sd_check_switch_mode(chip, SD_SWITCH_MODE,
 					func_group, func_to_switch, bus_width);
 			if (retval == STATUS_SUCCESS) {
-				switch_good = 1;
+				switch_good = true;
 				break;
 			}
 
-			RTSX_READ_REG(chip, SD_STAT1, &stat);
+			retval = rtsx_read_register(chip, SD_STAT1, &stat);
+			if (retval) {
+				rtsx_trace(chip);
+				return retval;
+			}
 			if (stat & SD_CRC16_ERR) {
 				dev_dbg(rtsx_dev(chip), "SD CRC16 error when switching mode\n");
-				TRACE_RET(chip, STATUS_FAIL);
+				rtsx_trace(chip);
+				return STATUS_FAIL;
 			}
 		}
 
@@ -1154,8 +1370,10 @@ static int sd_check_switch(struct rtsx_chip *chip,
 		wait_timeout(20);
 	}
 
-	if (!switch_good)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (!switch_good) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	return STATUS_SUCCESS;
 }
@@ -1170,8 +1388,10 @@ static int sd_switch_function(struct rtsx_chip *chip, u8 bus_width)
 	/* Get supported functions */
 	retval = sd_check_switch_mode(chip, SD_CHECK_MODE,
 			NO_ARGUMENT, NO_ARGUMENT, bus_width);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	sd_card->func_group1_mask &= ~(sd_card->sd_switch_fail);
 
@@ -1239,7 +1459,8 @@ static int sd_switch_function(struct rtsx_chip *chip, u8 bus_width)
 				sd_card->sd_switch_fail = SDR104_SUPPORT_MASK |
 					DDR50_SUPPORT_MASK | SDR50_SUPPORT_MASK;
 			}
-			TRACE_RET(chip, STATUS_FAIL);
+			rtsx_trace(chip);
+			return STATUS_FAIL;
 		}
 
 		if (func_to_switch == SDR104_SUPPORT)
@@ -1253,10 +1474,17 @@ static int sd_switch_function(struct rtsx_chip *chip, u8 bus_width)
 	}
 
 	if (CHK_SD_DDR50(sd_card)) {
-		RTSX_WRITE_REG(chip, SD_PUSH_POINT_CTL, 0x06, 0x04);
+		retval = rtsx_write_register(chip, SD_PUSH_POINT_CTL, 0x06,
+					     0x04);
+		if (retval) {
+			rtsx_trace(chip);
+			return retval;
+		}
 		retval = sd_set_sample_push_timing(chip);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, STATUS_FAIL);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 	}
 
 	if (!func_to_switch || (func_to_switch == HS_SUPPORT)) {
@@ -1310,15 +1538,22 @@ static int sd_switch_function(struct rtsx_chip *chip, u8 bus_width)
 		retval = sd_check_switch(chip, SD_FUNC_GROUP_4, func_to_switch,
 					bus_width);
 		if (retval != STATUS_SUCCESS) {
-			if (sd_check_err_code(chip, SD_NO_CARD))
-				TRACE_RET(chip, STATUS_FAIL);
+			if (sd_check_err_code(chip, SD_NO_CARD)) {
+				rtsx_trace(chip);
+				return STATUS_FAIL;
+			}
 		}
 		dev_dbg(rtsx_dev(chip), "Switch current limit finished! (%d)\n",
 			retval);
 	}
 
-	if (CHK_SD_DDR50(sd_card))
-		RTSX_WRITE_REG(chip, SD_PUSH_POINT_CTL, 0x06, 0);
+	if (CHK_SD_DDR50(sd_card)) {
+		retval = rtsx_write_register(chip, SD_PUSH_POINT_CTL, 0x06, 0);
+		if (retval) {
+			rtsx_trace(chip);
+			return retval;
+		}
+	}
 
 	return STATUS_SUCCESS;
 }
@@ -1330,7 +1565,11 @@ static int sd_wait_data_idle(struct rtsx_chip *chip)
 	u8 val = 0;
 
 	for (i = 0; i < 100; i++) {
-		RTSX_READ_REG(chip, SD_DATA_STATE, &val);
+		retval = rtsx_read_register(chip, SD_DATA_STATE, &val);
+		if (retval) {
+			rtsx_trace(chip);
+			return retval;
+		}
 		if (val & SD_DATA_IDLE) {
 			retval = STATUS_SUCCESS;
 			break;
@@ -1348,8 +1587,10 @@ static int sd_sdr_tuning_rx_cmd(struct rtsx_chip *chip, u8 sample_point)
 	u8 cmd[5];
 
 	retval = sd_change_phase(chip, sample_point, TUNE_RX);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	cmd[0] = 0x40 | SEND_TUNING_PATTERN;
 	cmd[1] = 0;
@@ -1363,7 +1604,8 @@ static int sd_sdr_tuning_rx_cmd(struct rtsx_chip *chip, u8 sample_point)
 		(void)sd_wait_data_idle(chip);
 
 		rtsx_clear_sd_error(chip);
-		TRACE_RET(chip, STATUS_FAIL);
+		rtsx_trace(chip);
+		return STATUS_FAIL;
 	}
 
 	return STATUS_SUCCESS;
@@ -1376,15 +1618,19 @@ static int sd_ddr_tuning_rx_cmd(struct rtsx_chip *chip, u8 sample_point)
 	u8 cmd[5];
 
 	retval = sd_change_phase(chip, sample_point, TUNE_RX);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	dev_dbg(rtsx_dev(chip), "sd ddr tuning rx\n");
 
 	retval = sd_send_cmd_get_rsp(chip, APP_CMD, sd_card->sd_addr,
 				SD_RSP_TYPE_R1, NULL, 0);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	cmd[0] = 0x40 | SD_STATUS;
 	cmd[1] = 0;
@@ -1398,7 +1644,8 @@ static int sd_ddr_tuning_rx_cmd(struct rtsx_chip *chip, u8 sample_point)
 		(void)sd_wait_data_idle(chip);
 
 		rtsx_clear_sd_error(chip);
-		TRACE_RET(chip, STATUS_FAIL);
+		rtsx_trace(chip);
+		return STATUS_FAIL;
 	}
 
 	return STATUS_SUCCESS;
@@ -1418,8 +1665,10 @@ static int mmc_ddr_tunning_rx_cmd(struct rtsx_chip *chip, u8 sample_point)
 		bus_width = SD_BUS_WIDTH_1;
 
 	retval = sd_change_phase(chip, sample_point, TUNE_RX);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	dev_dbg(rtsx_dev(chip), "mmc ddr tuning rx\n");
 
@@ -1435,7 +1684,8 @@ static int mmc_ddr_tunning_rx_cmd(struct rtsx_chip *chip, u8 sample_point)
 		(void)sd_wait_data_idle(chip);
 
 		rtsx_clear_sd_error(chip);
-		TRACE_RET(chip, STATUS_FAIL);
+		rtsx_trace(chip);
+		return STATUS_FAIL;
 	}
 
 	return STATUS_SUCCESS;
@@ -1447,11 +1697,17 @@ static int sd_sdr_tuning_tx_cmd(struct rtsx_chip *chip, u8 sample_point)
 	int retval;
 
 	retval = sd_change_phase(chip, sample_point, TUNE_TX);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
-	RTSX_WRITE_REG(chip, SD_CFG3, SD_RSP_80CLK_TIMEOUT_EN,
-		SD_RSP_80CLK_TIMEOUT_EN);
+	retval = rtsx_write_register(chip, SD_CFG3, SD_RSP_80CLK_TIMEOUT_EN,
+				     SD_RSP_80CLK_TIMEOUT_EN);
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
 
 	retval = sd_send_cmd_get_rsp(chip, SEND_STATUS, sd_card->sd_addr,
 		SD_RSP_TYPE_R1, NULL, 0);
@@ -1459,11 +1715,17 @@ static int sd_sdr_tuning_tx_cmd(struct rtsx_chip *chip, u8 sample_point)
 		if (sd_check_err_code(chip, SD_RSP_TIMEOUT)) {
 			rtsx_write_register(chip, SD_CFG3,
 					SD_RSP_80CLK_TIMEOUT_EN, 0);
-			TRACE_RET(chip, STATUS_FAIL);
+			rtsx_trace(chip);
+			return STATUS_FAIL;
 		}
 	}
 
-	RTSX_WRITE_REG(chip, SD_CFG3, SD_RSP_80CLK_TIMEOUT_EN, 0);
+	retval = rtsx_write_register(chip, SD_CFG3, SD_RSP_80CLK_TIMEOUT_EN,
+				     0);
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
 
 	return STATUS_SUCCESS;
 }
@@ -1475,8 +1737,10 @@ static int sd_ddr_tuning_tx_cmd(struct rtsx_chip *chip, u8 sample_point)
 	u8 cmd[5], bus_width;
 
 	retval = sd_change_phase(chip, sample_point, TUNE_TX);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	if (CHK_SD(sd_card)) {
 		bus_width = SD_BUS_WIDTH_4;
@@ -1490,11 +1754,17 @@ static int sd_ddr_tuning_tx_cmd(struct rtsx_chip *chip, u8 sample_point)
 	}
 
 	retval = sd_wait_state_data_ready(chip, 0x08, 1, 1000);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
-	RTSX_WRITE_REG(chip, SD_CFG3, SD_RSP_80CLK_TIMEOUT_EN,
-		SD_RSP_80CLK_TIMEOUT_EN);
+	retval = rtsx_write_register(chip, SD_CFG3, SD_RSP_80CLK_TIMEOUT_EN,
+				     SD_RSP_80CLK_TIMEOUT_EN);
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
 
 	cmd[0] = 0x40 | PROGRAM_CSD;
 	cmd[1] = 0;
@@ -1507,10 +1777,16 @@ static int sd_ddr_tuning_tx_cmd(struct rtsx_chip *chip, u8 sample_point)
 	if (retval != STATUS_SUCCESS) {
 		rtsx_clear_sd_error(chip);
 		rtsx_write_register(chip, SD_CFG3, SD_RSP_80CLK_TIMEOUT_EN, 0);
-		TRACE_RET(chip, STATUS_FAIL);
+		rtsx_trace(chip);
+		return STATUS_FAIL;
 	}
 
-	RTSX_WRITE_REG(chip, SD_CFG3, SD_RSP_80CLK_TIMEOUT_EN, 0);
+	retval = rtsx_write_register(chip, SD_CFG3, SD_RSP_80CLK_TIMEOUT_EN,
+				     0);
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
 
 	sd_send_cmd_get_rsp(chip, SEND_STATUS, sd_card->sd_addr, SD_RSP_TYPE_R1,
 			NULL, 0);
@@ -1524,7 +1800,8 @@ static u8 sd_search_final_phase(struct rtsx_chip *chip, u32 phase_map,
 	struct sd_info *sd_card = &(chip->sd_card);
 	struct timing_phase_path path[MAX_PHASE + 1];
 	int i, j, cont_path_cnt;
-	int new_block, max_len, final_path_idx;
+	bool new_block;
+	int max_len, final_path_idx;
 	u8 final_phase = 0xFF;
 
 	if (phase_map == 0xFFFFFFFF) {
@@ -1537,12 +1814,12 @@ static u8 sd_search_final_phase(struct rtsx_chip *chip, u32 phase_map,
 	}
 
 	cont_path_cnt = 0;
-	new_block = 1;
+	new_block = true;
 	j = 0;
 	for (i = 0; i < MAX_PHASE + 1; i++) {
 		if (phase_map & (1 << i)) {
 			if (new_block) {
-				new_block = 0;
+				new_block = false;
 				j = cont_path_cnt++;
 				path[j].start = i;
 				path[j].end = i;
@@ -1550,7 +1827,7 @@ static u8 sd_search_final_phase(struct rtsx_chip *chip, u32 phase_map,
 				path[j].end = i;
 			}
 		} else {
-			new_block = 1;
+			new_block = true;
 			if (cont_path_cnt) {
 				int idx = cont_path_cnt - 1;
 
@@ -1654,8 +1931,10 @@ static int sd_tuning_rx(struct rtsx_chip *chip)
 	} else {
 		if (CHK_MMC_DDR52(sd_card))
 			tuning_cmd = mmc_ddr_tunning_rx_cmd;
-		else
-			TRACE_RET(chip, STATUS_FAIL);
+		else {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 	}
 
 	for (i = 0; i < 3; i++) {
@@ -1663,7 +1942,8 @@ static int sd_tuning_rx(struct rtsx_chip *chip)
 		for (j = MAX_PHASE; j >= 0; j--) {
 			if (detect_card_cd(chip, SD_CARD) != STATUS_SUCCESS) {
 				sd_set_err_code(chip, SD_NO_CARD);
-				TRACE_RET(chip, STATUS_FAIL);
+				rtsx_trace(chip);
+				return STATUS_FAIL;
 			}
 
 			retval = tuning_cmd(chip, (u8)j);
@@ -1680,12 +1960,16 @@ static int sd_tuning_rx(struct rtsx_chip *chip)
 	dev_dbg(rtsx_dev(chip), "RX phase_map = 0x%08x\n", phase_map);
 
 	final_phase = sd_search_final_phase(chip, phase_map, TUNE_RX);
-	if (final_phase == 0xFF)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (final_phase == 0xFF) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	retval = sd_change_phase(chip, final_phase, TUNE_RX);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	return STATUS_SUCCESS;
 }
@@ -1698,8 +1982,12 @@ static int sd_ddr_pre_tuning_tx(struct rtsx_chip *chip)
 	u32 phase_map;
 	u8 final_phase;
 
-	RTSX_WRITE_REG(chip, SD_CFG3, SD_RSP_80CLK_TIMEOUT_EN,
-		SD_RSP_80CLK_TIMEOUT_EN);
+	retval = rtsx_write_register(chip, SD_CFG3, SD_RSP_80CLK_TIMEOUT_EN,
+				     SD_RSP_80CLK_TIMEOUT_EN);
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
 
 	phase_map = 0;
 	for (i = MAX_PHASE; i >= 0; i--) {
@@ -1707,7 +1995,8 @@ static int sd_ddr_pre_tuning_tx(struct rtsx_chip *chip)
 			sd_set_err_code(chip, SD_NO_CARD);
 			rtsx_write_register(chip, SD_CFG3,
 						SD_RSP_80CLK_TIMEOUT_EN, 0);
-			TRACE_RET(chip, STATUS_FAIL);
+			rtsx_trace(chip);
+			return STATUS_FAIL;
 		}
 
 		retval = sd_change_phase(chip, (u8)i, TUNE_TX);
@@ -1722,18 +2011,27 @@ static int sd_ddr_pre_tuning_tx(struct rtsx_chip *chip)
 			phase_map |= 1 << i;
 	}
 
-	RTSX_WRITE_REG(chip, SD_CFG3, SD_RSP_80CLK_TIMEOUT_EN, 0);
+	retval = rtsx_write_register(chip, SD_CFG3, SD_RSP_80CLK_TIMEOUT_EN,
+				     0);
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
 
 	dev_dbg(rtsx_dev(chip), "DDR TX pre tune phase_map = 0x%08x\n",
 		phase_map);
 
 	final_phase = sd_search_final_phase(chip, phase_map, TUNE_TX);
-	if (final_phase == 0xFF)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (final_phase == 0xFF) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	retval = sd_change_phase(chip, final_phase, TUNE_TX);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	dev_dbg(rtsx_dev(chip), "DDR TX pre tune phase: %d\n",
 		(int)final_phase);
@@ -1759,8 +2057,10 @@ static int sd_tuning_tx(struct rtsx_chip *chip)
 	} else {
 		if (CHK_MMC_DDR52(sd_card))
 			tuning_cmd = sd_ddr_tuning_tx_cmd;
-		else
-			TRACE_RET(chip, STATUS_FAIL);
+		else {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 	}
 
 	for (i = 0; i < 3; i++) {
@@ -1770,7 +2070,8 @@ static int sd_tuning_tx(struct rtsx_chip *chip)
 				sd_set_err_code(chip, SD_NO_CARD);
 				rtsx_write_register(chip, SD_CFG3,
 						    SD_RSP_80CLK_TIMEOUT_EN, 0);
-				TRACE_RET(chip, STATUS_FAIL);
+				rtsx_trace(chip);
+				return STATUS_FAIL;
 			}
 
 			retval = tuning_cmd(chip, (u8)j);
@@ -1787,12 +2088,16 @@ static int sd_tuning_tx(struct rtsx_chip *chip)
 	dev_dbg(rtsx_dev(chip), "TX phase_map = 0x%08x\n", phase_map);
 
 	final_phase = sd_search_final_phase(chip, phase_map, TUNE_TX);
-	if (final_phase == 0xFF)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (final_phase == 0xFF) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	retval = sd_change_phase(chip, final_phase, TUNE_TX);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	return STATUS_SUCCESS;
 }
@@ -1802,12 +2107,16 @@ static int sd_sdr_tuning(struct rtsx_chip *chip)
 	int retval;
 
 	retval = sd_tuning_tx(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	retval = sd_tuning_rx(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	return STATUS_SUCCESS;
 }
@@ -1818,23 +2127,31 @@ static int sd_ddr_tuning(struct rtsx_chip *chip)
 
 	if (!(chip->sd_ctl & SD_DDR_TX_PHASE_SET_BY_USER)) {
 		retval = sd_ddr_pre_tuning_tx(chip);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, STATUS_FAIL);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 	} else {
 		retval = sd_change_phase(chip, (u8)chip->sd_ddr_tx_phase,
 					TUNE_TX);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, STATUS_FAIL);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 	}
 
 	retval = sd_tuning_rx(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	if (!(chip->sd_ctl & SD_DDR_TX_PHASE_SET_BY_USER)) {
 		retval = sd_tuning_tx(chip);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, STATUS_FAIL);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 	}
 
 	return STATUS_SUCCESS;
@@ -1846,23 +2163,31 @@ static int mmc_ddr_tuning(struct rtsx_chip *chip)
 
 	if (!(chip->sd_ctl & MMC_DDR_TX_PHASE_SET_BY_USER)) {
 		retval = sd_ddr_pre_tuning_tx(chip);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, STATUS_FAIL);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 	} else {
 		retval = sd_change_phase(chip, (u8)chip->mmc_ddr_tx_phase,
 					TUNE_TX);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, STATUS_FAIL);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 	}
 
 	retval = sd_tuning_rx(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	if (!(chip->sd_ctl & MMC_DDR_TX_PHASE_SET_BY_USER)) {
 		retval = sd_tuning_tx(chip);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, STATUS_FAIL);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 	}
 
 	return STATUS_SUCCESS;
@@ -1875,12 +2200,16 @@ int sd_switch_clock(struct rtsx_chip *chip)
 	int re_tuning = 0;
 
 	retval = select_card(chip, SD_CARD);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	retval = switch_clock(chip, sd_card->sd_clock);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	if (re_tuning) {
 		if (CHK_SD(sd_card)) {
@@ -1893,8 +2222,10 @@ int sd_switch_clock(struct rtsx_chip *chip)
 				retval = mmc_ddr_tuning(chip);
 		}
 
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, STATUS_FAIL);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 	}
 
 	return STATUS_SUCCESS;
@@ -1924,41 +2255,100 @@ static int sd_prepare_reset(struct rtsx_chip *chip)
 	chip->sd_io = 0;
 
 	retval = sd_set_init_para(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return retval;
+	}
 
-	RTSX_WRITE_REG(chip, REG_SD_CFG1, 0xFF, 0x40);
+	retval = rtsx_write_register(chip, REG_SD_CFG1, 0xFF, 0x40);
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
 
-	RTSX_WRITE_REG(chip, CARD_STOP, SD_STOP | SD_CLR_ERR,
-		SD_STOP | SD_CLR_ERR);
+	retval = rtsx_write_register(chip, CARD_STOP, SD_STOP | SD_CLR_ERR,
+				     SD_STOP | SD_CLR_ERR);
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
 
 	retval = select_card(chip, SD_CARD);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	return STATUS_SUCCESS;
 }
 
 static int sd_pull_ctl_disable(struct rtsx_chip *chip)
 {
+	int retval;
+
 	if (CHECK_PID(chip, 0x5208)) {
-		RTSX_WRITE_REG(chip, CARD_PULL_CTL1, 0xFF,
-			XD_D3_PD | SD_D7_PD | SD_CLK_PD | SD_D5_PD);
-		RTSX_WRITE_REG(chip, CARD_PULL_CTL2, 0xFF,
-			SD_D6_PD | SD_D0_PD | SD_D1_PD | XD_D5_PD);
-		RTSX_WRITE_REG(chip, CARD_PULL_CTL3, 0xFF,
-			SD_D4_PD | XD_CE_PD | XD_CLE_PD | XD_CD_PU);
-		RTSX_WRITE_REG(chip, CARD_PULL_CTL4, 0xFF,
-			XD_RDY_PD | SD_D3_PD | SD_D2_PD | XD_ALE_PD);
-		RTSX_WRITE_REG(chip, CARD_PULL_CTL5, 0xFF,
-			MS_INS_PU | SD_WP_PD | SD_CD_PU | SD_CMD_PD);
-		RTSX_WRITE_REG(chip, CARD_PULL_CTL6, 0xFF, MS_D5_PD | MS_D4_PD);
+		retval = rtsx_write_register(chip, CARD_PULL_CTL1, 0xFF,
+					     XD_D3_PD | SD_D7_PD | SD_CLK_PD | SD_D5_PD);
+		if (retval) {
+			rtsx_trace(chip);
+			return retval;
+		}
+		retval = rtsx_write_register(chip, CARD_PULL_CTL2, 0xFF,
+					     SD_D6_PD | SD_D0_PD | SD_D1_PD | XD_D5_PD);
+		if (retval) {
+			rtsx_trace(chip);
+			return retval;
+		}
+		retval = rtsx_write_register(chip, CARD_PULL_CTL3, 0xFF,
+					     SD_D4_PD | XD_CE_PD | XD_CLE_PD | XD_CD_PU);
+		if (retval) {
+			rtsx_trace(chip);
+			return retval;
+		}
+		retval = rtsx_write_register(chip, CARD_PULL_CTL4, 0xFF,
+					     XD_RDY_PD | SD_D3_PD | SD_D2_PD | XD_ALE_PD);
+		if (retval) {
+			rtsx_trace(chip);
+			return retval;
+		}
+		retval = rtsx_write_register(chip, CARD_PULL_CTL5, 0xFF,
+					     MS_INS_PU | SD_WP_PD | SD_CD_PU | SD_CMD_PD);
+		if (retval) {
+			rtsx_trace(chip);
+			return retval;
+		}
+		retval = rtsx_write_register(chip, CARD_PULL_CTL6, 0xFF,
+					     MS_D5_PD | MS_D4_PD);
+		if (retval) {
+			rtsx_trace(chip);
+			return retval;
+		}
 	} else if (CHECK_PID(chip, 0x5288)) {
 		if (CHECK_BARO_PKG(chip, QFN)) {
-			RTSX_WRITE_REG(chip, CARD_PULL_CTL1, 0xFF, 0x55);
-			RTSX_WRITE_REG(chip, CARD_PULL_CTL2, 0xFF, 0x55);
-			RTSX_WRITE_REG(chip, CARD_PULL_CTL3, 0xFF, 0x4B);
-			RTSX_WRITE_REG(chip, CARD_PULL_CTL4, 0xFF, 0x69);
+			retval = rtsx_write_register(chip, CARD_PULL_CTL1,
+						     0xFF, 0x55);
+			if (retval) {
+				rtsx_trace(chip);
+				return retval;
+			}
+			retval = rtsx_write_register(chip, CARD_PULL_CTL2,
+						     0xFF, 0x55);
+			if (retval) {
+				rtsx_trace(chip);
+				return retval;
+			}
+			retval = rtsx_write_register(chip, CARD_PULL_CTL3,
+						     0xFF, 0x4B);
+			if (retval) {
+				rtsx_trace(chip);
+				return retval;
+			}
+			retval = rtsx_write_register(chip, CARD_PULL_CTL4,
+						     0xFF, 0x69);
+			if (retval) {
+				rtsx_trace(chip);
+				return retval;
+			}
 		}
 	}
 
@@ -1998,8 +2388,10 @@ int sd_pull_ctl_enable(struct rtsx_chip *chip)
 	}
 
 	retval = rtsx_send_cmd(chip, SD_CARD, 100);
-	if (retval < 0)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval < 0) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	return STATUS_SUCCESS;
 }
@@ -2009,29 +2401,41 @@ static int sd_init_power(struct rtsx_chip *chip)
 	int retval;
 
 	retval = sd_power_off_card3v3(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	if (!chip->ft2_fast_mode)
 		wait_timeout(250);
 
 	retval = enable_card_clock(chip, SD_CARD);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	if (chip->asic_code) {
 		retval = sd_pull_ctl_enable(chip);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, STATUS_FAIL);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 	} else {
-		RTSX_WRITE_REG(chip, FPGA_PULL_CTL, FPGA_SD_PULL_CTL_BIT | 0x20,
-			0);
+		retval = rtsx_write_register(chip, FPGA_PULL_CTL,
+					     FPGA_SD_PULL_CTL_BIT | 0x20, 0);
+		if (retval) {
+			rtsx_trace(chip);
+			return retval;
+		}
 	}
 
 	if (!chip->ft2_fast_mode) {
 		retval = card_power_on(chip, SD_CARD);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, STATUS_FAIL);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 
 		wait_timeout(260);
 
@@ -2039,21 +2443,37 @@ static int sd_init_power(struct rtsx_chip *chip)
 		if (chip->ocp_stat & (SD_OC_NOW | SD_OC_EVER)) {
 			dev_dbg(rtsx_dev(chip), "Over current, OCPSTAT is 0x%x\n",
 				chip->ocp_stat);
-			TRACE_RET(chip, STATUS_FAIL);
+			rtsx_trace(chip);
+			return STATUS_FAIL;
 		}
 #endif
 	}
 
-	RTSX_WRITE_REG(chip, CARD_OE, SD_OUTPUT_EN, SD_OUTPUT_EN);
+	retval = rtsx_write_register(chip, CARD_OE, SD_OUTPUT_EN,
+				     SD_OUTPUT_EN);
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
 
 	return STATUS_SUCCESS;
 }
 
 static int sd_dummy_clock(struct rtsx_chip *chip)
 {
-	RTSX_WRITE_REG(chip, REG_SD_CFG3, 0x01, 0x01);
+	int retval;
+
+	retval = rtsx_write_register(chip, REG_SD_CFG3, 0x01, 0x01);
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
 	wait_timeout(5);
-	RTSX_WRITE_REG(chip, REG_SD_CFG3, 0x01, 0);
+	retval = rtsx_write_register(chip, REG_SD_CFG3, 0x01, 0);
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
 
 	return STATUS_SUCCESS;
 }
@@ -2085,7 +2505,8 @@ static int sd_read_lba0(struct rtsx_chip *chip)
 		5, 512, 1, bus_width, NULL, 0, 100);
 	if (retval != STATUS_SUCCESS) {
 		rtsx_clear_sd_error(chip);
-		TRACE_RET(chip, STATUS_FAIL);
+		rtsx_trace(chip);
+		return STATUS_FAIL;
 	}
 
 	return STATUS_SUCCESS;
@@ -2101,8 +2522,10 @@ static int sd_check_wp_state(struct rtsx_chip *chip)
 
 	retval = sd_send_cmd_get_rsp(chip, APP_CMD,
 			sd_card->sd_addr, SD_RSP_TYPE_R1, NULL, 0);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	cmd[0] = 0x40 | SD_STATUS;
 	cmd[1] = 0;
@@ -2117,7 +2540,8 @@ static int sd_check_wp_state(struct rtsx_chip *chip)
 
 		sd_send_cmd_get_rsp(chip, SEND_STATUS, sd_card->sd_addr,
 				SD_RSP_TYPE_R1, NULL, 0);
-		TRACE_RET(chip, STATUS_FAIL);
+		rtsx_trace(chip);
+		return STATUS_FAIL;
 	}
 
 	dev_dbg(rtsx_dev(chip), "ACMD13:\n");
@@ -2141,14 +2565,15 @@ static int sd_check_wp_state(struct rtsx_chip *chip)
 static int reset_sd(struct rtsx_chip *chip)
 {
 	struct sd_info *sd_card = &(chip->sd_card);
-	int retval, i = 0, j = 0, k = 0, hi_cap_flow = 0;
-	int sd_dont_switch = 0;
-	int support_1v8 = 0;
-	int try_sdio = 1;
+	bool hi_cap_flow = false;
+	int retval, i = 0, j = 0, k = 0;
+	bool sd_dont_switch = false;
+	bool support_1v8 = false;
+	bool try_sdio = true;
 	u8 rsp[16];
 	u8 switch_bus_width;
 	u32 voltage = 0;
-	int sd20_mode = 0;
+	bool sd20_mode = false;
 
 	SET_SD(sd_card);
 
@@ -2157,7 +2582,7 @@ Switch_Fail:
 	i = 0;
 	j = 0;
 	k = 0;
-	hi_cap_flow = 0;
+	hi_cap_flow = false;
 
 #ifdef SUPPORT_SD_LOCK
 	if (sd_card->sd_lock_status & SD_UNLOCK_POW_ON)
@@ -2165,12 +2590,16 @@ Switch_Fail:
 #endif
 
 	retval = sd_prepare_reset(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	retval = sd_dummy_clock(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	if (CHK_SDIO_EXIST(chip) && !CHK_SDIO_IGNORED(chip) && try_sdio) {
 		int rty_cnt = 0;
@@ -2178,7 +2607,8 @@ Switch_Fail:
 		for (; rty_cnt < chip->sdio_retry_cnt; rty_cnt++) {
 			if (detect_card_cd(chip, SD_CARD) != STATUS_SUCCESS) {
 				sd_set_err_code(chip, SD_NO_CARD);
-				TRACE_RET(chip, STATUS_FAIL);
+				rtsx_trace(chip);
+				return STATUS_FAIL;
 			}
 
 			retval = sd_send_cmd_get_rsp(chip, IO_SEND_OP_COND, 0,
@@ -2190,7 +2620,8 @@ Switch_Fail:
 					dev_dbg(rtsx_dev(chip), "SD_IO card (Function number: %d)!\n",
 						func_num);
 					chip->sd_io = 1;
-					TRACE_RET(chip, STATUS_FAIL);
+					rtsx_trace(chip);
+					return STATUS_FAIL;
 				}
 
 				break;
@@ -2208,8 +2639,10 @@ Switch_Fail:
 RTY_SD_RST:
 	retval = sd_send_cmd_get_rsp(chip, GO_IDLE_STATE, 0, SD_RSP_TYPE_R0,
 				NULL, 0);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	wait_timeout(20);
 
@@ -2217,7 +2650,7 @@ RTY_SD_RST:
 				SD_RSP_TYPE_R7, rsp, 5);
 	if (retval == STATUS_SUCCESS) {
 		if ((rsp[4] == 0xAA) && ((rsp[3] & 0x0f) == 0x01)) {
-			hi_cap_flow = 1;
+			hi_cap_flow = true;
 			voltage = SUPPORT_VOLTAGE | 0x40000000;
 		}
 	}
@@ -2227,8 +2660,10 @@ RTY_SD_RST:
 
 		retval = sd_send_cmd_get_rsp(chip, GO_IDLE_STATE, 0,
 					SD_RSP_TYPE_R0, NULL, 0);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, STATUS_FAIL);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 
 		wait_timeout(20);
 	}
@@ -2239,14 +2674,17 @@ RTY_SD_RST:
 		if (retval != STATUS_SUCCESS) {
 			if (detect_card_cd(chip, SD_CARD) != STATUS_SUCCESS) {
 				sd_set_err_code(chip, SD_NO_CARD);
-				TRACE_RET(chip, STATUS_FAIL);
+				rtsx_trace(chip);
+				return STATUS_FAIL;
 			}
 
 			j++;
 			if (j < 3)
 				goto RTY_SD_RST;
-			else
-				TRACE_RET(chip, STATUS_FAIL);
+			else {
+				rtsx_trace(chip);
+				return STATUS_FAIL;
+			}
 		}
 
 		retval = sd_send_cmd_get_rsp(chip, SD_APP_OP_COND, voltage,
@@ -2255,16 +2693,20 @@ RTY_SD_RST:
 			k++;
 			if (k < 3)
 				goto RTY_SD_RST;
-			else
-				TRACE_RET(chip, STATUS_FAIL);
+			else {
+				rtsx_trace(chip);
+				return STATUS_FAIL;
+			}
 		}
 
 		i++;
 		wait_timeout(20);
 	} while (!(rsp[1] & 0x80) && (i < 255));
 
-	if (i == 255)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (i == 255) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	if (hi_cap_flow) {
 		if (rsp[1] & 0x40)
@@ -2272,29 +2714,35 @@ RTY_SD_RST:
 		else
 			CLR_SD_HCXC(sd_card);
 
-		support_1v8 = 0;
+		support_1v8 = false;
 	} else {
 		CLR_SD_HCXC(sd_card);
-		support_1v8 = 0;
+		support_1v8 = false;
 	}
 	dev_dbg(rtsx_dev(chip), "support_1v8 = %d\n", support_1v8);
 
 	if (support_1v8) {
 		retval = sd_voltage_switch(chip);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, STATUS_FAIL);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 	}
 
 	retval = sd_send_cmd_get_rsp(chip, ALL_SEND_CID, 0, SD_RSP_TYPE_R2,
 				NULL, 0);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	for (i = 0; i < 3; i++) {
 		retval = sd_send_cmd_get_rsp(chip, SEND_RELATIVE_ADDR, 0,
 					SD_RSP_TYPE_R6, rsp, 5);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, STATUS_FAIL);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 
 		sd_card->sd_addr = (u32)rsp[1] << 24;
 		sd_card->sd_addr += (u32)rsp[2] << 16;
@@ -2304,18 +2752,24 @@ RTY_SD_RST:
 	}
 
 	retval = sd_check_csd(chip, 1);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	retval = sd_select_card(chip, 1);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 #ifdef SUPPORT_SD_LOCK
 SD_UNLOCK_ENTRY:
 	retval = sd_update_lock_status(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	if (sd_card->sd_lock_status & SD_LOCKED) {
 		sd_card->sd_lock_status |= (SD_LOCK_1BIT_MODE | SD_PWD_EXIST);
@@ -2327,24 +2781,32 @@ SD_UNLOCK_ENTRY:
 
 	retval = sd_send_cmd_get_rsp(chip, APP_CMD, sd_card->sd_addr,
 				SD_RSP_TYPE_R1, NULL, 0);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	retval = sd_send_cmd_get_rsp(chip, SET_CLR_CARD_DETECT, 0,
 				SD_RSP_TYPE_R1, NULL, 0);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	if (support_1v8) {
 		retval = sd_send_cmd_get_rsp(chip, APP_CMD, sd_card->sd_addr,
 					SD_RSP_TYPE_R1, NULL, 0);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, STATUS_FAIL);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 
 		retval = sd_send_cmd_get_rsp(chip, SET_BUS_WIDTH, 2,
 					SD_RSP_TYPE_R1, NULL, 0);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, STATUS_FAIL);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 
 		switch_bus_width = SD_BUS_WIDTH_4;
 	} else {
@@ -2353,15 +2815,19 @@ SD_UNLOCK_ENTRY:
 
 	retval = sd_send_cmd_get_rsp(chip, SET_BLOCKLEN, 0x200, SD_RSP_TYPE_R1,
 				NULL, 0);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	retval = sd_set_clock_divider(chip, SD_CLK_DIVIDE_0);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	if (!(sd_card->raw_csd[4] & 0x40))
-		sd_dont_switch = 1;
+		sd_dont_switch = true;
 
 	if (!sd_dont_switch) {
 		if (sd20_mode) {
@@ -2378,16 +2844,16 @@ SD_UNLOCK_ENTRY:
 			retval = sd_switch_function(chip, switch_bus_width);
 			if (retval != STATUS_SUCCESS) {
 				sd_init_power(chip);
-				sd_dont_switch = 1;
-				try_sdio = 0;
+				sd_dont_switch = true;
+				try_sdio = false;
 
 				goto Switch_Fail;
 			}
 		} else {
 			if (support_1v8) {
 				sd_init_power(chip);
-				sd_dont_switch = 1;
-				try_sdio = 0;
+				sd_dont_switch = true;
+				try_sdio = false;
 
 				goto Switch_Fail;
 			}
@@ -2397,13 +2863,17 @@ SD_UNLOCK_ENTRY:
 	if (!support_1v8) {
 		retval = sd_send_cmd_get_rsp(chip, APP_CMD, sd_card->sd_addr,
 					SD_RSP_TYPE_R1, NULL, 0);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, STATUS_FAIL);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 
 		retval = sd_send_cmd_get_rsp(chip, SET_BUS_WIDTH, 2,
 					SD_RSP_TYPE_R1, NULL, 0);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, STATUS_FAIL);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 	}
 
 #ifdef SUPPORT_SD_LOCK
@@ -2413,12 +2883,18 @@ SD_UNLOCK_ENTRY:
 	if (!sd20_mode && CHK_SD30_SPEED(sd_card)) {
 		int read_lba0 = 1;
 
-		RTSX_WRITE_REG(chip, SD30_DRIVE_SEL, 0x07,
-			chip->sd30_drive_sel_1v8);
+		retval = rtsx_write_register(chip, SD30_DRIVE_SEL, 0x07,
+					     chip->sd30_drive_sel_1v8);
+		if (retval) {
+			rtsx_trace(chip);
+			return retval;
+		}
 
 		retval = sd_set_init_para(chip);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, STATUS_FAIL);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 
 		if (CHK_SD_DDR50(sd_card))
 			retval = sd_ddr_tuning(chip);
@@ -2427,14 +2903,17 @@ SD_UNLOCK_ENTRY:
 
 		if (retval != STATUS_SUCCESS) {
 			if (sd20_mode) {
-				TRACE_RET(chip, STATUS_FAIL);
+				rtsx_trace(chip);
+				return STATUS_FAIL;
 			} else {
 				retval = sd_init_power(chip);
-				if (retval != STATUS_SUCCESS)
-					TRACE_RET(chip, STATUS_FAIL);
+				if (retval != STATUS_SUCCESS) {
+					rtsx_trace(chip);
+					return STATUS_FAIL;
+				}
 
-				try_sdio = 0;
-				sd20_mode = 1;
+				try_sdio = false;
+				sd20_mode = true;
 				goto Switch_Fail;
 			}
 		}
@@ -2452,14 +2931,17 @@ SD_UNLOCK_ENTRY:
 			retval = sd_read_lba0(chip);
 			if (retval != STATUS_SUCCESS) {
 				if (sd20_mode) {
-					TRACE_RET(chip, STATUS_FAIL);
+					rtsx_trace(chip);
+					return STATUS_FAIL;
 				} else {
 					retval = sd_init_power(chip);
-					if (retval != STATUS_SUCCESS)
-						TRACE_RET(chip, STATUS_FAIL);
+					if (retval != STATUS_SUCCESS) {
+						rtsx_trace(chip);
+						return STATUS_FAIL;
+					}
 
-					try_sdio = 0;
-					sd20_mode = 1;
+					try_sdio = false;
+					sd20_mode = true;
 					goto Switch_Fail;
 				}
 			}
@@ -2467,15 +2949,27 @@ SD_UNLOCK_ENTRY:
 	}
 
 	retval = sd_check_wp_state(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	chip->card_bus_width[chip->card2lun[SD_CARD]] = 4;
 
 #ifdef SUPPORT_SD_LOCK
 	if (sd_card->sd_lock_status & SD_UNLOCK_POW_ON) {
-		RTSX_WRITE_REG(chip, REG_SD_BLOCK_CNT_H, 0xFF, 0x02);
-		RTSX_WRITE_REG(chip, REG_SD_BLOCK_CNT_L, 0xFF, 0x00);
+		retval = rtsx_write_register(chip, REG_SD_BLOCK_CNT_H, 0xFF,
+					     0x02);
+		if (retval) {
+			rtsx_trace(chip);
+			return retval;
+		}
+		retval = rtsx_write_register(chip, REG_SD_BLOCK_CNT_L, 0xFF,
+					     0x00);
+		if (retval) {
+			rtsx_trace(chip);
+			return retval;
+		}
 	}
 #endif
 
@@ -2493,8 +2987,10 @@ static int mmc_test_switch_bus(struct rtsx_chip *chip, u8 width)
 
 	retval = sd_send_cmd_get_rsp(chip, BUSTEST_W, 0, SD_RSP_TYPE_R1, NULL,
 				0);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, SWITCH_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return SWITCH_FAIL;
+	}
 
 	if (width == MMC_8BIT_BUS) {
 		buf[0] = 0x55;
@@ -2510,20 +3006,25 @@ static int mmc_test_switch_bus(struct rtsx_chip *chip, u8 width)
 	}
 
 	retval = rtsx_write_register(chip, REG_SD_CFG3, 0x02, 0x02);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, SWITCH_ERR);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return SWITCH_ERR;
+	}
 
 	retval = sd_write_data(chip, SD_TM_AUTO_WRITE_3,
 			NULL, 0, byte_cnt, 1, bus_width, buf, len, 100);
 	if (retval != STATUS_SUCCESS) {
 		rtsx_clear_sd_error(chip);
 		rtsx_write_register(chip, REG_SD_CFG3, 0x02, 0);
-		TRACE_RET(chip, SWITCH_ERR);
+		rtsx_trace(chip);
+		return SWITCH_ERR;
 	}
 
 	retval = rtsx_write_register(chip, REG_SD_CFG3, 0x02, 0);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, SWITCH_ERR);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return SWITCH_ERR;
+	}
 
 	dev_dbg(rtsx_dev(chip), "SD/MMC CMD %d\n", BUSTEST_R);
 
@@ -2558,7 +3059,8 @@ static int mmc_test_switch_bus(struct rtsx_chip *chip, u8 width)
 	retval = rtsx_send_cmd(chip, SD_CARD, 100);
 	if (retval < 0) {
 		rtsx_clear_sd_error(chip);
-		TRACE_RET(chip, SWITCH_ERR);
+		rtsx_trace(chip);
+		return SWITCH_ERR;
 	}
 
 	ptr = rtsx_get_cmd_data(chip) + 1;
@@ -2600,11 +3102,12 @@ static int mmc_test_switch_bus(struct rtsx_chip *chip, u8 width)
 		}
 	}
 
-	TRACE_RET(chip, SWITCH_FAIL);
+	rtsx_trace(chip);
+	return SWITCH_FAIL;
 }
 
 
-static int mmc_switch_timing_bus(struct rtsx_chip *chip, int switch_ddr)
+static int mmc_switch_timing_bus(struct rtsx_chip *chip, bool switch_ddr)
 {
 	struct sd_info *sd_card = &(chip->sd_card);
 	int retval;
@@ -2651,14 +3154,16 @@ static int mmc_switch_timing_bus(struct rtsx_chip *chip, int switch_ddr)
 			sd_send_cmd_get_rsp(chip, SEND_STATUS, sd_card->sd_addr,
 					SD_RSP_TYPE_R1, NULL, 0);
 		}
-		TRACE_RET(chip, STATUS_FAIL);
+		rtsx_trace(chip);
+		return STATUS_FAIL;
 	}
 
 	ptr = rtsx_get_cmd_data(chip);
 	if (ptr[0] & SD_TRANSFER_ERR) {
 		sd_send_cmd_get_rsp(chip, SEND_STATUS, sd_card->sd_addr,
 				SD_RSP_TYPE_R1, NULL, 0);
-		TRACE_RET(chip, STATUS_FAIL);
+		rtsx_trace(chip);
+		return STATUS_FAIL;
 	}
 
 	if (CHK_MMC_SECTOR_MODE(sd_card)) {
@@ -2690,8 +3195,10 @@ static int mmc_switch_timing_bus(struct rtsx_chip *chip, int switch_ddr)
 
 	sd_choose_proper_clock(chip);
 	retval = switch_clock(chip, sd_card->sd_clock);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	/* Test Bus Procedure */
 	retval = mmc_test_switch_bus(chip, MMC_8BIT_BUS);
@@ -2713,10 +3220,12 @@ static int mmc_switch_timing_bus(struct rtsx_chip *chip, int switch_ddr)
 			CLR_MMC_8BIT(sd_card);
 			CLR_MMC_4BIT(sd_card);
 		} else {
-			TRACE_RET(chip, STATUS_FAIL);
+			rtsx_trace(chip);
+			return STATUS_FAIL;
 		}
 	} else {
-		TRACE_RET(chip, STATUS_FAIL);
+		rtsx_trace(chip);
+		return STATUS_FAIL;
 	}
 
 	return STATUS_SUCCESS;
@@ -2727,7 +3236,7 @@ static int reset_mmc(struct rtsx_chip *chip)
 {
 	struct sd_info *sd_card = &(chip->sd_card);
 	int retval, i = 0, j = 0, k = 0;
-	int switch_ddr = 1;
+	bool switch_ddr = true;
 	u8 rsp[16];
 	u8 spec_ver = 0;
 	u32 temp;
@@ -2739,21 +3248,26 @@ static int reset_mmc(struct rtsx_chip *chip)
 
 Switch_Fail:
 	retval = sd_prepare_reset(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return retval;
+	}
 
 	SET_MMC(sd_card);
 
 RTY_MMC_RST:
 	retval = sd_send_cmd_get_rsp(chip, GO_IDLE_STATE, 0, SD_RSP_TYPE_R0,
 				NULL, 0);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	do {
 		if (detect_card_cd(chip, SD_CARD) != STATUS_SUCCESS) {
 			sd_set_err_code(chip, SD_NO_CARD);
-			TRACE_RET(chip, STATUS_FAIL);
+			rtsx_trace(chip);
+			return STATUS_FAIL;
 		}
 
 		retval = sd_send_cmd_get_rsp(chip, SEND_OP_COND,
@@ -2767,7 +3281,8 @@ RTY_MMC_RST:
 					sd_clr_err_code(chip);
 					goto RTY_MMC_RST;
 				} else {
-					TRACE_RET(chip, STATUS_FAIL);
+					rtsx_trace(chip);
+					return STATUS_FAIL;
 				}
 			} else {
 				j++;
@@ -2775,7 +3290,8 @@ RTY_MMC_RST:
 					sd_clr_err_code(chip);
 					goto RTY_MMC_RST;
 				} else {
-					TRACE_RET(chip, STATUS_FAIL);
+					rtsx_trace(chip);
+					return STATUS_FAIL;
 				}
 			}
 		}
@@ -2784,8 +3300,10 @@ RTY_MMC_RST:
 		i++;
 	} while (!(rsp[1] & 0x80) && (i < 255));
 
-	if (i == 255)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (i == 255) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	if ((rsp[1] & 0x60) == 0x40)
 		SET_MMC_SECTOR_MODE(sd_card);
@@ -2794,40 +3312,54 @@ RTY_MMC_RST:
 
 	retval = sd_send_cmd_get_rsp(chip, ALL_SEND_CID, 0, SD_RSP_TYPE_R2,
 				NULL, 0);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	sd_card->sd_addr = 0x00100000;
 	retval = sd_send_cmd_get_rsp(chip, SET_RELATIVE_ADDR, sd_card->sd_addr,
 				SD_RSP_TYPE_R6, rsp, 5);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	retval = sd_check_csd(chip, 1);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	spec_ver = (sd_card->raw_csd[0] & 0x3C) >> 2;
 
 	retval = sd_select_card(chip, 1);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	retval = sd_send_cmd_get_rsp(chip, SET_BLOCKLEN, 0x200, SD_RSP_TYPE_R1,
 				NULL, 0);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 #ifdef SUPPORT_SD_LOCK
 MMC_UNLOCK_ENTRY:
 	retval = sd_update_lock_status(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 #endif
 
 	retval = sd_set_clock_divider(chip, SD_CLK_DIVIDE_0);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	chip->card_bus_width[chip->card2lun[SD_CARD]] = 1;
 
@@ -2837,29 +3369,39 @@ MMC_UNLOCK_ENTRY:
 			retval = mmc_switch_timing_bus(chip, switch_ddr);
 			if (retval != STATUS_SUCCESS) {
 				retval = sd_init_power(chip);
-				if (retval != STATUS_SUCCESS)
-					TRACE_RET(chip, STATUS_FAIL);
+				if (retval != STATUS_SUCCESS) {
+					rtsx_trace(chip);
+					return STATUS_FAIL;
+				}
 				sd_card->mmc_dont_switch_bus = 1;
-				TRACE_GOTO(chip, Switch_Fail);
+				rtsx_trace(chip);
+				goto Switch_Fail;
 			}
 		}
 
-		if (CHK_MMC_SECTOR_MODE(sd_card) && (sd_card->capacity == 0))
-			TRACE_RET(chip, STATUS_FAIL);
+		if (CHK_MMC_SECTOR_MODE(sd_card) && (sd_card->capacity == 0)) {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 
 		if (switch_ddr && CHK_MMC_DDR52(sd_card)) {
 			retval = sd_set_init_para(chip);
-			if (retval != STATUS_SUCCESS)
-				TRACE_RET(chip, STATUS_FAIL);
+			if (retval != STATUS_SUCCESS) {
+				rtsx_trace(chip);
+				return STATUS_FAIL;
+			}
 
 			retval = mmc_ddr_tuning(chip);
 			if (retval != STATUS_SUCCESS) {
 				retval = sd_init_power(chip);
-				if (retval != STATUS_SUCCESS)
-					TRACE_RET(chip, STATUS_FAIL);
+				if (retval != STATUS_SUCCESS) {
+					rtsx_trace(chip);
+					return STATUS_FAIL;
+				}
 
-				switch_ddr = 0;
-				TRACE_GOTO(chip, Switch_Fail);
+				switch_ddr = false;
+				rtsx_trace(chip);
+				goto Switch_Fail;
 			}
 
 			retval = sd_wait_state_data_ready(chip, 0x08, 1, 1000);
@@ -2867,11 +3409,14 @@ MMC_UNLOCK_ENTRY:
 				retval = sd_read_lba0(chip);
 				if (retval != STATUS_SUCCESS) {
 					retval = sd_init_power(chip);
-					if (retval != STATUS_SUCCESS)
-						TRACE_RET(chip, STATUS_FAIL);
+					if (retval != STATUS_SUCCESS) {
+						rtsx_trace(chip);
+						return STATUS_FAIL;
+					}
 
-					switch_ddr = 0;
-					TRACE_GOTO(chip, Switch_Fail);
+					switch_ddr = false;
+					rtsx_trace(chip);
+					goto Switch_Fail;
 				}
 			}
 		}
@@ -2879,8 +3424,18 @@ MMC_UNLOCK_ENTRY:
 
 #ifdef SUPPORT_SD_LOCK
 	if (sd_card->sd_lock_status & SD_UNLOCK_POW_ON) {
-		RTSX_WRITE_REG(chip, REG_SD_BLOCK_CNT_H, 0xFF, 0x02);
-		RTSX_WRITE_REG(chip, REG_SD_BLOCK_CNT_L, 0xFF, 0x00);
+		retval = rtsx_write_register(chip, REG_SD_BLOCK_CNT_H, 0xFF,
+					     0x02);
+		if (retval) {
+			rtsx_trace(chip);
+			return retval;
+		}
+		retval = rtsx_write_register(chip, REG_SD_BLOCK_CNT_L, 0xFF,
+					     0x00);
+		if (retval) {
+			rtsx_trace(chip);
+			return retval;
+		}
 	}
 #endif
 
@@ -2902,71 +3457,103 @@ int reset_sd_card(struct rtsx_chip *chip)
 	chip->capacity[chip->card2lun[SD_CARD]] = 0;
 
 	retval = enable_card_clock(chip, SD_CARD);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	if (chip->ignore_sd && CHK_SDIO_EXIST(chip) &&
 		!CHK_SDIO_IGNORED(chip)) {
 		if (chip->asic_code) {
 			retval = sd_pull_ctl_enable(chip);
-			if (retval != STATUS_SUCCESS)
-				TRACE_RET(chip, STATUS_FAIL);
+			if (retval != STATUS_SUCCESS) {
+				rtsx_trace(chip);
+				return STATUS_FAIL;
+			}
 		} else {
 			retval = rtsx_write_register(chip, FPGA_PULL_CTL,
 						FPGA_SD_PULL_CTL_BIT | 0x20, 0);
-			if (retval != STATUS_SUCCESS)
-				TRACE_RET(chip, STATUS_FAIL);
+			if (retval != STATUS_SUCCESS) {
+				rtsx_trace(chip);
+				return STATUS_FAIL;
+			}
 		}
 		retval = card_share_mode(chip, SD_CARD);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, STATUS_FAIL);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 
 		chip->sd_io = 1;
-		TRACE_RET(chip, STATUS_FAIL);
+		rtsx_trace(chip);
+		return STATUS_FAIL;
 	}
 
 	retval = sd_init_power(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	if (chip->sd_ctl & RESET_MMC_FIRST) {
 		retval = reset_mmc(chip);
 		if (retval != STATUS_SUCCESS) {
-			if (sd_check_err_code(chip, SD_NO_CARD))
-				TRACE_RET(chip, STATUS_FAIL);
+			if (sd_check_err_code(chip, SD_NO_CARD)) {
+				rtsx_trace(chip);
+				return STATUS_FAIL;
+			}
 
 			retval = reset_sd(chip);
-			if (retval != STATUS_SUCCESS)
-				TRACE_RET(chip, STATUS_FAIL);
+			if (retval != STATUS_SUCCESS) {
+				rtsx_trace(chip);
+				return STATUS_FAIL;
+			}
 		}
 	} else {
 		retval = reset_sd(chip);
 		if (retval != STATUS_SUCCESS) {
-			if (sd_check_err_code(chip, SD_NO_CARD))
-				TRACE_RET(chip, STATUS_FAIL);
+			if (sd_check_err_code(chip, SD_NO_CARD)) {
+				rtsx_trace(chip);
+				return STATUS_FAIL;
+			}
 
 			if (chip->sd_io) {
-				TRACE_RET(chip, STATUS_FAIL);
+				rtsx_trace(chip);
+				return STATUS_FAIL;
 			} else {
 				retval = reset_mmc(chip);
-				if (retval != STATUS_SUCCESS)
-					TRACE_RET(chip, STATUS_FAIL);
+				if (retval != STATUS_SUCCESS) {
+					rtsx_trace(chip);
+					return STATUS_FAIL;
+				}
 			}
 		}
 	}
 
 	retval = sd_set_clock_divider(chip, SD_CLK_DIVIDE_0);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
-	RTSX_WRITE_REG(chip, REG_SD_BYTE_CNT_L, 0xFF, 0);
-	RTSX_WRITE_REG(chip, REG_SD_BYTE_CNT_H, 0xFF, 2);
+	retval = rtsx_write_register(chip, REG_SD_BYTE_CNT_L, 0xFF, 0);
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
+	retval = rtsx_write_register(chip, REG_SD_BYTE_CNT_H, 0xFF, 2);
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
 
 	chip->capacity[chip->card2lun[SD_CARD]] = sd_card->capacity;
 
 	retval = sd_set_init_para(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	dev_dbg(rtsx_dev(chip), "sd_card->sd_type = 0x%x\n", sd_card->sd_type);
 
@@ -2992,29 +3579,47 @@ static int reset_mmc_only(struct rtsx_chip *chip)
 	chip->capacity[chip->card2lun[SD_CARD]] = sd_card->capacity = 0;
 
 	retval = enable_card_clock(chip, SD_CARD);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	retval = sd_init_power(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	retval = reset_mmc(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	retval = sd_set_clock_divider(chip, SD_CLK_DIVIDE_0);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
-	RTSX_WRITE_REG(chip, REG_SD_BYTE_CNT_L, 0xFF, 0);
-	RTSX_WRITE_REG(chip, REG_SD_BYTE_CNT_H, 0xFF, 2);
+	retval = rtsx_write_register(chip, REG_SD_BYTE_CNT_L, 0xFF, 0);
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
+	retval = rtsx_write_register(chip, REG_SD_BYTE_CNT_H, 0xFF, 2);
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
 
 	chip->capacity[chip->card2lun[SD_CARD]] = sd_card->capacity;
 
 	retval = sd_set_init_para(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	dev_dbg(rtsx_dev(chip), "In reset_mmc_only, sd_card->sd_type = 0x%x\n",
 		sd_card->sd_type);
@@ -3032,15 +3637,18 @@ static int wait_data_buf_ready(struct rtsx_chip *chip)
 	for (i = 0; i < WAIT_DATA_READY_RTY_CNT; i++) {
 		if (detect_card_cd(chip, SD_CARD) != STATUS_SUCCESS) {
 			sd_set_err_code(chip, SD_NO_CARD);
-			TRACE_RET(chip, STATUS_FAIL);
+			rtsx_trace(chip);
+			return STATUS_FAIL;
 		}
 
 		sd_card->sd_data_buf_ready = 0;
 
 		retval = sd_send_cmd_get_rsp(chip, SEND_STATUS,
 				sd_card->sd_addr, SD_RSP_TYPE_R1, NULL, 0);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, STATUS_FAIL);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 
 		if (sd_card->sd_data_buf_ready) {
 			return sd_send_cmd_get_rsp(chip, SEND_STATUS,
@@ -3050,7 +3658,8 @@ static int wait_data_buf_ready(struct rtsx_chip *chip)
 
 	sd_set_err_code(chip, SD_TO_ERR);
 
-	TRACE_RET(chip, STATUS_FAIL);
+	rtsx_trace(chip);
+	return STATUS_FAIL;
 }
 
 void sd_stop_seq_mode(struct rtsx_chip *chip)
@@ -3118,8 +3727,10 @@ static inline int sd_auto_tune_clock(struct rtsx_chip *chip)
 	}
 
 	retval = sd_switch_clock(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	return STATUS_SUCCESS;
 }
@@ -3156,7 +3767,8 @@ int sd_rw(struct scsi_cmnd *srb, struct rtsx_chip *chip, u32 start_sector,
 			chip->card_fail |= SD_CARD;
 			chip->capacity[chip->card2lun[SD_CARD]] = 0;
 			chip->rw_need_retry = 1;
-			TRACE_RET(chip, STATUS_FAIL);
+			rtsx_trace(chip);
+			return STATUS_FAIL;
 		}
 	}
 
@@ -3170,7 +3782,8 @@ int sd_rw(struct scsi_cmnd *srb, struct rtsx_chip *chip, u32 start_sector,
 	retval = sd_switch_clock(chip);
 	if (retval != STATUS_SUCCESS) {
 		sd_set_err_code(chip, SD_IO_ERR);
-		TRACE_GOTO(chip, RW_FAIL);
+		rtsx_trace(chip);
+		goto RW_FAIL;
 	}
 
 	if (sd_card->seq_mode &&
@@ -3191,7 +3804,8 @@ int sd_rw(struct scsi_cmnd *srb, struct rtsx_chip *chip, u32 start_sector,
 		if (retval != STATUS_SUCCESS) {
 			chip->rw_need_retry = 1;
 			sd_set_err_code(chip, SD_STS_ERR);
-			TRACE_GOTO(chip, RW_FAIL);
+			rtsx_trace(chip);
+			goto RW_FAIL;
 		}
 
 		sd_card->seq_mode = 0;
@@ -3199,7 +3813,8 @@ int sd_rw(struct scsi_cmnd *srb, struct rtsx_chip *chip, u32 start_sector,
 		retval = rtsx_write_register(chip, RBCTL, RB_FLUSH, RB_FLUSH);
 		if (retval != STATUS_SUCCESS) {
 			sd_set_err_code(chip, SD_IO_ERR);
-			TRACE_GOTO(chip, RW_FAIL);
+			rtsx_trace(chip);
+			goto RW_FAIL;
 		}
 
 		if ((sd_card->pre_sec_cnt < 0x80)
@@ -3290,21 +3905,24 @@ int sd_rw(struct scsi_cmnd *srb, struct rtsx_chip *chip, u32 start_sector,
 
 				chip->rw_need_retry = 1;
 				sd_set_err_code(chip, SD_TO_ERR);
-				TRACE_GOTO(chip, RW_FAIL);
+				rtsx_trace(chip);
+				goto RW_FAIL;
 			}
 
 			retval = wait_data_buf_ready(chip);
 			if (retval != STATUS_SUCCESS) {
 				chip->rw_need_retry = 1;
 				sd_set_err_code(chip, SD_TO_ERR);
-				TRACE_GOTO(chip, RW_FAIL);
+				rtsx_trace(chip);
+				goto RW_FAIL;
 			}
 
 			retval = sd_send_cmd_get_rsp(chip, WRITE_MULTIPLE_BLOCK,
 					data_addr, SD_RSP_TYPE_R1, NULL, 0);
 			if (retval != STATUS_SUCCESS) {
 				chip->rw_need_retry = 1;
-				TRACE_GOTO(chip, RW_FAIL);
+				rtsx_trace(chip);
+				goto RW_FAIL;
 			}
 
 			rtsx_init_cmd(chip);
@@ -3348,7 +3966,8 @@ int sd_rw(struct scsi_cmnd *srb, struct rtsx_chip *chip, u32 start_sector,
 		if (detect_card_cd(chip, SD_CARD) != STATUS_SUCCESS) {
 			chip->rw_need_retry = 0;
 			dev_dbg(rtsx_dev(chip), "No card exist, exit sd_rw\n");
-			TRACE_RET(chip, STATUS_FAIL);
+			rtsx_trace(chip);
+			return STATUS_FAIL;
 		}
 
 		chip->rw_need_retry = 1;
@@ -3357,21 +3976,25 @@ int sd_rw(struct scsi_cmnd *srb, struct rtsx_chip *chip, u32 start_sector,
 					SD_RSP_TYPE_R1b, NULL, 0);
 		if (retval != STATUS_SUCCESS) {
 			sd_set_err_code(chip, SD_STS_ERR);
-			TRACE_GOTO(chip, RW_FAIL);
+			rtsx_trace(chip);
+			goto RW_FAIL;
 		}
 
 		if (stat & (SD_CRC7_ERR | SD_CRC16_ERR | SD_CRC_WRITE_ERR)) {
 			dev_dbg(rtsx_dev(chip), "SD CRC error, tune clock!\n");
 			sd_set_err_code(chip, SD_CRC_ERR);
-			TRACE_GOTO(chip, RW_FAIL);
+			rtsx_trace(chip);
+			goto RW_FAIL;
 		}
 
 		if (err == STATUS_TIMEDOUT) {
 			sd_set_err_code(chip, SD_TO_ERR);
-			TRACE_GOTO(chip, RW_FAIL);
+			rtsx_trace(chip);
+			goto RW_FAIL;
 		}
 
-		TRACE_RET(chip, err);
+		rtsx_trace(chip);
+		return err;
 	}
 
 	sd_card->pre_sec_addr = start_sector;
@@ -3386,7 +4009,8 @@ RW_FAIL:
 	if (detect_card_cd(chip, SD_CARD) != STATUS_SUCCESS) {
 		chip->rw_need_retry = 0;
 		dev_dbg(rtsx_dev(chip), "No card exist, exit sd_rw\n");
-		TRACE_RET(chip, STATUS_FAIL);
+		rtsx_trace(chip);
+		return STATUS_FAIL;
 	}
 
 	if (sd_check_err_code(chip, SD_CRC_ERR)) {
@@ -3407,7 +4031,8 @@ RW_FAIL:
 		}
 	}
 
-	TRACE_RET(chip, STATUS_FAIL);
+	rtsx_trace(chip);
+	return STATUS_FAIL;
 }
 
 #ifdef SUPPORT_CPRM
@@ -3417,7 +4042,7 @@ int soft_reset_sd_card(struct rtsx_chip *chip)
 }
 
 int ext_sd_send_cmd_get_rsp(struct rtsx_chip *chip, u8 cmd_idx,
-		u32 arg, u8 rsp_type, u8 *rsp, int rsp_len, int special_check)
+		u32 arg, u8 rsp_type, u8 *rsp, int rsp_len, bool special_check)
 {
 	int retval;
 	int timeout = 100;
@@ -3473,13 +4098,16 @@ RTY_SEND_CMD:
 
 			if (rsp_type & SD_WAIT_BUSY_END) {
 				retval = sd_check_data0_status(chip);
-				if (retval != STATUS_SUCCESS)
-					TRACE_RET(chip, retval);
+				if (retval != STATUS_SUCCESS) {
+					rtsx_trace(chip);
+					return retval;
+				}
 			} else {
 				sd_set_err_code(chip, SD_TO_ERR);
 			}
 		}
-		TRACE_RET(chip, STATUS_FAIL);
+		rtsx_trace(chip);
+		return STATUS_FAIL;
 	}
 
 	if (rsp_type == SD_RSP_TYPE_R0)
@@ -3489,14 +4117,16 @@ RTY_SEND_CMD:
 
 	if ((ptr[0] & 0xC0) != 0) {
 		sd_set_err_code(chip, SD_STS_ERR);
-		TRACE_RET(chip, STATUS_FAIL);
+		rtsx_trace(chip);
+		return STATUS_FAIL;
 	}
 
 	if (!(rsp_type & SD_NO_CHECK_CRC7)) {
 		if (ptr[stat_idx] & SD_CRC7_ERR) {
 			if (cmd_idx == WRITE_MULTIPLE_BLOCK) {
 				sd_set_err_code(chip, SD_CRC_ERR);
-				TRACE_RET(chip, STATUS_FAIL);
+				rtsx_trace(chip);
+				return STATUS_FAIL;
 			}
 			if (rty_cnt < SD_MAX_RETRY_COUNT) {
 				wait_timeout(20);
@@ -3504,16 +4134,19 @@ RTY_SEND_CMD:
 				goto RTY_SEND_CMD;
 			} else {
 				sd_set_err_code(chip, SD_CRC_ERR);
-				TRACE_RET(chip, STATUS_FAIL);
+				rtsx_trace(chip);
+				return STATUS_FAIL;
 			}
 		}
 	}
 
 	if ((cmd_idx == SELECT_CARD) || (cmd_idx == APP_CMD) ||
 		(cmd_idx == SEND_STATUS) || (cmd_idx == STOP_TRANSMISSION)) {
-		if ((cmd_idx != STOP_TRANSMISSION) && (special_check == 0)) {
-			if (ptr[1] & 0x80)
-				TRACE_RET(chip, STATUS_FAIL);
+		if ((cmd_idx != STOP_TRANSMISSION) && !special_check) {
+			if (ptr[1] & 0x80) {
+				rtsx_trace(chip);
+				return STATUS_FAIL;
+			}
 		}
 #ifdef SUPPORT_SD_LOCK
 		if (ptr[1] & 0x7D)
@@ -3521,19 +4154,26 @@ RTY_SEND_CMD:
 		if (ptr[1] & 0x7F)
 #endif
 		{
-			TRACE_RET(chip, STATUS_FAIL);
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
+		if (ptr[2] & 0xF8) {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
 		}
-		if (ptr[2] & 0xF8)
-			TRACE_RET(chip, STATUS_FAIL);
 
 		if (cmd_idx == SELECT_CARD) {
 			if (rsp_type == SD_RSP_TYPE_R2) {
-				if ((ptr[3] & 0x1E) != 0x04)
-					TRACE_RET(chip, STATUS_FAIL);
+				if ((ptr[3] & 0x1E) != 0x04) {
+					rtsx_trace(chip);
+					return STATUS_FAIL;
+				}
 
 			} else if (rsp_type == SD_RSP_TYPE_R0) {
-				if ((ptr[3] & 0x1E) != 0x03)
-					TRACE_RET(chip, STATUS_FAIL);
+				if ((ptr[3] & 0x1E) != 0x03) {
+					rtsx_trace(chip);
+					return STATUS_FAIL;
+				}
 			}
 		}
 	}
@@ -3570,8 +4210,10 @@ int ext_sd_get_rsp(struct rtsx_chip *chip, int len, u8 *rsp, u8 rsp_type)
 	rtsx_add_cmd(chip, READ_REG_CMD, REG_SD_CMD5, 0xFF, 0);
 
 	retval = rtsx_send_cmd(chip, SD_CARD, 100);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	if (rsp) {
 		int min_len = (rsp_len < len) ? rsp_len : len;
@@ -3617,7 +4259,8 @@ int sd_pass_thru_mode(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 	if (!(CHK_BIT(chip->lun_mc, lun))) {
 		SET_BIT(chip->lun_mc, lun);
 		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_CHANGE);
-		TRACE_RET(chip, TRANSPORT_FAILED);
+		rtsx_trace(chip);
+		return TRANSPORT_FAILED;
 	}
 
 	if ((0x53 != srb->cmnd[2]) || (0x44 != srb->cmnd[3]) ||
@@ -3625,7 +4268,8 @@ int sd_pass_thru_mode(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 		(0x61 != srb->cmnd[6]) || (0x72 != srb->cmnd[7]) ||
 		(0x64 != srb->cmnd[8])) {
 		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-		TRACE_RET(chip, TRANSPORT_FAILED);
+		rtsx_trace(chip);
+		return TRANSPORT_FAILED;
 	}
 
 	switch (srb->cmnd[1] & 0x0F) {
@@ -3639,7 +4283,8 @@ int sd_pass_thru_mode(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 
 	default:
 		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-		TRACE_RET(chip, TRANSPORT_FAILED);
+		rtsx_trace(chip);
+		return TRANSPORT_FAILED;
 	}
 
 	buf[5] = (1 == CHK_SD(sd_card)) ?  0x01 : 0x02;
@@ -3702,30 +4347,34 @@ int sd_execute_no_data(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 	unsigned int lun = SCSI_LUN(srb);
 	int retval, rsp_len;
 	u8 cmd_idx, rsp_type;
-	u8 standby = 0, acmd = 0;
+	bool standby = false, acmd = false;
 	u32 arg;
 
 	if (!sd_card->sd_pass_thru_en) {
 		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-		TRACE_RET(chip, TRANSPORT_FAILED);
+		rtsx_trace(chip);
+		return TRANSPORT_FAILED;
 	}
 
 	retval = sd_switch_clock(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, TRANSPORT_FAILED);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return TRANSPORT_FAILED;
+	}
 
 	if (sd_card->pre_cmd_err) {
 		sd_card->pre_cmd_err = 0;
 		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_CHANGE);
-		TRACE_RET(chip, TRANSPORT_FAILED);
+		rtsx_trace(chip);
+		return TRANSPORT_FAILED;
 	}
 
 	cmd_idx = srb->cmnd[2] & 0x3F;
 	if (srb->cmnd[1] & 0x02)
-		standby = 1;
+		standby = true;
 
 	if (srb->cmnd[1] & 0x01)
-		acmd = 1;
+		acmd = true;
 
 	arg = ((u32)srb->cmnd[3] << 24) | ((u32)srb->cmnd[4] << 16) |
 		((u32)srb->cmnd[5] << 8) | srb->cmnd[6];
@@ -3733,64 +4382,83 @@ int sd_execute_no_data(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 	retval = get_rsp_type(srb, &rsp_type, &rsp_len);
 	if (retval != STATUS_SUCCESS) {
 		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-		TRACE_RET(chip, TRANSPORT_FAILED);
+		rtsx_trace(chip);
+		return TRANSPORT_FAILED;
 	}
 	sd_card->last_rsp_type = rsp_type;
 
 	retval = sd_switch_clock(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, TRANSPORT_FAILED);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return TRANSPORT_FAILED;
+	}
 
 #ifdef SUPPORT_SD_LOCK
 	if ((sd_card->sd_lock_status & SD_LOCK_1BIT_MODE) == 0) {
 		if (CHK_MMC_8BIT(sd_card)) {
 			retval = rtsx_write_register(chip, REG_SD_CFG1, 0x03,
 						SD_BUS_WIDTH_8);
-			if (retval != STATUS_SUCCESS)
-				TRACE_RET(chip, TRANSPORT_FAILED);
+			if (retval != STATUS_SUCCESS) {
+				rtsx_trace(chip);
+				return TRANSPORT_FAILED;
+			}
 
 		} else if (CHK_SD(sd_card) || CHK_MMC_4BIT(sd_card)) {
 			retval = rtsx_write_register(chip, REG_SD_CFG1, 0x03,
 						SD_BUS_WIDTH_4);
-			if (retval != STATUS_SUCCESS)
-				TRACE_RET(chip, TRANSPORT_FAILED);
+			if (retval != STATUS_SUCCESS) {
+				rtsx_trace(chip);
+				return TRANSPORT_FAILED;
+			}
 		}
 	}
 #else
 	retval = rtsx_write_register(chip, REG_SD_CFG1, 0x03, SD_BUS_WIDTH_4);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, TRANSPORT_FAILED);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return TRANSPORT_FAILED;
+	}
 #endif
 
 	if (standby) {
 		retval = sd_select_card(chip, 0);
-		if (retval != STATUS_SUCCESS)
-			TRACE_GOTO(chip, SD_Execute_Cmd_Failed);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			goto SD_Execute_Cmd_Failed;
+		}
 	}
 
 	if (acmd) {
 		retval = ext_sd_send_cmd_get_rsp(chip, APP_CMD,
 						sd_card->sd_addr,
-						SD_RSP_TYPE_R1, NULL, 0, 0);
-		if (retval != STATUS_SUCCESS)
-			TRACE_GOTO(chip, SD_Execute_Cmd_Failed);
+						SD_RSP_TYPE_R1, NULL, 0, false);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			goto SD_Execute_Cmd_Failed;
+		}
 	}
 
 	retval = ext_sd_send_cmd_get_rsp(chip, cmd_idx, arg, rsp_type,
-			sd_card->rsp, rsp_len, 0);
-	if (retval != STATUS_SUCCESS)
-		TRACE_GOTO(chip, SD_Execute_Cmd_Failed);
+			sd_card->rsp, rsp_len, false);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		goto SD_Execute_Cmd_Failed;
+	}
 
 	if (standby) {
 		retval = sd_select_card(chip, 1);
-		if (retval != STATUS_SUCCESS)
-			TRACE_GOTO(chip, SD_Execute_Cmd_Failed);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			goto SD_Execute_Cmd_Failed;
+		}
 	}
 
 #ifdef SUPPORT_SD_LOCK
 	retval = sd_update_lock_status(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_GOTO(chip, SD_Execute_Cmd_Failed);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		goto SD_Execute_Cmd_Failed;
+	}
 #endif
 
 	scsi_set_resid(srb, 0);
@@ -3804,7 +4472,8 @@ SD_Execute_Cmd_Failed:
 	if (!(chip->card_ready & SD_CARD))
 		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_NOT_PRESENT);
 
-	TRACE_RET(chip, TRANSPORT_FAILED);
+	rtsx_trace(chip);
+	return TRANSPORT_FAILED;
 }
 
 int sd_execute_read_data(struct scsi_cmnd *srb, struct rtsx_chip *chip)
@@ -3812,35 +4481,39 @@ int sd_execute_read_data(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 	struct sd_info *sd_card = &(chip->sd_card);
 	unsigned int lun = SCSI_LUN(srb);
 	int retval, rsp_len, i;
-	int cmd13_checkbit = 0, read_err = 0;
+	bool read_err = false, cmd13_checkbit = false;
 	u8 cmd_idx, rsp_type, bus_width;
-	u8 send_cmd12 = 0, standby = 0, acmd = 0;
+	bool standby = false, send_cmd12 = false, acmd = false;
 	u32 data_len;
 
 	if (!sd_card->sd_pass_thru_en) {
 		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-		TRACE_RET(chip, TRANSPORT_FAILED);
+		rtsx_trace(chip);
+		return TRANSPORT_FAILED;
 	}
 
 	if (sd_card->pre_cmd_err) {
 		sd_card->pre_cmd_err = 0;
 		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_CHANGE);
-		TRACE_RET(chip, TRANSPORT_FAILED);
+		rtsx_trace(chip);
+		return TRANSPORT_FAILED;
 	}
 
 	retval = sd_switch_clock(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, TRANSPORT_FAILED);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return TRANSPORT_FAILED;
+	}
 
 	cmd_idx = srb->cmnd[2] & 0x3F;
 	if (srb->cmnd[1] & 0x04)
-		send_cmd12 = 1;
+		send_cmd12 = true;
 
 	if (srb->cmnd[1] & 0x02)
-		standby = 1;
+		standby = true;
 
 	if (srb->cmnd[1] & 0x01)
-		acmd = 1;
+		acmd = true;
 
 	data_len = ((u32)srb->cmnd[7] << 16) | ((u32)srb->cmnd[8]
 						<< 8) | srb->cmnd[9];
@@ -3848,13 +4521,16 @@ int sd_execute_read_data(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 	retval = get_rsp_type(srb, &rsp_type, &rsp_len);
 	if (retval != STATUS_SUCCESS) {
 		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-		TRACE_RET(chip, TRANSPORT_FAILED);
+		rtsx_trace(chip);
+		return TRANSPORT_FAILED;
 	}
 	sd_card->last_rsp_type = rsp_type;
 
 	retval = sd_switch_clock(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, TRANSPORT_FAILED);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return TRANSPORT_FAILED;
+	}
 
 #ifdef SUPPORT_SD_LOCK
 	if ((sd_card->sd_lock_status & SD_LOCK_1BIT_MODE) == 0) {
@@ -3874,23 +4550,29 @@ int sd_execute_read_data(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 
 	if (data_len < 512) {
 		retval = ext_sd_send_cmd_get_rsp(chip, SET_BLOCKLEN, data_len,
-				SD_RSP_TYPE_R1, NULL, 0, 0);
-		if (retval != STATUS_SUCCESS)
-			TRACE_GOTO(chip, SD_Execute_Read_Cmd_Failed);
+				SD_RSP_TYPE_R1, NULL, 0, false);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			goto SD_Execute_Read_Cmd_Failed;
+		}
 	}
 
 	if (standby) {
 		retval = sd_select_card(chip, 0);
-		if (retval != STATUS_SUCCESS)
-			TRACE_GOTO(chip, SD_Execute_Read_Cmd_Failed);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			goto SD_Execute_Read_Cmd_Failed;
+		}
 	}
 
 	if (acmd) {
 		retval = ext_sd_send_cmd_get_rsp(chip, APP_CMD,
 						sd_card->sd_addr,
-						SD_RSP_TYPE_R1, NULL, 0, 0);
-		if (retval != STATUS_SUCCESS)
-			TRACE_GOTO(chip, SD_Execute_Read_Cmd_Failed);
+						SD_RSP_TYPE_R1, NULL, 0, false);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			goto SD_Execute_Read_Cmd_Failed;
+		}
 	}
 
 	if (data_len <= 512) {
@@ -3909,16 +4591,19 @@ int sd_execute_read_data(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 		cmd[4] = srb->cmnd[6];
 
 		buf = kmalloc(data_len, GFP_KERNEL);
-		if (buf == NULL)
-			TRACE_RET(chip, TRANSPORT_ERROR);
+		if (buf == NULL) {
+			rtsx_trace(chip);
+			return TRANSPORT_ERROR;
+		}
 
 		retval = sd_read_data(chip, SD_TM_NORMAL_READ, cmd, 5, byte_cnt,
 				       blk_cnt, bus_width, buf, data_len, 2000);
 		if (retval != STATUS_SUCCESS) {
-			read_err = 1;
+			read_err = true;
 			kfree(buf);
 			rtsx_clear_sd_error(chip);
-			TRACE_GOTO(chip, SD_Execute_Read_Cmd_Failed);
+			rtsx_trace(chip);
+			goto SD_Execute_Read_Cmd_Failed;
 		}
 
 		min_len = min(data_len, scsi_bufflen(srb));
@@ -3964,49 +4649,63 @@ int sd_execute_read_data(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 					scsi_bufflen(srb), scsi_sg_count(srb),
 					DMA_FROM_DEVICE, 10000);
 		if (retval < 0) {
-			read_err = 1;
+			read_err = true;
 			rtsx_clear_sd_error(chip);
-			TRACE_GOTO(chip, SD_Execute_Read_Cmd_Failed);
+			rtsx_trace(chip);
+			goto SD_Execute_Read_Cmd_Failed;
 		}
 
 	} else {
-		TRACE_GOTO(chip, SD_Execute_Read_Cmd_Failed);
+		rtsx_trace(chip);
+		goto SD_Execute_Read_Cmd_Failed;
 	}
 
 	retval = ext_sd_get_rsp(chip, rsp_len, sd_card->rsp, rsp_type);
-	if (retval != STATUS_SUCCESS)
-		TRACE_GOTO(chip, SD_Execute_Read_Cmd_Failed);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		goto SD_Execute_Read_Cmd_Failed;
+	}
 
 	if (standby) {
 		retval = sd_select_card(chip, 1);
-		if (retval != STATUS_SUCCESS)
-			TRACE_GOTO(chip, SD_Execute_Read_Cmd_Failed);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			goto SD_Execute_Read_Cmd_Failed;
+		}
 	}
 
 	if (send_cmd12) {
 		retval = ext_sd_send_cmd_get_rsp(chip, STOP_TRANSMISSION,
-				0, SD_RSP_TYPE_R1b, NULL, 0, 0);
-		if (retval != STATUS_SUCCESS)
-			TRACE_GOTO(chip, SD_Execute_Read_Cmd_Failed);
+				0, SD_RSP_TYPE_R1b, NULL, 0, false);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			goto SD_Execute_Read_Cmd_Failed;
+		}
 	}
 
 	if (data_len < 512) {
 		retval = ext_sd_send_cmd_get_rsp(chip, SET_BLOCKLEN, 0x200,
-				SD_RSP_TYPE_R1, NULL, 0, 0);
-		if (retval != STATUS_SUCCESS)
-			TRACE_GOTO(chip, SD_Execute_Read_Cmd_Failed);
+				SD_RSP_TYPE_R1, NULL, 0, false);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			goto SD_Execute_Read_Cmd_Failed;
+		}
 
 		retval = rtsx_write_register(chip, SD_BYTE_CNT_H, 0xFF, 0x02);
-		if (retval != STATUS_SUCCESS)
-			TRACE_GOTO(chip, SD_Execute_Read_Cmd_Failed);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			goto SD_Execute_Read_Cmd_Failed;
+		}
 
 		retval = rtsx_write_register(chip, SD_BYTE_CNT_L, 0xFF, 0x00);
-		if (retval != STATUS_SUCCESS)
-			TRACE_GOTO(chip, SD_Execute_Read_Cmd_Failed);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			goto SD_Execute_Read_Cmd_Failed;
+		}
 	}
 
 	if ((srb->cmnd[1] & 0x02) || (srb->cmnd[1] & 0x04))
-		cmd13_checkbit = 1;
+		cmd13_checkbit = true;
 
 	for (i = 0; i < 3; i++) {
 		retval = ext_sd_send_cmd_get_rsp(chip, SEND_STATUS,
@@ -4016,8 +4715,10 @@ int sd_execute_read_data(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 		if (retval == STATUS_SUCCESS)
 			break;
 	}
-	if (retval != STATUS_SUCCESS)
-		TRACE_GOTO(chip, SD_Execute_Read_Cmd_Failed);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		goto SD_Execute_Read_Cmd_Failed;
+	}
 
 	scsi_set_resid(srb, 0);
 	return TRANSPORT_GOOD;
@@ -4033,7 +4734,8 @@ SD_Execute_Read_Cmd_Failed:
 	if (!(chip->card_ready & SD_CARD))
 		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_NOT_PRESENT);
 
-	TRACE_RET(chip, TRANSPORT_FAILED);
+	rtsx_trace(chip);
+	return TRANSPORT_FAILED;
 }
 
 int sd_execute_write_data(struct scsi_cmnd *srb, struct rtsx_chip *chip)
@@ -4041,9 +4743,9 @@ int sd_execute_write_data(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 	struct sd_info *sd_card = &(chip->sd_card);
 	unsigned int lun = SCSI_LUN(srb);
 	int retval, rsp_len, i;
-	int cmd13_checkbit = 0, write_err = 0;
+	bool write_err = false, cmd13_checkbit = false;
 	u8 cmd_idx, rsp_type;
-	u8 send_cmd12 = 0, standby = 0, acmd = 0;
+	bool standby = false, send_cmd12 = false, acmd = false;
 	u32 data_len, arg;
 #ifdef SUPPORT_SD_LOCK
 	int lock_cmd_fail = 0;
@@ -4053,28 +4755,32 @@ int sd_execute_write_data(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 
 	if (!sd_card->sd_pass_thru_en) {
 		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-		TRACE_RET(chip, TRANSPORT_FAILED);
+		rtsx_trace(chip);
+		return TRANSPORT_FAILED;
 	}
 
 	if (sd_card->pre_cmd_err) {
 		sd_card->pre_cmd_err = 0;
 		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_CHANGE);
-		TRACE_RET(chip, TRANSPORT_FAILED);
+		rtsx_trace(chip);
+		return TRANSPORT_FAILED;
 	}
 
 	retval = sd_switch_clock(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, TRANSPORT_FAILED);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return TRANSPORT_FAILED;
+	}
 
 	cmd_idx = srb->cmnd[2] & 0x3F;
 	if (srb->cmnd[1] & 0x04)
-		send_cmd12 = 1;
+		send_cmd12 = true;
 
 	if (srb->cmnd[1] & 0x02)
-		standby = 1;
+		standby = true;
 
 	if (srb->cmnd[1] & 0x01)
-		acmd = 1;
+		acmd = true;
 
 	data_len = ((u32)srb->cmnd[7] << 16) | ((u32)srb->cmnd[8]
 						<< 8) | srb->cmnd[9];
@@ -4091,68 +4797,87 @@ int sd_execute_write_data(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 	retval = get_rsp_type(srb, &rsp_type, &rsp_len);
 	if (retval != STATUS_SUCCESS) {
 		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-		TRACE_RET(chip, TRANSPORT_FAILED);
+		rtsx_trace(chip);
+		return TRANSPORT_FAILED;
 	}
 	sd_card->last_rsp_type = rsp_type;
 
 	retval = sd_switch_clock(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, TRANSPORT_FAILED);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return TRANSPORT_FAILED;
+	}
 
 #ifdef SUPPORT_SD_LOCK
 	if ((sd_card->sd_lock_status & SD_LOCK_1BIT_MODE) == 0) {
 		if (CHK_MMC_8BIT(sd_card)) {
 			retval = rtsx_write_register(chip, REG_SD_CFG1, 0x03,
 						SD_BUS_WIDTH_8);
-			if (retval != STATUS_SUCCESS)
-				TRACE_RET(chip, TRANSPORT_FAILED);
+			if (retval != STATUS_SUCCESS) {
+				rtsx_trace(chip);
+				return TRANSPORT_FAILED;
+			}
 
 		} else if (CHK_SD(sd_card) || CHK_MMC_4BIT(sd_card)) {
 			retval = rtsx_write_register(chip, REG_SD_CFG1, 0x03,
 						SD_BUS_WIDTH_4);
-			if (retval != STATUS_SUCCESS)
-				TRACE_RET(chip, TRANSPORT_FAILED);
+			if (retval != STATUS_SUCCESS) {
+				rtsx_trace(chip);
+				return TRANSPORT_FAILED;
+			}
 		}
 	}
 #else
 	retval = rtsx_write_register(chip, REG_SD_CFG1, 0x03, SD_BUS_WIDTH_4);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, TRANSPORT_FAILED);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return TRANSPORT_FAILED;
+	}
 #endif
 
 	if (data_len < 512) {
 		retval = ext_sd_send_cmd_get_rsp(chip, SET_BLOCKLEN, data_len,
-				SD_RSP_TYPE_R1, NULL, 0, 0);
-		if (retval != STATUS_SUCCESS)
-			TRACE_GOTO(chip, SD_Execute_Write_Cmd_Failed);
+				SD_RSP_TYPE_R1, NULL, 0, false);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			goto SD_Execute_Write_Cmd_Failed;
+		}
 	}
 
 	if (standby) {
 		retval = sd_select_card(chip, 0);
-		if (retval != STATUS_SUCCESS)
-			TRACE_GOTO(chip, SD_Execute_Write_Cmd_Failed);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			goto SD_Execute_Write_Cmd_Failed;
+		}
 	}
 
 	if (acmd) {
 		retval = ext_sd_send_cmd_get_rsp(chip, APP_CMD,
 						sd_card->sd_addr,
-						SD_RSP_TYPE_R1, NULL, 0, 0);
-		if (retval != STATUS_SUCCESS)
-			TRACE_GOTO(chip, SD_Execute_Write_Cmd_Failed);
+						SD_RSP_TYPE_R1, NULL, 0, false);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			goto SD_Execute_Write_Cmd_Failed;
+		}
 	}
 
 	retval = ext_sd_send_cmd_get_rsp(chip, cmd_idx, arg, rsp_type,
-			sd_card->rsp, rsp_len, 0);
-	if (retval != STATUS_SUCCESS)
-		TRACE_GOTO(chip, SD_Execute_Write_Cmd_Failed);
+			sd_card->rsp, rsp_len, false);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		goto SD_Execute_Write_Cmd_Failed;
+	}
 
 	if (data_len <= 512) {
 		u16 i;
 		u8 *buf;
 
 		buf = kmalloc(data_len, GFP_KERNEL);
-		if (buf == NULL)
-			TRACE_RET(chip, TRANSPORT_ERROR);
+		if (buf == NULL) {
+			rtsx_trace(chip);
+			return TRANSPORT_ERROR;
+		}
 
 		rtsx_stor_get_xfer_buf(buf, data_len, srb);
 
@@ -4170,7 +4895,8 @@ int sd_execute_write_data(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 			retval = rtsx_send_cmd(chip, 0, 250);
 			if (retval != STATUS_SUCCESS) {
 				kfree(buf);
-				TRACE_GOTO(chip, SD_Execute_Write_Cmd_Failed);
+				rtsx_trace(chip);
+				goto SD_Execute_Write_Cmd_Failed;
 			}
 
 			rtsx_init_cmd(chip);
@@ -4181,7 +4907,8 @@ int sd_execute_write_data(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 			retval = rtsx_send_cmd(chip, 0, 250);
 			if (retval != STATUS_SUCCESS) {
 				kfree(buf);
-				TRACE_GOTO(chip, SD_Execute_Write_Cmd_Failed);
+				rtsx_trace(chip);
+				goto SD_Execute_Write_Cmd_Failed;
 			}
 		} else {
 			rtsx_init_cmd(chip);
@@ -4192,7 +4919,8 @@ int sd_execute_write_data(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 			retval = rtsx_send_cmd(chip, 0, 250);
 			if (retval != STATUS_SUCCESS) {
 				kfree(buf);
-				TRACE_GOTO(chip, SD_Execute_Write_Cmd_Failed);
+				rtsx_trace(chip);
+				goto SD_Execute_Write_Cmd_Failed;
 			}
 		}
 
@@ -4243,13 +4971,15 @@ int sd_execute_write_data(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 					DMA_TO_DEVICE, 10000);
 
 	} else {
-		TRACE_GOTO(chip, SD_Execute_Write_Cmd_Failed);
+		rtsx_trace(chip);
+		goto SD_Execute_Write_Cmd_Failed;
 	}
 
 	if (retval < 0) {
-		write_err = 1;
+		write_err = true;
 		rtsx_clear_sd_error(chip);
-		TRACE_GOTO(chip, SD_Execute_Write_Cmd_Failed);
+		rtsx_trace(chip);
+		goto SD_Execute_Write_Cmd_Failed;
 	}
 
 #ifdef SUPPORT_SD_LOCK
@@ -4275,34 +5005,44 @@ int sd_execute_write_data(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 
 	if (standby) {
 		retval = sd_select_card(chip, 1);
-		if (retval != STATUS_SUCCESS)
-			TRACE_GOTO(chip, SD_Execute_Write_Cmd_Failed);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			goto SD_Execute_Write_Cmd_Failed;
+		}
 	}
 
 	if (send_cmd12) {
 		retval = ext_sd_send_cmd_get_rsp(chip, STOP_TRANSMISSION,
-				0, SD_RSP_TYPE_R1b, NULL, 0, 0);
-		if (retval != STATUS_SUCCESS)
-			TRACE_GOTO(chip, SD_Execute_Write_Cmd_Failed);
+				0, SD_RSP_TYPE_R1b, NULL, 0, false);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			goto SD_Execute_Write_Cmd_Failed;
+		}
 	}
 
 	if (data_len < 512) {
 		retval = ext_sd_send_cmd_get_rsp(chip, SET_BLOCKLEN, 0x200,
-				SD_RSP_TYPE_R1, NULL, 0, 0);
-		if (retval != STATUS_SUCCESS)
-			TRACE_GOTO(chip, SD_Execute_Write_Cmd_Failed);
+				SD_RSP_TYPE_R1, NULL, 0, false);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			goto SD_Execute_Write_Cmd_Failed;
+		}
 
 		retval = rtsx_write_register(chip, SD_BYTE_CNT_H, 0xFF, 0x02);
-		if (retval != STATUS_SUCCESS)
-			TRACE_GOTO(chip, SD_Execute_Write_Cmd_Failed);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			goto SD_Execute_Write_Cmd_Failed;
+		}
 
 		rtsx_write_register(chip, SD_BYTE_CNT_L, 0xFF, 0x00);
-		if (retval != STATUS_SUCCESS)
-			TRACE_GOTO(chip, SD_Execute_Write_Cmd_Failed);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			goto SD_Execute_Write_Cmd_Failed;
+		}
 	}
 
 	if ((srb->cmnd[1] & 0x02) || (srb->cmnd[1] & 0x04))
-		cmd13_checkbit = 1;
+		cmd13_checkbit = true;
 
 	for (i = 0; i < 3; i++) {
 		retval = ext_sd_send_cmd_get_rsp(chip, SEND_STATUS,
@@ -4312,8 +5052,10 @@ int sd_execute_write_data(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 		if (retval == STATUS_SUCCESS)
 			break;
 	}
-	if (retval != STATUS_SUCCESS)
-		TRACE_GOTO(chip, SD_Execute_Write_Cmd_Failed);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		goto SD_Execute_Write_Cmd_Failed;
+	}
 
 #ifdef SUPPORT_SD_LOCK
 	if (cmd_idx == LOCK_UNLOCK) {
@@ -4339,7 +5081,8 @@ int sd_execute_write_data(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 						retval = reset_sd(chip);
 						if (retval != STATUS_SUCCESS) {
 							sd_card->sd_lock_status &= ~(SD_UNLOCK_POW_ON | SD_SDR_RST);
-							TRACE_GOTO(chip, SD_Execute_Write_Cmd_Failed);
+							rtsx_trace(chip);
+							goto SD_Execute_Write_Cmd_Failed;
 						}
 					}
 
@@ -4352,7 +5095,8 @@ int sd_execute_write_data(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 	if (lock_cmd_fail) {
 		scsi_set_resid(srb, 0);
 		set_sense_type(chip, lun, SENSE_TYPE_NO_SENSE);
-		TRACE_RET(chip, TRANSPORT_FAILED);
+		rtsx_trace(chip);
+		return TRANSPORT_FAILED;
 	}
 #endif  /* SUPPORT_SD_LOCK */
 
@@ -4370,7 +5114,8 @@ SD_Execute_Write_Cmd_Failed:
 	if (!(chip->card_ready & SD_CARD))
 		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_NOT_PRESENT);
 
-	TRACE_RET(chip, TRANSPORT_FAILED);
+	rtsx_trace(chip);
+	return TRANSPORT_FAILED;
 }
 
 int sd_get_cmd_rsp(struct scsi_cmnd *srb, struct rtsx_chip *chip)
@@ -4382,20 +5127,23 @@ int sd_get_cmd_rsp(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 
 	if (!sd_card->sd_pass_thru_en) {
 		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-		TRACE_RET(chip, TRANSPORT_FAILED);
+		rtsx_trace(chip);
+		return TRANSPORT_FAILED;
 	}
 
 	if (sd_card->pre_cmd_err) {
 		sd_card->pre_cmd_err = 0;
 		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_CHANGE);
-		TRACE_RET(chip, TRANSPORT_FAILED);
+		rtsx_trace(chip);
+		return TRANSPORT_FAILED;
 	}
 
 	data_len = ((u16)srb->cmnd[7] << 8) | srb->cmnd[8];
 
 	if (sd_card->last_rsp_type == SD_RSP_TYPE_R0) {
 		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-		TRACE_RET(chip, TRANSPORT_FAILED);
+		rtsx_trace(chip);
+		return TRANSPORT_FAILED;
 	} else if (sd_card->last_rsp_type == SD_RSP_TYPE_R2) {
 		count = (data_len < 17) ? data_len : 17;
 	} else {
@@ -4420,13 +5168,15 @@ int sd_hw_rst(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 
 	if (!sd_card->sd_pass_thru_en) {
 		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-		TRACE_RET(chip, TRANSPORT_FAILED);
+		rtsx_trace(chip);
+		return TRANSPORT_FAILED;
 	}
 
 	if (sd_card->pre_cmd_err) {
 		sd_card->pre_cmd_err = 0;
 		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_CHANGE);
-		TRACE_RET(chip, TRANSPORT_FAILED);
+		rtsx_trace(chip);
+		return TRANSPORT_FAILED;
 	}
 
 	if ((0x53 != srb->cmnd[2]) || (0x44 != srb->cmnd[3]) ||
@@ -4434,7 +5184,8 @@ int sd_hw_rst(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 		(0x61 != srb->cmnd[6]) || (0x72 != srb->cmnd[7]) ||
 		(0x64 != srb->cmnd[8])) {
 		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-		TRACE_RET(chip, TRANSPORT_FAILED);
+		rtsx_trace(chip);
+		return TRANSPORT_FAILED;
 	}
 
 	switch (srb->cmnd[1] & 0x0F) {
@@ -4450,7 +5201,8 @@ int sd_hw_rst(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 #endif
 			set_sense_type(chip, lun, SENSE_TYPE_MEDIA_NOT_PRESENT);
 			sd_card->pre_cmd_err = 1;
-			TRACE_RET(chip, TRANSPORT_FAILED);
+			rtsx_trace(chip);
+			return TRANSPORT_FAILED;
 		}
 #ifdef SUPPORT_SD_LOCK
 		sd_card->sd_lock_status &= ~SD_SDR_RST;
@@ -4462,13 +5214,15 @@ int sd_hw_rst(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 		if (retval != STATUS_SUCCESS) {
 			set_sense_type(chip, lun, SENSE_TYPE_MEDIA_NOT_PRESENT);
 			sd_card->pre_cmd_err = 1;
-			TRACE_RET(chip, TRANSPORT_FAILED);
+			rtsx_trace(chip);
+			return TRANSPORT_FAILED;
 		}
 		break;
 
 	default:
 		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-		TRACE_RET(chip, TRANSPORT_FAILED);
+		rtsx_trace(chip);
+		return TRANSPORT_FAILED;
 	}
 
 	scsi_set_resid(srb, 0);
@@ -4492,26 +5246,41 @@ int sd_power_off_card3v3(struct rtsx_chip *chip)
 	int retval;
 
 	retval = disable_card_clock(chip, SD_CARD);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
-	RTSX_WRITE_REG(chip, CARD_OE, SD_OUTPUT_EN, 0);
+	retval = rtsx_write_register(chip, CARD_OE, SD_OUTPUT_EN, 0);
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
 
 	if (!chip->ft2_fast_mode) {
 		retval = card_power_off(chip, SD_CARD);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, STATUS_FAIL);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 
 		wait_timeout(50);
 	}
 
 	if (chip->asic_code) {
 		retval = sd_pull_ctl_disable(chip);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, STATUS_FAIL);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 	} else {
-		RTSX_WRITE_REG(chip, FPGA_PULL_CTL,
-			FPGA_SD_PULL_CTL_BIT | 0x20, FPGA_SD_PULL_CTL_BIT);
+		retval = rtsx_write_register(chip, FPGA_PULL_CTL,
+					     FPGA_SD_PULL_CTL_BIT | 0x20,
+					     FPGA_SD_PULL_CTL_BIT);
+		if (retval) {
+			rtsx_trace(chip);
+			return retval;
+		}
 	}
 
 	return STATUS_SUCCESS;
@@ -4538,8 +5307,10 @@ int release_sd_card(struct rtsx_chip *chip)
 	memset(sd_card->raw_scr, 0, 8);
 
 	retval = sd_power_off_card3v3(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	return STATUS_SUCCESS;
 }
diff --git a/drivers/staging/rts5208/sd.h b/drivers/staging/rts5208/sd.h
index 735b2d0f5a78..60b79280fb5f 100644
--- a/drivers/staging/rts5208/sd.h
+++ b/drivers/staging/rts5208/sd.h
@@ -287,7 +287,7 @@ int release_sd_card(struct rtsx_chip *chip);
 #ifdef SUPPORT_CPRM
 int soft_reset_sd_card(struct rtsx_chip *chip);
 int ext_sd_send_cmd_get_rsp(struct rtsx_chip *chip, u8 cmd_idx,
-		u32 arg, u8 rsp_type, u8 *rsp, int rsp_len, int special_check);
+		u32 arg, u8 rsp_type, u8 *rsp, int rsp_len, bool special_check);
 int ext_sd_get_rsp(struct rtsx_chip *chip, int len, u8 *rsp, u8 rsp_type);
 
 int sd_pass_thru_mode(struct scsi_cmnd *srb, struct rtsx_chip *chip);
diff --git a/drivers/staging/rts5208/spi.c b/drivers/staging/rts5208/spi.c
index 29f4a80844fa..e67e7ecc2cbd 100644
--- a/drivers/staging/rts5208/spi.c
+++ b/drivers/staging/rts5208/spi.c
@@ -25,9 +25,6 @@
 #include <linux/sched.h>
 
 #include "rtsx.h"
-#include "rtsx_transport.h"
-#include "rtsx_scsi.h"
-#include "rtsx_card.h"
 #include "spi.h"
 
 static inline void spi_set_err_code(struct rtsx_chip *chip, u8 err_code)
@@ -39,10 +36,20 @@ static inline void spi_set_err_code(struct rtsx_chip *chip, u8 err_code)
 
 static int spi_init(struct rtsx_chip *chip)
 {
-	RTSX_WRITE_REG(chip, SPI_CONTROL, 0xFF,
-		CS_POLARITY_LOW | DTO_MSB_FIRST | SPI_MASTER | SPI_MODE0 |
-		SPI_AUTO);
-	RTSX_WRITE_REG(chip, SPI_TCTL, EDO_TIMING_MASK, SAMPLE_DELAY_HALF);
+	int retval;
+
+	retval = rtsx_write_register(chip, SPI_CONTROL, 0xFF,
+				     CS_POLARITY_LOW | DTO_MSB_FIRST | SPI_MASTER | SPI_MODE0 | SPI_AUTO);
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
+	retval = rtsx_write_register(chip, SPI_TCTL, EDO_TIMING_MASK,
+				     SAMPLE_DELAY_HALF);
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
 
 	return STATUS_SUCCESS;
 }
@@ -52,25 +59,51 @@ static int spi_set_init_para(struct rtsx_chip *chip)
 	struct spi_info *spi = &(chip->spi);
 	int retval;
 
-	RTSX_WRITE_REG(chip, SPI_CLK_DIVIDER1, 0xFF, (u8)(spi->clk_div >> 8));
-	RTSX_WRITE_REG(chip, SPI_CLK_DIVIDER0, 0xFF, (u8)(spi->clk_div));
+	retval = rtsx_write_register(chip, SPI_CLK_DIVIDER1, 0xFF,
+				     (u8)(spi->clk_div >> 8));
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
+	retval = rtsx_write_register(chip, SPI_CLK_DIVIDER0, 0xFF,
+				     (u8)(spi->clk_div));
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
 
 	retval = switch_clock(chip, spi->spi_clock);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	retval = select_card(chip, SPI_CARD);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
-	RTSX_WRITE_REG(chip, CARD_CLK_EN, SPI_CLK_EN, SPI_CLK_EN);
-	RTSX_WRITE_REG(chip, CARD_OE, SPI_OUTPUT_EN, SPI_OUTPUT_EN);
+	retval = rtsx_write_register(chip, CARD_CLK_EN, SPI_CLK_EN,
+				     SPI_CLK_EN);
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
+	retval = rtsx_write_register(chip, CARD_OE, SPI_OUTPUT_EN,
+				     SPI_OUTPUT_EN);
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
 
 	wait_timeout(10);
 
 	retval = spi_init(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	return STATUS_SUCCESS;
 }
@@ -91,7 +124,8 @@ static int sf_polling_status(struct rtsx_chip *chip, int msec)
 	if (retval < 0) {
 		rtsx_clear_spi_error(chip);
 		spi_set_err_code(chip, SPI_BUSY_ERR);
-		TRACE_RET(chip, STATUS_FAIL);
+		rtsx_trace(chip);
+		return STATUS_FAIL;
 	}
 
 	return STATUS_SUCCESS;
@@ -119,7 +153,8 @@ static int sf_enable_write(struct rtsx_chip *chip, u8 ins)
 	if (retval < 0) {
 		rtsx_clear_spi_error(chip);
 		spi_set_err_code(chip, SPI_HW_ERR);
-		TRACE_RET(chip, STATUS_FAIL);
+		rtsx_trace(chip);
+		return STATUS_FAIL;
 	}
 
 	return STATUS_SUCCESS;
@@ -147,7 +182,8 @@ static int sf_disable_write(struct rtsx_chip *chip, u8 ins)
 	if (retval < 0) {
 		rtsx_clear_spi_error(chip);
 		spi_set_err_code(chip, SPI_HW_ERR);
-		TRACE_RET(chip, STATUS_FAIL);
+		rtsx_trace(chip);
+		return STATUS_FAIL;
 	}
 
 	return STATUS_SUCCESS;
@@ -205,7 +241,8 @@ static int sf_erase(struct rtsx_chip *chip, u8 ins, u8 addr_mode, u32 addr)
 	if (retval < 0) {
 		rtsx_clear_spi_error(chip);
 		spi_set_err_code(chip, SPI_HW_ERR);
-		TRACE_RET(chip, STATUS_FAIL);
+		rtsx_trace(chip);
+		return STATUS_FAIL;
 	}
 
 	return STATUS_SUCCESS;
@@ -221,25 +258,56 @@ static int spi_init_eeprom(struct rtsx_chip *chip)
 	else
 		clk = CLK_30;
 
-	RTSX_WRITE_REG(chip, SPI_CLK_DIVIDER1, 0xFF, 0x00);
-	RTSX_WRITE_REG(chip, SPI_CLK_DIVIDER0, 0xFF, 0x27);
+	retval = rtsx_write_register(chip, SPI_CLK_DIVIDER1, 0xFF, 0x00);
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
+	retval = rtsx_write_register(chip, SPI_CLK_DIVIDER0, 0xFF, 0x27);
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
 
 	retval = switch_clock(chip, clk);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	retval = select_card(chip, SPI_CARD);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
-	RTSX_WRITE_REG(chip, CARD_CLK_EN, SPI_CLK_EN, SPI_CLK_EN);
-	RTSX_WRITE_REG(chip, CARD_OE, SPI_OUTPUT_EN, SPI_OUTPUT_EN);
+	retval = rtsx_write_register(chip, CARD_CLK_EN, SPI_CLK_EN,
+				     SPI_CLK_EN);
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
+	retval = rtsx_write_register(chip, CARD_OE, SPI_OUTPUT_EN,
+				     SPI_OUTPUT_EN);
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
 
 	wait_timeout(10);
 
-	RTSX_WRITE_REG(chip, SPI_CONTROL, 0xFF,
-		CS_POLARITY_HIGH | SPI_EEPROM_AUTO);
-	RTSX_WRITE_REG(chip, SPI_TCTL, EDO_TIMING_MASK, SAMPLE_DELAY_HALF);
+	retval = rtsx_write_register(chip, SPI_CONTROL, 0xFF,
+				     CS_POLARITY_HIGH | SPI_EEPROM_AUTO);
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
+	retval = rtsx_write_register(chip, SPI_TCTL, EDO_TIMING_MASK,
+				     SAMPLE_DELAY_HALF);
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
 
 	return STATUS_SUCCESS;
 }
@@ -258,8 +326,10 @@ static int spi_eeprom_program_enable(struct rtsx_chip *chip)
 		SPI_TRANSFER0_END);
 
 	retval = rtsx_send_cmd(chip, 0, 100);
-	if (retval < 0)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval < 0) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	return STATUS_SUCCESS;
 }
@@ -269,12 +339,16 @@ int spi_erase_eeprom_chip(struct rtsx_chip *chip)
 	int retval;
 
 	retval = spi_init_eeprom(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	retval = spi_eeprom_program_enable(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	rtsx_init_cmd(chip);
 
@@ -288,10 +362,16 @@ int spi_erase_eeprom_chip(struct rtsx_chip *chip)
 		SPI_TRANSFER0_END);
 
 	retval = rtsx_send_cmd(chip, 0, 100);
-	if (retval < 0)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval < 0) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
-	RTSX_WRITE_REG(chip, CARD_GPIO_DIR, 0x01, 0x01);
+	retval = rtsx_write_register(chip, CARD_GPIO_DIR, 0x01, 0x01);
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
 
 	return STATUS_SUCCESS;
 }
@@ -301,12 +381,16 @@ int spi_erase_eeprom_byte(struct rtsx_chip *chip, u16 addr)
 	int retval;
 
 	retval = spi_init_eeprom(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	retval = spi_eeprom_program_enable(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	rtsx_init_cmd(chip);
 
@@ -322,10 +406,16 @@ int spi_erase_eeprom_byte(struct rtsx_chip *chip, u16 addr)
 		SPI_TRANSFER0_END);
 
 	retval = rtsx_send_cmd(chip, 0, 100);
-	if (retval < 0)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval < 0) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
-	RTSX_WRITE_REG(chip, CARD_GPIO_DIR, 0x01, 0x01);
+	retval = rtsx_write_register(chip, CARD_GPIO_DIR, 0x01, 0x01);
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
 
 	return STATUS_SUCCESS;
 }
@@ -337,8 +427,10 @@ int spi_read_eeprom(struct rtsx_chip *chip, u16 addr, u8 *val)
 	u8 data;
 
 	retval = spi_init_eeprom(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	rtsx_init_cmd(chip);
 
@@ -355,16 +447,26 @@ int spi_read_eeprom(struct rtsx_chip *chip, u16 addr, u8 *val)
 		SPI_TRANSFER0_END);
 
 	retval = rtsx_send_cmd(chip, 0, 100);
-	if (retval < 0)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval < 0) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	wait_timeout(5);
-	RTSX_READ_REG(chip, SPI_DATA, &data);
+	retval = rtsx_read_register(chip, SPI_DATA, &data);
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
 
 	if (val)
 		*val = data;
 
-	RTSX_WRITE_REG(chip, CARD_GPIO_DIR, 0x01, 0x01);
+	retval = rtsx_write_register(chip, CARD_GPIO_DIR, 0x01, 0x01);
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
 
 	return STATUS_SUCCESS;
 }
@@ -374,12 +476,16 @@ int spi_write_eeprom(struct rtsx_chip *chip, u16 addr, u8 val)
 	int retval;
 
 	retval = spi_init_eeprom(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	retval = spi_eeprom_program_enable(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	rtsx_init_cmd(chip);
 
@@ -396,10 +502,16 @@ int spi_write_eeprom(struct rtsx_chip *chip, u16 addr, u8 val)
 		SPI_TRANSFER0_END);
 
 	retval = rtsx_send_cmd(chip, 0, 100);
-	if (retval < 0)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval < 0) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
-	RTSX_WRITE_REG(chip, CARD_GPIO_DIR, 0x01, 0x01);
+	retval = rtsx_write_register(chip, CARD_GPIO_DIR, 0x01, 0x01);
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
 
 	return STATUS_SUCCESS;
 }
@@ -449,13 +561,15 @@ int spi_read_flash_id(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 	len = ((u16)(srb->cmnd[7]) << 8) | srb->cmnd[8];
 	if (len > 512) {
 		spi_set_err_code(chip, SPI_INVALID_COMMAND);
-		TRACE_RET(chip, STATUS_FAIL);
+		rtsx_trace(chip);
+		return STATUS_FAIL;
 	}
 
 	retval = spi_set_init_para(chip);
 	if (retval != STATUS_SUCCESS) {
 		spi_set_err_code(chip, SPI_HW_ERR);
-		TRACE_RET(chip, STATUS_FAIL);
+		rtsx_trace(chip);
+		return STATUS_FAIL;
 	}
 
 	rtsx_init_cmd(chip);
@@ -497,19 +611,23 @@ int spi_read_flash_id(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 	if (retval < 0) {
 		rtsx_clear_spi_error(chip);
 		spi_set_err_code(chip, SPI_HW_ERR);
-		TRACE_RET(chip, STATUS_FAIL);
+		rtsx_trace(chip);
+		return STATUS_FAIL;
 	}
 
 	if (len) {
 		buf = kmalloc(len, GFP_KERNEL);
-		if (!buf)
-			TRACE_RET(chip, STATUS_ERROR);
+		if (!buf) {
+			rtsx_trace(chip);
+			return STATUS_ERROR;
+		}
 
 		retval = rtsx_read_ppbuf(chip, buf, len);
 		if (retval != STATUS_SUCCESS) {
 			spi_set_err_code(chip, SPI_READ_ERR);
 			kfree(buf);
-			TRACE_RET(chip, STATUS_FAIL);
+			rtsx_trace(chip);
+			return STATUS_FAIL;
 		}
 
 		rtsx_stor_set_xfer_buf(buf, scsi_bufflen(srb), srb);
@@ -541,12 +659,15 @@ int spi_read_flash(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 	retval = spi_set_init_para(chip);
 	if (retval != STATUS_SUCCESS) {
 		spi_set_err_code(chip, SPI_HW_ERR);
-		TRACE_RET(chip, STATUS_FAIL);
+		rtsx_trace(chip);
+		return STATUS_FAIL;
 	}
 
 	buf = kmalloc(SF_PAGE_LEN, GFP_KERNEL);
-	if (buf == NULL)
-		TRACE_RET(chip, STATUS_ERROR);
+	if (buf == NULL) {
+		rtsx_trace(chip);
+		return STATUS_ERROR;
+	}
 
 	while (len) {
 		u16 pagelen = SF_PAGE_LEN - (u8)addr;
@@ -598,7 +719,8 @@ int spi_read_flash(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 			kfree(buf);
 			rtsx_clear_spi_error(chip);
 			spi_set_err_code(chip, SPI_HW_ERR);
-			TRACE_RET(chip, STATUS_FAIL);
+			rtsx_trace(chip);
+			return STATUS_FAIL;
 		}
 
 		rtsx_stor_access_xfer_buf(buf, pagelen, srb, &index, &offset,
@@ -634,19 +756,23 @@ int spi_write_flash(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 	retval = spi_set_init_para(chip);
 	if (retval != STATUS_SUCCESS) {
 		spi_set_err_code(chip, SPI_HW_ERR);
-		TRACE_RET(chip, STATUS_FAIL);
+		rtsx_trace(chip);
+		return STATUS_FAIL;
 	}
 
 	if (program_mode == BYTE_PROGRAM) {
 		buf = kmalloc(4, GFP_KERNEL);
-		if (!buf)
-			TRACE_RET(chip, STATUS_ERROR);
+		if (!buf) {
+			rtsx_trace(chip);
+			return STATUS_ERROR;
+		}
 
 		while (len) {
 			retval = sf_enable_write(chip, SPI_WREN);
 			if (retval != STATUS_SUCCESS) {
 				kfree(buf);
-				TRACE_RET(chip, STATUS_FAIL);
+				rtsx_trace(chip);
+				return STATUS_FAIL;
 			}
 
 			rtsx_stor_access_xfer_buf(buf, 1, srb, &index, &offset,
@@ -665,13 +791,15 @@ int spi_write_flash(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 				kfree(buf);
 				rtsx_clear_spi_error(chip);
 				spi_set_err_code(chip, SPI_HW_ERR);
-				TRACE_RET(chip, STATUS_FAIL);
+				rtsx_trace(chip);
+				return STATUS_FAIL;
 			}
 
 			retval = sf_polling_status(chip, 100);
 			if (retval != STATUS_SUCCESS) {
 				kfree(buf);
-				TRACE_RET(chip, STATUS_FAIL);
+				rtsx_trace(chip);
+				return STATUS_FAIL;
 			}
 
 			addr++;
@@ -684,12 +812,16 @@ int spi_write_flash(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 		int first_byte = 1;
 
 		retval = sf_enable_write(chip, SPI_WREN);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, STATUS_FAIL);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 
 		buf = kmalloc(4, GFP_KERNEL);
-		if (!buf)
-			TRACE_RET(chip, STATUS_ERROR);
+		if (!buf) {
+			rtsx_trace(chip);
+			return STATUS_ERROR;
+		}
 
 		while (len) {
 			rtsx_stor_access_xfer_buf(buf, 1, srb, &index, &offset,
@@ -713,13 +845,15 @@ int spi_write_flash(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 				kfree(buf);
 				rtsx_clear_spi_error(chip);
 				spi_set_err_code(chip, SPI_HW_ERR);
-				TRACE_RET(chip, STATUS_FAIL);
+				rtsx_trace(chip);
+				return STATUS_FAIL;
 			}
 
 			retval = sf_polling_status(chip, 100);
 			if (retval != STATUS_SUCCESS) {
 				kfree(buf);
-				TRACE_RET(chip, STATUS_FAIL);
+				rtsx_trace(chip);
+				return STATUS_FAIL;
 			}
 
 			len--;
@@ -728,16 +862,22 @@ int spi_write_flash(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 		kfree(buf);
 
 		retval = sf_disable_write(chip, SPI_WRDI);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, STATUS_FAIL);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 
 		retval = sf_polling_status(chip, 100);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, STATUS_FAIL);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 	} else if (program_mode == PAGE_PROGRAM) {
 		buf = kmalloc(SF_PAGE_LEN, GFP_KERNEL);
-		if (!buf)
-			TRACE_RET(chip, STATUS_NOMEM);
+		if (!buf) {
+			rtsx_trace(chip);
+			return STATUS_NOMEM;
+		}
 
 		while (len) {
 			u16 pagelen = SF_PAGE_LEN - (u8)addr;
@@ -748,7 +888,8 @@ int spi_write_flash(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 			retval = sf_enable_write(chip, SPI_WREN);
 			if (retval != STATUS_SUCCESS) {
 				kfree(buf);
-				TRACE_RET(chip, STATUS_FAIL);
+				rtsx_trace(chip);
+				return STATUS_FAIL;
 			}
 
 			rtsx_init_cmd(chip);
@@ -767,13 +908,15 @@ int spi_write_flash(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 				kfree(buf);
 				rtsx_clear_spi_error(chip);
 				spi_set_err_code(chip, SPI_HW_ERR);
-				TRACE_RET(chip, STATUS_FAIL);
+				rtsx_trace(chip);
+				return STATUS_FAIL;
 			}
 
 			retval = sf_polling_status(chip, 100);
 			if (retval != STATUS_SUCCESS) {
 				kfree(buf);
-				TRACE_RET(chip, STATUS_FAIL);
+				rtsx_trace(chip);
+				return STATUS_FAIL;
 			}
 
 			addr += pagelen;
@@ -783,7 +926,8 @@ int spi_write_flash(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 		kfree(buf);
 	} else {
 		spi_set_err_code(chip, SPI_INVALID_COMMAND);
-		TRACE_RET(chip, STATUS_FAIL);
+		rtsx_trace(chip);
+		return STATUS_FAIL;
 	}
 
 	return STATUS_SUCCESS;
@@ -805,28 +949,38 @@ int spi_erase_flash(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 	retval = spi_set_init_para(chip);
 	if (retval != STATUS_SUCCESS) {
 		spi_set_err_code(chip, SPI_HW_ERR);
-		TRACE_RET(chip, STATUS_FAIL);
+		rtsx_trace(chip);
+		return STATUS_FAIL;
 	}
 
 	if (erase_mode == PAGE_ERASE) {
 		retval = sf_enable_write(chip, SPI_WREN);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, STATUS_FAIL);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 
 		retval = sf_erase(chip, ins, 1, addr);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, STATUS_FAIL);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 	} else if (erase_mode == CHIP_ERASE) {
 		retval = sf_enable_write(chip, SPI_WREN);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, STATUS_FAIL);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 
 		retval = sf_erase(chip, ins, 0, 0);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, STATUS_FAIL);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 	} else {
 		spi_set_err_code(chip, SPI_INVALID_COMMAND);
-		TRACE_RET(chip, STATUS_FAIL);
+		rtsx_trace(chip);
+		return STATUS_FAIL;
 	}
 
 	return STATUS_SUCCESS;
@@ -844,12 +998,15 @@ int spi_write_flash_status(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 	retval = spi_set_init_para(chip);
 	if (retval != STATUS_SUCCESS) {
 		spi_set_err_code(chip, SPI_HW_ERR);
-		TRACE_RET(chip, STATUS_FAIL);
+		rtsx_trace(chip);
+		return STATUS_FAIL;
 	}
 
 	retval = sf_enable_write(chip, ewsr);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	rtsx_init_cmd(chip);
 
@@ -871,7 +1028,8 @@ int spi_write_flash_status(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 	if (retval != STATUS_SUCCESS) {
 		rtsx_clear_spi_error(chip);
 		spi_set_err_code(chip, SPI_HW_ERR);
-		TRACE_RET(chip, STATUS_FAIL);
+		rtsx_trace(chip);
+		return STATUS_FAIL;
 	}
 
 	return STATUS_SUCCESS;
diff --git a/drivers/staging/rts5208/trace.c b/drivers/staging/rts5208/trace.c
new file mode 100644
index 000000000000..1bddbdf3454b
--- /dev/null
+++ b/drivers/staging/rts5208/trace.c
@@ -0,0 +1,26 @@
+#include <linux/kernel.h>
+#include <linux/string.h>
+
+#include "rtsx.h"
+
+#ifdef _MSG_TRACE
+
+void _rtsx_trace(struct rtsx_chip *chip, const char *file, const char *func,
+		 int line)
+{
+	struct trace_msg_t *msg = &chip->trace_msg[chip->msg_idx];
+
+	file = kbasename(file);
+	dev_dbg(rtsx_dev(chip), "[%s][%s]:[%d]\n", file, func, line);
+
+	strncpy(msg->file, file, MSG_FILE_LEN - 1);
+	strncpy(msg->func, func, MSG_FUNC_LEN - 1);
+	msg->line = (u16)line;
+	get_current_time(msg->timeval_buf, TIME_VAL_LEN);
+	msg->valid = 1;
+
+	chip->msg_idx++;
+	if (chip->msg_idx >= TRACE_ITEM_CNT)
+		chip->msg_idx = 0;
+}
+#endif
diff --git a/drivers/staging/rts5208/trace.h b/drivers/staging/rts5208/trace.h
index a9ab4077b283..5b807874c1d7 100644
--- a/drivers/staging/rts5208/trace.h
+++ b/drivers/staging/rts5208/trace.h
@@ -24,64 +24,17 @@
 #ifndef __REALTEK_RTSX_TRACE_H
 #define __REALTEK_RTSX_TRACE_H
 
-#define _MSG_TRACE
+struct rtsx_chip;
 
 #ifdef _MSG_TRACE
-static inline char *filename(char *path)
+void _rtsx_trace(struct rtsx_chip *chip, const char *file, const char *func,
+		 int line);
+#define rtsx_trace(chip)						\
+	_rtsx_trace(chip, __FILE__, __func__, __LINE__)
+#else
+static inline void rtsx_trace(struct rtsx_chip *chip)
 {
-	char *ptr;
-
-	if (path == NULL)
-		return NULL;
-
-	ptr = path;
-
-	while (*ptr != '\0') {
-		if ((*ptr == '\\') || (*ptr == '/'))
-			path = ptr + 1;
-
-		ptr++;
-	}
-
-	return path;
 }
-
-#define TRACE_RET(chip, ret)						\
-	do {								\
-		char *_file = filename(__FILE__);			\
-		dev_dbg(rtsx_dev(chip), "[%s][%s]:[%d]\n", _file,	\
-			__func__, __LINE__);				\
-		(chip)->trace_msg[(chip)->msg_idx].line = (u16)(__LINE__); \
-		strncpy((chip)->trace_msg[(chip)->msg_idx].func, __func__, MSG_FUNC_LEN-1); \
-		strncpy((chip)->trace_msg[(chip)->msg_idx].file, _file, MSG_FILE_LEN-1); \
-		get_current_time((chip)->trace_msg[(chip)->msg_idx].timeval_buf, TIME_VAL_LEN);	\
-		(chip)->trace_msg[(chip)->msg_idx].valid = 1;		\
-		(chip)->msg_idx++;					\
-		if ((chip)->msg_idx >= TRACE_ITEM_CNT) {		\
-			(chip)->msg_idx = 0;				\
-		}							\
-		return ret;						\
-	} while (0)
-
-#define TRACE_GOTO(chip, label)						\
-	do {								\
-		char *_file = filename(__FILE__);			\
-		dev_dbg(rtsx_dev(chip), "[%s][%s]:[%d]\n", _file,	\
-			__func__, __LINE__);				\
-		(chip)->trace_msg[(chip)->msg_idx].line = (u16)(__LINE__); \
-		strncpy((chip)->trace_msg[(chip)->msg_idx].func, __func__, MSG_FUNC_LEN-1); \
-		strncpy((chip)->trace_msg[(chip)->msg_idx].file, _file, MSG_FILE_LEN-1); \
-		get_current_time((chip)->trace_msg[(chip)->msg_idx].timeval_buf, TIME_VAL_LEN);	\
-		(chip)->trace_msg[(chip)->msg_idx].valid = 1;		\
-		(chip)->msg_idx++;					\
-		if ((chip)->msg_idx >= TRACE_ITEM_CNT) {		\
-			(chip)->msg_idx = 0;				\
-		}							\
-		goto label;						\
-	} while (0)
-#else
-#define TRACE_RET(chip, ret)	return ret
-#define TRACE_GOTO(chip, label)	goto label
 #endif
 
 #endif  /* __REALTEK_RTSX_TRACE_H */
diff --git a/drivers/staging/rts5208/xd.c b/drivers/staging/rts5208/xd.c
index 0d029fe92b40..8fd108e50509 100644
--- a/drivers/staging/rts5208/xd.c
+++ b/drivers/staging/rts5208/xd.c
@@ -60,8 +60,10 @@ static int xd_set_init_para(struct rtsx_chip *chip)
 		xd_card->xd_clock = CLK_50;
 
 	retval = switch_clock(chip, xd_card->xd_clock);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	return STATUS_SUCCESS;
 }
@@ -72,12 +74,16 @@ static int xd_switch_clock(struct rtsx_chip *chip)
 	int retval;
 
 	retval = select_card(chip, XD_CARD);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	retval = switch_clock(chip, xd_card->xd_clock);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	return STATUS_SUCCESS;
 }
@@ -99,8 +105,10 @@ static int xd_read_id(struct rtsx_chip *chip, u8 id_cmd, u8 *id_buf, u8 buf_len)
 		rtsx_add_cmd(chip, READ_REG_CMD, (u16)(XD_ADDRESS1 + i), 0, 0);
 
 	retval = rtsx_send_cmd(chip, XD_CARD, 20);
-	if (retval < 0)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval < 0) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	ptr = rtsx_get_cmd_data(chip) + 1;
 	if (id_buf && buf_len) {
@@ -167,8 +175,10 @@ static int xd_read_redundant(struct rtsx_chip *chip, u32 page_addr,
 	rtsx_add_cmd(chip, READ_REG_CMD, XD_PARITY, 0, 0);
 
 	retval = rtsx_send_cmd(chip, XD_CARD, 500);
-	if (retval < 0)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval < 0) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	if (buf && buf_len) {
 		u8 *ptr = rtsx_get_cmd_data(chip) + 1;
@@ -186,8 +196,10 @@ static int xd_read_data_from_ppb(struct rtsx_chip *chip, int offset,
 {
 	int retval, i;
 
-	if (!buf || (buf_len < 0))
-		TRACE_RET(chip, STATUS_FAIL);
+	if (!buf || (buf_len < 0)) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	rtsx_init_cmd(chip);
 
@@ -198,7 +210,8 @@ static int xd_read_data_from_ppb(struct rtsx_chip *chip, int offset,
 	retval = rtsx_send_cmd(chip, 0, 250);
 	if (retval < 0) {
 		rtsx_clear_xd_error(chip);
-		TRACE_RET(chip, STATUS_FAIL);
+		rtsx_trace(chip);
+		return STATUS_FAIL;
 	}
 
 	memcpy(buf, rtsx_get_cmd_data(chip), buf_len);
@@ -212,8 +225,10 @@ static int xd_read_cis(struct rtsx_chip *chip, u32 page_addr, u8 *buf,
 	int retval;
 	u8 reg;
 
-	if (!buf || (buf_len < 10))
-		TRACE_RET(chip, STATUS_FAIL);
+	if (!buf || (buf_len < 10)) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	rtsx_init_cmd(chip);
 
@@ -233,25 +248,47 @@ static int xd_read_cis(struct rtsx_chip *chip, u32 page_addr, u8 *buf,
 	retval = rtsx_send_cmd(chip, XD_CARD, 250);
 	if (retval == -ETIMEDOUT) {
 		rtsx_clear_xd_error(chip);
-		TRACE_RET(chip, STATUS_FAIL);
+		rtsx_trace(chip);
+		return STATUS_FAIL;
 	}
 
-	RTSX_READ_REG(chip, XD_PAGE_STATUS, &reg);
+	retval = rtsx_read_register(chip, XD_PAGE_STATUS, &reg);
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
 	if (reg != XD_GPG) {
 		rtsx_clear_xd_error(chip);
-		TRACE_RET(chip, STATUS_FAIL);
+		rtsx_trace(chip);
+		return STATUS_FAIL;
 	}
 
-	RTSX_READ_REG(chip, XD_CTL, &reg);
+	retval = rtsx_read_register(chip, XD_CTL, &reg);
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
 	if (!(reg & XD_ECC1_ERROR) || !(reg & XD_ECC1_UNCORRECTABLE)) {
 		retval = xd_read_data_from_ppb(chip, 0, buf, buf_len);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, STATUS_FAIL);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 		if (reg & XD_ECC1_ERROR) {
 			u8 ecc_bit, ecc_byte;
 
-			RTSX_READ_REG(chip, XD_ECC_BIT1, &ecc_bit);
-			RTSX_READ_REG(chip, XD_ECC_BYTE1, &ecc_byte);
+			retval = rtsx_read_register(chip, XD_ECC_BIT1,
+						    &ecc_bit);
+			if (retval) {
+				rtsx_trace(chip);
+				return retval;
+			}
+			retval = rtsx_read_register(chip, XD_ECC_BYTE1,
+						    &ecc_byte);
+			if (retval) {
+				rtsx_trace(chip);
+				return retval;
+			}
 
 			dev_dbg(rtsx_dev(chip), "ECC_BIT1 = 0x%x, ECC_BYTE1 = 0x%x\n",
 				ecc_bit, ecc_byte);
@@ -267,13 +304,25 @@ static int xd_read_cis(struct rtsx_chip *chip, u32 page_addr, u8 *buf,
 		rtsx_clear_xd_error(chip);
 
 		retval = xd_read_data_from_ppb(chip, 256, buf, buf_len);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, STATUS_FAIL);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 		if (reg & XD_ECC2_ERROR) {
 			u8 ecc_bit, ecc_byte;
 
-			RTSX_READ_REG(chip, XD_ECC_BIT2, &ecc_bit);
-			RTSX_READ_REG(chip, XD_ECC_BYTE2, &ecc_byte);
+			retval = rtsx_read_register(chip, XD_ECC_BIT2,
+						    &ecc_bit);
+			if (retval) {
+				rtsx_trace(chip);
+				return retval;
+			}
+			retval = rtsx_read_register(chip, XD_ECC_BYTE2,
+						    &ecc_byte);
+			if (retval) {
+				rtsx_trace(chip);
+				return retval;
+			}
 
 			dev_dbg(rtsx_dev(chip), "ECC_BIT2 = 0x%x, ECC_BYTE2 = 0x%x\n",
 				ecc_bit, ecc_byte);
@@ -287,7 +336,8 @@ static int xd_read_cis(struct rtsx_chip *chip, u32 page_addr, u8 *buf,
 		}
 	} else {
 		rtsx_clear_xd_error(chip);
-		TRACE_RET(chip, STATUS_FAIL);
+		rtsx_trace(chip);
+		return STATUS_FAIL;
 	}
 
 	return STATUS_SUCCESS;
@@ -363,24 +413,71 @@ static void xd_fill_pull_ctl_enable(struct rtsx_chip *chip)
 
 static int xd_pull_ctl_disable(struct rtsx_chip *chip)
 {
+	int retval;
+
 	if (CHECK_PID(chip, 0x5208)) {
-		RTSX_WRITE_REG(chip, CARD_PULL_CTL1, 0xFF,
-			XD_D3_PD | XD_D2_PD | XD_D1_PD | XD_D0_PD);
-		RTSX_WRITE_REG(chip, CARD_PULL_CTL2, 0xFF,
-			XD_D7_PD | XD_D6_PD | XD_D5_PD | XD_D4_PD);
-		RTSX_WRITE_REG(chip, CARD_PULL_CTL3, 0xFF,
-			XD_WP_PD | XD_CE_PD | XD_CLE_PD | XD_CD_PU);
-		RTSX_WRITE_REG(chip, CARD_PULL_CTL4, 0xFF,
-			XD_RDY_PD | XD_WE_PD | XD_RE_PD | XD_ALE_PD);
-		RTSX_WRITE_REG(chip, CARD_PULL_CTL5, 0xFF,
-			MS_INS_PU | SD_WP_PD | SD_CD_PU | SD_CMD_PD);
-		RTSX_WRITE_REG(chip, CARD_PULL_CTL6, 0xFF, MS_D5_PD | MS_D4_PD);
+		retval = rtsx_write_register(chip, CARD_PULL_CTL1, 0xFF,
+					     XD_D3_PD | XD_D2_PD | XD_D1_PD | XD_D0_PD);
+		if (retval) {
+			rtsx_trace(chip);
+			return retval;
+		}
+		retval = rtsx_write_register(chip, CARD_PULL_CTL2, 0xFF,
+					     XD_D7_PD | XD_D6_PD | XD_D5_PD | XD_D4_PD);
+		if (retval) {
+			rtsx_trace(chip);
+			return retval;
+		}
+		retval = rtsx_write_register(chip, CARD_PULL_CTL3, 0xFF,
+					     XD_WP_PD | XD_CE_PD | XD_CLE_PD | XD_CD_PU);
+		if (retval) {
+			rtsx_trace(chip);
+			return retval;
+		}
+		retval = rtsx_write_register(chip, CARD_PULL_CTL4, 0xFF,
+					     XD_RDY_PD | XD_WE_PD | XD_RE_PD | XD_ALE_PD);
+		if (retval) {
+			rtsx_trace(chip);
+			return retval;
+		}
+		retval = rtsx_write_register(chip, CARD_PULL_CTL5, 0xFF,
+					     MS_INS_PU | SD_WP_PD | SD_CD_PU | SD_CMD_PD);
+		if (retval) {
+			rtsx_trace(chip);
+			return retval;
+		}
+		retval = rtsx_write_register(chip, CARD_PULL_CTL6, 0xFF,
+					     MS_D5_PD | MS_D4_PD);
+		if (retval) {
+			rtsx_trace(chip);
+			return retval;
+		}
 	} else if (CHECK_PID(chip, 0x5288)) {
 		if (CHECK_BARO_PKG(chip, QFN)) {
-			RTSX_WRITE_REG(chip, CARD_PULL_CTL1, 0xFF, 0x55);
-			RTSX_WRITE_REG(chip, CARD_PULL_CTL2, 0xFF, 0x55);
-			RTSX_WRITE_REG(chip, CARD_PULL_CTL3, 0xFF, 0x4B);
-			RTSX_WRITE_REG(chip, CARD_PULL_CTL4, 0xFF, 0x69);
+			retval = rtsx_write_register(chip, CARD_PULL_CTL1,
+						     0xFF, 0x55);
+			if (retval) {
+				rtsx_trace(chip);
+				return retval;
+			}
+			retval = rtsx_write_register(chip, CARD_PULL_CTL2,
+						     0xFF, 0x55);
+			if (retval) {
+				rtsx_trace(chip);
+				return retval;
+			}
+			retval = rtsx_write_register(chip, CARD_PULL_CTL3,
+						     0xFF, 0x4B);
+			if (retval) {
+				rtsx_trace(chip);
+				return retval;
+			}
+			retval = rtsx_write_register(chip, CARD_PULL_CTL4,
+						     0xFF, 0x69);
+			if (retval) {
+				rtsx_trace(chip);
+				return retval;
+			}
 		}
 	}
 
@@ -394,8 +491,10 @@ static int reset_xd(struct rtsx_chip *chip)
 	u8 *ptr, id_buf[4], redunt[11];
 
 	retval = select_card(chip, XD_CARD);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	rtsx_init_cmd(chip);
 
@@ -418,13 +517,17 @@ static int reset_xd(struct rtsx_chip *chip)
 	rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_OE, XD_OUTPUT_EN, 0);
 
 	retval = rtsx_send_cmd(chip, XD_CARD, 100);
-	if (retval < 0)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval < 0) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	if (!chip->ft2_fast_mode) {
 		retval = card_power_off(chip, XD_CARD);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, STATUS_FAIL);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 
 		wait_timeout(250);
 
@@ -439,19 +542,24 @@ static int reset_xd(struct rtsx_chip *chip)
 		}
 
 		retval = rtsx_send_cmd(chip, XD_CARD, 100);
-		if (retval < 0)
-			TRACE_RET(chip, STATUS_FAIL);
+		if (retval < 0) {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 
 		retval = card_power_on(chip, XD_CARD);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, STATUS_FAIL);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 
 #ifdef SUPPORT_OCP
 		wait_timeout(50);
 		if (chip->ocp_stat & (SD_OC_NOW | SD_OC_EVER)) {
 			dev_dbg(rtsx_dev(chip), "Over current, OCPSTAT is 0x%x\n",
 				chip->ocp_stat);
-			TRACE_RET(chip, STATUS_FAIL);
+			rtsx_trace(chip);
+			return STATUS_FAIL;
 		}
 #endif
 	}
@@ -472,15 +580,19 @@ static int reset_xd(struct rtsx_chip *chip)
 	rtsx_add_cmd(chip, WRITE_REG_CMD, XD_CTL, XD_CE_DISEN, XD_CE_DISEN);
 
 	retval = rtsx_send_cmd(chip, XD_CARD, 100);
-	if (retval < 0)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval < 0) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	if (!chip->ft2_fast_mode)
 		wait_timeout(200);
 
 	retval = xd_set_init_para(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	/* Read ID to check if the timing setting is right */
 	for (i = 0; i < 4; i++) {
@@ -502,8 +614,10 @@ static int reset_xd(struct rtsx_chip *chip)
 		rtsx_add_cmd(chip, READ_REG_CMD, XD_CTL, 0, 0);
 
 		retval = rtsx_send_cmd(chip, XD_CARD, 100);
-		if (retval < 0)
-			TRACE_RET(chip, STATUS_FAIL);
+		if (retval < 0) {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 
 		ptr = rtsx_get_cmd_data(chip) + 1;
 
@@ -515,8 +629,10 @@ static int reset_xd(struct rtsx_chip *chip)
 			continue;
 
 		retval = xd_read_id(chip, READ_ID, id_buf, 4);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, STATUS_FAIL);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 
 		dev_dbg(rtsx_dev(chip), "READ_ID: 0x%x 0x%x 0x%x 0x%x\n",
 			id_buf[0], id_buf[1], id_buf[2], id_buf[3]);
@@ -596,8 +712,10 @@ static int reset_xd(struct rtsx_chip *chip)
 		/* Confirm timing setting */
 		for (j = 0; j < 10; j++) {
 			retval = xd_read_id(chip, READ_ID, id_buf, 4);
-			if (retval != STATUS_SUCCESS)
-				TRACE_RET(chip, STATUS_FAIL);
+			if (retval != STATUS_SUCCESS) {
+				rtsx_trace(chip);
+				return STATUS_FAIL;
+			}
 
 			if (id_buf[1] != xd_card->device_code)
 				break;
@@ -613,23 +731,30 @@ static int reset_xd(struct rtsx_chip *chip)
 		xd_card->addr_cycle = 0;
 		xd_card->capacity = 0;
 
-		TRACE_RET(chip, STATUS_FAIL);
+		rtsx_trace(chip);
+		return STATUS_FAIL;
 	}
 
 	retval = xd_read_id(chip, READ_xD_ID, id_buf, 4);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 	dev_dbg(rtsx_dev(chip), "READ_xD_ID: 0x%x 0x%x 0x%x 0x%x\n",
 		id_buf[0], id_buf[1], id_buf[2], id_buf[3]);
-	if (id_buf[2] != XD_ID_CODE)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (id_buf[2] != XD_ID_CODE) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	/* Search CIS block */
 	for (i = 0; i < 24; i++) {
 		u32 page_addr;
 
-		if (detect_card_cd(chip, XD_CARD) != STATUS_SUCCESS)
-			TRACE_RET(chip, STATUS_FAIL);
+		if (detect_card_cd(chip, XD_CARD) != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 
 		page_addr = (u32)i << xd_card->block_shift;
 
@@ -667,8 +792,10 @@ static int reset_xd(struct rtsx_chip *chip)
 			page_addr += j;
 
 			retval = xd_read_cis(chip, page_addr, buf, 10);
-			if (retval != STATUS_SUCCESS)
-				TRACE_RET(chip, STATUS_FAIL);
+			if (retval != STATUS_SUCCESS) {
+				rtsx_trace(chip);
+				return STATUS_FAIL;
+			}
 
 			if ((buf[0] == 0x01) && (buf[1] == 0x03) &&
 				(buf[2] == 0xD9)
@@ -684,8 +811,10 @@ static int reset_xd(struct rtsx_chip *chip)
 	}
 
 	dev_dbg(rtsx_dev(chip), "CIS block: 0x%x\n", xd_card->cis_block);
-	if (xd_card->cis_block == 0xFFFF)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (xd_card->cis_block == 0xFFFF) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	chip->capacity[chip->card2lun[XD_CARD]] = xd_card->capacity;
 
@@ -739,15 +868,19 @@ static int xd_init_l2p_tbl(struct rtsx_chip *chip)
 	dev_dbg(rtsx_dev(chip), "xd_init_l2p_tbl: zone_cnt = %d\n",
 		xd_card->zone_cnt);
 
-	if (xd_card->zone_cnt < 1)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (xd_card->zone_cnt < 1) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	size = xd_card->zone_cnt * sizeof(struct zone_entry);
 	dev_dbg(rtsx_dev(chip), "Buffer size for l2p table is %d\n", size);
 
 	xd_card->zone = vmalloc(size);
-	if (!xd_card->zone)
-		TRACE_RET(chip, STATUS_ERROR);
+	if (!xd_card->zone) {
+		rtsx_trace(chip);
+		return STATUS_ERROR;
+	}
 
 	for (i = 0; i < xd_card->zone_cnt; i++) {
 		xd_card->zone[i].build_flag = 0;
@@ -927,16 +1060,22 @@ int reset_xd_card(struct rtsx_chip *chip)
 	xd_card->delay_write.delay_write_flag = 0;
 
 	retval = enable_card_clock(chip, XD_CARD);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	retval = reset_xd(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	retval = xd_init_l2p_tbl(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	return STATUS_SUCCESS;
 }
@@ -950,8 +1089,10 @@ static int xd_mark_bad_block(struct rtsx_chip *chip, u32 phy_blk)
 
 	dev_dbg(rtsx_dev(chip), "mark block 0x%x as bad block\n", phy_blk);
 
-	if (phy_blk == BLK_NOT_FOUND)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (phy_blk == BLK_NOT_FOUND) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	rtsx_init_cmd(chip);
 
@@ -986,7 +1127,8 @@ static int xd_mark_bad_block(struct rtsx_chip *chip, u32 phy_blk)
 			xd_set_err_code(chip, XD_PRG_ERROR);
 		else
 			xd_set_err_code(chip, XD_TO_ERROR);
-		TRACE_RET(chip, STATUS_FAIL);
+		rtsx_trace(chip);
+		return STATUS_FAIL;
 	}
 
 	return STATUS_SUCCESS;
@@ -1002,10 +1144,14 @@ static int xd_init_page(struct rtsx_chip *chip, u32 phy_blk,
 
 	dev_dbg(rtsx_dev(chip), "Init block 0x%x\n", phy_blk);
 
-	if (start_page > end_page)
-		TRACE_RET(chip, STATUS_FAIL);
-	if (phy_blk == BLK_NOT_FOUND)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (start_page > end_page) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
+	if (phy_blk == BLK_NOT_FOUND) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	rtsx_init_cmd(chip);
 
@@ -1040,7 +1186,8 @@ static int xd_init_page(struct rtsx_chip *chip, u32 phy_blk,
 		} else {
 			xd_set_err_code(chip, XD_TO_ERROR);
 		}
-		TRACE_RET(chip, STATUS_FAIL);
+		rtsx_trace(chip);
+		return STATUS_FAIL;
 	}
 
 	return STATUS_SUCCESS;
@@ -1057,24 +1204,34 @@ static int xd_copy_page(struct rtsx_chip *chip, u32 old_blk, u32 new_blk,
 	dev_dbg(rtsx_dev(chip), "Copy page from block 0x%x to block 0x%x\n",
 		old_blk, new_blk);
 
-	if (start_page > end_page)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (start_page > end_page) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
-	if ((old_blk == BLK_NOT_FOUND) || (new_blk == BLK_NOT_FOUND))
-		TRACE_RET(chip, STATUS_FAIL);
+	if ((old_blk == BLK_NOT_FOUND) || (new_blk == BLK_NOT_FOUND)) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	old_page = (old_blk << xd_card->block_shift) + start_page;
 	new_page = (new_blk << xd_card->block_shift) + start_page;
 
 	XD_CLR_BAD_NEWBLK(xd_card);
 
-	RTSX_WRITE_REG(chip, CARD_DATA_SOURCE, 0x01, PINGPONG_BUFFER);
+	retval = rtsx_write_register(chip, CARD_DATA_SOURCE, 0x01,
+				     PINGPONG_BUFFER);
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
 
 	for (i = start_page; i < end_page; i++) {
 		if (detect_card_cd(chip, XD_CARD) != STATUS_SUCCESS) {
 			rtsx_clear_xd_error(chip);
 			xd_set_err_code(chip, XD_NO_CARD);
-			TRACE_RET(chip, STATUS_FAIL);
+			rtsx_trace(chip);
+			return STATUS_FAIL;
 		}
 
 		rtsx_init_cmd(chip);
@@ -1100,7 +1257,8 @@ static int xd_copy_page(struct rtsx_chip *chip, u32 old_blk, u32 new_blk,
 				if (detect_card_cd(chip,
 					XD_CARD) != STATUS_SUCCESS) {
 					xd_set_err_code(chip, XD_NO_CARD);
-					TRACE_RET(chip, STATUS_FAIL);
+					rtsx_trace(chip);
+					return STATUS_FAIL;
 				}
 
 				if (((reg & (XD_ECC1_ERROR | XD_ECC1_UNCORRECTABLE)) ==
@@ -1119,7 +1277,8 @@ static int xd_copy_page(struct rtsx_chip *chip, u32 old_blk, u32 new_blk,
 				}
 			} else {
 				xd_set_err_code(chip, XD_TO_ERROR);
-				TRACE_RET(chip, STATUS_FAIL);
+				rtsx_trace(chip);
+				return STATUS_FAIL;
 			}
 		}
 
@@ -1147,7 +1306,8 @@ static int xd_copy_page(struct rtsx_chip *chip, u32 old_blk, u32 new_blk,
 			} else {
 				xd_set_err_code(chip, XD_TO_ERROR);
 			}
-			TRACE_RET(chip, STATUS_FAIL);
+			rtsx_trace(chip);
+			return STATUS_FAIL;
 		}
 
 		old_page++;
@@ -1172,14 +1332,17 @@ static int xd_reset_cmd(struct rtsx_chip *chip)
 	rtsx_add_cmd(chip, READ_REG_CMD, XD_CTL, 0, 0);
 
 	retval = rtsx_send_cmd(chip, XD_CARD, 100);
-	if (retval < 0)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval < 0) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	ptr = rtsx_get_cmd_data(chip) + 1;
 	if (((ptr[0] & READY_FLAG) == READY_STATE) && (ptr[1] & XD_RDY))
 		return STATUS_SUCCESS;
 
-	TRACE_RET(chip, STATUS_FAIL);
+	rtsx_trace(chip);
+	return STATUS_FAIL;
 }
 
 static int xd_erase_block(struct rtsx_chip *chip, u32 phy_blk)
@@ -1189,8 +1352,10 @@ static int xd_erase_block(struct rtsx_chip *chip, u32 phy_blk)
 	u8 reg = 0, *ptr;
 	int i, retval;
 
-	if (phy_blk == BLK_NOT_FOUND)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (phy_blk == BLK_NOT_FOUND) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	page_addr = phy_blk << xd_card->block_shift;
 
@@ -1212,13 +1377,16 @@ static int xd_erase_block(struct rtsx_chip *chip, u32 phy_blk)
 			if (reg & PROGRAM_ERROR) {
 				xd_mark_bad_block(chip, phy_blk);
 				xd_set_err_code(chip, XD_PRG_ERROR);
-				TRACE_RET(chip, STATUS_FAIL);
+				rtsx_trace(chip);
+				return STATUS_FAIL;
 			} else {
 				xd_set_err_code(chip, XD_ERASE_FAIL);
 			}
 			retval = xd_reset_cmd(chip);
-			if (retval != STATUS_SUCCESS)
-				TRACE_RET(chip, STATUS_FAIL);
+			if (retval != STATUS_SUCCESS) {
+				rtsx_trace(chip);
+				return STATUS_FAIL;
+			}
 			continue;
 		}
 
@@ -1226,7 +1394,8 @@ static int xd_erase_block(struct rtsx_chip *chip, u32 phy_blk)
 		if (*ptr & PROGRAM_ERROR) {
 			xd_mark_bad_block(chip, phy_blk);
 			xd_set_err_code(chip, XD_PRG_ERROR);
-			TRACE_RET(chip, STATUS_FAIL);
+			rtsx_trace(chip);
+			return STATUS_FAIL;
 		}
 
 		return STATUS_SUCCESS;
@@ -1234,7 +1403,8 @@ static int xd_erase_block(struct rtsx_chip *chip, u32 phy_blk)
 
 	xd_mark_bad_block(chip, phy_blk);
 	xd_set_err_code(chip, XD_ERASE_FAIL);
-	TRACE_RET(chip, STATUS_FAIL);
+	rtsx_trace(chip);
+	return STATUS_FAIL;
 }
 
 
@@ -1266,15 +1436,19 @@ static int xd_build_l2p_tbl(struct rtsx_chip *chip, int zone_no)
 
 	if (zone->l2p_table == NULL) {
 		zone->l2p_table = vmalloc(2000);
-		if (zone->l2p_table == NULL)
-			TRACE_GOTO(chip, Build_Fail);
+		if (zone->l2p_table == NULL) {
+			rtsx_trace(chip);
+			goto Build_Fail;
+		}
 	}
 	memset((u8 *)(zone->l2p_table), 0xff, 2000);
 
 	if (zone->free_table == NULL) {
 		zone->free_table = vmalloc(XD_FREE_TABLE_CNT * 2);
-		if (zone->free_table == NULL)
-			TRACE_GOTO(chip, Build_Fail);
+		if (zone->free_table == NULL) {
+			rtsx_trace(chip);
+			goto Build_Fail;
+		}
 	}
 	memset((u8 *)(zone->free_table), 0xff, XD_FREE_TABLE_CNT * 2);
 
@@ -1440,8 +1614,10 @@ static int xd_send_cmd(struct rtsx_chip *chip, u8 cmd)
 		XD_TRANSFER_END, XD_TRANSFER_END);
 
 	retval = rtsx_send_cmd(chip, XD_CARD, 200);
-	if (retval < 0)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval < 0) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	return STATUS_SUCCESS;
 }
@@ -1457,8 +1633,10 @@ static int xd_read_multiple_pages(struct rtsx_chip *chip, u32 phy_blk,
 	u8 reg_val, page_cnt;
 	int zone_no, retval, i;
 
-	if (start_page > end_page)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (start_page > end_page) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	page_cnt = end_page - start_page;
 	zone_no = (int)(log_blk / 1000);
@@ -1474,7 +1652,8 @@ static int xd_read_multiple_pages(struct rtsx_chip *chip, u32 phy_blk,
 
 			if (detect_card_cd(chip, XD_CARD) != STATUS_SUCCESS) {
 				xd_set_err_code(chip, XD_NO_CARD);
-				TRACE_RET(chip, STATUS_FAIL);
+				rtsx_trace(chip);
+				return STATUS_FAIL;
 			}
 		}
 	}
@@ -1509,21 +1688,31 @@ static int xd_read_multiple_pages(struct rtsx_chip *chip, u32 phy_blk,
 
 		if (retval == -ETIMEDOUT) {
 			xd_set_err_code(chip, XD_TO_ERROR);
-			TRACE_RET(chip, STATUS_FAIL);
+			rtsx_trace(chip);
+			return STATUS_FAIL;
 		} else {
-			TRACE_GOTO(chip, Fail);
+			rtsx_trace(chip);
+			goto Fail;
 		}
 	}
 
 	return STATUS_SUCCESS;
 
 Fail:
-	RTSX_READ_REG(chip, XD_PAGE_STATUS, &reg_val);
+	retval = rtsx_read_register(chip, XD_PAGE_STATUS, &reg_val);
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
 
 	if (reg_val !=  XD_GPG)
 		xd_set_err_code(chip, XD_PRG_ERROR);
 
-	RTSX_READ_REG(chip, XD_CTL, &reg_val);
+	retval = rtsx_read_register(chip, XD_CTL, &reg_val);
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
 
 	if (((reg_val & (XD_ECC1_ERROR | XD_ECC1_UNCORRECTABLE))
 				== (XD_ECC1_ERROR | XD_ECC1_UNCORRECTABLE))
@@ -1533,7 +1722,8 @@ Fail:
 
 		if (detect_card_cd(chip, XD_CARD) != STATUS_SUCCESS) {
 			xd_set_err_code(chip, XD_NO_CARD);
-			TRACE_RET(chip, STATUS_FAIL);
+			rtsx_trace(chip);
+			return STATUS_FAIL;
 		}
 
 		xd_set_err_code(chip, XD_ECC_ERROR);
@@ -1541,7 +1731,8 @@ Fail:
 		new_blk = xd_get_unused_block(chip, zone_no);
 		if (new_blk == NO_NEW_BLK) {
 			XD_CLR_BAD_OLDBLK(xd_card);
-			TRACE_RET(chip, STATUS_FAIL);
+			rtsx_trace(chip);
+			return STATUS_FAIL;
 		}
 
 		retval = xd_copy_page(chip, phy_blk, new_blk, 0,
@@ -1555,7 +1746,8 @@ Fail:
 				XD_CLR_BAD_NEWBLK(xd_card);
 			}
 			XD_CLR_BAD_OLDBLK(xd_card);
-			TRACE_RET(chip, STATUS_FAIL);
+			rtsx_trace(chip);
+			return STATUS_FAIL;
 		}
 		xd_set_l2p_tbl(chip, zone_no, log_off, (u16)(new_blk & 0x3FF));
 		xd_erase_block(chip, phy_blk);
@@ -1563,7 +1755,8 @@ Fail:
 		XD_CLR_BAD_OLDBLK(xd_card);
 	}
 
-	TRACE_RET(chip, STATUS_FAIL);
+	rtsx_trace(chip);
+	return STATUS_FAIL;
 }
 
 static int xd_finish_write(struct rtsx_chip *chip,
@@ -1576,8 +1769,10 @@ static int xd_finish_write(struct rtsx_chip *chip,
 	dev_dbg(rtsx_dev(chip), "xd_finish_write, old_blk = 0x%x, new_blk = 0x%x, log_blk = 0x%x\n",
 		old_blk, new_blk, log_blk);
 
-	if (page_off > xd_card->page_off)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (page_off > xd_card->page_off) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	zone_no = (int)(log_blk / 1000);
 	log_off = (u16)(log_blk % 1000);
@@ -1589,7 +1784,8 @@ static int xd_finish_write(struct rtsx_chip *chip,
 			retval = xd_erase_block(chip, new_blk);
 			if (retval == STATUS_SUCCESS)
 				xd_set_unused_block(chip, new_blk);
-			TRACE_RET(chip, STATUS_FAIL);
+			rtsx_trace(chip);
+			return STATUS_FAIL;
 		}
 	} else {
 		retval = xd_copy_page(chip, old_blk, new_blk,
@@ -1601,7 +1797,8 @@ static int xd_finish_write(struct rtsx_chip *chip,
 					xd_set_unused_block(chip, new_blk);
 			}
 			XD_CLR_BAD_NEWBLK(xd_card);
-			TRACE_RET(chip, STATUS_FAIL);
+			rtsx_trace(chip);
+			return STATUS_FAIL;
 		}
 
 		retval = xd_erase_block(chip, old_blk);
@@ -1633,8 +1830,10 @@ static int xd_prepare_write(struct rtsx_chip *chip,
 
 	if (page_off) {
 		retval = xd_copy_page(chip, old_blk, new_blk, 0, page_off);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, STATUS_FAIL);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 	}
 
 	return STATUS_SUCCESS;
@@ -1655,8 +1854,10 @@ static int xd_write_multiple_pages(struct rtsx_chip *chip, u32 old_blk,
 	dev_dbg(rtsx_dev(chip), "%s, old_blk = 0x%x, new_blk = 0x%x, log_blk = 0x%x\n",
 		__func__, old_blk, new_blk, log_blk);
 
-	if (start_page > end_page)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (start_page > end_page) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	page_cnt = end_page - start_page;
 	zone_no = (int)(log_blk / 1000);
@@ -1665,8 +1866,10 @@ static int xd_write_multiple_pages(struct rtsx_chip *chip, u32 old_blk,
 	page_addr = (new_blk << xd_card->block_shift) + start_page;
 
 	retval = xd_send_cmd(chip, READ1_1);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	rtsx_init_cmd(chip);
 
@@ -1701,9 +1904,11 @@ static int xd_write_multiple_pages(struct rtsx_chip *chip, u32 old_blk,
 
 		if (retval == -ETIMEDOUT) {
 			xd_set_err_code(chip, XD_TO_ERROR);
-			TRACE_RET(chip, STATUS_FAIL);
+			rtsx_trace(chip);
+			return STATUS_FAIL;
 		} else {
-			TRACE_GOTO(chip, Fail);
+			rtsx_trace(chip);
+			goto Fail;
 		}
 	}
 
@@ -1730,13 +1935,18 @@ static int xd_write_multiple_pages(struct rtsx_chip *chip, u32 old_blk,
 	return STATUS_SUCCESS;
 
 Fail:
-	RTSX_READ_REG(chip, XD_DAT, &reg_val);
+	retval = rtsx_read_register(chip, XD_DAT, &reg_val);
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
 	if (reg_val & PROGRAM_ERROR) {
 		xd_set_err_code(chip, XD_PRG_ERROR);
 		xd_mark_bad_block(chip, new_blk);
 	}
 
-	TRACE_RET(chip, STATUS_FAIL);
+	rtsx_trace(chip);
+	return STATUS_FAIL;
 }
 
 #ifdef XD_DELAY_WRITE
@@ -1749,16 +1959,20 @@ int xd_delay_write(struct rtsx_chip *chip)
 	if (delay_write->delay_write_flag) {
 		dev_dbg(rtsx_dev(chip), "xd_delay_write\n");
 		retval = xd_switch_clock(chip);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, STATUS_FAIL);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 
 		delay_write->delay_write_flag = 0;
 		retval = xd_finish_write(chip,
 				delay_write->old_phyblock,
 					delay_write->new_phyblock,
 				delay_write->logblock, delay_write->pageoff);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, STATUS_FAIL);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 	}
 
 	return STATUS_SUCCESS;
@@ -1790,14 +2004,17 @@ int xd_rw(struct scsi_cmnd *srb, struct rtsx_chip *chip,
 	ptr = (u8 *)scsi_sglist(srb);
 
 	retval = xd_switch_clock(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 
 	if (detect_card_cd(chip, XD_CARD) != STATUS_SUCCESS) {
 		chip->card_fail |= XD_CARD;
 		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_NOT_PRESENT);
-		TRACE_RET(chip, STATUS_FAIL);
+		rtsx_trace(chip);
+		return STATUS_FAIL;
 	}
 
 	log_blk = start_sector >> xd_card->block_shift;
@@ -1810,7 +2027,8 @@ int xd_rw(struct scsi_cmnd *srb, struct rtsx_chip *chip,
 		if (retval != STATUS_SUCCESS) {
 			chip->card_fail |= XD_CARD;
 			set_sense_type(chip, lun, SENSE_TYPE_MEDIA_NOT_PRESENT);
-			TRACE_RET(chip, STATUS_FAIL);
+			rtsx_trace(chip);
+			return STATUS_FAIL;
 		}
 	}
 
@@ -1828,7 +2046,8 @@ int xd_rw(struct scsi_cmnd *srb, struct rtsx_chip *chip,
 				if (retval != STATUS_SUCCESS) {
 					set_sense_type(chip, lun,
 						SENSE_TYPE_MEDIA_WRITE_ERR);
-					TRACE_RET(chip, STATUS_FAIL);
+					rtsx_trace(chip);
+					return STATUS_FAIL;
 				}
 			}
 			old_blk = delay_write->old_phyblock;
@@ -1844,7 +2063,8 @@ int xd_rw(struct scsi_cmnd *srb, struct rtsx_chip *chip,
 			if (retval != STATUS_SUCCESS) {
 				set_sense_type(chip, lun,
 					SENSE_TYPE_MEDIA_WRITE_ERR);
-				TRACE_RET(chip, STATUS_FAIL);
+				rtsx_trace(chip);
+				return STATUS_FAIL;
 			}
 #endif
 			old_blk = xd_get_l2p_tbl(chip, zone_no, log_off);
@@ -1853,7 +2073,8 @@ int xd_rw(struct scsi_cmnd *srb, struct rtsx_chip *chip,
 				(new_blk == BLK_NOT_FOUND)) {
 				set_sense_type(chip, lun,
 					SENSE_TYPE_MEDIA_WRITE_ERR);
-				TRACE_RET(chip, STATUS_FAIL);
+				rtsx_trace(chip);
+				return STATUS_FAIL;
 			}
 
 			retval = xd_prepare_write(chip, old_blk, new_blk,
@@ -1863,11 +2084,13 @@ int xd_rw(struct scsi_cmnd *srb, struct rtsx_chip *chip,
 					STATUS_SUCCESS) {
 					set_sense_type(chip, lun,
 						SENSE_TYPE_MEDIA_NOT_PRESENT);
-					TRACE_RET(chip, STATUS_FAIL);
+					rtsx_trace(chip);
+					return STATUS_FAIL;
 				}
 				set_sense_type(chip, lun,
 					SENSE_TYPE_MEDIA_WRITE_ERR);
-				TRACE_RET(chip, STATUS_FAIL);
+				rtsx_trace(chip);
+				return STATUS_FAIL;
 			}
 #ifdef XD_DELAY_WRITE
 		}
@@ -1879,11 +2102,13 @@ int xd_rw(struct scsi_cmnd *srb, struct rtsx_chip *chip,
 			if (detect_card_cd(chip, XD_CARD) != STATUS_SUCCESS) {
 				set_sense_type(chip, lun,
 					SENSE_TYPE_MEDIA_NOT_PRESENT);
-				TRACE_RET(chip, STATUS_FAIL);
+				rtsx_trace(chip);
+				return STATUS_FAIL;
 			}
 			set_sense_type(chip, lun,
 				SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
-			TRACE_RET(chip, STATUS_FAIL);
+			rtsx_trace(chip);
+			return STATUS_FAIL;
 		}
 #endif
 
@@ -1891,7 +2116,8 @@ int xd_rw(struct scsi_cmnd *srb, struct rtsx_chip *chip,
 		if (old_blk == BLK_NOT_FOUND) {
 			set_sense_type(chip, lun,
 				SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
-			TRACE_RET(chip, STATUS_FAIL);
+			rtsx_trace(chip);
+			return STATUS_FAIL;
 		}
 	}
 
@@ -1901,7 +2127,8 @@ int xd_rw(struct scsi_cmnd *srb, struct rtsx_chip *chip,
 		if (detect_card_cd(chip, XD_CARD) != STATUS_SUCCESS) {
 			chip->card_fail |= XD_CARD;
 			set_sense_type(chip, lun, SENSE_TYPE_MEDIA_NOT_PRESENT);
-			TRACE_RET(chip, STATUS_FAIL);
+			rtsx_trace(chip);
+			return STATUS_FAIL;
 		}
 
 		if ((start_page + total_sec_cnt) > (xd_card->page_off + 1))
@@ -1917,7 +2144,8 @@ int xd_rw(struct scsi_cmnd *srb, struct rtsx_chip *chip,
 			if (retval != STATUS_SUCCESS) {
 				set_sense_type(chip, lun,
 					SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
-				TRACE_RET(chip, STATUS_FAIL);
+				rtsx_trace(chip);
+				return STATUS_FAIL;
 			}
 		} else {
 			retval = xd_write_multiple_pages(chip, old_blk,
@@ -1927,7 +2155,8 @@ int xd_rw(struct scsi_cmnd *srb, struct rtsx_chip *chip,
 			if (retval != STATUS_SUCCESS) {
 				set_sense_type(chip, lun,
 					SENSE_TYPE_MEDIA_WRITE_ERR);
-				TRACE_RET(chip, STATUS_FAIL);
+				rtsx_trace(chip);
+				return STATUS_FAIL;
 			}
 		}
 
@@ -1948,7 +2177,8 @@ int xd_rw(struct scsi_cmnd *srb, struct rtsx_chip *chip,
 				chip->card_fail |= XD_CARD;
 				set_sense_type(chip, lun,
 					SENSE_TYPE_MEDIA_NOT_PRESENT);
-				TRACE_RET(chip, STATUS_FAIL);
+				rtsx_trace(chip);
+				return STATUS_FAIL;
 			}
 		}
 
@@ -1961,7 +2191,8 @@ int xd_rw(struct scsi_cmnd *srb, struct rtsx_chip *chip,
 				set_sense_type(chip, lun,
 					SENSE_TYPE_MEDIA_WRITE_ERR);
 
-			TRACE_RET(chip, STATUS_FAIL);
+			rtsx_trace(chip);
+			return STATUS_FAIL;
 		}
 
 		if (srb->sc_data_direction == DMA_TO_DEVICE) {
@@ -1969,7 +2200,8 @@ int xd_rw(struct scsi_cmnd *srb, struct rtsx_chip *chip,
 			if (new_blk == BLK_NOT_FOUND) {
 				set_sense_type(chip, lun,
 					SENSE_TYPE_MEDIA_WRITE_ERR);
-				TRACE_RET(chip, STATUS_FAIL);
+				rtsx_trace(chip);
+				return STATUS_FAIL;
 			}
 		}
 
@@ -1988,7 +2220,8 @@ int xd_rw(struct scsi_cmnd *srb, struct rtsx_chip *chip,
 		if (detect_card_cd(chip, XD_CARD) != STATUS_SUCCESS) {
 			chip->card_fail |= XD_CARD;
 			set_sense_type(chip, lun, SENSE_TYPE_MEDIA_NOT_PRESENT);
-			TRACE_RET(chip, STATUS_FAIL);
+			rtsx_trace(chip);
+			return STATUS_FAIL;
 		}
 
 		retval = xd_finish_write(chip, old_blk, new_blk,
@@ -1997,10 +2230,12 @@ int xd_rw(struct scsi_cmnd *srb, struct rtsx_chip *chip,
 			if (detect_card_cd(chip, XD_CARD) != STATUS_SUCCESS) {
 				set_sense_type(chip, lun,
 					SENSE_TYPE_MEDIA_NOT_PRESENT);
-				TRACE_RET(chip, STATUS_FAIL);
+				rtsx_trace(chip);
+				return STATUS_FAIL;
 			}
 			set_sense_type(chip, lun, SENSE_TYPE_MEDIA_WRITE_ERR);
-			TRACE_RET(chip, STATUS_FAIL);
+			rtsx_trace(chip);
+			return STATUS_FAIL;
 		}
 #endif
 	}
@@ -2049,25 +2284,39 @@ int xd_power_off_card3v3(struct rtsx_chip *chip)
 	int retval;
 
 	retval = disable_card_clock(chip, XD_CARD);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
-	RTSX_WRITE_REG(chip, CARD_OE, XD_OUTPUT_EN, 0);
+	retval = rtsx_write_register(chip, CARD_OE, XD_OUTPUT_EN, 0);
+	if (retval) {
+		rtsx_trace(chip);
+		return retval;
+	}
 
 	if (!chip->ft2_fast_mode) {
 		retval = card_power_off(chip, XD_CARD);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, STATUS_FAIL);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 
 		wait_timeout(50);
 	}
 
 	if (chip->asic_code) {
 		retval = xd_pull_ctl_disable(chip);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, STATUS_FAIL);
+		if (retval != STATUS_SUCCESS) {
+			rtsx_trace(chip);
+			return STATUS_FAIL;
+		}
 	} else {
-		RTSX_WRITE_REG(chip, FPGA_PULL_CTL, 0xFF, 0xDF);
+		retval = rtsx_write_register(chip, FPGA_PULL_CTL, 0xFF, 0xDF);
+		if (retval) {
+			rtsx_trace(chip);
+			return retval;
+		}
 	}
 
 	return STATUS_SUCCESS;
@@ -2087,8 +2336,10 @@ int release_xd_card(struct rtsx_chip *chip)
 	xd_free_l2p_tbl(chip);
 
 	retval = xd_power_off_card3v3(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
+	if (retval != STATUS_SUCCESS) {
+		rtsx_trace(chip);
+		return STATUS_FAIL;
+	}
 
 	return STATUS_SUCCESS;
 }
diff --git a/drivers/staging/skein/skein_block.c b/drivers/staging/skein/skein_block.c
index 9bd69ce3be00..b0cd9357348f 100644
--- a/drivers/staging/skein/skein_block.c
+++ b/drivers/staging/skein/skein_block.c
@@ -68,9 +68,7 @@ do {                                         \
 
 #if SKEIN_UNROLL_256 == 0
 #define R256(p0, p1, p2, p3, ROT, r_num) /* fully unrolled */ \
-do {                                                          \
-	ROUND256(p0, p1, p2, p3, ROT, r_num);                 \
-} while (0)
+	ROUND256(p0, p1, p2, p3, ROT, r_num)
 
 #define I256(R)                                                           \
 do {                                                                      \
@@ -152,9 +150,7 @@ do {                                                         \
 
 #if SKEIN_UNROLL_512 == 0
 #define R512(p0, p1, p2, p3, p4, p5, p6, p7, ROT, r_num) /* unrolled */ \
-do {                                                                    \
-	ROUND512(p0, p1, p2, p3, p4, p5, p6, p7, ROT, r_num);           \
-} while (0)
+	ROUND512(p0, p1, p2, p3, p4, p5, p6, p7, ROT, r_num)
 
 #define I512(R)                                                           \
 do {                                                                      \
diff --git a/drivers/staging/slicoss/slicoss.c b/drivers/staging/slicoss/slicoss.c
index 42d62ef56cb8..c2bda1d38e41 100644
--- a/drivers/staging/slicoss/slicoss.c
+++ b/drivers/staging/slicoss/slicoss.c
@@ -84,7 +84,6 @@
 #include <linux/seq_file.h>
 #include <linux/kthread.h>
 #include <linux/module.h>
-#include <linux/moduleparam.h>
 
 #include <linux/firmware.h>
 #include <linux/types.h>
@@ -99,8 +98,7 @@
 #include "slic.h"
 
 static uint slic_first_init = 1;
-static char *slic_banner = "Alacritech SLIC Technology(tm) Server "
-		"and Storage Accelerator (Non-Accelerated)";
+static char *slic_banner = "Alacritech SLIC Technology(tm) Server and Storage Accelerator (Non-Accelerated)";
 
 static char *slic_proc_version = "2.0.351  2006/07/14 12:26:00";
 
@@ -166,7 +164,7 @@ static void slic_mcast_set_bit(struct adapter *adapter, char *address)
 	/* Get the CRC polynomial for the mac address */
 	/* we use bits 1-8 (lsb), bitwise reversed,
 	 * msb (= lsb bit 0 before bitrev) is automatically discarded */
-	crcpoly = (ether_crc(ETH_ALEN, address)>>23);
+	crcpoly = ether_crc(ETH_ALEN, address)>>23;
 
 	/* We only have space on the SLIC for 64 entries.  Lop
 	 * off the top two bits. (2^6 = 64)
@@ -1852,7 +1850,7 @@ static void slic_xmit_build_request(struct adapter *adapter,
 
 	ihcmd = &hcmd->cmd64;
 
-	ihcmd->flags = (adapter->port << IHFLG_IFSHFT);
+	ihcmd->flags = adapter->port << IHFLG_IFSHFT;
 	ihcmd->command = IHCMD_XMT_REQ;
 	ihcmd->u.slic_buffers.totlen = skb->len;
 	phys_addr = pci_map_single(adapter->pcidev, skb->data, skb->len,
@@ -1864,8 +1862,8 @@ static void slic_xmit_build_request(struct adapter *adapter,
 	hcmd->cmdsize = (u32) ((((u64)&ihcmd->u.slic_buffers.bufs[1] -
 				     (u64) hcmd) + 31) >> 5);
 #else
-	hcmd->cmdsize = ((((u32) &ihcmd->u.slic_buffers.bufs[1] -
-			   (u32) hcmd) + 31) >> 5);
+	hcmd->cmdsize = (((u32)&ihcmd->u.slic_buffers.bufs[1] -
+				       (u32)hcmd) + 31) >> 5;
 #endif
 }
 
@@ -2315,9 +2313,8 @@ static int slic_if_init(struct adapter *adapter)
 	}
 	rc = slic_adapter_allocresources(adapter);
 	if (rc) {
-		dev_err(&dev->dev,
-			"%s: slic_adapter_allocresources FAILED %x\n",
-			__func__, rc);
+		dev_err(&dev->dev, "slic_adapter_allocresources FAILED %x\n",
+			rc);
 		slic_adapter_freeresources(adapter);
 		goto err;
 	}
@@ -2362,22 +2359,19 @@ static int slic_if_init(struct adapter *adapter)
 
 	adapter->state = ADAPT_UP;
 	if (!card->loadtimerset) {
-		init_timer(&card->loadtimer);
+		setup_timer(&card->loadtimer, &slic_timer_load_check,
+			    (ulong)card);
 		card->loadtimer.expires =
 		    jiffies + (SLIC_LOADTIMER_PERIOD * HZ);
-		card->loadtimer.data = (ulong) card;
-		card->loadtimer.function = &slic_timer_load_check;
 		add_timer(&card->loadtimer);
 
 		card->loadtimerset = 1;
 	}
 
 	if (!adapter->pingtimerset) {
-		init_timer(&adapter->pingtimer);
+		setup_timer(&adapter->pingtimer, &slic_timer_ping, (ulong)dev);
 		adapter->pingtimer.expires =
 		    jiffies + (PING_TIMER_INTERVAL * HZ);
-		adapter->pingtimer.data = (ulong) dev;
-		adapter->pingtimer.function = &slic_timer_ping;
 		add_timer(&adapter->pingtimer);
 		adapter->pingtimerset = 1;
 		adapter->card->pingstatus = ISR_PINGMASK;
@@ -2554,46 +2548,11 @@ static int slic_ioctl(struct net_device *dev, struct ifreq *rq, int cmd)
 		if (copy_from_user(data, rq->ifr_data, 28))
 			return -EFAULT;
 		intagg = data[0];
-		dev_err(&dev->dev, "%s: set interrupt aggregation to %d\n",
-			__func__, intagg);
+		dev_err(&dev->dev, "set interrupt aggregation to %d\n",
+			intagg);
 		slic_intagg_set(adapter, intagg);
 		return 0;
 
-#ifdef SLIC_TRACE_DUMP_ENABLED
-	case SIOCSLICTRACEDUMP:
-		{
-			u32 value;
-
-			DBG_IOCTL("slic_ioctl  SIOCSLIC_TRACE_DUMP\n");
-
-			if (copy_from_user(data, rq->ifr_data, 28)) {
-				PRINT_ERROR
-				    ("slic: copy_from_user FAILED getting initial simba param\n");
-				return -EFAULT;
-			}
-
-			value = data[0];
-			if (tracemon_request == SLIC_DUMP_DONE) {
-				PRINT_ERROR
-				    ("ATK Diagnostic Trace Dump Requested\n");
-				tracemon_request = SLIC_DUMP_REQUESTED;
-				tracemon_request_type = value;
-				tracemon_timestamp = jiffies;
-			} else if ((tracemon_request == SLIC_DUMP_REQUESTED) ||
-				   (tracemon_request ==
-				    SLIC_DUMP_IN_PROGRESS)) {
-				PRINT_ERROR
-				    ("ATK Diagnostic Trace Dump Requested but already in progress... ignore\n");
-			} else {
-				PRINT_ERROR
-				    ("ATK Diagnostic Trace Dump Requested\n");
-				tracemon_request = SLIC_DUMP_REQUESTED;
-				tracemon_request_type = value;
-				tracemon_timestamp = jiffies;
-			}
-			return 0;
-		}
-#endif
 	case SIOCETHTOOL:
 		if (copy_from_user(&ecmd, rq->ifr_data, sizeof(ecmd)))
 			return -EFAULT;
diff --git a/drivers/staging/sm750fb/Kconfig b/drivers/staging/sm750fb/Kconfig
new file mode 100644
index 000000000000..c40d088a4d3b
--- /dev/null
+++ b/drivers/staging/sm750fb/Kconfig
@@ -0,0 +1,10 @@
+config FB_SM750
+	tristate "Silicon Motion SM750 framebuffer support"
+	depends on FB && PCI
+	help
+	  Frame buffer driver for the Silicon Motion SM750 chip
+	  with 2D accelearion and dual head support.
+
+	  This driver is also available as a module. The module will be
+	  called sm750fb. If you want to compile it as a module, say M
+	  here and read <file:Documentation/kbuild/modules.txt>.
diff --git a/drivers/staging/sm750fb/Makefile b/drivers/staging/sm750fb/Makefile
new file mode 100644
index 000000000000..dcce3f487ed5
--- /dev/null
+++ b/drivers/staging/sm750fb/Makefile
@@ -0,0 +1,4 @@
+obj-$(CONFIG_FB_SM750)	+= sm750fb.o
+
+sm750fb-objs		:= sm750.o sm750_hw.o sm750_accel.o sm750_cursor.o ddk750_chip.o ddk750_power.o ddk750_mode.o
+sm750fb-objs		+= ddk750_display.o ddk750_help.o ddk750_swi2c.o ddk750_sii164.o ddk750_dvi.o ddk750_hwi2c.o
diff --git a/drivers/staging/sm750fb/TODO b/drivers/staging/sm750fb/TODO
new file mode 100644
index 000000000000..bc1617249395
--- /dev/null
+++ b/drivers/staging/sm750fb/TODO
@@ -0,0 +1,15 @@
+TODO:
+- lots of clechpatch cleanup
+- use kernel coding style
+- refine the code and remove unused code
+- check on hardware effects of removal of USE_HW_I2C and USE_DVICHIP (these two
+	are supposed to be sample code which is given here if someone wants to
+	use those functionalities)
+- move it to drivers/video/fbdev
+- modify the code for drm framework
+
+Please send any patches to
+	Greg Kroah-Hartman <greg@kroah.com>
+	Sudip Mukherjee <sudipm.mukherjee@gmail.com>
+	Teddy Wang <teddy.wang@siliconmotion.com>
+	Sudip Mukherjee <sudip@vectorindia.org>
diff --git a/drivers/staging/sm750fb/ddk750.h b/drivers/staging/sm750fb/ddk750.h
new file mode 100644
index 000000000000..2c10a08ed964
--- /dev/null
+++ b/drivers/staging/sm750fb/ddk750.h
@@ -0,0 +1,24 @@
+#ifndef DDK750_H__
+#define DDK750_H__
+/*******************************************************************
+*
+*         Copyright (c) 2007 by Silicon Motion, Inc. (SMI)
+*
+*  All rights are reserved. Reproduction or in part is prohibited
+*  without the written consent of the copyright owner.
+*
+*  RegSC.h --- SM718 SDK
+*  This file contains the definitions for the System Configuration registers.
+*
+*******************************************************************/
+#include "ddk750_reg.h"
+#include "ddk750_mode.h"
+#include "ddk750_chip.h"
+#include "ddk750_display.h"
+#include "ddk750_power.h"
+#include "ddk750_help.h"
+#ifdef USE_HW_I2C
+#include "ddk750_hwi2c.h"
+#endif
+#include "ddk750_swi2c.h"
+#endif
diff --git a/drivers/staging/sm750fb/ddk750_chip.c b/drivers/staging/sm750fb/ddk750_chip.c
new file mode 100644
index 000000000000..7b28328c92f8
--- /dev/null
+++ b/drivers/staging/sm750fb/ddk750_chip.c
@@ -0,0 +1,622 @@
+#include "ddk750_help.h"
+#include "ddk750_reg.h"
+#include "ddk750_chip.h"
+#include "ddk750_power.h"
+typedef struct _pllcalparam {
+	unsigned char power;/* d : 0~ 6*/
+	unsigned char pod;
+	unsigned char od;
+	unsigned char value;/* value of  2 power d (2^d) */
+}
+pllcalparam;
+
+
+logical_chip_type_t getChipType(void)
+{
+	unsigned short physicalID;
+	char physicalRev;
+	logical_chip_type_t chip;
+
+	physicalID = devId750;//either 0x718 or 0x750
+	physicalRev = revId750;
+
+	if (physicalID == 0x718)
+		chip = SM718;
+	else if (physicalID == 0x750) {
+		chip = SM750;
+		/* SM750 and SM750LE are different in their revision ID only. */
+		if (physicalRev == SM750LE_REVISION_ID)
+			chip = SM750LE;
+	} else
+		chip = SM_UNKNOWN;
+
+	return chip;
+}
+
+
+inline unsigned int twoToPowerOfx(unsigned long x)
+{
+	unsigned long i;
+	unsigned long result = 1;
+
+	for (i = 1; i <= x; i++)
+		result *= 2;
+	return result;
+}
+
+inline unsigned int calcPLL(pll_value_t *pPLL)
+{
+	return (pPLL->inputFreq * pPLL->M / pPLL->N / twoToPowerOfx(pPLL->OD) / twoToPowerOfx(pPLL->POD));
+}
+
+unsigned int getPllValue(clock_type_t clockType, pll_value_t *pPLL)
+{
+	unsigned int ulPllReg = 0;
+
+	pPLL->inputFreq = DEFAULT_INPUT_CLOCK;
+	pPLL->clockType = clockType;
+
+	switch (clockType) {
+	case MXCLK_PLL:
+		ulPllReg = PEEK32(MXCLK_PLL_CTRL);
+		break;
+	case PRIMARY_PLL:
+		ulPllReg = PEEK32(PANEL_PLL_CTRL);
+		break;
+	case SECONDARY_PLL:
+		ulPllReg = PEEK32(CRT_PLL_CTRL);
+		break;
+	case VGA0_PLL:
+		ulPllReg = PEEK32(VGA_PLL0_CTRL);
+		break;
+	case VGA1_PLL:
+		ulPllReg = PEEK32(VGA_PLL1_CTRL);
+		break;
+	}
+
+	pPLL->M = FIELD_GET(ulPllReg, PANEL_PLL_CTRL, M);
+	pPLL->N = FIELD_GET(ulPllReg, PANEL_PLL_CTRL, N);
+	pPLL->OD = FIELD_GET(ulPllReg, PANEL_PLL_CTRL, OD);
+	pPLL->POD = FIELD_GET(ulPllReg, PANEL_PLL_CTRL, POD);
+
+	return calcPLL(pPLL);
+}
+
+
+unsigned int getChipClock(void)
+{
+	pll_value_t pll;
+#if 1
+	if (getChipType() == SM750LE)
+		return MHz(130);
+#endif
+
+	return getPllValue(MXCLK_PLL, &pll);
+}
+
+
+/*
+ * This function set up the main chip clock.
+ *
+ * Input: Frequency to be set.
+ */
+void setChipClock(unsigned int frequency)
+{
+	pll_value_t pll;
+	unsigned int ulActualMxClk;
+#if 1
+	/* Cheok_0509: For SM750LE, the chip clock is fixed. Nothing to set. */
+	if (getChipType() == SM750LE)
+		return;
+#endif
+
+	if (frequency) {
+		/*
+		* Set up PLL, a structure to hold the value to be set in clocks.
+		*/
+		pll.inputFreq = DEFAULT_INPUT_CLOCK; /* Defined in CLOCK.H */
+		pll.clockType = MXCLK_PLL;
+
+		/*
+		* Call calcPllValue() to fill up the other fields for PLL structure.
+		* Sometime, the chip cannot set up the exact clock required by User.
+		* Return value from calcPllValue() gives the actual possible clock.
+		*/
+		ulActualMxClk = calcPllValue(frequency, &pll);
+
+		/* Master Clock Control: MXCLK_PLL */
+		POKE32(MXCLK_PLL_CTRL, formatPllReg(&pll));
+	}
+}
+
+
+
+void setMemoryClock(unsigned int frequency)
+{
+	unsigned int ulReg, divisor;
+ #if 1
+	/* Cheok_0509: For SM750LE, the memory clock is fixed. Nothing to set. */
+	if (getChipType() == SM750LE)
+		return;
+#endif
+	if (frequency) {
+		/* Set the frequency to the maximum frequency that the DDR Memory can take
+		which is 336MHz. */
+		if (frequency > MHz(336))
+			frequency = MHz(336);
+
+		/* Calculate the divisor */
+		divisor = (unsigned int) roundedDiv(getChipClock(), frequency);
+
+		/* Set the corresponding divisor in the register. */
+		ulReg = PEEK32(CURRENT_GATE);
+		switch (divisor) {
+		default:
+		case 1:
+			ulReg = FIELD_SET(ulReg, CURRENT_GATE, M2XCLK, DIV_1);
+			break;
+		case 2:
+			ulReg = FIELD_SET(ulReg, CURRENT_GATE, M2XCLK, DIV_2);
+			break;
+		case 3:
+			ulReg = FIELD_SET(ulReg, CURRENT_GATE, M2XCLK, DIV_3);
+			break;
+		case 4:
+			ulReg = FIELD_SET(ulReg, CURRENT_GATE, M2XCLK, DIV_4);
+			break;
+		}
+
+		setCurrentGate(ulReg);
+	}
+}
+
+
+/*
+ * This function set up the master clock (MCLK).
+ *
+ * Input: Frequency to be set.
+ *
+ * NOTE:
+ *      The maximum frequency the engine can run is 168MHz.
+ */
+void setMasterClock(unsigned int frequency)
+{
+	unsigned int ulReg, divisor;
+#if 1
+	/* Cheok_0509: For SM750LE, the memory clock is fixed. Nothing to set. */
+	if (getChipType() == SM750LE)
+		return;
+#endif
+	if (frequency) {
+		/* Set the frequency to the maximum frequency that the SM750 engine can
+		run, which is about 190 MHz. */
+		if (frequency > MHz(190))
+			frequency = MHz(190);
+
+		/* Calculate the divisor */
+		divisor = (unsigned int) roundedDiv(getChipClock(), frequency);
+
+		/* Set the corresponding divisor in the register. */
+		ulReg = PEEK32(CURRENT_GATE);
+		switch (divisor) {
+		default:
+		case 3:
+			ulReg = FIELD_SET(ulReg, CURRENT_GATE, MCLK, DIV_3);
+			break;
+		case 4:
+			ulReg = FIELD_SET(ulReg, CURRENT_GATE, MCLK, DIV_4);
+			break;
+		case 6:
+			ulReg = FIELD_SET(ulReg, CURRENT_GATE, MCLK, DIV_6);
+			break;
+		case 8:
+			ulReg = FIELD_SET(ulReg, CURRENT_GATE, MCLK, DIV_8);
+			break;
+		}
+
+		setCurrentGate(ulReg);
+		}
+}
+
+
+unsigned int ddk750_getVMSize(void)
+{
+	unsigned int reg;
+	unsigned int data;
+
+	/* sm750le only use 64 mb memory*/
+	if (getChipType() == SM750LE)
+		return MB(64);
+
+	/* for 750,always use power mode0*/
+	reg = PEEK32(MODE0_GATE);
+	reg = FIELD_SET(reg, MODE0_GATE, GPIO, ON);
+	POKE32(MODE0_GATE, reg);
+
+	/* get frame buffer size from GPIO */
+	reg = FIELD_GET(PEEK32(MISC_CTRL), MISC_CTRL, LOCALMEM_SIZE);
+	switch (reg) {
+	case MISC_CTRL_LOCALMEM_SIZE_8M:
+		data = MB(8);  break; /* 8  Mega byte */
+	case MISC_CTRL_LOCALMEM_SIZE_16M:
+		data = MB(16); break; /* 16 Mega byte */
+	case MISC_CTRL_LOCALMEM_SIZE_32M:
+		data = MB(32); break; /* 32 Mega byte */
+	case MISC_CTRL_LOCALMEM_SIZE_64M:
+		data = MB(64); break; /* 64 Mega byte */
+	default:
+		data = 0;
+		break;
+	}
+	return data;
+
+}
+
+int ddk750_initHw(initchip_param_t *pInitParam)
+{
+
+	unsigned int ulReg;
+#if 0
+	//move the code to map regiter function.
+	if (getChipType() == SM718) {
+		/* turn on big endian bit*/
+		ulReg = PEEK32(0x74);
+		/* now consider register definition in a big endian pattern*/
+		POKE32(0x74, ulReg|0x80000000);
+	}
+
+#endif
+
+
+	if (pInitParam->powerMode != 0 )
+		pInitParam->powerMode = 0;
+	setPowerMode(pInitParam->powerMode);
+
+	/* Enable display power gate & LOCALMEM power gate*/
+	ulReg = PEEK32(CURRENT_GATE);
+	ulReg = FIELD_SET(ulReg, CURRENT_GATE, DISPLAY, ON);
+	ulReg = FIELD_SET(ulReg, CURRENT_GATE, LOCALMEM, ON);
+	setCurrentGate(ulReg);
+
+	if (getChipType() != SM750LE) {
+		/*	set panel pll and graphic mode via mmio_88 */
+		ulReg = PEEK32(VGA_CONFIGURATION);
+		ulReg = FIELD_SET(ulReg, VGA_CONFIGURATION, PLL, PANEL);
+		ulReg = FIELD_SET(ulReg, VGA_CONFIGURATION, MODE, GRAPHIC);
+		POKE32(VGA_CONFIGURATION, ulReg);
+	} else {
+#if defined(__i386__) || defined( __x86_64__)
+		/* set graphic mode via IO method */
+		outb_p(0x88, 0x3d4);
+		outb_p(0x06, 0x3d5);
+#endif
+	}
+
+	/* Set the Main Chip Clock */
+	setChipClock(MHz((unsigned int)pInitParam->chipClock));
+
+	/* Set up memory clock. */
+	setMemoryClock(MHz(pInitParam->memClock));
+
+	/* Set up master clock */
+	setMasterClock(MHz(pInitParam->masterClock));
+
+
+	/* Reset the memory controller. If the memory controller is not reset in SM750,
+	   the system might hang when sw accesses the memory.
+	   The memory should be resetted after changing the MXCLK.
+	 */
+	if (pInitParam->resetMemory == 1) {
+		ulReg = PEEK32(MISC_CTRL);
+		ulReg = FIELD_SET(ulReg, MISC_CTRL, LOCALMEM_RESET, RESET);
+		POKE32(MISC_CTRL, ulReg);
+
+		ulReg = FIELD_SET(ulReg, MISC_CTRL, LOCALMEM_RESET, NORMAL);
+		POKE32(MISC_CTRL, ulReg);
+	}
+
+	if (pInitParam->setAllEngOff == 1) {
+		enable2DEngine(0);
+
+		/* Disable Overlay, if a former application left it on */
+		ulReg = PEEK32(VIDEO_DISPLAY_CTRL);
+		ulReg = FIELD_SET(ulReg, VIDEO_DISPLAY_CTRL, PLANE, DISABLE);
+		POKE32(VIDEO_DISPLAY_CTRL, ulReg);
+
+		/* Disable video alpha, if a former application left it on */
+		ulReg = PEEK32(VIDEO_ALPHA_DISPLAY_CTRL);
+		ulReg = FIELD_SET(ulReg, VIDEO_ALPHA_DISPLAY_CTRL, PLANE, DISABLE);
+		POKE32(VIDEO_ALPHA_DISPLAY_CTRL, ulReg);
+
+		/* Disable alpha plane, if a former application left it on */
+		ulReg = PEEK32(ALPHA_DISPLAY_CTRL);
+		ulReg = FIELD_SET(ulReg, ALPHA_DISPLAY_CTRL, PLANE, DISABLE);
+		POKE32(ALPHA_DISPLAY_CTRL, ulReg);
+
+#if 0
+		/* Disable LCD hardware cursor, if a former application left it on */
+		ulReg = PEEK32(PANEL_HWC_ADDRESS);
+		ulReg = FIELD_SET(ulReg, PANEL_HWC_ADDRESS, ENABLE, DISABLE);
+		POKE32(PANEL_HWC_ADDRESS, ulReg);
+
+		/* Disable CRT hardware cursor, if a former application left it on */
+		ulReg = PEEK32(CRT_HWC_ADDRESS);
+		ulReg = FIELD_SET(ulReg, CRT_HWC_ADDRESS, ENABLE, DISABLE);
+		POKE32(CRT_HWC_ADDRESS, ulReg);
+
+		/* Disable ZV Port 0, if a former application left it on */
+		ulReg = PEEK32(ZV0_CAPTURE_CTRL);
+		ulReg = FIELD_SET(ulReg, ZV0_CAPTURE_CTRL, CAP, DISABLE);
+		POKE32(ZV0_CAPTURE_CTRL, ulReg);
+
+		/* Disable ZV Port 1, if a former application left it on */
+		ulReg = PEEK32(ZV1_CAPTURE_CTRL);
+		ulReg = FIELD_SET(ulReg, ZV1_CAPTURE_CTRL, CAP, DISABLE);
+		POKE32(ZV1_CAPTURE_CTRL, ulReg);
+
+		/* Disable ZV Port Power, if a former application left it on */
+		enableZVPort(0);
+		/* Disable DMA Channel, if a former application left it on */
+		ulReg = PEEK32(DMA_ABORT_INTERRUPT);
+		ulReg = FIELD_SET(ulReg, DMA_ABORT_INTERRUPT, ABORT_1, ABORT);
+		POKE32(DMA_ABORT_INTERRUPT, ulReg);
+
+		/* Disable i2c */
+		enableI2C(0);
+#endif
+		/* Disable DMA Channel, if a former application left it on */
+		ulReg = PEEK32(DMA_ABORT_INTERRUPT);
+		ulReg = FIELD_SET(ulReg, DMA_ABORT_INTERRUPT, ABORT_1, ABORT);
+		POKE32(DMA_ABORT_INTERRUPT, ulReg);
+
+		/* Disable DMA Power, if a former application left it on */
+		enableDMA(0);
+	}
+
+	/* We can add more initialization as needed. */
+
+	return 0;
+}
+
+#if 0
+
+unsigned int absDiff(unsigned int a, unsigned int b)
+{
+	if ( a > b )
+		return(a - b);
+	else
+		return(b - a);
+}
+
+#endif
+/*
+	monk liu @ 4/6/2011:
+		   re-write the calculatePLL function of ddk750.
+		   the original version function does not use some mathematics tricks and shortcut
+		   when it doing the calculation of the best N,M,D combination
+		   I think this version gives a little upgrade in speed
+
+	750 pll clock formular:
+	Request Clock = (Input Clock * M )/(N * X)
+
+	Input Clock = 14318181 hz
+	X = 2 power D
+	D ={0,1,2,3,4,5,6}
+	M = {1,...,255}
+	N = {2,...,15}
+*/
+unsigned int calcPllValue(unsigned int request_orig, pll_value_t *pll)
+{
+	/* used for primary and secondary channel pixel clock pll */
+	static pllcalparam xparm_PIXEL[] = {
+		/* 2^0 = 1*/			{0, 0, 0, 1},
+		/* 2^ 1 =2*/			{1, 0, 1, 2},
+		/* 2^ 2  = 4*/		{2, 0, 2, 4},
+							{3, 0, 3, 8},
+							{4, 1, 3, 16},
+							{5, 2, 3, 32},
+		/* 2^6 = 64  */		{6, 3, 3, 64},
+							};
+
+	/* used for MXCLK (chip clock) */
+	static pllcalparam xparm_MXCLK[] = {
+		/* 2^0 = 1*/			{0, 0, 0, 1},
+		/* 2^ 1 =2*/			{1, 0, 1, 2},
+		/* 2^ 2  = 4*/		{2, 0, 2, 4},
+							{3, 0, 3, 8},
+							};
+
+	/* as sm750 register definition, N located in 2,15 and M located in 1,255	*/
+	int N, M, X, d;
+	int xcnt;
+	int miniDiff;
+	unsigned int RN, quo, rem, fl_quo;
+	unsigned int input, request;
+	unsigned int tmpClock, ret;
+	pllcalparam * xparm;
+
+#if 1
+	if (getChipType() == SM750LE) {
+		/* SM750LE don't have prgrammable PLL and M/N values to work on.
+		Just return the requested clock. */
+		return request_orig;
+	}
+#endif
+
+	ret = 0;
+	miniDiff = ~0;
+	request = request_orig / 1000;
+	input = pll->inputFreq / 1000;
+
+	/* for MXCLK register , no POD provided, so need be treated differently	*/
+
+	if (pll->clockType != MXCLK_PLL) {
+		xparm = &xparm_PIXEL[0];
+		xcnt = sizeof(xparm_PIXEL)/sizeof(xparm_PIXEL[0]);
+	} else {
+		xparm = &xparm_MXCLK[0];
+		xcnt = sizeof(xparm_MXCLK)/sizeof(xparm_MXCLK[0]);
+	}
+
+
+	for (N = 15; N > 1; N--) {
+		/* RN will not exceed maximum long if @request <= 285 MHZ (for 32bit cpu) */
+		RN = N * request;
+		quo = RN / input;
+		rem = RN % input;/* rem always small than 14318181 */
+		fl_quo = (rem * 10000 /input);
+
+		for (d = xcnt - 1; d >= 0; d--) {
+			X = xparm[d].value;
+			M = quo*X;
+			M += fl_quo * X / 10000;
+			/* round step */
+			M += (fl_quo*X % 10000)>5000?1:0;
+			if (M < 256 && M > 0) {
+				unsigned int diff;
+				tmpClock = pll->inputFreq *M / N / X;
+				diff = absDiff(tmpClock, request_orig);
+				if (diff < miniDiff) {
+					pll->M = M;
+					pll->N = N;
+					pll->OD = xparm[d].od;
+					pll->POD = xparm[d].pod;
+					miniDiff = diff;
+					ret = tmpClock;
+				}
+			}
+		}
+	}
+
+	//printk("Finally:  pll->n[%lu],m[%lu],od[%lu],pod[%lu]\n",pll->N,pll->M,pll->OD,pll->POD);
+	return ret;
+}
+
+unsigned int calcPllValue2(
+unsigned int ulRequestClk, /* Required pixel clock in Hz unit */
+pll_value_t *pPLL           /* Structure to hold the value to be set in PLL */
+)
+{
+	unsigned int M, N, OD, POD = 0, diff, pllClk, odPower, podPower;
+	unsigned int bestDiff = 0xffffffff; /* biggest 32 bit unsigned number */
+	unsigned int ret;
+    /* Init PLL structure to know states */
+	pPLL->M = 0;
+	pPLL->N = 0;
+	pPLL->OD = 0;
+	pPLL->POD = 0;
+
+    /* Sanity check: None at the moment */
+
+    /* Convert everything in Khz range in order to avoid calculation overflow */
+	pPLL->inputFreq /= 1000;
+	ulRequestClk /= 1000;
+
+#ifndef VALIDATION_CHIP
+    /* The maximum of post divider is 8. */
+	for (POD = 0; POD <= 3; POD++)
+#endif
+		{
+
+#ifndef VALIDATION_CHIP
+	/* MXCLK_PLL does not have post divider. */
+	if ((POD > 0) && (pPLL->clockType == MXCLK_PLL))
+		break;
+#endif
+
+	/* Work out 2 to the power of POD */
+	podPower = twoToPowerOfx(POD);
+
+	/* OD has only 2 bits [15:14] and its value must between 0 to 3 */
+	for (OD = 0; OD <= 3; OD++) {
+		/* Work out 2 to the power of OD */
+		odPower = twoToPowerOfx(OD);
+
+#ifdef VALIDATION_CHIP
+	if (odPower > 4)
+		podPower = 4;
+	else
+		podPower = odPower;
+#endif
+
+		/* N has 4 bits [11:8] and its value must between 2 and 15.
+		The N == 1 will behave differently --> Result is not correct. */
+	for (N = 2; N <= 15; N++) {
+		/* The formula for PLL is ulRequestClk = inputFreq * M / N / (2^OD)
+		In the following steps, we try to work out a best M value given the others are known.
+		To avoid decimal calculation, we use 1000 as multiplier for up to 3 decimal places of accuracy.
+		*/
+		M = ulRequestClk * N * odPower * 1000 / pPLL->inputFreq;
+		M = roundedDiv(M, 1000);
+
+		/* M field has only 8 bits, reject value bigger than 8 bits */
+		if (M < 256) {
+			/* Calculate the actual clock for a given M & N */
+			pllClk = pPLL->inputFreq * M / N / odPower / podPower;
+
+			/* How much are we different from the requirement */
+			diff = absDiff(pllClk, ulRequestClk);
+
+			if (diff < bestDiff) {
+				bestDiff = diff;
+
+				/* Store M and N values */
+				pPLL->M  = M;
+				pPLL->N  = N;
+				pPLL->OD = OD;
+
+#ifdef VALIDATION_CHIP
+			if (OD > 2)
+				POD = 2;
+			else
+				POD = OD;
+#endif
+
+			pPLL->POD = POD;
+			}
+		}
+	}
+	}
+	}
+
+    /* Restore input frequency from Khz to hz unit */
+//    pPLL->inputFreq *= 1000;
+	ulRequestClk *= 1000;
+	pPLL->inputFreq = DEFAULT_INPUT_CLOCK; /* Default reference clock */
+
+    /* Output debug information */
+	//DDKDEBUGPRINT((DISPLAY_LEVEL, "calcPllValue: Requested Frequency = %d\n", ulRequestClk));
+	//DDKDEBUGPRINT((DISPLAY_LEVEL, "calcPllValue: Input CLK = %dHz, M=%d, N=%d, OD=%d, POD=%d\n", pPLL->inputFreq, pPLL->M, pPLL->N, pPLL->OD, pPLL->POD));
+
+    /* Return actual frequency that the PLL can set */
+	ret = calcPLL(pPLL);
+	return ret;
+}
+
+
+
+
+
+unsigned int formatPllReg(pll_value_t *pPLL)
+{
+	unsigned int ulPllReg = 0;
+
+    /* Note that all PLL's have the same format. Here, we just use Panel PLL parameter
+       to work out the bit fields in the register.
+       On returning a 32 bit number, the value can be applied to any PLL in the calling function.
+    */
+	ulPllReg =
+	FIELD_SET(  0, PANEL_PLL_CTRL, BYPASS, OFF)
+	| FIELD_SET(  0, PANEL_PLL_CTRL, POWER,  ON)
+	| FIELD_SET(  0, PANEL_PLL_CTRL, INPUT,  OSC)
+#ifndef VALIDATION_CHIP
+	| FIELD_VALUE(0, PANEL_PLL_CTRL, POD,    pPLL->POD)
+#endif
+	| FIELD_VALUE(0, PANEL_PLL_CTRL, OD,     pPLL->OD)
+	| FIELD_VALUE(0, PANEL_PLL_CTRL, N,      pPLL->N)
+	| FIELD_VALUE(0, PANEL_PLL_CTRL, M,      pPLL->M);
+
+    return ulPllReg;
+}
+
+
diff --git a/drivers/staging/sm750fb/ddk750_chip.h b/drivers/staging/sm750fb/ddk750_chip.h
new file mode 100644
index 000000000000..04cb0d559245
--- /dev/null
+++ b/drivers/staging/sm750fb/ddk750_chip.h
@@ -0,0 +1,87 @@
+#ifndef DDK750_CHIP_H__
+#define DDK750_CHIP_H__
+#define DEFAULT_INPUT_CLOCK 14318181 /* Default reference clock */
+#ifndef SM750LE_REVISION_ID
+#define SM750LE_REVISION_ID ((unsigned char)0xfe)
+#endif
+
+#include <linux/io.h>
+
+/* This is all the chips recognized by this library */
+typedef enum _logical_chip_type_t
+{
+    SM_UNKNOWN,
+    SM718,
+    SM750,
+    SM750LE,
+}
+logical_chip_type_t;
+
+
+typedef enum _clock_type_t
+{
+	MXCLK_PLL,
+	PRIMARY_PLL,
+	SECONDARY_PLL,
+	VGA0_PLL,
+	VGA1_PLL,
+}
+clock_type_t;
+
+typedef struct _pll_value_t
+{
+    clock_type_t clockType;
+    unsigned long inputFreq; /* Input clock frequency to the PLL */
+
+    /* Use this when clockType = PANEL_PLL */
+    unsigned long M;
+    unsigned long N;
+    unsigned long OD;
+    unsigned long POD;
+}
+pll_value_t;
+
+/* input struct to initChipParam() function */
+typedef struct _initchip_param_t
+{
+    unsigned short powerMode;    /* Use power mode 0 or 1 */
+    unsigned short chipClock;    /* Speed of main chip clock in MHz unit
+                                    0 = keep the current clock setting
+                                    Others = the new main chip clock
+                                  */
+    unsigned short memClock;     /* Speed of memory clock in MHz unit
+                                    0 = keep the current clock setting
+                                    Others = the new memory clock
+                                  */
+    unsigned short masterClock;  /* Speed of master clock in MHz unit
+                                    0 = keep the current clock setting
+                                    Others = the new master clock
+                                  */
+    unsigned short setAllEngOff; /* 0 = leave all engine state untouched.
+                                    1 = make sure they are off: 2D, Overlay,
+                                    video alpha, alpha, hardware cursors
+                                 */
+    unsigned char resetMemory;   /* 0 = Do not reset the memory controller
+                                    1 = Reset the memory controller
+                                  */
+
+    /* More initialization parameter can be added if needed */
+}
+initchip_param_t;
+
+
+logical_chip_type_t getChipType(void);
+unsigned int calcPllValue(unsigned int request,pll_value_t *pll);
+unsigned int calcPllValue2(unsigned int,pll_value_t *);
+unsigned int formatPllReg(pll_value_t *pPLL);
+void ddk750_set_mmio(void __iomem *,unsigned short,char);
+unsigned int ddk750_getVMSize(void);
+int ddk750_initHw(initchip_param_t *);
+unsigned int getPllValue(clock_type_t clockType, pll_value_t *pPLL);
+unsigned int getChipClock(void);
+void setChipClock(unsigned int);
+void setMemoryClock(unsigned int frequency);
+void setMasterClock(unsigned int frequency);
+
+
+#endif
diff --git a/drivers/staging/sm750fb/ddk750_display.c b/drivers/staging/sm750fb/ddk750_display.c
new file mode 100644
index 000000000000..c84196ac055d
--- /dev/null
+++ b/drivers/staging/sm750fb/ddk750_display.c
@@ -0,0 +1,307 @@
+#include "ddk750_reg.h"
+#include "ddk750_help.h"
+#include "ddk750_display.h"
+#include "ddk750_power.h"
+#include "ddk750_dvi.h"
+
+#define primaryWaitVerticalSync(delay) waitNextVerticalSync(0,delay)
+
+static void setDisplayControl(int ctrl,int dispState)
+{
+	/* state != 0 means turn on both timing & plane en_bit */
+	unsigned long ulDisplayCtrlReg, ulReservedBits;
+	int cnt;
+
+	cnt = 0;
+
+	/* Set the primary display control */
+	if (!ctrl)
+	{
+		ulDisplayCtrlReg = PEEK32(PANEL_DISPLAY_CTRL);
+		/* Turn on/off the Panel display control */
+		if (dispState)
+		{
+			/* Timing should be enabled first before enabling the plane
+			 * because changing at the same time does not guarantee that
+			 * the plane will also enabled or disabled.
+     	     */
+			ulDisplayCtrlReg = FIELD_SET(ulDisplayCtrlReg,
+								PANEL_DISPLAY_CTRL, TIMING, ENABLE);
+			POKE32(PANEL_DISPLAY_CTRL, ulDisplayCtrlReg);
+
+			ulDisplayCtrlReg = FIELD_SET(ulDisplayCtrlReg,
+								PANEL_DISPLAY_CTRL, PLANE, ENABLE);
+
+			/* Added some masks to mask out the reserved bits.
+			 * Sometimes, the reserved bits are set/reset randomly when
+			 * writing to the PRIMARY_DISPLAY_CTRL, therefore, the register
+			 * reserved bits are needed to be masked out.
+			 */
+			ulReservedBits = FIELD_SET(0, PANEL_DISPLAY_CTRL, RESERVED_1_MASK, ENABLE) |
+				FIELD_SET(0, PANEL_DISPLAY_CTRL, RESERVED_2_MASK, ENABLE) |
+				FIELD_SET(0, PANEL_DISPLAY_CTRL, RESERVED_3_MASK, ENABLE);
+
+			/* Somehow the register value on the plane is not set
+			 * until a few delay. Need to write
+			 * and read it a couple times
+			 */
+			do
+			{
+				cnt++;
+				POKE32(PANEL_DISPLAY_CTRL, ulDisplayCtrlReg);
+			} while((PEEK32(PANEL_DISPLAY_CTRL) & ~ulReservedBits) !=
+					(ulDisplayCtrlReg & ~ulReservedBits));
+			printk("Set Panel Plane enbit:after tried %d times\n",cnt);
+		}
+		else
+		{
+			/* When turning off, there is no rule on the programming
+			 * sequence since whenever the clock is off, then it does not
+			 * matter whether the plane is enabled or disabled.
+			 * Note: Modifying the plane bit will take effect on the
+			 * next vertical sync. Need to find out if it is necessary to
+			 * wait for 1 vsync before modifying the timing enable bit.
+			 * */
+			ulDisplayCtrlReg = FIELD_SET(ulDisplayCtrlReg,
+								PANEL_DISPLAY_CTRL, PLANE, DISABLE);
+			POKE32(PANEL_DISPLAY_CTRL, ulDisplayCtrlReg);
+
+			ulDisplayCtrlReg = FIELD_SET(ulDisplayCtrlReg,
+								PANEL_DISPLAY_CTRL, TIMING, DISABLE);
+			POKE32(PANEL_DISPLAY_CTRL, ulDisplayCtrlReg);
+		}
+
+	}
+	/* Set the secondary display control */
+	else
+	{
+		ulDisplayCtrlReg = PEEK32(CRT_DISPLAY_CTRL);
+
+		if (dispState)
+		{
+			/* Timing should be enabled first before enabling the plane because changing at the
+			   same time does not guarantee that the plane will also enabled or disabled.
+			   */
+			ulDisplayCtrlReg = FIELD_SET(ulDisplayCtrlReg,
+								CRT_DISPLAY_CTRL, TIMING, ENABLE);
+			POKE32(CRT_DISPLAY_CTRL, ulDisplayCtrlReg);
+
+			ulDisplayCtrlReg = FIELD_SET(ulDisplayCtrlReg,
+								CRT_DISPLAY_CTRL, PLANE, ENABLE);
+
+			/* Added some masks to mask out the reserved bits.
+			 * Sometimes, the reserved bits are set/reset randomly when
+			 * writing to the PRIMARY_DISPLAY_CTRL, therefore, the register
+			 * reserved bits are needed to be masked out.
+			 */
+
+			ulReservedBits = FIELD_SET(0, CRT_DISPLAY_CTRL, RESERVED_1_MASK, ENABLE) |
+				FIELD_SET(0, CRT_DISPLAY_CTRL, RESERVED_2_MASK, ENABLE) |
+				FIELD_SET(0, CRT_DISPLAY_CTRL, RESERVED_3_MASK, ENABLE) |
+				FIELD_SET(0, CRT_DISPLAY_CTRL, RESERVED_4_MASK, ENABLE);
+
+			do
+			{
+				cnt++;
+				POKE32(CRT_DISPLAY_CTRL, ulDisplayCtrlReg);
+			} while((PEEK32(CRT_DISPLAY_CTRL) & ~ulReservedBits) !=
+					(ulDisplayCtrlReg & ~ulReservedBits));
+				printk("Set Crt Plane enbit:after tried %d times\n",cnt);
+		}
+		else
+		{
+			/* When turning off, there is no rule on the programming
+			 * sequence since whenever the clock is off, then it does not
+			 * matter whether the plane is enabled or disabled.
+			 * Note: Modifying the plane bit will take effect on the next
+			 * vertical sync. Need to find out if it is necessary to
+			 * wait for 1 vsync before modifying the timing enable bit.
+			 */
+			ulDisplayCtrlReg = FIELD_SET(ulDisplayCtrlReg,
+								CRT_DISPLAY_CTRL, PLANE, DISABLE);
+			POKE32(CRT_DISPLAY_CTRL, ulDisplayCtrlReg);
+
+			ulDisplayCtrlReg = FIELD_SET(ulDisplayCtrlReg,
+								CRT_DISPLAY_CTRL, TIMING, DISABLE);
+			POKE32(CRT_DISPLAY_CTRL, ulDisplayCtrlReg);
+		}
+	}
+}
+
+
+static void waitNextVerticalSync(int ctrl,int delay)
+{
+	unsigned int status;
+	if(!ctrl){
+		/* primary controller */
+
+        /* Do not wait when the Primary PLL is off or display control is already off.
+	           This will prevent the software to wait forever. */
+		if ((FIELD_GET(PEEK32(PANEL_PLL_CTRL), PANEL_PLL_CTRL, POWER) ==
+			 PANEL_PLL_CTRL_POWER_OFF) ||
+			(FIELD_GET(PEEK32(PANEL_DISPLAY_CTRL), PANEL_DISPLAY_CTRL, TIMING) ==
+			 PANEL_DISPLAY_CTRL_TIMING_DISABLE))
+		{
+			return;
+		}
+
+        while (delay-- > 0)
+        {
+            /* Wait for end of vsync. */
+            do
+            {
+                status = FIELD_GET(PEEK32(SYSTEM_CTRL),
+                                   SYSTEM_CTRL,
+                                   PANEL_VSYNC);
+            }
+            while (status == SYSTEM_CTRL_PANEL_VSYNC_ACTIVE);
+
+            /* Wait for start of vsync. */
+            do
+            {
+                status = FIELD_GET(PEEK32(SYSTEM_CTRL),
+                                   SYSTEM_CTRL,
+                                   PANEL_VSYNC);
+            }
+            while (status == SYSTEM_CTRL_PANEL_VSYNC_INACTIVE);
+        }
+
+	}else{
+
+		/* Do not wait when the Primary PLL is off or display control is already off.
+			   This will prevent the software to wait forever. */
+		if ((FIELD_GET(PEEK32(CRT_PLL_CTRL), CRT_PLL_CTRL, POWER) ==
+			 CRT_PLL_CTRL_POWER_OFF) ||
+			(FIELD_GET(PEEK32(CRT_DISPLAY_CTRL), CRT_DISPLAY_CTRL, TIMING) ==
+			 CRT_DISPLAY_CTRL_TIMING_DISABLE))
+		{
+			return;
+		}
+
+		while (delay-- > 0)
+		{
+			/* Wait for end of vsync. */
+			do
+			{
+				status = FIELD_GET(PEEK32(SYSTEM_CTRL),
+								   SYSTEM_CTRL,
+								   CRT_VSYNC);
+			}
+			while (status == SYSTEM_CTRL_CRT_VSYNC_ACTIVE);
+
+			/* Wait for start of vsync. */
+			do
+			{
+				status = FIELD_GET(PEEK32(SYSTEM_CTRL),
+								   SYSTEM_CTRL,
+								   CRT_VSYNC);
+			}
+			while (status == SYSTEM_CTRL_CRT_VSYNC_INACTIVE);
+		}
+	}
+}
+
+static void swPanelPowerSequence(int disp,int delay)
+{
+	unsigned int reg;
+
+	/* disp should be 1 to open sequence */
+	reg = PEEK32(PANEL_DISPLAY_CTRL);
+	reg = FIELD_VALUE(reg,PANEL_DISPLAY_CTRL,FPEN,disp);
+	POKE32(PANEL_DISPLAY_CTRL,reg);
+	primaryWaitVerticalSync(delay);
+
+
+	reg = PEEK32(PANEL_DISPLAY_CTRL);
+	reg = FIELD_VALUE(reg,PANEL_DISPLAY_CTRL,DATA,disp);
+	POKE32(PANEL_DISPLAY_CTRL,reg);
+	primaryWaitVerticalSync(delay);
+
+	reg = PEEK32(PANEL_DISPLAY_CTRL);
+	reg = FIELD_VALUE(reg,PANEL_DISPLAY_CTRL,VBIASEN,disp);
+	POKE32(PANEL_DISPLAY_CTRL,reg);
+	primaryWaitVerticalSync(delay);
+
+
+	reg = PEEK32(PANEL_DISPLAY_CTRL);
+	reg = FIELD_VALUE(reg,PANEL_DISPLAY_CTRL,FPEN,disp);
+	POKE32(PANEL_DISPLAY_CTRL,reg);
+	primaryWaitVerticalSync(delay);
+
+}
+
+void ddk750_setLogicalDispOut(disp_output_t output)
+{
+	unsigned int reg;
+	if(output & PNL_2_USAGE){
+		/* set panel path controller select */
+		reg = PEEK32(PANEL_DISPLAY_CTRL);
+		reg = FIELD_VALUE(reg,PANEL_DISPLAY_CTRL,SELECT,(output & PNL_2_MASK)>>PNL_2_OFFSET);
+		POKE32(PANEL_DISPLAY_CTRL,reg);
+	}
+
+	if(output & CRT_2_USAGE){
+		/* set crt path controller select */
+		reg = PEEK32(CRT_DISPLAY_CTRL);
+		reg = FIELD_VALUE(reg,CRT_DISPLAY_CTRL,SELECT,(output & CRT_2_MASK)>>CRT_2_OFFSET);
+		/*se blank off */
+		reg = FIELD_SET(reg,CRT_DISPLAY_CTRL,BLANK,OFF);
+		POKE32(CRT_DISPLAY_CTRL,reg);
+
+	}
+
+	if(output & PRI_TP_USAGE){
+		/* set primary timing and plane en_bit */
+		setDisplayControl(0,(output&PRI_TP_MASK)>>PRI_TP_OFFSET);
+	}
+
+	if(output & SEC_TP_USAGE){
+		/* set secondary timing and plane en_bit*/
+		setDisplayControl(1,(output&SEC_TP_MASK)>>SEC_TP_OFFSET);
+	}
+
+	if(output & PNL_SEQ_USAGE){
+		/* set  panel sequence */
+		swPanelPowerSequence((output&PNL_SEQ_MASK)>>PNL_SEQ_OFFSET,4);
+	}
+
+	if(output & DAC_USAGE)
+		setDAC((output & DAC_MASK)>>DAC_OFFSET);
+
+	if(output & DPMS_USAGE)
+		ddk750_setDPMS((output & DPMS_MASK) >> DPMS_OFFSET);
+}
+
+
+int ddk750_initDVIDisp(void)
+{
+    /* Initialize DVI. If the dviInit fail and the VendorID or the DeviceID are
+       not zeroed, then set the failure flag. If it is zeroe, it might mean
+       that the system is in Dual CRT Monitor configuration. */
+
+    /* De-skew enabled with default 111b value.
+       This will fix some artifacts problem in some mode on board 2.2.
+       Somehow this fix does not affect board 2.1.
+     */
+    if ((dviInit(1,  /* Select Rising Edge */
+                1,  /* Select 24-bit bus */
+                0,  /* Select Single Edge clock */
+                1,  /* Enable HSync as is */
+                1,  /* Enable VSync as is */
+                1,  /* Enable De-skew */
+                7,  /* Set the de-skew setting to maximum setup */
+                1,  /* Enable continuous Sync */
+                1,  /* Enable PLL Filter */
+                4   /* Use the recommended value for PLL Filter value */
+        ) != 0) && (dviGetVendorID() != 0x0000) && (dviGetDeviceID() != 0x0000))
+    {
+        return (-1);
+    }
+
+    /* TODO: Initialize other display component */
+
+    /* Success */
+    return 0;
+
+}
+
diff --git a/drivers/staging/sm750fb/ddk750_display.h b/drivers/staging/sm750fb/ddk750_display.h
new file mode 100644
index 000000000000..ae0f84c68de5
--- /dev/null
+++ b/drivers/staging/sm750fb/ddk750_display.h
@@ -0,0 +1,160 @@
+#ifndef DDK750_DISPLAY_H__
+#define DDK750_DISPLAY_H__
+
+/* panel path select
+	80000[29:28]
+*/
+
+#define PNL_2_OFFSET 0
+#define PNL_2_MASK (3 << PNL_2_OFFSET)
+#define PNL_2_USAGE	(PNL_2_MASK << 16)
+#define PNL_2_PRI 	((0 << PNL_2_OFFSET)|PNL_2_USAGE)
+#define PNL_2_SEC	((2 << PNL_2_OFFSET)|PNL_2_USAGE)
+
+
+/* primary timing & plane enable bit
+	1: 80000[8] & 80000[2] on
+	0: both off
+*/
+#define PRI_TP_OFFSET 4
+#define PRI_TP_MASK (1 << PRI_TP_OFFSET)
+#define PRI_TP_USAGE (PRI_TP_MASK << 16)
+#define PRI_TP_ON ((0x1 << PRI_TP_OFFSET)|PRI_TP_USAGE)
+#define PRI_TP_OFF ((0x0 << PRI_TP_OFFSET)|PRI_TP_USAGE)
+
+
+/* panel sequency status
+	80000[27:24]
+*/
+#define PNL_SEQ_OFFSET 6
+#define PNL_SEQ_MASK (1 << PNL_SEQ_OFFSET)
+#define PNL_SEQ_USAGE (PNL_SEQ_MASK << 16)
+#define PNL_SEQ_ON ((1 << PNL_SEQ_OFFSET)|PNL_SEQ_USAGE)
+#define PNL_SEQ_OFF ((0 << PNL_SEQ_OFFSET)|PNL_SEQ_USAGE)
+
+/* dual digital output
+	80000[19]
+*/
+#define DUAL_TFT_OFFSET 8
+#define DUAL_TFT_MASK (1 << DUAL_TFT_OFFSET)
+#define DUAL_TFT_USAGE (DUAL_TFT_MASK << 16)
+#define DUAL_TFT_ON ((1 << DUAL_TFT_OFFSET)|DUAL_TFT_USAGE)
+#define DUAL_TFT_OFF ((0 << DUAL_TFT_OFFSET)|DUAL_TFT_USAGE)
+
+/* secondary timing & plane enable bit
+	1:80200[8] & 80200[2] on
+	0: both off
+*/
+#define SEC_TP_OFFSET 5
+#define SEC_TP_MASK (1<< SEC_TP_OFFSET)
+#define SEC_TP_USAGE (SEC_TP_MASK << 16)
+#define SEC_TP_ON  ((0x1 << SEC_TP_OFFSET)|SEC_TP_USAGE)
+#define SEC_TP_OFF ((0x0 << SEC_TP_OFFSET)|SEC_TP_USAGE)
+
+/* crt path select
+	80200[19:18]
+*/
+#define CRT_2_OFFSET 2
+#define CRT_2_MASK (3 << CRT_2_OFFSET)
+#define CRT_2_USAGE (CRT_2_MASK << 16)
+#define CRT_2_PRI ((0x0 << CRT_2_OFFSET)|CRT_2_USAGE)
+#define CRT_2_SEC ((0x2 << CRT_2_OFFSET)|CRT_2_USAGE)
+
+
+/* DAC affect both DVI and DSUB
+	4[20]
+*/
+#define DAC_OFFSET 7
+#define DAC_MASK (1 << DAC_OFFSET)
+#define DAC_USAGE (DAC_MASK << 16)
+#define DAC_ON ((0x0<< DAC_OFFSET)|DAC_USAGE)
+#define DAC_OFF ((0x1 << DAC_OFFSET)|DAC_USAGE)
+
+/* DPMS only affect D-SUB head
+	0[31:30]
+*/
+#define DPMS_OFFSET 9
+#define DPMS_MASK (3 << DPMS_OFFSET)
+#define DPMS_USAGE (DPMS_MASK << 16)
+#define DPMS_OFF ((3 << DPMS_OFFSET)|DPMS_USAGE)
+#define DPMS_ON ((0 << DPMS_OFFSET)|DPMS_USAGE)
+
+
+
+/*
+	LCD1 means panel path TFT1  & panel path DVI (so enable DAC)
+	CRT means crt path DSUB
+*/
+#if 0
+typedef enum _disp_output_t
+{
+	NO_DISPLAY = DPMS_OFF,
+
+	LCD1_PRI = PNL_2_PRI|PRI_TP_ON|PNL_SEQ_ON|DPMS_OFF|DAC_ON,
+	LCD1_SEC = PNL_2_SEC|SEC_TP_ON|PNL_SEQ_ON|DPMS_OFF|DAC_ON,
+
+	LCD2_PRI = CRT_2_PRI|PRI_TP_ON|DUAL_TFT_ON|DPMS_OFF,
+	LCD2_SEC = CRT_2_SEC|SEC_TP_ON|DUAL_TFT_ON|DPMS_OFF,
+
+	DSUB_PRI = CRT_2_PRI|PRI_TP_ON|DAC_ON,
+	DSUB_SEC = CRT_2_SEC|SEC_TP_ON|DAC_ON,
+
+	LCD1_DSUB_PRI = PNL_2_PRI|PRI_TP_ON|PNL_SEQ_ON|
+					CRT_2_PRI|SEC_TP_OFF|DAC_ON,
+
+	LCD1_DSUB_SEC = PNL_2_SEC|SEC_TP_ON|PNL_SEQ_ON|
+					CRT_2_SEC|PRI_TP_OFF|DAC_ON,
+
+	/* LCD1 show primary and DSUB show secondary */
+	LCD1_DSUB_DUAL = PNL_2_PRI|PRI_TP_ON|PNL_SEQ_ON|
+					 CRT_2_SEC|SEC_TP_ON|DAC_ON,
+
+	/* LCD1 show secondary and DSUB show primary */
+	LCD1_DSUB_DUAL_SWAP = PNL_2_SEC|SEC_TP_ON|PNL_SEQ_ON|
+							CRT_2_PRI|PRI_TP_ON|DAC_ON,
+
+	LCD1_LCD2_PRI = PNL_2_PRI|PRI_TP_ON|PNL_SEQ_ON|
+					CRT_2_PRI|SEC_TP_OFF|DPMS_OFF|DUAL_TFT_ON,
+
+	LCD1_LCD2_SEC = PNL_2_SEC|SEC_TP_ON|PNL_SEQ_ON|
+					CRT_2_SEC|PRI_TP_OFF|DPMS_OFF|DUAL_TFT_ON,
+
+	LCD1_LCD2_DSUB_PRI = PNL_2_PRI|PRI_TP_ON|PNL_SEQ_ON|DAC_ON|
+						CRT_2_PRI|SEC_TP_OFF|DPMS_ON|DUAL_TFT_ON,
+
+	LCD1_LCD2_DSUB_SEC = PNL_2_SEC|SEC_TP_ON|PNL_SEQ_ON|DAC_ON|
+						CRT_2_SEC|PRI_TP_OFF|DPMS_ON|DUAL_TFT_ON,
+
+
+}
+disp_output_t;
+#else
+typedef enum _disp_output_t{
+	do_LCD1_PRI = PNL_2_PRI|PRI_TP_ON|PNL_SEQ_ON|DAC_ON,
+	do_LCD1_SEC = PNL_2_SEC|SEC_TP_ON|PNL_SEQ_ON|DAC_ON,
+#if 0
+	do_LCD2_PRI = CRT_2_PRI|PRI_TP_ON,
+	do_LCD2_SEC = CRT_2_SEC|SEC_TP_ON,
+#else
+	do_LCD2_PRI = CRT_2_PRI|PRI_TP_ON|DUAL_TFT_ON,
+	do_LCD2_SEC = CRT_2_SEC|SEC_TP_ON|DUAL_TFT_ON,
+#endif
+	/*
+	do_DSUB_PRI = CRT_2_PRI|PRI_TP_ON|DPMS_ON|DAC_ON,
+	do_DSUB_SEC = CRT_2_SEC|SEC_TP_ON|DPMS_ON|DAC_ON,
+	*/
+#if 0
+	do_CRT_PRI = CRT_2_PRI|PRI_TP_ON,
+	do_CRT_SEC = CRT_2_SEC|SEC_TP_ON,
+#else
+	do_CRT_PRI = CRT_2_PRI|PRI_TP_ON|DPMS_ON|DAC_ON,
+	do_CRT_SEC = CRT_2_SEC|SEC_TP_ON|DPMS_ON|DAC_ON,
+#endif
+}
+disp_output_t;
+#endif
+
+void ddk750_setLogicalDispOut(disp_output_t);
+int ddk750_initDVIDisp(void);
+
+#endif
diff --git a/drivers/staging/sm750fb/ddk750_dvi.c b/drivers/staging/sm750fb/ddk750_dvi.c
new file mode 100644
index 000000000000..f5932bbf13e8
--- /dev/null
+++ b/drivers/staging/sm750fb/ddk750_dvi.c
@@ -0,0 +1,99 @@
+#define USE_DVICHIP 
+#ifdef USE_DVICHIP
+#include "ddk750_help.h"
+#include "ddk750_reg.h"
+#include "ddk750_dvi.h"
+#include "ddk750_sii164.h"
+
+
+/* This global variable contains all the supported driver and its corresponding
+   function API. Please set the function pointer to NULL whenever the function
+   is not supported. */
+static dvi_ctrl_device_t g_dcftSupportedDviController[] =
+{
+#ifdef DVI_CTRL_SII164
+    {
+        .pfnInit = sii164InitChip,
+        .pfnGetVendorId = sii164GetVendorID,
+        .pfnGetDeviceId = sii164GetDeviceID,
+#ifdef SII164_FULL_FUNCTIONS
+        .pfnResetChip = sii164ResetChip,
+        .pfnGetChipString = sii164GetChipString,
+        .pfnSetPower = sii164SetPower,
+        .pfnEnableHotPlugDetection = sii164EnableHotPlugDetection,
+        .pfnIsConnected = sii164IsConnected,
+        .pfnCheckInterrupt = sii164CheckInterrupt,
+        .pfnClearInterrupt = sii164ClearInterrupt,
+#endif
+    },
+#endif
+};
+
+
+int dviInit(
+    unsigned char edgeSelect,
+    unsigned char busSelect,
+    unsigned char dualEdgeClkSelect,
+    unsigned char hsyncEnable,
+    unsigned char vsyncEnable,
+    unsigned char deskewEnable,
+    unsigned char deskewSetting,
+    unsigned char continuousSyncEnable,
+    unsigned char pllFilterEnable,
+    unsigned char pllFilterValue
+			)
+{
+	dvi_ctrl_device_t *pCurrentDviCtrl;
+	pCurrentDviCtrl = g_dcftSupportedDviController;
+	if(pCurrentDviCtrl->pfnInit != NULL)
+	{
+		return pCurrentDviCtrl->pfnInit(edgeSelect, busSelect, dualEdgeClkSelect, hsyncEnable,
+                              vsyncEnable, deskewEnable, deskewSetting, continuousSyncEnable,
+                              pllFilterEnable, pllFilterValue);
+	}
+	return -1;//error
+}
+
+
+/*
+ *  dviGetVendorID
+ *      This function gets the vendor ID of the DVI controller chip.
+ *
+ *  Output:
+ *      Vendor ID
+ */
+unsigned short dviGetVendorID(void)
+{
+    dvi_ctrl_device_t *pCurrentDviCtrl;
+
+    //pCurrentDviCtrl = getDviCtrl();
+    pCurrentDviCtrl = g_dcftSupportedDviController;
+    if (pCurrentDviCtrl != (dvi_ctrl_device_t *)0)
+        return pCurrentDviCtrl->pfnGetVendorId();
+
+    return 0x0000;
+}
+
+
+/*
+ *  dviGetDeviceID
+ *      This function gets the device ID of the DVI controller chip.
+ *
+ *  Output:
+ *      Device ID
+ */
+unsigned short dviGetDeviceID(void)
+{
+    dvi_ctrl_device_t *pCurrentDviCtrl;
+
+//    pCurrentDviCtrl = getDviCtrl();
+	pCurrentDviCtrl = g_dcftSupportedDviController;
+    if (pCurrentDviCtrl != (dvi_ctrl_device_t *)0)
+        return pCurrentDviCtrl->pfnGetDeviceId();
+
+    return 0x0000;
+}
+
+#endif
+
+
diff --git a/drivers/staging/sm750fb/ddk750_dvi.h b/drivers/staging/sm750fb/ddk750_dvi.h
new file mode 100644
index 000000000000..50bcec29b2c0
--- /dev/null
+++ b/drivers/staging/sm750fb/ddk750_dvi.h
@@ -0,0 +1,67 @@
+#ifndef DDK750_DVI_H__
+#define DDK750_DVI_H__
+
+/* dvi chip stuffs structros */
+
+typedef long (*PFN_DVICTRL_INIT)(
+    unsigned char edgeSelect,
+    unsigned char busSelect,
+    unsigned char dualEdgeClkSelect,
+    unsigned char hsyncEnable,
+    unsigned char vsyncEnable,
+    unsigned char deskewEnable,
+    unsigned char deskewSetting,
+    unsigned char continuousSyncEnable,
+    unsigned char pllFilterEnable,
+    unsigned char pllFilterValue);
+typedef void (*PFN_DVICTRL_RESETCHIP)(void);
+typedef char* (*PFN_DVICTRL_GETCHIPSTRING)(void);
+typedef unsigned short (*PFN_DVICTRL_GETVENDORID)(void);
+typedef unsigned short (*PFN_DVICTRL_GETDEVICEID)(void);
+typedef void (*PFN_DVICTRL_SETPOWER)(unsigned char powerUp);
+typedef void (*PFN_DVICTRL_HOTPLUGDETECTION)(unsigned char enableHotPlug);
+typedef unsigned char (*PFN_DVICTRL_ISCONNECTED)(void);
+typedef unsigned char (*PFN_DVICTRL_CHECKINTERRUPT)(void);
+typedef void (*PFN_DVICTRL_CLEARINTERRUPT)(void);
+
+
+
+/* Structure to hold all the function pointer to the DVI Controller. */
+typedef struct _dvi_ctrl_device_t
+{
+    PFN_DVICTRL_INIT                pfnInit;
+    PFN_DVICTRL_RESETCHIP           pfnResetChip;
+    PFN_DVICTRL_GETCHIPSTRING       pfnGetChipString;
+    PFN_DVICTRL_GETVENDORID         pfnGetVendorId;
+    PFN_DVICTRL_GETDEVICEID         pfnGetDeviceId;
+    PFN_DVICTRL_SETPOWER            pfnSetPower;
+    PFN_DVICTRL_HOTPLUGDETECTION    pfnEnableHotPlugDetection;
+    PFN_DVICTRL_ISCONNECTED         pfnIsConnected;
+    PFN_DVICTRL_CHECKINTERRUPT      pfnCheckInterrupt;
+    PFN_DVICTRL_CLEARINTERRUPT      pfnClearInterrupt;
+} dvi_ctrl_device_t;
+#define DVI_CTRL_SII164
+
+
+
+/* dvi functions prototype */
+int dviInit(
+    unsigned char edgeSelect,
+    unsigned char busSelect,
+    unsigned char dualEdgeClkSelect,
+    unsigned char hsyncEnable,
+    unsigned char vsyncEnable,
+    unsigned char deskewEnable,
+    unsigned char deskewSetting,
+    unsigned char continuousSyncEnable,
+    unsigned char pllFilterEnable,
+    unsigned char pllFilterValue
+);
+
+unsigned short dviGetVendorID(void);
+unsigned short dviGetDeviceID(void);
+
+
+
+#endif
+
diff --git a/drivers/staging/sm750fb/ddk750_help.c b/drivers/staging/sm750fb/ddk750_help.c
new file mode 100644
index 000000000000..c68ff3b5751a
--- /dev/null
+++ b/drivers/staging/sm750fb/ddk750_help.c
@@ -0,0 +1,19 @@
+//#include "ddk750_reg.h"
+//#include "ddk750_chip.h"
+#include "ddk750_help.h"
+
+void __iomem * mmio750 = NULL;
+char revId750 = 0;
+unsigned short devId750 = 0;
+
+/* after driver mapped io registers, use this function first */
+void ddk750_set_mmio(void __iomem * addr,unsigned short devId,char revId)
+{
+	mmio750 = addr;
+	devId750 = devId;
+	revId750 = revId;
+	if(revId == 0xfe)
+		printk("found sm750le\n");
+}
+
+
diff --git a/drivers/staging/sm750fb/ddk750_help.h b/drivers/staging/sm750fb/ddk750_help.h
new file mode 100644
index 000000000000..07c8264fac95
--- /dev/null
+++ b/drivers/staging/sm750fb/ddk750_help.h
@@ -0,0 +1,29 @@
+#ifndef DDK750_HELP_H__
+#define DDK750_HELP_H__
+#include "ddk750_chip.h"
+#ifndef USE_INTERNAL_REGISTER_ACCESS
+
+#include <linux/ioport.h>
+#include <asm/io.h>
+#include <asm/uaccess.h>
+#include "sm750_help.h"
+
+
+#if 0
+/* if 718 big endian turned on,be aware that don't use this driver for general use,only for ppc big-endian */
+#warning "big endian on target cpu and enable nature big endian support of 718 capability !"
+#define PEEK32(addr)  			__raw_readl(mmio750 + addr)
+#define POKE32(addr,data) 		__raw_writel(data, mmio750 + addr)
+#else /* software control endianess */
+#define PEEK32(addr) readl(addr + mmio750)
+#define POKE32(addr,data) writel(data, addr + mmio750)
+#endif
+
+extern void __iomem * mmio750;
+extern char revId750;
+extern unsigned short devId750;
+#else
+/* implement if you want use it*/
+#endif
+
+#endif
diff --git a/drivers/staging/sm750fb/ddk750_hwi2c.c b/drivers/staging/sm750fb/ddk750_hwi2c.c
new file mode 100644
index 000000000000..7826376ed705
--- /dev/null
+++ b/drivers/staging/sm750fb/ddk750_hwi2c.c
@@ -0,0 +1,271 @@
+#define USE_HW_I2C
+#ifdef USE_HW_I2C
+#include "ddk750_help.h"
+#include "ddk750_reg.h"
+#include "ddk750_hwi2c.h"
+#include "ddk750_power.h"
+
+#define MAX_HWI2C_FIFO                  16
+#define HWI2C_WAIT_TIMEOUT              0xF0000
+
+
+int hwI2CInit(
+    unsigned char busSpeedMode
+)
+{
+    unsigned int value;
+
+    /* Enable GPIO 30 & 31 as IIC clock & data */
+	value = PEEK32(GPIO_MUX);
+
+    value = FIELD_SET(value, GPIO_MUX, 30, I2C) |
+			FIELD_SET(0, GPIO_MUX, 31, I2C);
+	POKE32(GPIO_MUX, value);
+
+    /* Enable Hardware I2C power.
+       TODO: Check if we need to enable GPIO power?
+     */
+    enableI2C(1);
+
+    /* Enable the I2C Controller and set the bus speed mode */
+    value = PEEK32(I2C_CTRL);
+    if (busSpeedMode == 0)
+        value = FIELD_SET(value, I2C_CTRL, MODE, STANDARD);
+    else
+        value = FIELD_SET(value, I2C_CTRL, MODE, FAST);
+    value = FIELD_SET(value, I2C_CTRL, EN, ENABLE);
+    POKE32(I2C_CTRL, value);
+
+    return 0;
+}
+
+
+void hwI2CClose(void)
+{
+    unsigned int value;
+
+    /* Disable I2C controller */
+    value = PEEK32(I2C_CTRL);
+    value = FIELD_SET(value, I2C_CTRL, EN, DISABLE);
+    POKE32(I2C_CTRL, value);
+
+    /* Disable I2C Power */
+    enableI2C(0);
+
+    /* Set GPIO 30 & 31 back as GPIO pins */
+    value = PEEK32(GPIO_MUX);
+    value = FIELD_SET(value, GPIO_MUX, 30, GPIO);
+    value = FIELD_SET(value, GPIO_MUX, 31, GPIO);
+    POKE32(GPIO_MUX, value);
+}
+
+
+static long hwI2CWaitTXDone(void)
+{
+    unsigned int timeout;
+
+    /* Wait until the transfer is completed. */
+    timeout = HWI2C_WAIT_TIMEOUT;
+	while ((FIELD_GET(PEEK32(I2C_STATUS), I2C_STATUS, TX) != I2C_STATUS_TX_COMPLETED) &&
+           (timeout != 0))
+		timeout--;
+
+	if (timeout == 0)
+	    return (-1);
+
+    return 0;
+}
+
+
+
+/*
+ *  This function writes data to the i2c slave device registers.
+ *
+ *  Parameters:
+ *      deviceAddress   - i2c Slave device address
+ *      length          - Total number of bytes to be written to the device
+ *      pBuffer         - The buffer that contains the data to be written to the
+ *                     i2c device.
+ *
+ *  Return Value:
+ *      Total number of bytes those are actually written.
+ */
+static unsigned int hwI2CWriteData(
+    unsigned char deviceAddress,
+    unsigned int length,
+    unsigned char *pBuffer
+)
+{
+    unsigned char count, i;
+    unsigned int totalBytes = 0;
+
+    /* Set the Device Address */
+    POKE32(I2C_SLAVE_ADDRESS, deviceAddress & ~0x01);
+
+    /* Write data.
+     * Note:
+     *      Only 16 byte can be accessed per i2c start instruction.
+     */
+    do
+    {
+        /* Reset I2C by writing 0 to I2C_RESET register to clear the previous status. */
+        POKE32(I2C_RESET, 0);
+
+        /* Set the number of bytes to be written */
+        if (length < MAX_HWI2C_FIFO)
+            count = length - 1;
+        else
+            count = MAX_HWI2C_FIFO - 1;
+        POKE32(I2C_BYTE_COUNT, count);
+
+        /* Move the data to the I2C data register */
+	    for (i = 0; i <= count; i++)
+            POKE32(I2C_DATA0 + i, *pBuffer++);
+
+        /* Start the I2C */
+        POKE32(I2C_CTRL, FIELD_SET(PEEK32(I2C_CTRL), I2C_CTRL, CTRL, START));
+
+        /* Wait until the transfer is completed. */
+        if (hwI2CWaitTXDone() != 0)
+            break;
+
+        /* Substract length */
+        length -= (count + 1);
+
+        /* Total byte written */
+        totalBytes += (count + 1);
+
+    } while (length > 0);
+
+    return totalBytes;
+}
+
+
+
+
+/*
+ *  This function reads data from the slave device and stores them
+ *  in the given buffer
+ *
+ *  Parameters:
+ *      deviceAddress   - i2c Slave device address
+ *      length          - Total number of bytes to be read
+ *      pBuffer         - Pointer to a buffer to be filled with the data read
+ *                     from the slave device. It has to be the same size as the
+ *                     length to make sure that it can keep all the data read.
+ *
+ *  Return Value:
+ *      Total number of actual bytes read from the slave device
+ */
+static unsigned int hwI2CReadData(
+    unsigned char deviceAddress,
+    unsigned int length,
+    unsigned char *pBuffer
+)
+{
+    unsigned char count, i;
+    unsigned int totalBytes = 0;
+
+    /* Set the Device Address */
+    POKE32(I2C_SLAVE_ADDRESS, deviceAddress | 0x01);
+
+    /* Read data and save them to the buffer.
+     * Note:
+     *      Only 16 byte can be accessed per i2c start instruction.
+     */
+    do
+    {
+        /* Reset I2C by writing 0 to I2C_RESET register to clear all the status. */
+        POKE32(I2C_RESET, 0);
+
+        /* Set the number of bytes to be read */
+        if (length <= MAX_HWI2C_FIFO)
+            count = length - 1;
+        else
+            count = MAX_HWI2C_FIFO - 1;
+        POKE32(I2C_BYTE_COUNT, count);
+
+        /* Start the I2C */
+        POKE32(I2C_CTRL, FIELD_SET(PEEK32(I2C_CTRL), I2C_CTRL, CTRL, START));
+
+        /* Wait until transaction done. */
+        if (hwI2CWaitTXDone() != 0)
+            break;
+
+        /* Save the data to the given buffer */
+        for (i = 0; i <= count; i++)
+		    *pBuffer++ = PEEK32(I2C_DATA0 + i);
+
+        /* Substract length by 16 */
+        length -= (count + 1);
+
+        /* Number of bytes read. */
+        totalBytes += (count + 1);
+
+    } while (length > 0);
+
+    return totalBytes;
+}
+
+
+
+
+/*
+ *  This function reads the slave device's register
+ *
+ *  Parameters:
+ *      deviceAddress   - i2c Slave device address which register
+ *                        to be read from
+ *      registerIndex   - Slave device's register to be read
+ *
+ *  Return Value:
+ *      Register value
+ */
+unsigned char hwI2CReadReg(
+    unsigned char deviceAddress,
+    unsigned char registerIndex
+)
+{
+    unsigned char value = (0xFF);
+
+    if (hwI2CWriteData(deviceAddress, 1, &registerIndex) == 1)
+        hwI2CReadData(deviceAddress, 1, &value);
+
+    return value;
+}
+
+
+
+
+
+/*
+ *  This function writes a value to the slave device's register
+ *
+ *  Parameters:
+ *      deviceAddress   - i2c Slave device address which register
+ *                        to be written
+ *      registerIndex   - Slave device's register to be written
+ *      data            - Data to be written to the register
+ *
+ *  Result:
+ *          0   - Success
+ *         -1   - Fail
+ */
+int hwI2CWriteReg(
+    unsigned char deviceAddress,
+    unsigned char registerIndex,
+    unsigned char data
+)
+{
+    unsigned char value[2];
+
+    value[0] = registerIndex;
+    value[1] = data;
+    if (hwI2CWriteData(deviceAddress, 2, value) == 2)
+        return 0;
+
+    return (-1);
+}
+
+
+#endif
diff --git a/drivers/staging/sm750fb/ddk750_hwi2c.h b/drivers/staging/sm750fb/ddk750_hwi2c.h
new file mode 100644
index 000000000000..ad311493c9fc
--- /dev/null
+++ b/drivers/staging/sm750fb/ddk750_hwi2c.h
@@ -0,0 +1,10 @@
+#ifndef DDK750_HWI2C_H__
+#define DDK750_HWI2C_H__
+
+/* hwi2c functions */
+int hwI2CInit(unsigned char busSpeedMode);
+void hwI2CClose(void);
+
+unsigned char hwI2CReadReg(unsigned char deviceAddress,unsigned char registerIndex);
+int hwI2CWriteReg(unsigned char deviceAddress,unsigned char registerIndex,unsigned char data);
+#endif
diff --git a/drivers/staging/sm750fb/ddk750_mode.c b/drivers/staging/sm750fb/ddk750_mode.c
new file mode 100644
index 000000000000..2e418fb6ffde
--- /dev/null
+++ b/drivers/staging/sm750fb/ddk750_mode.c
@@ -0,0 +1,205 @@
+
+#include "ddk750_help.h"
+#include "ddk750_reg.h"
+#include "ddk750_mode.h"
+#include "ddk750_chip.h"
+
+/*
+	SM750LE only:
+    This function takes care extra registers and bit fields required to set
+    up a mode in SM750LE
+
+	Explanation about Display Control register:
+    HW only supports 7 predefined pixel clocks, and clock select is
+    in bit 29:27 of	Display Control register.
+*/
+static unsigned long displayControlAdjust_SM750LE(mode_parameter_t *pModeParam, unsigned long dispControl)
+{
+	unsigned long x, y;
+
+	x = pModeParam->horizontal_display_end;
+	y = pModeParam->vertical_display_end;
+
+    /* SM750LE has to set up the top-left and bottom-right
+       registers as well.
+       Note that normal SM750/SM718 only use those two register for
+       auto-centering mode.
+    */
+    POKE32(CRT_AUTO_CENTERING_TL,
+      FIELD_VALUE(0, CRT_AUTO_CENTERING_TL, TOP, 0)
+    | FIELD_VALUE(0, CRT_AUTO_CENTERING_TL, LEFT, 0));
+
+    POKE32(CRT_AUTO_CENTERING_BR,
+      FIELD_VALUE(0, CRT_AUTO_CENTERING_BR, BOTTOM, y-1)
+    | FIELD_VALUE(0, CRT_AUTO_CENTERING_BR, RIGHT, x-1));
+
+    /* Assume common fields in dispControl have been properly set before
+       calling this function.
+       This function only sets the extra fields in dispControl.
+    */
+
+	/* Clear bit 29:27 of display control register */
+    dispControl &= FIELD_CLEAR(CRT_DISPLAY_CTRL, CLK);
+
+	/* Set bit 29:27 of display control register for the right clock */
+	/* Note that SM750LE only need to supported 7 resoluitons. */
+	if ( x == 800 && y == 600 )
+    	dispControl = FIELD_SET(dispControl, CRT_DISPLAY_CTRL, CLK, PLL41);
+	else if (x == 1024 && y == 768)
+    	dispControl = FIELD_SET(dispControl, CRT_DISPLAY_CTRL, CLK, PLL65);
+	else if (x == 1152 && y == 864)
+    	dispControl = FIELD_SET(dispControl, CRT_DISPLAY_CTRL, CLK, PLL80);
+	else if (x == 1280 && y == 768)
+    	dispControl = FIELD_SET(dispControl, CRT_DISPLAY_CTRL, CLK, PLL80);
+	else if (x == 1280 && y == 720)
+    	dispControl = FIELD_SET(dispControl, CRT_DISPLAY_CTRL, CLK, PLL74);
+	else if (x == 1280 && y == 960)
+    	dispControl = FIELD_SET(dispControl, CRT_DISPLAY_CTRL, CLK, PLL108);
+	else if (x == 1280 && y == 1024)
+    	dispControl = FIELD_SET(dispControl, CRT_DISPLAY_CTRL, CLK, PLL108);
+	else /* default to VGA clock */
+    	dispControl = FIELD_SET(dispControl, CRT_DISPLAY_CTRL, CLK, PLL25);
+
+	/* Set bit 25:24 of display controller */
+    dispControl = FIELD_SET(dispControl, CRT_DISPLAY_CTRL, CRTSELECT, CRT);
+    dispControl = FIELD_SET(dispControl, CRT_DISPLAY_CTRL, RGBBIT, 24BIT);
+
+    /* Set bit 14 of display controller */
+    dispControl = FIELD_SET(dispControl, CRT_DISPLAY_CTRL, CLOCK_PHASE, ACTIVE_LOW);
+
+    POKE32(CRT_DISPLAY_CTRL, dispControl);
+
+	return dispControl;
+}
+
+
+
+/* only timing related registers will be  programed */
+static int programModeRegisters(mode_parameter_t * pModeParam,pll_value_t * pll)
+{
+	int ret = 0;
+	int cnt = 0;
+	unsigned int ulTmpValue,ulReg;
+	if(pll->clockType == SECONDARY_PLL)
+	{
+		/* programe secondary pixel clock */
+		POKE32(CRT_PLL_CTRL,formatPllReg(pll));
+        POKE32(CRT_HORIZONTAL_TOTAL,
+              FIELD_VALUE(0, CRT_HORIZONTAL_TOTAL, TOTAL, pModeParam->horizontal_total - 1)
+            | FIELD_VALUE(0, CRT_HORIZONTAL_TOTAL, DISPLAY_END, pModeParam->horizontal_display_end - 1));
+
+        POKE32(CRT_HORIZONTAL_SYNC,
+              FIELD_VALUE(0, CRT_HORIZONTAL_SYNC, WIDTH, pModeParam->horizontal_sync_width)
+            | FIELD_VALUE(0, CRT_HORIZONTAL_SYNC, START, pModeParam->horizontal_sync_start - 1));
+
+        POKE32(CRT_VERTICAL_TOTAL,
+              FIELD_VALUE(0, CRT_VERTICAL_TOTAL, TOTAL, pModeParam->vertical_total - 1)
+            | FIELD_VALUE(0, CRT_VERTICAL_TOTAL, DISPLAY_END, pModeParam->vertical_display_end - 1));
+
+        POKE32(CRT_VERTICAL_SYNC,
+              FIELD_VALUE(0, CRT_VERTICAL_SYNC, HEIGHT, pModeParam->vertical_sync_height)
+            | FIELD_VALUE(0, CRT_VERTICAL_SYNC, START, pModeParam->vertical_sync_start - 1));
+
+
+		ulTmpValue = FIELD_VALUE(0,CRT_DISPLAY_CTRL,VSYNC_PHASE,pModeParam->vertical_sync_polarity)|
+					  FIELD_VALUE(0,CRT_DISPLAY_CTRL,HSYNC_PHASE,pModeParam->horizontal_sync_polarity)|
+					  FIELD_SET(0,CRT_DISPLAY_CTRL,TIMING,ENABLE)|
+					  FIELD_SET(0,CRT_DISPLAY_CTRL,PLANE,ENABLE);
+
+
+		if(getChipType() == SM750LE){
+			displayControlAdjust_SM750LE(pModeParam,ulTmpValue);
+		}else{
+			ulReg = PEEK32(CRT_DISPLAY_CTRL)
+					& FIELD_CLEAR(CRT_DISPLAY_CTRL,VSYNC_PHASE)
+					& FIELD_CLEAR(CRT_DISPLAY_CTRL,HSYNC_PHASE)
+					& FIELD_CLEAR(CRT_DISPLAY_CTRL,TIMING)
+					& FIELD_CLEAR(CRT_DISPLAY_CTRL,PLANE);
+
+			 POKE32(CRT_DISPLAY_CTRL,ulTmpValue|ulReg);
+		}
+
+	}
+	else if(pll->clockType == PRIMARY_PLL)
+	{
+		unsigned int ulReservedBits;
+		POKE32(PANEL_PLL_CTRL,formatPllReg(pll));
+
+        POKE32(PANEL_HORIZONTAL_TOTAL,
+              FIELD_VALUE(0, PANEL_HORIZONTAL_TOTAL, TOTAL, pModeParam->horizontal_total - 1)
+            | FIELD_VALUE(0, PANEL_HORIZONTAL_TOTAL, DISPLAY_END, pModeParam->horizontal_display_end - 1));
+
+        POKE32(PANEL_HORIZONTAL_SYNC,
+              FIELD_VALUE(0, PANEL_HORIZONTAL_SYNC, WIDTH, pModeParam->horizontal_sync_width)
+            | FIELD_VALUE(0, PANEL_HORIZONTAL_SYNC, START, pModeParam->horizontal_sync_start - 1));
+
+        POKE32(PANEL_VERTICAL_TOTAL,
+              FIELD_VALUE(0, PANEL_VERTICAL_TOTAL, TOTAL, pModeParam->vertical_total - 1)
+            | FIELD_VALUE(0, PANEL_VERTICAL_TOTAL, DISPLAY_END, pModeParam->vertical_display_end - 1));
+
+        POKE32(PANEL_VERTICAL_SYNC,
+              FIELD_VALUE(0, PANEL_VERTICAL_SYNC, HEIGHT, pModeParam->vertical_sync_height)
+            | FIELD_VALUE(0, PANEL_VERTICAL_SYNC, START, pModeParam->vertical_sync_start - 1));
+
+		ulTmpValue = FIELD_VALUE(0,PANEL_DISPLAY_CTRL,VSYNC_PHASE,pModeParam->vertical_sync_polarity)|
+					FIELD_VALUE(0,PANEL_DISPLAY_CTRL,HSYNC_PHASE,pModeParam->horizontal_sync_polarity)|
+					FIELD_VALUE(0,PANEL_DISPLAY_CTRL,CLOCK_PHASE,pModeParam->clock_phase_polarity)|
+					FIELD_SET(0,PANEL_DISPLAY_CTRL,TIMING,ENABLE)|
+					FIELD_SET(0,PANEL_DISPLAY_CTRL,PLANE,ENABLE);
+
+        ulReservedBits = FIELD_SET(0, PANEL_DISPLAY_CTRL, RESERVED_1_MASK, ENABLE) |
+                         FIELD_SET(0, PANEL_DISPLAY_CTRL, RESERVED_2_MASK, ENABLE) |
+                         FIELD_SET(0, PANEL_DISPLAY_CTRL, RESERVED_3_MASK, ENABLE)|
+                         FIELD_SET(0,PANEL_DISPLAY_CTRL,VSYNC,ACTIVE_LOW);
+
+        ulReg = (PEEK32(PANEL_DISPLAY_CTRL) & ~ulReservedBits)
+              & FIELD_CLEAR(PANEL_DISPLAY_CTRL, CLOCK_PHASE)
+              & FIELD_CLEAR(PANEL_DISPLAY_CTRL, VSYNC_PHASE)
+              & FIELD_CLEAR(PANEL_DISPLAY_CTRL, HSYNC_PHASE)
+              & FIELD_CLEAR(PANEL_DISPLAY_CTRL, TIMING)
+              & FIELD_CLEAR(PANEL_DISPLAY_CTRL, PLANE);
+
+
+		/* May a hardware bug or just my test chip (not confirmed).
+		* PANEL_DISPLAY_CTRL register seems requiring few writes
+		* before a value can be succesfully written in.
+		* Added some masks to mask out the reserved bits.
+		* Note: This problem happens by design. The hardware will wait for the
+		*       next vertical sync to turn on/off the plane.
+		*/
+
+		POKE32(PANEL_DISPLAY_CTRL,ulTmpValue|ulReg);
+#if 1
+		while((PEEK32(PANEL_DISPLAY_CTRL) & ~ulReservedBits) != (ulTmpValue|ulReg))
+		{
+			cnt++;
+			if(cnt > 1000)
+				break;
+			POKE32(PANEL_DISPLAY_CTRL,ulTmpValue|ulReg);
+		}
+#endif
+	}
+	else{
+		ret = -1;
+	}
+	return ret;
+}
+
+int ddk750_setModeTiming(mode_parameter_t * parm,clock_type_t clock)
+{
+	pll_value_t pll;
+	unsigned int uiActualPixelClk;
+	pll.inputFreq = DEFAULT_INPUT_CLOCK;
+	pll.clockType = clock;
+
+	uiActualPixelClk = calcPllValue(parm->pixel_clock,&pll);
+	if(getChipType() == SM750LE){
+		/* set graphic mode via IO method */
+		outb_p(0x88,0x3d4);
+		outb_p(0x06,0x3d5);
+	}
+	programModeRegisters(parm,&pll);
+	return 0;
+}
+
+
diff --git a/drivers/staging/sm750fb/ddk750_mode.h b/drivers/staging/sm750fb/ddk750_mode.h
new file mode 100644
index 000000000000..6f8df96a8b02
--- /dev/null
+++ b/drivers/staging/sm750fb/ddk750_mode.h
@@ -0,0 +1,43 @@
+#ifndef DDK750_MODE_H__
+#define DDK750_MODE_H__
+
+#include "ddk750_chip.h"
+
+typedef enum _spolarity_t
+{
+    POS = 0, /* positive */
+    NEG, /* negative */
+}
+spolarity_t;
+
+
+typedef struct _mode_parameter_t
+{
+    /* Horizontal timing. */
+    unsigned long horizontal_total;
+    unsigned long horizontal_display_end;
+    unsigned long horizontal_sync_start;
+    unsigned long horizontal_sync_width;
+    spolarity_t horizontal_sync_polarity;
+
+    /* Vertical timing. */
+    unsigned long vertical_total;
+    unsigned long vertical_display_end;
+    unsigned long vertical_sync_start;
+    unsigned long vertical_sync_height;
+    spolarity_t vertical_sync_polarity;
+
+    /* Refresh timing. */
+    unsigned long pixel_clock;
+    unsigned long horizontal_frequency;
+    unsigned long vertical_frequency;
+
+    /* Clock Phase. This clock phase only applies to Panel. */
+    spolarity_t clock_phase_polarity;
+}
+mode_parameter_t;
+
+int ddk750_setModeTiming(mode_parameter_t *,clock_type_t);
+
+
+#endif
diff --git a/drivers/staging/sm750fb/ddk750_power.c b/drivers/staging/sm750fb/ddk750_power.c
new file mode 100644
index 000000000000..cbb97676b33c
--- /dev/null
+++ b/drivers/staging/sm750fb/ddk750_power.c
@@ -0,0 +1,239 @@
+#include "ddk750_help.h"
+#include "ddk750_reg.h"
+#include "ddk750_power.h"
+
+void ddk750_setDPMS(DPMS_t state)
+{
+	unsigned int value;
+	if(getChipType() == SM750LE){
+		value = PEEK32(CRT_DISPLAY_CTRL);
+		POKE32(CRT_DISPLAY_CTRL,FIELD_VALUE(value,CRT_DISPLAY_CTRL,DPMS,state));
+	}else{
+		value = PEEK32(SYSTEM_CTRL);
+		value= FIELD_VALUE(value,SYSTEM_CTRL,DPMS,state);
+		POKE32(SYSTEM_CTRL, value);
+	}
+}
+
+unsigned int getPowerMode(void)
+{
+	if(getChipType() == SM750LE)
+		return 0;
+    return (FIELD_GET(PEEK32(POWER_MODE_CTRL), POWER_MODE_CTRL, MODE));
+}
+
+
+/*
+ * SM50x can operate in one of three modes: 0, 1 or Sleep.
+ * On hardware reset, power mode 0 is default.
+ */
+void setPowerMode(unsigned int powerMode)
+{
+    unsigned int control_value = 0;
+
+    control_value = PEEK32(POWER_MODE_CTRL);
+
+	if(getChipType() == SM750LE)
+		return;
+
+    switch (powerMode)
+    {
+        case POWER_MODE_CTRL_MODE_MODE0:
+            control_value = FIELD_SET(control_value, POWER_MODE_CTRL, MODE, MODE0);
+            break;
+
+        case POWER_MODE_CTRL_MODE_MODE1:
+            control_value = FIELD_SET(control_value, POWER_MODE_CTRL, MODE, MODE1);
+            break;
+
+        case POWER_MODE_CTRL_MODE_SLEEP:
+            control_value = FIELD_SET(control_value, POWER_MODE_CTRL, MODE, SLEEP);
+            break;
+
+        default:
+            break;
+    }
+
+    /* Set up other fields in Power Control Register */
+    if (powerMode == POWER_MODE_CTRL_MODE_SLEEP)
+    {
+        control_value =
+#ifdef VALIDATION_CHIP
+            FIELD_SET(  control_value, POWER_MODE_CTRL, 336CLK, OFF) |
+#endif
+            FIELD_SET(  control_value, POWER_MODE_CTRL, OSC_INPUT,  OFF);
+    }
+    else
+    {
+        control_value =
+#ifdef VALIDATION_CHIP
+            FIELD_SET(  control_value, POWER_MODE_CTRL, 336CLK, ON) |
+#endif
+            FIELD_SET(  control_value, POWER_MODE_CTRL, OSC_INPUT,  ON);
+    }
+
+    /* Program new power mode. */
+    POKE32(POWER_MODE_CTRL, control_value);
+}
+
+void setCurrentGate(unsigned int gate)
+{
+    unsigned int gate_reg;
+    unsigned int mode;
+
+    /* Get current power mode. */
+    mode = getPowerMode();
+
+    switch (mode)
+    {
+        case POWER_MODE_CTRL_MODE_MODE0:
+            gate_reg = MODE0_GATE;
+            break;
+
+        case POWER_MODE_CTRL_MODE_MODE1:
+            gate_reg = MODE1_GATE;
+            break;
+
+        default:
+            gate_reg = MODE0_GATE;
+            break;
+    }
+    POKE32(gate_reg, gate);
+}
+
+
+
+/*
+ * This function enable/disable the 2D engine.
+ */
+void enable2DEngine(unsigned int enable)
+{
+    uint32_t gate;
+
+    gate = PEEK32(CURRENT_GATE);
+    if (enable)
+    {
+        gate = FIELD_SET(gate, CURRENT_GATE, DE,  ON);
+        gate = FIELD_SET(gate, CURRENT_GATE, CSC, ON);
+    }
+    else
+    {
+        gate = FIELD_SET(gate, CURRENT_GATE, DE,  OFF);
+        gate = FIELD_SET(gate, CURRENT_GATE, CSC, OFF);
+    }
+
+    setCurrentGate(gate);
+}
+
+
+/*
+ * This function enable/disable the ZV Port.
+ */
+void enableZVPort(unsigned int enable)
+{
+    uint32_t gate;
+
+    /* Enable ZV Port Gate */
+    gate = PEEK32(CURRENT_GATE);
+    if (enable)
+    {
+        gate = FIELD_SET(gate, CURRENT_GATE, ZVPORT, ON);
+#if 1
+        /* Using Software I2C */
+        gate = FIELD_SET(gate, CURRENT_GATE, GPIO, ON);
+#else
+        /* Using Hardware I2C */
+        gate = FIELD_SET(gate, CURRENT_GATE, I2C,    ON);
+#endif
+    }
+    else
+    {
+        /* Disable ZV Port Gate. There is no way to know whether the GPIO pins are being used
+           or not. Therefore, do not disable the GPIO gate. */
+        gate = FIELD_SET(gate, CURRENT_GATE, ZVPORT, OFF);
+    }
+
+    setCurrentGate(gate);
+}
+
+
+void enableSSP(unsigned int enable)
+{
+    uint32_t gate;
+
+    /* Enable SSP Gate */
+    gate = PEEK32(CURRENT_GATE);
+    if (enable)
+        gate = FIELD_SET(gate, CURRENT_GATE, SSP, ON);
+    else
+        gate = FIELD_SET(gate, CURRENT_GATE, SSP, OFF);
+
+    setCurrentGate(gate);
+}
+
+void enableDMA(unsigned int enable)
+{
+    uint32_t gate;
+
+    /* Enable DMA Gate */
+    gate = PEEK32(CURRENT_GATE);
+    if (enable)
+        gate = FIELD_SET(gate, CURRENT_GATE, DMA, ON);
+    else
+        gate = FIELD_SET(gate, CURRENT_GATE, DMA, OFF);
+
+    setCurrentGate(gate);
+}
+
+/*
+ * This function enable/disable the GPIO Engine
+ */
+void enableGPIO(unsigned int enable)
+{
+    uint32_t gate;
+
+    /* Enable GPIO Gate */
+    gate = PEEK32(CURRENT_GATE);
+    if (enable)
+        gate = FIELD_SET(gate, CURRENT_GATE, GPIO, ON);
+    else
+        gate = FIELD_SET(gate, CURRENT_GATE, GPIO, OFF);
+
+    setCurrentGate(gate);
+}
+
+/*
+ * This function enable/disable the PWM Engine
+ */
+void enablePWM(unsigned int enable)
+{
+    uint32_t gate;
+
+    /* Enable PWM Gate */
+    gate = PEEK32(CURRENT_GATE);
+    if (enable)
+        gate = FIELD_SET(gate, CURRENT_GATE, PWM, ON);
+    else
+        gate = FIELD_SET(gate, CURRENT_GATE, PWM, OFF);
+
+    setCurrentGate(gate);
+}
+
+/*
+ * This function enable/disable the I2C Engine
+ */
+void enableI2C(unsigned int enable)
+{
+    uint32_t gate;
+
+    /* Enable I2C Gate */
+    gate = PEEK32(CURRENT_GATE);
+    if (enable)
+        gate = FIELD_SET(gate, CURRENT_GATE, I2C, ON);
+    else
+        gate = FIELD_SET(gate, CURRENT_GATE, I2C, OFF);
+
+    setCurrentGate(gate);
+}
+
+
diff --git a/drivers/staging/sm750fb/ddk750_power.h b/drivers/staging/sm750fb/ddk750_power.h
new file mode 100644
index 000000000000..71dc7f980069
--- /dev/null
+++ b/drivers/staging/sm750fb/ddk750_power.h
@@ -0,0 +1,71 @@
+#ifndef DDK750_POWER_H__
+#define DDK750_POWER_H__
+
+typedef enum _DPMS_t
+{
+    crtDPMS_ON = 0x0,
+    crtDPMS_STANDBY = 0x1,
+    crtDPMS_SUSPEND = 0x2,
+    crtDPMS_OFF = 0x3,
+}
+DPMS_t;
+
+#define setDAC(off) \
+		{	\
+		POKE32(MISC_CTRL,FIELD_VALUE(PEEK32(MISC_CTRL),	\
+									MISC_CTRL,	\
+									DAC_POWER,	\
+									off));	\
+		}
+
+void ddk750_setDPMS(DPMS_t);
+
+unsigned int getPowerMode(void);
+
+/*
+ * This function sets the current power mode
+ */
+void setPowerMode(unsigned int powerMode);
+
+/*
+ * This function sets current gate
+ */
+void setCurrentGate(unsigned int gate);
+
+/*
+ * This function enable/disable the 2D engine.
+ */
+void enable2DEngine(unsigned int enable);
+
+/*
+ * This function enable/disable the ZV Port
+ */
+void enableZVPort(unsigned int enable);
+
+/*
+ * This function enable/disable the DMA Engine
+ */
+void enableDMA(unsigned int enable);
+
+/*
+ * This function enable/disable the GPIO Engine
+ */
+void enableGPIO(unsigned int enable);
+
+/*
+ * This function enable/disable the PWM Engine
+ */
+void enablePWM(unsigned int enable);
+
+/*
+ * This function enable/disable the I2C Engine
+ */
+void enableI2C(unsigned int enable);
+
+/*
+ * This function enable/disable the SSP.
+ */
+void enableSSP(unsigned int enable);
+
+
+#endif
diff --git a/drivers/staging/sm750fb/ddk750_reg.h b/drivers/staging/sm750fb/ddk750_reg.h
new file mode 100644
index 000000000000..2016f97d2a3d
--- /dev/null
+++ b/drivers/staging/sm750fb/ddk750_reg.h
@@ -0,0 +1,2616 @@
+#ifndef DDK750_REG_H__
+#define DDK750_REG_H__
+
+/* New register for SM750LE */
+#define DE_STATE1                                        0x100054
+#define DE_STATE1_DE_ABORT                               0:0
+#define DE_STATE1_DE_ABORT_OFF                           0
+#define DE_STATE1_DE_ABORT_ON                            1
+
+#define DE_STATE2                                        0x100058
+#define DE_STATE2_DE_FIFO                                3:3
+#define DE_STATE2_DE_FIFO_NOTEMPTY                       0
+#define DE_STATE2_DE_FIFO_EMPTY                          1
+#define DE_STATE2_DE_STATUS                              2:2
+#define DE_STATE2_DE_STATUS_IDLE                         0
+#define DE_STATE2_DE_STATUS_BUSY                         1
+#define DE_STATE2_DE_MEM_FIFO                            1:1
+#define DE_STATE2_DE_MEM_FIFO_NOTEMPTY                   0
+#define DE_STATE2_DE_MEM_FIFO_EMPTY                      1
+#define DE_STATE2_DE_RESERVED                            0:0
+
+
+
+#define SYSTEM_CTRL                                   0x000000
+#define SYSTEM_CTRL_DPMS                              31:30
+#define SYSTEM_CTRL_DPMS_VPHP                         0
+#define SYSTEM_CTRL_DPMS_VPHN                         1
+#define SYSTEM_CTRL_DPMS_VNHP                         2
+#define SYSTEM_CTRL_DPMS_VNHN                         3
+#define SYSTEM_CTRL_PCI_BURST                         29:29
+#define SYSTEM_CTRL_PCI_BURST_OFF                     0
+#define SYSTEM_CTRL_PCI_BURST_ON                      1
+#define SYSTEM_CTRL_PCI_MASTER                        25:25
+#define SYSTEM_CTRL_PCI_MASTER_OFF                    0
+#define SYSTEM_CTRL_PCI_MASTER_ON                     1
+#define SYSTEM_CTRL_LATENCY_TIMER                     24:24
+#define SYSTEM_CTRL_LATENCY_TIMER_ON                  0
+#define SYSTEM_CTRL_LATENCY_TIMER_OFF                 1
+#define SYSTEM_CTRL_DE_FIFO                           23:23
+#define SYSTEM_CTRL_DE_FIFO_NOTEMPTY                  0
+#define SYSTEM_CTRL_DE_FIFO_EMPTY                     1
+#define SYSTEM_CTRL_DE_STATUS                         22:22
+#define SYSTEM_CTRL_DE_STATUS_IDLE                    0
+#define SYSTEM_CTRL_DE_STATUS_BUSY                    1
+#define SYSTEM_CTRL_DE_MEM_FIFO                       21:21
+#define SYSTEM_CTRL_DE_MEM_FIFO_NOTEMPTY              0
+#define SYSTEM_CTRL_DE_MEM_FIFO_EMPTY                 1
+#define SYSTEM_CTRL_CSC_STATUS                        20:20
+#define SYSTEM_CTRL_CSC_STATUS_IDLE                   0
+#define SYSTEM_CTRL_CSC_STATUS_BUSY                   1
+#define SYSTEM_CTRL_CRT_VSYNC                         19:19
+#define SYSTEM_CTRL_CRT_VSYNC_INACTIVE                0
+#define SYSTEM_CTRL_CRT_VSYNC_ACTIVE                  1
+#define SYSTEM_CTRL_PANEL_VSYNC                       18:18
+#define SYSTEM_CTRL_PANEL_VSYNC_INACTIVE              0
+#define SYSTEM_CTRL_PANEL_VSYNC_ACTIVE                1
+#define SYSTEM_CTRL_CURRENT_BUFFER                    17:17
+#define SYSTEM_CTRL_CURRENT_BUFFER_NORMAL             0
+#define SYSTEM_CTRL_CURRENT_BUFFER_FLIP_PENDING       1
+#define SYSTEM_CTRL_DMA_STATUS                        16:16
+#define SYSTEM_CTRL_DMA_STATUS_IDLE                   0
+#define SYSTEM_CTRL_DMA_STATUS_BUSY                   1
+#define SYSTEM_CTRL_PCI_BURST_READ                    15:15
+#define SYSTEM_CTRL_PCI_BURST_READ_OFF                0
+#define SYSTEM_CTRL_PCI_BURST_READ_ON                 1
+#define SYSTEM_CTRL_DE_ABORT                          13:13
+#define SYSTEM_CTRL_DE_ABORT_OFF                      0
+#define SYSTEM_CTRL_DE_ABORT_ON                       1
+#define SYSTEM_CTRL_PCI_SUBSYS_ID_LOCK                11:11
+#define SYSTEM_CTRL_PCI_SUBSYS_ID_LOCK_OFF            0
+#define SYSTEM_CTRL_PCI_SUBSYS_ID_LOCK_ON             1
+#define SYSTEM_CTRL_PCI_RETRY                         7:7
+#define SYSTEM_CTRL_PCI_RETRY_ON                      0
+#define SYSTEM_CTRL_PCI_RETRY_OFF                     1
+#define SYSTEM_CTRL_PCI_SLAVE_BURST_READ_SIZE         5:4
+#define SYSTEM_CTRL_PCI_SLAVE_BURST_READ_SIZE_1       0
+#define SYSTEM_CTRL_PCI_SLAVE_BURST_READ_SIZE_2       1
+#define SYSTEM_CTRL_PCI_SLAVE_BURST_READ_SIZE_4       2
+#define SYSTEM_CTRL_PCI_SLAVE_BURST_READ_SIZE_8       3
+#define SYSTEM_CTRL_CRT_TRISTATE                      3:3
+#define SYSTEM_CTRL_CRT_TRISTATE_OFF                  0
+#define SYSTEM_CTRL_CRT_TRISTATE_ON                   1
+#define SYSTEM_CTRL_PCIMEM_TRISTATE                   2:2
+#define SYSTEM_CTRL_PCIMEM_TRISTATE_OFF               0
+#define SYSTEM_CTRL_PCIMEM_TRISTATE_ON                1
+#define SYSTEM_CTRL_LOCALMEM_TRISTATE                 1:1
+#define SYSTEM_CTRL_LOCALMEM_TRISTATE_OFF             0
+#define SYSTEM_CTRL_LOCALMEM_TRISTATE_ON              1
+#define SYSTEM_CTRL_PANEL_TRISTATE                    0:0
+#define SYSTEM_CTRL_PANEL_TRISTATE_OFF                0
+#define SYSTEM_CTRL_PANEL_TRISTATE_ON                 1
+
+#define MISC_CTRL                                     0x000004
+#define MISC_CTRL_DRAM_RERESH_COUNT                   27:27
+#define MISC_CTRL_DRAM_RERESH_COUNT_1ROW              0
+#define MISC_CTRL_DRAM_RERESH_COUNT_3ROW              1
+#define MISC_CTRL_DRAM_REFRESH_TIME                   26:25
+#define MISC_CTRL_DRAM_REFRESH_TIME_8                 0
+#define MISC_CTRL_DRAM_REFRESH_TIME_16                1
+#define MISC_CTRL_DRAM_REFRESH_TIME_32                2
+#define MISC_CTRL_DRAM_REFRESH_TIME_64                3
+#define MISC_CTRL_INT_OUTPUT                          24:24
+#define MISC_CTRL_INT_OUTPUT_NORMAL                   0
+#define MISC_CTRL_INT_OUTPUT_INVERT                   1
+#define MISC_CTRL_PLL_CLK_COUNT                       23:23
+#define MISC_CTRL_PLL_CLK_COUNT_OFF                   0
+#define MISC_CTRL_PLL_CLK_COUNT_ON                    1
+#define MISC_CTRL_DAC_POWER                           20:20
+#define MISC_CTRL_DAC_POWER_ON                        0
+#define MISC_CTRL_DAC_POWER_OFF                       1
+#define MISC_CTRL_CLK_SELECT                          16:16
+#define MISC_CTRL_CLK_SELECT_OSC                      0
+#define MISC_CTRL_CLK_SELECT_TESTCLK                  1
+#define MISC_CTRL_DRAM_COLUMN_SIZE                    15:14
+#define MISC_CTRL_DRAM_COLUMN_SIZE_256                0
+#define MISC_CTRL_DRAM_COLUMN_SIZE_512                1
+#define MISC_CTRL_DRAM_COLUMN_SIZE_1024               2
+#define MISC_CTRL_LOCALMEM_SIZE                       13:12
+#define MISC_CTRL_LOCALMEM_SIZE_8M                    3
+#define MISC_CTRL_LOCALMEM_SIZE_16M                   0
+#define MISC_CTRL_LOCALMEM_SIZE_32M                   1
+#define MISC_CTRL_LOCALMEM_SIZE_64M                   2
+#define MISC_CTRL_DRAM_TWTR                           11:11
+#define MISC_CTRL_DRAM_TWTR_2CLK                      0
+#define MISC_CTRL_DRAM_TWTR_1CLK                      1
+#define MISC_CTRL_DRAM_TWR                            10:10
+#define MISC_CTRL_DRAM_TWR_3CLK                       0
+#define MISC_CTRL_DRAM_TWR_2CLK                       1
+#define MISC_CTRL_DRAM_TRP                            9:9
+#define MISC_CTRL_DRAM_TRP_3CLK                       0
+#define MISC_CTRL_DRAM_TRP_4CLK                       1
+#define MISC_CTRL_DRAM_TRFC                           8:8
+#define MISC_CTRL_DRAM_TRFC_12CLK                     0
+#define MISC_CTRL_DRAM_TRFC_14CLK                     1
+#define MISC_CTRL_DRAM_TRAS                           7:7
+#define MISC_CTRL_DRAM_TRAS_7CLK                      0
+#define MISC_CTRL_DRAM_TRAS_8CLK                      1
+#define MISC_CTRL_LOCALMEM_RESET                      6:6
+#define MISC_CTRL_LOCALMEM_RESET_RESET                0
+#define MISC_CTRL_LOCALMEM_RESET_NORMAL               1
+#define MISC_CTRL_LOCALMEM_STATE                      5:5
+#define MISC_CTRL_LOCALMEM_STATE_ACTIVE               0
+#define MISC_CTRL_LOCALMEM_STATE_INACTIVE             1
+#define MISC_CTRL_CPU_CAS_LATENCY                     4:4
+#define MISC_CTRL_CPU_CAS_LATENCY_2CLK                0
+#define MISC_CTRL_CPU_CAS_LATENCY_3CLK                1
+#define MISC_CTRL_DLL                                 3:3
+#define MISC_CTRL_DLL_ON                              0
+#define MISC_CTRL_DLL_OFF                             1
+#define MISC_CTRL_DRAM_OUTPUT                         2:2
+#define MISC_CTRL_DRAM_OUTPUT_LOW                     0
+#define MISC_CTRL_DRAM_OUTPUT_HIGH                    1
+#define MISC_CTRL_LOCALMEM_BUS_SIZE                   1:1
+#define MISC_CTRL_LOCALMEM_BUS_SIZE_32                0
+#define MISC_CTRL_LOCALMEM_BUS_SIZE_64                1
+#define MISC_CTRL_EMBEDDED_LOCALMEM                   0:0
+#define MISC_CTRL_EMBEDDED_LOCALMEM_ON                0
+#define MISC_CTRL_EMBEDDED_LOCALMEM_OFF               1
+
+#define GPIO_MUX                                      0x000008
+#define GPIO_MUX_31                                   31:31
+#define GPIO_MUX_31_GPIO                              0
+#define GPIO_MUX_31_I2C                               1
+#define GPIO_MUX_30                                   30:30
+#define GPIO_MUX_30_GPIO                              0
+#define GPIO_MUX_30_I2C                               1
+#define GPIO_MUX_29                                   29:29
+#define GPIO_MUX_29_GPIO                              0
+#define GPIO_MUX_29_SSP1                              1
+#define GPIO_MUX_28                                   28:28
+#define GPIO_MUX_28_GPIO                              0
+#define GPIO_MUX_28_SSP1                              1
+#define GPIO_MUX_27                                   27:27
+#define GPIO_MUX_27_GPIO                              0
+#define GPIO_MUX_27_SSP1                              1
+#define GPIO_MUX_26                                   26:26
+#define GPIO_MUX_26_GPIO                              0
+#define GPIO_MUX_26_SSP1                              1
+#define GPIO_MUX_25                                   25:25
+#define GPIO_MUX_25_GPIO                              0
+#define GPIO_MUX_25_SSP1                              1
+#define GPIO_MUX_24                                   24:24
+#define GPIO_MUX_24_GPIO                              0
+#define GPIO_MUX_24_SSP0                              1
+#define GPIO_MUX_23                                   23:23
+#define GPIO_MUX_23_GPIO                              0
+#define GPIO_MUX_23_SSP0                              1
+#define GPIO_MUX_22                                   22:22
+#define GPIO_MUX_22_GPIO                              0
+#define GPIO_MUX_22_SSP0                              1
+#define GPIO_MUX_21                                   21:21
+#define GPIO_MUX_21_GPIO                              0
+#define GPIO_MUX_21_SSP0                              1
+#define GPIO_MUX_20                                   20:20
+#define GPIO_MUX_20_GPIO                              0
+#define GPIO_MUX_20_SSP0                              1
+#define GPIO_MUX_19                                   19:19
+#define GPIO_MUX_19_GPIO                              0
+#define GPIO_MUX_19_PWM                               1
+#define GPIO_MUX_18                                   18:18
+#define GPIO_MUX_18_GPIO                              0
+#define GPIO_MUX_18_PWM                               1
+#define GPIO_MUX_17                                   17:17
+#define GPIO_MUX_17_GPIO                              0
+#define GPIO_MUX_17_PWM                               1
+#define GPIO_MUX_16                                   16:16
+#define GPIO_MUX_16_GPIO_ZVPORT                       0
+#define GPIO_MUX_16_TEST_DATA                         1
+#define GPIO_MUX_15                                   15:15
+#define GPIO_MUX_15_GPIO_ZVPORT                       0
+#define GPIO_MUX_15_TEST_DATA                         1
+#define GPIO_MUX_14                                   14:14
+#define GPIO_MUX_14_GPIO_ZVPORT                       0
+#define GPIO_MUX_14_TEST_DATA                         1
+#define GPIO_MUX_13                                   13:13
+#define GPIO_MUX_13_GPIO_ZVPORT                       0
+#define GPIO_MUX_13_TEST_DATA                         1
+#define GPIO_MUX_12                                   12:12
+#define GPIO_MUX_12_GPIO_ZVPORT                       0
+#define GPIO_MUX_12_TEST_DATA                         1
+#define GPIO_MUX_11                                   11:11
+#define GPIO_MUX_11_GPIO_ZVPORT                       0
+#define GPIO_MUX_11_TEST_DATA                         1
+#define GPIO_MUX_10                                   10:10
+#define GPIO_MUX_10_GPIO_ZVPORT                       0
+#define GPIO_MUX_10_TEST_DATA                         1
+#define GPIO_MUX_9                                    9:9
+#define GPIO_MUX_9_GPIO_ZVPORT                        0
+#define GPIO_MUX_9_TEST_DATA                          1
+#define GPIO_MUX_8                                    8:8
+#define GPIO_MUX_8_GPIO_ZVPORT                        0
+#define GPIO_MUX_8_TEST_DATA                          1
+#define GPIO_MUX_7                                    7:7
+#define GPIO_MUX_7_GPIO_ZVPORT                        0
+#define GPIO_MUX_7_TEST_DATA                          1
+#define GPIO_MUX_6                                    6:6
+#define GPIO_MUX_6_GPIO_ZVPORT                        0
+#define GPIO_MUX_6_TEST_DATA                          1
+#define GPIO_MUX_5                                    5:5
+#define GPIO_MUX_5_GPIO_ZVPORT                        0
+#define GPIO_MUX_5_TEST_DATA                          1
+#define GPIO_MUX_4                                    4:4
+#define GPIO_MUX_4_GPIO_ZVPORT                        0
+#define GPIO_MUX_4_TEST_DATA                          1
+#define GPIO_MUX_3                                    3:3
+#define GPIO_MUX_3_GPIO_ZVPORT                        0
+#define GPIO_MUX_3_TEST_DATA                          1
+#define GPIO_MUX_2                                    2:2
+#define GPIO_MUX_2_GPIO_ZVPORT                        0
+#define GPIO_MUX_2_TEST_DATA                          1
+#define GPIO_MUX_1                                    1:1
+#define GPIO_MUX_1_GPIO_ZVPORT                        0
+#define GPIO_MUX_1_TEST_DATA                          1
+#define GPIO_MUX_0                                    0:0
+#define GPIO_MUX_0_GPIO_ZVPORT                        0
+#define GPIO_MUX_0_TEST_DATA                          1
+
+#define LOCALMEM_ARBITRATION                          0x00000C
+#define LOCALMEM_ARBITRATION_ROTATE                   28:28
+#define LOCALMEM_ARBITRATION_ROTATE_OFF               0
+#define LOCALMEM_ARBITRATION_ROTATE_ON                1
+#define LOCALMEM_ARBITRATION_VGA                      26:24
+#define LOCALMEM_ARBITRATION_VGA_OFF                  0
+#define LOCALMEM_ARBITRATION_VGA_PRIORITY_1           1
+#define LOCALMEM_ARBITRATION_VGA_PRIORITY_2           2
+#define LOCALMEM_ARBITRATION_VGA_PRIORITY_3           3
+#define LOCALMEM_ARBITRATION_VGA_PRIORITY_4           4
+#define LOCALMEM_ARBITRATION_VGA_PRIORITY_5           5
+#define LOCALMEM_ARBITRATION_VGA_PRIORITY_6           6
+#define LOCALMEM_ARBITRATION_VGA_PRIORITY_7           7
+#define LOCALMEM_ARBITRATION_DMA                      22:20
+#define LOCALMEM_ARBITRATION_DMA_OFF                  0
+#define LOCALMEM_ARBITRATION_DMA_PRIORITY_1           1
+#define LOCALMEM_ARBITRATION_DMA_PRIORITY_2           2
+#define LOCALMEM_ARBITRATION_DMA_PRIORITY_3           3
+#define LOCALMEM_ARBITRATION_DMA_PRIORITY_4           4
+#define LOCALMEM_ARBITRATION_DMA_PRIORITY_5           5
+#define LOCALMEM_ARBITRATION_DMA_PRIORITY_6           6
+#define LOCALMEM_ARBITRATION_DMA_PRIORITY_7           7
+#define LOCALMEM_ARBITRATION_ZVPORT1                  18:16
+#define LOCALMEM_ARBITRATION_ZVPORT1_OFF              0
+#define LOCALMEM_ARBITRATION_ZVPORT1_PRIORITY_1       1
+#define LOCALMEM_ARBITRATION_ZVPORT1_PRIORITY_2       2
+#define LOCALMEM_ARBITRATION_ZVPORT1_PRIORITY_3       3
+#define LOCALMEM_ARBITRATION_ZVPORT1_PRIORITY_4       4
+#define LOCALMEM_ARBITRATION_ZVPORT1_PRIORITY_5       5
+#define LOCALMEM_ARBITRATION_ZVPORT1_PRIORITY_6       6
+#define LOCALMEM_ARBITRATION_ZVPORT1_PRIORITY_7       7
+#define LOCALMEM_ARBITRATION_ZVPORT0                  14:12
+#define LOCALMEM_ARBITRATION_ZVPORT0_OFF              0
+#define LOCALMEM_ARBITRATION_ZVPORT0_PRIORITY_1       1
+#define LOCALMEM_ARBITRATION_ZVPORT0_PRIORITY_2       2
+#define LOCALMEM_ARBITRATION_ZVPORT0_PRIORITY_3       3
+#define LOCALMEM_ARBITRATION_ZVPORT0_PRIORITY_4       4
+#define LOCALMEM_ARBITRATION_ZVPORT0_PRIORITY_5       5
+#define LOCALMEM_ARBITRATION_ZVPORT0_PRIORITY_6       6
+#define LOCALMEM_ARBITRATION_ZVPORT0_PRIORITY_7       7
+#define LOCALMEM_ARBITRATION_VIDEO                    10:8
+#define LOCALMEM_ARBITRATION_VIDEO_OFF                0
+#define LOCALMEM_ARBITRATION_VIDEO_PRIORITY_1         1
+#define LOCALMEM_ARBITRATION_VIDEO_PRIORITY_2         2
+#define LOCALMEM_ARBITRATION_VIDEO_PRIORITY_3         3
+#define LOCALMEM_ARBITRATION_VIDEO_PRIORITY_4         4
+#define LOCALMEM_ARBITRATION_VIDEO_PRIORITY_5         5
+#define LOCALMEM_ARBITRATION_VIDEO_PRIORITY_6         6
+#define LOCALMEM_ARBITRATION_VIDEO_PRIORITY_7         7
+#define LOCALMEM_ARBITRATION_PANEL                    6:4
+#define LOCALMEM_ARBITRATION_PANEL_OFF                0
+#define LOCALMEM_ARBITRATION_PANEL_PRIORITY_1         1
+#define LOCALMEM_ARBITRATION_PANEL_PRIORITY_2         2
+#define LOCALMEM_ARBITRATION_PANEL_PRIORITY_3         3
+#define LOCALMEM_ARBITRATION_PANEL_PRIORITY_4         4
+#define LOCALMEM_ARBITRATION_PANEL_PRIORITY_5         5
+#define LOCALMEM_ARBITRATION_PANEL_PRIORITY_6         6
+#define LOCALMEM_ARBITRATION_PANEL_PRIORITY_7         7
+#define LOCALMEM_ARBITRATION_CRT                      2:0
+#define LOCALMEM_ARBITRATION_CRT_OFF                  0
+#define LOCALMEM_ARBITRATION_CRT_PRIORITY_1           1
+#define LOCALMEM_ARBITRATION_CRT_PRIORITY_2           2
+#define LOCALMEM_ARBITRATION_CRT_PRIORITY_3           3
+#define LOCALMEM_ARBITRATION_CRT_PRIORITY_4           4
+#define LOCALMEM_ARBITRATION_CRT_PRIORITY_5           5
+#define LOCALMEM_ARBITRATION_CRT_PRIORITY_6           6
+#define LOCALMEM_ARBITRATION_CRT_PRIORITY_7           7
+
+#define PCIMEM_ARBITRATION                            0x000010
+#define PCIMEM_ARBITRATION_ROTATE                     28:28
+#define PCIMEM_ARBITRATION_ROTATE_OFF                 0
+#define PCIMEM_ARBITRATION_ROTATE_ON                  1
+#define PCIMEM_ARBITRATION_VGA                        26:24
+#define PCIMEM_ARBITRATION_VGA_OFF                    0
+#define PCIMEM_ARBITRATION_VGA_PRIORITY_1             1
+#define PCIMEM_ARBITRATION_VGA_PRIORITY_2             2
+#define PCIMEM_ARBITRATION_VGA_PRIORITY_3             3
+#define PCIMEM_ARBITRATION_VGA_PRIORITY_4             4
+#define PCIMEM_ARBITRATION_VGA_PRIORITY_5             5
+#define PCIMEM_ARBITRATION_VGA_PRIORITY_6             6
+#define PCIMEM_ARBITRATION_VGA_PRIORITY_7             7
+#define PCIMEM_ARBITRATION_DMA                        22:20
+#define PCIMEM_ARBITRATION_DMA_OFF                    0
+#define PCIMEM_ARBITRATION_DMA_PRIORITY_1             1
+#define PCIMEM_ARBITRATION_DMA_PRIORITY_2             2
+#define PCIMEM_ARBITRATION_DMA_PRIORITY_3             3
+#define PCIMEM_ARBITRATION_DMA_PRIORITY_4             4
+#define PCIMEM_ARBITRATION_DMA_PRIORITY_5             5
+#define PCIMEM_ARBITRATION_DMA_PRIORITY_6             6
+#define PCIMEM_ARBITRATION_DMA_PRIORITY_7             7
+#define PCIMEM_ARBITRATION_ZVPORT1                    18:16
+#define PCIMEM_ARBITRATION_ZVPORT1_OFF                0
+#define PCIMEM_ARBITRATION_ZVPORT1_PRIORITY_1         1
+#define PCIMEM_ARBITRATION_ZVPORT1_PRIORITY_2         2
+#define PCIMEM_ARBITRATION_ZVPORT1_PRIORITY_3         3
+#define PCIMEM_ARBITRATION_ZVPORT1_PRIORITY_4         4
+#define PCIMEM_ARBITRATION_ZVPORT1_PRIORITY_5         5
+#define PCIMEM_ARBITRATION_ZVPORT1_PRIORITY_6         6
+#define PCIMEM_ARBITRATION_ZVPORT1_PRIORITY_7         7
+#define PCIMEM_ARBITRATION_ZVPORT0                    14:12
+#define PCIMEM_ARBITRATION_ZVPORT0_OFF                0
+#define PCIMEM_ARBITRATION_ZVPORT0_PRIORITY_1         1
+#define PCIMEM_ARBITRATION_ZVPORT0_PRIORITY_2         2
+#define PCIMEM_ARBITRATION_ZVPORT0_PRIORITY_3         3
+#define PCIMEM_ARBITRATION_ZVPORT0_PRIORITY_4         4
+#define PCIMEM_ARBITRATION_ZVPORT0_PRIORITY_5         5
+#define PCIMEM_ARBITRATION_ZVPORT0_PRIORITY_6         6
+#define PCIMEM_ARBITRATION_ZVPORT0_PRIORITY_7         7
+#define PCIMEM_ARBITRATION_VIDEO                      10:8
+#define PCIMEM_ARBITRATION_VIDEO_OFF                  0
+#define PCIMEM_ARBITRATION_VIDEO_PRIORITY_1           1
+#define PCIMEM_ARBITRATION_VIDEO_PRIORITY_2           2
+#define PCIMEM_ARBITRATION_VIDEO_PRIORITY_3           3
+#define PCIMEM_ARBITRATION_VIDEO_PRIORITY_4           4
+#define PCIMEM_ARBITRATION_VIDEO_PRIORITY_5           5
+#define PCIMEM_ARBITRATION_VIDEO_PRIORITY_6           6
+#define PCIMEM_ARBITRATION_VIDEO_PRIORITY_7           7
+#define PCIMEM_ARBITRATION_PANEL                      6:4
+#define PCIMEM_ARBITRATION_PANEL_OFF                  0
+#define PCIMEM_ARBITRATION_PANEL_PRIORITY_1           1
+#define PCIMEM_ARBITRATION_PANEL_PRIORITY_2           2
+#define PCIMEM_ARBITRATION_PANEL_PRIORITY_3           3
+#define PCIMEM_ARBITRATION_PANEL_PRIORITY_4           4
+#define PCIMEM_ARBITRATION_PANEL_PRIORITY_5           5
+#define PCIMEM_ARBITRATION_PANEL_PRIORITY_6           6
+#define PCIMEM_ARBITRATION_PANEL_PRIORITY_7           7
+#define PCIMEM_ARBITRATION_CRT                        2:0
+#define PCIMEM_ARBITRATION_CRT_OFF                    0
+#define PCIMEM_ARBITRATION_CRT_PRIORITY_1             1
+#define PCIMEM_ARBITRATION_CRT_PRIORITY_2             2
+#define PCIMEM_ARBITRATION_CRT_PRIORITY_3             3
+#define PCIMEM_ARBITRATION_CRT_PRIORITY_4             4
+#define PCIMEM_ARBITRATION_CRT_PRIORITY_5             5
+#define PCIMEM_ARBITRATION_CRT_PRIORITY_6             6
+#define PCIMEM_ARBITRATION_CRT_PRIORITY_7             7
+
+#define RAW_INT                                       0x000020
+#define RAW_INT_ZVPORT1_VSYNC                         4:4
+#define RAW_INT_ZVPORT1_VSYNC_INACTIVE                0
+#define RAW_INT_ZVPORT1_VSYNC_ACTIVE                  1
+#define RAW_INT_ZVPORT1_VSYNC_CLEAR                   1
+#define RAW_INT_ZVPORT0_VSYNC                         3:3
+#define RAW_INT_ZVPORT0_VSYNC_INACTIVE                0
+#define RAW_INT_ZVPORT0_VSYNC_ACTIVE                  1
+#define RAW_INT_ZVPORT0_VSYNC_CLEAR                   1
+#define RAW_INT_CRT_VSYNC                             2:2
+#define RAW_INT_CRT_VSYNC_INACTIVE                    0
+#define RAW_INT_CRT_VSYNC_ACTIVE                      1
+#define RAW_INT_CRT_VSYNC_CLEAR                       1
+#define RAW_INT_PANEL_VSYNC                           1:1
+#define RAW_INT_PANEL_VSYNC_INACTIVE                  0
+#define RAW_INT_PANEL_VSYNC_ACTIVE                    1
+#define RAW_INT_PANEL_VSYNC_CLEAR                     1
+#define RAW_INT_VGA_VSYNC                             0:0
+#define RAW_INT_VGA_VSYNC_INACTIVE                    0
+#define RAW_INT_VGA_VSYNC_ACTIVE                      1
+#define RAW_INT_VGA_VSYNC_CLEAR                       1
+
+#define INT_STATUS                                    0x000024
+#define INT_STATUS_GPIO31                             31:31
+#define INT_STATUS_GPIO31_INACTIVE                    0
+#define INT_STATUS_GPIO31_ACTIVE                      1
+#define INT_STATUS_GPIO30                             30:30
+#define INT_STATUS_GPIO30_INACTIVE                    0
+#define INT_STATUS_GPIO30_ACTIVE                      1
+#define INT_STATUS_GPIO29                             29:29
+#define INT_STATUS_GPIO29_INACTIVE                    0
+#define INT_STATUS_GPIO29_ACTIVE                      1
+#define INT_STATUS_GPIO28                             28:28
+#define INT_STATUS_GPIO28_INACTIVE                    0
+#define INT_STATUS_GPIO28_ACTIVE                      1
+#define INT_STATUS_GPIO27                             27:27
+#define INT_STATUS_GPIO27_INACTIVE                    0
+#define INT_STATUS_GPIO27_ACTIVE                      1
+#define INT_STATUS_GPIO26                             26:26
+#define INT_STATUS_GPIO26_INACTIVE                    0
+#define INT_STATUS_GPIO26_ACTIVE                      1
+#define INT_STATUS_GPIO25                             25:25
+#define INT_STATUS_GPIO25_INACTIVE                    0
+#define INT_STATUS_GPIO25_ACTIVE                      1
+#define INT_STATUS_I2C                                12:12
+#define INT_STATUS_I2C_INACTIVE                       0
+#define INT_STATUS_I2C_ACTIVE                         1
+#define INT_STATUS_PWM                                11:11
+#define INT_STATUS_PWM_INACTIVE                       0
+#define INT_STATUS_PWM_ACTIVE                         1
+#define INT_STATUS_DMA1                               10:10
+#define INT_STATUS_DMA1_INACTIVE                      0
+#define INT_STATUS_DMA1_ACTIVE                        1
+#define INT_STATUS_DMA0                               9:9
+#define INT_STATUS_DMA0_INACTIVE                      0
+#define INT_STATUS_DMA0_ACTIVE                        1
+#define INT_STATUS_PCI                                8:8
+#define INT_STATUS_PCI_INACTIVE                       0
+#define INT_STATUS_PCI_ACTIVE                         1
+#define INT_STATUS_SSP1                               7:7
+#define INT_STATUS_SSP1_INACTIVE                      0
+#define INT_STATUS_SSP1_ACTIVE                        1
+#define INT_STATUS_SSP0                               6:6
+#define INT_STATUS_SSP0_INACTIVE                      0
+#define INT_STATUS_SSP0_ACTIVE                        1
+#define INT_STATUS_DE                                 5:5
+#define INT_STATUS_DE_INACTIVE                        0
+#define INT_STATUS_DE_ACTIVE                          1
+#define INT_STATUS_ZVPORT1_VSYNC                      4:4
+#define INT_STATUS_ZVPORT1_VSYNC_INACTIVE             0
+#define INT_STATUS_ZVPORT1_VSYNC_ACTIVE               1
+#define INT_STATUS_ZVPORT0_VSYNC                      3:3
+#define INT_STATUS_ZVPORT0_VSYNC_INACTIVE             0
+#define INT_STATUS_ZVPORT0_VSYNC_ACTIVE               1
+#define INT_STATUS_CRT_VSYNC                          2:2
+#define INT_STATUS_CRT_VSYNC_INACTIVE                 0
+#define INT_STATUS_CRT_VSYNC_ACTIVE                   1
+#define INT_STATUS_PANEL_VSYNC                        1:1
+#define INT_STATUS_PANEL_VSYNC_INACTIVE               0
+#define INT_STATUS_PANEL_VSYNC_ACTIVE                 1
+#define INT_STATUS_VGA_VSYNC                          0:0
+#define INT_STATUS_VGA_VSYNC_INACTIVE                 0
+#define INT_STATUS_VGA_VSYNC_ACTIVE                   1
+
+#define INT_MASK                                      0x000028
+#define INT_MASK_GPIO31                               31:31
+#define INT_MASK_GPIO31_DISABLE                       0
+#define INT_MASK_GPIO31_ENABLE                        1
+#define INT_MASK_GPIO30                               30:30
+#define INT_MASK_GPIO30_DISABLE                       0
+#define INT_MASK_GPIO30_ENABLE                        1
+#define INT_MASK_GPIO29                               29:29
+#define INT_MASK_GPIO29_DISABLE                       0
+#define INT_MASK_GPIO29_ENABLE                        1
+#define INT_MASK_GPIO28                               28:28
+#define INT_MASK_GPIO28_DISABLE                       0
+#define INT_MASK_GPIO28_ENABLE                        1
+#define INT_MASK_GPIO27                               27:27
+#define INT_MASK_GPIO27_DISABLE                       0
+#define INT_MASK_GPIO27_ENABLE                        1
+#define INT_MASK_GPIO26                               26:26
+#define INT_MASK_GPIO26_DISABLE                       0
+#define INT_MASK_GPIO26_ENABLE                        1
+#define INT_MASK_GPIO25                               25:25
+#define INT_MASK_GPIO25_DISABLE                       0
+#define INT_MASK_GPIO25_ENABLE                        1
+#define INT_MASK_I2C                                  12:12
+#define INT_MASK_I2C_DISABLE                          0
+#define INT_MASK_I2C_ENABLE                           1
+#define INT_MASK_PWM                                  11:11
+#define INT_MASK_PWM_DISABLE                          0
+#define INT_MASK_PWM_ENABLE                           1
+#define INT_MASK_DMA1                                 10:10
+#define INT_MASK_DMA1_DISABLE                         0
+#define INT_MASK_DMA1_ENABLE                          1
+#define INT_MASK_DMA                                  9:9
+#define INT_MASK_DMA_DISABLE                          0
+#define INT_MASK_DMA_ENABLE                           1
+#define INT_MASK_PCI                                  8:8
+#define INT_MASK_PCI_DISABLE                          0
+#define INT_MASK_PCI_ENABLE                           1
+#define INT_MASK_SSP1                                 7:7
+#define INT_MASK_SSP1_DISABLE                         0
+#define INT_MASK_SSP1_ENABLE                          1
+#define INT_MASK_SSP0                                 6:6
+#define INT_MASK_SSP0_DISABLE                         0
+#define INT_MASK_SSP0_ENABLE                          1
+#define INT_MASK_DE                                   5:5
+#define INT_MASK_DE_DISABLE                           0
+#define INT_MASK_DE_ENABLE                            1
+#define INT_MASK_ZVPORT1_VSYNC                        4:4
+#define INT_MASK_ZVPORT1_VSYNC_DISABLE                0
+#define INT_MASK_ZVPORT1_VSYNC_ENABLE                 1
+#define INT_MASK_ZVPORT0_VSYNC                        3:3
+#define INT_MASK_ZVPORT0_VSYNC_DISABLE                0
+#define INT_MASK_ZVPORT0_VSYNC_ENABLE                 1
+#define INT_MASK_CRT_VSYNC                            2:2
+#define INT_MASK_CRT_VSYNC_DISABLE                    0
+#define INT_MASK_CRT_VSYNC_ENABLE                     1
+#define INT_MASK_PANEL_VSYNC                          1:1
+#define INT_MASK_PANEL_VSYNC_DISABLE                  0
+#define INT_MASK_PANEL_VSYNC_ENABLE                   1
+#define INT_MASK_VGA_VSYNC                            0:0
+#define INT_MASK_VGA_VSYNC_DISABLE                    0
+#define INT_MASK_VGA_VSYNC_ENABLE                     1
+
+#define CURRENT_GATE                                  0x000040
+#define CURRENT_GATE_MCLK                             15:14
+#ifdef VALIDATION_CHIP
+    #define CURRENT_GATE_MCLK_112MHZ                      0
+    #define CURRENT_GATE_MCLK_84MHZ                       1
+    #define CURRENT_GATE_MCLK_56MHZ                       2
+    #define CURRENT_GATE_MCLK_42MHZ                       3
+#else
+    #define CURRENT_GATE_MCLK_DIV_3                       0
+    #define CURRENT_GATE_MCLK_DIV_4                       1
+    #define CURRENT_GATE_MCLK_DIV_6                       2
+    #define CURRENT_GATE_MCLK_DIV_8                       3
+#endif
+#define CURRENT_GATE_M2XCLK                           13:12
+#ifdef VALIDATION_CHIP
+    #define CURRENT_GATE_M2XCLK_336MHZ                    0
+    #define CURRENT_GATE_M2XCLK_168MHZ                    1
+    #define CURRENT_GATE_M2XCLK_112MHZ                    2
+    #define CURRENT_GATE_M2XCLK_84MHZ                     3
+#else
+    #define CURRENT_GATE_M2XCLK_DIV_1                     0
+    #define CURRENT_GATE_M2XCLK_DIV_2                     1
+    #define CURRENT_GATE_M2XCLK_DIV_3                     2
+    #define CURRENT_GATE_M2XCLK_DIV_4                     3
+#endif
+#define CURRENT_GATE_VGA                              10:10
+#define CURRENT_GATE_VGA_OFF                          0
+#define CURRENT_GATE_VGA_ON                           1
+#define CURRENT_GATE_PWM                              9:9
+#define CURRENT_GATE_PWM_OFF                          0
+#define CURRENT_GATE_PWM_ON                           1
+#define CURRENT_GATE_I2C                              8:8
+#define CURRENT_GATE_I2C_OFF                          0
+#define CURRENT_GATE_I2C_ON                           1
+#define CURRENT_GATE_SSP                              7:7
+#define CURRENT_GATE_SSP_OFF                          0
+#define CURRENT_GATE_SSP_ON                           1
+#define CURRENT_GATE_GPIO                             6:6
+#define CURRENT_GATE_GPIO_OFF                         0
+#define CURRENT_GATE_GPIO_ON                          1
+#define CURRENT_GATE_ZVPORT                           5:5
+#define CURRENT_GATE_ZVPORT_OFF                       0
+#define CURRENT_GATE_ZVPORT_ON                        1
+#define CURRENT_GATE_CSC                              4:4
+#define CURRENT_GATE_CSC_OFF                          0
+#define CURRENT_GATE_CSC_ON                           1
+#define CURRENT_GATE_DE                               3:3
+#define CURRENT_GATE_DE_OFF                           0
+#define CURRENT_GATE_DE_ON                            1
+#define CURRENT_GATE_DISPLAY                          2:2
+#define CURRENT_GATE_DISPLAY_OFF                      0
+#define CURRENT_GATE_DISPLAY_ON                       1
+#define CURRENT_GATE_LOCALMEM                         1:1
+#define CURRENT_GATE_LOCALMEM_OFF                     0
+#define CURRENT_GATE_LOCALMEM_ON                      1
+#define CURRENT_GATE_DMA                              0:0
+#define CURRENT_GATE_DMA_OFF                          0
+#define CURRENT_GATE_DMA_ON                           1
+
+#define MODE0_GATE                                    0x000044
+#define MODE0_GATE_MCLK                               15:14
+#define MODE0_GATE_MCLK_112MHZ                        0
+#define MODE0_GATE_MCLK_84MHZ                         1
+#define MODE0_GATE_MCLK_56MHZ                         2
+#define MODE0_GATE_MCLK_42MHZ                         3
+#define MODE0_GATE_M2XCLK                             13:12
+#define MODE0_GATE_M2XCLK_336MHZ                      0
+#define MODE0_GATE_M2XCLK_168MHZ                      1
+#define MODE0_GATE_M2XCLK_112MHZ                      2
+#define MODE0_GATE_M2XCLK_84MHZ                       3
+#define MODE0_GATE_VGA                                10:10
+#define MODE0_GATE_VGA_OFF                            0
+#define MODE0_GATE_VGA_ON                             1
+#define MODE0_GATE_PWM                                9:9
+#define MODE0_GATE_PWM_OFF                            0
+#define MODE0_GATE_PWM_ON                             1
+#define MODE0_GATE_I2C                                8:8
+#define MODE0_GATE_I2C_OFF                            0
+#define MODE0_GATE_I2C_ON                             1
+#define MODE0_GATE_SSP                                7:7
+#define MODE0_GATE_SSP_OFF                            0
+#define MODE0_GATE_SSP_ON                             1
+#define MODE0_GATE_GPIO                               6:6
+#define MODE0_GATE_GPIO_OFF                           0
+#define MODE0_GATE_GPIO_ON                            1
+#define MODE0_GATE_ZVPORT                             5:5
+#define MODE0_GATE_ZVPORT_OFF                         0
+#define MODE0_GATE_ZVPORT_ON                          1
+#define MODE0_GATE_CSC                                4:4
+#define MODE0_GATE_CSC_OFF                            0
+#define MODE0_GATE_CSC_ON                             1
+#define MODE0_GATE_DE                                 3:3
+#define MODE0_GATE_DE_OFF                             0
+#define MODE0_GATE_DE_ON                              1
+#define MODE0_GATE_DISPLAY                            2:2
+#define MODE0_GATE_DISPLAY_OFF                        0
+#define MODE0_GATE_DISPLAY_ON                         1
+#define MODE0_GATE_LOCALMEM                           1:1
+#define MODE0_GATE_LOCALMEM_OFF                       0
+#define MODE0_GATE_LOCALMEM_ON                        1
+#define MODE0_GATE_DMA                                0:0
+#define MODE0_GATE_DMA_OFF                            0
+#define MODE0_GATE_DMA_ON                             1
+
+#define MODE1_GATE                                    0x000048
+#define MODE1_GATE_MCLK                               15:14
+#define MODE1_GATE_MCLK_112MHZ                        0
+#define MODE1_GATE_MCLK_84MHZ                         1
+#define MODE1_GATE_MCLK_56MHZ                         2
+#define MODE1_GATE_MCLK_42MHZ                         3
+#define MODE1_GATE_M2XCLK                             13:12
+#define MODE1_GATE_M2XCLK_336MHZ                      0
+#define MODE1_GATE_M2XCLK_168MHZ                      1
+#define MODE1_GATE_M2XCLK_112MHZ                      2
+#define MODE1_GATE_M2XCLK_84MHZ                       3
+#define MODE1_GATE_VGA                                10:10
+#define MODE1_GATE_VGA_OFF                            0
+#define MODE1_GATE_VGA_ON                             1
+#define MODE1_GATE_PWM                                9:9
+#define MODE1_GATE_PWM_OFF                            0
+#define MODE1_GATE_PWM_ON                             1
+#define MODE1_GATE_I2C                                8:8
+#define MODE1_GATE_I2C_OFF                            0
+#define MODE1_GATE_I2C_ON                             1
+#define MODE1_GATE_SSP                                7:7
+#define MODE1_GATE_SSP_OFF                            0
+#define MODE1_GATE_SSP_ON                             1
+#define MODE1_GATE_GPIO                               6:6
+#define MODE1_GATE_GPIO_OFF                           0
+#define MODE1_GATE_GPIO_ON                            1
+#define MODE1_GATE_ZVPORT                             5:5
+#define MODE1_GATE_ZVPORT_OFF                         0
+#define MODE1_GATE_ZVPORT_ON                          1
+#define MODE1_GATE_CSC                                4:4
+#define MODE1_GATE_CSC_OFF                            0
+#define MODE1_GATE_CSC_ON                             1
+#define MODE1_GATE_DE                                 3:3
+#define MODE1_GATE_DE_OFF                             0
+#define MODE1_GATE_DE_ON                              1
+#define MODE1_GATE_DISPLAY                            2:2
+#define MODE1_GATE_DISPLAY_OFF                        0
+#define MODE1_GATE_DISPLAY_ON                         1
+#define MODE1_GATE_LOCALMEM                           1:1
+#define MODE1_GATE_LOCALMEM_OFF                       0
+#define MODE1_GATE_LOCALMEM_ON                        1
+#define MODE1_GATE_DMA                                0:0
+#define MODE1_GATE_DMA_OFF                            0
+#define MODE1_GATE_DMA_ON                             1
+
+#define POWER_MODE_CTRL                               0x00004C
+#ifdef VALIDATION_CHIP
+    #define POWER_MODE_CTRL_336CLK                    4:4
+    #define POWER_MODE_CTRL_336CLK_OFF                0
+    #define POWER_MODE_CTRL_336CLK_ON                 1
+#endif
+#define POWER_MODE_CTRL_OSC_INPUT                     3:3
+#define POWER_MODE_CTRL_OSC_INPUT_OFF                 0
+#define POWER_MODE_CTRL_OSC_INPUT_ON                  1
+#define POWER_MODE_CTRL_ACPI                          2:2
+#define POWER_MODE_CTRL_ACPI_OFF                      0
+#define POWER_MODE_CTRL_ACPI_ON                       1
+#define POWER_MODE_CTRL_MODE                          1:0
+#define POWER_MODE_CTRL_MODE_MODE0                    0
+#define POWER_MODE_CTRL_MODE_MODE1                    1
+#define POWER_MODE_CTRL_MODE_SLEEP                    2
+
+#define PCI_MASTER_BASE                               0x000050
+#define PCI_MASTER_BASE_ADDRESS                       7:0
+
+#define DEVICE_ID                                     0x000054
+#define DEVICE_ID_DEVICE_ID                           31:16
+#define DEVICE_ID_REVISION_ID                         7:0
+
+#define PLL_CLK_COUNT                                 0x000058
+#define PLL_CLK_COUNT_COUNTER                         15:0
+
+#define PANEL_PLL_CTRL                                0x00005C
+#define PANEL_PLL_CTRL_BYPASS                         18:18
+#define PANEL_PLL_CTRL_BYPASS_OFF                     0
+#define PANEL_PLL_CTRL_BYPASS_ON                      1
+#define PANEL_PLL_CTRL_POWER                          17:17
+#define PANEL_PLL_CTRL_POWER_OFF                      0
+#define PANEL_PLL_CTRL_POWER_ON                       1
+#define PANEL_PLL_CTRL_INPUT                          16:16
+#define PANEL_PLL_CTRL_INPUT_OSC                      0
+#define PANEL_PLL_CTRL_INPUT_TESTCLK                  1
+#ifdef VALIDATION_CHIP
+    #define PANEL_PLL_CTRL_OD                         15:14
+#else
+    #define PANEL_PLL_CTRL_POD                        15:14
+    #define PANEL_PLL_CTRL_OD                         13:12
+#endif
+#define PANEL_PLL_CTRL_N                              11:8
+#define PANEL_PLL_CTRL_M                              7:0
+
+#define CRT_PLL_CTRL                                  0x000060
+#define CRT_PLL_CTRL_BYPASS                           18:18
+#define CRT_PLL_CTRL_BYPASS_OFF                       0
+#define CRT_PLL_CTRL_BYPASS_ON                        1
+#define CRT_PLL_CTRL_POWER                            17:17
+#define CRT_PLL_CTRL_POWER_OFF                        0
+#define CRT_PLL_CTRL_POWER_ON                         1
+#define CRT_PLL_CTRL_INPUT                            16:16
+#define CRT_PLL_CTRL_INPUT_OSC                        0
+#define CRT_PLL_CTRL_INPUT_TESTCLK                    1
+#ifdef VALIDATION_CHIP
+    #define CRT_PLL_CTRL_OD                           15:14
+#else
+    #define CRT_PLL_CTRL_POD                          15:14
+    #define CRT_PLL_CTRL_OD                           13:12
+#endif
+#define CRT_PLL_CTRL_N                                11:8
+#define CRT_PLL_CTRL_M                                7:0
+
+#define VGA_PLL0_CTRL                                 0x000064
+#define VGA_PLL0_CTRL_BYPASS                          18:18
+#define VGA_PLL0_CTRL_BYPASS_OFF                      0
+#define VGA_PLL0_CTRL_BYPASS_ON                       1
+#define VGA_PLL0_CTRL_POWER                           17:17
+#define VGA_PLL0_CTRL_POWER_OFF                       0
+#define VGA_PLL0_CTRL_POWER_ON                        1
+#define VGA_PLL0_CTRL_INPUT                           16:16
+#define VGA_PLL0_CTRL_INPUT_OSC                       0
+#define VGA_PLL0_CTRL_INPUT_TESTCLK                   1
+#ifdef VALIDATION_CHIP
+    #define VGA_PLL0_CTRL_OD                          15:14
+#else
+    #define VGA_PLL0_CTRL_POD                         15:14
+    #define VGA_PLL0_CTRL_OD                          13:12
+#endif
+#define VGA_PLL0_CTRL_N                               11:8
+#define VGA_PLL0_CTRL_M                               7:0
+
+#define VGA_PLL1_CTRL                                 0x000068
+#define VGA_PLL1_CTRL_BYPASS                          18:18
+#define VGA_PLL1_CTRL_BYPASS_OFF                      0
+#define VGA_PLL1_CTRL_BYPASS_ON                       1
+#define VGA_PLL1_CTRL_POWER                           17:17
+#define VGA_PLL1_CTRL_POWER_OFF                       0
+#define VGA_PLL1_CTRL_POWER_ON                        1
+#define VGA_PLL1_CTRL_INPUT                           16:16
+#define VGA_PLL1_CTRL_INPUT_OSC                       0
+#define VGA_PLL1_CTRL_INPUT_TESTCLK                   1
+#ifdef VALIDATION_CHIP
+    #define VGA_PLL1_CTRL_OD                          15:14
+#else
+    #define VGA_PLL1_CTRL_POD                         15:14
+    #define VGA_PLL1_CTRL_OD                          13:12
+#endif
+#define VGA_PLL1_CTRL_N                               11:8
+#define VGA_PLL1_CTRL_M                               7:0
+
+#define SCRATCH_DATA                                  0x00006c
+
+#ifndef VALIDATION_CHIP
+
+#define MXCLK_PLL_CTRL                                0x000070
+#define MXCLK_PLL_CTRL_BYPASS                         18:18
+#define MXCLK_PLL_CTRL_BYPASS_OFF                     0
+#define MXCLK_PLL_CTRL_BYPASS_ON                      1
+#define MXCLK_PLL_CTRL_POWER                          17:17
+#define MXCLK_PLL_CTRL_POWER_OFF                      0
+#define MXCLK_PLL_CTRL_POWER_ON                       1
+#define MXCLK_PLL_CTRL_INPUT                          16:16
+#define MXCLK_PLL_CTRL_INPUT_OSC                      0
+#define MXCLK_PLL_CTRL_INPUT_TESTCLK                  1
+#define MXCLK_PLL_CTRL_POD                            15:14
+#define MXCLK_PLL_CTRL_OD                             13:12
+#define MXCLK_PLL_CTRL_N                              11:8
+#define MXCLK_PLL_CTRL_M                              7:0
+
+#define VGA_CONFIGURATION                             0x000088
+#define VGA_CONFIGURATION_USER_DEFINE                 5:4
+#define VGA_CONFIGURATION_PLL                         2:2
+#define VGA_CONFIGURATION_PLL_VGA                     0
+#define VGA_CONFIGURATION_PLL_PANEL                   1
+#define VGA_CONFIGURATION_MODE                        1:1
+#define VGA_CONFIGURATION_MODE_TEXT                   0
+#define VGA_CONFIGURATION_MODE_GRAPHIC                1
+
+#endif
+
+#define GPIO_DATA                                       0x010000
+#define GPIO_DATA_31                                    31:31
+#define GPIO_DATA_30                                    30:30
+#define GPIO_DATA_29                                    29:29
+#define GPIO_DATA_28                                    28:28
+#define GPIO_DATA_27                                    27:27
+#define GPIO_DATA_26                                    26:26
+#define GPIO_DATA_25                                    25:25
+#define GPIO_DATA_24                                    24:24
+#define GPIO_DATA_23                                    23:23
+#define GPIO_DATA_22                                    22:22
+#define GPIO_DATA_21                                    21:21
+#define GPIO_DATA_20                                    20:20
+#define GPIO_DATA_19                                    19:19
+#define GPIO_DATA_18                                    18:18
+#define GPIO_DATA_17                                    17:17
+#define GPIO_DATA_16                                    16:16
+#define GPIO_DATA_15                                    15:15
+#define GPIO_DATA_14                                    14:14
+#define GPIO_DATA_13                                    13:13
+#define GPIO_DATA_12                                    12:12
+#define GPIO_DATA_11                                    11:11
+#define GPIO_DATA_10                                    10:10
+#define GPIO_DATA_9                                     9:9
+#define GPIO_DATA_8                                     8:8
+#define GPIO_DATA_7                                     7:7
+#define GPIO_DATA_6                                     6:6
+#define GPIO_DATA_5                                     5:5
+#define GPIO_DATA_4                                     4:4
+#define GPIO_DATA_3                                     3:3
+#define GPIO_DATA_2                                     2:2
+#define GPIO_DATA_1                                     1:1
+#define GPIO_DATA_0                                     0:0
+
+#define GPIO_DATA_DIRECTION                             0x010004
+#define GPIO_DATA_DIRECTION_31                          31:31
+#define GPIO_DATA_DIRECTION_31_INPUT                    0
+#define GPIO_DATA_DIRECTION_31_OUTPUT                   1
+#define GPIO_DATA_DIRECTION_30                          30:30
+#define GPIO_DATA_DIRECTION_30_INPUT                    0
+#define GPIO_DATA_DIRECTION_30_OUTPUT                   1
+#define GPIO_DATA_DIRECTION_29                          29:29
+#define GPIO_DATA_DIRECTION_29_INPUT                    0
+#define GPIO_DATA_DIRECTION_29_OUTPUT                   1
+#define GPIO_DATA_DIRECTION_28                          28:28
+#define GPIO_DATA_DIRECTION_28_INPUT                    0
+#define GPIO_DATA_DIRECTION_28_OUTPUT                   1
+#define GPIO_DATA_DIRECTION_27                          27:27
+#define GPIO_DATA_DIRECTION_27_INPUT                    0
+#define GPIO_DATA_DIRECTION_27_OUTPUT                   1
+#define GPIO_DATA_DIRECTION_26                          26:26
+#define GPIO_DATA_DIRECTION_26_INPUT                    0
+#define GPIO_DATA_DIRECTION_26_OUTPUT                   1
+#define GPIO_DATA_DIRECTION_25                          25:25
+#define GPIO_DATA_DIRECTION_25_INPUT                    0
+#define GPIO_DATA_DIRECTION_25_OUTPUT                   1
+#define GPIO_DATA_DIRECTION_24                          24:24
+#define GPIO_DATA_DIRECTION_24_INPUT                    0
+#define GPIO_DATA_DIRECTION_24_OUTPUT                   1
+#define GPIO_DATA_DIRECTION_23                          23:23
+#define GPIO_DATA_DIRECTION_23_INPUT                    0
+#define GPIO_DATA_DIRECTION_23_OUTPUT                   1
+#define GPIO_DATA_DIRECTION_22                          22:22
+#define GPIO_DATA_DIRECTION_22_INPUT                    0
+#define GPIO_DATA_DIRECTION_22_OUTPUT                   1
+#define GPIO_DATA_DIRECTION_21                          21:21
+#define GPIO_DATA_DIRECTION_21_INPUT                    0
+#define GPIO_DATA_DIRECTION_21_OUTPUT                   1
+#define GPIO_DATA_DIRECTION_20                          20:20
+#define GPIO_DATA_DIRECTION_20_INPUT                    0
+#define GPIO_DATA_DIRECTION_20_OUTPUT                   1
+#define GPIO_DATA_DIRECTION_19                          19:19
+#define GPIO_DATA_DIRECTION_19_INPUT                    0
+#define GPIO_DATA_DIRECTION_19_OUTPUT                   1
+#define GPIO_DATA_DIRECTION_18                          18:18
+#define GPIO_DATA_DIRECTION_18_INPUT                    0
+#define GPIO_DATA_DIRECTION_18_OUTPUT                   1
+#define GPIO_DATA_DIRECTION_17                          17:17
+#define GPIO_DATA_DIRECTION_17_INPUT                    0
+#define GPIO_DATA_DIRECTION_17_OUTPUT                   1
+#define GPIO_DATA_DIRECTION_16                          16:16
+#define GPIO_DATA_DIRECTION_16_INPUT                    0
+#define GPIO_DATA_DIRECTION_16_OUTPUT                   1
+#define GPIO_DATA_DIRECTION_15                          15:15
+#define GPIO_DATA_DIRECTION_15_INPUT                    0
+#define GPIO_DATA_DIRECTION_15_OUTPUT                   1
+#define GPIO_DATA_DIRECTION_14                          14:14
+#define GPIO_DATA_DIRECTION_14_INPUT                    0
+#define GPIO_DATA_DIRECTION_14_OUTPUT                   1
+#define GPIO_DATA_DIRECTION_13                          13:13
+#define GPIO_DATA_DIRECTION_13_INPUT                    0
+#define GPIO_DATA_DIRECTION_13_OUTPUT                   1
+#define GPIO_DATA_DIRECTION_12                          12:12
+#define GPIO_DATA_DIRECTION_12_INPUT                    0
+#define GPIO_DATA_DIRECTION_12_OUTPUT                   1
+#define GPIO_DATA_DIRECTION_11                          11:11
+#define GPIO_DATA_DIRECTION_11_INPUT                    0
+#define GPIO_DATA_DIRECTION_11_OUTPUT                   1
+#define GPIO_DATA_DIRECTION_10                          10:10
+#define GPIO_DATA_DIRECTION_10_INPUT                    0
+#define GPIO_DATA_DIRECTION_10_OUTPUT                   1
+#define GPIO_DATA_DIRECTION_9                           9:9
+#define GPIO_DATA_DIRECTION_9_INPUT                     0
+#define GPIO_DATA_DIRECTION_9_OUTPUT                    1
+#define GPIO_DATA_DIRECTION_8                           8:8
+#define GPIO_DATA_DIRECTION_8_INPUT                     0
+#define GPIO_DATA_DIRECTION_8_OUTPUT                    1
+#define GPIO_DATA_DIRECTION_7                           7:7
+#define GPIO_DATA_DIRECTION_7_INPUT                     0
+#define GPIO_DATA_DIRECTION_7_OUTPUT                    1
+#define GPIO_DATA_DIRECTION_6                           6:6
+#define GPIO_DATA_DIRECTION_6_INPUT                     0
+#define GPIO_DATA_DIRECTION_6_OUTPUT                    1
+#define GPIO_DATA_DIRECTION_5                           5:5
+#define GPIO_DATA_DIRECTION_5_INPUT                     0
+#define GPIO_DATA_DIRECTION_5_OUTPUT                    1
+#define GPIO_DATA_DIRECTION_4                           4:4
+#define GPIO_DATA_DIRECTION_4_INPUT                     0
+#define GPIO_DATA_DIRECTION_4_OUTPUT                    1
+#define GPIO_DATA_DIRECTION_3                           3:3
+#define GPIO_DATA_DIRECTION_3_INPUT                     0
+#define GPIO_DATA_DIRECTION_3_OUTPUT                    1
+#define GPIO_DATA_DIRECTION_2                           2:2
+#define GPIO_DATA_DIRECTION_2_INPUT                     0
+#define GPIO_DATA_DIRECTION_2_OUTPUT                    1
+#define GPIO_DATA_DIRECTION_1                           131
+#define GPIO_DATA_DIRECTION_1_INPUT                     0
+#define GPIO_DATA_DIRECTION_1_OUTPUT                    1
+#define GPIO_DATA_DIRECTION_0                           0:0
+#define GPIO_DATA_DIRECTION_0_INPUT                     0
+#define GPIO_DATA_DIRECTION_0_OUTPUT                    1
+
+#define GPIO_INTERRUPT_SETUP                            0x010008
+#define GPIO_INTERRUPT_SETUP_TRIGGER_31                 22:22
+#define GPIO_INTERRUPT_SETUP_TRIGGER_31_EDGE            0
+#define GPIO_INTERRUPT_SETUP_TRIGGER_31_LEVEL           1
+#define GPIO_INTERRUPT_SETUP_TRIGGER_30                 21:21
+#define GPIO_INTERRUPT_SETUP_TRIGGER_30_EDGE            0
+#define GPIO_INTERRUPT_SETUP_TRIGGER_30_LEVEL           1
+#define GPIO_INTERRUPT_SETUP_TRIGGER_29                 20:20
+#define GPIO_INTERRUPT_SETUP_TRIGGER_29_EDGE            0
+#define GPIO_INTERRUPT_SETUP_TRIGGER_29_LEVEL           1
+#define GPIO_INTERRUPT_SETUP_TRIGGER_28                 19:19
+#define GPIO_INTERRUPT_SETUP_TRIGGER_28_EDGE            0
+#define GPIO_INTERRUPT_SETUP_TRIGGER_28_LEVEL           1
+#define GPIO_INTERRUPT_SETUP_TRIGGER_27                 18:18
+#define GPIO_INTERRUPT_SETUP_TRIGGER_27_EDGE            0
+#define GPIO_INTERRUPT_SETUP_TRIGGER_27_LEVEL           1
+#define GPIO_INTERRUPT_SETUP_TRIGGER_26                 17:17
+#define GPIO_INTERRUPT_SETUP_TRIGGER_26_EDGE            0
+#define GPIO_INTERRUPT_SETUP_TRIGGER_26_LEVEL           1
+#define GPIO_INTERRUPT_SETUP_TRIGGER_25                 16:16
+#define GPIO_INTERRUPT_SETUP_TRIGGER_25_EDGE            0
+#define GPIO_INTERRUPT_SETUP_TRIGGER_25_LEVEL           1
+#define GPIO_INTERRUPT_SETUP_ACTIVE_31                  14:14
+#define GPIO_INTERRUPT_SETUP_ACTIVE_31_LOW              0
+#define GPIO_INTERRUPT_SETUP_ACTIVE_31_HIGH             1
+#define GPIO_INTERRUPT_SETUP_ACTIVE_30                  13:13
+#define GPIO_INTERRUPT_SETUP_ACTIVE_30_LOW              0
+#define GPIO_INTERRUPT_SETUP_ACTIVE_30_HIGH             1
+#define GPIO_INTERRUPT_SETUP_ACTIVE_29                  12:12
+#define GPIO_INTERRUPT_SETUP_ACTIVE_29_LOW              0
+#define GPIO_INTERRUPT_SETUP_ACTIVE_29_HIGH             1
+#define GPIO_INTERRUPT_SETUP_ACTIVE_28                  11:11
+#define GPIO_INTERRUPT_SETUP_ACTIVE_28_LOW              0
+#define GPIO_INTERRUPT_SETUP_ACTIVE_28_HIGH             1
+#define GPIO_INTERRUPT_SETUP_ACTIVE_27                  10:10
+#define GPIO_INTERRUPT_SETUP_ACTIVE_27_LOW              0
+#define GPIO_INTERRUPT_SETUP_ACTIVE_27_HIGH             1
+#define GPIO_INTERRUPT_SETUP_ACTIVE_26                  9:9
+#define GPIO_INTERRUPT_SETUP_ACTIVE_26_LOW              0
+#define GPIO_INTERRUPT_SETUP_ACTIVE_26_HIGH             1
+#define GPIO_INTERRUPT_SETUP_ACTIVE_25                  8:8
+#define GPIO_INTERRUPT_SETUP_ACTIVE_25_LOW              0
+#define GPIO_INTERRUPT_SETUP_ACTIVE_25_HIGH             1
+#define GPIO_INTERRUPT_SETUP_ENABLE_31                  6:6
+#define GPIO_INTERRUPT_SETUP_ENABLE_31_GPIO             0
+#define GPIO_INTERRUPT_SETUP_ENABLE_31_INTERRUPT        1
+#define GPIO_INTERRUPT_SETUP_ENABLE_30                  5:5
+#define GPIO_INTERRUPT_SETUP_ENABLE_30_GPIO             0
+#define GPIO_INTERRUPT_SETUP_ENABLE_30_INTERRUPT        1
+#define GPIO_INTERRUPT_SETUP_ENABLE_29                  4:4
+#define GPIO_INTERRUPT_SETUP_ENABLE_29_GPIO             0
+#define GPIO_INTERRUPT_SETUP_ENABLE_29_INTERRUPT        1
+#define GPIO_INTERRUPT_SETUP_ENABLE_28                  3:3
+#define GPIO_INTERRUPT_SETUP_ENABLE_28_GPIO             0
+#define GPIO_INTERRUPT_SETUP_ENABLE_28_INTERRUPT        1
+#define GPIO_INTERRUPT_SETUP_ENABLE_27                  2:2
+#define GPIO_INTERRUPT_SETUP_ENABLE_27_GPIO             0
+#define GPIO_INTERRUPT_SETUP_ENABLE_27_INTERRUPT        1
+#define GPIO_INTERRUPT_SETUP_ENABLE_26                  1:1
+#define GPIO_INTERRUPT_SETUP_ENABLE_26_GPIO             0
+#define GPIO_INTERRUPT_SETUP_ENABLE_26_INTERRUPT        1
+#define GPIO_INTERRUPT_SETUP_ENABLE_25                  0:0
+#define GPIO_INTERRUPT_SETUP_ENABLE_25_GPIO             0
+#define GPIO_INTERRUPT_SETUP_ENABLE_25_INTERRUPT        1
+
+#define GPIO_INTERRUPT_STATUS                           0x01000C
+#define GPIO_INTERRUPT_STATUS_31                        22:22
+#define GPIO_INTERRUPT_STATUS_31_INACTIVE               0
+#define GPIO_INTERRUPT_STATUS_31_ACTIVE                 1
+#define GPIO_INTERRUPT_STATUS_31_RESET                  1
+#define GPIO_INTERRUPT_STATUS_30                        21:21
+#define GPIO_INTERRUPT_STATUS_30_INACTIVE               0
+#define GPIO_INTERRUPT_STATUS_30_ACTIVE                 1
+#define GPIO_INTERRUPT_STATUS_30_RESET                  1
+#define GPIO_INTERRUPT_STATUS_29                        20:20
+#define GPIO_INTERRUPT_STATUS_29_INACTIVE               0
+#define GPIO_INTERRUPT_STATUS_29_ACTIVE                 1
+#define GPIO_INTERRUPT_STATUS_29_RESET                  1
+#define GPIO_INTERRUPT_STATUS_28                        19:19
+#define GPIO_INTERRUPT_STATUS_28_INACTIVE               0
+#define GPIO_INTERRUPT_STATUS_28_ACTIVE                 1
+#define GPIO_INTERRUPT_STATUS_28_RESET                  1
+#define GPIO_INTERRUPT_STATUS_27                        18:18
+#define GPIO_INTERRUPT_STATUS_27_INACTIVE               0
+#define GPIO_INTERRUPT_STATUS_27_ACTIVE                 1
+#define GPIO_INTERRUPT_STATUS_27_RESET                  1
+#define GPIO_INTERRUPT_STATUS_26                        17:17
+#define GPIO_INTERRUPT_STATUS_26_INACTIVE               0
+#define GPIO_INTERRUPT_STATUS_26_ACTIVE                 1
+#define GPIO_INTERRUPT_STATUS_26_RESET                  1
+#define GPIO_INTERRUPT_STATUS_25                        16:16
+#define GPIO_INTERRUPT_STATUS_25_INACTIVE               0
+#define GPIO_INTERRUPT_STATUS_25_ACTIVE                 1
+#define GPIO_INTERRUPT_STATUS_25_RESET                  1
+
+
+#define PANEL_DISPLAY_CTRL                            0x080000
+#define PANEL_DISPLAY_CTRL_RESERVED_1_MASK            31:30
+#define PANEL_DISPLAY_CTRL_RESERVED_1_MASK_DISABLE    0
+#define PANEL_DISPLAY_CTRL_RESERVED_1_MASK_ENABLE     3
+#define PANEL_DISPLAY_CTRL_SELECT                     29:28
+#define PANEL_DISPLAY_CTRL_SELECT_PANEL               0
+#define PANEL_DISPLAY_CTRL_SELECT_VGA                 1
+#define PANEL_DISPLAY_CTRL_SELECT_CRT                 2
+#define PANEL_DISPLAY_CTRL_FPEN                       27:27
+#define PANEL_DISPLAY_CTRL_FPEN_LOW                   0
+#define PANEL_DISPLAY_CTRL_FPEN_HIGH                  1
+#define PANEL_DISPLAY_CTRL_VBIASEN                    26:26
+#define PANEL_DISPLAY_CTRL_VBIASEN_LOW                0
+#define PANEL_DISPLAY_CTRL_VBIASEN_HIGH               1
+#define PANEL_DISPLAY_CTRL_DATA                       25:25
+#define PANEL_DISPLAY_CTRL_DATA_DISABLE               0
+#define PANEL_DISPLAY_CTRL_DATA_ENABLE                1
+#define PANEL_DISPLAY_CTRL_FPVDDEN                    24:24
+#define PANEL_DISPLAY_CTRL_FPVDDEN_LOW                0
+#define PANEL_DISPLAY_CTRL_FPVDDEN_HIGH               1
+#define PANEL_DISPLAY_CTRL_RESERVED_2_MASK            23:20
+#define PANEL_DISPLAY_CTRL_RESERVED_2_MASK_DISABLE    0
+#define PANEL_DISPLAY_CTRL_RESERVED_2_MASK_ENABLE     15
+
+#define PANEL_DISPLAY_CTRL_TFT_DISP 19:18
+#define PANEL_DISPLAY_CTRL_TFT_DISP_24 0
+#define PANEL_DISPLAY_CTRL_TFT_DISP_36 1
+#define PANEL_DISPLAY_CTRL_TFT_DISP_18 2
+
+
+#define PANEL_DISPLAY_CTRL_DUAL_DISPLAY               19:19
+#define PANEL_DISPLAY_CTRL_DUAL_DISPLAY_DISABLE       0
+#define PANEL_DISPLAY_CTRL_DUAL_DISPLAY_ENABLE        1
+#define PANEL_DISPLAY_CTRL_DOUBLE_PIXEL               18:18
+#define PANEL_DISPLAY_CTRL_DOUBLE_PIXEL_DISABLE       0
+#define PANEL_DISPLAY_CTRL_DOUBLE_PIXEL_ENABLE        1
+#define PANEL_DISPLAY_CTRL_FIFO                       17:16
+#define PANEL_DISPLAY_CTRL_FIFO_1                     0
+#define PANEL_DISPLAY_CTRL_FIFO_3                     1
+#define PANEL_DISPLAY_CTRL_FIFO_7                     2
+#define PANEL_DISPLAY_CTRL_FIFO_11                    3
+#define PANEL_DISPLAY_CTRL_RESERVED_3_MASK            15:15
+#define PANEL_DISPLAY_CTRL_RESERVED_3_MASK_DISABLE    0
+#define PANEL_DISPLAY_CTRL_RESERVED_3_MASK_ENABLE     1
+#define PANEL_DISPLAY_CTRL_CLOCK_PHASE                14:14
+#define PANEL_DISPLAY_CTRL_CLOCK_PHASE_ACTIVE_HIGH    0
+#define PANEL_DISPLAY_CTRL_CLOCK_PHASE_ACTIVE_LOW     1
+#define PANEL_DISPLAY_CTRL_VSYNC_PHASE                13:13
+#define PANEL_DISPLAY_CTRL_VSYNC_PHASE_ACTIVE_HIGH    0
+#define PANEL_DISPLAY_CTRL_VSYNC_PHASE_ACTIVE_LOW     1
+#define PANEL_DISPLAY_CTRL_HSYNC_PHASE                12:12
+#define PANEL_DISPLAY_CTRL_HSYNC_PHASE_ACTIVE_HIGH    0
+#define PANEL_DISPLAY_CTRL_HSYNC_PHASE_ACTIVE_LOW     1
+#define PANEL_DISPLAY_CTRL_VSYNC                      11:11
+#define PANEL_DISPLAY_CTRL_VSYNC_ACTIVE_HIGH          0
+#define PANEL_DISPLAY_CTRL_VSYNC_ACTIVE_LOW           1
+#define PANEL_DISPLAY_CTRL_CAPTURE_TIMING             10:10
+#define PANEL_DISPLAY_CTRL_CAPTURE_TIMING_DISABLE     0
+#define PANEL_DISPLAY_CTRL_CAPTURE_TIMING_ENABLE      1
+#define PANEL_DISPLAY_CTRL_COLOR_KEY                  9:9
+#define PANEL_DISPLAY_CTRL_COLOR_KEY_DISABLE          0
+#define PANEL_DISPLAY_CTRL_COLOR_KEY_ENABLE           1
+#define PANEL_DISPLAY_CTRL_TIMING                     8:8
+#define PANEL_DISPLAY_CTRL_TIMING_DISABLE             0
+#define PANEL_DISPLAY_CTRL_TIMING_ENABLE              1
+#define PANEL_DISPLAY_CTRL_VERTICAL_PAN_DIR           7:7
+#define PANEL_DISPLAY_CTRL_VERTICAL_PAN_DIR_DOWN      0
+#define PANEL_DISPLAY_CTRL_VERTICAL_PAN_DIR_UP        1
+#define PANEL_DISPLAY_CTRL_VERTICAL_PAN               6:6
+#define PANEL_DISPLAY_CTRL_VERTICAL_PAN_DISABLE       0
+#define PANEL_DISPLAY_CTRL_VERTICAL_PAN_ENABLE        1
+#define PANEL_DISPLAY_CTRL_HORIZONTAL_PAN_DIR         5:5
+#define PANEL_DISPLAY_CTRL_HORIZONTAL_PAN_DIR_RIGHT   0
+#define PANEL_DISPLAY_CTRL_HORIZONTAL_PAN_DIR_LEFT    1
+#define PANEL_DISPLAY_CTRL_HORIZONTAL_PAN             4:4
+#define PANEL_DISPLAY_CTRL_HORIZONTAL_PAN_DISABLE     0
+#define PANEL_DISPLAY_CTRL_HORIZONTAL_PAN_ENABLE      1
+#define PANEL_DISPLAY_CTRL_GAMMA                      3:3
+#define PANEL_DISPLAY_CTRL_GAMMA_DISABLE              0
+#define PANEL_DISPLAY_CTRL_GAMMA_ENABLE               1
+#define PANEL_DISPLAY_CTRL_PLANE                      2:2
+#define PANEL_DISPLAY_CTRL_PLANE_DISABLE              0
+#define PANEL_DISPLAY_CTRL_PLANE_ENABLE               1
+#define PANEL_DISPLAY_CTRL_FORMAT                     1:0
+#define PANEL_DISPLAY_CTRL_FORMAT_8                   0
+#define PANEL_DISPLAY_CTRL_FORMAT_16                  1
+#define PANEL_DISPLAY_CTRL_FORMAT_32                  2
+
+#define PANEL_PAN_CTRL                                0x080004
+#define PANEL_PAN_CTRL_VERTICAL_PAN                   31:24
+#define PANEL_PAN_CTRL_VERTICAL_VSYNC                 21:16
+#define PANEL_PAN_CTRL_HORIZONTAL_PAN                 15:8
+#define PANEL_PAN_CTRL_HORIZONTAL_VSYNC               5:0
+
+#define PANEL_COLOR_KEY                               0x080008
+#define PANEL_COLOR_KEY_MASK                          31:16
+#define PANEL_COLOR_KEY_VALUE                         15:0
+
+#define PANEL_FB_ADDRESS                              0x08000C
+#define PANEL_FB_ADDRESS_STATUS                       31:31
+#define PANEL_FB_ADDRESS_STATUS_CURRENT               0
+#define PANEL_FB_ADDRESS_STATUS_PENDING               1
+#define PANEL_FB_ADDRESS_EXT                          27:27
+#define PANEL_FB_ADDRESS_EXT_LOCAL                    0
+#define PANEL_FB_ADDRESS_EXT_EXTERNAL                 1
+#define PANEL_FB_ADDRESS_ADDRESS                      25:0
+
+#define PANEL_FB_WIDTH                                0x080010
+#define PANEL_FB_WIDTH_WIDTH                          29:16
+#define PANEL_FB_WIDTH_OFFSET                         13:0
+
+#define PANEL_WINDOW_WIDTH                            0x080014
+#define PANEL_WINDOW_WIDTH_WIDTH                      27:16
+#define PANEL_WINDOW_WIDTH_X                          11:0
+
+#define PANEL_WINDOW_HEIGHT                           0x080018
+#define PANEL_WINDOW_HEIGHT_HEIGHT                    27:16
+#define PANEL_WINDOW_HEIGHT_Y                         11:0
+
+#define PANEL_PLANE_TL                                0x08001C
+#define PANEL_PLANE_TL_TOP                            26:16
+#define PANEL_PLANE_TL_LEFT                           10:0
+
+#define PANEL_PLANE_BR                                0x080020
+#define PANEL_PLANE_BR_BOTTOM                         26:16
+#define PANEL_PLANE_BR_RIGHT                          10:0
+
+#define PANEL_HORIZONTAL_TOTAL                        0x080024
+#define PANEL_HORIZONTAL_TOTAL_TOTAL                  27:16
+#define PANEL_HORIZONTAL_TOTAL_DISPLAY_END            11:0
+
+#define PANEL_HORIZONTAL_SYNC                         0x080028
+#define PANEL_HORIZONTAL_SYNC_WIDTH                   23:16
+#define PANEL_HORIZONTAL_SYNC_START                   11:0
+
+#define PANEL_VERTICAL_TOTAL                          0x08002C
+#define PANEL_VERTICAL_TOTAL_TOTAL                    26:16
+#define PANEL_VERTICAL_TOTAL_DISPLAY_END              10:0
+
+#define PANEL_VERTICAL_SYNC                           0x080030
+#define PANEL_VERTICAL_SYNC_HEIGHT                    21:16
+#define PANEL_VERTICAL_SYNC_START                     10:0
+
+#define PANEL_CURRENT_LINE                            0x080034
+#define PANEL_CURRENT_LINE_LINE                       10:0
+
+/* Video Control */
+
+#define VIDEO_DISPLAY_CTRL                              0x080040
+#define VIDEO_DISPLAY_CTRL_LINE_BUFFER                  18:18
+#define VIDEO_DISPLAY_CTRL_LINE_BUFFER_DISABLE          0
+#define VIDEO_DISPLAY_CTRL_LINE_BUFFER_ENABLE           1
+#define VIDEO_DISPLAY_CTRL_FIFO                         17:16
+#define VIDEO_DISPLAY_CTRL_FIFO_1                       0
+#define VIDEO_DISPLAY_CTRL_FIFO_3                       1
+#define VIDEO_DISPLAY_CTRL_FIFO_7                       2
+#define VIDEO_DISPLAY_CTRL_FIFO_11                      3
+#define VIDEO_DISPLAY_CTRL_BUFFER                       15:15
+#define VIDEO_DISPLAY_CTRL_BUFFER_0                     0
+#define VIDEO_DISPLAY_CTRL_BUFFER_1                     1
+#define VIDEO_DISPLAY_CTRL_CAPTURE                      14:14
+#define VIDEO_DISPLAY_CTRL_CAPTURE_DISABLE              0
+#define VIDEO_DISPLAY_CTRL_CAPTURE_ENABLE               1
+#define VIDEO_DISPLAY_CTRL_DOUBLE_BUFFER                13:13
+#define VIDEO_DISPLAY_CTRL_DOUBLE_BUFFER_DISABLE        0
+#define VIDEO_DISPLAY_CTRL_DOUBLE_BUFFER_ENABLE         1
+#define VIDEO_DISPLAY_CTRL_BYTE_SWAP                    12:12
+#define VIDEO_DISPLAY_CTRL_BYTE_SWAP_DISABLE            0
+#define VIDEO_DISPLAY_CTRL_BYTE_SWAP_ENABLE             1
+#define VIDEO_DISPLAY_CTRL_VERTICAL_SCALE               11:11
+#define VIDEO_DISPLAY_CTRL_VERTICAL_SCALE_NORMAL        0
+#define VIDEO_DISPLAY_CTRL_VERTICAL_SCALE_HALF          1
+#define VIDEO_DISPLAY_CTRL_HORIZONTAL_SCALE             10:10
+#define VIDEO_DISPLAY_CTRL_HORIZONTAL_SCALE_NORMAL      0
+#define VIDEO_DISPLAY_CTRL_HORIZONTAL_SCALE_HALF        1
+#define VIDEO_DISPLAY_CTRL_VERTICAL_MODE                9:9
+#define VIDEO_DISPLAY_CTRL_VERTICAL_MODE_REPLICATE      0
+#define VIDEO_DISPLAY_CTRL_VERTICAL_MODE_INTERPOLATE    1
+#define VIDEO_DISPLAY_CTRL_HORIZONTAL_MODE              8:8
+#define VIDEO_DISPLAY_CTRL_HORIZONTAL_MODE_REPLICATE    0
+#define VIDEO_DISPLAY_CTRL_HORIZONTAL_MODE_INTERPOLATE  1
+#define VIDEO_DISPLAY_CTRL_PIXEL                        7:4
+#define VIDEO_DISPLAY_CTRL_GAMMA                        3:3
+#define VIDEO_DISPLAY_CTRL_GAMMA_DISABLE                0
+#define VIDEO_DISPLAY_CTRL_GAMMA_ENABLE                 1
+#define VIDEO_DISPLAY_CTRL_PLANE                        2:2
+#define VIDEO_DISPLAY_CTRL_PLANE_DISABLE                0
+#define VIDEO_DISPLAY_CTRL_PLANE_ENABLE                 1
+#define VIDEO_DISPLAY_CTRL_FORMAT                       1:0
+#define VIDEO_DISPLAY_CTRL_FORMAT_8                     0
+#define VIDEO_DISPLAY_CTRL_FORMAT_16                    1
+#define VIDEO_DISPLAY_CTRL_FORMAT_32                    2
+#define VIDEO_DISPLAY_CTRL_FORMAT_YUV                   3
+
+#define VIDEO_FB_0_ADDRESS                            0x080044
+#define VIDEO_FB_0_ADDRESS_STATUS                     31:31
+#define VIDEO_FB_0_ADDRESS_STATUS_CURRENT             0
+#define VIDEO_FB_0_ADDRESS_STATUS_PENDING             1
+#define VIDEO_FB_0_ADDRESS_EXT                        27:27
+#define VIDEO_FB_0_ADDRESS_EXT_LOCAL                  0
+#define VIDEO_FB_0_ADDRESS_EXT_EXTERNAL               1
+#define VIDEO_FB_0_ADDRESS_ADDRESS                    25:0
+
+#define VIDEO_FB_WIDTH                                0x080048
+#define VIDEO_FB_WIDTH_WIDTH                          29:16
+#define VIDEO_FB_WIDTH_OFFSET                         13:0
+
+#define VIDEO_FB_0_LAST_ADDRESS                       0x08004C
+#define VIDEO_FB_0_LAST_ADDRESS_EXT                   27:27
+#define VIDEO_FB_0_LAST_ADDRESS_EXT_LOCAL             0
+#define VIDEO_FB_0_LAST_ADDRESS_EXT_EXTERNAL          1
+#define VIDEO_FB_0_LAST_ADDRESS_ADDRESS               25:0
+
+#define VIDEO_PLANE_TL                                0x080050
+#define VIDEO_PLANE_TL_TOP                            26:16
+#define VIDEO_PLANE_TL_LEFT                           10:0
+
+#define VIDEO_PLANE_BR                                0x080054
+#define VIDEO_PLANE_BR_BOTTOM                         26:16
+#define VIDEO_PLANE_BR_RIGHT                          10:0
+
+#define VIDEO_SCALE                                   0x080058
+#define VIDEO_SCALE_VERTICAL_MODE                     31:31
+#define VIDEO_SCALE_VERTICAL_MODE_EXPAND              0
+#define VIDEO_SCALE_VERTICAL_MODE_SHRINK              1
+#define VIDEO_SCALE_VERTICAL_SCALE                    27:16
+#define VIDEO_SCALE_HORIZONTAL_MODE                   15:15
+#define VIDEO_SCALE_HORIZONTAL_MODE_EXPAND            0
+#define VIDEO_SCALE_HORIZONTAL_MODE_SHRINK            1
+#define VIDEO_SCALE_HORIZONTAL_SCALE                  11:0
+
+#define VIDEO_INITIAL_SCALE                           0x08005C
+#define VIDEO_INITIAL_SCALE_FB_1                      27:16
+#define VIDEO_INITIAL_SCALE_FB_0                      11:0
+
+#define VIDEO_YUV_CONSTANTS                           0x080060
+#define VIDEO_YUV_CONSTANTS_Y                         31:24
+#define VIDEO_YUV_CONSTANTS_R                         23:16
+#define VIDEO_YUV_CONSTANTS_G                         15:8
+#define VIDEO_YUV_CONSTANTS_B                         7:0
+
+#define VIDEO_FB_1_ADDRESS                            0x080064
+#define VIDEO_FB_1_ADDRESS_STATUS                     31:31
+#define VIDEO_FB_1_ADDRESS_STATUS_CURRENT             0
+#define VIDEO_FB_1_ADDRESS_STATUS_PENDING             1
+#define VIDEO_FB_1_ADDRESS_EXT                        27:27
+#define VIDEO_FB_1_ADDRESS_EXT_LOCAL                  0
+#define VIDEO_FB_1_ADDRESS_EXT_EXTERNAL               1
+#define VIDEO_FB_1_ADDRESS_ADDRESS                    25:0
+
+#define VIDEO_FB_1_LAST_ADDRESS                       0x080068
+#define VIDEO_FB_1_LAST_ADDRESS_EXT                   27:27
+#define VIDEO_FB_1_LAST_ADDRESS_EXT_LOCAL             0
+#define VIDEO_FB_1_LAST_ADDRESS_EXT_EXTERNAL          1
+#define VIDEO_FB_1_LAST_ADDRESS_ADDRESS               25:0
+
+/* Video Alpha Control */
+
+#define VIDEO_ALPHA_DISPLAY_CTRL                        0x080080
+#define VIDEO_ALPHA_DISPLAY_CTRL_SELECT                 28:28
+#define VIDEO_ALPHA_DISPLAY_CTRL_SELECT_PER_PIXEL       0
+#define VIDEO_ALPHA_DISPLAY_CTRL_SELECT_ALPHA           1
+#define VIDEO_ALPHA_DISPLAY_CTRL_ALPHA                  27:24
+#define VIDEO_ALPHA_DISPLAY_CTRL_FIFO                   17:16
+#define VIDEO_ALPHA_DISPLAY_CTRL_FIFO_1                 0
+#define VIDEO_ALPHA_DISPLAY_CTRL_FIFO_3                 1
+#define VIDEO_ALPHA_DISPLAY_CTRL_FIFO_7                 2
+#define VIDEO_ALPHA_DISPLAY_CTRL_FIFO_11                3
+#define VIDEO_ALPHA_DISPLAY_CTRL_VERT_SCALE             11:11
+#define VIDEO_ALPHA_DISPLAY_CTRL_VERT_SCALE_NORMAL      0
+#define VIDEO_ALPHA_DISPLAY_CTRL_VERT_SCALE_HALF        1
+#define VIDEO_ALPHA_DISPLAY_CTRL_HORZ_SCALE             10:10
+#define VIDEO_ALPHA_DISPLAY_CTRL_HORZ_SCALE_NORMAL      0
+#define VIDEO_ALPHA_DISPLAY_CTRL_HORZ_SCALE_HALF        1
+#define VIDEO_ALPHA_DISPLAY_CTRL_VERT_MODE              9:9
+#define VIDEO_ALPHA_DISPLAY_CTRL_VERT_MODE_REPLICATE    0
+#define VIDEO_ALPHA_DISPLAY_CTRL_VERT_MODE_INTERPOLATE  1
+#define VIDEO_ALPHA_DISPLAY_CTRL_HORZ_MODE              8:8
+#define VIDEO_ALPHA_DISPLAY_CTRL_HORZ_MODE_REPLICATE    0
+#define VIDEO_ALPHA_DISPLAY_CTRL_HORZ_MODE_INTERPOLATE  1
+#define VIDEO_ALPHA_DISPLAY_CTRL_PIXEL                  7:4
+#define VIDEO_ALPHA_DISPLAY_CTRL_CHROMA_KEY             3:3
+#define VIDEO_ALPHA_DISPLAY_CTRL_CHROMA_KEY_DISABLE     0
+#define VIDEO_ALPHA_DISPLAY_CTRL_CHROMA_KEY_ENABLE      1
+#define VIDEO_ALPHA_DISPLAY_CTRL_PLANE                  2:2
+#define VIDEO_ALPHA_DISPLAY_CTRL_PLANE_DISABLE          0
+#define VIDEO_ALPHA_DISPLAY_CTRL_PLANE_ENABLE           1
+#define VIDEO_ALPHA_DISPLAY_CTRL_FORMAT                 1:0
+#define VIDEO_ALPHA_DISPLAY_CTRL_FORMAT_8               0
+#define VIDEO_ALPHA_DISPLAY_CTRL_FORMAT_16              1
+#define VIDEO_ALPHA_DISPLAY_CTRL_FORMAT_ALPHA_4_4       2
+#define VIDEO_ALPHA_DISPLAY_CTRL_FORMAT_ALPHA_4_4_4_4   3
+
+#define VIDEO_ALPHA_FB_ADDRESS                        0x080084
+#define VIDEO_ALPHA_FB_ADDRESS_STATUS                 31:31
+#define VIDEO_ALPHA_FB_ADDRESS_STATUS_CURRENT         0
+#define VIDEO_ALPHA_FB_ADDRESS_STATUS_PENDING         1
+#define VIDEO_ALPHA_FB_ADDRESS_EXT                    27:27
+#define VIDEO_ALPHA_FB_ADDRESS_EXT_LOCAL              0
+#define VIDEO_ALPHA_FB_ADDRESS_EXT_EXTERNAL           1
+#define VIDEO_ALPHA_FB_ADDRESS_ADDRESS                25:0
+
+#define VIDEO_ALPHA_FB_WIDTH                          0x080088
+#define VIDEO_ALPHA_FB_WIDTH_WIDTH                    29:16
+#define VIDEO_ALPHA_FB_WIDTH_OFFSET                   13:0
+
+#define VIDEO_ALPHA_FB_LAST_ADDRESS                   0x08008C
+#define VIDEO_ALPHA_FB_LAST_ADDRESS_EXT               27:27
+#define VIDEO_ALPHA_FB_LAST_ADDRESS_EXT_LOCAL         0
+#define VIDEO_ALPHA_FB_LAST_ADDRESS_EXT_EXTERNAL      1
+#define VIDEO_ALPHA_FB_LAST_ADDRESS_ADDRESS           25:0
+
+#define VIDEO_ALPHA_PLANE_TL                          0x080090
+#define VIDEO_ALPHA_PLANE_TL_TOP                      26:16
+#define VIDEO_ALPHA_PLANE_TL_LEFT                     10:0
+
+#define VIDEO_ALPHA_PLANE_BR                          0x080094
+#define VIDEO_ALPHA_PLANE_BR_BOTTOM                   26:16
+#define VIDEO_ALPHA_PLANE_BR_RIGHT                    10:0
+
+#define VIDEO_ALPHA_SCALE                             0x080098
+#define VIDEO_ALPHA_SCALE_VERTICAL_MODE               31:31
+#define VIDEO_ALPHA_SCALE_VERTICAL_MODE_EXPAND        0
+#define VIDEO_ALPHA_SCALE_VERTICAL_MODE_SHRINK        1
+#define VIDEO_ALPHA_SCALE_VERTICAL_SCALE              27:16
+#define VIDEO_ALPHA_SCALE_HORIZONTAL_MODE             15:15
+#define VIDEO_ALPHA_SCALE_HORIZONTAL_MODE_EXPAND      0
+#define VIDEO_ALPHA_SCALE_HORIZONTAL_MODE_SHRINK      1
+#define VIDEO_ALPHA_SCALE_HORIZONTAL_SCALE            11:0
+
+#define VIDEO_ALPHA_INITIAL_SCALE                     0x08009C
+#define VIDEO_ALPHA_INITIAL_SCALE_VERTICAL            27:16
+#define VIDEO_ALPHA_INITIAL_SCALE_HORIZONTAL          11:0
+
+#define VIDEO_ALPHA_CHROMA_KEY                        0x0800A0
+#define VIDEO_ALPHA_CHROMA_KEY_MASK                   31:16
+#define VIDEO_ALPHA_CHROMA_KEY_VALUE                  15:0
+
+#define VIDEO_ALPHA_COLOR_LOOKUP_01                   0x0800A4
+#define VIDEO_ALPHA_COLOR_LOOKUP_01_1                 31:16
+#define VIDEO_ALPHA_COLOR_LOOKUP_01_1_RED             31:27
+#define VIDEO_ALPHA_COLOR_LOOKUP_01_1_GREEN           26:21
+#define VIDEO_ALPHA_COLOR_LOOKUP_01_1_BLUE            20:16
+#define VIDEO_ALPHA_COLOR_LOOKUP_01_0                 15:0
+#define VIDEO_ALPHA_COLOR_LOOKUP_01_0_RED             15:11
+#define VIDEO_ALPHA_COLOR_LOOKUP_01_0_GREEN           10:5
+#define VIDEO_ALPHA_COLOR_LOOKUP_01_0_BLUE            4:0
+
+#define VIDEO_ALPHA_COLOR_LOOKUP_23                   0x0800A8
+#define VIDEO_ALPHA_COLOR_LOOKUP_23_3                 31:16
+#define VIDEO_ALPHA_COLOR_LOOKUP_23_3_RED             31:27
+#define VIDEO_ALPHA_COLOR_LOOKUP_23_3_GREEN           26:21
+#define VIDEO_ALPHA_COLOR_LOOKUP_23_3_BLUE            20:16
+#define VIDEO_ALPHA_COLOR_LOOKUP_23_2                 15:0
+#define VIDEO_ALPHA_COLOR_LOOKUP_23_2_RED             15:11
+#define VIDEO_ALPHA_COLOR_LOOKUP_23_2_GREEN           10:5
+#define VIDEO_ALPHA_COLOR_LOOKUP_23_2_BLUE            4:0
+
+#define VIDEO_ALPHA_COLOR_LOOKUP_45                   0x0800AC
+#define VIDEO_ALPHA_COLOR_LOOKUP_45_5                 31:16
+#define VIDEO_ALPHA_COLOR_LOOKUP_45_5_RED             31:27
+#define VIDEO_ALPHA_COLOR_LOOKUP_45_5_GREEN           26:21
+#define VIDEO_ALPHA_COLOR_LOOKUP_45_5_BLUE            20:16
+#define VIDEO_ALPHA_COLOR_LOOKUP_45_4                 15:0
+#define VIDEO_ALPHA_COLOR_LOOKUP_45_4_RED             15:11
+#define VIDEO_ALPHA_COLOR_LOOKUP_45_4_GREEN           10:5
+#define VIDEO_ALPHA_COLOR_LOOKUP_45_4_BLUE            4:0
+
+#define VIDEO_ALPHA_COLOR_LOOKUP_67                   0x0800B0
+#define VIDEO_ALPHA_COLOR_LOOKUP_67_7                 31:16
+#define VIDEO_ALPHA_COLOR_LOOKUP_67_7_RED             31:27
+#define VIDEO_ALPHA_COLOR_LOOKUP_67_7_GREEN           26:21
+#define VIDEO_ALPHA_COLOR_LOOKUP_67_7_BLUE            20:16
+#define VIDEO_ALPHA_COLOR_LOOKUP_67_6                 15:0
+#define VIDEO_ALPHA_COLOR_LOOKUP_67_6_RED             15:11
+#define VIDEO_ALPHA_COLOR_LOOKUP_67_6_GREEN           10:5
+#define VIDEO_ALPHA_COLOR_LOOKUP_67_6_BLUE            4:0
+
+#define VIDEO_ALPHA_COLOR_LOOKUP_89                   0x0800B4
+#define VIDEO_ALPHA_COLOR_LOOKUP_89_9                 31:16
+#define VIDEO_ALPHA_COLOR_LOOKUP_89_9_RED             31:27
+#define VIDEO_ALPHA_COLOR_LOOKUP_89_9_GREEN           26:21
+#define VIDEO_ALPHA_COLOR_LOOKUP_89_9_BLUE            20:16
+#define VIDEO_ALPHA_COLOR_LOOKUP_89_8                 15:0
+#define VIDEO_ALPHA_COLOR_LOOKUP_89_8_RED             15:11
+#define VIDEO_ALPHA_COLOR_LOOKUP_89_8_GREEN           10:5
+#define VIDEO_ALPHA_COLOR_LOOKUP_89_8_BLUE            4:0
+
+#define VIDEO_ALPHA_COLOR_LOOKUP_AB                   0x0800B8
+#define VIDEO_ALPHA_COLOR_LOOKUP_AB_B                 31:16
+#define VIDEO_ALPHA_COLOR_LOOKUP_AB_B_RED             31:27
+#define VIDEO_ALPHA_COLOR_LOOKUP_AB_B_GREEN           26:21
+#define VIDEO_ALPHA_COLOR_LOOKUP_AB_B_BLUE            20:16
+#define VIDEO_ALPHA_COLOR_LOOKUP_AB_A                 15:0
+#define VIDEO_ALPHA_COLOR_LOOKUP_AB_A_RED             15:11
+#define VIDEO_ALPHA_COLOR_LOOKUP_AB_A_GREEN           10:5
+#define VIDEO_ALPHA_COLOR_LOOKUP_AB_A_BLUE            4:0
+
+#define VIDEO_ALPHA_COLOR_LOOKUP_CD                   0x0800BC
+#define VIDEO_ALPHA_COLOR_LOOKUP_CD_D                 31:16
+#define VIDEO_ALPHA_COLOR_LOOKUP_CD_D_RED             31:27
+#define VIDEO_ALPHA_COLOR_LOOKUP_CD_D_GREEN           26:21
+#define VIDEO_ALPHA_COLOR_LOOKUP_CD_D_BLUE            20:16
+#define VIDEO_ALPHA_COLOR_LOOKUP_CD_C                 15:0
+#define VIDEO_ALPHA_COLOR_LOOKUP_CD_C_RED             15:11
+#define VIDEO_ALPHA_COLOR_LOOKUP_CD_C_GREEN           10:5
+#define VIDEO_ALPHA_COLOR_LOOKUP_CD_C_BLUE            4:0
+
+#define VIDEO_ALPHA_COLOR_LOOKUP_EF                   0x0800C0
+#define VIDEO_ALPHA_COLOR_LOOKUP_EF_F                 31:16
+#define VIDEO_ALPHA_COLOR_LOOKUP_EF_F_RED             31:27
+#define VIDEO_ALPHA_COLOR_LOOKUP_EF_F_GREEN           26:21
+#define VIDEO_ALPHA_COLOR_LOOKUP_EF_F_BLUE            20:16
+#define VIDEO_ALPHA_COLOR_LOOKUP_EF_E                 15:0
+#define VIDEO_ALPHA_COLOR_LOOKUP_EF_E_RED             15:11
+#define VIDEO_ALPHA_COLOR_LOOKUP_EF_E_GREEN           10:5
+#define VIDEO_ALPHA_COLOR_LOOKUP_EF_E_BLUE            4:0
+
+/* Panel Cursor Control */
+
+#define PANEL_HWC_ADDRESS                             0x0800F0
+#define PANEL_HWC_ADDRESS_ENABLE                      31:31
+#define PANEL_HWC_ADDRESS_ENABLE_DISABLE              0
+#define PANEL_HWC_ADDRESS_ENABLE_ENABLE               1
+#define PANEL_HWC_ADDRESS_EXT                         27:27
+#define PANEL_HWC_ADDRESS_EXT_LOCAL                   0
+#define PANEL_HWC_ADDRESS_EXT_EXTERNAL                1
+#define PANEL_HWC_ADDRESS_ADDRESS                     25:0
+
+#define PANEL_HWC_LOCATION                            0x0800F4
+#define PANEL_HWC_LOCATION_TOP                        27:27
+#define PANEL_HWC_LOCATION_TOP_INSIDE                 0
+#define PANEL_HWC_LOCATION_TOP_OUTSIDE                1
+#define PANEL_HWC_LOCATION_Y                          26:16
+#define PANEL_HWC_LOCATION_LEFT                       11:11
+#define PANEL_HWC_LOCATION_LEFT_INSIDE                0
+#define PANEL_HWC_LOCATION_LEFT_OUTSIDE               1
+#define PANEL_HWC_LOCATION_X                          10:0
+
+#define PANEL_HWC_COLOR_12                            0x0800F8
+#define PANEL_HWC_COLOR_12_2_RGB565                   31:16
+#define PANEL_HWC_COLOR_12_1_RGB565                   15:0
+
+#define PANEL_HWC_COLOR_3                             0x0800FC
+#define PANEL_HWC_COLOR_3_RGB565                      15:0
+
+/* Old Definitions +++ */
+#define PANEL_HWC_COLOR_01                            0x0800F8
+#define PANEL_HWC_COLOR_01_1_RED                      31:27
+#define PANEL_HWC_COLOR_01_1_GREEN                    26:21
+#define PANEL_HWC_COLOR_01_1_BLUE                     20:16
+#define PANEL_HWC_COLOR_01_0_RED                      15:11
+#define PANEL_HWC_COLOR_01_0_GREEN                    10:5
+#define PANEL_HWC_COLOR_01_0_BLUE                     4:0
+
+#define PANEL_HWC_COLOR_2                             0x0800FC
+#define PANEL_HWC_COLOR_2_RED                         15:11
+#define PANEL_HWC_COLOR_2_GREEN                       10:5
+#define PANEL_HWC_COLOR_2_BLUE                        4:0
+/* Old Definitions --- */
+
+/* Alpha Control */
+
+#define ALPHA_DISPLAY_CTRL                            0x080100
+#define ALPHA_DISPLAY_CTRL_SELECT                     28:28
+#define ALPHA_DISPLAY_CTRL_SELECT_PER_PIXEL           0
+#define ALPHA_DISPLAY_CTRL_SELECT_ALPHA               1
+#define ALPHA_DISPLAY_CTRL_ALPHA                      27:24
+#define ALPHA_DISPLAY_CTRL_FIFO                       17:16
+#define ALPHA_DISPLAY_CTRL_FIFO_1                     0
+#define ALPHA_DISPLAY_CTRL_FIFO_3                     1
+#define ALPHA_DISPLAY_CTRL_FIFO_7                     2
+#define ALPHA_DISPLAY_CTRL_FIFO_11                    3
+#define ALPHA_DISPLAY_CTRL_PIXEL                      7:4
+#define ALPHA_DISPLAY_CTRL_CHROMA_KEY                 3:3
+#define ALPHA_DISPLAY_CTRL_CHROMA_KEY_DISABLE         0
+#define ALPHA_DISPLAY_CTRL_CHROMA_KEY_ENABLE          1
+#define ALPHA_DISPLAY_CTRL_PLANE                      2:2
+#define ALPHA_DISPLAY_CTRL_PLANE_DISABLE              0
+#define ALPHA_DISPLAY_CTRL_PLANE_ENABLE               1
+#define ALPHA_DISPLAY_CTRL_FORMAT                     1:0
+#define ALPHA_DISPLAY_CTRL_FORMAT_16                  1
+#define ALPHA_DISPLAY_CTRL_FORMAT_ALPHA_4_4           2
+#define ALPHA_DISPLAY_CTRL_FORMAT_ALPHA_4_4_4_4       3
+
+#define ALPHA_FB_ADDRESS                              0x080104
+#define ALPHA_FB_ADDRESS_STATUS                       31:31
+#define ALPHA_FB_ADDRESS_STATUS_CURRENT               0
+#define ALPHA_FB_ADDRESS_STATUS_PENDING               1
+#define ALPHA_FB_ADDRESS_EXT                          27:27
+#define ALPHA_FB_ADDRESS_EXT_LOCAL                    0
+#define ALPHA_FB_ADDRESS_EXT_EXTERNAL                 1
+#define ALPHA_FB_ADDRESS_ADDRESS                      25:0
+
+#define ALPHA_FB_WIDTH                                0x080108
+#define ALPHA_FB_WIDTH_WIDTH                          29:16
+#define ALPHA_FB_WIDTH_OFFSET                         13:0
+
+#define ALPHA_PLANE_TL                                0x08010C
+#define ALPHA_PLANE_TL_TOP                            26:16
+#define ALPHA_PLANE_TL_LEFT                           10:0
+
+#define ALPHA_PLANE_BR                                0x080110
+#define ALPHA_PLANE_BR_BOTTOM                         26:16
+#define ALPHA_PLANE_BR_RIGHT                          10:0
+
+#define ALPHA_CHROMA_KEY                              0x080114
+#define ALPHA_CHROMA_KEY_MASK                         31:16
+#define ALPHA_CHROMA_KEY_VALUE                        15:0
+
+#define ALPHA_COLOR_LOOKUP_01                         0x080118
+#define ALPHA_COLOR_LOOKUP_01_1                       31:16
+#define ALPHA_COLOR_LOOKUP_01_1_RED                   31:27
+#define ALPHA_COLOR_LOOKUP_01_1_GREEN                 26:21
+#define ALPHA_COLOR_LOOKUP_01_1_BLUE                  20:16
+#define ALPHA_COLOR_LOOKUP_01_0                       15:0
+#define ALPHA_COLOR_LOOKUP_01_0_RED                   15:11
+#define ALPHA_COLOR_LOOKUP_01_0_GREEN                 10:5
+#define ALPHA_COLOR_LOOKUP_01_0_BLUE                  4:0
+
+#define ALPHA_COLOR_LOOKUP_23                         0x08011C
+#define ALPHA_COLOR_LOOKUP_23_3                       31:16
+#define ALPHA_COLOR_LOOKUP_23_3_RED                   31:27
+#define ALPHA_COLOR_LOOKUP_23_3_GREEN                 26:21
+#define ALPHA_COLOR_LOOKUP_23_3_BLUE                  20:16
+#define ALPHA_COLOR_LOOKUP_23_2                       15:0
+#define ALPHA_COLOR_LOOKUP_23_2_RED                   15:11
+#define ALPHA_COLOR_LOOKUP_23_2_GREEN                 10:5
+#define ALPHA_COLOR_LOOKUP_23_2_BLUE                  4:0
+
+#define ALPHA_COLOR_LOOKUP_45                         0x080120
+#define ALPHA_COLOR_LOOKUP_45_5                       31:16
+#define ALPHA_COLOR_LOOKUP_45_5_RED                   31:27
+#define ALPHA_COLOR_LOOKUP_45_5_GREEN                 26:21
+#define ALPHA_COLOR_LOOKUP_45_5_BLUE                  20:16
+#define ALPHA_COLOR_LOOKUP_45_4                       15:0
+#define ALPHA_COLOR_LOOKUP_45_4_RED                   15:11
+#define ALPHA_COLOR_LOOKUP_45_4_GREEN                 10:5
+#define ALPHA_COLOR_LOOKUP_45_4_BLUE                  4:0
+
+#define ALPHA_COLOR_LOOKUP_67                         0x080124
+#define ALPHA_COLOR_LOOKUP_67_7                       31:16
+#define ALPHA_COLOR_LOOKUP_67_7_RED                   31:27
+#define ALPHA_COLOR_LOOKUP_67_7_GREEN                 26:21
+#define ALPHA_COLOR_LOOKUP_67_7_BLUE                  20:16
+#define ALPHA_COLOR_LOOKUP_67_6                       15:0
+#define ALPHA_COLOR_LOOKUP_67_6_RED                   15:11
+#define ALPHA_COLOR_LOOKUP_67_6_GREEN                 10:5
+#define ALPHA_COLOR_LOOKUP_67_6_BLUE                  4:0
+
+#define ALPHA_COLOR_LOOKUP_89                         0x080128
+#define ALPHA_COLOR_LOOKUP_89_9                       31:16
+#define ALPHA_COLOR_LOOKUP_89_9_RED                   31:27
+#define ALPHA_COLOR_LOOKUP_89_9_GREEN                 26:21
+#define ALPHA_COLOR_LOOKUP_89_9_BLUE                  20:16
+#define ALPHA_COLOR_LOOKUP_89_8                       15:0
+#define ALPHA_COLOR_LOOKUP_89_8_RED                   15:11
+#define ALPHA_COLOR_LOOKUP_89_8_GREEN                 10:5
+#define ALPHA_COLOR_LOOKUP_89_8_BLUE                  4:0
+
+#define ALPHA_COLOR_LOOKUP_AB                         0x08012C
+#define ALPHA_COLOR_LOOKUP_AB_B                       31:16
+#define ALPHA_COLOR_LOOKUP_AB_B_RED                   31:27
+#define ALPHA_COLOR_LOOKUP_AB_B_GREEN                 26:21
+#define ALPHA_COLOR_LOOKUP_AB_B_BLUE                  20:16
+#define ALPHA_COLOR_LOOKUP_AB_A                       15:0
+#define ALPHA_COLOR_LOOKUP_AB_A_RED                   15:11
+#define ALPHA_COLOR_LOOKUP_AB_A_GREEN                 10:5
+#define ALPHA_COLOR_LOOKUP_AB_A_BLUE                  4:0
+
+#define ALPHA_COLOR_LOOKUP_CD                         0x080130
+#define ALPHA_COLOR_LOOKUP_CD_D                       31:16
+#define ALPHA_COLOR_LOOKUP_CD_D_RED                   31:27
+#define ALPHA_COLOR_LOOKUP_CD_D_GREEN                 26:21
+#define ALPHA_COLOR_LOOKUP_CD_D_BLUE                  20:16
+#define ALPHA_COLOR_LOOKUP_CD_C                       15:0
+#define ALPHA_COLOR_LOOKUP_CD_C_RED                   15:11
+#define ALPHA_COLOR_LOOKUP_CD_C_GREEN                 10:5
+#define ALPHA_COLOR_LOOKUP_CD_C_BLUE                  4:0
+
+#define ALPHA_COLOR_LOOKUP_EF                         0x080134
+#define ALPHA_COLOR_LOOKUP_EF_F                       31:16
+#define ALPHA_COLOR_LOOKUP_EF_F_RED                   31:27
+#define ALPHA_COLOR_LOOKUP_EF_F_GREEN                 26:21
+#define ALPHA_COLOR_LOOKUP_EF_F_BLUE                  20:16
+#define ALPHA_COLOR_LOOKUP_EF_E                       15:0
+#define ALPHA_COLOR_LOOKUP_EF_E_RED                   15:11
+#define ALPHA_COLOR_LOOKUP_EF_E_GREEN                 10:5
+#define ALPHA_COLOR_LOOKUP_EF_E_BLUE                  4:0
+
+/* CRT Graphics Control */
+
+#define CRT_DISPLAY_CTRL                              0x080200
+#define CRT_DISPLAY_CTRL_RESERVED_1_MASK  			  31:27
+#define CRT_DISPLAY_CTRL_RESERVED_1_MASK_DISABLE  			  0
+#define CRT_DISPLAY_CTRL_RESERVED_1_MASK_ENABLE 			  0x1F
+
+/* SM750LE definition */
+#define CRT_DISPLAY_CTRL_DPMS                         31:30
+#define CRT_DISPLAY_CTRL_DPMS_0                       0
+#define CRT_DISPLAY_CTRL_DPMS_1                       1
+#define CRT_DISPLAY_CTRL_DPMS_2                       2
+#define CRT_DISPLAY_CTRL_DPMS_3                       3
+#define CRT_DISPLAY_CTRL_CLK                          29:27
+#define CRT_DISPLAY_CTRL_CLK_PLL25                    0
+#define CRT_DISPLAY_CTRL_CLK_PLL41                    1
+#define CRT_DISPLAY_CTRL_CLK_PLL62                    2
+#define CRT_DISPLAY_CTRL_CLK_PLL65                    3
+#define CRT_DISPLAY_CTRL_CLK_PLL74                    4
+#define CRT_DISPLAY_CTRL_CLK_PLL80                    5
+#define CRT_DISPLAY_CTRL_CLK_PLL108                   6
+#define CRT_DISPLAY_CTRL_CLK_RESERVED                 7
+#define CRT_DISPLAY_CTRL_SHIFT_VGA_DAC                26:26
+#define CRT_DISPLAY_CTRL_SHIFT_VGA_DAC_DISABLE        1
+#define CRT_DISPLAY_CTRL_SHIFT_VGA_DAC_ENABLE         0
+
+
+#define CRT_DISPLAY_CTRL_RESERVED_2_MASK 			  25:24
+#define CRT_DISPLAY_CTRL_RESERVED_2_MASK_ENABLE 			  3
+#define CRT_DISPLAY_CTRL_RESERVED_2_MASK_DISABLE 			  0
+
+/* SM750LE definition */
+#define CRT_DISPLAY_CTRL_CRTSELECT                    25:25
+#define CRT_DISPLAY_CTRL_CRTSELECT_VGA                0
+#define CRT_DISPLAY_CTRL_CRTSELECT_CRT                1
+#define CRT_DISPLAY_CTRL_RGBBIT                       24:24
+#define CRT_DISPLAY_CTRL_RGBBIT_24BIT                 0
+#define CRT_DISPLAY_CTRL_RGBBIT_12BIT                 1
+
+
+#define CRT_DISPLAY_CTRL_RESERVED_3_MASK 			  15:15
+#define CRT_DISPLAY_CTRL_RESERVED_3_MASK_DISABLE      0
+#define CRT_DISPLAY_CTRL_RESERVED_3_MASK_ENABLE       1
+
+#define CRT_DISPLAY_CTRL_RESERVED_4_MASK 			  9:9
+#define CRT_DISPLAY_CTRL_RESERVED_4_MASK_DISABLE      0
+#define CRT_DISPLAY_CTRL_RESERVED_4_MASK_ENABLE       1
+
+#ifndef VALIDATION_CHIP
+    #define CRT_DISPLAY_CTRL_SHIFT_VGA_DAC            26:26
+    #define CRT_DISPLAY_CTRL_SHIFT_VGA_DAC_DISABLE    1
+    #define CRT_DISPLAY_CTRL_SHIFT_VGA_DAC_ENABLE     0
+    #define CRT_DISPLAY_CTRL_CENTERING                24:24
+    #define CRT_DISPLAY_CTRL_CENTERING_DISABLE        0
+    #define CRT_DISPLAY_CTRL_CENTERING_ENABLE         1
+#endif
+#define CRT_DISPLAY_CTRL_LOCK_TIMING                  23:23
+#define CRT_DISPLAY_CTRL_LOCK_TIMING_DISABLE          0
+#define CRT_DISPLAY_CTRL_LOCK_TIMING_ENABLE           1
+#define CRT_DISPLAY_CTRL_EXPANSION                    22:22
+#define CRT_DISPLAY_CTRL_EXPANSION_DISABLE            0
+#define CRT_DISPLAY_CTRL_EXPANSION_ENABLE             1
+#define CRT_DISPLAY_CTRL_VERTICAL_MODE                21:21
+#define CRT_DISPLAY_CTRL_VERTICAL_MODE_REPLICATE      0
+#define CRT_DISPLAY_CTRL_VERTICAL_MODE_INTERPOLATE    1
+#define CRT_DISPLAY_CTRL_HORIZONTAL_MODE              20:20
+#define CRT_DISPLAY_CTRL_HORIZONTAL_MODE_REPLICATE    0
+#define CRT_DISPLAY_CTRL_HORIZONTAL_MODE_INTERPOLATE  1
+#define CRT_DISPLAY_CTRL_SELECT                       19:18
+#define CRT_DISPLAY_CTRL_SELECT_PANEL                 0
+#define CRT_DISPLAY_CTRL_SELECT_VGA                   1
+#define CRT_DISPLAY_CTRL_SELECT_CRT                   2
+#define CRT_DISPLAY_CTRL_FIFO                         17:16
+#define CRT_DISPLAY_CTRL_FIFO_1                       0
+#define CRT_DISPLAY_CTRL_FIFO_3                       1
+#define CRT_DISPLAY_CTRL_FIFO_7                       2
+#define CRT_DISPLAY_CTRL_FIFO_11                      3
+#define CRT_DISPLAY_CTRL_CLOCK_PHASE                  14:14
+#define CRT_DISPLAY_CTRL_CLOCK_PHASE_ACTIVE_HIGH      0
+#define CRT_DISPLAY_CTRL_CLOCK_PHASE_ACTIVE_LOW       1
+#define CRT_DISPLAY_CTRL_VSYNC_PHASE                  13:13
+#define CRT_DISPLAY_CTRL_VSYNC_PHASE_ACTIVE_HIGH      0
+#define CRT_DISPLAY_CTRL_VSYNC_PHASE_ACTIVE_LOW       1
+#define CRT_DISPLAY_CTRL_HSYNC_PHASE                  12:12
+#define CRT_DISPLAY_CTRL_HSYNC_PHASE_ACTIVE_HIGH      0
+#define CRT_DISPLAY_CTRL_HSYNC_PHASE_ACTIVE_LOW       1
+#define CRT_DISPLAY_CTRL_BLANK                        10:10
+#define CRT_DISPLAY_CTRL_BLANK_OFF                    0
+#define CRT_DISPLAY_CTRL_BLANK_ON                     1
+#define CRT_DISPLAY_CTRL_TIMING                       8:8
+#define CRT_DISPLAY_CTRL_TIMING_DISABLE               0
+#define CRT_DISPLAY_CTRL_TIMING_ENABLE                1
+#define CRT_DISPLAY_CTRL_PIXEL                        7:4
+#define CRT_DISPLAY_CTRL_GAMMA                        3:3
+#define CRT_DISPLAY_CTRL_GAMMA_DISABLE                0
+#define CRT_DISPLAY_CTRL_GAMMA_ENABLE                 1
+#define CRT_DISPLAY_CTRL_PLANE                        2:2
+#define CRT_DISPLAY_CTRL_PLANE_DISABLE                0
+#define CRT_DISPLAY_CTRL_PLANE_ENABLE                 1
+#define CRT_DISPLAY_CTRL_FORMAT                       1:0
+#define CRT_DISPLAY_CTRL_FORMAT_8                     0
+#define CRT_DISPLAY_CTRL_FORMAT_16                    1
+#define CRT_DISPLAY_CTRL_FORMAT_32                    2
+#define CRT_DISPLAY_CTRL_RESERVED_BITS_MASK           0xFF000200
+
+#define CRT_FB_ADDRESS                                0x080204
+#define CRT_FB_ADDRESS_STATUS                         31:31
+#define CRT_FB_ADDRESS_STATUS_CURRENT                 0
+#define CRT_FB_ADDRESS_STATUS_PENDING                 1
+#define CRT_FB_ADDRESS_EXT                            27:27
+#define CRT_FB_ADDRESS_EXT_LOCAL                      0
+#define CRT_FB_ADDRESS_EXT_EXTERNAL                   1
+#define CRT_FB_ADDRESS_ADDRESS                        25:0
+
+#define CRT_FB_WIDTH                                  0x080208
+#define CRT_FB_WIDTH_WIDTH                            29:16
+#define CRT_FB_WIDTH_OFFSET                           13:0
+
+#define CRT_HORIZONTAL_TOTAL                          0x08020C
+#define CRT_HORIZONTAL_TOTAL_TOTAL                    27:16
+#define CRT_HORIZONTAL_TOTAL_DISPLAY_END              11:0
+
+#define CRT_HORIZONTAL_SYNC                           0x080210
+#define CRT_HORIZONTAL_SYNC_WIDTH                     23:16
+#define CRT_HORIZONTAL_SYNC_START                     11:0
+
+#define CRT_VERTICAL_TOTAL                            0x080214
+#define CRT_VERTICAL_TOTAL_TOTAL                      26:16
+#define CRT_VERTICAL_TOTAL_DISPLAY_END                10:0
+
+#define CRT_VERTICAL_SYNC                             0x080218
+#define CRT_VERTICAL_SYNC_HEIGHT                      21:16
+#define CRT_VERTICAL_SYNC_START                       10:0
+
+#define CRT_SIGNATURE_ANALYZER                        0x08021C
+#define CRT_SIGNATURE_ANALYZER_STATUS                 31:16
+#define CRT_SIGNATURE_ANALYZER_ENABLE                 3:3
+#define CRT_SIGNATURE_ANALYZER_ENABLE_DISABLE         0
+#define CRT_SIGNATURE_ANALYZER_ENABLE_ENABLE          1
+#define CRT_SIGNATURE_ANALYZER_RESET                  2:2
+#define CRT_SIGNATURE_ANALYZER_RESET_NORMAL           0
+#define CRT_SIGNATURE_ANALYZER_RESET_RESET            1
+#define CRT_SIGNATURE_ANALYZER_SOURCE                 1:0
+#define CRT_SIGNATURE_ANALYZER_SOURCE_RED             0
+#define CRT_SIGNATURE_ANALYZER_SOURCE_GREEN           1
+#define CRT_SIGNATURE_ANALYZER_SOURCE_BLUE            2
+
+#define CRT_CURRENT_LINE                              0x080220
+#define CRT_CURRENT_LINE_LINE                         10:0
+
+#define CRT_MONITOR_DETECT                            0x080224
+#define CRT_MONITOR_DETECT_VALUE                      25:25
+#define CRT_MONITOR_DETECT_VALUE_DISABLE              0
+#define CRT_MONITOR_DETECT_VALUE_ENABLE               1
+#define CRT_MONITOR_DETECT_ENABLE                     24:24
+#define CRT_MONITOR_DETECT_ENABLE_DISABLE             0
+#define CRT_MONITOR_DETECT_ENABLE_ENABLE              1
+#define CRT_MONITOR_DETECT_RED                        23:16
+#define CRT_MONITOR_DETECT_GREEN                      15:8
+#define CRT_MONITOR_DETECT_BLUE                       7:0
+
+#define CRT_SCALE                                     0x080228
+#define CRT_SCALE_VERTICAL_MODE                       31:31
+#define CRT_SCALE_VERTICAL_MODE_EXPAND                0
+#define CRT_SCALE_VERTICAL_MODE_SHRINK                1
+#define CRT_SCALE_VERTICAL_SCALE                      27:16
+#define CRT_SCALE_HORIZONTAL_MODE                     15:15
+#define CRT_SCALE_HORIZONTAL_MODE_EXPAND              0
+#define CRT_SCALE_HORIZONTAL_MODE_SHRINK              1
+#define CRT_SCALE_HORIZONTAL_SCALE                    11:0
+
+/* CRT Cursor Control */
+
+#define CRT_HWC_ADDRESS                               0x080230
+#define CRT_HWC_ADDRESS_ENABLE                        31:31
+#define CRT_HWC_ADDRESS_ENABLE_DISABLE                0
+#define CRT_HWC_ADDRESS_ENABLE_ENABLE                 1
+#define CRT_HWC_ADDRESS_EXT                           27:27
+#define CRT_HWC_ADDRESS_EXT_LOCAL                     0
+#define CRT_HWC_ADDRESS_EXT_EXTERNAL                  1
+#define CRT_HWC_ADDRESS_ADDRESS                       25:0
+
+#define CRT_HWC_LOCATION                              0x080234
+#define CRT_HWC_LOCATION_TOP                          27:27
+#define CRT_HWC_LOCATION_TOP_INSIDE                   0
+#define CRT_HWC_LOCATION_TOP_OUTSIDE                  1
+#define CRT_HWC_LOCATION_Y                            26:16
+#define CRT_HWC_LOCATION_LEFT                         11:11
+#define CRT_HWC_LOCATION_LEFT_INSIDE                  0
+#define CRT_HWC_LOCATION_LEFT_OUTSIDE                 1
+#define CRT_HWC_LOCATION_X                            10:0
+
+#define CRT_HWC_COLOR_12                              0x080238
+#define CRT_HWC_COLOR_12_2_RGB565                     31:16
+#define CRT_HWC_COLOR_12_1_RGB565                     15:0
+
+#define CRT_HWC_COLOR_3                               0x08023C
+#define CRT_HWC_COLOR_3_RGB565                        15:0
+
+/* Old Definitions +++. Need to be removed if no application use it. */
+#if 0
+    #define CRT_HWC_COLOR_01                          0x080238
+    #define CRT_HWC_COLOR_01_1_RED                    31:27
+    #define CRT_HWC_COLOR_01_1_GREEN                  26:21
+    #define CRT_HWC_COLOR_01_1_BLUE                   20:16
+    #define CRT_HWC_COLOR_01_0_RED                    15:11
+    #define CRT_HWC_COLOR_01_0_GREEN                  10:5
+    #define CRT_HWC_COLOR_01_0_BLUE                   4:0
+
+    #define CRT_HWC_COLOR_2                           0x08023C
+    #define CRT_HWC_COLOR_2_RED                       15:11
+    #define CRT_HWC_COLOR_2_GREEN                     10:5
+    #define CRT_HWC_COLOR_2_BLUE                      4:0
+#endif
+/* Old Definitions --- */
+
+/* This vertical expansion below start at 0x080240 ~ 0x080264 */
+#define CRT_VERTICAL_EXPANSION                        0x080240
+#ifndef VALIDATION_CHIP
+    #define CRT_VERTICAL_CENTERING_VALUE              31:24
+#endif
+#define CRT_VERTICAL_EXPANSION_COMPARE_VALUE          23:16
+#define CRT_VERTICAL_EXPANSION_LINE_BUFFER            15:12
+#define CRT_VERTICAL_EXPANSION_SCALE_FACTOR           11:0
+
+/* This horizontal expansion below start at 0x080268 ~ 0x08027C */
+#define CRT_HORIZONTAL_EXPANSION                      0x080268
+#ifndef VALIDATION_CHIP
+    #define CRT_HORIZONTAL_CENTERING_VALUE            31:24
+#endif
+#define CRT_HORIZONTAL_EXPANSION_COMPARE_VALUE        23:16
+#define CRT_HORIZONTAL_EXPANSION_SCALE_FACTOR         11:0
+
+#ifndef VALIDATION_CHIP
+    /* Auto Centering */
+    #define CRT_AUTO_CENTERING_TL                     0x080280
+    #define CRT_AUTO_CENTERING_TL_TOP                 26:16
+    #define CRT_AUTO_CENTERING_TL_LEFT                10:0
+
+    #define CRT_AUTO_CENTERING_BR                     0x080284
+    #define CRT_AUTO_CENTERING_BR_BOTTOM              26:16
+    #define CRT_AUTO_CENTERING_BR_RIGHT               10:0
+#endif
+
+/* sm750le new register to control panel output */
+#define DISPLAY_CONTROL_750LE 	0x80288
+/* Palette RAM */
+
+/* Panel Pallete register starts at 0x080400 ~ 0x0807FC */
+#define PANEL_PALETTE_RAM                             0x080400
+
+/* Panel Pallete register starts at 0x080C00 ~ 0x080FFC */
+#define CRT_PALETTE_RAM                               0x080C00
+
+/* 2D registers
+ * move their defination into general lynx_accel.h file
+ * because all smi graphic chip share the same drawing engine
+ * register format */
+#if 0
+#define DE_SOURCE                                       0x100000
+#define DE_SOURCE_WRAP                                  31:31
+#define DE_SOURCE_WRAP_DISABLE                          0
+#define DE_SOURCE_WRAP_ENABLE                           1
+
+/*
+ * The following definitions are used in different setting
+ */
+
+/* Use these definitions in XY addressing mode or linear addressing mode. */
+#define DE_SOURCE_X_K1                                  27:16
+#define DE_SOURCE_Y_K2                                  11:0
+
+/* Use this definition in host write mode for mono. The Y_K2 is not used
+   in host write mode. */
+#define DE_SOURCE_X_K1_MONO                             20:16
+
+/* Use these definitions in Bresenham line drawing mode. */
+#define DE_SOURCE_X_K1_LINE                             29:16
+#define DE_SOURCE_Y_K2_LINE                             13:0
+
+#define DE_DESTINATION                                  0x100004
+#define DE_DESTINATION_WRAP                             31:31
+#define DE_DESTINATION_WRAP_DISABLE                     0
+#define DE_DESTINATION_WRAP_ENABLE                      1
+#if 1
+    #define DE_DESTINATION_X                            27:16
+    #define DE_DESTINATION_Y                            11:0
+#else
+    #define DE_DESTINATION_X                            28:16
+    #define DE_DESTINATION_Y                            15:0
+#endif
+
+#define DE_DIMENSION                                    0x100008
+#define DE_DIMENSION_X                                  28:16
+#define DE_DIMENSION_Y_ET                               15:0
+
+#define DE_CONTROL                                      0x10000C
+#define DE_CONTROL_STATUS                               31:31
+#define DE_CONTROL_STATUS_STOP                          0
+#define DE_CONTROL_STATUS_START                         1
+#define DE_CONTROL_PATTERN                              30:30
+#define DE_CONTROL_PATTERN_MONO                         0
+#define DE_CONTROL_PATTERN_COLOR                        1
+#define DE_CONTROL_UPDATE_DESTINATION_X                 29:29
+#define DE_CONTROL_UPDATE_DESTINATION_X_DISABLE         0
+#define DE_CONTROL_UPDATE_DESTINATION_X_ENABLE          1
+#define DE_CONTROL_QUICK_START                          28:28
+#define DE_CONTROL_QUICK_START_DISABLE                  0
+#define DE_CONTROL_QUICK_START_ENABLE                   1
+#define DE_CONTROL_DIRECTION                            27:27
+#define DE_CONTROL_DIRECTION_LEFT_TO_RIGHT              0
+#define DE_CONTROL_DIRECTION_RIGHT_TO_LEFT              1
+#define DE_CONTROL_MAJOR                                26:26
+#define DE_CONTROL_MAJOR_X                              0
+#define DE_CONTROL_MAJOR_Y                              1
+#define DE_CONTROL_STEP_X                               25:25
+#define DE_CONTROL_STEP_X_POSITIVE                      0
+#define DE_CONTROL_STEP_X_NEGATIVE                      1
+#define DE_CONTROL_STEP_Y                               24:24
+#define DE_CONTROL_STEP_Y_POSITIVE                      0
+#define DE_CONTROL_STEP_Y_NEGATIVE                      1
+#define DE_CONTROL_STRETCH                              23:23
+#define DE_CONTROL_STRETCH_DISABLE                      0
+#define DE_CONTROL_STRETCH_ENABLE                       1
+#define DE_CONTROL_HOST                                 22:22
+#define DE_CONTROL_HOST_COLOR                           0
+#define DE_CONTROL_HOST_MONO                            1
+#define DE_CONTROL_LAST_PIXEL                           21:21
+#define DE_CONTROL_LAST_PIXEL_OFF                       0
+#define DE_CONTROL_LAST_PIXEL_ON                        1
+#define DE_CONTROL_COMMAND                              20:16
+#define DE_CONTROL_COMMAND_BITBLT                       0
+#define DE_CONTROL_COMMAND_RECTANGLE_FILL               1
+#define DE_CONTROL_COMMAND_DE_TILE                      2
+#define DE_CONTROL_COMMAND_TRAPEZOID_FILL               3
+#define DE_CONTROL_COMMAND_ALPHA_BLEND                  4
+#define DE_CONTROL_COMMAND_RLE_STRIP                    5
+#define DE_CONTROL_COMMAND_SHORT_STROKE                 6
+#define DE_CONTROL_COMMAND_LINE_DRAW                    7
+#define DE_CONTROL_COMMAND_HOST_WRITE                   8
+#define DE_CONTROL_COMMAND_HOST_READ                    9
+#define DE_CONTROL_COMMAND_HOST_WRITE_BOTTOM_UP         10
+#define DE_CONTROL_COMMAND_ROTATE                       11
+#define DE_CONTROL_COMMAND_FONT                         12
+#define DE_CONTROL_COMMAND_TEXTURE_LOAD                 15
+#define DE_CONTROL_ROP_SELECT                           15:15
+#define DE_CONTROL_ROP_SELECT_ROP3                      0
+#define DE_CONTROL_ROP_SELECT_ROP2                      1
+#define DE_CONTROL_ROP2_SOURCE                          14:14
+#define DE_CONTROL_ROP2_SOURCE_BITMAP                   0
+#define DE_CONTROL_ROP2_SOURCE_PATTERN                  1
+#define DE_CONTROL_MONO_DATA                            13:12
+#define DE_CONTROL_MONO_DATA_NOT_PACKED                 0
+#define DE_CONTROL_MONO_DATA_8_PACKED                   1
+#define DE_CONTROL_MONO_DATA_16_PACKED                  2
+#define DE_CONTROL_MONO_DATA_32_PACKED                  3
+#define DE_CONTROL_REPEAT_ROTATE                        11:11
+#define DE_CONTROL_REPEAT_ROTATE_DISABLE                0
+#define DE_CONTROL_REPEAT_ROTATE_ENABLE                 1
+#define DE_CONTROL_TRANSPARENCY_MATCH                   10:10
+#define DE_CONTROL_TRANSPARENCY_MATCH_OPAQUE            0
+#define DE_CONTROL_TRANSPARENCY_MATCH_TRANSPARENT       1
+#define DE_CONTROL_TRANSPARENCY_SELECT                  9:9
+#define DE_CONTROL_TRANSPARENCY_SELECT_SOURCE           0
+#define DE_CONTROL_TRANSPARENCY_SELECT_DESTINATION      1
+#define DE_CONTROL_TRANSPARENCY                         8:8
+#define DE_CONTROL_TRANSPARENCY_DISABLE                 0
+#define DE_CONTROL_TRANSPARENCY_ENABLE                  1
+#define DE_CONTROL_ROP                                  7:0
+
+/* Pseudo fields. */
+
+#define DE_CONTROL_SHORT_STROKE_DIR                     27:24
+#define DE_CONTROL_SHORT_STROKE_DIR_225                 0
+#define DE_CONTROL_SHORT_STROKE_DIR_135                 1
+#define DE_CONTROL_SHORT_STROKE_DIR_315                 2
+#define DE_CONTROL_SHORT_STROKE_DIR_45                  3
+#define DE_CONTROL_SHORT_STROKE_DIR_270                 4
+#define DE_CONTROL_SHORT_STROKE_DIR_90                  5
+#define DE_CONTROL_SHORT_STROKE_DIR_180                 8
+#define DE_CONTROL_SHORT_STROKE_DIR_0                   10
+#define DE_CONTROL_ROTATION                             25:24
+#define DE_CONTROL_ROTATION_0                           0
+#define DE_CONTROL_ROTATION_270                         1
+#define DE_CONTROL_ROTATION_90                          2
+#define DE_CONTROL_ROTATION_180                         3
+
+#define DE_PITCH                                        0x100010
+#define DE_PITCH_DESTINATION                            28:16
+#define DE_PITCH_SOURCE                                 12:0
+
+#define DE_FOREGROUND                                   0x100014
+#define DE_FOREGROUND_COLOR                             31:0
+
+#define DE_BACKGROUND                                   0x100018
+#define DE_BACKGROUND_COLOR                             31:0
+
+#define DE_STRETCH_FORMAT                               0x10001C
+#define DE_STRETCH_FORMAT_PATTERN_XY                    30:30
+#define DE_STRETCH_FORMAT_PATTERN_XY_NORMAL             0
+#define DE_STRETCH_FORMAT_PATTERN_XY_OVERWRITE          1
+#define DE_STRETCH_FORMAT_PATTERN_Y                     29:27
+#define DE_STRETCH_FORMAT_PATTERN_X                     25:23
+#define DE_STRETCH_FORMAT_PIXEL_FORMAT                  21:20
+#define DE_STRETCH_FORMAT_PIXEL_FORMAT_8                0
+#define DE_STRETCH_FORMAT_PIXEL_FORMAT_16               1
+#define DE_STRETCH_FORMAT_PIXEL_FORMAT_32               2
+#define DE_STRETCH_FORMAT_ADDRESSING                    19:16
+#define DE_STRETCH_FORMAT_ADDRESSING_XY                 0
+#define DE_STRETCH_FORMAT_ADDRESSING_LINEAR             15
+#define DE_STRETCH_FORMAT_SOURCE_HEIGHT                 11:0
+
+#define DE_COLOR_COMPARE                                0x100020
+#define DE_COLOR_COMPARE_COLOR                          23:0
+
+#define DE_COLOR_COMPARE_MASK                           0x100024
+#define DE_COLOR_COMPARE_MASK_MASKS                     23:0
+
+#define DE_MASKS                                        0x100028
+#define DE_MASKS_BYTE_MASK                              31:16
+#define DE_MASKS_BIT_MASK                               15:0
+
+#define DE_CLIP_TL                                      0x10002C
+#define DE_CLIP_TL_TOP                                  31:16
+#define DE_CLIP_TL_STATUS                               13:13
+#define DE_CLIP_TL_STATUS_DISABLE                       0
+#define DE_CLIP_TL_STATUS_ENABLE                        1
+#define DE_CLIP_TL_INHIBIT                              12:12
+#define DE_CLIP_TL_INHIBIT_OUTSIDE                      0
+#define DE_CLIP_TL_INHIBIT_INSIDE                       1
+#define DE_CLIP_TL_LEFT                                 11:0
+
+#define DE_CLIP_BR                                      0x100030
+#define DE_CLIP_BR_BOTTOM                               31:16
+#define DE_CLIP_BR_RIGHT                                12:0
+
+#define DE_MONO_PATTERN_LOW                             0x100034
+#define DE_MONO_PATTERN_LOW_PATTERN                     31:0
+
+#define DE_MONO_PATTERN_HIGH                            0x100038
+#define DE_MONO_PATTERN_HIGH_PATTERN                    31:0
+
+#define DE_WINDOW_WIDTH                                 0x10003C
+#define DE_WINDOW_WIDTH_DESTINATION                     28:16
+#define DE_WINDOW_WIDTH_SOURCE                          12:0
+
+#define DE_WINDOW_SOURCE_BASE                           0x100040
+#define DE_WINDOW_SOURCE_BASE_EXT                       27:27
+#define DE_WINDOW_SOURCE_BASE_EXT_LOCAL                 0
+#define DE_WINDOW_SOURCE_BASE_EXT_EXTERNAL              1
+#define DE_WINDOW_SOURCE_BASE_CS                        26:26
+#define DE_WINDOW_SOURCE_BASE_CS_0                      0
+#define DE_WINDOW_SOURCE_BASE_CS_1                      1
+#define DE_WINDOW_SOURCE_BASE_ADDRESS                   25:0
+
+#define DE_WINDOW_DESTINATION_BASE                      0x100044
+#define DE_WINDOW_DESTINATION_BASE_EXT                  27:27
+#define DE_WINDOW_DESTINATION_BASE_EXT_LOCAL            0
+#define DE_WINDOW_DESTINATION_BASE_EXT_EXTERNAL         1
+#define DE_WINDOW_DESTINATION_BASE_CS                   26:26
+#define DE_WINDOW_DESTINATION_BASE_CS_0                 0
+#define DE_WINDOW_DESTINATION_BASE_CS_1                 1
+#define DE_WINDOW_DESTINATION_BASE_ADDRESS              25:0
+
+#define DE_ALPHA                                        0x100048
+#define DE_ALPHA_VALUE                                  7:0
+
+#define DE_WRAP                                         0x10004C
+#define DE_WRAP_X                                       31:16
+#define DE_WRAP_Y                                       15:0
+
+#define DE_STATUS                                       0x100050
+#define DE_STATUS_CSC                                   1:1
+#define DE_STATUS_CSC_CLEAR                             0
+#define DE_STATUS_CSC_NOT_ACTIVE                        0
+#define DE_STATUS_CSC_ACTIVE                            1
+#define DE_STATUS_2D                                    0:0
+#define DE_STATUS_2D_CLEAR                              0
+#define DE_STATUS_2D_NOT_ACTIVE                         0
+#define DE_STATUS_2D_ACTIVE                             1
+#endif
+/* Color Space Conversion registers. */
+
+#define CSC_Y_SOURCE_BASE                               0x1000C8
+#define CSC_Y_SOURCE_BASE_EXT                           27:27
+#define CSC_Y_SOURCE_BASE_EXT_LOCAL                     0
+#define CSC_Y_SOURCE_BASE_EXT_EXTERNAL                  1
+#define CSC_Y_SOURCE_BASE_CS                            26:26
+#define CSC_Y_SOURCE_BASE_CS_0                          0
+#define CSC_Y_SOURCE_BASE_CS_1                          1
+#define CSC_Y_SOURCE_BASE_ADDRESS                       25:0
+
+#define CSC_CONSTANTS                                   0x1000CC
+#define CSC_CONSTANTS_Y                                 31:24
+#define CSC_CONSTANTS_R                                 23:16
+#define CSC_CONSTANTS_G                                 15:8
+#define CSC_CONSTANTS_B                                 7:0
+
+#define CSC_Y_SOURCE_X                                  0x1000D0
+#define CSC_Y_SOURCE_X_INTEGER                          26:16
+#define CSC_Y_SOURCE_X_FRACTION                         15:3
+
+#define CSC_Y_SOURCE_Y                                  0x1000D4
+#define CSC_Y_SOURCE_Y_INTEGER                          27:16
+#define CSC_Y_SOURCE_Y_FRACTION                         15:3
+
+#define CSC_U_SOURCE_BASE                               0x1000D8
+#define CSC_U_SOURCE_BASE_EXT                           27:27
+#define CSC_U_SOURCE_BASE_EXT_LOCAL                     0
+#define CSC_U_SOURCE_BASE_EXT_EXTERNAL                  1
+#define CSC_U_SOURCE_BASE_CS                            26:26
+#define CSC_U_SOURCE_BASE_CS_0                          0
+#define CSC_U_SOURCE_BASE_CS_1                          1
+#define CSC_U_SOURCE_BASE_ADDRESS                       25:0
+
+#define CSC_V_SOURCE_BASE                               0x1000DC
+#define CSC_V_SOURCE_BASE_EXT                           27:27
+#define CSC_V_SOURCE_BASE_EXT_LOCAL                     0
+#define CSC_V_SOURCE_BASE_EXT_EXTERNAL                  1
+#define CSC_V_SOURCE_BASE_CS                            26:26
+#define CSC_V_SOURCE_BASE_CS_0                          0
+#define CSC_V_SOURCE_BASE_CS_1                          1
+#define CSC_V_SOURCE_BASE_ADDRESS                       25:0
+
+#define CSC_SOURCE_DIMENSION                            0x1000E0
+#define CSC_SOURCE_DIMENSION_X                          31:16
+#define CSC_SOURCE_DIMENSION_Y                          15:0
+
+#define CSC_SOURCE_PITCH                                0x1000E4
+#define CSC_SOURCE_PITCH_Y                              31:16
+#define CSC_SOURCE_PITCH_UV                             15:0
+
+#define CSC_DESTINATION                                 0x1000E8
+#define CSC_DESTINATION_WRAP                            31:31
+#define CSC_DESTINATION_WRAP_DISABLE                    0
+#define CSC_DESTINATION_WRAP_ENABLE                     1
+#define CSC_DESTINATION_X                               27:16
+#define CSC_DESTINATION_Y                               11:0
+
+#define CSC_DESTINATION_DIMENSION                       0x1000EC
+#define CSC_DESTINATION_DIMENSION_X                     31:16
+#define CSC_DESTINATION_DIMENSION_Y                     15:0
+
+#define CSC_DESTINATION_PITCH                           0x1000F0
+#define CSC_DESTINATION_PITCH_X                         31:16
+#define CSC_DESTINATION_PITCH_Y                         15:0
+
+#define CSC_SCALE_FACTOR                                0x1000F4
+#define CSC_SCALE_FACTOR_HORIZONTAL                     31:16
+#define CSC_SCALE_FACTOR_VERTICAL                       15:0
+
+#define CSC_DESTINATION_BASE                            0x1000F8
+#define CSC_DESTINATION_BASE_EXT                        27:27
+#define CSC_DESTINATION_BASE_EXT_LOCAL                  0
+#define CSC_DESTINATION_BASE_EXT_EXTERNAL               1
+#define CSC_DESTINATION_BASE_CS                         26:26
+#define CSC_DESTINATION_BASE_CS_0                       0
+#define CSC_DESTINATION_BASE_CS_1                       1
+#define CSC_DESTINATION_BASE_ADDRESS                    25:0
+
+#define CSC_CONTROL                                     0x1000FC
+#define CSC_CONTROL_STATUS                              31:31
+#define CSC_CONTROL_STATUS_STOP                         0
+#define CSC_CONTROL_STATUS_START                        1
+#define CSC_CONTROL_SOURCE_FORMAT                       30:28
+#define CSC_CONTROL_SOURCE_FORMAT_YUV422                0
+#define CSC_CONTROL_SOURCE_FORMAT_YUV420I               1
+#define CSC_CONTROL_SOURCE_FORMAT_YUV420                2
+#define CSC_CONTROL_SOURCE_FORMAT_YVU9                  3
+#define CSC_CONTROL_SOURCE_FORMAT_IYU1                  4
+#define CSC_CONTROL_SOURCE_FORMAT_IYU2                  5
+#define CSC_CONTROL_SOURCE_FORMAT_RGB565                6
+#define CSC_CONTROL_SOURCE_FORMAT_RGB8888               7
+#define CSC_CONTROL_DESTINATION_FORMAT                  27:26
+#define CSC_CONTROL_DESTINATION_FORMAT_RGB565           0
+#define CSC_CONTROL_DESTINATION_FORMAT_RGB8888          1
+#define CSC_CONTROL_HORIZONTAL_FILTER                   25:25
+#define CSC_CONTROL_HORIZONTAL_FILTER_DISABLE           0
+#define CSC_CONTROL_HORIZONTAL_FILTER_ENABLE            1
+#define CSC_CONTROL_VERTICAL_FILTER                     24:24
+#define CSC_CONTROL_VERTICAL_FILTER_DISABLE             0
+#define CSC_CONTROL_VERTICAL_FILTER_ENABLE              1
+#define CSC_CONTROL_BYTE_ORDER                          23:23
+#define CSC_CONTROL_BYTE_ORDER_YUYV                     0
+#define CSC_CONTROL_BYTE_ORDER_UYVY                     1
+
+#define DE_DATA_PORT                                    0x110000
+
+#define I2C_BYTE_COUNT                                  0x010040
+#define I2C_BYTE_COUNT_COUNT                            3:0
+
+#define I2C_CTRL                                        0x010041
+#define I2C_CTRL_INT                                    4:4
+#define I2C_CTRL_INT_DISABLE                            0
+#define I2C_CTRL_INT_ENABLE                             1
+#define I2C_CTRL_DIR                                    3:3
+#define I2C_CTRL_DIR_WR                                 0
+#define I2C_CTRL_DIR_RD                                 1
+#define I2C_CTRL_CTRL                                   2:2
+#define I2C_CTRL_CTRL_STOP                              0
+#define I2C_CTRL_CTRL_START                             1
+#define I2C_CTRL_MODE                                   1:1
+#define I2C_CTRL_MODE_STANDARD                          0
+#define I2C_CTRL_MODE_FAST                              1
+#define I2C_CTRL_EN                                     0:0
+#define I2C_CTRL_EN_DISABLE                             0
+#define I2C_CTRL_EN_ENABLE                              1
+
+#define I2C_STATUS                                      0x010042
+#define I2C_STATUS_TX                                   3:3
+#define I2C_STATUS_TX_PROGRESS                          0
+#define I2C_STATUS_TX_COMPLETED                         1
+#define I2C_TX_DONE                                     0x08
+#define I2C_STATUS_ERR                                  2:2
+#define I2C_STATUS_ERR_NORMAL                           0
+#define I2C_STATUS_ERR_ERROR                            1
+#define I2C_STATUS_ERR_CLEAR                            0
+#define I2C_STATUS_ACK                                  1:1
+#define I2C_STATUS_ACK_RECEIVED                         0
+#define I2C_STATUS_ACK_NOT                              1
+#define I2C_STATUS_BSY                                  0:0
+#define I2C_STATUS_BSY_IDLE                             0
+#define I2C_STATUS_BSY_BUSY                             1
+
+#define I2C_RESET                                       0x010042
+#define I2C_RESET_BUS_ERROR                             2:2
+#define I2C_RESET_BUS_ERROR_CLEAR                       0
+
+#define I2C_SLAVE_ADDRESS                               0x010043
+#define I2C_SLAVE_ADDRESS_ADDRESS                       7:1
+#define I2C_SLAVE_ADDRESS_RW                            0:0
+#define I2C_SLAVE_ADDRESS_RW_W                          0
+#define I2C_SLAVE_ADDRESS_RW_R                          1
+
+#define I2C_DATA0                                       0x010044
+#define I2C_DATA1                                       0x010045
+#define I2C_DATA2                                       0x010046
+#define I2C_DATA3                                       0x010047
+#define I2C_DATA4                                       0x010048
+#define I2C_DATA5                                       0x010049
+#define I2C_DATA6                                       0x01004A
+#define I2C_DATA7                                       0x01004B
+#define I2C_DATA8                                       0x01004C
+#define I2C_DATA9                                       0x01004D
+#define I2C_DATA10                                      0x01004E
+#define I2C_DATA11                                      0x01004F
+#define I2C_DATA12                                      0x010050
+#define I2C_DATA13                                      0x010051
+#define I2C_DATA14                                      0x010052
+#define I2C_DATA15                                      0x010053
+
+
+#define ZV0_CAPTURE_CTRL                                0x090000
+#define ZV0_CAPTURE_CTRL_FIELD_INPUT                    27:27
+#define ZV0_CAPTURE_CTRL_FIELD_INPUT_EVEN_FIELD         0
+#define ZV0_CAPTURE_CTRL_FIELD_INPUT_ODD_FIELD          1
+#define ZV0_CAPTURE_CTRL_SCAN                           26:26
+#define ZV0_CAPTURE_CTRL_SCAN_PROGRESSIVE               0
+#define ZV0_CAPTURE_CTRL_SCAN_INTERLACE                 1
+#define ZV0_CAPTURE_CTRL_CURRENT_BUFFER                 25:25
+#define ZV0_CAPTURE_CTRL_CURRENT_BUFFER_0               0
+#define ZV0_CAPTURE_CTRL_CURRENT_BUFFER_1               1
+#define ZV0_CAPTURE_CTRL_VERTICAL_SYNC                  24:24
+#define ZV0_CAPTURE_CTRL_VERTICAL_SYNC_INACTIVE         0
+#define ZV0_CAPTURE_CTRL_VERTICAL_SYNC_ACTIVE           1
+#define ZV0_CAPTURE_CTRL_ADJ                            19:19
+#define ZV0_CAPTURE_CTRL_ADJ_NORMAL                     0
+#define ZV0_CAPTURE_CTRL_ADJ_DELAY                      1
+#define ZV0_CAPTURE_CTRL_HA                             18:18
+#define ZV0_CAPTURE_CTRL_HA_DISABLE                     0
+#define ZV0_CAPTURE_CTRL_HA_ENABLE                      1
+#define ZV0_CAPTURE_CTRL_VSK                            17:17
+#define ZV0_CAPTURE_CTRL_VSK_DISABLE                    0
+#define ZV0_CAPTURE_CTRL_VSK_ENABLE                     1
+#define ZV0_CAPTURE_CTRL_HSK                            16:16
+#define ZV0_CAPTURE_CTRL_HSK_DISABLE                    0
+#define ZV0_CAPTURE_CTRL_HSK_ENABLE                     1
+#define ZV0_CAPTURE_CTRL_FD                             15:15
+#define ZV0_CAPTURE_CTRL_FD_RISING                      0
+#define ZV0_CAPTURE_CTRL_FD_FALLING                     1
+#define ZV0_CAPTURE_CTRL_VP                             14:14
+#define ZV0_CAPTURE_CTRL_VP_HIGH                        0
+#define ZV0_CAPTURE_CTRL_VP_LOW                         1
+#define ZV0_CAPTURE_CTRL_HP                             13:13
+#define ZV0_CAPTURE_CTRL_HP_HIGH                        0
+#define ZV0_CAPTURE_CTRL_HP_LOW                         1
+#define ZV0_CAPTURE_CTRL_CP                             12:12
+#define ZV0_CAPTURE_CTRL_CP_HIGH                        0
+#define ZV0_CAPTURE_CTRL_CP_LOW                         1
+#define ZV0_CAPTURE_CTRL_UVS                            11:11
+#define ZV0_CAPTURE_CTRL_UVS_DISABLE                    0
+#define ZV0_CAPTURE_CTRL_UVS_ENABLE                     1
+#define ZV0_CAPTURE_CTRL_BS                             10:10
+#define ZV0_CAPTURE_CTRL_BS_DISABLE                     0
+#define ZV0_CAPTURE_CTRL_BS_ENABLE                      1
+#define ZV0_CAPTURE_CTRL_CS                             9:9
+#define ZV0_CAPTURE_CTRL_CS_16                          0
+#define ZV0_CAPTURE_CTRL_CS_8                           1
+#define ZV0_CAPTURE_CTRL_CF                             8:8
+#define ZV0_CAPTURE_CTRL_CF_YUV                         0
+#define ZV0_CAPTURE_CTRL_CF_RGB                         1
+#define ZV0_CAPTURE_CTRL_FS                             7:7
+#define ZV0_CAPTURE_CTRL_FS_DISABLE                     0
+#define ZV0_CAPTURE_CTRL_FS_ENABLE                      1
+#define ZV0_CAPTURE_CTRL_WEAVE                          6:6
+#define ZV0_CAPTURE_CTRL_WEAVE_DISABLE                  0
+#define ZV0_CAPTURE_CTRL_WEAVE_ENABLE                   1
+#define ZV0_CAPTURE_CTRL_BOB                            5:5
+#define ZV0_CAPTURE_CTRL_BOB_DISABLE                    0
+#define ZV0_CAPTURE_CTRL_BOB_ENABLE                     1
+#define ZV0_CAPTURE_CTRL_DB                             4:4
+#define ZV0_CAPTURE_CTRL_DB_DISABLE                     0
+#define ZV0_CAPTURE_CTRL_DB_ENABLE                      1
+#define ZV0_CAPTURE_CTRL_CC                             3:3
+#define ZV0_CAPTURE_CTRL_CC_CONTINUE                    0
+#define ZV0_CAPTURE_CTRL_CC_CONDITION                   1
+#define ZV0_CAPTURE_CTRL_RGB                            2:2
+#define ZV0_CAPTURE_CTRL_RGB_DISABLE                    0
+#define ZV0_CAPTURE_CTRL_RGB_ENABLE                     1
+#define ZV0_CAPTURE_CTRL_656                            1:1
+#define ZV0_CAPTURE_CTRL_656_DISABLE                    0
+#define ZV0_CAPTURE_CTRL_656_ENABLE                     1
+#define ZV0_CAPTURE_CTRL_CAP                            0:0
+#define ZV0_CAPTURE_CTRL_CAP_DISABLE                    0
+#define ZV0_CAPTURE_CTRL_CAP_ENABLE                     1
+
+#define ZV0_CAPTURE_CLIP                                0x090004
+#define ZV0_CAPTURE_CLIP_YCLIP_EVEN_FIELD                25:16
+#define ZV0_CAPTURE_CLIP_YCLIP                          25:16
+#define ZV0_CAPTURE_CLIP_XCLIP                          9:0
+
+#define ZV0_CAPTURE_SIZE                                0x090008
+#define ZV0_CAPTURE_SIZE_HEIGHT                         26:16
+#define ZV0_CAPTURE_SIZE_WIDTH                          10:0
+
+#define ZV0_CAPTURE_BUF0_ADDRESS                        0x09000C
+#define ZV0_CAPTURE_BUF0_ADDRESS_STATUS                 31:31
+#define ZV0_CAPTURE_BUF0_ADDRESS_STATUS_CURRENT         0
+#define ZV0_CAPTURE_BUF0_ADDRESS_STATUS_PENDING         1
+#define ZV0_CAPTURE_BUF0_ADDRESS_EXT                    27:27
+#define ZV0_CAPTURE_BUF0_ADDRESS_EXT_LOCAL              0
+#define ZV0_CAPTURE_BUF0_ADDRESS_EXT_EXTERNAL           1
+#define ZV0_CAPTURE_BUF0_ADDRESS_CS                     26:26
+#define ZV0_CAPTURE_BUF0_ADDRESS_CS_0                   0
+#define ZV0_CAPTURE_BUF0_ADDRESS_CS_1                   1
+#define ZV0_CAPTURE_BUF0_ADDRESS_ADDRESS                25:0
+
+#define ZV0_CAPTURE_BUF1_ADDRESS                        0x090010
+#define ZV0_CAPTURE_BUF1_ADDRESS_STATUS                 31:31
+#define ZV0_CAPTURE_BUF1_ADDRESS_STATUS_CURRENT         0
+#define ZV0_CAPTURE_BUF1_ADDRESS_STATUS_PENDING         1
+#define ZV0_CAPTURE_BUF1_ADDRESS_EXT                    27:27
+#define ZV0_CAPTURE_BUF1_ADDRESS_EXT_LOCAL              0
+#define ZV0_CAPTURE_BUF1_ADDRESS_EXT_EXTERNAL           1
+#define ZV0_CAPTURE_BUF1_ADDRESS_CS                     26:26
+#define ZV0_CAPTURE_BUF1_ADDRESS_CS_0                   0
+#define ZV0_CAPTURE_BUF1_ADDRESS_CS_1                   1
+#define ZV0_CAPTURE_BUF1_ADDRESS_ADDRESS                25:0
+
+#define ZV0_CAPTURE_BUF_OFFSET                          0x090014
+#ifndef VALIDATION_CHIP
+    #define ZV0_CAPTURE_BUF_OFFSET_YCLIP_ODD_FIELD      25:16
+#endif
+#define ZV0_CAPTURE_BUF_OFFSET_OFFSET                   15:0
+
+#define ZV0_CAPTURE_FIFO_CTRL                           0x090018
+#define ZV0_CAPTURE_FIFO_CTRL_FIFO                      2:0
+#define ZV0_CAPTURE_FIFO_CTRL_FIFO_0                    0
+#define ZV0_CAPTURE_FIFO_CTRL_FIFO_1                    1
+#define ZV0_CAPTURE_FIFO_CTRL_FIFO_2                    2
+#define ZV0_CAPTURE_FIFO_CTRL_FIFO_3                    3
+#define ZV0_CAPTURE_FIFO_CTRL_FIFO_4                    4
+#define ZV0_CAPTURE_FIFO_CTRL_FIFO_5                    5
+#define ZV0_CAPTURE_FIFO_CTRL_FIFO_6                    6
+#define ZV0_CAPTURE_FIFO_CTRL_FIFO_7                    7
+
+#define ZV0_CAPTURE_YRGB_CONST                          0x09001C
+#define ZV0_CAPTURE_YRGB_CONST_Y                        31:24
+#define ZV0_CAPTURE_YRGB_CONST_R                        23:16
+#define ZV0_CAPTURE_YRGB_CONST_G                        15:8
+#define ZV0_CAPTURE_YRGB_CONST_B                        7:0
+
+#define ZV0_CAPTURE_LINE_COMP                           0x090020
+#define ZV0_CAPTURE_LINE_COMP_LC                        10:0
+
+/* ZV1 */
+
+#define ZV1_CAPTURE_CTRL                                0x098000
+#define ZV1_CAPTURE_CTRL_FIELD_INPUT                    27:27
+#define ZV1_CAPTURE_CTRL_FIELD_INPUT_EVEN_FIELD         0
+#define ZV1_CAPTURE_CTRL_FIELD_INPUT_ODD_FIELD          0
+#define ZV1_CAPTURE_CTRL_SCAN                           26:26
+#define ZV1_CAPTURE_CTRL_SCAN_PROGRESSIVE               0
+#define ZV1_CAPTURE_CTRL_SCAN_INTERLACE                 1
+#define ZV1_CAPTURE_CTRL_CURRENT_BUFFER                 25:25
+#define ZV1_CAPTURE_CTRL_CURRENT_BUFFER_0               0
+#define ZV1_CAPTURE_CTRL_CURRENT_BUFFER_1               1
+#define ZV1_CAPTURE_CTRL_VERTICAL_SYNC                  24:24
+#define ZV1_CAPTURE_CTRL_VERTICAL_SYNC_INACTIVE         0
+#define ZV1_CAPTURE_CTRL_VERTICAL_SYNC_ACTIVE           1
+#define ZV1_CAPTURE_CTRL_PANEL                          20:20
+#define ZV1_CAPTURE_CTRL_PANEL_DISABLE                  0
+#define ZV1_CAPTURE_CTRL_PANEL_ENABLE                   1
+#define ZV1_CAPTURE_CTRL_ADJ                            19:19
+#define ZV1_CAPTURE_CTRL_ADJ_NORMAL                     0
+#define ZV1_CAPTURE_CTRL_ADJ_DELAY                      1
+#define ZV1_CAPTURE_CTRL_HA                             18:18
+#define ZV1_CAPTURE_CTRL_HA_DISABLE                     0
+#define ZV1_CAPTURE_CTRL_HA_ENABLE                      1
+#define ZV1_CAPTURE_CTRL_VSK                            17:17
+#define ZV1_CAPTURE_CTRL_VSK_DISABLE                    0
+#define ZV1_CAPTURE_CTRL_VSK_ENABLE                     1
+#define ZV1_CAPTURE_CTRL_HSK                            16:16
+#define ZV1_CAPTURE_CTRL_HSK_DISABLE                    0
+#define ZV1_CAPTURE_CTRL_HSK_ENABLE                     1
+#define ZV1_CAPTURE_CTRL_FD                             15:15
+#define ZV1_CAPTURE_CTRL_FD_RISING                      0
+#define ZV1_CAPTURE_CTRL_FD_FALLING                     1
+#define ZV1_CAPTURE_CTRL_VP                             14:14
+#define ZV1_CAPTURE_CTRL_VP_HIGH                        0
+#define ZV1_CAPTURE_CTRL_VP_LOW                         1
+#define ZV1_CAPTURE_CTRL_HP                             13:13
+#define ZV1_CAPTURE_CTRL_HP_HIGH                        0
+#define ZV1_CAPTURE_CTRL_HP_LOW                         1
+#define ZV1_CAPTURE_CTRL_CP                             12:12
+#define ZV1_CAPTURE_CTRL_CP_HIGH                        0
+#define ZV1_CAPTURE_CTRL_CP_LOW                         1
+#define ZV1_CAPTURE_CTRL_UVS                            11:11
+#define ZV1_CAPTURE_CTRL_UVS_DISABLE                    0
+#define ZV1_CAPTURE_CTRL_UVS_ENABLE                     1
+#define ZV1_CAPTURE_CTRL_BS                             10:10
+#define ZV1_CAPTURE_CTRL_BS_DISABLE                     0
+#define ZV1_CAPTURE_CTRL_BS_ENABLE                      1
+#define ZV1_CAPTURE_CTRL_CS                             9:9
+#define ZV1_CAPTURE_CTRL_CS_16                          0
+#define ZV1_CAPTURE_CTRL_CS_8                           1
+#define ZV1_CAPTURE_CTRL_CF                             8:8
+#define ZV1_CAPTURE_CTRL_CF_YUV                         0
+#define ZV1_CAPTURE_CTRL_CF_RGB                         1
+#define ZV1_CAPTURE_CTRL_FS                             7:7
+#define ZV1_CAPTURE_CTRL_FS_DISABLE                     0
+#define ZV1_CAPTURE_CTRL_FS_ENABLE                      1
+#define ZV1_CAPTURE_CTRL_WEAVE                          6:6
+#define ZV1_CAPTURE_CTRL_WEAVE_DISABLE                  0
+#define ZV1_CAPTURE_CTRL_WEAVE_ENABLE                   1
+#define ZV1_CAPTURE_CTRL_BOB                            5:5
+#define ZV1_CAPTURE_CTRL_BOB_DISABLE                    0
+#define ZV1_CAPTURE_CTRL_BOB_ENABLE                     1
+#define ZV1_CAPTURE_CTRL_DB                             4:4
+#define ZV1_CAPTURE_CTRL_DB_DISABLE                     0
+#define ZV1_CAPTURE_CTRL_DB_ENABLE                      1
+#define ZV1_CAPTURE_CTRL_CC                             3:3
+#define ZV1_CAPTURE_CTRL_CC_CONTINUE                    0
+#define ZV1_CAPTURE_CTRL_CC_CONDITION                   1
+#define ZV1_CAPTURE_CTRL_RGB                            2:2
+#define ZV1_CAPTURE_CTRL_RGB_DISABLE                    0
+#define ZV1_CAPTURE_CTRL_RGB_ENABLE                     1
+#define ZV1_CAPTURE_CTRL_656                            1:1
+#define ZV1_CAPTURE_CTRL_656_DISABLE                    0
+#define ZV1_CAPTURE_CTRL_656_ENABLE                     1
+#define ZV1_CAPTURE_CTRL_CAP                            0:0
+#define ZV1_CAPTURE_CTRL_CAP_DISABLE                    0
+#define ZV1_CAPTURE_CTRL_CAP_ENABLE                     1
+
+#define ZV1_CAPTURE_CLIP                                0x098004
+#define ZV1_CAPTURE_CLIP_YCLIP                          25:16
+#define ZV1_CAPTURE_CLIP_XCLIP                          9:0
+
+#define ZV1_CAPTURE_SIZE                                0x098008
+#define ZV1_CAPTURE_SIZE_HEIGHT                         26:16
+#define ZV1_CAPTURE_SIZE_WIDTH                          10:0
+
+#define ZV1_CAPTURE_BUF0_ADDRESS                        0x09800C
+#define ZV1_CAPTURE_BUF0_ADDRESS_STATUS                 31:31
+#define ZV1_CAPTURE_BUF0_ADDRESS_STATUS_CURRENT         0
+#define ZV1_CAPTURE_BUF0_ADDRESS_STATUS_PENDING         1
+#define ZV1_CAPTURE_BUF0_ADDRESS_EXT                    27:27
+#define ZV1_CAPTURE_BUF0_ADDRESS_EXT_LOCAL              0
+#define ZV1_CAPTURE_BUF0_ADDRESS_EXT_EXTERNAL           1
+#define ZV1_CAPTURE_BUF0_ADDRESS_CS                     26:26
+#define ZV1_CAPTURE_BUF0_ADDRESS_CS_0                   0
+#define ZV1_CAPTURE_BUF0_ADDRESS_CS_1                   1
+#define ZV1_CAPTURE_BUF0_ADDRESS_ADDRESS                25:0
+
+#define ZV1_CAPTURE_BUF1_ADDRESS                        0x098010
+#define ZV1_CAPTURE_BUF1_ADDRESS_STATUS                 31:31
+#define ZV1_CAPTURE_BUF1_ADDRESS_STATUS_CURRENT         0
+#define ZV1_CAPTURE_BUF1_ADDRESS_STATUS_PENDING         1
+#define ZV1_CAPTURE_BUF1_ADDRESS_EXT                    27:27
+#define ZV1_CAPTURE_BUF1_ADDRESS_EXT_LOCAL              0
+#define ZV1_CAPTURE_BUF1_ADDRESS_EXT_EXTERNAL           1
+#define ZV1_CAPTURE_BUF1_ADDRESS_CS                     26:26
+#define ZV1_CAPTURE_BUF1_ADDRESS_CS_0                   0
+#define ZV1_CAPTURE_BUF1_ADDRESS_CS_1                   1
+#define ZV1_CAPTURE_BUF1_ADDRESS_ADDRESS                25:0
+
+#define ZV1_CAPTURE_BUF_OFFSET                          0x098014
+#define ZV1_CAPTURE_BUF_OFFSET_OFFSET                   15:0
+
+#define ZV1_CAPTURE_FIFO_CTRL                           0x098018
+#define ZV1_CAPTURE_FIFO_CTRL_FIFO                      2:0
+#define ZV1_CAPTURE_FIFO_CTRL_FIFO_0                    0
+#define ZV1_CAPTURE_FIFO_CTRL_FIFO_1                    1
+#define ZV1_CAPTURE_FIFO_CTRL_FIFO_2                    2
+#define ZV1_CAPTURE_FIFO_CTRL_FIFO_3                    3
+#define ZV1_CAPTURE_FIFO_CTRL_FIFO_4                    4
+#define ZV1_CAPTURE_FIFO_CTRL_FIFO_5                    5
+#define ZV1_CAPTURE_FIFO_CTRL_FIFO_6                    6
+#define ZV1_CAPTURE_FIFO_CTRL_FIFO_7                    7
+
+#define ZV1_CAPTURE_YRGB_CONST                          0x09801C
+#define ZV1_CAPTURE_YRGB_CONST_Y                        31:24
+#define ZV1_CAPTURE_YRGB_CONST_R                        23:16
+#define ZV1_CAPTURE_YRGB_CONST_G                        15:8
+#define ZV1_CAPTURE_YRGB_CONST_B                        7:0
+
+#define DMA_1_SOURCE                                    0x0D0010
+#define DMA_1_SOURCE_ADDRESS_EXT                        27:27
+#define DMA_1_SOURCE_ADDRESS_EXT_LOCAL                  0
+#define DMA_1_SOURCE_ADDRESS_EXT_EXTERNAL               1
+#define DMA_1_SOURCE_ADDRESS_CS                         26:26
+#define DMA_1_SOURCE_ADDRESS_CS_0                       0
+#define DMA_1_SOURCE_ADDRESS_CS_1                       1
+#define DMA_1_SOURCE_ADDRESS                            25:0
+
+#define DMA_1_DESTINATION                               0x0D0014
+#define DMA_1_DESTINATION_ADDRESS_EXT                   27:27
+#define DMA_1_DESTINATION_ADDRESS_EXT_LOCAL             0
+#define DMA_1_DESTINATION_ADDRESS_EXT_EXTERNAL          1
+#define DMA_1_DESTINATION_ADDRESS_CS                    26:26
+#define DMA_1_DESTINATION_ADDRESS_CS_0                  0
+#define DMA_1_DESTINATION_ADDRESS_CS_1                  1
+#define DMA_1_DESTINATION_ADDRESS                       25:0
+
+#define DMA_1_SIZE_CONTROL                              0x0D0018
+#define DMA_1_SIZE_CONTROL_STATUS                       31:31
+#define DMA_1_SIZE_CONTROL_STATUS_IDLE                  0
+#define DMA_1_SIZE_CONTROL_STATUS_ACTIVE                1
+#define DMA_1_SIZE_CONTROL_SIZE                         23:0
+
+#define DMA_ABORT_INTERRUPT                             0x0D0020
+#define DMA_ABORT_INTERRUPT_ABORT_1                     5:5
+#define DMA_ABORT_INTERRUPT_ABORT_1_ENABLE              0
+#define DMA_ABORT_INTERRUPT_ABORT_1_ABORT               1
+#define DMA_ABORT_INTERRUPT_ABORT_0                     4:4
+#define DMA_ABORT_INTERRUPT_ABORT_0_ENABLE              0
+#define DMA_ABORT_INTERRUPT_ABORT_0_ABORT               1
+#define DMA_ABORT_INTERRUPT_INT_1                       1:1
+#define DMA_ABORT_INTERRUPT_INT_1_CLEAR                 0
+#define DMA_ABORT_INTERRUPT_INT_1_FINISHED              1
+#define DMA_ABORT_INTERRUPT_INT_0                       0:0
+#define DMA_ABORT_INTERRUPT_INT_0_CLEAR                 0
+#define DMA_ABORT_INTERRUPT_INT_0_FINISHED              1
+
+
+
+
+
+/* Default i2c CLK and Data GPIO. These are the default i2c pins */
+#define DEFAULT_I2C_SCL                     30
+#define DEFAULT_I2C_SDA                     31
+
+
+#define GPIO_DATA_SM750LE                               0x020018
+#define GPIO_DATA_SM750LE_1                             1:1
+#define GPIO_DATA_SM750LE_0                             0:0
+
+#define GPIO_DATA_DIRECTION_SM750LE                     0x02001C
+#define GPIO_DATA_DIRECTION_SM750LE_1                   1:1
+#define GPIO_DATA_DIRECTION_SM750LE_1_INPUT             0
+#define GPIO_DATA_DIRECTION_SM750LE_1_OUTPUT            1
+#define GPIO_DATA_DIRECTION_SM750LE_0                   0:0
+#define GPIO_DATA_DIRECTION_SM750LE_0_INPUT             0
+#define GPIO_DATA_DIRECTION_SM750LE_0_OUTPUT            1
+
+
+#endif
diff --git a/drivers/staging/sm750fb/ddk750_sii164.c b/drivers/staging/sm750fb/ddk750_sii164.c
new file mode 100644
index 000000000000..3d224d6a74ff
--- /dev/null
+++ b/drivers/staging/sm750fb/ddk750_sii164.c
@@ -0,0 +1,425 @@
+#define USE_DVICHIP
+#ifdef USE_DVICHIP
+
+#include "ddk750_sii164.h"
+#include "ddk750_hwi2c.h"
+
+/* I2C Address of each SII164 chip */
+#define SII164_I2C_ADDRESS                  0x70
+
+/* Define this definition to use hardware i2c. */
+#define USE_HW_I2C
+
+#ifdef USE_HW_I2C
+    #define i2cWriteReg hwI2CWriteReg
+    #define i2cReadReg  hwI2CReadReg
+#else
+    #define i2cWriteReg swI2CWriteReg
+    #define i2cReadReg  swI2CReadReg
+#endif
+
+/* SII164 Vendor and Device ID */
+#define SII164_VENDOR_ID                    0x0001
+#define SII164_DEVICE_ID                    0x0006
+
+#ifdef SII164_FULL_FUNCTIONS
+/* Name of the DVI Controller chip */
+static char *gDviCtrlChipName = "Silicon Image SiI 164";
+#endif
+
+/*
+ *  sii164GetVendorID
+ *      This function gets the vendor ID of the DVI controller chip.
+ *
+ *  Output:
+ *      Vendor ID
+ */
+unsigned short sii164GetVendorID(void)
+{
+    unsigned short vendorID;
+
+    vendorID = ((unsigned short) i2cReadReg(SII164_I2C_ADDRESS, SII164_VENDOR_ID_HIGH) << 8) |
+                (unsigned short) i2cReadReg(SII164_I2C_ADDRESS, SII164_VENDOR_ID_LOW);
+
+    return vendorID;
+}
+
+/*
+ *  sii164GetDeviceID
+ *      This function gets the device ID of the DVI controller chip.
+ *
+ *  Output:
+ *      Device ID
+ */
+unsigned short sii164GetDeviceID(void)
+{
+    unsigned short deviceID;
+
+    deviceID = ((unsigned short) i2cReadReg(SII164_I2C_ADDRESS, SII164_DEVICE_ID_HIGH) << 8) |
+                (unsigned short) i2cReadReg(SII164_I2C_ADDRESS, SII164_DEVICE_ID_LOW);
+
+    return deviceID;
+}
+
+
+
+/* DVI.C will handle all SiI164 chip stuffs and try it best to make code minimal and useful */
+
+/*
+ *  sii164InitChip
+ *      This function initialize and detect the DVI controller chip.
+ *
+ *  Input:
+ *      edgeSelect          - Edge Select:
+ *                              0 = Input data is falling edge latched (falling edge
+ *                                  latched first in dual edge mode)
+ *                              1 = Input data is rising edge latched (rising edge
+ *                                  latched first in dual edge mode)
+ *      busSelect           - Input Bus Select:
+ *                              0 = Input data bus is 12-bits wide
+ *                              1 = Input data bus is 24-bits wide
+ *      dualEdgeClkSelect   - Dual Edge Clock Select
+ *                              0 = Input data is single edge latched
+ *                              1 = Input data is dual edge latched
+ *      hsyncEnable         - Horizontal Sync Enable:
+ *                              0 = HSYNC input is transmitted as fixed LOW
+ *                              1 = HSYNC input is transmitted as is
+ *      vsyncEnable         - Vertical Sync Enable:
+ *                              0 = VSYNC input is transmitted as fixed LOW
+ *                              1 = VSYNC input is transmitted as is
+ *      deskewEnable        - De-skewing Enable:
+ *                              0 = De-skew disabled
+ *                              1 = De-skew enabled
+ *      deskewSetting       - De-skewing Setting (increment of 260psec)
+ *                              0 = 1 step --> minimum setup / maximum hold
+ *                              1 = 2 step
+ *                              2 = 3 step
+ *                              3 = 4 step
+ *                              4 = 5 step
+ *                              5 = 6 step
+ *                              6 = 7 step
+ *                              7 = 8 step --> maximum setup / minimum hold
+ *      continuousSyncEnable- SYNC Continuous:
+ *                              0 = Disable
+ *                              1 = Enable
+ *      pllFilterEnable     - PLL Filter Enable
+ *                              0 = Disable PLL Filter
+ *                              1 = Enable PLL Filter
+ *      pllFilterValue      - PLL Filter characteristics:
+ *                              0~7 (recommended value is 4)
+ *
+ *  Output:
+ *      0   - Success
+ *     -1   - Fail.
+ */
+long sii164InitChip(
+    unsigned char edgeSelect,
+    unsigned char busSelect,
+    unsigned char dualEdgeClkSelect,
+    unsigned char hsyncEnable,
+    unsigned char vsyncEnable,
+    unsigned char deskewEnable,
+    unsigned char deskewSetting,
+    unsigned char continuousSyncEnable,
+    unsigned char pllFilterEnable,
+    unsigned char pllFilterValue
+)
+{
+    //unsigned char ucRegIndex, ucRegValue;
+    //unsigned char ucDeviceAddress,
+	unsigned char config;
+    //unsigned long delayCount;
+
+    /* Initialize the i2c bus */
+#ifdef USE_HW_I2C
+    /* Use fast mode. */
+    hwI2CInit(1);
+#else
+    swI2CInit(DEFAULT_I2C_SCL, DEFAULT_I2C_SDA);
+#endif
+
+    /* Check if SII164 Chip exists */
+    if ((sii164GetVendorID() == SII164_VENDOR_ID) && (sii164GetDeviceID() == SII164_DEVICE_ID))
+    {
+
+#ifdef DDKDEBUG
+        //sii164PrintRegisterValues();
+#endif
+        /*
+         *  Initialize SII164 controller chip.
+         */
+
+        /* Select the edge */
+        if (edgeSelect == 0)
+            config = SII164_CONFIGURATION_LATCH_FALLING;
+        else
+            config = SII164_CONFIGURATION_LATCH_RISING;
+
+        /* Select bus wide */
+        if (busSelect == 0)
+            config |= SII164_CONFIGURATION_BUS_12BITS;
+        else
+            config |= SII164_CONFIGURATION_BUS_24BITS;
+
+        /* Select Dual/Single Edge Clock */
+        if (dualEdgeClkSelect == 0)
+            config |= SII164_CONFIGURATION_CLOCK_SINGLE;
+        else
+            config |= SII164_CONFIGURATION_CLOCK_DUAL;
+
+        /* Select HSync Enable */
+        if (hsyncEnable == 0)
+            config |= SII164_CONFIGURATION_HSYNC_FORCE_LOW;
+        else
+            config |= SII164_CONFIGURATION_HSYNC_AS_IS;
+
+        /* Select VSync Enable */
+        if (vsyncEnable == 0)
+            config |= SII164_CONFIGURATION_VSYNC_FORCE_LOW;
+        else
+            config |= SII164_CONFIGURATION_VSYNC_AS_IS;
+
+        i2cWriteReg(SII164_I2C_ADDRESS, SII164_CONFIGURATION, config);
+
+        /* De-skew enabled with default 111b value.
+           This will fix some artifacts problem in some mode on board 2.2.
+           Somehow this fix does not affect board 2.1.
+         */
+        if (deskewEnable == 0)
+            config = SII164_DESKEW_DISABLE;
+        else
+            config = SII164_DESKEW_ENABLE;
+
+        switch (deskewSetting)
+        {
+            case 0:
+                config |= SII164_DESKEW_1_STEP;
+                break;
+            case 1:
+                config |= SII164_DESKEW_2_STEP;
+                break;
+            case 2:
+                config |= SII164_DESKEW_3_STEP;
+                break;
+            case 3:
+                config |= SII164_DESKEW_4_STEP;
+                break;
+            case 4:
+                config |= SII164_DESKEW_5_STEP;
+                break;
+            case 5:
+                config |= SII164_DESKEW_6_STEP;
+                break;
+            case 6:
+                config |= SII164_DESKEW_7_STEP;
+                break;
+            case 7:
+                config |= SII164_DESKEW_8_STEP;
+                break;
+        }
+        i2cWriteReg(SII164_I2C_ADDRESS, SII164_DESKEW, config);
+
+        /* Enable/Disable Continuous Sync. */
+        if (continuousSyncEnable == 0)
+            config = SII164_PLL_FILTER_SYNC_CONTINUOUS_DISABLE;
+        else
+            config = SII164_PLL_FILTER_SYNC_CONTINUOUS_ENABLE;
+
+        /* Enable/Disable PLL Filter */
+        if (pllFilterEnable == 0)
+            config |= SII164_PLL_FILTER_DISABLE;
+        else
+            config |= SII164_PLL_FILTER_ENABLE;
+
+        /* Set the PLL Filter value */
+        config |= ((pllFilterValue & 0x07) << 1);
+
+        i2cWriteReg(SII164_I2C_ADDRESS, SII164_PLL, config);
+
+        /* Recover from Power Down and enable output. */
+        config = i2cReadReg(SII164_I2C_ADDRESS, SII164_CONFIGURATION);
+        config |= SII164_CONFIGURATION_POWER_NORMAL;
+        i2cWriteReg(SII164_I2C_ADDRESS, SII164_CONFIGURATION, config);
+
+#ifdef DDKDEBUG
+        //sii164PrintRegisterValues();
+#endif
+
+        return 0;
+    }
+
+    /* Return -1 if initialization fails. */
+    return (-1);
+}
+
+
+
+
+
+/* below sii164 function is not neccessary */
+
+#ifdef SII164_FULL_FUNCTIONS
+
+/*
+ *  sii164ResetChip
+ *      This function resets the DVI Controller Chip.
+ */
+void sii164ResetChip(void)
+{
+    /* Power down */
+    sii164SetPower(0);
+    sii164SetPower(1);
+}
+
+
+/*
+ * sii164GetChipString
+ *      This function returns a char string name of the current DVI Controller chip.
+ *      It's convenient for application need to display the chip name.
+ */
+char *sii164GetChipString(void)
+{
+    return gDviCtrlChipName;
+}
+
+
+/*
+ *  sii164SetPower
+ *      This function sets the power configuration of the DVI Controller Chip.
+ *
+ *  Input:
+ *      powerUp - Flag to set the power down or up
+ */
+void sii164SetPower(
+    unsigned char powerUp
+)
+{
+    unsigned char config;
+
+    config = i2cReadReg(SII164_I2C_ADDRESS, SII164_CONFIGURATION);
+    if (powerUp == 1)
+    {
+        /* Power up the chip */
+        config &= ~SII164_CONFIGURATION_POWER_MASK;
+        config |= SII164_CONFIGURATION_POWER_NORMAL;
+        i2cWriteReg(SII164_I2C_ADDRESS, SII164_CONFIGURATION, config);
+    }
+    else
+    {
+        /* Power down the chip */
+        config &= ~SII164_CONFIGURATION_POWER_MASK;
+        config |= SII164_CONFIGURATION_POWER_DOWN;
+        i2cWriteReg(SII164_I2C_ADDRESS, SII164_CONFIGURATION, config);
+    }
+}
+
+
+/*
+ *  sii164SelectHotPlugDetectionMode
+ *      This function selects the mode of the hot plug detection.
+ */
+static void sii164SelectHotPlugDetectionMode(
+    sii164_hot_plug_mode_t hotPlugMode
+)
+{
+    unsigned char detectReg;
+
+    detectReg = i2cReadReg(SII164_I2C_ADDRESS, SII164_DETECT) & ~SII164_DETECT_MONITOR_SENSE_OUTPUT_FLAG;
+    switch (hotPlugMode)
+    {
+        case SII164_HOTPLUG_DISABLE:
+            detectReg |= SII164_DETECT_MONITOR_SENSE_OUTPUT_HIGH;
+            break;
+        case SII164_HOTPLUG_USE_MDI:
+            detectReg &= ~SII164_DETECT_INTERRUPT_MASK;
+            detectReg |= SII164_DETECT_INTERRUPT_BY_HTPLG_PIN;
+            detectReg |= SII164_DETECT_MONITOR_SENSE_OUTPUT_MDI;
+            break;
+        case SII164_HOTPLUG_USE_RSEN:
+            detectReg |= SII164_DETECT_MONITOR_SENSE_OUTPUT_RSEN;
+            break;
+        case SII164_HOTPLUG_USE_HTPLG:
+            detectReg |= SII164_DETECT_MONITOR_SENSE_OUTPUT_HTPLG;
+            break;
+    }
+
+    i2cWriteReg(SII164_I2C_ADDRESS, SII164_DETECT, detectReg);
+}
+
+/*
+ *  sii164EnableHotPlugDetection
+ *      This function enables the Hot Plug detection.
+ *
+ *  enableHotPlug   - Enable (=1) / disable (=0) Hot Plug detection
+ */
+void sii164EnableHotPlugDetection(
+    unsigned char enableHotPlug
+)
+{
+    unsigned char detectReg;
+    detectReg = i2cReadReg(SII164_I2C_ADDRESS, SII164_DETECT);
+
+    /* Depending on each DVI controller, need to enable the hot plug based on each
+       individual chip design. */
+    if (enableHotPlug != 0)
+        sii164SelectHotPlugDetectionMode(SII164_HOTPLUG_USE_MDI);
+    else
+        sii164SelectHotPlugDetectionMode(SII164_HOTPLUG_DISABLE);
+}
+
+/*
+ *  sii164IsConnected
+ *      Check if the DVI Monitor is connected.
+ *
+ *  Output:
+ *      0   - Not Connected
+ *      1   - Connected
+ */
+unsigned char sii164IsConnected(void)
+{
+    unsigned char hotPlugValue;
+
+    hotPlugValue = i2cReadReg(SII164_I2C_ADDRESS, SII164_DETECT) & SII164_DETECT_HOT_PLUG_STATUS_MASK;
+    if (hotPlugValue == SII164_DETECT_HOT_PLUG_STATUS_ON)
+        return 1;
+    else
+        return 0;
+}
+
+/*
+ *  sii164CheckInterrupt
+ *      Checks if interrupt has occured.
+ *
+ *  Output:
+ *      0   - No interrupt
+ *      1   - Interrupt occurs
+ */
+unsigned char sii164CheckInterrupt(void)
+{
+    unsigned char detectReg;
+
+    detectReg = i2cReadReg(SII164_I2C_ADDRESS, SII164_DETECT) & SII164_DETECT_MONITOR_STATE_MASK;
+    if (detectReg == SII164_DETECT_MONITOR_STATE_CHANGE)
+        return 1;
+    else
+        return 0;
+}
+
+/*
+ *  sii164ClearInterrupt
+ *      Clear the hot plug interrupt.
+ */
+void sii164ClearInterrupt(void)
+{
+    unsigned char detectReg;
+
+    /* Clear the MDI interrupt */
+    detectReg = i2cReadReg(SII164_I2C_ADDRESS, SII164_DETECT);
+    i2cWriteReg(SII164_I2C_ADDRESS, SII164_DETECT, detectReg | SII164_DETECT_MONITOR_STATE_CLEAR);
+}
+
+#endif
+
+#endif
+
+
diff --git a/drivers/staging/sm750fb/ddk750_sii164.h b/drivers/staging/sm750fb/ddk750_sii164.h
new file mode 100644
index 000000000000..2b4c7d3381df
--- /dev/null
+++ b/drivers/staging/sm750fb/ddk750_sii164.h
@@ -0,0 +1,172 @@
+#ifndef DDK750_SII164_H__
+#define DDK750_SII164_H__
+
+#define USE_DVICHIP
+
+/* Hot Plug detection mode structure */
+typedef enum _sii164_hot_plug_mode_t
+{
+    SII164_HOTPLUG_DISABLE = 0,         /* Disable Hot Plug output bit (always high). */
+    SII164_HOTPLUG_USE_MDI,             /* Use Monitor Detect Interrupt bit. */
+    SII164_HOTPLUG_USE_RSEN,            /* Use Receiver Sense detect bit. */
+    SII164_HOTPLUG_USE_HTPLG            /* Use Hot Plug detect bit. */
+} sii164_hot_plug_mode_t;
+
+
+/* Silicon Image SiI164 chip prototype */
+long sii164InitChip(
+    unsigned char edgeSelect,
+    unsigned char busSelect,
+    unsigned char dualEdgeClkSelect,
+    unsigned char hsyncEnable,
+    unsigned char vsyncEnable,
+    unsigned char deskewEnable,
+    unsigned char deskewSetting,
+    unsigned char continuousSyncEnable,
+    unsigned char pllFilterEnable,
+    unsigned char pllFilterValue
+);
+
+unsigned short sii164GetVendorID(void);
+unsigned short sii164GetDeviceID(void);
+
+
+#ifdef SII164_FULL_FUNCTIONS
+void sii164ResetChip(void);
+char *sii164GetChipString(void);
+void sii164SetPower(unsigned char powerUp);
+void sii164EnableHotPlugDetection(unsigned char enableHotPlug);
+unsigned char sii164IsConnected(void);
+unsigned char sii164CheckInterrupt(void);
+void sii164ClearInterrupt(void);
+#endif
+/* below register definination is used for Silicon Image SiI164 DVI controller chip */
+/*
+ * Vendor ID registers
+ */
+#define SII164_VENDOR_ID_LOW                        0x00
+#define SII164_VENDOR_ID_HIGH                       0x01
+
+/*
+ * Device ID registers
+ */
+#define SII164_DEVICE_ID_LOW                        0x02
+#define SII164_DEVICE_ID_HIGH                       0x03
+
+/*
+ * Device Revision
+ */
+#define SII164_DEVICE_REVISION                      0x04
+
+/*
+ * Frequency Limitation registers
+ */
+#define SII164_FREQUENCY_LIMIT_LOW                  0x06
+#define SII164_FREQUENCY_LIMIT_HIGH                 0x07
+
+/*
+ * Power Down and Input Signal Configuration registers
+ */
+#define SII164_CONFIGURATION                        0x08
+
+/* Power down (PD) */
+#define SII164_CONFIGURATION_POWER_DOWN             0x00
+#define SII164_CONFIGURATION_POWER_NORMAL           0x01
+#define SII164_CONFIGURATION_POWER_MASK             0x01
+
+/* Input Edge Latch Select (EDGE) */
+#define SII164_CONFIGURATION_LATCH_FALLING          0x00
+#define SII164_CONFIGURATION_LATCH_RISING           0x02
+
+/* Bus Select (BSEL) */
+#define SII164_CONFIGURATION_BUS_12BITS             0x00
+#define SII164_CONFIGURATION_BUS_24BITS             0x04
+
+/* Dual Edge Clock Select (DSEL) */
+#define SII164_CONFIGURATION_CLOCK_SINGLE           0x00
+#define SII164_CONFIGURATION_CLOCK_DUAL             0x08
+
+/* Horizontal Sync Enable (HEN) */
+#define SII164_CONFIGURATION_HSYNC_FORCE_LOW        0x00
+#define SII164_CONFIGURATION_HSYNC_AS_IS            0x10
+
+/* Vertical Sync Enable (VEN) */
+#define SII164_CONFIGURATION_VSYNC_FORCE_LOW        0x00
+#define SII164_CONFIGURATION_VSYNC_AS_IS            0x20
+
+/*
+ * Detection registers
+ */
+#define SII164_DETECT                               0x09
+
+/* Monitor Detect Interrupt (MDI) */
+#define SII164_DETECT_MONITOR_STATE_CHANGE          0x00
+#define SII164_DETECT_MONITOR_STATE_NO_CHANGE       0x01
+#define SII164_DETECT_MONITOR_STATE_CLEAR           0x01
+#define SII164_DETECT_MONITOR_STATE_MASK            0x01
+
+/* Hot Plug detect Input (HTPLG) */
+#define SII164_DETECT_HOT_PLUG_STATUS_OFF           0x00
+#define SII164_DETECT_HOT_PLUG_STATUS_ON            0x02
+#define SII164_DETECT_HOT_PLUG_STATUS_MASK          0x02
+
+/* Receiver Sense (RSEN) */
+#define SII164_DETECT_RECEIVER_SENSE_NOT_DETECTED   0x00
+#define SII164_DETECT_RECEIVER_SENSE_DETECTED       0x04
+
+/* Interrupt Generation Method (TSEL) */
+#define SII164_DETECT_INTERRUPT_BY_RSEN_PIN         0x00
+#define SII164_DETECT_INTERRUPT_BY_HTPLG_PIN        0x08
+#define SII164_DETECT_INTERRUPT_MASK                0x08
+
+/* Monitor Sense Output (MSEN) */
+#define SII164_DETECT_MONITOR_SENSE_OUTPUT_HIGH     0x00
+#define SII164_DETECT_MONITOR_SENSE_OUTPUT_MDI      0x10
+#define SII164_DETECT_MONITOR_SENSE_OUTPUT_RSEN     0x20
+#define SII164_DETECT_MONITOR_SENSE_OUTPUT_HTPLG    0x30
+#define SII164_DETECT_MONITOR_SENSE_OUTPUT_FLAG     0x30
+
+/*
+ * Skewing registers
+ */
+#define SII164_DESKEW                               0x0A
+
+/* General Purpose Input (CTL[3:1]) */
+#define SII164_DESKEW_GENERAL_PURPOSE_INPUT_MASK    0x0E
+
+/* De-skewing Enable bit (DKEN) */
+#define SII164_DESKEW_DISABLE                       0x00
+#define SII164_DESKEW_ENABLE                        0x10
+
+/* De-skewing Setting (DK[3:1])*/
+#define SII164_DESKEW_1_STEP                        0x00
+#define SII164_DESKEW_2_STEP                        0x20
+#define SII164_DESKEW_3_STEP                        0x40
+#define SII164_DESKEW_4_STEP                        0x60
+#define SII164_DESKEW_5_STEP                        0x80
+#define SII164_DESKEW_6_STEP                        0xA0
+#define SII164_DESKEW_7_STEP                        0xC0
+#define SII164_DESKEW_8_STEP                        0xE0
+
+/*
+ * User Configuration Data registers (CFG 7:0)
+ */
+#define SII164_USER_CONFIGURATION                   0x0B
+
+/*
+ * PLL registers
+ */
+#define SII164_PLL                                  0x0C
+
+/* PLL Filter Value (PLLF) */
+#define SII164_PLL_FILTER_VALUE_MASK                0x0E
+
+/* PLL Filter Enable (PFEN) */
+#define SII164_PLL_FILTER_DISABLE                   0x00
+#define SII164_PLL_FILTER_ENABLE                    0x01
+
+/* Sync Continuous (SCNT) */
+#define SII164_PLL_FILTER_SYNC_CONTINUOUS_DISABLE   0x00
+#define SII164_PLL_FILTER_SYNC_CONTINUOUS_ENABLE    0x80
+
+#endif
diff --git a/drivers/staging/sm750fb/ddk750_swi2c.c b/drivers/staging/sm750fb/ddk750_swi2c.c
new file mode 100644
index 000000000000..901b3737f1ed
--- /dev/null
+++ b/drivers/staging/sm750fb/ddk750_swi2c.c
@@ -0,0 +1,522 @@
+/*******************************************************************
+*
+*         Copyright (c) 2007 by Silicon Motion, Inc. (SMI)
+*
+*  All rights are reserved. Reproduction or in part is prohibited
+*  without the written consent of the copyright owner.
+*
+*  swi2c.c --- SM750/SM718 DDK
+*  This file contains the source code for I2C using software
+*  implementation.
+*
+*******************************************************************/
+#include "ddk750_help.h"
+#include "ddk750_reg.h"
+#include "ddk750_swi2c.h"
+#include "ddk750_power.h"
+
+
+/*******************************************************************
+ * I2C Software Master Driver:
+ * ===========================
+ * Each i2c cycle is split into 4 sections. Each of these section marks
+ * a point in time where the SCL or SDA may be changed.
+ *
+ * 1 Cycle == |  Section I. |  Section 2. |  Section 3. |  Section 4. |
+ *            +-------------+-------------+-------------+-------------+
+ *            | SCL set LOW |SCL no change| SCL set HIGH|SCL no change|
+ *
+ *                                          ____________ _____________
+ * SCL == XXXX _____________ ____________ /
+ *
+ * I.e. the SCL may only be changed in section 1. and section 3. while
+ * the SDA may only be changed in section 2. and section 4. The table
+ * below gives the changes for these 2 lines in the varios sections.
+ *
+ * Section changes Table:
+ * ======================
+ * blank = no change, L = set bit LOW, H = set bit HIGH
+ *
+ *                                | 1.| 2.| 3.| 4.|
+ *                 ---------------+---+---+---+---+
+ *                 Tx Start   SDA |   | H |   | L |
+ *                            SCL | L |   | H |   |
+ *                 ---------------+---+---+---+---+
+ *                 Tx Stop    SDA |   | L |   | H |
+ *                            SCL | L |   | H |   |
+ *                 ---------------+---+---+---+---+
+ *                 Tx bit H   SDA |   | H |   |   |
+ *                            SCL | L |   | H |   |
+ *                 ---------------+---+---+---+---+
+ *                 Tx bit L   SDA |   | L |   |   |
+ *                            SCL | L |   | H |   |
+ *                 ---------------+---+---+---+---+
+ *
+ ******************************************************************/
+
+/* GPIO pins used for this I2C. It ranges from 0 to 63. */
+static unsigned char g_i2cClockGPIO = DEFAULT_I2C_SCL;
+static unsigned char g_i2cDataGPIO = DEFAULT_I2C_SDA;
+
+/*
+ *  Below is the variable declaration for the GPIO pin register usage
+ *  for the i2c Clock and i2c Data.
+ *
+ *  Note:
+ *      Notice that the GPIO usage for the i2c clock and i2c Data are
+ *      separated. This is to make this code flexible enough when
+ *      two separate GPIO pins for the clock and data are located
+ *      in two different GPIO register set (worst case).
+ */
+
+/* i2c Clock GPIO Register usage */
+static unsigned long g_i2cClkGPIOMuxReg = GPIO_MUX;
+static unsigned long g_i2cClkGPIODataReg = GPIO_DATA;
+static unsigned long g_i2cClkGPIODataDirReg = GPIO_DATA_DIRECTION;
+
+/* i2c Data GPIO Register usage */
+static unsigned long g_i2cDataGPIOMuxReg = GPIO_MUX;
+static unsigned long g_i2cDataGPIODataReg = GPIO_DATA;
+static unsigned long g_i2cDataGPIODataDirReg = GPIO_DATA_DIRECTION;
+
+/*
+ *  This function puts a delay between command
+ */
+static void swI2CWait(void)
+{
+	/* find a bug:
+	 * peekIO method works well before suspend/resume
+	 * but after suspend, peekIO(0x3ce,0x61) & 0x10
+	 * always be non-zero,which makes the while loop
+	 * never finish.
+	 * use non-ultimate for loop below is safe
+	 * */
+#if 0
+    /* Change wait algorithm to use PCI bus clock,
+       it's more reliable than counter loop ..
+       write 0x61 to 0x3ce and read from 0x3cf
+       */
+	while(peekIO(0x3ce,0x61) & 0x10);
+#else
+    int i, Temp;
+
+    for(i=0; i<600; i++)
+    {
+        Temp = i;
+        Temp += i;
+    }
+#endif
+}
+
+/*
+ *  This function set/reset the SCL GPIO pin
+ *
+ *  Parameters:
+ *      value    - Bit value to set to the SCL or SDA (0 = low, 1 = high)
+ *
+ *  Notes:
+ *      When setting SCL to high, just set the GPIO as input where the pull up
+ *      resistor will pull the signal up. Do not use software to pull up the
+ *      signal because the i2c will fail when other device try to drive the
+ *      signal due to SM50x will drive the signal to always high.
+ */
+void swI2CSCL(unsigned char value)
+{
+    unsigned long ulGPIOData;
+    unsigned long ulGPIODirection;
+
+    ulGPIODirection = PEEK32(g_i2cClkGPIODataDirReg);
+    if (value)      /* High */
+    {
+        /* Set direction as input. This will automatically pull the signal up. */
+        ulGPIODirection &= ~(1 << g_i2cClockGPIO);
+        POKE32(g_i2cClkGPIODataDirReg, ulGPIODirection);
+    }
+    else            /* Low */
+    {
+        /* Set the signal down */
+        ulGPIOData = PEEK32(g_i2cClkGPIODataReg);
+        ulGPIOData &= ~(1 << g_i2cClockGPIO);
+        POKE32(g_i2cClkGPIODataReg, ulGPIOData);
+
+        /* Set direction as output */
+        ulGPIODirection |= (1 << g_i2cClockGPIO);
+        POKE32(g_i2cClkGPIODataDirReg, ulGPIODirection);
+    }
+}
+
+/*
+ *  This function set/reset the SDA GPIO pin
+ *
+ *  Parameters:
+ *      value    - Bit value to set to the SCL or SDA (0 = low, 1 = high)
+ *
+ *  Notes:
+ *      When setting SCL to high, just set the GPIO as input where the pull up
+ *      resistor will pull the signal up. Do not use software to pull up the
+ *      signal because the i2c will fail when other device try to drive the
+ *      signal due to SM50x will drive the signal to always high.
+ */
+void swI2CSDA(unsigned char value)
+{
+    unsigned long ulGPIOData;
+    unsigned long ulGPIODirection;
+
+    ulGPIODirection = PEEK32(g_i2cDataGPIODataDirReg);
+    if (value)      /* High */
+    {
+        /* Set direction as input. This will automatically pull the signal up. */
+        ulGPIODirection &= ~(1 << g_i2cDataGPIO);
+        POKE32(g_i2cDataGPIODataDirReg, ulGPIODirection);
+    }
+    else            /* Low */
+    {
+        /* Set the signal down */
+        ulGPIOData = PEEK32(g_i2cDataGPIODataReg);
+        ulGPIOData &= ~(1 << g_i2cDataGPIO);
+        POKE32(g_i2cDataGPIODataReg, ulGPIOData);
+
+        /* Set direction as output */
+        ulGPIODirection |= (1 << g_i2cDataGPIO);
+        POKE32(g_i2cDataGPIODataDirReg, ulGPIODirection);
+    }
+}
+
+/*
+ *  This function read the data from the SDA GPIO pin
+ *
+ *  Return Value:
+ *      The SDA data bit sent by the Slave
+ */
+static unsigned char swI2CReadSDA(void)
+{
+    unsigned long ulGPIODirection;
+    unsigned long ulGPIOData;
+
+    /* Make sure that the direction is input (High) */
+    ulGPIODirection = PEEK32(g_i2cDataGPIODataDirReg);
+    if ((ulGPIODirection & (1 << g_i2cDataGPIO)) != (~(1 << g_i2cDataGPIO)))
+    {
+        ulGPIODirection &= ~(1 << g_i2cDataGPIO);
+        POKE32(g_i2cDataGPIODataDirReg, ulGPIODirection);
+    }
+
+    /* Now read the SDA line */
+    ulGPIOData = PEEK32(g_i2cDataGPIODataReg);
+    if (ulGPIOData & (1 << g_i2cDataGPIO))
+        return 1;
+    else
+        return 0;
+}
+
+/*
+ *  This function sends ACK signal
+ */
+static void swI2CAck(void)
+{
+    return;  /* Single byte read is ok without it. */
+}
+
+/*
+ *  This function sends the start command to the slave device
+ */
+static void swI2CStart(void)
+{
+    /* Start I2C */
+    swI2CSDA(1);
+    swI2CSCL(1);
+    swI2CSDA(0);
+}
+
+/*
+ *  This function sends the stop command to the slave device
+ */
+static void swI2CStop(void)
+{
+    /* Stop the I2C */
+    swI2CSCL(1);
+    swI2CSDA(0);
+    swI2CSDA(1);
+}
+
+/*
+ *  This function writes one byte to the slave device
+ *
+ *  Parameters:
+ *      data    - Data to be write to the slave device
+ *
+ *  Return Value:
+ *       0   - Success
+ *      -1   - Fail to write byte
+ */
+static long swI2CWriteByte(unsigned char data)
+{
+    unsigned char value = data;
+    int i;
+
+    /* Sending the data bit by bit */
+    for (i=0; i<8; i++)
+    {
+        /* Set SCL to low */
+        swI2CSCL(0);
+
+        /* Send data bit */
+        if ((value & 0x80) != 0)
+            swI2CSDA(1);
+        else
+            swI2CSDA(0);
+
+        swI2CWait();
+
+        /* Toggle clk line to one */
+        swI2CSCL(1);
+        swI2CWait();
+
+        /* Shift byte to be sent */
+        value = value << 1;
+    }
+
+    /* Set the SCL Low and SDA High (prepare to get input) */
+    swI2CSCL(0);
+    swI2CSDA(1);
+
+    /* Set the SCL High for ack */
+    swI2CWait();
+    swI2CSCL(1);
+    swI2CWait();
+
+    /* Read SDA, until SDA==0 */
+    for(i=0; i<0xff; i++)
+    {
+        if (!swI2CReadSDA())
+            break;
+
+        swI2CSCL(0);
+        swI2CWait();
+        swI2CSCL(1);
+        swI2CWait();
+    }
+
+    /* Set the SCL Low and SDA High */
+    swI2CSCL(0);
+    swI2CSDA(1);
+
+    if (i<0xff)
+        return 0;
+    else
+        return -1;
+}
+
+/*
+ *  This function reads one byte from the slave device
+ *
+ *  Parameters:
+ *      ack    - Flag to indicate either to send the acknowledge
+ *            message to the slave device or not
+ *
+ *  Return Value:
+ *      One byte data read from the Slave device
+ */
+static unsigned char swI2CReadByte(unsigned char ack)
+{
+    int i;
+    unsigned char data = 0;
+
+    for(i=7; i>=0; i--)
+    {
+        /* Set the SCL to Low and SDA to High (Input) */
+        swI2CSCL(0);
+        swI2CSDA(1);
+        swI2CWait();
+
+        /* Set the SCL High */
+        swI2CSCL(1);
+        swI2CWait();
+
+        /* Read data bits from SDA */
+        data |= (swI2CReadSDA() << i);
+    }
+
+    if (ack)
+        swI2CAck();
+
+    /* Set the SCL Low and SDA High */
+    swI2CSCL(0);
+    swI2CSDA(1);
+
+    return data;
+}
+
+/*
+ * This function initializes GPIO port for SW I2C communication.
+ *
+ * Parameters:
+ *      i2cClkGPIO      - The GPIO pin to be used as i2c SCL
+ *      i2cDataGPIO     - The GPIO pin to be used as i2c SDA
+ *
+ * Return Value:
+ *      -1   - Fail to initialize the i2c
+ *       0   - Success
+ */
+static long swI2CInit_SM750LE(unsigned char i2cClkGPIO,
+			      unsigned char i2cDataGPIO)
+{
+    int i;
+
+    /* Initialize the GPIO pin for the i2c Clock Register */
+    g_i2cClkGPIODataReg = GPIO_DATA_SM750LE;
+    g_i2cClkGPIODataDirReg = GPIO_DATA_DIRECTION_SM750LE;
+
+    /* Initialize the Clock GPIO Offset */
+    g_i2cClockGPIO = i2cClkGPIO;
+
+    /* Initialize the GPIO pin for the i2c Data Register */
+    g_i2cDataGPIODataReg = GPIO_DATA_SM750LE;
+    g_i2cDataGPIODataDirReg = GPIO_DATA_DIRECTION_SM750LE;
+
+    /* Initialize the Data GPIO Offset */
+    g_i2cDataGPIO = i2cDataGPIO;
+
+    /* Note that SM750LE don't have GPIO MUX and power is always on */
+
+    /* Clear the i2c lines. */
+    for(i=0; i<9; i++)
+        swI2CStop();
+
+    return 0;
+}
+
+/*
+ * This function initializes the i2c attributes and bus
+ *
+ * Parameters:
+ *      i2cClkGPIO      - The GPIO pin to be used as i2c SCL
+ *      i2cDataGPIO     - The GPIO pin to be used as i2c SDA
+ *
+ * Return Value:
+ *      -1   - Fail to initialize the i2c
+ *       0   - Success
+ */
+long swI2CInit(
+    unsigned char i2cClkGPIO,
+    unsigned char i2cDataGPIO
+)
+{
+    int i;
+
+    /* Return 0 if the GPIO pins to be used is out of range. The range is only from [0..63] */
+    if ((i2cClkGPIO > 31) || (i2cDataGPIO > 31))
+        return -1;
+
+    if (getChipType() == SM750LE)
+        return swI2CInit_SM750LE(i2cClkGPIO, i2cDataGPIO);
+
+    /* Initialize the GPIO pin for the i2c Clock Register */
+    g_i2cClkGPIOMuxReg = GPIO_MUX;
+    g_i2cClkGPIODataReg = GPIO_DATA;
+    g_i2cClkGPIODataDirReg = GPIO_DATA_DIRECTION;
+
+    /* Initialize the Clock GPIO Offset */
+    g_i2cClockGPIO = i2cClkGPIO;
+
+    /* Initialize the GPIO pin for the i2c Data Register */
+    g_i2cDataGPIOMuxReg = GPIO_MUX;
+    g_i2cDataGPIODataReg = GPIO_DATA;
+    g_i2cDataGPIODataDirReg = GPIO_DATA_DIRECTION;
+
+    /* Initialize the Data GPIO Offset */
+    g_i2cDataGPIO = i2cDataGPIO;
+
+    /* Enable the GPIO pins for the i2c Clock and Data (GPIO MUX) */
+    POKE32(g_i2cClkGPIOMuxReg,
+                      PEEK32(g_i2cClkGPIOMuxReg) & ~(1 << g_i2cClockGPIO));
+    POKE32(g_i2cDataGPIOMuxReg,
+                      PEEK32(g_i2cDataGPIOMuxReg) & ~(1 << g_i2cDataGPIO));
+
+    /* Enable GPIO power */
+    enableGPIO(1);
+
+    /* Clear the i2c lines. */
+    for(i=0; i<9; i++)
+        swI2CStop();
+
+    return 0;
+}
+
+/*
+ *  This function reads the slave device's register
+ *
+ *  Parameters:
+ *      deviceAddress   - i2c Slave device address which register
+ *                        to be read from
+ *      registerIndex   - Slave device's register to be read
+ *
+ *  Return Value:
+ *      Register value
+ */
+unsigned char swI2CReadReg(
+    unsigned char deviceAddress,
+    unsigned char registerIndex
+)
+{
+    unsigned char data;
+
+    /* Send the Start signal */
+    swI2CStart();
+
+    /* Send the device address */
+    swI2CWriteByte(deviceAddress);
+
+    /* Send the register index */
+    swI2CWriteByte(registerIndex);
+
+    /* Get the bus again and get the data from the device read address */
+    swI2CStart();
+    swI2CWriteByte(deviceAddress + 1);
+    data = swI2CReadByte(1);
+
+    /* Stop swI2C and release the bus */
+    swI2CStop();
+
+    return data;
+}
+
+/*
+ *  This function writes a value to the slave device's register
+ *
+ *  Parameters:
+ *      deviceAddress   - i2c Slave device address which register
+ *                        to be written
+ *      registerIndex   - Slave device's register to be written
+ *      data            - Data to be written to the register
+ *
+ *  Result:
+ *          0   - Success
+ *         -1   - Fail
+ */
+long swI2CWriteReg(
+    unsigned char deviceAddress,
+    unsigned char registerIndex,
+    unsigned char data
+)
+{
+    long returnValue = 0;
+
+    /* Send the Start signal */
+    swI2CStart();
+
+    /* Send the device address and read the data. All should return success
+       in order for the writing processed to be successful
+     */
+    if ((swI2CWriteByte(deviceAddress) != 0) ||
+        (swI2CWriteByte(registerIndex) != 0) ||
+        (swI2CWriteByte(data) != 0))
+    {
+        returnValue = -1;
+    }
+
+    /* Stop i2c and release the bus */
+    swI2CStop();
+
+    return returnValue;
+}
diff --git a/drivers/staging/sm750fb/ddk750_swi2c.h b/drivers/staging/sm750fb/ddk750_swi2c.h
new file mode 100644
index 000000000000..ec5463b98ddf
--- /dev/null
+++ b/drivers/staging/sm750fb/ddk750_swi2c.h
@@ -0,0 +1,92 @@
+/*******************************************************************
+*
+*         Copyright (c) 2007 by Silicon Motion, Inc. (SMI)
+*
+*  All rights are reserved. Reproduction or in part is prohibited
+*  without the written consent of the copyright owner.
+*
+*  swi2c.h --- SM750/SM718 DDK
+*  This file contains the definitions for i2c using software
+*  implementation.
+*
+*******************************************************************/
+#ifndef _SWI2C_H_
+#define _SWI2C_H_
+
+/* Default i2c CLK and Data GPIO. These are the default i2c pins */
+#define DEFAULT_I2C_SCL                     30
+#define DEFAULT_I2C_SDA                     31
+
+/*
+ * This function initializes the i2c attributes and bus
+ *
+ * Parameters:
+ *      i2cClkGPIO  - The GPIO pin to be used as i2c SCL
+ *      i2cDataGPIO - The GPIO pin to be used as i2c SDA
+ *
+ * Return Value:
+ *      -1   - Fail to initialize the i2c
+ *       0   - Success
+ */
+long swI2CInit(
+    unsigned char i2cClkGPIO,
+    unsigned char i2cDataGPIO
+);
+
+/*
+ *  This function reads the slave device's register
+ *
+ *  Parameters:
+ *      deviceAddress   - i2c Slave device address which register
+ *                        to be read from
+ *      registerIndex   - Slave device's register to be read
+ *
+ *  Return Value:
+ *      Register value
+ */
+unsigned char swI2CReadReg(
+    unsigned char deviceAddress,
+    unsigned char registerIndex
+);
+
+/*
+ *  This function writes a value to the slave device's register
+ *
+ *  Parameters:
+ *      deviceAddress   - i2c Slave device address which register
+ *                        to be written
+ *      registerIndex   - Slave device's register to be written
+ *      data            - Data to be written to the register
+ *
+ *  Result:
+ *          0   - Success
+ *         -1   - Fail
+ */
+long swI2CWriteReg(
+    unsigned char deviceAddress,
+    unsigned char registerIndex,
+    unsigned char data
+);
+
+/*
+ *  These two functions are used to toggle the data on the SCL and SDA I2C lines.
+ *  The used of these two functions are not recommended unless it is necessary.
+ */
+
+/*
+ *  This function set/reset the SCL GPIO pin
+ *
+ *  Parameters:
+ *      value	- Bit value to set to the SCL or SDA (0 = low, 1 = high)
+ */
+void swI2CSCL(unsigned char value);
+
+/*
+ *  This function set/reset the SDA GPIO pin
+ *
+ *  Parameters:
+ *      value	- Bit value to set to the SCL or SDA (0 = low, 1 = high)
+ */
+void swI2CSDA(unsigned char value);
+
+#endif  /* _SWI2C_H_ */
diff --git a/drivers/staging/sm750fb/modedb.h b/drivers/staging/sm750fb/modedb.h
new file mode 100644
index 000000000000..c5275c6fffaf
--- /dev/null
+++ b/drivers/staging/sm750fb/modedb.h
@@ -0,0 +1,221 @@
+
+static const struct fb_videomode modedb2[] = {
+    {
+	/* 640x400 @ 70 Hz, 31.5 kHz hsync */
+	NULL, 70, 640, 400, 39721, 40, 24, 39, 9, 96, 2,
+	0, FB_VMODE_NONINTERLACED
+    }, {
+	/* 640x480 @ 60 Hz, 31.5 kHz hsync */
+	NULL, 60, 640, 480, 39721, 40, 24, 32, 11, 96, 2,
+	0, FB_VMODE_NONINTERLACED
+    }, {
+	/* 800x600 @ 56 Hz, 35.15 kHz hsync */
+	NULL, 56, 800, 600, 27777, 128, 24, 22, 1, 72, 2,
+	0, FB_VMODE_NONINTERLACED
+    }, {
+	/* 1024x768 @ 87 Hz interlaced, 35.5 kHz hsync */
+	NULL, 87, 1024, 768, 22271, 56, 24, 33, 8, 160, 8,
+	0, FB_VMODE_INTERLACED
+    }, {
+	/* 640x400 @ 85 Hz, 37.86 kHz hsync */
+	NULL, 85, 640, 400, 31746, 96, 32, 41, 1, 64, 3,
+	FB_SYNC_VERT_HIGH_ACT, FB_VMODE_NONINTERLACED
+    }, {
+	/* 640x480 @ 72 Hz, 36.5 kHz hsync */
+	NULL, 72, 640, 480, 31746, 144, 40, 30, 8, 40, 3,
+	0, FB_VMODE_NONINTERLACED
+    }, {
+	/* 640x480 @ 75 Hz, 37.50 kHz hsync */
+	NULL, 75, 640, 480, 31746, 120, 16, 16, 1, 64, 3,
+	0, FB_VMODE_NONINTERLACED
+    }, {
+	/* 800x600 @ 60 Hz, 37.8 kHz hsync */
+	NULL, 60, 800, 600, 25000, 88, 40, 23, 1, 128, 4,
+	FB_SYNC_HOR_HIGH_ACT|FB_SYNC_VERT_HIGH_ACT, FB_VMODE_NONINTERLACED
+    }, {
+	/* 640x480 @ 85 Hz, 43.27 kHz hsync */
+	NULL, 85, 640, 480, 27777, 80, 56, 25, 1, 56, 3,
+	0, FB_VMODE_NONINTERLACED
+    }, {
+	/* 1152x864 @ 89 Hz interlaced, 44 kHz hsync */
+	NULL, 69, 1152, 864, 15384, 96, 16, 110, 1, 216, 10,
+	0, FB_VMODE_INTERLACED
+    }, {
+	/* 800x600 @ 72 Hz, 48.0 kHz hsync */
+	NULL, 72, 800, 600, 20000, 64, 56, 23, 37, 120, 6,
+	FB_SYNC_HOR_HIGH_ACT|FB_SYNC_VERT_HIGH_ACT, FB_VMODE_NONINTERLACED
+    }, {
+	/* 1024x768 @ 60 Hz, 48.4 kHz hsync */
+	NULL, 60, 1024, 768, 15384, 168, 8, 29, 3, 144, 6,
+	0, FB_VMODE_NONINTERLACED
+    }, {
+	/* 640x480 @ 100 Hz, 53.01 kHz hsync */
+	NULL, 100, 640, 480, 21834, 96, 32, 36, 8, 96, 6,
+	0, FB_VMODE_NONINTERLACED
+    }, {
+	/* 1152x864 @ 60 Hz, 53.5 kHz hsync */
+	NULL, 60, 1152, 864, 11123, 208, 64, 16, 4, 256, 8,
+	0, FB_VMODE_NONINTERLACED
+    }, {
+	/* 800x600 @ 85 Hz, 55.84 kHz hsync */
+	NULL, 85, 800, 600, 16460, 160, 64, 36, 16, 64, 5,
+	0, FB_VMODE_NONINTERLACED
+    }, {
+	/* 1024x768 @ 70 Hz, 56.5 kHz hsync */
+	NULL, 70, 1024, 768, 13333, 144, 24, 29, 3, 136, 6,
+	0, FB_VMODE_NONINTERLACED
+    }, {
+        /*  1280x960-60 VESA */
+        NULL, 60, 1280, 960, 9259, 312, 96, 36, 1, 112, 3,
+        FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,  FB_VMODE_NONINTERLACED, FB_MODE_IS_VESA
+    }, {
+        /*  1280x1024-60 VESA */
+        NULL, 60, 1280, 1024, 9259, 248, 48, 38, 1, 112, 3,
+        FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT, FB_VMODE_NONINTERLACED, FB_MODE_IS_VESA
+    }, {
+	/* 1280x1024 @ 87 Hz interlaced, 51 kHz hsync */
+	NULL, 87, 1280, 1024, 12500, 56, 16, 128, 1, 216, 12,
+	0, FB_VMODE_INTERLACED
+    }, {
+	/* 800x600 @ 100 Hz, 64.02 kHz hsync */
+	NULL, 100, 800, 600, 14357, 160, 64, 30, 4, 64, 6,
+	0, FB_VMODE_NONINTERLACED
+    }, {
+	/* 1024x768 @ 76 Hz, 62.5 kHz hsync */
+	NULL, 76, 1024, 768, 11764, 208, 8, 36, 16, 120, 3,
+	0, FB_VMODE_NONINTERLACED
+    }, {
+	/* 1152x864 @ 70 Hz, 62.4 kHz hsync */
+	NULL, 70, 1152, 864, 10869, 106, 56, 20, 1, 160, 10,
+	0, FB_VMODE_NONINTERLACED
+    }, {
+	/* 1280x1024 @ 61 Hz, 64.2 kHz hsync */
+	NULL, 61, 1280, 1024, 9090, 200, 48, 26, 1, 184, 3,
+	0, FB_VMODE_NONINTERLACED
+    }, {
+	/* 1400x1050 @ 60Hz, 63.9 kHz hsync */
+	NULL, 68, 1400, 1050, 9259, 136, 40, 13, 1, 112, 3,
+	0, FB_VMODE_NONINTERLACED
+    }, {
+	/* 1400x1050 @ 75,107 Hz, 82,392 kHz +hsync +vsync*/
+	NULL, 75, 1400, 1050, 9271, 120, 56, 13, 0, 112, 3,
+	FB_SYNC_HOR_HIGH_ACT|FB_SYNC_VERT_HIGH_ACT, FB_VMODE_NONINTERLACED
+    }, {
+	/* 1400x1050 @ 60 Hz, ? kHz +hsync +vsync*/
+        NULL, 60, 1400, 1050, 9259, 128, 40, 12, 0, 112, 3,
+	FB_SYNC_HOR_HIGH_ACT|FB_SYNC_VERT_HIGH_ACT, FB_VMODE_NONINTERLACED
+    }, {
+	/* 1024x768 @ 85 Hz, 70.24 kHz hsync */
+	NULL, 85, 1024, 768, 10111, 192, 32, 34, 14, 160, 6,
+	0, FB_VMODE_NONINTERLACED
+    }, {
+	/* 1152x864 @ 78 Hz, 70.8 kHz hsync */
+	NULL, 78, 1152, 864, 9090, 228, 88, 32, 0, 84, 12,
+	0, FB_VMODE_NONINTERLACED
+    }, {
+	/* 1280x1024 @ 70 Hz, 74.59 kHz hsync */
+	NULL, 70, 1280, 1024, 7905, 224, 32, 28, 8, 160, 8,
+	0, FB_VMODE_NONINTERLACED
+    }, {
+	/* 1600x1200 @ 60Hz, 75.00 kHz hsync */
+	NULL, 60, 1600, 1200, 6172, 304, 64, 46, 1, 192, 3,
+	FB_SYNC_HOR_HIGH_ACT|FB_SYNC_VERT_HIGH_ACT, FB_VMODE_NONINTERLACED
+    }, {
+	/* 1152x864 @ 84 Hz, 76.0 kHz hsync */
+	NULL, 84, 1152, 864, 7407, 184, 312, 32, 0, 128, 12,
+	0, FB_VMODE_NONINTERLACED
+    }, {
+	/* 1280x1024 @ 74 Hz, 78.85 kHz hsync */
+	NULL, 74, 1280, 1024, 7407, 256, 32, 34, 3, 144, 3,
+	0, FB_VMODE_NONINTERLACED
+    }, {
+	/* 1024x768 @ 100Hz, 80.21 kHz hsync */
+	NULL, 100, 1024, 768, 8658, 192, 32, 21, 3, 192, 10,
+	0, FB_VMODE_NONINTERLACED
+    }, {
+	/* 1280x1024 @ 76 Hz, 81.13 kHz hsync */
+	NULL, 76, 1280, 1024, 7407, 248, 32, 34, 3, 104, 3,
+	0, FB_VMODE_NONINTERLACED
+    }, {
+	/* 1600x1200 @ 70 Hz, 87.50 kHz hsync */
+	NULL, 70, 1600, 1200, 5291, 304, 64, 46, 1, 192, 3,
+	0, FB_VMODE_NONINTERLACED
+    }, {
+	/* 1152x864 @ 100 Hz, 89.62 kHz hsync */
+	NULL, 100, 1152, 864, 7264, 224, 32, 17, 2, 128, 19,
+	0, FB_VMODE_NONINTERLACED
+    }, {
+	/* 1280x1024 @ 85 Hz, 91.15 kHz hsync */
+	NULL, 85, 1280, 1024, 6349, 224, 64, 44, 1, 160, 3,
+	FB_SYNC_HOR_HIGH_ACT|FB_SYNC_VERT_HIGH_ACT, FB_VMODE_NONINTERLACED
+    }, {
+	/* 1600x1200 @ 75 Hz, 93.75 kHz hsync */
+	NULL, 75, 1600, 1200, 4938, 304, 64, 46, 1, 192, 3,
+	FB_SYNC_HOR_HIGH_ACT|FB_SYNC_VERT_HIGH_ACT, FB_VMODE_NONINTERLACED
+    }, {
+	/* 1600x1200 @ 85 Hz, 105.77 kHz hsync */
+	NULL, 85, 1600, 1200, 4545, 272, 16, 37, 4, 192, 3,
+	FB_SYNC_HOR_HIGH_ACT|FB_SYNC_VERT_HIGH_ACT, FB_VMODE_NONINTERLACED
+    }, {
+	/* 1280x1024 @ 100 Hz, 107.16 kHz hsync */
+	NULL, 100, 1280, 1024, 5502, 256, 32, 26, 7, 128, 15,
+	0, FB_VMODE_NONINTERLACED
+    }, {
+	/* 1800x1440 @ 64Hz, 96.15 kHz hsync  */
+	NULL, 64, 1800, 1440, 4347, 304, 96, 46, 1, 192, 3,
+	FB_SYNC_HOR_HIGH_ACT|FB_SYNC_VERT_HIGH_ACT, FB_VMODE_NONINTERLACED
+    }, {
+	/* 1800x1440 @ 70Hz, 104.52 kHz hsync  */
+	NULL, 70, 1800, 1440, 4000, 304, 96, 46, 1, 192, 3,
+	FB_SYNC_HOR_HIGH_ACT|FB_SYNC_VERT_HIGH_ACT, FB_VMODE_NONINTERLACED
+    }, {
+	/* 512x384 @ 78 Hz, 31.50 kHz hsync */
+	NULL, 78, 512, 384, 49603, 48, 16, 16, 1, 64, 3,
+	0, FB_VMODE_NONINTERLACED
+    }, {
+	/* 512x384 @ 85 Hz, 34.38 kHz hsync */
+	NULL, 85, 512, 384, 45454, 48, 16, 16, 1, 64, 3,
+	0, FB_VMODE_NONINTERLACED
+    }, {
+	/* 320x200 @ 70 Hz, 31.5 kHz hsync, 8:5 aspect ratio */
+	NULL, 70, 320, 200, 79440, 16, 16, 20, 4, 48, 1,
+	0, FB_VMODE_DOUBLE
+    }, {
+	/* 320x240 @ 60 Hz, 31.5 kHz hsync, 4:3 aspect ratio */
+	NULL, 60, 320, 240, 79440, 16, 16, 16, 5, 48, 1,
+	0, FB_VMODE_DOUBLE
+    }, {
+	/* 320x240 @ 72 Hz, 36.5 kHz hsync */
+	NULL, 72, 320, 240, 63492, 16, 16, 16, 4, 48, 2,
+	0, FB_VMODE_DOUBLE
+    }, {
+	/* 400x300 @ 56 Hz, 35.2 kHz hsync, 4:3 aspect ratio */
+	NULL, 56, 400, 300, 55555, 64, 16, 10, 1, 32, 1,
+	0, FB_VMODE_DOUBLE
+    }, {
+	/* 400x300 @ 60 Hz, 37.8 kHz hsync */
+	NULL, 60, 400, 300, 50000, 48, 16, 11, 1, 64, 2,
+	0, FB_VMODE_DOUBLE
+    }, {
+	/* 400x300 @ 72 Hz, 48.0 kHz hsync */
+	NULL, 72, 400, 300, 40000, 32, 24, 11, 19, 64, 3,
+	0, FB_VMODE_DOUBLE
+    }, {
+	/* 480x300 @ 56 Hz, 35.2 kHz hsync, 8:5 aspect ratio */
+	NULL, 56, 480, 300, 46176, 80, 16, 10, 1, 40, 1,
+	0, FB_VMODE_DOUBLE
+    }, {
+	/* 480x300 @ 60 Hz, 37.8 kHz hsync */
+	NULL, 60, 480, 300, 41858, 56, 16, 11, 1, 80, 2,
+	0, FB_VMODE_DOUBLE
+    }, {
+	/* 480x300 @ 63 Hz, 39.6 kHz hsync */
+	NULL, 63, 480, 300, 40000, 56, 16, 11, 1, 80, 2,
+	0, FB_VMODE_DOUBLE
+    }, {
+	/* 480x300 @ 72 Hz, 48.0 kHz hsync */
+	NULL, 72, 480, 300, 33386, 40, 24, 11, 19, 80, 3,
+	0, FB_VMODE_DOUBLE
+    },
+};
+static const int nmodedb2 = sizeof(modedb2);
diff --git a/drivers/staging/sm750fb/readme b/drivers/staging/sm750fb/readme
new file mode 100644
index 000000000000..ab9af791653d
--- /dev/null
+++ b/drivers/staging/sm750fb/readme
@@ -0,0 +1,38 @@
+Introduction:
+	SM750 of Silicon MOtion is pci express display controller device.
+	The SM750 embedded graphics features include:
+	- dual display
+	- 2D acceleration
+	- 16MB integrated video memory
+
+About the kernel module paramter of driver:
+
+	Use 1280,8bpp index color and 60 hz mode:
+	insmod ./sm750fb.ko g_option="1280x1024-8@60"
+
+	Disable MTRR,Disable 2d acceleration,Disable hardware cursor,
+	and use a 800x600 mode :
+	insmod ./sm750fb.ko g_option="noaccel:nomtrr:nohwc:800x600"
+
+	dual frame buffer for driver with "dual" parameter
+	insmod ./sm750fb.ko g_option="dual,800x600:1024x768"
+	it will create fb0 and fb1 (or fb1,fb2 if fb0 already exist) under /dev
+	and user can use con2fb to link fbX and ttyX
+
+	Notes:
+	1) if you build the driver with built-in method, the paramter
+		you edited in the grub config file will be also the
+		same format as above modular method,but additionaly add
+		"video=sm750fb:"
+		ahead of parameters,so,it looks like:
+		video=sm750fb:noaccel,1280x1024@60,otherparam,etc...
+		it equal to modular method with below command:
+		insmod ./sm750fb.ko g_option="noaccel:1280x1024@60:otherparm:etc..."
+
+	2) if you put 800x600 into the paramter without bpp and
+		refresh rate, kernel driver will defaulty use 16bpp and 60hz
+
+Important:
+	if you have vesafb enabled in your config then /dev/fb0 will be created by vesafb
+	and this driver will use fb1, fb2. In that case, you need to configure your X-server
+	to use fb1. Another simple althernative is to disable vesafb from your config.
diff --git a/drivers/staging/sm750fb/sm750.c b/drivers/staging/sm750fb/sm750.c
new file mode 100644
index 000000000000..3c7ea95dd9f9
--- /dev/null
+++ b/drivers/staging/sm750fb/sm750.c
@@ -0,0 +1,1403 @@
+#include<linux/kernel.h>
+#include<linux/module.h>
+#include<linux/errno.h>
+#include<linux/string.h>
+#include<linux/mm.h>
+#include<linux/slab.h>
+#include<linux/delay.h>
+#include<linux/fb.h>
+#include<linux/ioport.h>
+#include<linux/init.h>
+#include<linux/pci.h>
+#include<linux/mm_types.h>
+#include<linux/vmalloc.h>
+#include<linux/pagemap.h>
+#include<linux/screen_info.h>
+#include<linux/vmalloc.h>
+#include<linux/pagemap.h>
+#include <linux/console.h>
+#ifdef CONFIG_MTRR
+#include <asm/mtrr.h>
+#endif
+#include <asm/fb.h>
+#include "sm750.h"
+#include "sm750_hw.h"
+#include "sm750_accel.h"
+#include "sm750_cursor.h"
+
+#include "modedb.h"
+
+int smi_indent = 0;
+
+
+/*
+ * #ifdef __BIG_ENDIAN
+ * ssize_t lynxfb_ops_write(struct fb_info *info, const char __user *buf,
+ * size_t count, loff_t *ppos);
+ * ssize_t lynxfb_ops_read(struct fb_info *info, char __user *buf,
+ * size_t count, loff_t *ppos);
+ * #endif
+ */
+
+typedef void (*PROC_SPEC_SETUP)(struct lynx_share*, char *);
+typedef int (*PROC_SPEC_MAP)(struct lynx_share*, struct pci_dev*);
+typedef int (*PROC_SPEC_INITHW)(struct lynx_share*, struct pci_dev*);
+
+
+/* common var for all device */
+static int g_hwcursor = 1;
+static int g_noaccel;
+#ifdef CONFIG_MTRR
+static int g_nomtrr;
+#endif
+static const char *g_fbmode[] = {NULL, NULL};
+static const char *g_def_fbmode = "800x600-16@60";
+static char *g_settings = NULL;
+static int g_dualview;
+static char *g_option = NULL;
+
+
+static const struct fb_videomode lynx750_ext[] = {
+	/*	1024x600-60 VESA	[1.71:1] */
+	{NULL,  60, 1024, 600, 20423, 144,  40, 18, 1, 104, 3,
+	 FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
+	 FB_VMODE_NONINTERLACED},
+
+	/*	1024x600-70 VESA */
+	{NULL,  70, 1024, 600, 17211, 152,  48, 21, 1, 104, 3,
+	 FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
+	 FB_VMODE_NONINTERLACED},
+
+	/*	1024x600-75 VESA */
+	{NULL,  75, 1024, 600, 15822, 160,  56, 23, 1, 104, 3,
+	 FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
+	 FB_VMODE_NONINTERLACED},
+
+	/*	1024x600-85 VESA */
+	{NULL,  85, 1024, 600, 13730, 168,  56, 26, 1, 112, 3,
+	 FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
+	 FB_VMODE_NONINTERLACED},
+
+	/*	720x480	*/
+	{NULL, 60,  720,  480,  37427, 88,   16, 13, 1,   72,  3,
+	 FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
+	 FB_VMODE_NONINTERLACED},
+
+	/*	1280x720		[1.78:1]	*/
+	{NULL, 60,  1280,  720,  13426, 162, 86, 22, 1,  136, 3,
+	 FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
+	 FB_VMODE_NONINTERLACED},
+
+	/*	1280x768@60 */
+	{NULL, 60, 1280, 768, 12579, 192, 64, 20, 3, 128, 7,
+	 FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
+	 FB_VMODE_NONINTERLACED},
+
+	{NULL, 60, 1360, 768, 11804, 208, 64, 23, 1, 144, 3,
+	 FB_SYNC_HOR_HIGH_ACT|FB_VMODE_NONINTERLACED},
+
+	/*	1360 x 768	[1.77083:1]	*/
+	{NULL,  60, 1360, 768, 11804, 208,  64, 23, 1, 144, 3,
+	 FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
+	 FB_VMODE_NONINTERLACED},
+
+	/*	1368 x 768      [1.78:1]	*/
+	{NULL, 60,  1368,  768,  11647, 216, 72, 23, 1,  144, 3,
+	 FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
+	 FB_VMODE_NONINTERLACED},
+
+	/*	1440 x 900		[16:10]	*/
+	{NULL, 60, 1440, 900, 9392, 232, 80, 28, 1, 152, 3,
+	 FB_SYNC_VERT_HIGH_ACT,
+	 FB_VMODE_NONINTERLACED},
+
+	/*	1440x960		[15:10]	*/
+	{NULL, 60, 1440, 960, 8733, 240, 88, 30, 1, 152, 3,
+	 FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
+	 FB_VMODE_NONINTERLACED},
+
+	/*	1920x1080	[16:9]	*/
+	{NULL, 60, 1920, 1080, 6734, 148, 88, 41, 1, 44, 3,
+	 FB_SYNC_VERT_HIGH_ACT,
+	 FB_VMODE_NONINTERLACED},
+};
+
+
+
+
+/* no hardware cursor supported under version 2.6.10, kernel bug */
+static int lynxfb_ops_cursor(struct fb_info *info, struct fb_cursor *fbcursor)
+{
+	struct lynxfb_par *par;
+	struct lynxfb_crtc *crtc;
+	struct lynx_cursor *cursor;
+
+	par = info->par;
+	crtc = &par->crtc;
+	cursor = &crtc->cursor;
+
+	if (fbcursor->image.width > cursor->maxW ||
+	   fbcursor->image.height > cursor->maxH ||
+	   fbcursor->image.depth > 1) {
+		return -ENXIO;
+	}
+
+	cursor->disable(cursor);
+	if (fbcursor->set & FB_CUR_SETSIZE)
+		cursor->setSize(cursor,
+				fbcursor->image.width,
+				fbcursor->image.height);
+
+	if (fbcursor->set & FB_CUR_SETPOS)
+		cursor->setPos(cursor,
+			       fbcursor->image.dx - info->var.xoffset,
+			       fbcursor->image.dy - info->var.yoffset);
+
+	if (fbcursor->set & FB_CUR_SETCMAP) {
+		/* get the 16bit color of kernel means */
+		u16 fg, bg;
+
+		fg = ((info->cmap.red[fbcursor->image.fg_color] & 0xf800))|
+		      ((info->cmap.green[fbcursor->image.fg_color] & 0xfc00) >> 5)|
+		      ((info->cmap.blue[fbcursor->image.fg_color] & 0xf800) >> 11);
+
+		bg = ((info->cmap.red[fbcursor->image.bg_color] & 0xf800))|
+		      ((info->cmap.green[fbcursor->image.bg_color] & 0xfc00) >> 5)|
+		      ((info->cmap.blue[fbcursor->image.bg_color] & 0xf800) >> 11);
+
+		cursor->setColor(cursor, fg, bg);
+	}
+
+
+	if (fbcursor->set & (FB_CUR_SETSHAPE | FB_CUR_SETIMAGE)) {
+		cursor->setData(cursor,
+				fbcursor->rop,
+				fbcursor->image.data,
+				fbcursor->mask);
+	}
+
+	if (fbcursor->enable)
+		cursor->enable(cursor);
+
+	return 0;
+}
+
+static void lynxfb_ops_fillrect(struct fb_info *info,
+				const struct fb_fillrect *region)
+{
+	struct lynxfb_par *par;
+	struct lynx_share *share;
+	unsigned int base, pitch, Bpp, rop;
+	u32 color;
+
+	if (info->state != FBINFO_STATE_RUNNING)
+		return;
+
+	par = info->par;
+	share = par->share;
+
+	/* each time 2d function begin to work,below three variable always need
+	 * be set, seems we can put them together in some place  */
+	base = par->crtc.oScreen;
+	pitch = info->fix.line_length;
+	Bpp = info->var.bits_per_pixel >> 3;
+
+	color = (Bpp == 1)?region->color:((u32 *)info->pseudo_palette)[region->color];
+	rop = (region->rop != ROP_COPY) ? HW_ROP2_XOR:HW_ROP2_COPY;
+
+	/*
+	 * If not use spin_lock,system will die if user load driver
+	 * and immediatly unload driver frequently (dual)
+	 */
+	if (share->dual)
+		spin_lock(&share->slock);
+
+	share->accel.de_fillrect(&share->accel,
+				 base, pitch, Bpp,
+				 region->dx, region->dy,
+				 region->width, region->height,
+				 color, rop);
+	if (share->dual)
+		spin_unlock(&share->slock);
+}
+
+static void lynxfb_ops_copyarea(struct fb_info *info,
+				const struct fb_copyarea *region)
+{
+	struct lynxfb_par *par;
+	struct lynx_share *share;
+	unsigned int base, pitch, Bpp;
+
+	par = info->par;
+	share = par->share;
+
+	/* each time 2d function begin to work,below three variable always need
+	 * be set, seems we can put them together in some place  */
+	base = par->crtc.oScreen;
+	pitch = info->fix.line_length;
+	Bpp = info->var.bits_per_pixel >> 3;
+
+	/*
+	 * If not use spin_lock, system will die if user load driver
+	 * and immediatly unload driver frequently (dual)
+	 */
+	if (share->dual)
+		spin_lock(&share->slock);
+
+	share->accel.de_copyarea(&share->accel,
+				 base, pitch, region->sx, region->sy,
+				 base, pitch, Bpp, region->dx, region->dy,
+				 region->width, region->height, HW_ROP2_COPY);
+	if (share->dual)
+		spin_unlock(&share->slock);
+}
+
+static void lynxfb_ops_imageblit(struct fb_info *info,
+				 const struct fb_image *image)
+{
+	unsigned int base, pitch, Bpp;
+	unsigned int fgcol, bgcol;
+	struct lynxfb_par *par;
+	struct lynx_share *share;
+
+	par = info->par;
+	share = par->share;
+	/* each time 2d function begin to work,below three variable always need
+	 * be set, seems we can put them together in some place  */
+	base = par->crtc.oScreen;
+	pitch = info->fix.line_length;
+	Bpp = info->var.bits_per_pixel >> 3;
+
+	if (image->depth == 1) {
+		if (info->fix.visual == FB_VISUAL_TRUECOLOR ||
+		    info->fix.visual == FB_VISUAL_DIRECTCOLOR) {
+			fgcol = ((u32 *)info->pseudo_palette)[image->fg_color];
+			bgcol = ((u32 *)info->pseudo_palette)[image->bg_color];
+		} else {
+			fgcol = image->fg_color;
+			bgcol = image->bg_color;
+		}
+		goto _do_work;
+	}
+	return;
+_do_work:
+	/*
+	 * If not use spin_lock, system will die if user load driver
+	 * and immediatly unload driver frequently (dual)
+	 */
+	if (share->dual)
+		spin_lock(&share->slock);
+
+	share->accel.de_imageblit(&share->accel,
+				  image->data, image->width>>3, 0,
+				  base, pitch, Bpp,
+				  image->dx, image->dy,
+				  image->width, image->height,
+				  fgcol, bgcol, HW_ROP2_COPY);
+	if (share->dual)
+		spin_unlock(&share->slock);
+}
+
+static int lynxfb_ops_pan_display(struct fb_var_screeninfo *var,
+				  struct fb_info *info)
+{
+	struct lynxfb_par *par;
+	struct lynxfb_crtc *crtc;
+	int ret;
+
+
+	if (!info)
+		return -EINVAL;
+
+	ret = 0;
+	par = info->par;
+	crtc = &par->crtc;
+	ret = crtc->proc_panDisplay(crtc, var, info);
+
+	return ret;
+}
+
+static int lynxfb_ops_set_par(struct fb_info *info)
+{
+	struct lynxfb_par *par;
+	struct lynx_share *share;
+	struct lynxfb_crtc *crtc;
+	struct lynxfb_output *output;
+	struct fb_var_screeninfo *var;
+	struct fb_fix_screeninfo *fix;
+	int ret;
+	unsigned int line_length;
+
+	if (!info)
+		return -EINVAL;
+
+	ret = 0;
+	par = info->par;
+	share = par->share;
+	crtc = &par->crtc;
+	output = &par->output;
+	var = &info->var;
+	fix = &info->fix;
+
+	/* fix structur is not so FIX ... */
+	line_length = var->xres_virtual * var->bits_per_pixel / 8;
+	line_length = PADDING(crtc->line_pad, line_length);
+	fix->line_length = line_length;
+	pr_err("fix->line_length = %d\n", fix->line_length);
+
+	/* var->red,green,blue,transp are need to be set by driver
+	 * and these data should be set before setcolreg routine
+	 * */
+
+	switch (var->bits_per_pixel) {
+	case 8:
+		fix->visual = FB_VISUAL_PSEUDOCOLOR;
+		var->red.offset = 0;
+		var->red.length = 8;
+		var->green.offset = 0;
+		var->green.length = 8;
+		var->blue.offset = 0;
+		var->blue.length = 8;
+		var->transp.length = 0;
+		var->transp.offset = 0;
+		break;
+	case 16:
+		var->red.offset = 11;
+		var->red.length = 5;
+		var->green.offset = 5;
+		var->green.length = 6;
+		var->blue.offset = 0;
+		var->blue.length = 5;
+		var->transp.length = 0;
+		var->transp.offset = 0;
+		fix->visual = FB_VISUAL_TRUECOLOR;
+		break;
+	case 24:
+	case 32:
+		var->red.offset = 16;
+		var->red.length = 8;
+		var->green.offset = 8;
+		var->green.length = 8;
+		var->blue.offset = 0;
+		var->blue.length = 8;
+		fix->visual = FB_VISUAL_TRUECOLOR;
+		break;
+	default:
+		ret = -EINVAL;
+		break;
+	}
+	var->height = var->width = -1;
+	var->accel_flags = 0;/*FB_ACCELF_TEXT;*/
+
+	if (ret) {
+		pr_err("pixel bpp format not satisfied\n.");
+		return ret;
+	}
+	ret = crtc->proc_setMode(crtc, var, fix);
+	if (!ret)
+		ret = output->proc_setMode(output, var, fix);
+	return ret;
+}
+
+static inline unsigned int chan_to_field(unsigned int chan,
+					 struct fb_bitfield *bf)
+{
+	chan &= 0xffff;
+	chan >>= 16 - bf->length;
+	return chan << bf->offset;
+}
+
+#ifdef CONFIG_PM
+static int lynxfb_suspend(struct pci_dev *pdev, pm_message_t mesg)
+{
+	struct fb_info *info;
+	struct lynx_share *share;
+	int ret;
+
+	if (mesg.event == pdev->dev.power.power_state.event)
+		return 0;
+
+	ret = 0;
+	share = pci_get_drvdata(pdev);
+	switch (mesg.event) {
+	case PM_EVENT_FREEZE:
+	case PM_EVENT_PRETHAW:
+		pdev->dev.power.power_state = mesg;
+		return 0;
+	}
+
+	console_lock();
+	if (mesg.event & PM_EVENT_SLEEP) {
+		info = share->fbinfo[0];
+		if (info)
+			/* 1 means do suspend */
+			fb_set_suspend(info, 1);
+		info = share->fbinfo[1];
+		if (info)
+			/* 1 means do suspend */
+			fb_set_suspend(info, 1);
+
+		ret = pci_save_state(pdev);
+		if (ret) {
+			pr_err("error:%d occurred in pci_save_state\n", ret);
+			return ret;
+		}
+
+		/* set chip to sleep mode */
+		if (share->suspend)
+			(*share->suspend)(share);
+
+		pci_disable_device(pdev);
+		ret = pci_set_power_state(pdev, pci_choose_state(pdev, mesg));
+		if (ret) {
+			pr_err("error:%d occurred in pci_set_power_state\n", ret);
+			return ret;
+		}
+	}
+
+	pdev->dev.power.power_state = mesg;
+	console_unlock();
+	return ret;
+}
+
+static int lynxfb_resume(struct pci_dev *pdev)
+{
+	struct fb_info *info;
+	struct lynx_share *share;
+
+	struct lynxfb_par *par;
+	struct lynxfb_crtc *crtc;
+	struct lynx_cursor *cursor;
+
+	int ret;
+
+
+	ret = 0;
+	share = pci_get_drvdata(pdev);
+
+	console_lock();
+
+	ret = pci_set_power_state(pdev, PCI_D0);
+	if (ret) {
+		pr_err("error:%d occured in pci_set_power_state\n", ret);
+		return ret;
+	}
+
+
+	if (pdev->dev.power.power_state.event != PM_EVENT_FREEZE) {
+		pci_restore_state(pdev);
+		ret = pci_enable_device(pdev);
+		if (ret) {
+			pr_err("error:%d occured in pci_enable_device\n", ret);
+			return ret;
+		}
+		pci_set_master(pdev);
+	}
+	if (share->resume)
+		(*share->resume)(share);
+
+	hw_sm750_inithw(share, pdev);
+
+
+	info = share->fbinfo[0];
+
+	if (info) {
+		par = info->par;
+		crtc = &par->crtc;
+		cursor = &crtc->cursor;
+		memset_io(cursor->vstart, 0x0, cursor->size);
+		memset_io(crtc->vScreen, 0x0, crtc->vidmem_size);
+		lynxfb_ops_set_par(info);
+		fb_set_suspend(info, 0);
+	}
+
+	info = share->fbinfo[1];
+
+	if (info) {
+		par = info->par;
+		crtc = &par->crtc;
+		cursor = &crtc->cursor;
+		memset_io(cursor->vstart, 0x0, cursor->size);
+		memset_io(crtc->vScreen, 0x0, crtc->vidmem_size);
+		lynxfb_ops_set_par(info);
+		fb_set_suspend(info, 0);
+	}
+
+
+	console_unlock();
+	return ret;
+}
+#endif
+
+static int lynxfb_ops_check_var(struct fb_var_screeninfo *var,
+				struct fb_info *info)
+{
+	struct lynxfb_par *par;
+	struct lynxfb_crtc *crtc;
+	struct lynxfb_output *output;
+	struct lynx_share *share;
+	int ret;
+	resource_size_t request;
+
+
+	par = info->par;
+	crtc = &par->crtc;
+	output = &par->output;
+	share = par->share;
+	ret = 0;
+
+	pr_debug("check var:%dx%d-%d\n",
+		 var->xres,
+		 var->yres,
+		 var->bits_per_pixel);
+
+
+	switch (var->bits_per_pixel) {
+	case 8:
+	case 16:
+	case 24: /* support 24 bpp for only lynx712/722/720 */
+	case 32:
+		break;
+	default:
+		pr_err("bpp %d not supported\n", var->bits_per_pixel);
+		ret = -EINVAL;
+		goto exit;
+	}
+
+	switch (var->bits_per_pixel) {
+	case 8:
+		info->fix.visual = FB_VISUAL_PSEUDOCOLOR;
+		var->red.offset = 0;
+		var->red.length = 8;
+		var->green.offset = 0;
+		var->green.length = 8;
+		var->blue.offset = 0;
+		var->blue.length = 8;
+		var->transp.length = 0;
+		var->transp.offset = 0;
+		break;
+	case 16:
+		var->red.offset = 11;
+		var->red.length = 5;
+		var->green.offset = 5;
+		var->green.length = 6;
+		var->blue.offset = 0;
+		var->blue.length = 5;
+		var->transp.length = 0;
+		var->transp.offset = 0;
+		info->fix.visual = FB_VISUAL_TRUECOLOR;
+		break;
+	case 24:
+	case 32:
+		var->red.offset = 16;
+		var->red.length = 8;
+		var->green.offset = 8;
+		var->green.length = 8;
+		var->blue.offset = 0;
+		var->blue.length = 8;
+		info->fix.visual = FB_VISUAL_TRUECOLOR;
+		break;
+	default:
+		ret = -EINVAL;
+		break;
+	}
+	var->height = var->width = -1;
+	var->accel_flags = 0;/* FB_ACCELF_TEXT; */
+
+	/* check if current fb's video memory big enought to hold the onscreen*/
+	request = var->xres_virtual * (var->bits_per_pixel >> 3);
+	/* defaulty crtc->channel go with par->index */
+
+	request = PADDING(crtc->line_pad, request);
+	request = request * var->yres_virtual;
+	if (crtc->vidmem_size < request) {
+		pr_err("not enough video memory for mode\n");
+		return -ENOMEM;
+	}
+
+	ret = output->proc_checkMode(output, var);
+	if (!ret)
+		ret = crtc->proc_checkMode(crtc, var);
+exit:
+	return ret;
+}
+
+
+static int lynxfb_ops_setcolreg(unsigned regno,
+				unsigned red,
+				unsigned green,
+				unsigned blue,
+				unsigned transp,
+				struct fb_info *info)
+{
+	struct lynxfb_par *par;
+	struct lynxfb_crtc *crtc;
+	struct fb_var_screeninfo *var;
+	int ret;
+
+	par = info->par;
+	crtc = &par->crtc;
+	var = &info->var;
+	ret = 0;
+
+	if (regno > 256) {
+		pr_err("regno = %d\n", regno);
+		return -EINVAL;
+	}
+
+	if (info->var.grayscale)
+		red = green = blue = (red * 77 + green * 151 + blue * 28) >> 8;
+
+	if (var->bits_per_pixel == 8 &&
+	    info->fix.visual == FB_VISUAL_PSEUDOCOLOR) {
+		red >>= 8;
+		green >>= 8;
+		blue >>= 8;
+		ret = crtc->proc_setColReg(crtc, regno, red, green, blue);
+		goto exit;
+	}
+
+
+	if (info->fix.visual == FB_VISUAL_TRUECOLOR && regno < 256) {
+		u32 val;
+
+		if (var->bits_per_pixel == 16 ||
+		    var->bits_per_pixel == 32 ||
+		    var->bits_per_pixel == 24) {
+			val = chan_to_field(red, &var->red);
+			val |= chan_to_field(green, &var->green);
+			val |= chan_to_field(blue, &var->blue);
+			par->pseudo_palette[regno] = val;
+			goto exit;
+		}
+	}
+
+	ret = -EINVAL;
+
+exit:
+	return ret;
+}
+
+static int lynxfb_ops_blank(int blank, struct fb_info *info)
+{
+	struct lynxfb_par *par;
+	struct lynxfb_output *output;
+
+	pr_debug("blank = %d.\n", blank);
+	par = info->par;
+	output = &par->output;
+	return output->proc_setBLANK(output, blank);
+}
+
+static int sm750fb_set_drv(struct lynxfb_par *par)
+{
+	int ret;
+	struct lynx_share *share;
+	struct sm750_share *spec_share;
+	struct lynxfb_output *output;
+	struct lynxfb_crtc *crtc;
+
+	ret = 0;
+
+	share = par->share;
+	spec_share = container_of(share, struct sm750_share, share);
+	output = &par->output;
+	crtc = &par->crtc;
+
+	crtc->vidmem_size = (share->dual)?share->vidmem_size>>1:share->vidmem_size;
+	/* setup crtc and output member */
+	spec_share->hwCursor = g_hwcursor;
+
+	crtc->proc_setMode = hw_sm750_crtc_setMode;
+	crtc->proc_checkMode = hw_sm750_crtc_checkMode;
+	crtc->proc_setColReg = hw_sm750_setColReg;
+	crtc->proc_panDisplay = hw_sm750_pan_display;
+	crtc->clear = hw_sm750_crtc_clear;
+	crtc->line_pad = 16;
+	crtc->xpanstep = 8;
+	crtc->ypanstep = 1;
+	crtc->ywrapstep = 0;
+
+	output->proc_setMode = hw_sm750_output_setMode;
+	output->proc_checkMode = hw_sm750_output_checkMode;
+
+	output->proc_setBLANK = (share->revid == SM750LE_REVISION_ID)?hw_sm750le_setBLANK:hw_sm750_setBLANK;
+	output->clear = hw_sm750_output_clear;
+	/* chip specific phase */
+	share->accel.de_wait = (share->revid == SM750LE_REVISION_ID)?hw_sm750le_deWait : hw_sm750_deWait;
+	switch (spec_share->state.dataflow) {
+	case sm750_simul_pri:
+		output->paths = sm750_pnc;
+		crtc->channel = sm750_primary;
+		crtc->oScreen = 0;
+		crtc->vScreen = share->pvMem;
+		pr_info("use simul primary mode\n");
+		break;
+	case sm750_simul_sec:
+		output->paths = sm750_pnc;
+		crtc->channel = sm750_secondary;
+		crtc->oScreen = 0;
+		crtc->vScreen = share->pvMem;
+		break;
+	case sm750_dual_normal:
+		if (par->index == 0) {
+			output->paths = sm750_panel;
+			crtc->channel = sm750_primary;
+			crtc->oScreen = 0;
+			crtc->vScreen = share->pvMem;
+		} else {
+			output->paths = sm750_crt;
+			crtc->channel = sm750_secondary;
+			/* not consider of padding stuffs for oScreen,need fix */
+			crtc->oScreen = (share->vidmem_size >> 1);
+			crtc->vScreen = share->pvMem + crtc->oScreen;
+		}
+		break;
+	case sm750_dual_swap:
+		if (par->index == 0) {
+			output->paths = sm750_panel;
+			crtc->channel = sm750_secondary;
+			crtc->oScreen = 0;
+			crtc->vScreen = share->pvMem;
+		} else {
+			output->paths = sm750_crt;
+			crtc->channel = sm750_primary;
+			/* not consider of padding stuffs for oScreen,need fix */
+			crtc->oScreen = (share->vidmem_size >> 1);
+			crtc->vScreen = share->pvMem + crtc->oScreen;
+		}
+		break;
+	default:
+		ret = -EINVAL;
+	}
+
+	return ret;
+}
+
+static struct fb_ops lynxfb_ops = {
+	.owner = THIS_MODULE,
+	.fb_check_var =  lynxfb_ops_check_var,
+	.fb_set_par = lynxfb_ops_set_par,
+	.fb_setcolreg = lynxfb_ops_setcolreg,
+	.fb_blank = lynxfb_ops_blank,
+	.fb_fillrect = cfb_fillrect,
+	.fb_imageblit = cfb_imageblit,
+	.fb_copyarea = cfb_copyarea,
+	/* cursor */
+	.fb_cursor = lynxfb_ops_cursor,
+};
+
+
+static int lynxfb_set_fbinfo(struct fb_info *info, int index)
+{
+	int i;
+	struct lynxfb_par *par;
+	struct lynx_share *share;
+	struct lynxfb_crtc *crtc;
+	struct lynxfb_output *output;
+	struct fb_var_screeninfo *var;
+	struct fb_fix_screeninfo *fix;
+
+	const struct fb_videomode *pdb[] = {
+		lynx750_ext, NULL, vesa_modes,
+	};
+	int cdb[] = {ARRAY_SIZE(lynx750_ext), 0, VESA_MODEDB_SIZE};
+	static const char *mdb_desc[] = {
+		"driver prepared modes",
+		"kernel prepared default modedb",
+		"kernel HELPERS prepared vesa_modes",
+	};
+
+
+	static const char *fixId[2] = {
+		"sm750_fb1", "sm750_fb2",
+	};
+
+	int ret, line_length;
+
+	ret = 0;
+	par = (struct lynxfb_par *)info->par;
+	share = par->share;
+	crtc = &par->crtc;
+	output = &par->output;
+	var = &info->var;
+	fix = &info->fix;
+
+	/* set index */
+	par->index = index;
+	output->channel = &crtc->channel;
+	sm750fb_set_drv(par);
+	lynxfb_ops.fb_pan_display = lynxfb_ops_pan_display;
+
+
+	/* set current cursor variable and proc pointer,
+	 * must be set after crtc member initialized */
+	crtc->cursor.offset = crtc->oScreen + crtc->vidmem_size - 1024;
+	crtc->cursor.mmio = share->pvReg + 0x800f0 + (int)crtc->channel * 0x140;
+
+	pr_info("crtc->cursor.mmio = %p\n", crtc->cursor.mmio);
+	crtc->cursor.maxH = crtc->cursor.maxW = 64;
+	crtc->cursor.size = crtc->cursor.maxH*crtc->cursor.maxW*2/8;
+	crtc->cursor.disable = hw_cursor_disable;
+	crtc->cursor.enable = hw_cursor_enable;
+	crtc->cursor.setColor = hw_cursor_setColor;
+	crtc->cursor.setPos = hw_cursor_setPos;
+	crtc->cursor.setSize = hw_cursor_setSize;
+	crtc->cursor.setData = hw_cursor_setData;
+	crtc->cursor.vstart = share->pvMem + crtc->cursor.offset;
+
+
+	crtc->cursor.share = share;
+		memset_io(crtc->cursor.vstart, 0, crtc->cursor.size);
+	if (!g_hwcursor) {
+		lynxfb_ops.fb_cursor = NULL;
+		crtc->cursor.disable(&crtc->cursor);
+	}
+
+
+	/* set info->fbops, must be set before fb_find_mode */
+	if (!share->accel_off) {
+		/* use 2d acceleration */
+		lynxfb_ops.fb_fillrect = lynxfb_ops_fillrect;
+		lynxfb_ops.fb_copyarea = lynxfb_ops_copyarea;
+		lynxfb_ops.fb_imageblit = lynxfb_ops_imageblit;
+	}
+	info->fbops = &lynxfb_ops;
+
+	if (!g_fbmode[index]) {
+		g_fbmode[index] = g_def_fbmode;
+		if (index)
+			g_fbmode[index] = g_fbmode[0];
+	}
+
+
+	for (i = 0; i < 3; i++) {
+
+		ret = fb_find_mode(var, info, g_fbmode[index],
+				   pdb[i], cdb[i], NULL, 8);
+
+		if (ret == 1) {
+			pr_info("success! use specified mode:%s in %s\n",
+				g_fbmode[index],
+				mdb_desc[i]);
+			break;
+		} else if (ret == 2) {
+			pr_warn("use specified mode:%s in %s,with an ignored refresh rate\n",
+				g_fbmode[index],
+				mdb_desc[i]);
+			break;
+		} else if (ret == 3) {
+			pr_warn("wanna use default mode\n");
+			/*break;*/
+		} else if (ret == 4) {
+			pr_warn("fall back to any valid mode\n");
+		} else {
+			pr_warn("ret = %d,fb_find_mode failed,with %s\n",
+				ret,
+				mdb_desc[i]);
+		}
+	}
+
+	/* some member of info->var had been set by fb_find_mode */
+
+	pr_info("Member of info->var is :\n\
+		xres=%d\n\
+		yres=%d\n\
+		xres_virtual=%d\n\
+		yres_virtual=%d\n\
+		xoffset=%d\n\
+		yoffset=%d\n\
+		bits_per_pixel=%d\n \
+		...\n",
+		var->xres,
+		var->yres,
+		var->xres_virtual,
+		var->yres_virtual,
+		var->xoffset,
+		var->yoffset,
+		var->bits_per_pixel);
+
+	/* set par */
+	par->info = info;
+
+	/* set info */
+	line_length = PADDING(crtc->line_pad,
+			      (var->xres_virtual * var->bits_per_pixel/8));
+
+	info->pseudo_palette = &par->pseudo_palette[0];
+	info->screen_base = crtc->vScreen;
+	pr_debug("screen_base vaddr = %p\n", info->screen_base);
+	info->screen_size = line_length * var->yres_virtual;
+	info->flags = FBINFO_FLAG_DEFAULT|0;
+
+	/* set info->fix */
+	fix->type = FB_TYPE_PACKED_PIXELS;
+	fix->type_aux = 0;
+	fix->xpanstep = crtc->xpanstep;
+	fix->ypanstep = crtc->ypanstep;
+	fix->ywrapstep = crtc->ywrapstep;
+	fix->accel = FB_ACCEL_SMI;
+
+	strlcpy(fix->id, fixId[index], sizeof(fix->id));
+
+
+	fix->smem_start = crtc->oScreen + share->vidmem_start;
+	pr_info("fix->smem_start = %lx\n", fix->smem_start);
+	/* according to mmap experiment from user space application,
+	 * fix->mmio_len should not larger than virtual size
+	 * (xres_virtual x yres_virtual x ByPP)
+	 * Below line maybe buggy when user mmap fb dev node and write
+	 * data into the bound over virtual size
+	 * */
+	fix->smem_len = crtc->vidmem_size;
+	pr_info("fix->smem_len = %x\n", fix->smem_len);
+	info->screen_size = fix->smem_len;
+	fix->line_length = line_length;
+	fix->mmio_start = share->vidreg_start;
+	pr_info("fix->mmio_start = %lx\n", fix->mmio_start);
+	fix->mmio_len = share->vidreg_size;
+	pr_info("fix->mmio_len = %x\n", fix->mmio_len);
+	switch (var->bits_per_pixel) {
+	case 8:
+		fix->visual = FB_VISUAL_PSEUDOCOLOR;
+		break;
+	case 16:
+	case 32:
+		fix->visual = FB_VISUAL_TRUECOLOR;
+		break;
+	}
+
+	/* set var */
+	var->activate = FB_ACTIVATE_NOW;
+	var->accel_flags = 0;
+	var->vmode = FB_VMODE_NONINTERLACED;
+
+	pr_debug("#1 show info->cmap : \nstart=%d,len=%d,red=%p,green=%p,blue=%p,transp=%p\n",
+		 info->cmap.start, info->cmap.len,
+		 info->cmap.red, info->cmap.green, info->cmap.blue,
+		 info->cmap.transp);
+
+	ret = fb_alloc_cmap(&info->cmap, 256, 0);
+	if (ret < 0) {
+		pr_err("Could not allcate memory for cmap.\n");
+		goto exit;
+	}
+
+	pr_debug("#2 show info->cmap :\nstart=%d,len=%d,red=%p,green=%p,blue=%p,transp=%p\n",
+		 info->cmap.start, info->cmap.len,
+		 info->cmap.red, info->cmap.green, info->cmap.blue,
+		 info->cmap.transp);
+
+exit:
+	lynxfb_ops_check_var(var, info);
+	return ret;
+}
+
+/*	chip specific g_option configuration routine */
+static void sm750fb_setup(struct lynx_share *share, char *src)
+{
+	struct sm750_share *spec_share;
+	char *opt;
+#ifdef CAP_EXPENSION
+	char *exp_res;
+#endif
+	int swap;
+
+
+	spec_share = container_of(share, struct sm750_share, share);
+#ifdef CAP_EXPENSIION
+	exp_res = NULL;
+#endif
+	swap = 0;
+
+	spec_share->state.initParm.chip_clk = 0;
+	spec_share->state.initParm.mem_clk = 0;
+	spec_share->state.initParm.master_clk = 0;
+	spec_share->state.initParm.powerMode = 0;
+	spec_share->state.initParm.setAllEngOff = 0;
+	spec_share->state.initParm.resetMemory = 1;
+
+	/* defaultly turn g_hwcursor on for both view */
+	g_hwcursor = 3;
+
+	if (!src || !*src) {
+		pr_warn("no specific g_option.\n");
+		goto NO_PARAM;
+	}
+
+	while ((opt = strsep(&src, ":")) != NULL && *opt != 0) {
+		pr_err("opt=%s\n", opt);
+		pr_err("src=%s\n", src);
+
+		if (!strncmp(opt, "swap", strlen("swap")))
+			swap = 1;
+		else if (!strncmp(opt, "nocrt", strlen("nocrt")))
+			spec_share->state.nocrt = 1;
+		else if (!strncmp(opt, "36bit", strlen("36bit")))
+			spec_share->state.pnltype = sm750_doubleTFT;
+		else if (!strncmp(opt, "18bit", strlen("18bit")))
+			spec_share->state.pnltype = sm750_dualTFT;
+		else if (!strncmp(opt, "24bit", strlen("24bit")))
+			spec_share->state.pnltype = sm750_24TFT;
+#ifdef CAP_EXPANSION
+		else if (!strncmp(opt, "exp:", strlen("exp:")))
+			exp_res = opt + strlen("exp:");
+#endif
+		else if (!strncmp(opt, "nohwc0", strlen("nohwc0")))
+			g_hwcursor &= ~0x1;
+		else if (!strncmp(opt, "nohwc1", strlen("nohwc1")))
+			g_hwcursor &= ~0x2;
+		else if (!strncmp(opt, "nohwc", strlen("nohwc")))
+			g_hwcursor = 0;
+		else {
+			if (!g_fbmode[0]) {
+				g_fbmode[0] = opt;
+				pr_info("find fbmode0 : %s\n", g_fbmode[0]);
+			} else if (!g_fbmode[1]) {
+				g_fbmode[1] = opt;
+				pr_info("find fbmode1 : %s\n", g_fbmode[1]);
+			} else {
+				pr_warn("How many view you wann set?\n");
+			}
+		}
+	}
+#ifdef CAP_EXPANSION
+	if (getExpRes(exp_res,
+		      &spec_share->state.xLCD,
+		      &spec_share->state.yLCD)) {
+		/* seems exp_res is not valid */
+		spec_share->state.xLCD = spec_share->state.yLCD = 0;
+	}
+#endif
+
+NO_PARAM:
+	if (share->revid != SM750LE_REVISION_ID) {
+		if (share->dual) {
+			if (swap)
+				spec_share->state.dataflow = sm750_dual_swap;
+			else
+				spec_share->state.dataflow = sm750_dual_normal;
+		} else {
+			if (swap)
+				spec_share->state.dataflow = sm750_simul_sec;
+			else
+				spec_share->state.dataflow = sm750_simul_pri;
+		}
+	} else {
+		/* SM750LE only have one crt channel */
+		spec_share->state.dataflow = sm750_simul_sec;
+		/* sm750le do not have complex attributes */
+		spec_share->state.nocrt = 0;
+	}
+}
+
+static int lynxfb_pci_probe(struct pci_dev *pdev,
+			    const struct pci_device_id * ent)
+{
+	struct fb_info *info[] = {NULL, NULL};
+	struct lynx_share *share = NULL;
+
+	struct sm750_share *spec_share = NULL;
+	size_t spec_offset = 0;
+	int fbidx;
+
+
+	/* enable device */
+	if (pci_enable_device(pdev)) {
+		pr_err("can not enable device.\n");
+		goto err_enable;
+	}
+
+	/* though offset of share in sm750_share is 0,
+	 * we use this marcro as the same */
+	spec_offset = offsetof(struct sm750_share, share);
+
+	spec_share = kzalloc(sizeof(*spec_share), GFP_KERNEL);
+	if (!spec_share) {
+		pr_err("Could not allocate memory for share.\n");
+		goto err_share;
+	}
+
+	/* setting share structure */
+	share = (struct lynx_share *)(&(spec_share->share));
+	share->fbinfo[0] = share->fbinfo[1] = NULL;
+	share->devid = pdev->device;
+	share->revid = pdev->revision;
+
+	pr_info("share->revid = %02x\n", share->revid);
+	share->pdev = pdev;
+#ifdef CONFIG_MTRR
+	share->mtrr_off = g_nomtrr;
+	share->mtrr.vram = 0;
+	share->mtrr.vram_added = 0;
+#endif
+	share->accel_off = g_noaccel;
+	share->dual = g_dualview;
+	spin_lock_init(&share->slock);
+
+	if (!share->accel_off) {
+		/* hook deInit and 2d routines, notes that below hw_xxx
+		 * routine can work on most of lynx chips
+		 * if some chip need specific function,
+		 * please hook it in smXXX_set_drv routine */
+		share->accel.de_init = hw_de_init;
+		share->accel.de_fillrect = hw_fillrect;
+		share->accel.de_copyarea = hw_copyarea;
+		share->accel.de_imageblit = hw_imageblit;
+		pr_info("enable 2d acceleration\n");
+	} else {
+		pr_info("disable 2d acceleration\n");
+	}
+
+	/* call chip specific setup routine  */
+	sm750fb_setup(share, g_settings);
+
+	/* call chip specific mmap routine */
+	if (hw_sm750_map(share, pdev)) {
+		pr_err("Memory map failed\n");
+		goto err_map;
+	}
+
+#ifdef CONFIG_MTRR
+	if (!share->mtrr_off) {
+		pr_info("enable mtrr\n");
+		share->mtrr.vram = mtrr_add(share->vidmem_start,
+					    share->vidmem_size,
+					    MTRR_TYPE_WRCOMB, 1);
+
+		if (share->mtrr.vram < 0) {
+			/* don't block driver with the failure of MTRR */
+			pr_err("Unable to setup MTRR.\n");
+		} else {
+			share->mtrr.vram_added = 1;
+			pr_info("MTRR added succesfully\n");
+		}
+	}
+#endif
+
+	memset_io(share->pvMem, 0, share->vidmem_size);
+
+	pr_info("sm%3x mmio address = %p\n", share->devid, share->pvReg);
+
+	pci_set_drvdata(pdev, share);
+
+	/* call chipInit routine */
+	hw_sm750_inithw(share, pdev);
+
+	/* allocate frame buffer info structor according to g_dualview */
+	fbidx = 0;
+ALLOC_FB:
+	info[fbidx] = framebuffer_alloc(sizeof(struct lynxfb_par), &pdev->dev);
+	if (!info[fbidx]) {
+		pr_err("Could not allocate framebuffer #%d.\n", fbidx);
+		if (fbidx == 0)
+			goto err_info0_alloc;
+		else
+			goto err_info1_alloc;
+	} else {
+		struct lynxfb_par *par;
+		int errno;
+
+		pr_info("framebuffer #%d alloc okay\n", fbidx);
+		share->fbinfo[fbidx] = info[fbidx];
+		par = info[fbidx]->par;
+		par->share = share;
+
+		/* set fb_info structure */
+		if (lynxfb_set_fbinfo(info[fbidx], fbidx)) {
+			pr_err("Failed to initial fb_info #%d.\n", fbidx);
+			if (fbidx == 0)
+				goto err_info0_set;
+			else
+				goto err_info1_set;
+		}
+
+		/* register frame buffer */
+		pr_info("Ready to register framebuffer #%d.\n", fbidx);
+		errno = register_framebuffer(info[fbidx]);
+		if (errno < 0) {
+			pr_err("Failed to register fb_info #%d. err %d\n",
+			       fbidx,
+			       errno);
+			if (fbidx == 0)
+				goto err_register0;
+			else
+				goto err_register1;
+		}
+		pr_info("Accomplished register framebuffer #%d.\n", fbidx);
+	}
+
+	/* no dual view by far */
+	fbidx++;
+	if (share->dual && fbidx < 2)
+		goto ALLOC_FB;
+
+	return 0;
+
+err_register1:
+err_info1_set:
+	framebuffer_release(info[1]);
+err_info1_alloc:
+	unregister_framebuffer(info[0]);
+err_register0:
+err_info0_set:
+	framebuffer_release(info[0]);
+err_info0_alloc:
+err_map:
+	kfree(spec_share);
+err_share:
+err_enable:
+	return -ENODEV;
+}
+
+static void __exit lynxfb_pci_remove(struct pci_dev *pdev)
+{
+	struct fb_info *info;
+	struct lynx_share *share;
+	void *spec_share;
+	struct lynxfb_par *par;
+	int cnt;
+
+	cnt = 2;
+	share = pci_get_drvdata(pdev);
+
+	while (cnt-- > 0) {
+		info = share->fbinfo[cnt];
+		if (!info)
+			continue;
+		par = info->par;
+
+		unregister_framebuffer(info);
+		/* clean crtc & output allocations */
+		par->crtc.clear(&par->crtc);
+		par->output.clear(&par->output);
+		/* release frame buffer */
+		framebuffer_release(info);
+	}
+#ifdef CONFIG_MTRR
+	if (share->mtrr.vram_added)
+		mtrr_del(share->mtrr.vram,
+			 share->vidmem_start,
+			 share->vidmem_size);
+#endif
+
+	iounmap(share->pvReg);
+	iounmap(share->pvMem);
+	spec_share = container_of(share, struct sm750_share, share);
+	kfree(g_settings);
+	kfree(spec_share);
+	pci_set_drvdata(pdev, NULL);
+}
+
+static int __init lynxfb_setup(char *options)
+{
+	int len;
+	char *opt, *tmp;
+
+
+	if (!options || !*options) {
+		pr_warn("no options.\n");
+		return 0;
+	}
+
+	pr_info("options:%s\n", options);
+
+	len = strlen(options) + 1;
+	g_settings = kzalloc(len, GFP_KERNEL);
+	if (!g_settings)
+		return -ENOMEM;
+
+	tmp = g_settings;
+
+	/*	Notes:
+		char * strsep(char **s,const char * ct);
+		@s: the string to be searched
+		@ct :the characters to search for
+
+		strsep() updates @options to pointer after the first found token
+		it also returns the pointer ahead the token.
+		*/
+	while ((opt = strsep(&options, ":")) != NULL) {
+		/* options that mean for any lynx chips are configured here */
+		if (!strncmp(opt, "noaccel", strlen("noaccel")))
+			g_noaccel = 1;
+#ifdef CONFIG_MTRR
+		else if (!strncmp(opt, "nomtrr", strlen("nomtrr")))
+			g_nomtrr = 1;
+#endif
+		else if (!strncmp(opt, "dual", strlen("dual")))
+			g_dualview = 1;
+		else {
+			strcat(tmp, opt);
+			tmp += strlen(opt);
+			if (options != NULL)
+				*tmp++ = ':';
+			else
+				*tmp++ = 0;
+		}
+	}
+
+	/* misc g_settings are transport to chip specific routines */
+	pr_info("parameter left for chip specific analysis:%s\n", g_settings);
+	return 0;
+}
+
+static struct pci_device_id smi_pci_table[] = {
+	{ PCI_DEVICE(0x126f, 0x0750), },
+	{0,}
+};
+
+MODULE_DEVICE_TABLE(pci, smi_pci_table);
+
+static struct pci_driver lynxfb_driver = {
+	.name =		"sm750fb",
+	.id_table =	smi_pci_table,
+	.probe =	lynxfb_pci_probe,
+	.remove =	lynxfb_pci_remove,
+#ifdef CONFIG_PM
+	.suspend = lynxfb_suspend,
+	.resume = lynxfb_resume,
+#endif
+};
+
+
+static int __init lynxfb_init(void)
+{
+	char *option;
+	int ret;
+
+#ifdef MODULE
+	option = g_option;
+#else
+	if (fb_get_options("sm750fb", &option))
+		return -ENODEV;
+#endif
+
+	lynxfb_setup(option);
+	ret = pci_register_driver(&lynxfb_driver);
+	return ret;
+}
+module_init(lynxfb_init);
+
+static void __exit lynxfb_exit(void)
+{
+	pci_unregister_driver(&lynxfb_driver);
+}
+module_exit(lynxfb_exit);
+
+module_param(g_option, charp, S_IRUGO);
+
+MODULE_PARM_DESC(g_option,
+		 "\n\t\tCommon options:\n"
+		 "\t\tnoaccel:disable 2d capabilities\n"
+		 "\t\tnomtrr:disable MTRR attribute for video memory\n"
+		 "\t\tdualview:dual frame buffer feature enabled\n"
+		 "\t\tnohwc:disable hardware cursor\n"
+		 "\t\tUsual example:\n"
+		 "\t\tinsmod ./sm750fb.ko g_option=\"noaccel,nohwc,1280x1024-8@60\"\n"
+		 );
+
+MODULE_AUTHOR("monk liu <monk.liu@siliconmotion.com>");
+MODULE_AUTHOR("Sudip Mukherjee <sudip@vectorindia.org>");
+MODULE_DESCRIPTION("Frame buffer driver for SM750 chipset");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/staging/sm750fb/sm750.h b/drivers/staging/sm750fb/sm750.h
new file mode 100644
index 000000000000..0847d2bd95c8
--- /dev/null
+++ b/drivers/staging/sm750fb/sm750.h
@@ -0,0 +1,186 @@
+#ifndef LYNXDRV_H_
+#define LYNXDRV_H_
+
+
+
+#define FB_ACCEL_SMI 0xab
+/* please use revision id to distinguish sm750le and sm750*/
+#define SPC_SM750 	0
+
+//#define SPC_SM750LE 8
+
+#define MB(x) ((x)<<20)
+#define MHZ(x) ((x) * 1000000)
+/* align should be 2,4,8,16 */
+#define PADDING(align,data) (((data)+(align)-1)&(~((align) -1)))
+extern int smi_indent;
+
+
+struct lynx_accel{
+	/* base virtual address of DPR registers */
+	volatile unsigned char __iomem * dprBase;
+	/* base virtual address of de data port */
+	volatile unsigned char __iomem * dpPortBase;
+
+	/* function fointers */
+	void (*de_init)(struct lynx_accel *);
+
+	int (*de_wait)(void);/* see if hardware ready to work */
+
+	int (*de_fillrect)(struct lynx_accel *,u32,u32,u32,
+							u32,u32,u32,u32,u32,u32);
+
+	int (*de_copyarea)(struct lynx_accel *,u32,u32,u32,u32,
+						u32,u32,u32,u32,
+						u32,u32,u32,u32);
+
+	int (*de_imageblit)(struct lynx_accel *,const char *,u32,u32,u32,
+						u32,u32,u32,u32,u32,u32,u32,u32,u32);
+
+};
+
+/* 	lynx_share stands for a presentation of two frame buffer
+	that use one smi adaptor , it is similar to a basic class of C++
+*/
+struct lynx_share{
+	/* common members */
+	u16 devid;
+	u8 revid;
+	struct pci_dev * pdev;
+	struct fb_info * fbinfo[2];
+	struct lynx_accel accel;
+	int accel_off;
+	int dual;
+#ifdef CONFIG_MTRR
+		int mtrr_off;
+		struct{
+			int vram;
+			int vram_added;
+		}mtrr;
+#endif
+	/* all smi graphic adaptor got below attributes */
+	unsigned long vidmem_start;
+	unsigned long vidreg_start;
+	__u32 vidmem_size;
+	__u32 vidreg_size;
+	void __iomem * pvReg;
+	unsigned char __iomem * pvMem;
+	/* locks*/
+	spinlock_t slock;
+	/* function pointers */
+	void (*suspend)(struct lynx_share*);
+	void (*resume)(struct lynx_share*);
+};
+
+struct lynx_cursor{
+	/* cursor width ,height and size */
+	int w;
+	int h;
+	int size;
+	/* hardware limitation */
+	int maxW;
+	int maxH;
+	/* base virtual address and offset  of cursor image */
+	char __iomem * vstart;
+	int offset;
+	/* mmio addr of hw cursor */
+	volatile char __iomem * mmio;
+	/* the lynx_share of this adaptor */
+	struct lynx_share * share;
+	/* proc_routines */
+	void (*enable)(struct lynx_cursor *);
+	void (*disable)(struct lynx_cursor *);
+	void (*setSize)(struct lynx_cursor *,int,int);
+	void (*setPos)(struct lynx_cursor *,int,int);
+	void (*setColor)(struct lynx_cursor *,u32,u32);
+	void (*setData)(struct lynx_cursor *,u16,const u8*,const u8*);
+};
+
+struct lynxfb_crtc{
+	unsigned char __iomem * vCursor;//virtual address of cursor
+	unsigned char __iomem * vScreen;//virtual address of on_screen
+	int oCursor;//cursor address offset in vidmem
+	int oScreen;//onscreen address offset in vidmem
+	int channel;/* which channel this crtc stands for*/
+	resource_size_t vidmem_size;/* this view's video memory max size */
+
+	/* below attributes belong to info->fix, their value depends on specific adaptor*/
+	u16 line_pad;/* padding information:0,1,2,4,8,16,... */
+	u16 xpanstep;
+	u16 ypanstep;
+	u16 ywrapstep;
+
+	void * priv;
+
+	int(*proc_setMode)(struct lynxfb_crtc*,
+						struct fb_var_screeninfo*,
+						struct fb_fix_screeninfo*);
+
+	int(*proc_checkMode)(struct lynxfb_crtc*,struct fb_var_screeninfo*);
+	int(*proc_setColReg)(struct lynxfb_crtc*,ushort,ushort,ushort,ushort);
+	void (*clear)(struct lynxfb_crtc*);
+        /* pan display */
+	int (*proc_panDisplay)(struct lynxfb_crtc *,
+			       const struct fb_var_screeninfo *,
+			       const struct fb_info *);
+	/* cursor information */
+	struct lynx_cursor cursor;
+};
+
+struct lynxfb_output{
+	int dpms;
+	int paths;
+	/* 	which paths(s) this output stands for,for sm750:
+		paths=1:means output for panel paths
+		paths=2:means output for crt paths
+		paths=3:means output for both panel and crt paths
+	*/
+
+	int * channel;
+	/* 	which channel these outputs linked with,for sm750:
+		*channel=0 means primary channel
+		*channel=1 means secondary channel
+		output->channel ==> &crtc->channel
+	*/
+	void * priv;
+
+	int(*proc_setMode)(struct lynxfb_output*,
+						struct fb_var_screeninfo*,
+						struct fb_fix_screeninfo*);
+
+	int(*proc_checkMode)(struct lynxfb_output*,struct fb_var_screeninfo*);
+	int(*proc_setBLANK)(struct lynxfb_output*,int);
+	void  (*clear)(struct lynxfb_output*);
+};
+
+struct lynxfb_par{
+	/* either 0 or 1 for dual head adaptor,0 is the older one registered */
+	int index;
+	unsigned int pseudo_palette[256];
+	struct lynxfb_crtc crtc;
+	struct lynxfb_output output;
+	struct fb_info * info;
+	struct lynx_share * share;
+};
+
+#ifndef offsetof
+#define offsetof(TYPE, MEMBER) ((size_t) &((TYPE *)0)->MEMBER)
+#endif
+
+
+#define PS_TO_HZ(ps)	\
+			({ 	\
+			unsigned long long hz = 1000*1000*1000*1000ULL;	\
+			do_div(hz,ps);	\
+			(unsigned long)hz;})
+
+static inline unsigned long ps_to_hz(unsigned int psvalue)
+{
+	unsigned long long numerator=1000*1000*1000*1000ULL;
+	/* 10^12 / picosecond period gives frequency in Hz */
+	do_div(numerator, psvalue);
+	return (unsigned long)numerator;
+}
+
+
+#endif
diff --git a/drivers/staging/sm750fb/sm750_accel.c b/drivers/staging/sm750fb/sm750_accel.c
new file mode 100644
index 000000000000..c5a372690737
--- /dev/null
+++ b/drivers/staging/sm750fb/sm750_accel.c
@@ -0,0 +1,437 @@
+#include<linux/module.h>
+#include<linux/kernel.h>
+#include<linux/errno.h>
+#include<linux/string.h>
+#include<linux/mm.h>
+#include<linux/slab.h>
+#include<linux/delay.h>
+#include<linux/fb.h>
+#include<linux/ioport.h>
+#include<linux/init.h>
+#include<linux/pci.h>
+#include<linux/vmalloc.h>
+#include<linux/pagemap.h>
+#include <linux/console.h>
+#include<linux/platform_device.h>
+#include<linux/screen_info.h>
+
+#include "sm750.h"
+#include "sm750_accel.h"
+#include "sm750_help.h"
+static inline void write_dpr(struct lynx_accel * accel,int offset,u32 regValue)
+{
+	writel(regValue,accel->dprBase + offset);
+}
+
+static inline u32 read_dpr(struct lynx_accel * accel,int offset)
+{
+	return readl(accel->dprBase + offset);
+}
+
+static inline void write_dpPort(struct lynx_accel * accel,u32 data)
+{
+	writel(data,accel->dpPortBase);
+}
+
+void hw_de_init(struct lynx_accel * accel)
+{
+	/* setup 2d engine registers */
+	u32 reg,clr;
+	
+	write_dpr(accel,DE_MASKS,0xFFFFFFFF);
+
+	/* dpr1c */
+	reg = FIELD_SET(0,DE_STRETCH_FORMAT,PATTERN_XY,NORMAL)|
+		FIELD_VALUE(0,DE_STRETCH_FORMAT,PATTERN_Y,0)|
+		FIELD_VALUE(0,DE_STRETCH_FORMAT,PATTERN_X,0)|
+		FIELD_SET(0,DE_STRETCH_FORMAT,ADDRESSING,XY)|
+		FIELD_VALUE(0,DE_STRETCH_FORMAT,SOURCE_HEIGHT,3);
+
+	clr = FIELD_CLEAR(DE_STRETCH_FORMAT,PATTERN_XY)&
+		FIELD_CLEAR(DE_STRETCH_FORMAT,PATTERN_Y)&
+		FIELD_CLEAR(DE_STRETCH_FORMAT,PATTERN_X)&
+		FIELD_CLEAR(DE_STRETCH_FORMAT,ADDRESSING)&
+		FIELD_CLEAR(DE_STRETCH_FORMAT,SOURCE_HEIGHT);
+
+	/* DE_STRETCH bpp format need be initilized in setMode routine */
+	write_dpr(accel,DE_STRETCH_FORMAT,(read_dpr(accel,DE_STRETCH_FORMAT) & clr) | reg);
+
+	/* disable clipping and transparent */
+	write_dpr(accel,DE_CLIP_TL,0);//dpr2c
+	write_dpr(accel,DE_CLIP_BR,0);//dpr30
+
+	write_dpr(accel,DE_COLOR_COMPARE_MASK,0);//dpr24
+	write_dpr(accel,DE_COLOR_COMPARE,0);
+
+	reg = FIELD_SET(0,DE_CONTROL,TRANSPARENCY,DISABLE)|
+		FIELD_SET(0,DE_CONTROL,TRANSPARENCY_MATCH,OPAQUE)|
+		FIELD_SET(0,DE_CONTROL,TRANSPARENCY_SELECT,SOURCE);
+
+	clr = FIELD_CLEAR(DE_CONTROL,TRANSPARENCY)&
+		FIELD_CLEAR(DE_CONTROL,TRANSPARENCY_MATCH)&
+		FIELD_CLEAR(DE_CONTROL,TRANSPARENCY_SELECT);
+
+	/* dpr0c */
+	write_dpr(accel,DE_CONTROL,(read_dpr(accel,DE_CONTROL)&clr)|reg);
+}
+
+/* set2dformat only be called from setmode functions
+ * but if you need dual framebuffer driver,need call set2dformat
+ * every time you use 2d function */
+
+void hw_set2dformat(struct lynx_accel * accel,int fmt)
+{
+	u32 reg;
+	
+	/* fmt=0,1,2 for 8,16,32,bpp on sm718/750/502 */
+	reg = read_dpr(accel,DE_STRETCH_FORMAT);
+	reg = FIELD_VALUE(reg,DE_STRETCH_FORMAT,PIXEL_FORMAT,fmt);
+	write_dpr(accel,DE_STRETCH_FORMAT,reg);
+}
+
+int hw_fillrect(struct lynx_accel * accel,
+				u32 base,u32 pitch,u32 Bpp,
+				u32 x,u32 y,u32 width,u32 height,
+				u32 color,u32 rop)
+{
+	u32 deCtrl;
+
+	if(accel->de_wait() != 0)
+	{
+		/* int time wait and always busy,seems hardware
+		 * got something error */
+		pr_debug("%s:De engine always bussy\n",__func__);
+		return -1;
+	}
+
+	write_dpr(accel,DE_WINDOW_DESTINATION_BASE,base);//dpr40
+	write_dpr(accel,DE_PITCH,
+			FIELD_VALUE(0,DE_PITCH,DESTINATION,pitch/Bpp)|
+			FIELD_VALUE(0,DE_PITCH,SOURCE,pitch/Bpp));//dpr10
+
+	write_dpr(accel,DE_WINDOW_WIDTH,
+			FIELD_VALUE(0,DE_WINDOW_WIDTH,DESTINATION,pitch/Bpp)|
+			FIELD_VALUE(0,DE_WINDOW_WIDTH,SOURCE,pitch/Bpp));//dpr44
+
+	write_dpr(accel,DE_FOREGROUND,color);//DPR14
+
+	write_dpr(accel,DE_DESTINATION,
+			FIELD_SET(0,DE_DESTINATION,WRAP,DISABLE)|
+			FIELD_VALUE(0,DE_DESTINATION,X,x)|
+			FIELD_VALUE(0,DE_DESTINATION,Y,y));//dpr4
+
+	write_dpr(accel,DE_DIMENSION,
+			FIELD_VALUE(0,DE_DIMENSION,X,width)|
+			FIELD_VALUE(0,DE_DIMENSION,Y_ET,height));//dpr8
+
+	deCtrl =
+		FIELD_SET(0,DE_CONTROL,STATUS,START)|
+		FIELD_SET(0,DE_CONTROL,DIRECTION,LEFT_TO_RIGHT)|
+		FIELD_SET(0,DE_CONTROL,LAST_PIXEL,ON)|
+		FIELD_SET(0,DE_CONTROL,COMMAND,RECTANGLE_FILL)|
+		FIELD_SET(0,DE_CONTROL,ROP_SELECT,ROP2)|
+		FIELD_VALUE(0,DE_CONTROL,ROP,rop);//dpr0xc
+
+	write_dpr(accel,DE_CONTROL,deCtrl);
+	return 0;
+}
+
+int hw_copyarea(
+struct lynx_accel * accel,
+unsigned int sBase,  /* Address of source: offset in frame buffer */
+unsigned int sPitch, /* Pitch value of source surface in BYTE */
+unsigned int sx,
+unsigned int sy,     /* Starting coordinate of source surface */
+unsigned int dBase,  /* Address of destination: offset in frame buffer */
+unsigned int dPitch, /* Pitch value of destination surface in BYTE */
+unsigned int Bpp,    /* Color depth of destination surface */
+unsigned int dx,
+unsigned int dy,     /* Starting coordinate of destination surface */
+unsigned int width,
+unsigned int height, /* width and height of rectangle in pixel value */
+unsigned int rop2)   /* ROP value */
+{
+    unsigned int nDirection, de_ctrl;
+    int opSign;
+    nDirection = LEFT_TO_RIGHT;
+	/* Direction of ROP2 operation: 1 = Left to Right, (-1) = Right to Left */
+    opSign = 1;
+    de_ctrl = 0;
+
+    /* If source and destination are the same surface, need to check for overlay cases */
+    if (sBase == dBase && sPitch == dPitch)
+    {
+        /* Determine direction of operation */
+        if (sy < dy)
+        {
+            /* +----------+
+               |S         |
+               |   +----------+
+               |   |      |   |
+               |   |      |   |
+               +---|------+   |
+                   |         D|
+                   +----------+ */
+
+            nDirection = BOTTOM_TO_TOP;
+        }
+        else if (sy > dy)
+        {
+            /* +----------+
+               |D         |
+               |   +----------+
+               |   |      |   |
+               |   |      |   |
+               +---|------+   |
+                   |         S|
+                   +----------+ */
+
+            nDirection = TOP_TO_BOTTOM;
+        }
+        else
+        {
+            /* sy == dy */
+
+            if (sx <= dx)
+            {
+                /* +------+---+------+
+                   |S     |   |     D|
+                   |      |   |      |
+                   |      |   |      |
+                   |      |   |      |
+                   +------+---+------+ */
+
+                nDirection = RIGHT_TO_LEFT;
+            }
+            else
+            {
+                /* sx > dx */
+
+                /* +------+---+------+
+                   |D     |   |     S|
+                   |      |   |      |
+                   |      |   |      |
+                   |      |   |      |
+                   +------+---+------+ */
+
+                nDirection = LEFT_TO_RIGHT;
+            }
+        }
+    }
+
+    if ((nDirection == BOTTOM_TO_TOP) || (nDirection == RIGHT_TO_LEFT))
+    {
+        sx += width - 1;
+        sy += height - 1;
+        dx += width - 1;
+        dy += height - 1;
+        opSign = (-1);
+    }
+
+    /* Note:
+       DE_FOREGROUND are DE_BACKGROUND are don't care.
+       DE_COLOR_COMPARE and DE_COLOR_COMPARE_MAKS are set by set deSetTransparency().
+    */
+
+    /* 2D Source Base.
+       It is an address offset (128 bit aligned) from the beginning of frame buffer.
+    */
+    write_dpr(accel,DE_WINDOW_SOURCE_BASE, sBase);//dpr40
+
+    /* 2D Destination Base.
+       It is an address offset (128 bit aligned) from the beginning of frame buffer.
+    */
+    write_dpr(accel,DE_WINDOW_DESTINATION_BASE, dBase);//dpr44
+
+#if 0
+    /* Program pitch (distance between the 1st points of two adjacent lines).
+       Note that input pitch is BYTE value, but the 2D Pitch register uses
+       pixel values. Need Byte to pixel convertion.
+    */
+	if(Bpp == 3){
+			sx *= 3;
+			dx *= 3;
+			width *= 3;
+		write_dpr(accel,DE_PITCH,
+				FIELD_VALUE(0, DE_PITCH, DESTINATION, dPitch) |
+				FIELD_VALUE(0, DE_PITCH, SOURCE,      sPitch));//dpr10
+	}
+	else
+#endif
+	{
+		write_dpr(accel,DE_PITCH,
+				FIELD_VALUE(0, DE_PITCH, DESTINATION, (dPitch/Bpp)) |
+				FIELD_VALUE(0, DE_PITCH, SOURCE,      (sPitch/Bpp)));//dpr10
+	}
+
+    /* Screen Window width in Pixels.
+       2D engine uses this value to calculate the linear address in frame buffer for a given point.
+    */
+    write_dpr(accel,DE_WINDOW_WIDTH,
+        FIELD_VALUE(0, DE_WINDOW_WIDTH, DESTINATION, (dPitch/Bpp)) |
+        FIELD_VALUE(0, DE_WINDOW_WIDTH, SOURCE,      (sPitch/Bpp)));//dpr3c
+
+	if (accel->de_wait() != 0){
+		return -1;
+	}
+
+    {
+
+        write_dpr(accel,DE_SOURCE,
+            FIELD_SET  (0, DE_SOURCE, WRAP, DISABLE) |
+            FIELD_VALUE(0, DE_SOURCE, X_K1, sx)   |
+            FIELD_VALUE(0, DE_SOURCE, Y_K2, sy));//dpr0
+        write_dpr(accel,DE_DESTINATION,
+            FIELD_SET  (0, DE_DESTINATION, WRAP, DISABLE) |
+            FIELD_VALUE(0, DE_DESTINATION, X,    dx)  |
+            FIELD_VALUE(0, DE_DESTINATION, Y,    dy));//dpr04
+        write_dpr(accel,DE_DIMENSION,
+            FIELD_VALUE(0, DE_DIMENSION, X,    width) |
+            FIELD_VALUE(0, DE_DIMENSION, Y_ET, height));//dpr08
+
+        de_ctrl =
+            FIELD_VALUE(0, DE_CONTROL, ROP, rop2) |
+            FIELD_SET(0, DE_CONTROL, ROP_SELECT, ROP2) |
+            FIELD_SET(0, DE_CONTROL, COMMAND, BITBLT) |
+            ((nDirection == RIGHT_TO_LEFT) ?
+            FIELD_SET(0, DE_CONTROL, DIRECTION, RIGHT_TO_LEFT)
+            : FIELD_SET(0, DE_CONTROL, DIRECTION, LEFT_TO_RIGHT)) |
+            FIELD_SET(0, DE_CONTROL, STATUS, START);
+		write_dpr(accel,DE_CONTROL,de_ctrl);//dpr0c
+    }
+
+    return 0;
+}
+
+static unsigned int deGetTransparency(struct lynx_accel * accel)
+{
+    unsigned int de_ctrl;
+
+    de_ctrl = read_dpr(accel,DE_CONTROL);
+
+    de_ctrl &=
+        FIELD_MASK(DE_CONTROL_TRANSPARENCY_MATCH) |
+        FIELD_MASK(DE_CONTROL_TRANSPARENCY_SELECT)|
+        FIELD_MASK(DE_CONTROL_TRANSPARENCY);
+
+    return de_ctrl;
+}
+
+int hw_imageblit(struct lynx_accel *accel,
+		 const char *pSrcbuf, /* pointer to start of source buffer in system memory */
+		 u32 srcDelta,          /* Pitch value (in bytes) of the source buffer, +ive means top down and -ive mean button up */
+		 u32 startBit, /* Mono data can start at any bit in a byte, this value should be 0 to 7 */
+		 u32 dBase,    /* Address of destination: offset in frame buffer */
+		 u32 dPitch,   /* Pitch value of destination surface in BYTE */
+		 u32 bytePerPixel,      /* Color depth of destination surface */
+		 u32 dx,
+		 u32 dy,       /* Starting coordinate of destination surface */
+		 u32 width,
+		 u32 height,   /* width and height of rectange in pixel value */
+		 u32 fColor,   /* Foreground color (corresponding to a 1 in the monochrome data */
+		 u32 bColor,   /* Background color (corresponding to a 0 in the monochrome data */
+		 u32 rop2)     /* ROP value */
+{
+    unsigned int ulBytesPerScan;
+    unsigned int ul4BytesPerScan;
+    unsigned int ulBytesRemain;
+    unsigned int de_ctrl = 0;
+    unsigned char ajRemain[4];
+    int i, j;
+
+    startBit &= 7; /* Just make sure the start bit is within legal range */
+    ulBytesPerScan = (width + startBit + 7) / 8;
+    ul4BytesPerScan = ulBytesPerScan & ~3;
+    ulBytesRemain = ulBytesPerScan & 3;
+
+	if(accel->de_wait() != 0)
+    {
+//		inf_msg("*** ImageBlit return -1 ***\n");
+        return -1;
+    }
+
+    /* 2D Source Base.
+       Use 0 for HOST Blt.
+    */
+    write_dpr(accel,DE_WINDOW_SOURCE_BASE, 0);
+
+    /* 2D Destination Base.
+       It is an address offset (128 bit aligned) from the beginning of frame buffer.
+    */
+    write_dpr(accel,DE_WINDOW_DESTINATION_BASE, dBase);
+#if 0
+    /* Program pitch (distance between the 1st points of two adjacent lines).
+       Note that input pitch is BYTE value, but the 2D Pitch register uses
+       pixel values. Need Byte to pixel convertion.
+    */
+	if(bytePerPixel == 3 ){
+		dx *= 3;
+		width *= 3;
+		startBit *= 3;
+		write_dpr(accel,DE_PITCH,
+				FIELD_VALUE(0, DE_PITCH, DESTINATION, dPitch) |
+				FIELD_VALUE(0, DE_PITCH, SOURCE,      dPitch));//dpr10
+
+	}
+	else
+#endif
+	{
+		write_dpr(accel,DE_PITCH,
+				FIELD_VALUE(0, DE_PITCH, DESTINATION, dPitch/bytePerPixel) |
+				FIELD_VALUE(0, DE_PITCH, SOURCE,      dPitch/bytePerPixel));//dpr10
+	}
+
+    /* Screen Window width in Pixels.
+       2D engine uses this value to calculate the linear address in frame buffer for a given point.
+    */
+    write_dpr(accel,DE_WINDOW_WIDTH,
+        FIELD_VALUE(0, DE_WINDOW_WIDTH, DESTINATION, (dPitch/bytePerPixel)) |
+        FIELD_VALUE(0, DE_WINDOW_WIDTH, SOURCE,      (dPitch/bytePerPixel)));
+
+    /* Note: For 2D Source in Host Write, only X_K1_MONO field is needed, and Y_K2 field is not used.
+             For mono bitmap, use startBit for X_K1. */
+    write_dpr(accel,DE_SOURCE,
+        FIELD_SET  (0, DE_SOURCE, WRAP, DISABLE)       |
+        FIELD_VALUE(0, DE_SOURCE, X_K1_MONO, startBit));//dpr00
+
+    write_dpr(accel,DE_DESTINATION,
+        FIELD_SET  (0, DE_DESTINATION, WRAP, DISABLE) |
+        FIELD_VALUE(0, DE_DESTINATION, X,    dx)    |
+        FIELD_VALUE(0, DE_DESTINATION, Y,    dy));//dpr04
+
+    write_dpr(accel,DE_DIMENSION,
+        FIELD_VALUE(0, DE_DIMENSION, X,    width) |
+        FIELD_VALUE(0, DE_DIMENSION, Y_ET, height));//dpr08
+
+    write_dpr(accel,DE_FOREGROUND, fColor);
+    write_dpr(accel,DE_BACKGROUND, bColor);
+
+	de_ctrl = FIELD_VALUE(0, DE_CONTROL, ROP, rop2)         |
+		FIELD_SET(0, DE_CONTROL, ROP_SELECT, ROP2)    |
+		FIELD_SET(0, DE_CONTROL, COMMAND, HOST_WRITE) |
+		FIELD_SET(0, DE_CONTROL, HOST, MONO)          |
+		FIELD_SET(0, DE_CONTROL, STATUS, START);
+
+	write_dpr(accel,DE_CONTROL, de_ctrl | deGetTransparency(accel));
+
+    /* Write MONO data (line by line) to 2D Engine data port */
+    for (i=0; i<height; i++)
+    {
+        /* For each line, send the data in chunks of 4 bytes */
+        for (j=0; j<(ul4BytesPerScan/4); j++)
+        {
+            write_dpPort(accel, *(unsigned int *)(pSrcbuf + (j * 4)));
+        }
+
+        if (ulBytesRemain)
+        {
+            memcpy(ajRemain, pSrcbuf+ul4BytesPerScan, ulBytesRemain);
+            write_dpPort(accel, *(unsigned int *)ajRemain);
+        }
+
+        pSrcbuf += srcDelta;
+    }
+
+    return 0;
+}
+
diff --git a/drivers/staging/sm750fb/sm750_accel.h b/drivers/staging/sm750fb/sm750_accel.h
new file mode 100644
index 000000000000..3ee0bd89257b
--- /dev/null
+++ b/drivers/staging/sm750fb/sm750_accel.h
@@ -0,0 +1,275 @@
+#ifndef ACCEL_H__
+#define ACCEL_H__
+
+#define HW_ROP2_COPY 0xc
+#define HW_ROP2_XOR 0x6
+
+/* notes: below address are the offset value from de_base_address (0x100000)*/
+
+/* for sm718/750/502 de_base is at mmreg_1mb*/
+#define DE_BASE_ADDR_TYPE1 	0x100000
+/* for sm712,de_base is at mmreg_32kb */
+#define DE_BASE_ADDR_TYPE2  0x8000
+/* for sm722,de_base is at mmreg_0 */
+#define DE_BASE_ADDR_TYPE3 0
+
+/* type1 data port address is at mmreg_0x110000*/
+#define DE_PORT_ADDR_TYPE1 0x110000
+/* for sm712,data port address is at mmreg_0 */
+#define DE_PORT_ADDR_TYPE2 0x100000
+/* for sm722,data port address is at mmreg_1mb */
+#define DE_PORT_ADDR_TYPE3 0x100000
+
+#define DE_SOURCE                                       0x0
+#define DE_SOURCE_WRAP                                  31:31
+#define DE_SOURCE_WRAP_DISABLE                          0
+#define DE_SOURCE_WRAP_ENABLE                           1
+#define DE_SOURCE_X_K1                                  29:16
+#define DE_SOURCE_Y_K2                                  15:0
+#define DE_SOURCE_X_K1_MONO 							20:16
+
+#define DE_DESTINATION                                  0x4
+#define DE_DESTINATION_WRAP                             31:31
+#define DE_DESTINATION_WRAP_DISABLE                     0
+#define DE_DESTINATION_WRAP_ENABLE                      1
+#define DE_DESTINATION_X                                28:16
+#define DE_DESTINATION_Y                                15:0
+
+#define DE_DIMENSION                                    0x8
+#define DE_DIMENSION_X                                  28:16
+#define DE_DIMENSION_Y_ET                               15:0
+
+#define DE_CONTROL                                      0xC
+#define DE_CONTROL_STATUS                               31:31
+#define DE_CONTROL_STATUS_STOP                          0
+#define DE_CONTROL_STATUS_START                         1
+#define DE_CONTROL_PATTERN                              30:30
+#define DE_CONTROL_PATTERN_MONO                         0
+#define DE_CONTROL_PATTERN_COLOR                        1
+#define DE_CONTROL_UPDATE_DESTINATION_X                 29:29
+#define DE_CONTROL_UPDATE_DESTINATION_X_DISABLE         0
+#define DE_CONTROL_UPDATE_DESTINATION_X_ENABLE          1
+#define DE_CONTROL_QUICK_START                          28:28
+#define DE_CONTROL_QUICK_START_DISABLE                  0
+#define DE_CONTROL_QUICK_START_ENABLE                   1
+#define DE_CONTROL_DIRECTION                            27:27
+#define DE_CONTROL_DIRECTION_LEFT_TO_RIGHT              0
+#define DE_CONTROL_DIRECTION_RIGHT_TO_LEFT              1
+#define DE_CONTROL_MAJOR                                26:26
+#define DE_CONTROL_MAJOR_X                              0
+#define DE_CONTROL_MAJOR_Y                              1
+#define DE_CONTROL_STEP_X                               25:25
+#define DE_CONTROL_STEP_X_POSITIVE                      1
+#define DE_CONTROL_STEP_X_NEGATIVE                      0
+#define DE_CONTROL_STEP_Y                               24:24
+#define DE_CONTROL_STEP_Y_POSITIVE                      1
+#define DE_CONTROL_STEP_Y_NEGATIVE                      0
+#define DE_CONTROL_STRETCH                              23:23
+#define DE_CONTROL_STRETCH_DISABLE                      0
+#define DE_CONTROL_STRETCH_ENABLE                       1
+#define DE_CONTROL_HOST                                 22:22
+#define DE_CONTROL_HOST_COLOR                           0
+#define DE_CONTROL_HOST_MONO                            1
+#define DE_CONTROL_LAST_PIXEL                           21:21
+#define DE_CONTROL_LAST_PIXEL_OFF                       0
+#define DE_CONTROL_LAST_PIXEL_ON                        1
+#define DE_CONTROL_COMMAND                              20:16
+#define DE_CONTROL_COMMAND_BITBLT                       0
+#define DE_CONTROL_COMMAND_RECTANGLE_FILL               1
+#define DE_CONTROL_COMMAND_DE_TILE                      2
+#define DE_CONTROL_COMMAND_TRAPEZOID_FILL               3
+#define DE_CONTROL_COMMAND_ALPHA_BLEND                  4
+#define DE_CONTROL_COMMAND_RLE_STRIP                    5
+#define DE_CONTROL_COMMAND_SHORT_STROKE                 6
+#define DE_CONTROL_COMMAND_LINE_DRAW                    7
+#define DE_CONTROL_COMMAND_HOST_WRITE                   8
+#define DE_CONTROL_COMMAND_HOST_READ                    9
+#define DE_CONTROL_COMMAND_HOST_WRITE_BOTTOM_UP         10
+#define DE_CONTROL_COMMAND_ROTATE                       11
+#define DE_CONTROL_COMMAND_FONT                         12
+#define DE_CONTROL_COMMAND_TEXTURE_LOAD                 15
+#define DE_CONTROL_ROP_SELECT                           15:15
+#define DE_CONTROL_ROP_SELECT_ROP3                      0
+#define DE_CONTROL_ROP_SELECT_ROP2                      1
+#define DE_CONTROL_ROP2_SOURCE                          14:14
+#define DE_CONTROL_ROP2_SOURCE_BITMAP                   0
+#define DE_CONTROL_ROP2_SOURCE_PATTERN                  1
+#define DE_CONTROL_MONO_DATA                            13:12
+#define DE_CONTROL_MONO_DATA_NOT_PACKED                 0
+#define DE_CONTROL_MONO_DATA_8_PACKED                   1
+#define DE_CONTROL_MONO_DATA_16_PACKED                  2
+#define DE_CONTROL_MONO_DATA_32_PACKED                  3
+#define DE_CONTROL_REPEAT_ROTATE                        11:11
+#define DE_CONTROL_REPEAT_ROTATE_DISABLE                0
+#define DE_CONTROL_REPEAT_ROTATE_ENABLE                 1
+#define DE_CONTROL_TRANSPARENCY_MATCH                   10:10
+#define DE_CONTROL_TRANSPARENCY_MATCH_OPAQUE            0
+#define DE_CONTROL_TRANSPARENCY_MATCH_TRANSPARENT       1
+#define DE_CONTROL_TRANSPARENCY_SELECT                  9:9
+#define DE_CONTROL_TRANSPARENCY_SELECT_SOURCE           0
+#define DE_CONTROL_TRANSPARENCY_SELECT_DESTINATION      1
+#define DE_CONTROL_TRANSPARENCY                         8:8
+#define DE_CONTROL_TRANSPARENCY_DISABLE                 0
+#define DE_CONTROL_TRANSPARENCY_ENABLE                  1
+#define DE_CONTROL_ROP                                  7:0
+
+// Pseudo fields.
+
+#define DE_CONTROL_SHORT_STROKE_DIR                     27:24
+#define DE_CONTROL_SHORT_STROKE_DIR_225                 0
+#define DE_CONTROL_SHORT_STROKE_DIR_135                 1
+#define DE_CONTROL_SHORT_STROKE_DIR_315                 2
+#define DE_CONTROL_SHORT_STROKE_DIR_45                  3
+#define DE_CONTROL_SHORT_STROKE_DIR_270                 4
+#define DE_CONTROL_SHORT_STROKE_DIR_90                  5
+#define DE_CONTROL_SHORT_STROKE_DIR_180                 8
+#define DE_CONTROL_SHORT_STROKE_DIR_0                   10
+#define DE_CONTROL_ROTATION                             25:24
+#define DE_CONTROL_ROTATION_0                           0
+#define DE_CONTROL_ROTATION_270                         1
+#define DE_CONTROL_ROTATION_90                          2
+#define DE_CONTROL_ROTATION_180                         3
+
+#define DE_PITCH                                        0x000010
+#define DE_PITCH_DESTINATION                            28:16
+#define DE_PITCH_SOURCE                                 12:0
+
+#define DE_FOREGROUND                                   0x000014
+#define DE_FOREGROUND_COLOR                             31:0
+
+#define DE_BACKGROUND                                   0x000018
+#define DE_BACKGROUND_COLOR                             31:0
+
+#define DE_STRETCH_FORMAT                               0x00001C
+#define DE_STRETCH_FORMAT_PATTERN_XY                    30:30
+#define DE_STRETCH_FORMAT_PATTERN_XY_NORMAL             0
+#define DE_STRETCH_FORMAT_PATTERN_XY_OVERWRITE          1
+#define DE_STRETCH_FORMAT_PATTERN_Y                     29:27
+#define DE_STRETCH_FORMAT_PATTERN_X                     25:23
+#define DE_STRETCH_FORMAT_PIXEL_FORMAT                  21:20
+#define DE_STRETCH_FORMAT_PIXEL_FORMAT_8                0
+#define DE_STRETCH_FORMAT_PIXEL_FORMAT_16               1
+#define DE_STRETCH_FORMAT_PIXEL_FORMAT_32               2
+#define DE_STRETCH_FORMAT_PIXEL_FORMAT_24               3
+
+#define DE_STRETCH_FORMAT_ADDRESSING                    19:16
+#define DE_STRETCH_FORMAT_ADDRESSING_XY                 0
+#define DE_STRETCH_FORMAT_ADDRESSING_LINEAR             15
+#define DE_STRETCH_FORMAT_SOURCE_HEIGHT                 11:0
+
+#define DE_COLOR_COMPARE                                0x000020
+#define DE_COLOR_COMPARE_COLOR                          23:0
+
+#define DE_COLOR_COMPARE_MASK                           0x000024
+#define DE_COLOR_COMPARE_MASK_MASKS                     23:0
+
+#define DE_MASKS                                        0x000028
+#define DE_MASKS_BYTE_MASK                              31:16
+#define DE_MASKS_BIT_MASK                               15:0
+
+#define DE_CLIP_TL                                      0x00002C
+#define DE_CLIP_TL_TOP                                  31:16
+#define DE_CLIP_TL_STATUS                               13:13
+#define DE_CLIP_TL_STATUS_DISABLE                       0
+#define DE_CLIP_TL_STATUS_ENABLE                        1
+#define DE_CLIP_TL_INHIBIT                              12:12
+#define DE_CLIP_TL_INHIBIT_OUTSIDE                      0
+#define DE_CLIP_TL_INHIBIT_INSIDE                       1
+#define DE_CLIP_TL_LEFT                                 11:0
+
+#define DE_CLIP_BR                                      0x000030
+#define DE_CLIP_BR_BOTTOM                               31:16
+#define DE_CLIP_BR_RIGHT                                12:0
+
+#define DE_MONO_PATTERN_LOW                             0x000034
+#define DE_MONO_PATTERN_LOW_PATTERN                     31:0
+
+#define DE_MONO_PATTERN_HIGH                            0x000038
+#define DE_MONO_PATTERN_HIGH_PATTERN                    31:0
+
+#define DE_WINDOW_WIDTH                                 0x00003C
+#define DE_WINDOW_WIDTH_DESTINATION                     28:16
+#define DE_WINDOW_WIDTH_SOURCE                          12:0
+
+#define DE_WINDOW_SOURCE_BASE                           0x000040
+#define DE_WINDOW_SOURCE_BASE_EXT                       27:27
+#define DE_WINDOW_SOURCE_BASE_EXT_LOCAL                 0
+#define DE_WINDOW_SOURCE_BASE_EXT_EXTERNAL              1
+#define DE_WINDOW_SOURCE_BASE_CS                        26:26
+#define DE_WINDOW_SOURCE_BASE_CS_0                      0
+#define DE_WINDOW_SOURCE_BASE_CS_1                      1
+#define DE_WINDOW_SOURCE_BASE_ADDRESS                   25:0
+
+#define DE_WINDOW_DESTINATION_BASE                      0x000044
+#define DE_WINDOW_DESTINATION_BASE_EXT                  27:27
+#define DE_WINDOW_DESTINATION_BASE_EXT_LOCAL            0
+#define DE_WINDOW_DESTINATION_BASE_EXT_EXTERNAL         1
+#define DE_WINDOW_DESTINATION_BASE_CS                   26:26
+#define DE_WINDOW_DESTINATION_BASE_CS_0                 0
+#define DE_WINDOW_DESTINATION_BASE_CS_1                 1
+#define DE_WINDOW_DESTINATION_BASE_ADDRESS              25:0
+
+#define DE_ALPHA                                        0x000048
+#define DE_ALPHA_VALUE                                  7:0
+
+#define DE_WRAP                                         0x00004C
+#define DE_WRAP_X                                       31:16
+#define DE_WRAP_Y                                       15:0
+
+#define DE_STATUS                                       0x000050
+#define DE_STATUS_CSC                                   1:1
+#define DE_STATUS_CSC_CLEAR                             0
+#define DE_STATUS_CSC_NOT_ACTIVE                        0
+#define DE_STATUS_CSC_ACTIVE                            1
+#define DE_STATUS_2D                                    0:0
+#define DE_STATUS_2D_CLEAR                              0
+#define DE_STATUS_2D_NOT_ACTIVE                         0
+#define DE_STATUS_2D_ACTIVE                             1
+
+
+
+/* blt direction */
+#define TOP_TO_BOTTOM 0
+#define LEFT_TO_RIGHT 0
+#define BOTTOM_TO_TOP 1
+#define RIGHT_TO_LEFT 1
+
+void hw_set2dformat(struct lynx_accel * accel,int fmt);
+
+void hw_de_init(struct lynx_accel * accel);
+
+int hw_fillrect(struct lynx_accel * accel,
+				u32 base,u32 pitch,u32 Bpp,
+				u32 x,u32 y,u32 width,u32 height,
+				u32 color,u32 rop);
+
+int hw_copyarea(
+struct lynx_accel * accel,
+unsigned int sBase,  /* Address of source: offset in frame buffer */
+unsigned int sPitch, /* Pitch value of source surface in BYTE */
+unsigned int sx,
+unsigned int sy,     /* Starting coordinate of source surface */
+unsigned int dBase,  /* Address of destination: offset in frame buffer */
+unsigned int dPitch, /* Pitch value of destination surface in BYTE */
+unsigned int bpp,    /* Color depth of destination surface */
+unsigned int dx,
+unsigned int dy,     /* Starting coordinate of destination surface */
+unsigned int width,
+unsigned int height, /* width and height of rectangle in pixel value */
+unsigned int rop2);
+
+int hw_imageblit(struct lynx_accel *accel,
+		 const char *pSrcbuf, /* pointer to start of source buffer in system memory */
+		 u32 srcDelta,          /* Pitch value (in bytes) of the source buffer, +ive means top down and -ive mean button up */
+		 u32 startBit, /* Mono data can start at any bit in a byte, this value should be 0 to 7 */
+		 u32 dBase,    /* Address of destination: offset in frame buffer */
+		 u32 dPitch,   /* Pitch value of destination surface in BYTE */
+		 u32 bytePerPixel,      /* Color depth of destination surface */
+		 u32 dx,
+		 u32 dy,       /* Starting coordinate of destination surface */
+		 u32 width,
+		 u32 height,   /* width and height of rectange in pixel value */
+		 u32 fColor,   /* Foreground color (corresponding to a 1 in the monochrome data */
+		 u32 bColor,   /* Background color (corresponding to a 0 in the monochrome data */
+		 u32 rop2);
+#endif
diff --git a/drivers/staging/sm750fb/sm750_cursor.c b/drivers/staging/sm750fb/sm750_cursor.c
new file mode 100644
index 000000000000..68d5cbc3e513
--- /dev/null
+++ b/drivers/staging/sm750fb/sm750_cursor.c
@@ -0,0 +1,251 @@
+#include<linux/module.h>
+#include<linux/kernel.h>
+#include<linux/errno.h>
+#include<linux/string.h>
+#include<linux/mm.h>
+#include<linux/slab.h>
+#include<linux/delay.h>
+#include<linux/fb.h>
+#include<linux/ioport.h>
+#include<linux/init.h>
+#include<linux/pci.h>
+#include<linux/vmalloc.h>
+#include<linux/pagemap.h>
+#include <linux/console.h>
+#include<linux/platform_device.h>
+#include<linux/screen_info.h>
+
+#include "sm750.h"
+#include "sm750_help.h"
+#include "sm750_cursor.h"
+
+
+#define PEEK32(addr) \
+readl(cursor->mmio + (addr))
+
+#define POKE32(addr,data) \
+writel((data),cursor->mmio + (addr))
+
+/* cursor control for voyager and 718/750*/
+#define HWC_ADDRESS                         0x0
+#define HWC_ADDRESS_ENABLE                  31:31
+#define HWC_ADDRESS_ENABLE_DISABLE          0
+#define HWC_ADDRESS_ENABLE_ENABLE           1
+#define HWC_ADDRESS_EXT                     27:27
+#define HWC_ADDRESS_EXT_LOCAL               0
+#define HWC_ADDRESS_EXT_EXTERNAL            1
+#define HWC_ADDRESS_CS                      26:26
+#define HWC_ADDRESS_CS_0                    0
+#define HWC_ADDRESS_CS_1                    1
+#define HWC_ADDRESS_ADDRESS                 25:0
+
+#define HWC_LOCATION                        0x4
+#define HWC_LOCATION_TOP                    27:27
+#define HWC_LOCATION_TOP_INSIDE             0
+#define HWC_LOCATION_TOP_OUTSIDE            1
+#define HWC_LOCATION_Y                      26:16
+#define HWC_LOCATION_LEFT                   11:11
+#define HWC_LOCATION_LEFT_INSIDE            0
+#define HWC_LOCATION_LEFT_OUTSIDE           1
+#define HWC_LOCATION_X                      10:0
+
+#define HWC_COLOR_12                        0x8
+#define HWC_COLOR_12_2_RGB565               31:16
+#define HWC_COLOR_12_1_RGB565               15:0
+
+#define HWC_COLOR_3                         0xC
+#define HWC_COLOR_3_RGB565                  15:0
+
+
+/* hw_cursor_xxx works for voyager,718 and 750 */
+void hw_cursor_enable(struct lynx_cursor * cursor)
+{
+	u32 reg;
+	reg = FIELD_VALUE(0,HWC_ADDRESS,ADDRESS,cursor->offset)|
+			FIELD_SET(0,HWC_ADDRESS,EXT,LOCAL)|
+			FIELD_SET(0,HWC_ADDRESS,ENABLE,ENABLE);
+	POKE32(HWC_ADDRESS,reg);
+}
+void hw_cursor_disable(struct lynx_cursor * cursor)
+{
+	POKE32(HWC_ADDRESS,0);
+}
+
+void hw_cursor_setSize(struct lynx_cursor * cursor,
+						int w,int h)
+{
+	cursor->w = w;
+	cursor->h = h;
+}
+void hw_cursor_setPos(struct lynx_cursor * cursor,
+						int x,int y)
+{
+	u32 reg;
+	reg = FIELD_VALUE(0,HWC_LOCATION,Y,y)|
+			FIELD_VALUE(0,HWC_LOCATION,X,x);
+	POKE32(HWC_LOCATION,reg);
+}
+void hw_cursor_setColor(struct lynx_cursor * cursor,
+						u32 fg,u32 bg)
+{
+	POKE32(HWC_COLOR_12,(fg<<16)|(bg&0xffff));
+	POKE32(HWC_COLOR_3,0xffe0);
+}
+
+void hw_cursor_setData(struct lynx_cursor * cursor,
+			u16 rop,const u8* pcol,const u8* pmsk)
+{
+	int i,j,count,pitch,offset;
+	u8 color,mask,opr;
+	u16 data;
+	void __iomem *pbuffer, *pstart;
+
+	/*  in byte*/
+	pitch = cursor->w >> 3;
+
+	/* in byte	*/
+	count = pitch * cursor->h;
+
+	/* in byte */
+	offset = cursor->maxW * 2 / 8;
+
+	data = 0;
+	pstart = cursor->vstart;
+	pbuffer = pstart;
+
+/*
+	if(odd &1){
+		hw_cursor_setData2(cursor,rop,pcol,pmsk);
+	}
+	odd++;
+	if(odd > 0xfffffff0)
+		odd=0;
+*/
+
+	for(i=0;i<count;i++)
+	{
+		color = *pcol++;
+		mask = *pmsk++;
+		data = 0;
+
+		/* either method below works well,
+		 * but method 2 shows no lag
+		 * and method 1 seems a bit wrong*/
+#if 0
+		if(rop == ROP_XOR)
+			opr = mask ^ color;
+		else
+			opr = mask & color;
+
+		for(j=0;j<8;j++)
+		{
+
+			if(opr & (0x80 >> j))
+			{	//use fg color,id = 2
+				data |= 2 << (j*2);
+			}else{
+				//use bg color,id = 1
+				data |= 1 << (j*2);
+			}
+		}
+#else
+		for(j=0;j<8;j++){
+			if(mask & (0x80>>j)){
+				if(rop == ROP_XOR)
+					opr = mask ^ color;
+				else
+					opr = mask & color;
+
+				/* 2 stands for forecolor and 1 for backcolor */
+				data |= ((opr & (0x80>>j))?2:1)<<(j*2);
+			}
+		}
+#endif
+		iowrite16(data, pbuffer);
+
+		/* assume pitch is 1,2,4,8,...*/
+#if 0
+		if(!((i+1)&(pitch-1)))   /* below line equal to is line */
+#else
+		if((i+1) % pitch == 0)
+#endif
+		{
+			/* need a return */
+			pstart += offset;
+			pbuffer = pstart;
+		}else{
+			pbuffer += sizeof(u16);
+		}
+
+	}
+
+
+}
+
+
+void hw_cursor_setData2(struct lynx_cursor * cursor,
+			u16 rop,const u8* pcol,const u8* pmsk)
+{
+	int i,j,count,pitch,offset;
+	u8 color, mask;
+	u16 data;
+	void __iomem *pbuffer, *pstart;
+
+	/*  in byte*/
+	pitch = cursor->w >> 3;
+
+	/* in byte	*/
+	count = pitch * cursor->h;
+
+	/* in byte */
+	offset = cursor->maxW * 2 / 8;
+
+	data = 0;
+	pstart = cursor->vstart;
+	pbuffer = pstart;
+
+	for(i=0;i<count;i++)
+	{
+		color = *pcol++;
+		mask = *pmsk++;
+		data = 0;
+
+		/* either method below works well, but method 2 shows no lag */
+#if 0
+		if(rop == ROP_XOR)
+			opr = mask ^ color;
+		else
+			opr = mask & color;
+
+		for(j=0;j<8;j++)
+		{
+
+			if(opr & (0x80 >> j))
+			{	//use fg color,id = 2
+				data |= 2 << (j*2);
+			}else{
+				//use bg color,id = 1
+				data |= 1 << (j*2);
+			}
+		}
+#else
+		for(j=0;j<8;j++){
+			if(mask & (1<<j))
+				data |= ((color & (1<<j))?1:2)<<(j*2);
+		}
+#endif
+		iowrite16(data, pbuffer);
+
+		/* assume pitch is 1,2,4,8,...*/
+		if(!(i&(pitch-1)))
+		//if((i+1) % pitch == 0)
+		{
+			/* need a return */
+			pstart += offset;
+			pbuffer = pstart;
+		}else{
+			pbuffer += sizeof(u16);
+		}
+
+	}
+}
diff --git a/drivers/staging/sm750fb/sm750_cursor.h b/drivers/staging/sm750fb/sm750_cursor.h
new file mode 100644
index 000000000000..8cede0721332
--- /dev/null
+++ b/drivers/staging/sm750fb/sm750_cursor.h
@@ -0,0 +1,17 @@
+#ifndef LYNX_CURSOR_H__
+#define LYNX_CURSOR_H__
+
+/* hw_cursor_xxx works for voyager,718 and 750 */
+void hw_cursor_enable(struct lynx_cursor * cursor);
+void hw_cursor_disable(struct lynx_cursor * cursor);
+void hw_cursor_setSize(struct lynx_cursor * cursor,
+						int w,int h);
+void hw_cursor_setPos(struct lynx_cursor * cursor,
+						int x,int y);
+void hw_cursor_setColor(struct lynx_cursor * cursor,
+						u32 fg,u32 bg);
+void hw_cursor_setData(struct lynx_cursor * cursor,
+			u16 rop,const u8* data,const u8* mask);
+void hw_cursor_setData2(struct lynx_cursor * cursor,
+			u16 rop,const u8* data,const u8* mask);
+#endif
diff --git a/drivers/staging/sm750fb/sm750_help.h b/drivers/staging/sm750fb/sm750_help.h
new file mode 100644
index 000000000000..e0128d2a9ead
--- /dev/null
+++ b/drivers/staging/sm750fb/sm750_help.h
@@ -0,0 +1,111 @@
+#ifndef LYNX_HELP_H__
+#define LYNX_HELP_H__
+/*****************************************************************************\
+ *                                FIELD MACROS                               *
+\*****************************************************************************/
+
+#define _LSB(f)             (0 ? f)
+#define _MSB(f)             (1 ? f)
+#define _COUNT(f)           (_MSB(f) - _LSB(f) + 1)
+
+#define RAW_MASK(f)         (0xFFFFFFFF >> (32 - _COUNT(f)))
+#define GET_MASK(f)         (RAW_MASK(f) << _LSB(f))
+#define GET_FIELD(d,f)      (((d) >> _LSB(f)) & RAW_MASK(f))
+#define TEST_FIELD(d,f,v)   (GET_FIELD(d,f) == f ## _ ## v)
+#define SET_FIELD(d,f,v)    (((d) & ~GET_MASK(f)) | \
+                            (((f ## _ ## v) & RAW_MASK(f)) << _LSB(f)))
+#define SET_FIELDV(d,f,v)   (((d) & ~GET_MASK(f)) | \
+                            (((v) & RAW_MASK(f)) << _LSB(f)))
+
+
+////////////////////////////////////////////////////////////////////////////////
+//                                                                            //
+// Internal macros                                                            //
+//                                                                            //
+////////////////////////////////////////////////////////////////////////////////
+
+#define _F_START(f)             (0 ? f)
+#define _F_END(f)               (1 ? f)
+#define _F_SIZE(f)              (1 + _F_END(f) - _F_START(f))
+#define _F_MASK(f)              (((1 << _F_SIZE(f)) - 1) << _F_START(f))
+#define _F_NORMALIZE(v, f)      (((v) & _F_MASK(f)) >> _F_START(f))
+#define _F_DENORMALIZE(v, f)    (((v) << _F_START(f)) & _F_MASK(f))
+
+
+////////////////////////////////////////////////////////////////////////////////
+//                                                                            //
+// Global macros                                                              //
+//                                                                            //
+////////////////////////////////////////////////////////////////////////////////
+
+#define FIELD_GET(x, reg, field) \
+( \
+    _F_NORMALIZE((x), reg ## _ ## field) \
+)
+
+#define FIELD_SET(x, reg, field, value) \
+( \
+    (x & ~_F_MASK(reg ## _ ## field)) \
+    | _F_DENORMALIZE(reg ## _ ## field ## _ ## value, reg ## _ ## field) \
+)
+
+#define FIELD_VALUE(x, reg, field, value) \
+( \
+    (x & ~_F_MASK(reg ## _ ## field)) \
+    | _F_DENORMALIZE(value, reg ## _ ## field) \
+)
+
+#define FIELD_CLEAR(reg, field) \
+( \
+    ~ _F_MASK(reg ## _ ## field) \
+)
+
+
+////////////////////////////////////////////////////////////////////////////////
+//                                                                            //
+// Field Macros                                                               //
+//                                                                            //
+////////////////////////////////////////////////////////////////////////////////
+
+#define FIELD_START(field)              (0 ? field)
+#define FIELD_END(field)                (1 ? field)
+#define FIELD_SIZE(field)               (1 + FIELD_END(field) - FIELD_START(field))
+#define FIELD_MASK(field)               (((1 << (FIELD_SIZE(field)-1)) | ((1 << (FIELD_SIZE(field)-1)) - 1)) << FIELD_START(field))
+#define FIELD_NORMALIZE(reg, field)     (((reg) & FIELD_MASK(field)) >> FIELD_START(field))
+#define FIELD_DENORMALIZE(field, value) (((value) << FIELD_START(field)) & FIELD_MASK(field))
+
+#define FIELD_INIT(reg, field, value)   FIELD_DENORMALIZE(reg ## _ ## field, \
+                                                          reg ## _ ## field ## _ ## value)
+#define FIELD_INIT_VAL(reg, field, value) \
+                                        (FIELD_DENORMALIZE(reg ## _ ## field, value))
+#define FIELD_VAL_SET(x, r, f, v)       x = x & ~FIELD_MASK(r ## _ ## f) \
+                                              | FIELD_DENORMALIZE(r ## _ ## f, r ## _ ## f ## _ ## v)
+
+#define RGB(r, g, b) \
+( \
+    (unsigned long) (((r) << 16) | ((g) << 8) | (b)) \
+)
+
+#define RGB16(r, g, b) \
+( \
+    (unsigned short) ((((r) & 0xF8) << 8) | (((g) & 0xFC) << 3) | (((b) & 0xF8) >> 3)) \
+)
+
+static inline unsigned int absDiff(unsigned int a,unsigned int b)
+{
+	if(a<b)
+		return b-a;
+	else
+		return a-b;
+}
+
+/* n / d + 1 / 2 = (2n + d) / 2d */
+#define roundedDiv(num,denom)	((2 * (num) + (denom)) / (2 * (denom)))
+#define MB(x) ((x)<<20)
+#define KB(x) ((x)<<10)
+#define MHz(x) ((x) * 1000000)
+
+
+
+
+#endif
diff --git a/drivers/staging/sm750fb/sm750_hw.c b/drivers/staging/sm750fb/sm750_hw.c
new file mode 100644
index 000000000000..9f0d06da12fb
--- /dev/null
+++ b/drivers/staging/sm750fb/sm750_hw.c
@@ -0,0 +1,642 @@
+#include <linux/version.h>
+#include<linux/module.h>
+#include<linux/kernel.h>
+#include<linux/errno.h>
+#include<linux/string.h>
+#include<linux/mm.h>
+#include<linux/slab.h>
+#include<linux/delay.h>
+#include<linux/fb.h>
+#include<linux/ioport.h>
+#include<linux/init.h>
+#include<linux/pci.h>
+#include<linux/vmalloc.h>
+#include<linux/pagemap.h>
+#include <linux/console.h>
+#ifdef CONFIG_MTRR
+#include <asm/mtrr.h>
+#endif
+#include<linux/platform_device.h>
+#include<linux/screen_info.h>
+
+#include "sm750.h"
+#include "sm750_hw.h"
+#include "ddk750.h"
+#include "sm750_accel.h"
+
+int hw_sm750_map(struct lynx_share* share, struct pci_dev* pdev)
+{
+	int ret;
+	struct sm750_share * spec_share;
+	
+
+	spec_share = container_of(share, struct sm750_share,share);
+	ret = 0;
+
+	share->vidreg_start  = pci_resource_start(pdev, 1);
+	share->vidreg_size = MB(2);
+
+	pr_info("mmio phyAddr = %lx\n", share->vidreg_start);
+
+	/* reserve the vidreg space of smi adaptor
+	 * if you do this, u need to add release region code
+	 * in lynxfb_remove, or memory will not be mapped again
+	 * successfully
+	 * */
+
+	if((ret = pci_request_region(pdev, 1, "sm750fb")))
+	{
+		pr_err("Can not request PCI regions.\n");
+		goto exit;
+	}
+
+	/* now map mmio and vidmem*/
+	share->pvReg = ioremap_nocache(share->vidreg_start, share->vidreg_size);
+	if(!share->pvReg){
+		pr_err("mmio failed\n");
+		ret = -EFAULT;
+		goto exit;
+	}else{
+		pr_info("mmio virtual addr = %p\n", share->pvReg);
+	}
+
+	
+	share->accel.dprBase = share->pvReg + DE_BASE_ADDR_TYPE1;
+	share->accel.dpPortBase = share->pvReg + DE_PORT_ADDR_TYPE1;
+
+	ddk750_set_mmio(share->pvReg,share->devid, share->revid);
+
+	share->vidmem_start = pci_resource_start(pdev, 0);
+	/* don't use pdev_resource[x].end - resource[x].start to
+	 * calculate the resource size,its only the maximum available
+	 * size but not the actual size,use
+	 * @hw_sm750_getVMSize function can be safe.
+	 * */
+	share->vidmem_size = hw_sm750_getVMSize(share);
+	pr_info("video memory phyAddr = %lx, size = %u bytes\n",
+	share->vidmem_start, share->vidmem_size);
+
+	/* reserve the vidmem space of smi adaptor */
+#if 0
+	if((ret = pci_request_region(pdev,0,_moduleName_)))
+	{
+		pr_err("Can not request PCI regions.\n");
+		goto exit;
+	}
+#endif
+
+	share->pvMem = ioremap(share->vidmem_start,
+							share->vidmem_size);
+
+	if(!share->pvMem){
+		pr_err("Map video memory failed\n");
+		ret = -EFAULT;
+		goto exit;
+	}else{
+		pr_info("video memory vaddr = %p\n", share->pvMem);
+	}
+exit:
+	return ret;
+}
+
+
+
+int hw_sm750_inithw(struct lynx_share* share, struct pci_dev * pdev)
+{
+	struct sm750_share * spec_share;
+	struct init_status * parm;
+	
+	spec_share = container_of(share, struct sm750_share,share);
+	parm = &spec_share->state.initParm;
+	if(parm->chip_clk == 0)
+		parm->chip_clk = (getChipType() == SM750LE)?
+						DEFAULT_SM750LE_CHIP_CLOCK :
+						DEFAULT_SM750_CHIP_CLOCK;
+
+	if(parm->mem_clk == 0)
+		parm->mem_clk = parm->chip_clk;
+	if(parm->master_clk == 0)
+		parm->master_clk = parm->chip_clk/3;
+
+	ddk750_initHw((initchip_param_t *)&spec_share->state.initParm);
+	/* for sm718,open pci burst */
+	if(share->devid == 0x718){
+		POKE32(SYSTEM_CTRL,
+				FIELD_SET(PEEK32(SYSTEM_CTRL), SYSTEM_CTRL, PCI_BURST, ON));
+	}
+
+	/* sm750 use sii164, it can be setup with default value
+	 * by on power, so initDVIDisp can be skipped */
+#if 0
+	ddk750_initDVIDisp();
+#endif
+
+	if(getChipType() != SM750LE)
+	{
+		/* does user need CRT ?*/
+		if(spec_share->state.nocrt){
+			POKE32(MISC_CTRL,
+					FIELD_SET(PEEK32(MISC_CTRL),
+					MISC_CTRL,
+					DAC_POWER, OFF));
+			/* shut off dpms */
+			POKE32(SYSTEM_CTRL,
+					FIELD_SET(PEEK32(SYSTEM_CTRL),
+					SYSTEM_CTRL,
+					DPMS, VNHN));
+		}else{
+			POKE32(MISC_CTRL,
+					FIELD_SET(PEEK32(MISC_CTRL),
+					MISC_CTRL,
+					DAC_POWER, ON));
+			/* turn on dpms */
+			POKE32(SYSTEM_CTRL,
+					FIELD_SET(PEEK32(SYSTEM_CTRL),
+					SYSTEM_CTRL,
+					DPMS, VPHP));
+		}
+
+		switch (spec_share->state.pnltype){
+			case sm750_doubleTFT:
+			case sm750_24TFT:
+			case sm750_dualTFT:
+			POKE32(PANEL_DISPLAY_CTRL,
+				FIELD_VALUE(PEEK32(PANEL_DISPLAY_CTRL),
+							PANEL_DISPLAY_CTRL,
+							TFT_DISP,
+							spec_share->state.pnltype));
+			break;
+		}
+	}else{
+		/* for 750LE ,no DVI chip initilization makes Monitor no signal */
+		/* Set up GPIO for software I2C to program DVI chip in the
+		   Xilinx SP605 board, in order to have video signal.
+		 */
+        swI2CInit(0,1);
+
+
+        /* Customer may NOT use CH7301 DVI chip, which has to be
+           initialized differently.
+         */
+        if (swI2CReadReg(0xec, 0x4a) == 0x95)
+        {
+            /* The following register values for CH7301 are from
+               Chrontel app note and our experiment.
+             */
+			pr_info("yes,CH7301 DVI chip found\n");
+            swI2CWriteReg(0xec, 0x1d, 0x16);
+            swI2CWriteReg(0xec, 0x21, 0x9);
+            swI2CWriteReg(0xec, 0x49, 0xC0);
+			pr_info("okay,CH7301 DVI chip setup done\n");
+        }
+	}
+
+	/* init 2d engine */
+	if(!share->accel_off){
+		hw_sm750_initAccel(share);
+//		share->accel.de_wait = hw_sm750_deWait;
+	}
+
+	return 0;
+}
+
+
+resource_size_t hw_sm750_getVMSize(struct lynx_share * share)
+{
+	resource_size_t ret;
+	
+	ret = ddk750_getVMSize();
+	return ret;
+}
+
+
+
+int hw_sm750_output_checkMode(struct lynxfb_output* output, struct fb_var_screeninfo* var)
+{
+	
+	return 0;
+}
+
+
+int hw_sm750_output_setMode(struct lynxfb_output* output,
+									struct fb_var_screeninfo* var, struct fb_fix_screeninfo* fix)
+{
+	int ret;
+	disp_output_t dispSet;
+	int channel;
+	
+	ret = 0;
+	dispSet = 0;
+	channel = *output->channel;
+
+
+	if(getChipType() != SM750LE){
+		if(channel == sm750_primary){
+			pr_info("primary channel\n");
+			if(output->paths & sm750_panel)
+				dispSet |= do_LCD1_PRI;
+			if(output->paths & sm750_crt)
+				dispSet |= do_CRT_PRI;
+
+		}else{
+			pr_info("secondary channel\n");
+			if(output->paths & sm750_panel)
+				dispSet |= do_LCD1_SEC;
+			if(output->paths & sm750_crt)
+				dispSet |= do_CRT_SEC;
+
+		}
+		ddk750_setLogicalDispOut(dispSet);
+	}else{
+		/* just open DISPLAY_CONTROL_750LE register bit 3:0*/
+		u32 reg;
+		reg = PEEK32(DISPLAY_CONTROL_750LE);
+		reg |= 0xf;
+		POKE32(DISPLAY_CONTROL_750LE, reg);
+	}
+
+	pr_info("ddk setlogicdispout done \n");
+	return ret;
+}
+
+void hw_sm750_output_clear(struct lynxfb_output* output)
+{
+	
+	return;
+}
+
+int hw_sm750_crtc_checkMode(struct lynxfb_crtc* crtc, struct fb_var_screeninfo* var)
+{
+	struct lynx_share * share;
+	
+
+	share = container_of(crtc, struct lynxfb_par,crtc)->share;
+
+	switch (var->bits_per_pixel){
+		case 8:
+		case 16:
+			break;
+		case 32:
+			if (share->revid == SM750LE_REVISION_ID) {
+				pr_debug("750le do not support 32bpp\n");
+				return -EINVAL;
+			}
+			break;
+		default:
+			return -EINVAL;
+
+	}
+
+	return 0;
+}
+
+
+/*
+	set the controller's mode for @crtc charged with @var and @fix parameters
+*/
+int hw_sm750_crtc_setMode(struct lynxfb_crtc* crtc,
+								struct fb_var_screeninfo* var,
+								struct fb_fix_screeninfo* fix)
+{
+	int ret,fmt;
+	u32 reg;
+	mode_parameter_t modparm;
+	clock_type_t clock;
+	struct lynx_share * share;
+	struct lynxfb_par * par;
+
+	
+	ret = 0;
+	par = container_of(crtc, struct lynxfb_par, crtc);
+	share = par->share;
+#if 1
+	if(!share->accel_off){
+		/* set 2d engine pixel format according to mode bpp */
+		switch(var->bits_per_pixel){
+			case 8:
+				fmt = 0;
+				break;
+			case 16:
+				fmt = 1;
+				break;
+			case 32:
+			default:
+				fmt = 2;
+				break;
+		}
+		hw_set2dformat(&share->accel, fmt);
+	}
+#endif
+
+	/* set timing */
+//	modparm.pixel_clock = PS_TO_HZ(var->pixclock);
+	modparm.pixel_clock = ps_to_hz(var->pixclock);
+	modparm.vertical_sync_polarity = (var->sync & FB_SYNC_HOR_HIGH_ACT) ? POS:NEG;
+	modparm.horizontal_sync_polarity = (var->sync & FB_SYNC_VERT_HIGH_ACT) ? POS:NEG;
+	modparm.clock_phase_polarity = (var->sync& FB_SYNC_COMP_HIGH_ACT) ? POS:NEG;
+	modparm.horizontal_display_end = var->xres;
+	modparm.horizontal_sync_width = var->hsync_len;
+	modparm.horizontal_sync_start = var->xres + var->right_margin;
+	modparm.horizontal_total = var->xres + var->left_margin + var->right_margin + var->hsync_len;
+	modparm.vertical_display_end = var->yres;
+	modparm.vertical_sync_height = var->vsync_len;
+	modparm.vertical_sync_start = var->yres + var->lower_margin;
+	modparm.vertical_total = var->yres + var->upper_margin + var->lower_margin + var->vsync_len;
+
+	/* choose pll */
+	if(crtc->channel != sm750_secondary)
+		clock = PRIMARY_PLL;
+	else
+		clock = SECONDARY_PLL;
+
+	pr_debug("Request pixel clock = %lu\n", modparm.pixel_clock);
+	ret = ddk750_setModeTiming(&modparm, clock);
+	if(ret){
+		pr_err("Set mode timing failed\n");
+		goto exit;
+	}
+
+	if(crtc->channel != sm750_secondary){
+		/* set pitch, offset ,width,start address ,etc... */
+		POKE32(PANEL_FB_ADDRESS,
+			FIELD_SET(0, PANEL_FB_ADDRESS, STATUS, CURRENT)|
+			FIELD_SET(0, PANEL_FB_ADDRESS, EXT, LOCAL)|
+			FIELD_VALUE(0, PANEL_FB_ADDRESS, ADDRESS, crtc->oScreen));
+
+		reg = var->xres * (var->bits_per_pixel >> 3);
+		/* crtc->channel is not equal to par->index on numeric,be aware of that */
+		reg = PADDING(crtc->line_pad,reg);
+
+		POKE32(PANEL_FB_WIDTH,
+			FIELD_VALUE(0, PANEL_FB_WIDTH, WIDTH, reg)|
+			FIELD_VALUE(0, PANEL_FB_WIDTH, OFFSET, fix->line_length));
+
+		POKE32(PANEL_WINDOW_WIDTH,
+			FIELD_VALUE(0, PANEL_WINDOW_WIDTH, WIDTH, var->xres -1)|
+			FIELD_VALUE(0, PANEL_WINDOW_WIDTH, X, var->xoffset));
+
+		POKE32(PANEL_WINDOW_HEIGHT,
+			FIELD_VALUE(0, PANEL_WINDOW_HEIGHT, HEIGHT, var->yres_virtual - 1)|
+			FIELD_VALUE(0, PANEL_WINDOW_HEIGHT, Y, var->yoffset));
+
+		POKE32(PANEL_PLANE_TL, 0);
+
+		POKE32(PANEL_PLANE_BR,
+			FIELD_VALUE(0, PANEL_PLANE_BR, BOTTOM, var->yres - 1)|
+			FIELD_VALUE(0, PANEL_PLANE_BR,RIGHT, var->xres - 1));
+
+		/* set pixel format */
+		reg = PEEK32(PANEL_DISPLAY_CTRL);
+		POKE32(PANEL_DISPLAY_CTRL,
+			FIELD_VALUE(reg,
+			PANEL_DISPLAY_CTRL, FORMAT,
+			(var->bits_per_pixel >> 4)
+			));
+	}else{
+		/* not implemented now */
+		POKE32(CRT_FB_ADDRESS, crtc->oScreen);
+		reg = var->xres * (var->bits_per_pixel >> 3);
+		/* crtc->channel is not equal to par->index on numeric,be aware of that */
+		reg = PADDING(crtc->line_pad, reg);
+
+		POKE32(CRT_FB_WIDTH,
+			FIELD_VALUE(0, CRT_FB_WIDTH, WIDTH, reg)|
+			FIELD_VALUE(0, CRT_FB_WIDTH, OFFSET, fix->line_length));
+
+		/* SET PIXEL FORMAT */
+		reg = PEEK32(CRT_DISPLAY_CTRL);
+		reg = FIELD_VALUE(reg, CRT_DISPLAY_CTRL, FORMAT, var->bits_per_pixel >> 4);
+		POKE32(CRT_DISPLAY_CTRL, reg);
+
+	}
+
+
+exit:
+	return ret;
+}
+
+void hw_sm750_crtc_clear(struct lynxfb_crtc* crtc)
+{
+	
+	return;
+}
+
+int hw_sm750_setColReg(struct lynxfb_crtc* crtc, ushort index,
+								ushort red, ushort green, ushort blue)
+{
+	static unsigned int add[]={PANEL_PALETTE_RAM,CRT_PALETTE_RAM};
+	POKE32(add[crtc->channel] + index*4, (red<<16)|(green<<8)|blue);
+	return 0;
+}
+
+int hw_sm750le_setBLANK(struct lynxfb_output * output, int blank){
+	int dpms,crtdb;
+	
+	switch(blank)
+	{
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,10)
+		case FB_BLANK_UNBLANK:
+#else
+		case VESA_NO_BLANKING:
+#endif
+			dpms = CRT_DISPLAY_CTRL_DPMS_0;
+			crtdb = CRT_DISPLAY_CTRL_BLANK_OFF;
+			break;
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,10)
+		case FB_BLANK_NORMAL:
+			dpms = CRT_DISPLAY_CTRL_DPMS_0;
+			crtdb = CRT_DISPLAY_CTRL_BLANK_ON;
+			break;
+#endif
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,10)
+		case FB_BLANK_VSYNC_SUSPEND:
+#else
+		case VESA_VSYNC_SUSPEND:
+#endif
+			dpms = CRT_DISPLAY_CTRL_DPMS_2;
+			crtdb = CRT_DISPLAY_CTRL_BLANK_ON;
+			break;
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,10)
+		case FB_BLANK_HSYNC_SUSPEND:
+#else
+		case VESA_HSYNC_SUSPEND:
+#endif
+			dpms = CRT_DISPLAY_CTRL_DPMS_1;
+			crtdb = CRT_DISPLAY_CTRL_BLANK_ON;
+			break;
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,10)
+		case FB_BLANK_POWERDOWN:
+#else
+		case VESA_POWERDOWN:
+#endif
+			dpms = CRT_DISPLAY_CTRL_DPMS_3;
+			crtdb = CRT_DISPLAY_CTRL_BLANK_ON;
+			break;
+		default:
+			return -EINVAL;
+	}
+
+	if(output->paths & sm750_crt){
+		POKE32(CRT_DISPLAY_CTRL, FIELD_VALUE(PEEK32(CRT_DISPLAY_CTRL), CRT_DISPLAY_CTRL, DPMS, dpms));
+		POKE32(CRT_DISPLAY_CTRL, FIELD_VALUE(PEEK32(CRT_DISPLAY_CTRL), CRT_DISPLAY_CTRL, BLANK, crtdb));
+	}
+	return 0;
+}
+
+int hw_sm750_setBLANK(struct lynxfb_output* output,int blank)
+{
+	unsigned int dpms, pps, crtdb;
+	
+	dpms = pps = crtdb = 0;
+
+	switch (blank)
+	{
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,10)
+		case FB_BLANK_UNBLANK:
+#else
+		case VESA_NO_BLANKING:
+#endif
+			pr_info("flag = FB_BLANK_UNBLANK \n");
+			dpms = SYSTEM_CTRL_DPMS_VPHP;
+			pps = PANEL_DISPLAY_CTRL_DATA_ENABLE;
+			crtdb = CRT_DISPLAY_CTRL_BLANK_OFF;
+			break;
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,10)
+		case FB_BLANK_NORMAL:
+			pr_info("flag = FB_BLANK_NORMAL \n");
+			dpms = SYSTEM_CTRL_DPMS_VPHP;
+			pps = PANEL_DISPLAY_CTRL_DATA_DISABLE;
+			crtdb = CRT_DISPLAY_CTRL_BLANK_ON;
+			break;
+#endif
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,10)
+		case FB_BLANK_VSYNC_SUSPEND:
+#else
+		case VESA_VSYNC_SUSPEND:
+#endif
+			dpms = SYSTEM_CTRL_DPMS_VNHP;
+			pps = PANEL_DISPLAY_CTRL_DATA_DISABLE;
+			crtdb = CRT_DISPLAY_CTRL_BLANK_ON;
+			break;
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,10)
+		case FB_BLANK_HSYNC_SUSPEND:
+#else
+		case VESA_HSYNC_SUSPEND:
+#endif
+			dpms = SYSTEM_CTRL_DPMS_VPHN;
+			pps = PANEL_DISPLAY_CTRL_DATA_DISABLE;
+			crtdb = CRT_DISPLAY_CTRL_BLANK_ON;
+			break;
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,10)
+		case FB_BLANK_POWERDOWN:
+#else
+		case VESA_POWERDOWN:
+#endif
+			dpms = SYSTEM_CTRL_DPMS_VNHN;
+			pps = PANEL_DISPLAY_CTRL_DATA_DISABLE;
+			crtdb = CRT_DISPLAY_CTRL_BLANK_ON;
+			break;
+	}
+
+	if(output->paths & sm750_crt){
+
+		POKE32(SYSTEM_CTRL,FIELD_VALUE(PEEK32(SYSTEM_CTRL), SYSTEM_CTRL, DPMS, dpms));
+		POKE32(CRT_DISPLAY_CTRL,FIELD_VALUE(PEEK32(CRT_DISPLAY_CTRL), CRT_DISPLAY_CTRL,BLANK, crtdb));
+	}
+
+	if(output->paths & sm750_panel){
+		POKE32(PANEL_DISPLAY_CTRL, FIELD_VALUE(PEEK32(PANEL_DISPLAY_CTRL), PANEL_DISPLAY_CTRL, DATA, pps));
+	}
+
+	return 0;
+}
+
+
+void hw_sm750_initAccel(struct lynx_share * share)
+{
+	u32 reg;
+	enable2DEngine(1);
+
+	if(getChipType() == SM750LE){
+		reg = PEEK32(DE_STATE1);
+		reg = FIELD_SET(reg, DE_STATE1, DE_ABORT,ON);
+		POKE32(DE_STATE1,reg);
+
+		reg = PEEK32(DE_STATE1);
+		reg = FIELD_SET(reg, DE_STATE1, DE_ABORT,OFF);
+		POKE32(DE_STATE1, reg);
+
+	}else{
+		/* engine reset */
+		reg = PEEK32(SYSTEM_CTRL);
+	    reg = FIELD_SET(reg, SYSTEM_CTRL, DE_ABORT,ON);
+		POKE32(SYSTEM_CTRL, reg);
+
+		reg = PEEK32(SYSTEM_CTRL);
+		reg = FIELD_SET(reg, SYSTEM_CTRL, DE_ABORT,OFF);
+		POKE32(SYSTEM_CTRL, reg);
+	}
+
+	/* call 2d init */
+	share->accel.de_init(&share->accel);
+}
+
+int hw_sm750le_deWait(void)
+{
+	int i=0x10000000;
+	while(i--){
+		unsigned int dwVal = PEEK32(DE_STATE2);
+		if((FIELD_GET(dwVal, DE_STATE2, DE_STATUS) == DE_STATE2_DE_STATUS_IDLE) &&
+			(FIELD_GET(dwVal, DE_STATE2, DE_FIFO)  == DE_STATE2_DE_FIFO_EMPTY) &&
+			(FIELD_GET(dwVal, DE_STATE2, DE_MEM_FIFO) == DE_STATE2_DE_MEM_FIFO_EMPTY))
+		{
+			return 0;
+		}
+	}
+	/* timeout error */
+	return -1;
+}
+
+
+int hw_sm750_deWait(void)
+{
+	int i=0x10000000;
+	while(i--){
+		unsigned int dwVal = PEEK32(SYSTEM_CTRL);
+		if((FIELD_GET(dwVal,SYSTEM_CTRL,DE_STATUS) == SYSTEM_CTRL_DE_STATUS_IDLE) &&
+			(FIELD_GET(dwVal,SYSTEM_CTRL,DE_FIFO)  == SYSTEM_CTRL_DE_FIFO_EMPTY) &&
+			(FIELD_GET(dwVal,SYSTEM_CTRL,DE_MEM_FIFO) == SYSTEM_CTRL_DE_MEM_FIFO_EMPTY))
+		{
+			return 0;
+		}
+	}
+	/* timeout error */
+	return -1;
+}
+
+int hw_sm750_pan_display(struct lynxfb_crtc *crtc,
+        const struct fb_var_screeninfo *var,
+        const struct fb_info *info)
+{
+    uint32_t total;
+    //check params
+    if ((var->xoffset + var->xres > var->xres_virtual) ||
+            (var->yoffset + var->yres > var->yres_virtual)) {
+        return -EINVAL;
+    }
+
+    total = var->yoffset * info->fix.line_length +
+        ((var->xoffset * var->bits_per_pixel) >> 3);
+    total += crtc->oScreen;
+    if (crtc->channel == sm750_primary) {
+        POKE32(PANEL_FB_ADDRESS,
+                FIELD_VALUE(PEEK32(PANEL_FB_ADDRESS),
+                    PANEL_FB_ADDRESS, ADDRESS, total));
+    } else {
+        POKE32(CRT_FB_ADDRESS,
+                FIELD_VALUE(PEEK32(CRT_FB_ADDRESS),
+                    CRT_FB_ADDRESS, ADDRESS, total));
+    }
+    return 0;
+}
+
diff --git a/drivers/staging/sm750fb/sm750_hw.h b/drivers/staging/sm750fb/sm750_hw.h
new file mode 100644
index 000000000000..b05be5e99f51
--- /dev/null
+++ b/drivers/staging/sm750fb/sm750_hw.h
@@ -0,0 +1,104 @@
+#ifndef LYNX_HW750_H__
+#define LYNX_HW750_H__
+
+
+#define DEFAULT_SM750_CHIP_CLOCK 		290
+#define DEFAULT_SM750LE_CHIP_CLOCK  	333
+#ifndef SM750LE_REVISION_ID
+#define SM750LE_REVISION_ID (unsigned char)0xfe
+#endif
+
+//#define DEFAULT_MEM_CLOCK	(DEFAULT_SM750_CHIP_CLOCK/1)
+//#define DEFAULT_MASTER_CLOCK	(DEFAULT_SM750_CHIP_CLOCK/3)
+
+
+enum sm750_pnltype{
+
+	sm750_24TFT = 0,/* 24bit tft */
+
+	sm750_dualTFT = 2,/* dual 18 bit tft */
+
+	sm750_doubleTFT = 1,/* 36 bit double pixel tft */
+};
+
+/* vga channel is not concerned  */
+enum sm750_dataflow{
+	sm750_simul_pri,/* primary => all head */
+
+	sm750_simul_sec,/* secondary => all head */
+
+	sm750_dual_normal,/* 	primary => panel head and secondary => crt */
+
+	sm750_dual_swap,/* 	primary => crt head and secondary => panel */
+};
+
+
+enum sm750_channel{
+	sm750_primary = 0,
+	/* enum value equal to the register filed data */
+	sm750_secondary = 1,
+};
+
+enum sm750_path{
+	sm750_panel = 1,
+	sm750_crt = 2,
+	sm750_pnc = 3,/* panel and crt */
+};
+
+struct init_status{
+	ushort powerMode;
+	/* below three clocks are in unit of MHZ*/
+	ushort chip_clk;
+	ushort mem_clk;
+	ushort master_clk;
+	ushort setAllEngOff;
+	ushort resetMemory;
+};
+
+struct sm750_state{
+	struct init_status initParm;
+	enum sm750_pnltype pnltype;
+	enum sm750_dataflow dataflow;
+	int nocrt;
+	int xLCD;
+	int yLCD;
+};
+
+/* 	sm750_share stands for a presentation of two frame buffer
+	that use one sm750 adaptor, it is similiar to the super class of lynx_share
+	in C++
+*/
+
+struct sm750_share{
+	/* it's better to put lynx_share struct to the first place of sm750_share */
+	struct lynx_share share;
+	struct sm750_state state;
+	int hwCursor;
+	/* 	0: no hardware cursor
+		1: primary crtc hw cursor enabled,
+		2: secondary crtc hw cursor enabled
+		3: both ctrc hw cursor enabled
+	*/
+};
+
+int hw_sm750_map(struct lynx_share* share,struct pci_dev* pdev);
+int hw_sm750_inithw(struct lynx_share*,struct pci_dev *);
+void hw_sm750_initAccel(struct lynx_share *);
+int hw_sm750_deWait(void);
+int hw_sm750le_deWait(void);
+
+resource_size_t hw_sm750_getVMSize(struct lynx_share *);
+int hw_sm750_output_checkMode(struct lynxfb_output*,struct fb_var_screeninfo*);
+int hw_sm750_output_setMode(struct lynxfb_output*,struct fb_var_screeninfo*,struct fb_fix_screeninfo*);
+int hw_sm750_crtc_checkMode(struct lynxfb_crtc*,struct fb_var_screeninfo*);
+int hw_sm750_crtc_setMode(struct lynxfb_crtc*,struct fb_var_screeninfo*,struct fb_fix_screeninfo*);
+int hw_sm750_setColReg(struct lynxfb_crtc*,ushort,ushort,ushort,ushort);
+int hw_sm750_setBLANK(struct lynxfb_output*,int);
+int hw_sm750le_setBLANK(struct lynxfb_output*,int);
+void hw_sm750_crtc_clear(struct lynxfb_crtc*);
+void hw_sm750_output_clear(struct lynxfb_output*);
+int hw_sm750_pan_display(struct lynxfb_crtc *crtc,
+        const struct fb_var_screeninfo *var,
+        const struct fb_info *info);
+
+#endif
diff --git a/drivers/staging/sm7xxfb/sm7xx.h b/drivers/staging/sm7xxfb/sm7xx.h
index 7cc1896938b6..c5d62534e4a4 100644
--- a/drivers/staging/sm7xxfb/sm7xx.h
+++ b/drivers/staging/sm7xxfb/sm7xx.h
@@ -119,7 +119,7 @@ struct ModeInit {
 /**********************************************************************
 			 SM712 Mode table.
  **********************************************************************/
-struct ModeInit vgamode[] = {
+static struct ModeInit vgamode[] = {
 	{
 	 /*  mode#0: 640 x 480  16Bpp  60Hz */
 	 640, 480, 16, 60,
diff --git a/drivers/staging/sm7xxfb/sm7xxfb.c b/drivers/staging/sm7xxfb/sm7xxfb.c
index ebd95365ffae..77f51a075004 100644
--- a/drivers/staging/sm7xxfb/sm7xxfb.c
+++ b/drivers/staging/sm7xxfb/sm7xxfb.c
@@ -113,13 +113,15 @@ static struct vesa_mode vesa_mode_table[] = {
 
 static struct screen_info smtc_scr_info;
 
+static char *mode_option;
+
 /* process command line options, get vga parameter */
-static int __init sm7xx_vga_setup(char *options)
+static void __init sm7xx_vga_setup(char *options)
 {
 	int i;
 
 	if (!options || !*options)
-		return -EINVAL;
+		return;
 
 	smtc_scr_info.lfb_width = 0;
 	smtc_scr_info.lfb_height = 0;
@@ -133,13 +135,10 @@ static int __init sm7xx_vga_setup(char *options)
 			smtc_scr_info.lfb_height =
 						vesa_mode_table[i].lfb_height;
 			smtc_scr_info.lfb_depth  = vesa_mode_table[i].lfb_depth;
-			return 0;
+			return;
 		}
 	}
-
-	return -1;
 }
-__setup("vga=", sm7xx_vga_setup);
 
 static void sm712_setpalette(int regno, unsigned red, unsigned green,
 			     unsigned blue, struct fb_info *info)
@@ -777,6 +776,12 @@ static int smtcfb_pci_probe(struct pci_dev *pdev,
 	if (err)
 		return err;
 
+	err = pci_request_region(pdev, 0, "sm7xxfb");
+	if (err < 0) {
+		dev_err(&pdev->dev, "cannot reserve framebuffer region\n");
+		goto failed_regions;
+	}
+
 	sprintf(smtcfb_fix.id, "sm%Xfb", ent->device);
 
 	sfb = smtc_alloc_fb_info(pdev);
@@ -906,6 +911,9 @@ failed_fb:
 	smtc_free_fb_info(sfb);
 
 failed_free:
+	pci_release_region(pdev, 0);
+
+failed_regions:
 	pci_disable_device(pdev);
 
 	return err;
@@ -923,6 +931,8 @@ static const struct pci_device_id smtcfb_pci_table[] = {
 	{0,}
 };
 
+MODULE_DEVICE_TABLE(pci, smtcfb_pci_table);
+
 static void smtcfb_pci_remove(struct pci_dev *pdev)
 {
 	struct smtcfb_info *sfb;
@@ -932,6 +942,8 @@ static void smtcfb_pci_remove(struct pci_dev *pdev)
 	smtc_unmap_mmio(sfb);
 	unregister_framebuffer(&sfb->fb);
 	smtc_free_fb_info(sfb);
+	pci_release_region(pdev, 0);
+	pci_disable_device(pdev);
 }
 
 #ifdef CONFIG_PM
@@ -1017,7 +1029,29 @@ static struct pci_driver smtcfb_driver = {
 	.driver.pm  = SM7XX_PM_OPS,
 };
 
-module_pci_driver(smtcfb_driver);
+static int __init sm712fb_init(void)
+{
+#ifndef MODULE
+	char *option = NULL;
+
+	if (fb_get_options("sm712fb", &option))
+		return -ENODEV;
+	if (option && *option)
+		mode_option = option;
+#endif
+	sm7xx_vga_setup(mode_option);
+
+	return pci_register_driver(&smtcfb_driver);
+}
+
+module_init(sm712fb_init);
+
+static void __exit sm712fb_exit(void)
+{
+	pci_unregister_driver(&smtcfb_driver);
+}
+
+module_exit(sm712fb_exit);
 
 MODULE_AUTHOR("Siliconmotion ");
 MODULE_DESCRIPTION("Framebuffer driver for SMI Graphic Cards");
diff --git a/drivers/staging/speakup/kobjects.c b/drivers/staging/speakup/kobjects.c
index 3708bc13ae86..0211df60004a 100644
--- a/drivers/staging/speakup/kobjects.c
+++ b/drivers/staging/speakup/kobjects.c
@@ -840,12 +840,10 @@ static ssize_t message_show(struct kobject *kobj,
 static ssize_t message_store(struct kobject *kobj, struct kobj_attribute *attr,
 	const char *buf, size_t count)
 {
-	ssize_t retval = 0;
 	struct msg_group_t *group = spk_find_msg_group(attr->attr.name);
 
 	BUG_ON(!group);
-	retval = message_store_helper(buf, count, group);
-	return retval;
+	return message_store_helper(buf, count, group);
 }
 
 /*
diff --git a/drivers/staging/speakup/main.c b/drivers/staging/speakup/main.c
index e9f0c150d246..c955976414ee 100644
--- a/drivers/staging/speakup/main.c
+++ b/drivers/staging/speakup/main.c
@@ -423,7 +423,8 @@ static void announce_edge(struct vc_data *vc, int msg_id)
 	if (spk_bleeps & 1)
 		bleep(spk_y);
 	if ((spk_bleeps & 2) && (msg_id < edge_quiet))
-		synth_printf("%s\n", spk_msg_get(MSG_EDGE_MSGS_START + msg_id - 1));
+		synth_printf("%s\n",
+			spk_msg_get(MSG_EDGE_MSGS_START + msg_id - 1));
 }
 
 static void speak_char(u_char ch)
@@ -1131,7 +1132,8 @@ static void spkup_write(const char *in_buf, int count)
 	if (in_count > 2 && rep_count > 2) {
 		if (last_type & CH_RPT) {
 			synth_printf(" ");
-			synth_printf(spk_msg_get(MSG_REPEAT_DESC2), ++rep_count);
+			synth_printf(spk_msg_get(MSG_REPEAT_DESC2),
+					++rep_count);
 			synth_printf(" ");
 		}
 		rep_count = 0;
@@ -1527,7 +1529,7 @@ static void update_color_buffer(struct vc_data *vc, const char *ic, int len)
 	int i, bi, hi;
 	int vc_num = vc->vc_num;
 
-	bi = ((vc->vc_attr & 0x70) >> 4);
+	bi = (vc->vc_attr & 0x70) >> 4;
 	hi = speakup_console[vc_num]->ht.highsize[bi];
 
 	i = 0;
@@ -1847,7 +1849,8 @@ static void speakup_win_set(struct vc_data *vc)
 			win_right = spk_x;
 		}
 		snprintf(info, sizeof(info), spk_msg_get(MSG_WINDOW_BOUNDARY),
-			 (win_start) ? spk_msg_get(MSG_END) : spk_msg_get(MSG_START),
+			 (win_start) ?
+				spk_msg_get(MSG_END) : spk_msg_get(MSG_START),
 			 (int)spk_y + 1, (int)spk_x + 1);
 	}
 	synth_printf("%s\n", info);
diff --git a/drivers/staging/speakup/serialio.h b/drivers/staging/speakup/serialio.h
index 317bb8432a04..1b399214ecf7 100644
--- a/drivers/staging/speakup/serialio.h
+++ b/drivers/staging/speakup/serialio.h
@@ -34,6 +34,7 @@ struct old_serial_port {
 #define SPK_TIMEOUT 100
 #define BOTH_EMPTY (UART_LSR_TEMT | UART_LSR_THRE)
 
-#define spk_serial_tx_busy() ((inb(speakup_info.port_tts + UART_LSR) & BOTH_EMPTY) != BOTH_EMPTY)
+#define spk_serial_tx_busy() \
+	((inb(speakup_info.port_tts + UART_LSR) & BOTH_EMPTY) != BOTH_EMPTY)
 
 #endif
diff --git a/drivers/staging/speakup/speakup.h b/drivers/staging/speakup/speakup.h
index 898dce5e1243..a7f4962427f3 100644
--- a/drivers/staging/speakup/speakup.h
+++ b/drivers/staging/speakup/speakup.h
@@ -61,10 +61,12 @@ extern struct st_var_header *spk_get_var_header(enum var_id_t var_id);
 extern struct st_var_header *spk_var_header_by_name(const char *name);
 extern struct punc_var_t *spk_get_punc_var(enum var_id_t var_id);
 extern int spk_set_num_var(int val, struct st_var_header *var, int how);
-extern int spk_set_string_var(const char *page, struct st_var_header *var, int len);
+extern int spk_set_string_var(const char *page, struct st_var_header *var,
+				int len);
 extern int spk_set_mask_bits(const char *input, const int which, const int how);
 extern special_func spk_special_handler;
-extern int spk_handle_help(struct vc_data *vc, u_char type, u_char ch, u_short key);
+extern int spk_handle_help(struct vc_data *vc, u_char type, u_char ch,
+				u_short key);
 extern int synth_init(char *name);
 extern void synth_release(void);
 
diff --git a/drivers/staging/speakup/speakup_acntpc.c b/drivers/staging/speakup/speakup_acntpc.c
index f0bfd9e99191..f418893928ec 100644
--- a/drivers/staging/speakup/speakup_acntpc.c
+++ b/drivers/staging/speakup/speakup_acntpc.c
@@ -318,18 +318,8 @@ module_param_named(start, synth_acntpc.startup, short, S_IRUGO);
 MODULE_PARM_DESC(port, "Set the port for the synthesizer (override probing).");
 MODULE_PARM_DESC(start, "Start the synthesizer once it is loaded.");
 
-static int __init acntpc_init(void)
-{
-	return synth_add(&synth_acntpc);
-}
-
-static void __exit acntpc_exit(void)
-{
-	synth_remove(&synth_acntpc);
-}
+module_spk_synth(synth_acntpc);
 
-module_init(acntpc_init);
-module_exit(acntpc_exit);
 MODULE_AUTHOR("Kirk Reiser <kirk@braille.uwo.ca>");
 MODULE_AUTHOR("David Borowski");
 MODULE_DESCRIPTION("Speakup support for Accent PC synthesizer");
diff --git a/drivers/staging/speakup/speakup_acntsa.c b/drivers/staging/speakup/speakup_acntsa.c
index 3f2b5698a3d8..af2690f38950 100644
--- a/drivers/staging/speakup/speakup_acntsa.c
+++ b/drivers/staging/speakup/speakup_acntsa.c
@@ -143,18 +143,8 @@ module_param_named(start, synth_acntsa.startup, short, S_IRUGO);
 MODULE_PARM_DESC(ser, "Set the serial port for the synthesizer (0-based).");
 MODULE_PARM_DESC(start, "Start the synthesizer once it is loaded.");
 
-static int __init acntsa_init(void)
-{
-	return synth_add(&synth_acntsa);
-}
-
-static void __exit acntsa_exit(void)
-{
-	synth_remove(&synth_acntsa);
-}
+module_spk_synth(synth_acntsa);
 
-module_init(acntsa_init);
-module_exit(acntsa_exit);
 MODULE_AUTHOR("Kirk Reiser <kirk@braille.uwo.ca>");
 MODULE_AUTHOR("David Borowski");
 MODULE_DESCRIPTION("Speakup support for Accent SA synthesizer");
diff --git a/drivers/staging/speakup/speakup_apollo.c b/drivers/staging/speakup/speakup_apollo.c
index 678b263e551c..51788f7d4480 100644
--- a/drivers/staging/speakup/speakup_apollo.c
+++ b/drivers/staging/speakup/speakup_apollo.c
@@ -207,18 +207,8 @@ module_param_named(start, synth_apollo.startup, short, S_IRUGO);
 MODULE_PARM_DESC(ser, "Set the serial port for the synthesizer (0-based).");
 MODULE_PARM_DESC(start, "Start the synthesizer once it is loaded.");
 
-static int __init apollo_init(void)
-{
-	return synth_add(&synth_apollo);
-}
-
-static void __exit apollo_exit(void)
-{
-	synth_remove(&synth_apollo);
-}
+module_spk_synth(synth_apollo);
 
-module_init(apollo_init);
-module_exit(apollo_exit);
 MODULE_AUTHOR("Kirk Reiser <kirk@braille.uwo.ca>");
 MODULE_AUTHOR("David Borowski");
 MODULE_DESCRIPTION("Speakup support for Apollo II synthesizer");
diff --git a/drivers/staging/speakup/speakup_audptr.c b/drivers/staging/speakup/speakup_audptr.c
index 5cbaec8335fe..ea89e36ecd0d 100644
--- a/drivers/staging/speakup/speakup_audptr.c
+++ b/drivers/staging/speakup/speakup_audptr.c
@@ -177,18 +177,8 @@ module_param_named(start, synth_audptr.startup, short, S_IRUGO);
 MODULE_PARM_DESC(ser, "Set the serial port for the synthesizer (0-based).");
 MODULE_PARM_DESC(start, "Start the synthesizer once it is loaded.");
 
-static int __init audptr_init(void)
-{
-	return synth_add(&synth_audptr);
-}
-
-static void __exit audptr_exit(void)
-{
-	synth_remove(&synth_audptr);
-}
+module_spk_synth(synth_audptr);
 
-module_init(audptr_init);
-module_exit(audptr_exit);
 MODULE_AUTHOR("Kirk Reiser <kirk@braille.uwo.ca>");
 MODULE_AUTHOR("David Borowski");
 MODULE_DESCRIPTION("Speakup support for Audapter synthesizer");
diff --git a/drivers/staging/speakup/speakup_bns.c b/drivers/staging/speakup/speakup_bns.c
index 2f070282a85d..80f8358d4199 100644
--- a/drivers/staging/speakup/speakup_bns.c
+++ b/drivers/staging/speakup/speakup_bns.c
@@ -127,18 +127,8 @@ module_param_named(start, synth_bns.startup, short, S_IRUGO);
 MODULE_PARM_DESC(ser, "Set the serial port for the synthesizer (0-based).");
 MODULE_PARM_DESC(start, "Start the synthesizer once it is loaded.");
 
-static int __init bns_init(void)
-{
-	return synth_add(&synth_bns);
-}
+module_spk_synth(synth_bns);
 
-static void __exit bns_exit(void)
-{
-	synth_remove(&synth_bns);
-}
-
-module_init(bns_init);
-module_exit(bns_exit);
 MODULE_AUTHOR("Kirk Reiser <kirk@braille.uwo.ca>");
 MODULE_AUTHOR("David Borowski");
 MODULE_DESCRIPTION("Speakup support for Braille 'n Speak synthesizers");
diff --git a/drivers/staging/speakup/speakup_decext.c b/drivers/staging/speakup/speakup_decext.c
index 555029036dc2..e0b5db9bb46e 100644
--- a/drivers/staging/speakup/speakup_decext.c
+++ b/drivers/staging/speakup/speakup_decext.c
@@ -207,10 +207,12 @@ static void do_catch_up(struct spk_synth *synth)
 			if (time_after_eq(jiffies, jiff_max)) {
 				if (!in_escape)
 					spk_serial_out(PROCSPEECH);
-				spin_lock_irqsave(&speakup_info.spinlock, flags);
+				spin_lock_irqsave(&speakup_info.spinlock,
+							flags);
 				jiffy_delta_val = jiffy_delta->u.n.value;
 				delay_time_val = delay_time->u.n.value;
-				spin_unlock_irqrestore(&speakup_info.spinlock, flags);
+				spin_unlock_irqrestore(&speakup_info.spinlock,
+							flags);
 				schedule_timeout(msecs_to_jiffies
 						 (delay_time_val));
 				jiff_max = jiffies + jiffy_delta_val;
@@ -234,18 +236,8 @@ module_param_named(start, synth_decext.startup, short, S_IRUGO);
 MODULE_PARM_DESC(ser, "Set the serial port for the synthesizer (0-based).");
 MODULE_PARM_DESC(start, "Start the synthesizer once it is loaded.");
 
-static int __init decext_init(void)
-{
-	return synth_add(&synth_decext);
-}
-
-static void __exit decext_exit(void)
-{
-	synth_remove(&synth_decext);
-}
+module_spk_synth(synth_decext);
 
-module_init(decext_init);
-module_exit(decext_exit);
 MODULE_AUTHOR("Kirk Reiser <kirk@braille.uwo.ca>");
 MODULE_AUTHOR("David Borowski");
 MODULE_DESCRIPTION("Speakup support for DECtalk External synthesizers");
diff --git a/drivers/staging/speakup/speakup_decpc.c b/drivers/staging/speakup/speakup_decpc.c
index 7c9c432e48d9..437e13a85943 100644
--- a/drivers/staging/speakup/speakup_decpc.c
+++ b/drivers/staging/speakup/speakup_decpc.c
@@ -423,10 +423,12 @@ static void do_catch_up(struct spk_synth *synth)
 			if (time_after_eq(jiffies, jiff_max)) {
 				if (!in_escape)
 					dt_sendchar(PROCSPEECH);
-				spin_lock_irqsave(&speakup_info.spinlock, flags);
+				spin_lock_irqsave(&speakup_info.spinlock,
+							flags);
 				jiffy_delta_val = jiffy_delta->u.n.value;
 				delay_time_val = delay_time->u.n.value;
-				spin_unlock_irqrestore(&speakup_info.spinlock, flags);
+				spin_unlock_irqrestore(&speakup_info.spinlock,
+							flags);
 				schedule_timeout(msecs_to_jiffies
 						 (delay_time_val));
 				jiff_max = jiffies + jiffy_delta_val;
@@ -491,18 +493,8 @@ module_param_named(start, synth_dec_pc.startup, short, S_IRUGO);
 
 MODULE_PARM_DESC(start, "Start the synthesizer once it is loaded.");
 
-static int __init decpc_init(void)
-{
-	return synth_add(&synth_dec_pc);
-}
-
-static void __exit decpc_exit(void)
-{
-	synth_remove(&synth_dec_pc);
-}
+module_spk_synth(synth_dec_pc);
 
-module_init(decpc_init);
-module_exit(decpc_exit);
 MODULE_AUTHOR("Kirk Reiser <kirk@braille.uwo.ca>");
 MODULE_AUTHOR("David Borowski");
 MODULE_DESCRIPTION("Speakup support for DECtalk PC synthesizers");
diff --git a/drivers/staging/speakup/speakup_dectlk.c b/drivers/staging/speakup/speakup_dectlk.c
index 69e7c2b41b94..b5a23d42f4d5 100644
--- a/drivers/staging/speakup/speakup_dectlk.c
+++ b/drivers/staging/speakup/speakup_dectlk.c
@@ -306,18 +306,8 @@ module_param_named(start, synth_dectlk.startup, short, S_IRUGO);
 MODULE_PARM_DESC(ser, "Set the serial port for the synthesizer (0-based).");
 MODULE_PARM_DESC(start, "Start the synthesizer once it is loaded.");
 
-static int __init dectlk_init(void)
-{
-	return synth_add(&synth_dectlk);
-}
-
-static void __exit dectlk_exit(void)
-{
-	synth_remove(&synth_dectlk);
-}
+module_spk_synth(synth_dectlk);
 
-module_init(dectlk_init);
-module_exit(dectlk_exit);
 MODULE_AUTHOR("Kirk Reiser <kirk@braille.uwo.ca>");
 MODULE_AUTHOR("David Borowski");
 MODULE_DESCRIPTION("Speakup support for DECtalk Express synthesizers");
diff --git a/drivers/staging/speakup/speakup_dtlk.c b/drivers/staging/speakup/speakup_dtlk.c
index 89592c0b9151..345efd3344b0 100644
--- a/drivers/staging/speakup/speakup_dtlk.c
+++ b/drivers/staging/speakup/speakup_dtlk.c
@@ -388,18 +388,8 @@ module_param_named(start, synth_dtlk.startup, short, S_IRUGO);
 MODULE_PARM_DESC(port, "Set the port for the synthesizer (override probing).");
 MODULE_PARM_DESC(start, "Start the synthesizer once it is loaded.");
 
-static int __init dtlk_init(void)
-{
-	return synth_add(&synth_dtlk);
-}
-
-static void __exit dtlk_exit(void)
-{
-	synth_remove(&synth_dtlk);
-}
+module_spk_synth(synth_dtlk);
 
-module_init(dtlk_init);
-module_exit(dtlk_exit);
 MODULE_AUTHOR("Kirk Reiser <kirk@braille.uwo.ca>");
 MODULE_AUTHOR("David Borowski");
 MODULE_DESCRIPTION("Speakup support for DoubleTalk PC synthesizers");
diff --git a/drivers/staging/speakup/speakup_dummy.c b/drivers/staging/speakup/speakup_dummy.c
index 362342a194af..f66811269475 100644
--- a/drivers/staging/speakup/speakup_dummy.c
+++ b/drivers/staging/speakup/speakup_dummy.c
@@ -129,18 +129,8 @@ module_param_named(start, synth_dummy.startup, short, S_IRUGO);
 MODULE_PARM_DESC(ser, "Set the serial port for the synthesizer (0-based).");
 MODULE_PARM_DESC(start, "Start the synthesizer once it is loaded.");
 
-static int __init dummy_init(void)
-{
-	return synth_add(&synth_dummy);
-}
+module_spk_synth(synth_dummy);
 
-static void __exit dummy_exit(void)
-{
-	synth_remove(&synth_dummy);
-}
-
-module_init(dummy_init);
-module_exit(dummy_exit);
 MODULE_AUTHOR("Samuel Thibault <samuel.thibault@ens-lyon.org>");
 MODULE_DESCRIPTION("Speakup support for text console");
 MODULE_LICENSE("GPL");
diff --git a/drivers/staging/speakup/speakup_keypc.c b/drivers/staging/speakup/speakup_keypc.c
index cef20fdda646..6ea027365664 100644
--- a/drivers/staging/speakup/speakup_keypc.c
+++ b/drivers/staging/speakup/speakup_keypc.c
@@ -319,18 +319,8 @@ module_param_named(start, synth_keypc.startup, short, S_IRUGO);
 MODULE_PARM_DESC(port, "Set the port for the synthesizer (override probing).");
 MODULE_PARM_DESC(start, "Start the synthesizer once it is loaded.");
 
-static int __init keypc_init(void)
-{
-	return synth_add(&synth_keypc);
-}
-
-static void __exit keypc_exit(void)
-{
-	synth_remove(&synth_keypc);
-}
+module_spk_synth(synth_keypc);
 
-module_init(keypc_init);
-module_exit(keypc_exit);
 MODULE_AUTHOR("David Borowski");
 MODULE_DESCRIPTION("Speakup support for Keynote Gold PC synthesizers");
 MODULE_LICENSE("GPL");
diff --git a/drivers/staging/speakup/speakup_ltlk.c b/drivers/staging/speakup/speakup_ltlk.c
index 377a6e2b8b93..cc4806be806b 100644
--- a/drivers/staging/speakup/speakup_ltlk.c
+++ b/drivers/staging/speakup/speakup_ltlk.c
@@ -175,18 +175,8 @@ module_param_named(start, synth_ltlk.startup, short, S_IRUGO);
 MODULE_PARM_DESC(ser, "Set the serial port for the synthesizer (0-based).");
 MODULE_PARM_DESC(start, "Start the synthesizer once it is loaded.");
 
-static int __init ltlk_init(void)
-{
-	return synth_add(&synth_ltlk);
-}
-
-static void __exit ltlk_exit(void)
-{
-	synth_remove(&synth_ltlk);
-}
+module_spk_synth(synth_ltlk);
 
-module_init(ltlk_init);
-module_exit(ltlk_exit);
 MODULE_AUTHOR("Kirk Reiser <kirk@braille.uwo.ca>");
 MODULE_AUTHOR("David Borowski");
 MODULE_DESCRIPTION("Speakup support for DoubleTalk LT/LiteTalk synthesizers");
diff --git a/drivers/staging/speakup/speakup_soft.c b/drivers/staging/speakup/speakup_soft.c
index e6e93fbd183a..fb31bb95d83a 100644
--- a/drivers/staging/speakup/speakup_soft.c
+++ b/drivers/staging/speakup/speakup_soft.c
@@ -350,19 +350,8 @@ module_param_named(start, synth_soft.startup, short, S_IRUGO);
 
 MODULE_PARM_DESC(start, "Start the synthesizer once it is loaded.");
 
+module_spk_synth(synth_soft);
 
-static int __init soft_init(void)
-{
-	return synth_add(&synth_soft);
-}
-
-static void __exit soft_exit(void)
-{
-	synth_remove(&synth_soft);
-}
-
-module_init(soft_init);
-module_exit(soft_exit);
 MODULE_AUTHOR("Kirk Reiser <kirk@braille.uwo.ca>");
 MODULE_DESCRIPTION("Speakup userspace software synthesizer support");
 MODULE_LICENSE("GPL");
diff --git a/drivers/staging/speakup/speakup_spkout.c b/drivers/staging/speakup/speakup_spkout.c
index bccddf8b89fd..1007a6168c3c 100644
--- a/drivers/staging/speakup/speakup_spkout.c
+++ b/drivers/staging/speakup/speakup_spkout.c
@@ -146,18 +146,8 @@ module_param_named(start, synth_spkout.startup, short, S_IRUGO);
 MODULE_PARM_DESC(ser, "Set the serial port for the synthesizer (0-based).");
 MODULE_PARM_DESC(start, "Start the synthesizer once it is loaded.");
 
-static int __init spkout_init(void)
-{
-	return synth_add(&synth_spkout);
-}
-
-static void __exit spkout_exit(void)
-{
-	synth_remove(&synth_spkout);
-}
+module_spk_synth(synth_spkout);
 
-module_init(spkout_init);
-module_exit(spkout_exit);
 MODULE_AUTHOR("Kirk Reiser <kirk@braille.uwo.ca>");
 MODULE_AUTHOR("David Borowski");
 MODULE_DESCRIPTION("Speakup support for Speak Out synthesizers");
diff --git a/drivers/staging/speakup/speakup_txprt.c b/drivers/staging/speakup/speakup_txprt.c
index dbe84b13772c..6c21e7112210 100644
--- a/drivers/staging/speakup/speakup_txprt.c
+++ b/drivers/staging/speakup/speakup_txprt.c
@@ -127,18 +127,8 @@ module_param_named(start, synth_txprt.startup, short, S_IRUGO);
 MODULE_PARM_DESC(ser, "Set the serial port for the synthesizer (0-based).");
 MODULE_PARM_DESC(start, "Start the synthesizer once it is loaded.");
 
-static int __init txprt_init(void)
-{
-	return synth_add(&synth_txprt);
-}
+module_spk_synth(synth_txprt);
 
-static void __exit txprt_exit(void)
-{
-	synth_remove(&synth_txprt);
-}
-
-module_init(txprt_init);
-module_exit(txprt_exit);
 MODULE_AUTHOR("Kirk Reiser <kirk@braille.uwo.ca>");
 MODULE_AUTHOR("David Borowski");
 MODULE_DESCRIPTION("Speakup support for Transport synthesizers");
diff --git a/drivers/staging/speakup/spk_priv.h b/drivers/staging/speakup/spk_priv.h
index 637ba6760ec0..1ef3795b865d 100644
--- a/drivers/staging/speakup/spk_priv.h
+++ b/drivers/staging/speakup/spk_priv.h
@@ -44,34 +44,34 @@
 
 #define KT_SPKUP 15
 
-extern const struct old_serial_port *spk_serial_init(int index);
-extern void spk_stop_serial_interrupt(void);
-extern int spk_wait_for_xmitr(void);
-extern unsigned char spk_serial_in(void);
-extern unsigned char spk_serial_in_nowait(void);
-extern int spk_serial_out(const char ch);
-extern void spk_serial_release(void);
+const struct old_serial_port *spk_serial_init(int index);
+void spk_stop_serial_interrupt(void);
+int spk_wait_for_xmitr(void);
+unsigned char spk_serial_in(void);
+unsigned char spk_serial_in_nowait(void);
+int spk_serial_out(const char ch);
+void spk_serial_release(void);
 
-extern char synth_buffer_getc(void);
-extern char synth_buffer_peek(void);
-extern int synth_buffer_empty(void);
-extern struct var_t *spk_get_var(enum var_id_t var_id);
-extern ssize_t spk_var_show(struct kobject *kobj, struct kobj_attribute *attr,
-	char *buf);
-extern ssize_t spk_var_store(struct kobject *kobj, struct kobj_attribute *attr,
-	const char *buf, size_t count);
+char synth_buffer_getc(void);
+char synth_buffer_peek(void);
+int synth_buffer_empty(void);
+struct var_t *spk_get_var(enum var_id_t var_id);
+ssize_t spk_var_show(struct kobject *kobj, struct kobj_attribute *attr,
+		     char *buf);
+ssize_t spk_var_store(struct kobject *kobj, struct kobj_attribute *attr,
+		      const char *buf, size_t count);
 
-extern int spk_serial_synth_probe(struct spk_synth *synth);
-extern const char *spk_synth_immediate(struct spk_synth *synth, const char *buff);
-extern void spk_do_catch_up(struct spk_synth *synth);
-extern void spk_synth_flush(struct spk_synth *synth);
-extern int spk_synth_is_alive_nop(struct spk_synth *synth);
-extern int spk_synth_is_alive_restart(struct spk_synth *synth);
-extern void synth_printf(const char *buf, ...);
-extern int synth_request_region(u_long, u_long);
-extern int synth_release_region(u_long, u_long);
-extern int synth_add(struct spk_synth *in_synth);
-extern void synth_remove(struct spk_synth *in_synth);
+int spk_serial_synth_probe(struct spk_synth *synth);
+const char *spk_synth_immediate(struct spk_synth *synth, const char *buff);
+void spk_do_catch_up(struct spk_synth *synth);
+void spk_synth_flush(struct spk_synth *synth);
+int spk_synth_is_alive_nop(struct spk_synth *synth);
+int spk_synth_is_alive_restart(struct spk_synth *synth);
+void synth_printf(const char *buf, ...);
+int synth_request_region(u_long, u_long);
+int synth_release_region(u_long, u_long);
+int synth_add(struct spk_synth *in_synth);
+void synth_remove(struct spk_synth *in_synth);
 
 extern struct speakup_info_t speakup_info;
 
diff --git a/drivers/staging/speakup/spk_types.h b/drivers/staging/speakup/spk_types.h
index 8c565c94b8b2..e8ff5d7d6419 100644
--- a/drivers/staging/speakup/spk_types.h
+++ b/drivers/staging/speakup/spk_types.h
@@ -16,6 +16,7 @@
 #include <linux/spinlock.h>
 #include <linux/mutex.h>
 #include <linux/io.h>		/* for inb_p, outb_p, inb, outb, etc... */
+#include <linux/device.h>
 
 enum var_type_t {
 	VAR_NUM = 0,
@@ -167,7 +168,8 @@ struct spk_synth {
 	int *default_vol;
 	int (*probe)(struct spk_synth *synth);
 	void (*release)(void);
-	const char *(*synth_immediate)(struct spk_synth *synth, const char *buff);
+	const char *(*synth_immediate)(struct spk_synth *synth,
+					const char *buff);
 	void (*catch_up)(struct spk_synth *synth);
 	void (*flush)(struct spk_synth *synth);
 	int (*is_alive)(struct spk_synth *synth);
@@ -179,6 +181,16 @@ struct spk_synth {
 	struct attribute_group attributes;
 };
 
+/**
+ * module_spk_synth() - Helper macro for registering a speakup driver
+ * @__spk_synth: spk_synth struct
+ * Helper macro for speakup drivers which do not do anything special in module
+ * init/exit. This eliminates a lot of boilerplate. Each module may only
+ * use this macro once, and calling it replaces module_init() and module_exit()
+ */
+#define module_spk_synth(__spk_synth) \
+	module_driver(__spk_synth, synth_add, synth_remove)
+
 struct speakup_info_t {
 	spinlock_t spinlock;
 	int port_tts;
diff --git a/drivers/staging/ste_rmi4/synaptics_i2c_rmi4.c b/drivers/staging/ste_rmi4/synaptics_i2c_rmi4.c
index f92ae1d24f9f..0f524bb7b41d 100644
--- a/drivers/staging/ste_rmi4/synaptics_i2c_rmi4.c
+++ b/drivers/staging/ste_rmi4/synaptics_i2c_rmi4.c
@@ -209,7 +209,7 @@ static int synaptics_rmi4_set_page(struct synaptics_rmi4_data *pdata,
 		txbuf[1]	= page;
 		retval	= i2c_master_send(i2c, txbuf, PAGE_LEN);
 		if (retval != PAGE_LEN)
-			dev_err(&i2c->dev, "%s:failed:%d\n", __func__, retval);
+			dev_err(&i2c->dev, "failed:%d\n", retval);
 		else
 			pdata->current_page = page;
 	} else
@@ -283,7 +283,7 @@ static int synaptics_rmi4_i2c_byte_write(struct synaptics_rmi4_data *pdata,
 	retval		= i2c_master_send(pdata->i2c_client, txbuf, 2);
 	/* Add in retry on writes only in certain error return values */
 	if (retval != 2) {
-		dev_err(&i2c->dev, "%s:failed:%d\n", __func__, retval);
+		dev_err(&i2c->dev, "failed:%d\n", retval);
 		retval = -EIO;
 	} else
 		retval = 1;
@@ -830,8 +830,8 @@ static int synaptics_rmi4_i2c_query_device(struct synaptics_rmi4_data *pdata)
 
 	/* Check if this is a Synaptics device - report if not. */
 	if (pdata->rmi4_mod_info.manufacturer_id != 1)
-		dev_err(&client->dev, "%s: non-Synaptics mfg id:%d\n",
-			__func__, pdata->rmi4_mod_info.manufacturer_id);
+		dev_err(&client->dev, "non-Synaptics mfg id:%d\n",
+			pdata->rmi4_mod_info.manufacturer_id);
 
 	list_for_each_entry(rfi, &pdata->rmi4_mod_info.support_fn_list, link)
 		data_sources += rfi->num_of_data_sources;
@@ -990,8 +990,8 @@ static int synaptics_rmi4_probe
 					platformdata->irq_type,
 					DRIVER_NAME, rmi4_data);
 	if (retval) {
-		dev_err(&client->dev, "%s:Unable to get attn irq %d\n",
-				__func__, client->irq);
+		dev_err(&client->dev, "Unable to get attn irq %d\n",
+			client->irq);
 		goto err_query_dev;
 	}
 
@@ -1112,12 +1112,11 @@ static int synaptics_rmi4_resume(struct device *dev)
 	return 0;
 }
 
-static const struct dev_pm_ops synaptics_rmi4_dev_pm_ops = {
-	.suspend = synaptics_rmi4_suspend,
-	.resume  = synaptics_rmi4_resume,
-};
 #endif
 
+static SIMPLE_DEV_PM_OPS(synaptics_rmi4_dev_pm_ops, synaptics_rmi4_suspend,
+			 synaptics_rmi4_resume);
+
 static const struct i2c_device_id synaptics_rmi4_id_table[] = {
 	{ DRIVER_NAME, 0 },
 	{ },
@@ -1128,9 +1127,7 @@ static struct i2c_driver synaptics_rmi4_driver = {
 	.driver = {
 		.name	=	DRIVER_NAME,
 		.owner	=	THIS_MODULE,
-#ifdef CONFIG_PM
 		.pm	=	&synaptics_rmi4_dev_pm_ops,
-#endif
 	},
 	.probe		=	synaptics_rmi4_probe,
 	.remove		=	synaptics_rmi4_remove,
diff --git a/drivers/staging/unisys/common-spar/include/channels/iochannel.h b/drivers/staging/unisys/common-spar/include/channels/iochannel.h
index eb7efe484f6f..3bd7579e1daf 100644
--- a/drivers/staging/unisys/common-spar/include/channels/iochannel.h
+++ b/drivers/staging/unisys/common-spar/include/channels/iochannel.h
@@ -337,7 +337,7 @@ struct uiscmdrsp_scsi {
     /* peripheral type of 3 - processor */
     /* specifies device capable, but not present */
 
-#define DEV_HISUPPORT 0x10;	/* HiSup = 1; shows support for report luns */
+#define DEV_HISUPPORT 0x10	/* HiSup = 1; shows support for report luns */
     /* must be returned for lun 0. */
 
 /* NOTE: Linux code assumes inquiry contains 36 bytes. Without checking length
diff --git a/drivers/staging/unisys/common-spar/include/vmcallinterface.h b/drivers/staging/unisys/common-spar/include/vmcallinterface.h
index 78333719c496..59a7459eb962 100644
--- a/drivers/staging/unisys/common-spar/include/vmcallinterface.h
+++ b/drivers/staging/unisys/common-spar/include/vmcallinterface.h
@@ -79,18 +79,15 @@ enum vmcall_monitor_interface_method_tuple { /* VMCALL identification tuples  */
 #define ISSUE_IO_VMCALL(method, param, result) \
 	(result = unisys_vmcall(method, (param) & 0xFFFFFFFF,	\
 				(param) >> 32))
-#define ISSUE_IO_EXTENDED_VMCALL(method, param1, param2,	\
-				 param3, result)			\
-	(result = unisys_extended_vmcall(method, param1,	\
-					 param2, param3))
+#define ISSUE_IO_EXTENDED_VMCALL(method, param1, param2, param3) \
+	unisys_extended_vmcall(method, param1, param2, param3)
 
     /* The following uses VMCALL_POST_CODE_LOGEVENT interface but is currently
      * not used much */
 #define ISSUE_IO_VMCALL_POSTCODE_SEVERITY(postcode, severity)		\
 do {									\
-	u32 _tempresult = VMCALL_SUCCESS;				\
 	ISSUE_IO_EXTENDED_VMCALL(VMCALL_POST_CODE_LOGEVENT, severity,	\
-				 MDS_APPOS, postcode, _tempresult);	\
+				 MDS_APPOS, postcode);			\
 } while (0)
 #endif
 
diff --git a/drivers/staging/unisys/include/procobjecttree.h b/drivers/staging/unisys/include/procobjecttree.h
index 1174056ec3d9..809c6794290e 100644
--- a/drivers/staging/unisys/include/procobjecttree.h
+++ b/drivers/staging/unisys/include/procobjecttree.h
@@ -26,7 +26,6 @@
 #ifndef __PROCOBJECTTREE_H__
 #define __PROCOBJECTTREE_H__
 
-#include "uniklog.h"
 #include "timskmod.h"
 
 /* These are opaque structures to users.
diff --git a/drivers/staging/unisys/include/timskmod.h b/drivers/staging/unisys/include/timskmod.h
index 4019a0d63645..5a933d7bf39f 100644
--- a/drivers/staging/unisys/include/timskmod.h
+++ b/drivers/staging/unisys/include/timskmod.h
@@ -68,15 +68,6 @@
 #define HOSTADDRESS unsigned long long
 #endif
 
-/** Try to evaulate the provided expression, and do a RETINT(x) iff
- *  the expression evaluates to < 0.
- */
-#define ASSERT(cond)                                           \
-	do { if (!(cond))                                      \
-			HUHDRV("ASSERT failed - %s",	       \
-			       __stringify(cond));	       \
-	} while (0)
-
 #define sizeofmember(TYPE, MEMBER) (sizeof(((TYPE *)0)->MEMBER))
 /** "Covered quotient" function */
 #define COVQ(v, d)  (((v) + (d) - 1) / (d))
@@ -87,14 +78,8 @@
 		(void *)(p2) = SWAPPOINTERS_TEMP;	\
 	} while (0)
 
-#define PRINTKDRV(fmt, args...) LOGINF(fmt, ## args)
-#define TBDDRV(fmt, args...)    LOGERR(fmt, ## args)
-#define HUHDRV(fmt, args...)    LOGERR(fmt, ## args)
-#define ERRDRV(fmt, args...)    LOGERR(fmt, ## args)
 #define WARNDRV(fmt, args...)   LOGWRN(fmt, ## args)
 #define SECUREDRV(fmt, args...) LOGWRN(fmt, ## args)
-#define INFODRV(fmt, args...)   LOGINF(fmt, ## args)
-#define DEBUGDRV(fmt, args...)  DBGINF(fmt, ## args)
 
 #define PRINTKDEV(devname, fmt, args...)  LOGINFDEV(devname, fmt, ## args)
 #define TBDDEV(devname, fmt, args...)     LOGERRDEV(devname, fmt, ## args)
@@ -105,7 +90,6 @@
 #define SECUREDEV(devname, fmt, args...)  LOGWRNDEV(devname, fmt, ## args)
 #define INFODEV(devname, fmt, args...)    LOGINFDEV(devname, fmt, ## args)
 #define INFODEVX(devno, fmt, args...)     LOGINFDEVX(devno, fmt, ## args)
-#define DEBUGDEV(devname, fmt, args...)   DBGINFDEV(devname, fmt, ## args)
 
 /** Verifies the consistency of your PRIVATEDEVICEDATA structure using
  *  conventional "signature" fields:
diff --git a/drivers/staging/unisys/include/uisqueue.h b/drivers/staging/unisys/include/uisqueue.h
index 25b6181d78af..08ba16ea840e 100644
--- a/drivers/staging/unisys/include/uisqueue.h
+++ b/drivers/staging/unisys/include/uisqueue.h
@@ -25,7 +25,6 @@
 
 #include "linux/version.h"
 #include "iochannel.h"
-#include "uniklog.h"
 #include <linux/atomic.h>
 #include <linux/semaphore.h>
 #include <linux/uuid.h>
diff --git a/drivers/staging/unisys/include/uisthread.h b/drivers/staging/unisys/include/uisthread.h
index aa86ade7cb13..52c3eb4ded2c 100644
--- a/drivers/staging/unisys/include/uisthread.h
+++ b/drivers/staging/unisys/include/uisthread.h
@@ -27,7 +27,6 @@
 struct uisthread_info {
 	struct task_struct *task;
 	int id;
-	int should_stop;
 	struct completion has_stopped;
 };
 
diff --git a/drivers/staging/unisys/include/uisutils.h b/drivers/staging/unisys/include/uisutils.h
index 7414220676d3..c7d0ba8aafd8 100644
--- a/drivers/staging/unisys/include/uisutils.h
+++ b/drivers/staging/unisys/include/uisutils.h
@@ -65,17 +65,7 @@ struct req_handler_info {
 	struct list_head list_link;	/* links into ReqHandlerInfo_list */
 };
 
-struct req_handler_info *req_handler_add(uuid_le switch_uuid,
-				const char *switch_type_name,
-				int (*controlfunc)(struct io_msgs *),
-				unsigned long min_channel_bytes,
-				int (*svr_channel_ok)(unsigned long
-							 channel_bytes),
-				int (*svr_channel_init)(void *x,
-						unsigned char *client_str,
-						u32 client_str_len, u64 bytes));
 struct req_handler_info *req_handler_find(uuid_le switch_uuid);
-int req_handler_del(uuid_le switch_uuid);
 
 #define uislib_ioremap_cache(addr, size) \
 	dbg_ioremap_cache(addr, size, __FILE__, __LINE__)
@@ -115,19 +105,7 @@ int uisutil_add_proc_line_ex(int *total, char **buffer, int *buffer_remaining,
 
 int uisctrl_register_req_handler(int type, void *fptr,
 			struct ultra_vbus_deviceinfo *chipset_driver_info);
-int uisctrl_register_req_handler_ex(uuid_le switch_guid,
-			const char *switch_type_name,
-			int (*fptr)(struct io_msgs *),
-			unsigned long min_channel_bytes,
-			int (*svr_channel_ok)(unsigned long
-					      channel_bytes),
-			int (*svr_channel_init)(void *x,
-						unsigned char *client_str,
-						u32 client_str_len,
-						u64 bytes),
-			struct ultra_vbus_deviceinfo *chipset_driver_info);
 
-int uisctrl_unregister_req_handler_ex(uuid_le switch_uuid);
 unsigned char *util_map_virt(struct phys_info *sg);
 void util_unmap_virt(struct phys_info *sg);
 unsigned char *util_map_virt_atomic(struct phys_info *sg);
@@ -206,21 +184,10 @@ wait_for_valid_guid(uuid_le __iomem *guid)
 			      (void __iomem *)guid, sizeof(uuid_le));
 		if (uuid_le_cmp(tmpguid, NULL_UUID_LE) != 0)
 			break;
-		LOGERR("Waiting for non-0 GUID (why???)...\n");
 		UIS_THREAD_WAIT_SEC(5);
 	}
-	LOGERR("OK... GUID is non-0 now\n");
 }
 
-/* CopyFragsInfoFromSkb returns the number of entries added to frags array
- * Returns -1 on failure.
- */
-unsigned int uisutil_copy_fragsinfo_from_skb(unsigned char *calling_ctx,
-					     void *skb_in,
-					     unsigned int firstfraglen,
-					     unsigned int frags_max,
-					     struct phys_info frags[]);
-
 static inline unsigned int
 issue_vmcall_io_controlvm_addr(u64 *control_addr, u32 *control_bytes)
 {
diff --git a/drivers/staging/unisys/include/uniklog.h b/drivers/staging/unisys/include/uniklog.h
deleted file mode 100644
index ecd1bdb6d097..000000000000
--- a/drivers/staging/unisys/include/uniklog.h
+++ /dev/null
@@ -1,191 +0,0 @@
-/* uniklog.h
- *
- * Copyright (C) 2010 - 2013 UNISYS CORPORATION
- * All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or (at
- * your option) any later version.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE, GOOD TITLE or
- * NON INFRINGEMENT.  See the GNU General Public License for more
- * details.
- */
-
-/* This module contains macros to aid developers in logging messages.
- *
- * This module is affected by the DEBUG compiletime option.
- *
- */
-#ifndef __UNIKLOG_H__
-#define __UNIKLOG_H__
-
-#include <linux/printk.h>
-
-/*
- * # DBGINF
- *
- * \brief Log debug informational message - log a LOG_INFO message only
- *        if DEBUG compiletime option enabled
- *
- * \param devname the device name of the device reporting this message, or
- *                NULL if this message is NOT device-related.
- * \param fmt printf()-style format string containing the message to log.
- * \param args Optional arguments to be formatted and inserted into the
- *             format string.
- * \return nothing
- *
- * Log a message at the LOG_INFO level, but only if DEBUG is enabled.  If
- * DEBUG is disabled, this expands to a no-op.
- */
-
-/*
- * # DBGVER
- *
- * \brief Log debug verbose message - log a LOG_DEBUG message only if
- *        DEBUG compiletime option enabled
- *
- * \param devname the device name of the device reporting this message, or
- *                NULL if this message is NOT device-related.
- * \param fmt printf()-style format string containing the message to log.
- * \param args Optional arguments to be formatted and inserted into the
- *             format string.
- * \return nothing
- *
- * Log a message at the LOG_DEBUG level, but only if DEBUG is enabled.  If
- * DEBUG is disabled, this expands to a no-op.  Note also that LOG_DEBUG
- * messages can be enabled/disabled at runtime as well.
- */
-#define DBGINFDEV(devname, fmt, args...)        do { } while (0)
-#define DBGVERDEV(devname, fmt, args...)        do { } while (0)
-#define DBGINF(fmt, args...)                    do { } while (0)
-#define DBGVER(fmt, args...)                    do { } while (0)
-
-/*
- * # LOGINF
- *
- * \brief Log informational message - logs a message at the LOG_INFO level
- *
- * \param devname the device name of the device reporting this message, or
- *                NULL if this message is NOT device-related.
- * \param fmt printf()-style format string containing the message to log.
- * \param args Optional arguments to be formatted and inserted into the
- *             format string.
- * \return nothing
- *
- * Logs the specified message at the LOG_INFO level.
- */
-
-#define LOGINF(fmt, args...) pr_info(fmt, ## args)
-#define LOGINFDEV(devname, fmt, args...) \
-	pr_info("%s " fmt, devname, ## args)
-#define LOGINFDEVX(devno, fmt, args...) \
-	pr_info("dev%d " fmt, devno, ## args)
-#define LOGINFNAME(vnic, fmt, args...)				\
-	do {								\
-		if (vnic != NULL) {					\
-			pr_info("%s " fmt, vnic->name, ## args);	\
-		} else {						\
-			pr_info(fmt, ## args);				\
-		}							\
-	} while (0)
-
-/*
- * # LOGVER
- *
- * \brief Log verbose message - logs a message at the LOG_DEBUG level,
- *        which can be disabled at runtime
- *
- * \param devname the device name of the device reporting this message, or
- *                NULL if this message is NOT device-related.
- * \param fmt printf()-style format string containing the message to log.
- * \param args Optional arguments to be formatted and inserted into the format
- * \param string.
- * \return nothing
- *
- * Logs the specified message at the LOG_DEBUG level.  Note also that
- * LOG_DEBUG messages can be enabled/disabled at runtime as well.
- */
-#define LOGVER(fmt, args...) pr_debug(fmt, ## args)
-#define LOGVERDEV(devname, fmt, args...) \
-	pr_debug("%s " fmt, devname, ## args)
-#define LOGVERNAME(vnic, fmt, args...)					\
-	do {								\
-		if (vnic != NULL) {					\
-			pr_debug("%s " fmt, vnic->name, ## args);	\
-		} else {						\
-			pr_debug(fmt, ## args);				\
-		}							\
-	} while (0)
-
-/*
- * # LOGERR
- *
- * \brief Log error message - logs a message at the LOG_ERR level,
- *        including source line number information
- *
- * \param devname the device name of the device reporting this message, or
- *                NULL if this message is NOT device-related.
- * \param fmt printf()-style format string containing the message to log.
- * \param args Optional arguments to be formatted and inserted into the format
- * \param string.
- * \return nothing
- *
- * Logs the specified error message at the LOG_ERR level.  It will also
- * include the file, line number, and function name of where the error
- * originated in the log message.
- */
-#define LOGERR(fmt, args...) pr_err(fmt, ## args)
-#define LOGERRDEV(devname, fmt, args...) \
-	pr_err("%s " fmt, devname, ## args)
-#define LOGERRDEVX(devno, fmt, args...) \
-	pr_err("dev%d " fmt, devno, ## args)
-#define LOGERRNAME(vnic, fmt, args...)				\
-	do {								\
-		if (vnic != NULL) {					\
-			pr_err("%s " fmt, vnic->name, ## args);	\
-		} else {						\
-			pr_err(fmt, ## args);				\
-		}							\
-	} while (0)
-#define LOGORDUMPERR(seqfile, fmt, args...) do {		\
-		if (seqfile) {					\
-			seq_printf(seqfile, fmt, ## args);	\
-		} else {					\
-			LOGERR(fmt, ## args);			\
-		}						\
-	} while (0)
-
-/*
- * # LOGWRN
- *
- * \brief Log warning message - Logs a message at the LOG_WARNING level,
- *        including source line number information
- *
- * \param devname the device name of the device reporting this message, or
- *                NULL if this message is NOT device-related.
- * \param fmt printf()-style format string containing the message to log.
- * \param args Optional arguments to be formatted and inserted into the format
- * \param string.
- * \return nothing
- *
- * Logs the specified error message at the LOG_WARNING level.  It will also
- * include the file, line number, and function name of where the error
- * originated in the log message.
- */
-#define LOGWRN(fmt, args...) pr_warn(fmt, ## args)
-#define LOGWRNDEV(devname, fmt, args...) \
-	pr_warn("%s " fmt, devname, ## args)
-#define LOGWRNNAME(vnic, fmt, args...) \
-	do {								\
-		if (vnic != NULL) {					\
-			pr_warn("%s " fmt, vnic->name, ## args);	\
-		} else {						\
-			pr_warn(fmt, ## args);				\
-		}							\
-	} while (0)
-
-#endif /* __UNIKLOG_H__ */
diff --git a/drivers/staging/unisys/uislib/Kconfig b/drivers/staging/unisys/uislib/Kconfig
index a712eb82224a..c39a0a21ae5f 100644
--- a/drivers/staging/unisys/uislib/Kconfig
+++ b/drivers/staging/unisys/uislib/Kconfig
@@ -4,7 +4,7 @@
 
 config UNISYS_UISLIB
 	tristate "Unisys uislib driver"
-	depends on UNISYSSPAR && UNISYS_VISORCHIPSET && HAS_IOMEM
+	select UNISYS_VISORCHIPSET
 	---help---
 	If you say Y here, you will enable the Unisys uislib driver.
 
diff --git a/drivers/staging/unisys/uislib/Makefile b/drivers/staging/unisys/uislib/Makefile
index 08e620d17497..860f494f132f 100644
--- a/drivers/staging/unisys/uislib/Makefile
+++ b/drivers/staging/unisys/uislib/Makefile
@@ -7,8 +7,6 @@ obj-$(CONFIG_UNISYS_UISLIB)	+= visoruislib.o
 visoruislib-y := uislib.o uisqueue.o  uisthread.o  uisutils.o
 
 ccflags-y += -Idrivers/staging/unisys/include
-ccflags-y += -Idrivers/staging/unisys/channels
 ccflags-y += -Idrivers/staging/unisys/visorchipset
-ccflags-y += -Idrivers/staging/unisys/sparstopdriver
 ccflags-y += -Idrivers/staging/unisys/common-spar/include
 ccflags-y += -Idrivers/staging/unisys/common-spar/include/channels
diff --git a/drivers/staging/unisys/uislib/uislib.c b/drivers/staging/unisys/uislib/uislib.c
index a9eeddeba735..f93d0bb11b12 100644
--- a/drivers/staging/unisys/uislib/uislib.c
+++ b/drivers/staging/unisys/uislib/uislib.c
@@ -29,7 +29,6 @@
 #include <linux/uuid.h>
 
 #include <linux/version.h>
-#include "uniklog.h"
 #include "diagnostics/appos_subsystems.h"
 #include "uisutils.h"
 #include "vbuschannel.h"
@@ -132,14 +131,10 @@ static __iomem void *init_vbus_channel(u64 ch_addr, u32 ch_bytes)
 {
 	void __iomem *ch = uislib_ioremap_cache(ch_addr, ch_bytes);
 
-	if (!ch) {
-		LOGERR("CONTROLVM_BUS_CREATE error: ioremap_cache of channelAddr:%Lx for channelBytes:%llu failed",
-		       (unsigned long long)ch_addr,
-		       (unsigned long long)ch_bytes);
+	if (!ch)
 		return NULL;
-	}
+
 	if (!SPAR_VBUS_CHANNEL_OK_CLIENT(ch)) {
-		ERRDRV("%s channel cannot be used", __func__);
 		uislib_iounmap(ch);
 		return NULL;
 	}
@@ -154,8 +149,6 @@ create_bus(struct controlvm_message *msg, char *buf)
 	size_t size;
 
 	if (max_bus_count == bus_list_count) {
-		LOGERR("CONTROLVM_BUS_CREATE Failed: max buses:%d already created\n",
-		       max_bus_count);
 		POSTCODE_LINUX_3(BUS_CREATE_FAILURE_PC, max_bus_count,
 				 POSTCODE_SEVERITY_ERR);
 		return CONTROLVM_RESP_ERROR_MAX_BUSES;
@@ -172,7 +165,6 @@ create_bus(struct controlvm_message *msg, char *buf)
 	    (dev_count * sizeof(struct device_info *));
 	bus = kzalloc(size, GFP_ATOMIC);
 	if (!bus) {
-		LOGERR("CONTROLVM_BUS_CREATE Failed: kmalloc for bus failed.\n");
 		POSTCODE_LINUX_3(BUS_CREATE_FAILURE_PC, bus_no,
 				 POSTCODE_SEVERITY_ERR);
 		return CONTROLVM_RESP_ERROR_KMALLOC_FAILED;
@@ -209,8 +201,6 @@ create_bus(struct controlvm_message *msg, char *buf)
 		/* found a bus already in the list with same bus_no -
 		 * reject add
 		 */
-		LOGERR("CONTROLVM_BUS_CREATE Failed: bus %d already exists.\n",
-		       bus->bus_no);
 		POSTCODE_LINUX_3(BUS_CREATE_FAILURE_PC, bus->bus_no,
 				 POSTCODE_SEVERITY_ERR);
 		kfree(bus);
@@ -234,14 +224,12 @@ create_bus(struct controlvm_message *msg, char *buf)
 		cmd.add_vbus.bus_uuid = msg->cmd.create_bus.bus_data_type_uuid;
 		cmd.add_vbus.instance_uuid = msg->cmd.create_bus.bus_inst_uuid;
 		if (!virt_control_chan_func) {
-			LOGERR("CONTROLVM_BUS_CREATE Failed: virtpci callback not registered.");
 			POSTCODE_LINUX_3(BUS_CREATE_FAILURE_PC, bus->bus_no,
 					 POSTCODE_SEVERITY_ERR);
 			kfree(bus);
 			return CONTROLVM_RESP_ERROR_VIRTPCI_DRIVER_FAILURE;
 		}
 		if (!virt_control_chan_func(&cmd)) {
-			LOGERR("CONTROLVM_BUS_CREATE Failed: virtpci GUEST_ADD_VBUS returned error.");
 			POSTCODE_LINUX_3(BUS_CREATE_FAILURE_PC, bus->bus_no,
 					 POSTCODE_SEVERITY_ERR);
 			kfree(bus);
@@ -287,17 +275,13 @@ destroy_bus(struct controlvm_message *msg, char *buf)
 	}
 
 	if (!bus) {
-		LOGERR("CONTROLVM_BUS_DESTROY Failed: failed to find bus %d.\n",
-		       bus_no);
 		read_unlock(&bus_list_lock);
 		return CONTROLVM_RESP_ERROR_ALREADY_DONE;
 	}
 
 	/* verify that this bus has no devices. */
 	for (i = 0; i < bus->device_count; i++) {
-		if (bus->device[i] != NULL) {
-			LOGERR("CONTROLVM_BUS_DESTROY Failed: device %i attached to bus %d.",
-			       i, bus_no);
+		if (bus->device[i]) {
 			read_unlock(&bus_list_lock);
 			return CONTROLVM_RESP_ERROR_BUS_DEVICE_ATTACHED;
 		}
@@ -311,14 +295,11 @@ destroy_bus(struct controlvm_message *msg, char *buf)
 	   with this bus. */
 	cmd.msgtype = GUEST_DEL_VBUS;
 	cmd.del_vbus.bus_no = bus_no;
-	if (!virt_control_chan_func) {
-		LOGERR("CONTROLVM_BUS_DESTROY Failed: virtpci callback not registered.");
+	if (!virt_control_chan_func)
 		return CONTROLVM_RESP_ERROR_VIRTPCI_DRIVER_FAILURE;
-	}
-	if (!virt_control_chan_func(&cmd)) {
-		LOGERR("CONTROLVM_BUS_DESTROY Failed: virtpci GUEST_DEL_VBUS returned error.");
+
+	if (!virt_control_chan_func(&cmd))
 		return CONTROLVM_RESP_ERROR_VIRTPCI_DRIVER_CALLBACK_ERROR;
-	}
 
 	/* finally, remove the bus from the list */
 remove:
@@ -357,7 +338,6 @@ static int create_device(struct controlvm_message *msg, char *buf)
 
 	dev = kzalloc(sizeof(*dev), GFP_ATOMIC);
 	if (!dev) {
-		LOGERR("CONTROLVM_DEVICE_CREATE Failed: kmalloc for dev failed.\n");
 		POSTCODE_LINUX_4(DEVICE_CREATE_FAILURE_PC, dev_no, bus_no,
 				 POSTCODE_SEVERITY_ERR);
 		return CONTROLVM_RESP_ERROR_KMALLOC_FAILED;
@@ -381,9 +361,6 @@ static int create_device(struct controlvm_message *msg, char *buf)
 			 */
 			min_size = req_handler->min_channel_bytes;
 		if (min_size > msg->cmd.create_device.channel_bytes) {
-			LOGERR("CONTROLVM_DEVICE_CREATE Failed: channel size is too small, channel size:0x%lx, required size:0x%lx",
-			       (ulong)msg->cmd.create_device.channel_bytes,
-			       (ulong)min_size);
 			POSTCODE_LINUX_4(DEVICE_CREATE_FAILURE_PC, dev_no,
 					 bus_no, POSTCODE_SEVERITY_ERR);
 			result = CONTROLVM_RESP_ERROR_CHANNEL_SIZE_TOO_SMALL;
@@ -393,9 +370,6 @@ static int create_device(struct controlvm_message *msg, char *buf)
 		    uislib_ioremap_cache(dev->channel_addr,
 					 msg->cmd.create_device.channel_bytes);
 		if (!dev->chanptr) {
-			LOGERR("CONTROLVM_DEVICE_CREATE Failed: ioremap_cache of channelAddr:%Lx for channelBytes:%llu failed",
-			       dev->channel_addr,
-			       msg->cmd.create_device.channel_bytes);
 			result = CONTROLVM_RESP_ERROR_IOREMAP_FAILED;
 			POSTCODE_LINUX_4(DEVICE_CREATE_FAILURE_PC, dev_no,
 					 bus_no, POSTCODE_SEVERITY_ERR);
@@ -411,8 +385,6 @@ static int create_device(struct controlvm_message *msg, char *buf)
 			continue;
 		/* make sure the device number is valid */
 		if (dev_no >= bus->device_count) {
-			LOGERR("CONTROLVM_DEVICE_CREATE Failed: device (%d) >= deviceCount (%d).",
-			       dev_no, bus->device_count);
 			result = CONTROLVM_RESP_ERROR_MAX_DEVICES;
 			POSTCODE_LINUX_4(DEVICE_CREATE_FAILURE_PC, dev_no,
 					 bus_no, POSTCODE_SEVERITY_ERR);
@@ -421,8 +393,6 @@ static int create_device(struct controlvm_message *msg, char *buf)
 		}
 		/* make sure this device is not already set */
 		if (bus->device[dev_no]) {
-			LOGERR("CONTROLVM_DEVICE_CREATE Failed: device %d is already exists.",
-			       dev_no);
 			POSTCODE_LINUX_4(DEVICE_CREATE_FAILURE_PC,
 					 dev_no, bus_no,
 					 POSTCODE_SEVERITY_ERR);
@@ -445,8 +415,6 @@ static int create_device(struct controlvm_message *msg, char *buf)
 			wait_for_valid_guid(&((struct channel_header __iomem *)
 					    (dev->chanptr))->chtype);
 			if (!SPAR_VHBA_CHANNEL_OK_CLIENT(dev->chanptr)) {
-				LOGERR("CONTROLVM_DEVICE_CREATE Failed:[CLIENT]VHBA dev %d chan invalid.",
-				       dev_no);
 				POSTCODE_LINUX_4(DEVICE_CREATE_FAILURE_PC,
 						 dev_no, bus_no,
 						 POSTCODE_SEVERITY_ERR);
@@ -464,8 +432,6 @@ static int create_device(struct controlvm_message *msg, char *buf)
 			wait_for_valid_guid(&((struct channel_header __iomem *)
 					    (dev->chanptr))->chtype);
 			if (!SPAR_VNIC_CHANNEL_OK_CLIENT(dev->chanptr)) {
-				LOGERR("CONTROLVM_DEVICE_CREATE Failed: VNIC[CLIENT] dev %d chan invalid.",
-				       dev_no);
 				POSTCODE_LINUX_4(DEVICE_CREATE_FAILURE_PC,
 						 dev_no, bus_no,
 						 POSTCODE_SEVERITY_ERR);
@@ -479,7 +445,6 @@ static int create_device(struct controlvm_message *msg, char *buf)
 			cmd.add_vnic.instance_uuid = dev->instance_uuid;
 			cmd.add_vhba.intr = dev->intr;
 		} else {
-			LOGERR("CONTROLVM_DEVICE_CREATE Failed: unknown channelTypeGuid.\n");
 			POSTCODE_LINUX_4(DEVICE_CREATE_FAILURE_PC, dev_no,
 					 bus_no, POSTCODE_SEVERITY_ERR);
 			result = CONTROLVM_RESP_ERROR_CHANNEL_TYPE_UNKNOWN;
@@ -487,7 +452,6 @@ static int create_device(struct controlvm_message *msg, char *buf)
 		}
 
 		if (!virt_control_chan_func) {
-			LOGERR("CONTROLVM_DEVICE_CREATE Failed: virtpci callback not registered.");
 			POSTCODE_LINUX_4(DEVICE_CREATE_FAILURE_PC, dev_no,
 					 bus_no, POSTCODE_SEVERITY_ERR);
 			result = CONTROLVM_RESP_ERROR_VIRTPCI_DRIVER_FAILURE;
@@ -495,7 +459,6 @@ static int create_device(struct controlvm_message *msg, char *buf)
 		}
 
 		if (!virt_control_chan_func(&cmd)) {
-			LOGERR("CONTROLVM_DEVICE_CREATE Failed: virtpci GUEST_ADD_[VHBA||VNIC] returned error.");
 			POSTCODE_LINUX_4(DEVICE_CREATE_FAILURE_PC, dev_no,
 					 bus_no, POSTCODE_SEVERITY_ERR);
 			result =
@@ -510,8 +473,6 @@ static int create_device(struct controlvm_message *msg, char *buf)
 	}
 	read_unlock(&bus_list_lock);
 
-	LOGERR("CONTROLVM_DEVICE_CREATE Failed: failed to find bus %d.",
-	       bus_no);
 	POSTCODE_LINUX_4(DEVICE_CREATE_FAILURE_PC, dev_no, bus_no,
 			 POSTCODE_SEVERITY_ERR);
 	result = CONTROLVM_RESP_ERROR_BUS_INVALID;
@@ -542,15 +503,11 @@ static int pause_device(struct controlvm_message *msg)
 		if (bus->bus_no == bus_no) {
 			/* make sure the device number is valid */
 			if (dev_no >= bus->device_count) {
-				LOGERR("CONTROLVM_DEVICE_CHANGESTATE:pause Failed: device(%d) >= deviceCount(%d).",
-				       dev_no, bus->device_count);
 				retval = CONTROLVM_RESP_ERROR_DEVICE_INVALID;
 			} else {
 				/* make sure this device exists */
 				dev = bus->device[dev_no];
 				if (!dev) {
-					LOGERR("CONTROLVM_DEVICE_CHANGESTATE:pause Failed: device %d does not exist.",
-					       dev_no);
 					retval =
 					  CONTROLVM_RESP_ERROR_ALREADY_DONE;
 				}
@@ -558,11 +515,9 @@ static int pause_device(struct controlvm_message *msg)
 			break;
 		}
 	}
-	if (!bus) {
-		LOGERR("CONTROLVM_DEVICE_CHANGESTATE:pause Failed: bus %d does not exist",
-		       bus_no);
+	if (!bus)
 		retval = CONTROLVM_RESP_ERROR_BUS_INVALID;
-	}
+
 	read_unlock(&bus_list_lock);
 	if (retval == CONTROLVM_RESP_SUCCESS) {
 		/* the msg is bound for virtpci; send
@@ -577,15 +532,11 @@ static int pause_device(struct controlvm_message *msg)
 			cmd.msgtype = GUEST_PAUSE_VNIC;
 			cmd.pause_vnic.chanptr = dev->chanptr;
 		} else {
-			LOGERR("CONTROLVM_DEVICE_CHANGESTATE:pause Failed: unknown channelTypeGuid.\n");
 			return CONTROLVM_RESP_ERROR_CHANNEL_TYPE_UNKNOWN;
 		}
-		if (!virt_control_chan_func) {
-			LOGERR("CONTROLVM_DEVICE_CHANGESTATE Failed: virtpci callback not registered.");
+		if (!virt_control_chan_func)
 			return CONTROLVM_RESP_ERROR_VIRTPCI_DRIVER_FAILURE;
-		}
 		if (!virt_control_chan_func(&cmd)) {
-			LOGERR("CONTROLVM_DEVICE_CHANGESTATE:pause Failed: virtpci GUEST_PAUSE_[VHBA||VNIC] returned error.");
 			return
 			  CONTROLVM_RESP_ERROR_VIRTPCI_DRIVER_CALLBACK_ERROR;
 		}
@@ -609,15 +560,11 @@ static int resume_device(struct controlvm_message *msg)
 		if (bus->bus_no == bus_no) {
 			/* make sure the device number is valid */
 			if (dev_no >= bus->device_count) {
-				LOGERR("CONTROLVM_DEVICE_CHANGESTATE:resume Failed: device(%d) >= deviceCount(%d).",
-				       dev_no, bus->device_count);
 				retval = CONTROLVM_RESP_ERROR_DEVICE_INVALID;
 			} else {
 				/* make sure this device exists */
 				dev = bus->device[dev_no];
 				if (!dev) {
-					LOGERR("CONTROLVM_DEVICE_CHANGESTATE:resume Failed: device %d does not exist.",
-					       dev_no);
 					retval =
 					  CONTROLVM_RESP_ERROR_ALREADY_DONE;
 				}
@@ -626,11 +573,9 @@ static int resume_device(struct controlvm_message *msg)
 		}
 	}
 
-	if (!bus) {
-		LOGERR("CONTROLVM_DEVICE_CHANGESTATE:resume Failed: bus %d does not exist",
-		       bus_no);
+	if (!bus)
 		retval = CONTROLVM_RESP_ERROR_BUS_INVALID;
-	}
+
 	read_unlock(&bus_list_lock);
 	/* the msg is bound for virtpci; send
 	 * guest_msgs struct to callback
@@ -645,15 +590,11 @@ static int resume_device(struct controlvm_message *msg)
 			cmd.msgtype = GUEST_RESUME_VNIC;
 			cmd.resume_vnic.chanptr = dev->chanptr;
 		} else {
-			LOGERR("CONTROLVM_DEVICE_CHANGESTATE:resume Failed: unknown channelTypeGuid.\n");
 			return CONTROLVM_RESP_ERROR_CHANNEL_TYPE_UNKNOWN;
 		}
-		if (!virt_control_chan_func) {
-			LOGERR("CONTROLVM_DEVICE_CHANGESTATE Failed: virtpci callback not registered.");
+		if (!virt_control_chan_func)
 			return CONTROLVM_RESP_ERROR_VIRTPCI_DRIVER_FAILURE;
-		}
 		if (!virt_control_chan_func(&cmd)) {
-			LOGERR("CONTROLVM_DEVICE_CHANGESTATE:resume Failed: virtpci GUEST_RESUME_[VHBA||VNIC] returned error.");
 			return
 			  CONTROLVM_RESP_ERROR_VIRTPCI_DRIVER_CALLBACK_ERROR;
 		}
@@ -673,21 +614,15 @@ static int destroy_device(struct controlvm_message *msg, char *buf)
 	dev_no = msg->cmd.destroy_device.bus_no;
 
 	read_lock(&bus_list_lock);
-	LOGINF("destroy_device called for bus_no=%u, dev_no=%u", bus_no,
-	       dev_no);
 	for (bus = bus_list; bus; bus = bus->next) {
 		if (bus->bus_no == bus_no) {
 			/* make sure the device number is valid */
 			if (dev_no >= bus->device_count) {
-				LOGERR("CONTROLVM_DEVICE_DESTROY Failed: device(%d) >= device_count(%d).",
-				       dev_no, bus->device_count);
 				retval = CONTROLVM_RESP_ERROR_DEVICE_INVALID;
 			} else {
 				/* make sure this device exists */
 				dev = bus->device[dev_no];
 				if (!dev) {
-					LOGERR("CONTROLVM_DEVICE_DESTROY Failed: device %d does not exist.",
-					       dev_no);
 					retval =
 					     CONTROLVM_RESP_ERROR_ALREADY_DONE;
 				}
@@ -696,11 +631,8 @@ static int destroy_device(struct controlvm_message *msg, char *buf)
 		}
 	}
 
-	if (!bus) {
-		LOGERR("CONTROLVM_DEVICE_DESTROY Failed: bus %d does not exist",
-		       bus_no);
+	if (!bus)
 		retval = CONTROLVM_RESP_ERROR_BUS_INVALID;
-	}
 	read_unlock(&bus_list_lock);
 	if (retval == CONTROLVM_RESP_SUCCESS) {
 		/* the msg is bound for virtpci; send
@@ -715,17 +647,14 @@ static int destroy_device(struct controlvm_message *msg, char *buf)
 			cmd.msgtype = GUEST_DEL_VNIC;
 			cmd.del_vnic.chanptr = dev->chanptr;
 		} else {
-			LOGERR("CONTROLVM_DEVICE_DESTROY Failed: unknown channelTypeGuid.\n");
 			return
 			    CONTROLVM_RESP_ERROR_CHANNEL_TYPE_UNKNOWN;
 		}
 		if (!virt_control_chan_func) {
-			LOGERR("CONTROLVM_DEVICE_DESTROY Failed: virtpci callback not registered.");
 			return
 			    CONTROLVM_RESP_ERROR_VIRTPCI_DRIVER_FAILURE;
 		}
 		if (!virt_control_chan_func(&cmd)) {
-			LOGERR("CONTROLVM_DEVICE_DESTROY Failed: virtpci GUEST_DEL_[VHBA||VNIC] returned error.");
 			return
 			    CONTROLVM_RESP_ERROR_VIRTPCI_DRIVER_CALLBACK_ERROR;
 		}
@@ -734,15 +663,11 @@ static int destroy_device(struct controlvm_message *msg, char *buf)
  * on which accesses the channel and you will get a "unable to handle
  * kernel paging request"
  */
-		if (dev->polling) {
-			LOGINF("calling uislib_disable_channel_interrupts");
+		if (dev->polling)
 			uislib_disable_channel_interrupts(bus_no, dev_no);
-		}
 		/* unmap the channel memory for the device. */
-		if (!msg->hdr.flags.test_message) {
-			LOGINF("destroy_device, doing iounmap");
+		if (!msg->hdr.flags.test_message)
 			uislib_iounmap(dev->chanptr);
-		}
 		kfree(dev);
 		bus->device[dev_no] = NULL;
 	}
@@ -780,10 +705,8 @@ static int delete_bus_glue(u32 bus_no)
 
 	init_msg_header(&msg, CONTROLVM_BUS_DESTROY, 0, 0);
 	msg.cmd.destroy_bus.bus_no = bus_no;
-	if (destroy_bus(&msg, NULL) != CONTROLVM_RESP_SUCCESS) {
-		LOGERR("destroy_bus failed. bus_no=0x%x\n", bus_no);
+	if (destroy_bus(&msg, NULL) != CONTROLVM_RESP_SUCCESS)
 		return 0;
-	}
 	return 1;
 }
 
@@ -794,11 +717,8 @@ static int delete_device_glue(u32 bus_no, u32 dev_no)
 	init_msg_header(&msg, CONTROLVM_DEVICE_DESTROY, 0, 0);
 	msg.cmd.destroy_device.bus_no = bus_no;
 	msg.cmd.destroy_device.dev_no = dev_no;
-	if (destroy_device(&msg, NULL) != CONTROLVM_RESP_SUCCESS) {
-		LOGERR("destroy_device failed. bus_no=0x%x dev_no=0x%x\n",
-		       bus_no, dev_no);
+	if (destroy_device(&msg, NULL) != CONTROLVM_RESP_SUCCESS)
 		return 0;
-	}
 	return 1;
 }
 
@@ -808,7 +728,6 @@ uislib_client_inject_add_bus(u32 bus_no, uuid_le inst_uuid,
 {
 	struct controlvm_message msg;
 
-	LOGINF("enter busNo=0x%x\n", bus_no);
 	/* step 0: init the chipset */
 	POSTCODE_LINUX_3(CHIPSET_INIT_ENTRY_PC, bus_no, POSTCODE_SEVERITY_INFO);
 
@@ -824,11 +743,8 @@ uislib_client_inject_add_bus(u32 bus_no, uuid_le inst_uuid,
 		*/
 		msg.cmd.init_chipset.bus_count = 23;
 		msg.cmd.init_chipset.switch_count = 0;
-		if (init_chipset(&msg, NULL) != CONTROLVM_RESP_SUCCESS) {
-			LOGERR("init_chipset failed.\n");
+		if (init_chipset(&msg, NULL) != CONTROLVM_RESP_SUCCESS)
 			return 0;
-		}
-		LOGINF("chipset initialized\n");
 		POSTCODE_LINUX_3(CHIPSET_INIT_EXIT_PC, bus_no,
 				 POSTCODE_SEVERITY_INFO);
 	}
@@ -842,7 +758,6 @@ uislib_client_inject_add_bus(u32 bus_no, uuid_le inst_uuid,
 	msg.cmd.create_bus.channel_addr = channel_addr;
 	msg.cmd.create_bus.channel_bytes = n_channel_bytes;
 	if (create_bus(&msg, NULL) != CONTROLVM_RESP_SUCCESS) {
-		LOGERR("create_bus failed.\n");
 		POSTCODE_LINUX_3(BUS_CREATE_FAILURE_PC, bus_no,
 				 POSTCODE_SEVERITY_ERR);
 		return 0;
@@ -871,11 +786,8 @@ uislib_client_inject_pause_vhba(u32 bus_no, u32 dev_no)
 	msg.cmd.device_change_state.dev_no = dev_no;
 	msg.cmd.device_change_state.state = segment_state_standby;
 	rc = pause_device(&msg);
-	if (rc != CONTROLVM_RESP_SUCCESS) {
-		LOGERR("VHBA pause_device failed. busNo=0x%x devNo=0x%x\n",
-		       bus_no, dev_no);
+	if (rc != CONTROLVM_RESP_SUCCESS)
 		return rc;
-	}
 	return 0;
 }
 EXPORT_SYMBOL_GPL(uislib_client_inject_pause_vhba);
@@ -891,11 +803,8 @@ uislib_client_inject_resume_vhba(u32 bus_no, u32 dev_no)
 	msg.cmd.device_change_state.dev_no = dev_no;
 	msg.cmd.device_change_state.state = segment_state_running;
 	rc = resume_device(&msg);
-	if (rc != CONTROLVM_RESP_SUCCESS) {
-		LOGERR("VHBA resume_device failed. busNo=0x%x devNo=0x%x\n",
-		       bus_no, dev_no);
+	if (rc != CONTROLVM_RESP_SUCCESS)
 		return rc;
-	}
 	return 0;
 }
 EXPORT_SYMBOL_GPL(uislib_client_inject_resume_vhba);
@@ -908,7 +817,6 @@ uislib_client_inject_add_vhba(u32 bus_no, u32 dev_no,
 {
 	struct controlvm_message msg;
 
-	LOGINF(" enter busNo=0x%x devNo=0x%x\n", bus_no, dev_no);
 	/* chipset init'ed with bus bus has been previously created -
 	* Verify it still exists step 2: create the VHBA device on the
 	* bus
@@ -932,8 +840,6 @@ uislib_client_inject_add_vhba(u32 bus_no, u32 dev_no,
 		       sizeof(struct irq_info));
 	msg.cmd.create_device.channel_addr = phys_chan_addr;
 	if (chan_bytes < MIN_IO_CHANNEL_SIZE) {
-		LOGERR("wrong channel size.chan_bytes = 0x%x IO_CHANNEL_SIZE= 0x%x\n",
-		       chan_bytes, (unsigned int)MIN_IO_CHANNEL_SIZE);
 		POSTCODE_LINUX_4(VHBA_CREATE_FAILURE_PC, chan_bytes,
 				 MIN_IO_CHANNEL_SIZE, POSTCODE_SEVERITY_ERR);
 		return 0;
@@ -941,7 +847,6 @@ uislib_client_inject_add_vhba(u32 bus_no, u32 dev_no,
 	msg.cmd.create_device.channel_bytes = chan_bytes;
 	msg.cmd.create_device.data_type_uuid = spar_vhba_channel_protocol_uuid;
 	if (create_device(&msg, NULL) != CONTROLVM_RESP_SUCCESS) {
-		LOGERR("VHBA create_device failed.\n");
 		POSTCODE_LINUX_4(VHBA_CREATE_FAILURE_PC, dev_no, bus_no,
 				 POSTCODE_SEVERITY_ERR);
 		return 0;
@@ -967,7 +872,6 @@ uislib_client_inject_add_vnic(u32 bus_no, u32 dev_no,
 {
 	struct controlvm_message msg;
 
-	LOGINF(" enter busNo=0x%x devNo=0x%x\n", bus_no, dev_no);
 	/* chipset init'ed with bus bus has been previously created -
 	* Verify it still exists step 2: create the VNIC device on the
 	* bus
@@ -991,8 +895,6 @@ uislib_client_inject_add_vnic(u32 bus_no, u32 dev_no,
 		       sizeof(struct irq_info));
 	msg.cmd.create_device.channel_addr = phys_chan_addr;
 	if (chan_bytes < MIN_IO_CHANNEL_SIZE) {
-		LOGERR("wrong channel size.chan_bytes = 0x%x IO_CHANNEL_SIZE= 0x%x\n",
-		       chan_bytes, (unsigned int)MIN_IO_CHANNEL_SIZE);
 		POSTCODE_LINUX_4(VNIC_CREATE_FAILURE_PC, chan_bytes,
 				 MIN_IO_CHANNEL_SIZE, POSTCODE_SEVERITY_ERR);
 		return 0;
@@ -1000,7 +902,6 @@ uislib_client_inject_add_vnic(u32 bus_no, u32 dev_no,
 	msg.cmd.create_device.channel_bytes = chan_bytes;
 	msg.cmd.create_device.data_type_uuid = spar_vnic_channel_protocol_uuid;
 	if (create_device(&msg, NULL) != CONTROLVM_RESP_SUCCESS) {
-		LOGERR("VNIC create_device failed.\n");
 		POSTCODE_LINUX_4(VNIC_CREATE_FAILURE_PC, dev_no, bus_no,
 				 POSTCODE_SEVERITY_ERR);
 		return 0;
@@ -1023,11 +924,8 @@ uislib_client_inject_pause_vnic(u32 bus_no, u32 dev_no)
 	msg.cmd.device_change_state.dev_no = dev_no;
 	msg.cmd.device_change_state.state = segment_state_standby;
 	rc = pause_device(&msg);
-	if (rc != CONTROLVM_RESP_SUCCESS) {
-		LOGERR("VNIC pause_device failed. busNo=0x%x devNo=0x%x\n",
-		       bus_no, dev_no);
+	if (rc != CONTROLVM_RESP_SUCCESS)
 		return -1;
-	}
 	return 0;
 }
 EXPORT_SYMBOL_GPL(uislib_client_inject_pause_vnic);
@@ -1043,11 +941,8 @@ uislib_client_inject_resume_vnic(u32 bus_no, u32 dev_no)
 	msg.cmd.device_change_state.dev_no = dev_no;
 	msg.cmd.device_change_state.state = segment_state_running;
 	rc = resume_device(&msg);
-	if (rc != CONTROLVM_RESP_SUCCESS) {
-		LOGERR("VNIC resume_device failed. busNo=0x%x devNo=0x%x\n",
-		       bus_no, dev_no);
+	if (rc != CONTROLVM_RESP_SUCCESS)
 		return -1;
-	}
 	return 0;
 }
 EXPORT_SYMBOL_GPL(uislib_client_inject_resume_vnic);
@@ -1069,11 +964,8 @@ uislib_cache_alloc(struct kmem_cache *cur_pool, char *fn, int ln)
 	*/
 	void *p = kmem_cache_alloc(cur_pool, GFP_ATOMIC | __GFP_NORETRY);
 
-	if (p == NULL) {
-		LOGERR("uislib_malloc failed to alloc uiscmdrsp @%s:%d",
-		       fn, ln);
+	if (!p)
 		return NULL;
-	}
 	return p;
 }
 EXPORT_SYMBOL_GPL(uislib_cache_alloc);
@@ -1081,10 +973,8 @@ EXPORT_SYMBOL_GPL(uislib_cache_alloc);
 void
 uislib_cache_free(struct kmem_cache *cur_pool, void *p, char *fn, int ln)
 {
-	if (p == NULL) {
-		LOGERR("uislib_free NULL pointer @%s:%d", fn, ln);
+	if (!p)
 		return;
-	}
 	kmem_cache_free(cur_pool, p);
 }
 EXPORT_SYMBOL_GPL(uislib_cache_free);
@@ -1162,20 +1052,16 @@ static ssize_t info_debugfs_read(struct file *file, char __user *buf,
 	int remaining_bytes = PROC_READ_BUFFER_SIZE;
 
 /* *start = buf; */
-	if (debug_buf == NULL) {
-		DBGINF("debug_buf == NULL; allocating buffer.\n.");
+	if (!debug_buf) {
 		debug_buf = vmalloc(PROC_READ_BUFFER_SIZE);
 
-		if (debug_buf == NULL) {
-			LOGERR("failed to allocate buffer to provide proc data.\n");
+		if (!debug_buf)
 			return -ENOMEM;
-		}
 	}
 
 	temp = debug_buf;
 
 	if ((*offset == 0) || (!debug_buf_valid)) {
-		DBGINF("calling info_debugfs_read_helper.\n");
 		/* if the read fails, then -1 will be returned */
 		total_bytes = info_debugfs_read_helper(&temp, &remaining_bytes);
 		debug_buf_valid = 1;
@@ -1196,17 +1082,9 @@ static struct device_info *find_dev(u32 bus_no, u32 dev_no)
 	for (bus = bus_list; bus; bus = bus->next) {
 		if (bus->bus_no == bus_no) {
 			/* make sure the device number is valid */
-			if (dev_no >= bus->device_count) {
-				LOGERR("%s bad bus_no, dev_no=%d,%d",
-				       __func__,
-				       (int)bus_no, (int)dev_no);
+			if (dev_no >= bus->device_count)
 				break;
-			}
 			dev = bus->device[dev_no];
-			if (!dev)
-				LOGERR("%s bad bus_no, dev_no=%d,%d",
-				       __func__,
-				       (int)bus_no, (int)dev_no);
 			break;
 		}
 	}
@@ -1253,7 +1131,6 @@ static int process_incoming(void *v)
 				wait_cycles = (cur_cycles - old_cycles);
 		}
 	}
-	LOGINF("wait_cycles=%llu", wait_cycles);
 	cycles_before_wait = wait_cycles;
 	idle_cycles = 0;
 	poll_dev_start = 0;
@@ -1281,7 +1158,7 @@ static int process_incoming(void *v)
 				* Reschedule work to occur as soon as
 				* possible. */
 				idle_cycles = 0;
-				if (new_tail == NULL) {
+				if (!new_tail) {
 					dev->first_busy_cnt++;
 					if (!
 					    (list_is_last
@@ -1294,10 +1171,10 @@ static int process_incoming(void *v)
 					}
 				}
 			}
-			if (incoming_ti.should_stop)
+			if (kthread_should_stop())
 				break;
 		}
-		if (new_tail != NULL) {
+		if (new_tail) {
 			tot_moved_to_tail_cnt++;
 			list_move_tail(new_tail, &poll_dev_chan);
 		}
@@ -1311,12 +1188,10 @@ static int process_incoming(void *v)
 		* - there is no input waiting on any of the channels
 		* - we have received a signal to stop this thread
 		*/
-		if (incoming_ti.should_stop)
+		if (kthread_should_stop())
 			break;
-		if (en_smart_wakeup == 0xFF) {
-			LOGINF("en_smart_wakeup set to 0xff, to force exiting process_incoming");
+		if (en_smart_wakeup == 0xFF)
 			break;
-		}
 		/* wait for POLLJIFFIES_NORMAL jiffies, or until
 		* someone wakes up poll_dev_wake_q,
 		* whichever comes first only do a wait when we have
@@ -1335,7 +1210,6 @@ static int process_incoming(void *v)
 			idle_cycles = idle_cycles + delta_cycles;
 		}
 	}
-	DBGINF("exiting.\n");
 	complete_and_exit(&incoming_ti.has_stopped, 0);
 }
 
@@ -1346,7 +1220,6 @@ initialize_incoming_thread(void)
 		return TRUE;
 	if (!uisthread_start(&incoming_ti,
 			     &process_incoming, NULL, "dev_incoming")) {
-		LOGERR("uisthread_start initialize_incoming_thread ****FAILED");
 		return FALSE;
 	}
 	incoming_started = TRUE;
@@ -1367,11 +1240,9 @@ uislib_enable_channel_interrupts(u32 bus_no, u32 dev_no,
 	struct device_info *dev;
 
 	dev = find_dev(bus_no, dev_no);
-	if (!dev) {
-		LOGERR("%s busNo=%d, devNo=%d", __func__, (int)(bus_no),
-		       (int)(dev_no));
+	if (!dev)
 		return;
-	}
+
 	down(&poll_dev_lock);
 	initialize_incoming_thread();
 	dev->interrupt = interrupt;
@@ -1392,11 +1263,8 @@ uislib_disable_channel_interrupts(u32 bus_no, u32 dev_no)
 	struct device_info *dev;
 
 	dev = find_dev(bus_no, dev_no);
-	if (!dev) {
-		LOGERR("%s busNo=%d, devNo=%d", __func__, (int)(bus_no),
-		       (int)(dev_no));
+	if (!dev)
 		return;
-	}
 	down(&poll_dev_lock);
 	list_del(&dev->list_polling_device_channels);
 	dev->polling = FALSE;
@@ -1446,27 +1314,6 @@ uislib_mod_init(void)
 	if (!unisys_spar_platform)
 		return -ENODEV;
 
-	LOGINF("MONITORAPIS");
-
-	LOGINF("sizeof(struct uiscmdrsp):%lu bytes\n",
-	       (ulong)sizeof(struct uiscmdrsp));
-	LOGINF("sizeof(struct phys_info):%lu\n",
-	       (ulong)sizeof(struct phys_info));
-	LOGINF("sizeof(uiscmdrsp_scsi):%lu\n",
-	       (ulong)sizeof(struct uiscmdrsp_scsi));
-	LOGINF("sizeof(uiscmdrsp_net):%lu\n",
-	       (ulong)sizeof(struct uiscmdrsp_net));
-	LOGINF("sizeof(CONTROLVM_MESSAGE):%lu bytes\n",
-	       (ulong)sizeof(struct controlvm_message));
-	LOGINF("sizeof(struct spar_controlvm_channel_protocol):%lu bytes\n",
-	       (ulong)sizeof(struct spar_controlvm_channel_protocol));
-	LOGINF("sizeof(CHANNEL_HEADER):%lu bytes\n",
-	       (ulong)sizeof(struct channel_header));
-	LOGINF("sizeof(struct spar_io_channel_protocol):%lu bytes\n",
-	       (ulong)sizeof(struct spar_io_channel_protocol));
-	LOGINF("SIZEOF_CMDRSP:%lu bytes\n", SIZEOF_CMDRSP);
-	LOGINF("SIZEOF_PROTOCOL:%lu bytes\n", SIZEOF_PROTOCOL);
-
 	/* initialize global pointers to NULL */
 	bus_list = NULL;
 	bus_list_count = 0;
@@ -1514,8 +1361,6 @@ uislib_mod_exit(void)
 	debugfs_remove(cycles_before_wait_debugfs_read);
 	debugfs_remove(platformnumber_debugfs_read);
 	debugfs_remove(dir_debugfs);
-
-	DBGINF("goodbye.\n");
 }
 
 module_init(uislib_mod_init);
diff --git a/drivers/staging/unisys/uislib/uisqueue.c b/drivers/staging/unisys/uislib/uisqueue.c
index 71bb7b608e9a..d46dd7428a30 100644
--- a/drivers/staging/unisys/uislib/uisqueue.c
+++ b/drivers/staging/unisys/uislib/uisqueue.c
@@ -295,12 +295,10 @@ uisqueue_put_cmdrsp_with_lock_client(struct uisqueue_info *queueinfo,
 	while (!do_locked_client_insert(queueinfo, whichqueue, cmdrsp,
 					(spinlock_t *)insertlock,
 					channel_id)) {
-		if (oktowait != OK_TO_WAIT) {
-			LOGERR("****FAILED visor_signal_insert failed; cannot wait; insert aborted\n");
+		if (oktowait != OK_TO_WAIT)
 			return 0;	/* failed to queue */
-		}
+
 		/* try again */
-		LOGERR("****FAILED visor_signal_insert failed; waiting to try again\n");
 		set_current_state(TASK_INTERRUPTIBLE);
 		schedule_timeout(msecs_to_jiffies(10));
 	}
diff --git a/drivers/staging/unisys/uislib/uisthread.c b/drivers/staging/unisys/uislib/uisthread.c
index 25adf1a7307c..d3c973b617ee 100644
--- a/drivers/staging/unisys/uislib/uisthread.c
+++ b/drivers/staging/unisys/uislib/uisthread.c
@@ -20,12 +20,9 @@
 #include <linux/signal.h>
 #include <linux/sched.h>
 #include <linux/kthread.h>
-#include "uniklog.h"
 #include "uisutils.h"
 #include "uisthread.h"
 
-#define KILL(a, b, c) kill_pid(find_vpid(a), b, c)
-
 /* this is shorter than using __FILE__ (full path name) in
  * debug/info/error messages
  */
@@ -41,17 +38,14 @@ int
 uisthread_start(struct uisthread_info *thrinfo,
 		int (*threadfn)(void *), void *thrcontext, char *name)
 {
-	thrinfo->should_stop = 0;
 	/* used to stop the thread */
 	init_completion(&thrinfo->has_stopped);
-	thrinfo->task = kthread_create(threadfn, thrcontext, name, NULL);
+	thrinfo->task = kthread_run(threadfn, thrcontext, name);
 	if (IS_ERR(thrinfo->task)) {
 		thrinfo->id = 0;
 		return 0;	/* failure */
 	}
 	thrinfo->id = thrinfo->task->pid;
-	wake_up_process(thrinfo->task);
-	LOGINF("started thread pid:%d\n", thrinfo->id);
 	return 1;
 }
 EXPORT_SYMBOL_GPL(uisthread_start);
@@ -59,27 +53,17 @@ EXPORT_SYMBOL_GPL(uisthread_start);
 void
 uisthread_stop(struct uisthread_info *thrinfo)
 {
-	int ret;
 	int stopped = 0;
 
 	if (thrinfo->id == 0)
 		return;		/* thread not running */
 
-	LOGINF("uisthread_stop stopping id:%d\n", thrinfo->id);
-	thrinfo->should_stop = 1;
-	ret = KILL(thrinfo->id, SIGHUP, 1);
-	if (ret) {
-		LOGERR("unable to signal thread %d\n", ret);
-	} else {
-		/* give up if the thread has NOT died in 1 minute */
-		if (wait_for_completion_timeout(&thrinfo->has_stopped, 60 * HZ))
-			stopped = 1;
-		else
-			LOGERR("timed out trying to signal thread\n");
-	}
-	if (stopped) {
-		LOGINF("uisthread_stop stopped id:%d\n", thrinfo->id);
+	kthread_stop(thrinfo->task);
+	/* give up if the thread has NOT died in 1 minute */
+	if (wait_for_completion_timeout(&thrinfo->has_stopped, 60 * HZ))
+		stopped = 1;
+
+	if (stopped)
 		thrinfo->id = 0;
-	}
 }
 EXPORT_SYMBOL_GPL(uisthread_stop);
diff --git a/drivers/staging/unisys/uislib/uisutils.c b/drivers/staging/unisys/uislib/uisutils.c
index 31318d246252..26ab76526813 100644
--- a/drivers/staging/unisys/uislib/uisutils.c
+++ b/drivers/staging/unisys/uislib/uisutils.c
@@ -21,7 +21,6 @@
 #include <linux/uuid.h>
 #include <linux/spinlock.h>
 #include <linux/list.h>
-#include "uniklog.h"
 #include "uisutils.h"
 #include "version.h"
 #include "vbushelper.h"
@@ -53,7 +52,6 @@ uisutil_add_proc_line_ex(int *total, char **buffer, int *buffer_remaining,
 	va_list args;
 	int len;
 
-	DBGINF("buffer = 0x%p : *buffer = 0x%p.\n", buffer, *buffer);
 	va_start(args, format);
 	len = vsnprintf(*buffer, *buffer_remaining, format, args);
 	va_end(args);
@@ -61,7 +59,6 @@ uisutil_add_proc_line_ex(int *total, char **buffer, int *buffer_remaining,
 		*buffer += *buffer_remaining;
 		*total += *buffer_remaining;
 		*buffer_remaining = 0;
-		LOGERR("bytes remaining is too small!\n");
 		return -1;
 	}
 	*buffer_remaining -= len;
@@ -75,8 +72,6 @@ int
 uisctrl_register_req_handler(int type, void *fptr,
 			     struct ultra_vbus_deviceinfo *chipset_driver_info)
 {
-	LOGINF("type = %d, fptr = 0x%p.\n", type, fptr);
-
 	switch (type) {
 	case 2:
 		if (fptr) {
@@ -91,7 +86,6 @@ uisctrl_register_req_handler(int type, void *fptr,
 		break;
 
 	default:
-		LOGERR("invalid type %d.\n", type);
 		return 0;
 	}
 	if (chipset_driver_info)
@@ -102,71 +96,6 @@ uisctrl_register_req_handler(int type, void *fptr,
 }
 EXPORT_SYMBOL_GPL(uisctrl_register_req_handler);
 
-int
-uisctrl_register_req_handler_ex(uuid_le switch_uuid,
-			const char *switch_type_name,
-			int (*controlfunc)(struct io_msgs *),
-			unsigned long min_channel_bytes,
-			int (*server_channel_ok)(unsigned long channel_bytes),
-			int (*server_channel_init)(void *x,
-						unsigned char *client_str,
-						u32 client_str_len, u64 bytes),
-			struct ultra_vbus_deviceinfo *chipset_driver_info)
-{
-	struct req_handler_info *req_handler;
-
-	LOGINF("type=%pUL, controlfunc=0x%p.\n",
-	       &switch_uuid, controlfunc);
-	if (!controlfunc) {
-		LOGERR("%pUL: controlfunc must be supplied\n", &switch_uuid);
-		return 0;
-	}
-	if (!server_channel_ok) {
-		LOGERR("%pUL: Server_Channel_Ok must be supplied\n",
-				&switch_uuid);
-		return 0;
-	}
-	if (!server_channel_init) {
-		LOGERR("%pUL: Server_Channel_Init must be supplied\n",
-				&switch_uuid);
-		return 0;
-	}
-	req_handler = req_handler_add(switch_uuid,
-				      switch_type_name,
-				      controlfunc,
-				      min_channel_bytes,
-				      server_channel_ok, server_channel_init);
-	if (!req_handler) {
-		LOGERR("failed to add %pUL to server list\n", &switch_uuid);
-		return 0;
-	}
-
-	atomic_inc(&uisutils_registered_services);
-	if (chipset_driver_info) {
-		bus_device_info_init(chipset_driver_info, "chipset",
-				     "uislib", VERSION, NULL);
-		return 1;
-	}
-
-	LOGERR("failed to register type %pUL.\n", &switch_uuid);
-	return 0;
-}
-EXPORT_SYMBOL_GPL(uisctrl_register_req_handler_ex);
-
-int
-uisctrl_unregister_req_handler_ex(uuid_le switch_uuid)
-{
-	LOGINF("type=%pUL.\n", &switch_uuid);
-	if (req_handler_del(switch_uuid) < 0) {
-		LOGERR("failed to remove %pUL from server list\n",
-		       &switch_uuid);
-		return 0;
-	}
-	atomic_dec(&uisutils_registered_services);
-	return 1;
-}
-EXPORT_SYMBOL_GPL(uisctrl_unregister_req_handler_ex);
-
 /*
  * unsigned int uisutil_copy_fragsinfo_from_skb(unsigned char *calling_ctx,
  *					     void *skb_in,
@@ -185,117 +114,11 @@ EXPORT_SYMBOL_GPL(uisctrl_unregister_req_handler_ex);
  *					    return value indicates number of
  *					    entries filled in frags
  */
-unsigned int
-uisutil_copy_fragsinfo_from_skb(unsigned char *calling_ctx, void *skb_in,
-				unsigned int firstfraglen,
-				unsigned int frags_max,
-				struct phys_info frags[])
-{
-	unsigned int count = 0, ii, size, offset = 0, numfrags;
-	struct sk_buff *skb = skb_in;
-
-	numfrags = skb_shinfo(skb)->nr_frags;
-
-	while (firstfraglen) {
-		if (count == frags_max) {
-			LOGERR("%s frags array too small: max:%d count:%d\n",
-			       calling_ctx, frags_max, count);
-			return -1;	/* failure */
-		}
-		frags[count].pi_pfn =
-		    page_to_pfn(virt_to_page(skb->data + offset));
-		frags[count].pi_off =
-		    (unsigned long)(skb->data + offset) & PI_PAGE_MASK;
-		size =
-		    min(firstfraglen,
-			(unsigned int)(PI_PAGE_SIZE - frags[count].pi_off));
-		/* can take smallest of firstfraglen(what's left) OR
-		* bytes left in the page
-		*/
-		frags[count].pi_len = size;
-		firstfraglen -= size;
-		offset += size;
-		count++;
-	}
-	if (!numfrags)
-		goto dolist;
-
-	if ((count + numfrags) > frags_max) {
-		LOGERR("**** FAILED %s frags array too small: max:%d count+nr_frags:%d\n",
-		       calling_ctx, frags_max, count + numfrags);
-		return -1;	/* failure */
-	}
-
-	for (ii = 0; ii < numfrags; ii++) {
-		count = add_physinfo_entries(page_to_pfn(
-				skb_frag_page(&skb_shinfo(skb)->frags[ii])),
-					skb_shinfo(skb)->frags[ii].
-					page_offset,
-					skb_shinfo(skb)->frags[ii].
-					size, count, frags_max,
-					frags);
-		if (count == 0) {
-			LOGERR("**** FAILED to add physinfo entries\n");
-			return -1;	/* failure */
-		}
-	}
-
-dolist: if (skb_shinfo(skb)->frag_list) {
-		struct sk_buff *skbinlist;
-		int c;
-
-		for (skbinlist = skb_shinfo(skb)->frag_list; skbinlist;
-		     skbinlist = skbinlist->next) {
-			c = uisutil_copy_fragsinfo_from_skb("recursive",
-				skbinlist,
-				skbinlist->len - skbinlist->data_len,
-				frags_max - count,
-				&frags[count]);
-			if (c == -1) {
-				LOGERR("**** FAILED recursive call failed\n");
-				return -1;
-			}
-			count += c;
-		}
-	}
-	return count;
-}
-EXPORT_SYMBOL_GPL(uisutil_copy_fragsinfo_from_skb);
 
 static LIST_HEAD(req_handler_info_list); /* list of struct req_handler_info */
 static DEFINE_SPINLOCK(req_handler_info_list_lock);
 
 struct req_handler_info *
-req_handler_add(uuid_le switch_uuid,
-	      const char *switch_type_name,
-	      int (*controlfunc)(struct io_msgs *),
-	      unsigned long min_channel_bytes,
-	      int (*server_channel_ok)(unsigned long channel_bytes),
-	      int (*server_channel_init)
-	       (void *x, unsigned char *clientstr, u32 clientstr_len,
-		u64 bytes))
-{
-	struct req_handler_info *rc = NULL;
-
-	rc = kzalloc(sizeof(*rc), GFP_ATOMIC);
-	if (!rc)
-		return NULL;
-	rc->switch_uuid = switch_uuid;
-	rc->controlfunc = controlfunc;
-	rc->min_channel_bytes = min_channel_bytes;
-	rc->server_channel_ok = server_channel_ok;
-	rc->server_channel_init = server_channel_init;
-	if (switch_type_name)
-		strncpy(rc->switch_type_name, switch_type_name,
-			sizeof(rc->switch_type_name) - 1);
-	spin_lock(&req_handler_info_list_lock);
-	list_add_tail(&rc->list_link, &req_handler_info_list);
-	spin_unlock(&req_handler_info_list_lock);
-
-	return rc;
-}
-
-struct req_handler_info *
 req_handler_find(uuid_le switch_uuid)
 {
 	struct list_head *lelt, *tmp;
@@ -312,23 +135,3 @@ req_handler_find(uuid_le switch_uuid)
 	spin_unlock(&req_handler_info_list_lock);
 	return NULL;
 }
-
-int
-req_handler_del(uuid_le switch_uuid)
-{
-	struct list_head *lelt, *tmp;
-	struct req_handler_info *entry = NULL;
-	int rc = -1;
-
-	spin_lock(&req_handler_info_list_lock);
-	list_for_each_safe(lelt, tmp, &req_handler_info_list) {
-		entry = list_entry(lelt, struct req_handler_info, list_link);
-		if (uuid_le_cmp(entry->switch_uuid, switch_uuid) == 0) {
-			list_del(lelt);
-			kfree(entry);
-			rc++;
-		}
-	}
-	spin_unlock(&req_handler_info_list_lock);
-	return rc;
-}
diff --git a/drivers/staging/unisys/virthba/Kconfig b/drivers/staging/unisys/virthba/Kconfig
index 9af98fc7acbc..dfadfc49114a 100644
--- a/drivers/staging/unisys/virthba/Kconfig
+++ b/drivers/staging/unisys/virthba/Kconfig
@@ -4,7 +4,10 @@
 
 config UNISYS_VIRTHBA
 	tristate "Unisys virthba driver"
-	depends on UNISYSSPAR && UNISYS_VISORCHIPSET && UNISYS_UISLIB && UNISYS_VIRTPCI && SCSI
+	depends on SCSI
+	select UNISYS_VISORCHIPSET
+	select UNISYS_UISLIB
+	select UNISYS_VIRTPCI
 	---help---
 	If you say Y here, you will enable the Unisys virthba driver.
 
diff --git a/drivers/staging/unisys/virthba/Makefile b/drivers/staging/unisys/virthba/Makefile
index ba55ae12488e..a4e403739183 100644
--- a/drivers/staging/unisys/virthba/Makefile
+++ b/drivers/staging/unisys/virthba/Makefile
@@ -6,7 +6,6 @@ obj-$(CONFIG_UNISYS_VIRTHBA)	+= virthba.o
 
 ccflags-y += -Idrivers/staging/unisys/include
 ccflags-y += -Idrivers/staging/unisys/uislib
-ccflags-y += -Idrivers/staging/unisys/timskmod
 ccflags-y += -Idrivers/staging/unisys/visorchipset
 ccflags-y += -Idrivers/staging/unisys/virtpci
 ccflags-y += -Idrivers/staging/unisys/common-spar/include
diff --git a/drivers/staging/unisys/virthba/virthba.c b/drivers/staging/unisys/virthba/virthba.c
index e6ecea560495..d9001cca0f73 100644
--- a/drivers/staging/unisys/virthba/virthba.c
+++ b/drivers/staging/unisys/virthba/virthba.c
@@ -26,14 +26,11 @@
  * which start with an 8 digit sequence number, a colon, and then
  * letters after that */
 
-#undef DBGINF
-
 #include <linux/kernel.h>
 #ifdef CONFIG_MODVERSIONS
 #include <config/modversions.h>
 #endif
 
-#include "uniklog.h"
 #include "diagnostics/appos_subsystems.h"
 #include "uisutils.h"
 #include "uisqueue.h"
@@ -261,11 +258,9 @@ add_scsipending_entry(struct virthba_info *vhbainfo, char cmdtype, void *new)
 
 	spin_lock_irqsave(&vhbainfo->privlock, flags);
 	insert_location = vhbainfo->nextinsert;
-	while (vhbainfo->pending[insert_location].sent != NULL) {
+	while (vhbainfo->pending[insert_location].sent) {
 		insert_location = (insert_location + 1) % MAX_PENDING_REQUESTS;
 		if (insert_location == (int)vhbainfo->nextinsert) {
-			LOGERR("Queue should be full. insert_location<<%d>>  Unable to find open slot for pending commands.\n",
-			     insert_location);
 			spin_unlock_irqrestore(&vhbainfo->privlock, flags);
 			return -1;
 		}
@@ -286,7 +281,6 @@ add_scsipending_entry_with_wait(struct virthba_info *vhbainfo, char cmdtype,
 	int insert_location = add_scsipending_entry(vhbainfo, cmdtype, new);
 
 	while (insert_location == -1) {
-		LOGERR("Failed to find empty queue slot.  Waiting to try again\n");
 		set_current_state(TASK_INTERRUPTIBLE);
 		schedule_timeout(msecs_to_jiffies(10));
 		insert_location = add_scsipending_entry(vhbainfo, cmdtype, new);
@@ -301,16 +295,8 @@ del_scsipending_entry(struct virthba_info *vhbainfo, uintptr_t del)
 	unsigned long flags;
 	void *sent = NULL;
 
-	if (del >= MAX_PENDING_REQUESTS) {
-		LOGERR("Invalid queue position <<%lu>> given to delete. MAX_PENDING_REQUESTS <<%d>>\n",
-		     (unsigned long)del, MAX_PENDING_REQUESTS);
-	} else {
+	if (del < MAX_PENDING_REQUESTS) {
 		spin_lock_irqsave(&vhbainfo->privlock, flags);
-
-		if (vhbainfo->pending[del].sent == NULL)
-			LOGERR("Deleting already cleared queue entry at <<%lu>>.\n",
-			     (unsigned long)del);
-
 		sent = vhbainfo->pending[del].sent;
 
 		vhbainfo->pending[del].cmdtype = 0;
@@ -357,13 +343,7 @@ send_disk_add_remove(struct diskaddremove *dar)
 		error =
 		    scsi_add_device(dar->shost, dar->channel, dar->id,
 				    dar->lun);
-		if (error)
-			LOGERR("Failed scsi_add_device: host_no=%d[chan=%d:id=%d:lun=%d]\n",
-			     dar->shost->host_no, dar->channel, dar->id,
-			     dar->lun);
-	} else
-		LOGERR("Failed scsi_device_lookup:[chan=%d:id=%d:lun=%d]\n",
-		       dar->channel, dar->id, dar->lun);
+	}
 	kfree(dar);
 }
 
@@ -408,10 +388,6 @@ process_disk_notify(struct Scsi_Host *shost, struct uiscmdrsp *cmdrsp)
 		dar->id = cmdrsp->disknotify.id;
 		dar->lun = cmdrsp->disknotify.lun;
 		QUEUE_DISKADDREMOVE(dar);
-	} else {
-		LOGERR("kmalloc failed for dar. host_no=%d[chan=%d:id=%d:lun=%d]\n",
-		     shost->host_no, cmdrsp->disknotify.channel,
-		     cmdrsp->disknotify.id, cmdrsp->disknotify.lun);
 	}
 }
 
@@ -427,7 +403,7 @@ virthba_isr(int irq, void *dev_id)
 	u64 mask;
 	unsigned long long rc1;
 
-	if (virthbainfo == NULL)
+	if (!virthbainfo)
 		return IRQ_NONE;
 	virthbainfo->interrupts_rcvd++;
 	channel_header = virthbainfo->chinfo.queueinfo->chan;
@@ -467,13 +443,6 @@ virthba_probe(struct virtpci_dev *virtpcidev, const struct pci_device_id *id)
 	struct signal_queue_header __iomem *pqhdr;
 	u64 mask;
 
-	LOGVER("entering virthba_probe...\n");
-	LOGVER("virtpcidev bus_no<<%d>>devNo<<%d>>", virtpcidev->bus_no,
-	       virtpcidev->device_no);
-
-	LOGINF("entering virthba_probe...\n");
-	LOGINF("virtpcidev bus_no<<%d>>devNo<<%d>>", virtpcidev->bus_no,
-	       virtpcidev->device_no);
 	POSTCODE_LINUX_2(VHBA_PROBE_ENTRY_PC, POSTCODE_SEVERITY_INFO);
 	/* call scsi_host_alloc to register a scsi host adapter
 	 * instance - this virthba that has just been created is an
@@ -482,7 +451,6 @@ virthba_probe(struct virtpci_dev *virtpcidev, const struct pci_device_id *id)
 	 * initialization.  The host is not published to the scsi
 	 * midlayer until scsi_add_host is called.
 	 */
-	DBGINF("calling scsi_host_alloc.\n");
 
 	/* arg 2 passed in length of extra space we want allocated
 	 * with scsi_host struct for our own use scsi_host_alloc
@@ -490,12 +458,9 @@ virthba_probe(struct virtpci_dev *virtpcidev, const struct pci_device_id *id)
 	 */
 	scsihost = scsi_host_alloc(&virthba_driver_template,
 				   sizeof(struct virthba_info));
-	if (scsihost == NULL)
+	if (!scsihost)
 		return -ENODEV;
 
-	DBGINF("scsihost: 0x%p, scsihost->this_id: %d, host_no: %d.\n",
-	       scsihost, scsihost->this_id, scsihost->host_no);
-
 	scsihost->this_id = UIS_MAGIC_VHBA;
 	/* linux treats max-channel differently than max-id & max-lun.
 	 * In the latter cases, those two values result in 0 to max-1
@@ -503,12 +468,6 @@ virthba_probe(struct virtpci_dev *virtpcidev, const struct pci_device_id *id)
 	 * scan is 0 to max (inclusive); so we will subtract one from
 	 * the max-channel value.
 	 */
-	LOGINF("virtpcidev->scsi.max.max_channel=%u, max_id=%u, max_lun=%u, cmd_per_lun=%u, max_io_size=%u\n",
-	     (unsigned)virtpcidev->scsi.max.max_channel - 1,
-	     (unsigned)virtpcidev->scsi.max.max_id,
-	     (unsigned)virtpcidev->scsi.max.max_lun,
-	     (unsigned)virtpcidev->scsi.max.cmd_per_lun,
-	     (unsigned)virtpcidev->scsi.max.max_io_size);
 	scsihost->max_channel = (unsigned)virtpcidev->scsi.max.max_channel;
 	scsihost->max_id = (unsigned)virtpcidev->scsi.max.max_id;
 	scsihost->max_lun = (unsigned)virtpcidev->scsi.max.max_lun;
@@ -519,15 +478,6 @@ virthba_probe(struct virtpci_dev *virtpcidev, const struct pci_device_id *id)
 	    (unsigned short)(virtpcidev->scsi.max.max_io_size / PAGE_SIZE);
 	if (scsihost->sg_tablesize > MAX_PHYS_INFO)
 		scsihost->sg_tablesize = MAX_PHYS_INFO;
-	LOGINF("scsihost->max_channel=%u, max_id=%u, max_lun=%llu, cmd_per_lun=%u, max_sectors=%hu, sg_tablesize=%hu\n",
-	     scsihost->max_channel, scsihost->max_id, scsihost->max_lun,
-	     scsihost->cmd_per_lun, scsihost->max_sectors,
-	     scsihost->sg_tablesize);
-	LOGINF("scsihost->can_queue=%u, scsihost->cmd_per_lun=%u, max_sectors=%hu, sg_tablesize=%hu\n",
-	     scsihost->can_queue, scsihost->cmd_per_lun, scsihost->max_sectors,
-	     scsihost->sg_tablesize);
-
-	DBGINF("calling scsi_add_host\n");
 
 	/* this creates "host%d" in sysfs.  If 2nd argument is NULL,
 	 * then this generic /sys/devices/platform/host?  device is
@@ -538,7 +488,6 @@ virthba_probe(struct virtpci_dev *virtpcidev, const struct pci_device_id *id)
 	 */
 	error = scsi_add_host(scsihost, &virtpcidev->generic_dev);
 	if (error) {
-		LOGERR("scsi_add_host ****FAILED 0x%x  TBD - RECOVER\n", error);
 		POSTCODE_LINUX_2(VHBA_PROBE_FAILURE_PC, POSTCODE_SEVERITY_ERR);
 		/* decr refcount on scsihost which was incremented by
 		 * scsi_add_host so the scsi_host gets deleted
@@ -550,7 +499,7 @@ virthba_probe(struct virtpci_dev *virtpcidev, const struct pci_device_id *id)
 	virthbainfo = (struct virthba_info *)scsihost->hostdata;
 	memset(virthbainfo, 0, sizeof(struct virthba_info));
 	for (i = 0; i < VIRTHBASOPENMAX; i++) {
-		if (virthbas_open[i].virthbainfo == NULL) {
+		if (!virthbas_open[i].virthbainfo) {
 			virthbas_open[i].virthbainfo = virthbainfo;
 			break;
 		}
@@ -560,9 +509,6 @@ virthba_probe(struct virtpci_dev *virtpcidev, const struct pci_device_id *id)
 	virthbainfo->virtpcidev = virtpcidev;
 	spin_lock_init(&virthbainfo->chinfo.insertlock);
 
-	DBGINF("generic_dev: 0x%p, queueinfo: 0x%p.\n",
-	       &virtpcidev->generic_dev, &virtpcidev->queueinfo);
-
 	init_waitqueue_head(&virthbainfo->rsp_queue);
 	spin_lock_init(&virthbainfo->privlock);
 	memset(&virthbainfo->pending, 0, sizeof(virthbainfo->pending));
@@ -584,8 +530,6 @@ virthba_probe(struct virtpci_dev *virtpcidev, const struct pci_device_id *id)
 	       ULTRA_IO_CHANNEL_IS_POLLING,
 	       &virthbainfo->chinfo.queueinfo->chan->features);
 	/* start thread that will receive scsicmnd responses */
-	DBGINF("starting rsp thread -- queueinfo: 0x%p, threadinfo: 0x%p.\n",
-	       virthbainfo->chinfo.queueinfo, &virthbainfo->chinfo.threadinfo);
 
 	channel_header = virthbainfo->chinfo.queueinfo->chan;
 	pqhdr = (struct signal_queue_header __iomem *)
@@ -596,7 +540,6 @@ virthba_probe(struct virtpci_dev *virtpcidev, const struct pci_device_id *id)
 	if (!uisthread_start(&virthbainfo->chinfo.threadinfo,
 			     process_incoming_rsps,
 			     virthbainfo, "vhba_incoming")) {
-		LOGERR("uisthread_start rsp ****FAILED\n");
 		/* decr refcount on scsihost which was incremented by
 		 * scsi_add_host so the scsi_host gets deleted
 		 */
@@ -604,42 +547,26 @@ virthba_probe(struct virtpci_dev *virtpcidev, const struct pci_device_id *id)
 		scsi_host_put(scsihost);
 		return -ENODEV;
 	}
-	LOGINF("sendInterruptHandle=0x%16llX",
-	       virthbainfo->intr.send_irq_handle);
-	LOGINF("recvInterruptHandle=0x%16llX",
-	       virthbainfo->intr.recv_irq_handle);
-	LOGINF("recvInterruptVector=0x%8X",
-	       virthbainfo->intr.recv_irq_vector);
-	LOGINF("recvInterruptShared=0x%2X",
-	       virthbainfo->intr.recv_irq_shared);
-	LOGINF("scsihost.hostt->name=%s", scsihost->hostt->name);
 	virthbainfo->interrupt_vector =
 	    virthbainfo->intr.recv_irq_handle & INTERRUPT_VECTOR_MASK;
 	rsp = request_irq(virthbainfo->interrupt_vector, handler, IRQF_SHARED,
 			  scsihost->hostt->name, virthbainfo);
 	if (rsp != 0) {
-		LOGERR("request_irq(%d) uislib_virthba_ISR request failed with rsp=%d\n",
-		       virthbainfo->interrupt_vector, rsp);
 		virthbainfo->interrupt_vector = -1;
 		POSTCODE_LINUX_2(VHBA_PROBE_FAILURE_PC, POSTCODE_SEVERITY_ERR);
 	} else {
-		u64 __iomem *Features_addr =
+		u64 __iomem *features_addr =
 		    &virthbainfo->chinfo.queueinfo->chan->features;
-		LOGERR("request_irq(%d) uislib_virthba_ISR request succeeded\n",
-		       virthbainfo->interrupt_vector);
 		mask = ~(ULTRA_IO_CHANNEL_IS_POLLING |
 			 ULTRA_IO_DRIVER_DISABLES_INTS);
-		uisqueue_interlocked_and(Features_addr, mask);
+		uisqueue_interlocked_and(features_addr, mask);
 		mask = ULTRA_IO_DRIVER_ENABLES_INTS;
-		uisqueue_interlocked_or(Features_addr, mask);
+		uisqueue_interlocked_or(features_addr, mask);
 		rsltq_wait_usecs = 4000000;
 	}
 
-	DBGINF("calling scsi_scan_host.\n");
 	scsi_scan_host(scsihost);
-	DBGINF("return from scsi_scan_host.\n");
 
-	LOGINF("virthba added scsihost:0x%p\n", scsihost);
 	POSTCODE_LINUX_2(VHBA_PROBE_EXIT_PC, POSTCODE_SEVERITY_INFO);
 	return 0;
 }
@@ -651,28 +578,18 @@ virthba_remove(struct virtpci_dev *virtpcidev)
 	struct Scsi_Host *scsihost =
 	    (struct Scsi_Host *)virtpcidev->scsi.scsihost;
 
-	LOGINF("virtpcidev bus_no<<%d>>devNo<<%d>>", virtpcidev->bus_no,
-	       virtpcidev->device_no);
 	virthbainfo = (struct virthba_info *)scsihost->hostdata;
 	if (virthbainfo->interrupt_vector != -1)
 		free_irq(virthbainfo->interrupt_vector, virthbainfo);
-	LOGINF("Removing virtpcidev: 0x%p, virthbainfo: 0x%p\n", virtpcidev,
-	       virthbainfo);
 
-	DBGINF("removing scsihost: 0x%p, scsihost->this_id: %d\n", scsihost,
-	       scsihost->this_id);
 	scsi_remove_host(scsihost);
 
-	DBGINF("stopping thread.\n");
 	uisthread_stop(&virthbainfo->chinfo.threadinfo);
 
-	DBGINF("calling scsi_host_put\n");
-
 	/* decr refcount on scsihost which was incremented by
 	 * scsi_add_host so the scsi_host gets deleted
 	 */
 	scsi_host_put(scsihost);
-	LOGINF("virthba removed scsi_host.\n");
 }
 
 static int
@@ -686,19 +603,12 @@ forward_vdiskmgmt_command(enum vdisk_mgmt_types vdiskcmdtype,
 	int notifyresult = 0xffff;
 	wait_queue_head_t notifyevent;
 
-	LOGINF("vDiskMgmt:%d %d:%d:%d\n", vdiskcmdtype,
-	       vdest->channel, vdest->id, vdest->lun);
-
-	if (virthbainfo->serverdown || virthbainfo->serverchangingstate) {
-		DBGINF("Server is down/changing state. Returning Failure.\n");
+	if (virthbainfo->serverdown || virthbainfo->serverchangingstate)
 		return FAILED;
-	}
 
 	cmdrsp = kzalloc(SIZEOF_CMDRSP, GFP_ATOMIC);
-	if (cmdrsp == NULL) {
-		LOGERR("kmalloc of cmdrsp failed.\n");
-		return FAILED;	/* reject */
-	}
+	if (!cmdrsp)
+		return FAILED;  /* reject */
 
 	init_waitqueue_head(&notifyevent);
 
@@ -727,10 +637,7 @@ forward_vdiskmgmt_command(enum vdisk_mgmt_types vdiskcmdtype,
 					     &virthbainfo->chinfo.insertlock,
 					     DONT_ISSUE_INTERRUPT, (u64)NULL,
 					     OK_TO_WAIT, "vhba");
-	LOGINF("VdiskMgmt waiting on event notifyevent=0x%p\n",
-	       cmdrsp->scsitaskmgmt.notify);
 	wait_event(notifyevent, notifyresult != 0xffff);
-	LOGINF("VdiskMgmt complete; result:%d\n", cmdrsp->vdiskmgmt.result);
 	kfree(cmdrsp);
 	return SUCCESS;
 }
@@ -749,19 +656,12 @@ forward_taskmgmt_command(enum task_mgmt_types tasktype,
 	int notifyresult = 0xffff;
 	wait_queue_head_t notifyevent;
 
-	LOGINF("TaskMgmt:%d %d:%d:%llu\n", tasktype,
-	       scsidev->channel, scsidev->id, scsidev->lun);
-
-	if (virthbainfo->serverdown || virthbainfo->serverchangingstate) {
-		DBGINF("Server is down/changing state. Returning Failure.\n");
+	if (virthbainfo->serverdown || virthbainfo->serverchangingstate)
 		return FAILED;
-	}
 
 	cmdrsp = kzalloc(SIZEOF_CMDRSP, GFP_ATOMIC);
-	if (cmdrsp == NULL) {
-		LOGERR("kmalloc of cmdrsp failed.\n");
+	if (!cmdrsp)
 		return FAILED;	/* reject */
-	}
 
 	init_waitqueue_head(&notifyevent);
 
@@ -789,10 +689,7 @@ forward_taskmgmt_command(enum task_mgmt_types tasktype,
 					     &virthbainfo->chinfo.insertlock,
 					     DONT_ISSUE_INTERRUPT, (u64)NULL,
 					     OK_TO_WAIT, "vhba");
-	LOGINF("TaskMgmt waiting on event notifyevent=0x%p\n",
-	       cmdrsp->scsitaskmgmt.notify);
 	wait_event(notifyevent, notifyresult != 0xffff);
-	LOGINF("TaskMgmt complete; result:%d\n", cmdrsp->scsitaskmgmt.result);
 	kfree(cmdrsp);
 	return SUCCESS;
 }
@@ -882,7 +779,6 @@ static int
 virthba_host_reset_handler(struct scsi_cmnd *scsicmd)
 {
 	/* issue TASK_MGMT_TARGET_RESET for each target on each bus for host */
-	LOGERR("virthba_host_reset_handler Not yet implemented\n");
 	return SUCCESS;
 }
 
@@ -899,7 +795,6 @@ virthba_get_info(struct Scsi_Host *shp)
 static int
 virthba_ioctl(struct scsi_device *dev, int cmd, void __user *arg)
 {
-	DBGINF("In virthba_ioctl: ioctl: cmd=0x%x\n", cmd);
 	return -EINVAL;
 }
 
@@ -923,16 +818,11 @@ virthba_queue_command_lck(struct scsi_cmnd *scsicmd,
 	struct scatterlist *sgl = NULL;
 	int sg_failed = 0;
 
-	if (virthbainfo->serverdown || virthbainfo->serverchangingstate) {
-		DBGINF("Server is down/changing state. Returning SCSI_MLQUEUE_DEVICE_BUSY.\n");
+	if (virthbainfo->serverdown || virthbainfo->serverchangingstate)
 		return SCSI_MLQUEUE_DEVICE_BUSY;
-	}
-
 	cmdrsp = kzalloc(SIZEOF_CMDRSP, GFP_ATOMIC);
-	if (cmdrsp == NULL) {
-		LOGERR("kmalloc of cmdrsp failed.\n");
+	if (!cmdrsp)
 		return 1;	/* reject the command */
-	}
 
 	/* now saving everything we need from scsi_cmd into cmdrsp
 	 * before we queue cmdrsp set type to command - as opposed to
@@ -947,7 +837,6 @@ virthba_queue_command_lck(struct scsi_cmnd *scsicmd,
 	if (insert_location != -1) {
 		cmdrsp->scsi.scsicmd = (void *)(uintptr_t)insert_location;
 	} else {
-		LOGERR("Queue is full. Returning busy.\n");
 		kfree(cmdrsp);
 		return SCSI_MLQUEUE_DEVICE_BUSY;
 	}
@@ -968,8 +857,6 @@ virthba_queue_command_lck(struct scsi_cmnd *scsicmd,
 		max_buff_len = cmdrsp->scsi.bufflen;
 
 	if (scsi_sg_count(scsicmd) > MAX_PHYS_INFO) {
-		LOGERR("scsicmd use_sg:%d greater than MAX:%d\n",
-		       scsi_sg_count(scsicmd), MAX_PHYS_INFO);
 		del_scsipending_entry(virthbainfo, (uintptr_t)insert_location);
 		kfree(cmdrsp);
 		return 1;	/* reject the command */
@@ -982,11 +869,8 @@ virthba_queue_command_lck(struct scsi_cmnd *scsicmd,
 	/* convert buffer to phys information */
 	if (scsi_sg_count(scsicmd) == 0) {
 		if (scsi_bufflen(scsicmd) > 0) {
-			LOGERR("**** FAILED No scatter list for bufflen > 0\n");
 			BUG_ON(scsi_sg_count(scsicmd) == 0);
 		}
-		DBGINF("No sg; buffer:0x%p bufflen:%d\n",
-		       scsi_sglist(scsicmd), scsi_bufflen(scsicmd));
 	} else {
 		/* buffer is scatterlist - copy it out */
 		sgl = scsi_sglist(scsicmd);
@@ -994,21 +878,9 @@ virthba_queue_command_lck(struct scsi_cmnd *scsicmd,
 		for_each_sg(sgl, sg, scsi_sg_count(scsicmd), i) {
 			cmdrsp->scsi.gpi_list[i].address = sg_phys(sg);
 			cmdrsp->scsi.gpi_list[i].length = sg->length;
-			if ((i != 0) && (sg->offset != 0))
-				LOGINF("Offset on a sg_entry other than zero =<<%d>>.\n",
-				       sg->offset);
 		}
 
 		if (sg_failed) {
-			LOGERR("Start sg_list dump (entries %d, bufflen %d)...\n",
-			       scsi_sg_count(scsicmd), cmdrsp->scsi.bufflen);
-			for_each_sg(sgl, sg, scsi_sg_count(scsicmd), i) {
-				LOGERR("   Entry(%d): page->[0x%p], phys->[0x%Lx], off(%d), len(%d)\n",
-				       i, sg_page(sg),
-				       (unsigned long long)sg_phys(sg),
-				       sg->offset, sg->length);
-			}
-			LOGERR("Done sg_list dump.\n");
 			/* BUG(); ***** For now, let it fail in uissd
 			 * if it is a problem, as it might just
 			 * work
@@ -1027,7 +899,6 @@ virthba_queue_command_lck(struct scsi_cmnd *scsicmd,
 						 (u64)NULL, DONT_WAIT, "vhba");
 	if (i == 0) {
 		/* queue must be full - and we said don't wait - return busy */
-		LOGERR("uisqueue_put_cmdrsp_with_lock ****FAILED\n");
 		kfree(cmdrsp);
 		del_scsipending_entry(virthbainfo, (uintptr_t)insert_location);
 		return SCSI_MLQUEUE_DEVICE_BUSY;
@@ -1052,10 +923,9 @@ virthba_slave_alloc(struct scsi_device *scsidev)
 	struct Scsi_Host *scsihost = (struct Scsi_Host *)scsidev->host;
 
 	virthbainfo = (struct virthba_info *)scsihost->hostdata;
-	if (!virthbainfo) {
-		LOGERR("Could not find virthba_info for scsihost\n");
+	if (!virthbainfo)
 		return 0;	/* even though we errored, treat as success */
-	}
+
 	for (vdisk = &virthbainfo->head; vdisk->next; vdisk = vdisk->next) {
 		if (vdisk->next->valid &&
 		    (vdisk->next->channel == scsidev->channel) &&
@@ -1064,10 +934,8 @@ virthba_slave_alloc(struct scsi_device *scsidev)
 			return 0;
 	}
 	tmpvdisk = kzalloc(sizeof(*tmpvdisk), GFP_ATOMIC);
-	if (!tmpvdisk) {	/* error allocating */
-		LOGERR("Could not allocate memory for disk\n");
+	if (!tmpvdisk)
 		return 0;
-	}
 
 	tmpvdisk->channel = scsidev->channel;
 	tmpvdisk->id = scsidev->id;
@@ -1094,8 +962,6 @@ virthba_slave_destroy(struct scsi_device *scsidev)
 	struct Scsi_Host *scsihost = (struct Scsi_Host *)scsidev->host;
 
 	virthbainfo = (struct virthba_info *)scsihost->hostdata;
-	if (!virthbainfo)
-		LOGERR("Could not find virthba_info for scsihost\n");
 	for (vdisk = &virthbainfo->head; vdisk->next; vdisk = vdisk->next) {
 		if (vdisk->next->valid &&
 		    (vdisk->next->channel == scsidev->channel) &&
@@ -1140,19 +1006,6 @@ do_scsi_linuxstat(struct uiscmdrsp *cmdrsp, struct scsi_cmnd *scsicmd)
 
 		if (atomic_read(&vdisk->error_count) < VIRTHBA_ERROR_COUNT) {
 			atomic_inc(&vdisk->error_count);
-			LOGERR("SCSICMD ****FAILED scsicmd:0x%p op:0x%x <%d:%d:%d:%llu> 0x%x-0x%x-0x%x-0x%x-0x%x.\n",
-			       scsicmd, cmdrsp->scsi.cmnd[0],
-			       scsidev->host->host_no, scsidev->id,
-			       scsidev->channel, scsidev->lun,
-			       cmdrsp->scsi.linuxstat, sd->valid, sd->sense_key,
-			       sd->additional_sense_code,
-			       sd->additional_sense_code_qualifier);
-			if (atomic_read(&vdisk->error_count) ==
-			    VIRTHBA_ERROR_COUNT) {
-				LOGERR("Throtling SCSICMD errors disk <%d:%d:%d:%llu>\n",
-				       scsidev->host->host_no, scsidev->id,
-				       scsidev->channel, scsidev->lun);
-			}
 			atomic_set(&vdisk->ios_threshold, IOS_ERROR_THRESHOLD);
 		}
 	}
@@ -1188,7 +1041,6 @@ do_scsi_nolinuxstat(struct uiscmdrsp *cmdrsp, struct scsi_cmnd *scsicmd)
 
 		if (scsi_sg_count(scsicmd) == 0) {
 			if (scsi_bufflen(scsicmd) > 0) {
-				LOGERR("**** FAILED No scatter list for bufflen > 0\n");
 				BUG_ON(scsi_sg_count(scsicmd) ==
 				       0);
 			}
@@ -1199,8 +1051,6 @@ do_scsi_nolinuxstat(struct uiscmdrsp *cmdrsp, struct scsi_cmnd *scsicmd)
 
 		sg = scsi_sglist(scsicmd);
 		for (i = 0; i < scsi_sg_count(scsicmd); i++) {
-			DBGVER("copying OUT OF buf into 0x%p %d\n",
-			       sg_page(sg + i), sg[i].length);
 			thispage_orig = kmap_atomic(sg_page(sg + i));
 			thispage = (void *)((unsigned long)thispage_orig |
 					     sg[i].offset);
@@ -1219,7 +1069,6 @@ do_scsi_nolinuxstat(struct uiscmdrsp *cmdrsp, struct scsi_cmnd *scsicmd)
 			if (atomic_read(&vdisk->ios_threshold) > 0) {
 				atomic_dec(&vdisk->ios_threshold);
 				if (atomic_read(&vdisk->ios_threshold) == 0) {
-					LOGERR("Resetting error count for disk\n");
 					atomic_set(&vdisk->error_count, 0);
 				}
 			}
@@ -1230,8 +1079,6 @@ do_scsi_nolinuxstat(struct uiscmdrsp *cmdrsp, struct scsi_cmnd *scsicmd)
 static void
 complete_scsi_command(struct uiscmdrsp *cmdrsp, struct scsi_cmnd *scsicmd)
 {
-	DBGINF("cmdrsp: 0x%p, scsistat:0x%x.\n", cmdrsp, cmdrsp->scsi.scsistat);
-
 	/* take what we need out of cmdrsp and complete the scsicmd */
 	scsicmd->result = cmdrsp->scsi.linuxstat;
 	if (cmdrsp->scsi.linuxstat)
@@ -1239,10 +1086,8 @@ complete_scsi_command(struct uiscmdrsp *cmdrsp, struct scsi_cmnd *scsicmd)
 	else
 		do_scsi_nolinuxstat(cmdrsp, scsicmd);
 
-	if (scsicmd->scsi_done) {
-		DBGVER("Scsi_DONE\n");
+	if (scsicmd->scsi_done)
 		scsicmd->scsi_done(scsicmd);
-	}
 }
 
 static inline void
@@ -1252,7 +1097,6 @@ complete_vdiskmgmt_command(struct uiscmdrsp *cmdrsp)
 	/* wake up the error handler that is waiting for this */
 	*(int *)cmdrsp->vdiskmgmt.notifyresult = cmdrsp->vdiskmgmt.result;
 	wake_up_all((wait_queue_head_t *)cmdrsp->vdiskmgmt.notify);
-	LOGINF("set notify result to %d\n", cmdrsp->vdiskmgmt.result);
 }
 
 static inline void
@@ -1263,7 +1107,6 @@ complete_taskmgmt_command(struct uiscmdrsp *cmdrsp)
 	*(int *)cmdrsp->scsitaskmgmt.notifyresult =
 	    cmdrsp->scsitaskmgmt.result;
 	wake_up_all((wait_queue_head_t *)cmdrsp->scsitaskmgmt.notify);
-	LOGINF("set notify result to %d\n", cmdrsp->scsitaskmgmt.result);
 }
 
 static void
@@ -1319,8 +1162,7 @@ drain_queue(struct virthba_info *virthbainfo, struct chaninfo *dc,
 						    cmdrsp->vdiskmgmt.scsicmd))
 				break;
 			complete_vdiskmgmt_command(cmdrsp);
-		} else
-			LOGERR("Invalid cmdtype %d\n", cmdrsp->cmdtype);
+		}
 		/* cmdrsp is now available for reuse */
 	}
 }
@@ -1341,13 +1183,14 @@ process_incoming_rsps(void *v)
 	UIS_DAEMONIZE("vhba_incoming");
 	/* alloc once and reuse */
 	cmdrsp = kmalloc(SZ, GFP_ATOMIC);
-	if (cmdrsp == NULL) {
-		LOGERR("process_incoming_rsps ****FAILED to malloc - thread exiting\n");
+	if (!cmdrsp) {
 		complete_and_exit(&dc->threadinfo.has_stopped, 0);
 		return 0;
 	}
 	mask = ULTRA_CHANNEL_ENABLE_INTS;
 	while (1) {
+		if (kthread_should_stop())
+			break;
 		wait_event_interruptible_timeout(virthbainfo->rsp_queue,
 			 (atomic_read(&virthbainfo->interrupt_rcvd) == 1),
 				      usecs_to_jiffies(rsltq_wait_usecs));
@@ -1355,13 +1198,10 @@ process_incoming_rsps(void *v)
 		/* drain queue */
 		drain_queue(virthbainfo, dc, cmdrsp);
 		rc1 = uisqueue_interlocked_or(virthbainfo->flags_addr, mask);
-		if (dc->threadinfo.should_stop)
-			break;
 	}
 
 	kfree(cmdrsp);
 
-	DBGINF("exiting processing incoming rsps.\n");
 	complete_and_exit(&dc->threadinfo.has_stopped, 0);
 }
 
@@ -1386,7 +1226,7 @@ static ssize_t info_debugfs_read(struct file *file,
 		return -ENOMEM;
 
 	for (i = 0; i < VIRTHBASOPENMAX; i++) {
-		if (virthbas_open[i].virthbainfo == NULL)
+		if (!virthbas_open[i].virthbainfo)
 			continue;
 
 		virthbainfo = virthbas_open[i].virthbainfo;
@@ -1436,23 +1276,17 @@ static ssize_t enable_ints_write(struct file *file, const char __user *buffer,
 		return -EINVAL;
 
 	buf[count] = '\0';
-	if (copy_from_user(buf, buffer, count)) {
-		LOGERR("copy_from_user failed. buf<<%.*s>> count<<%lu>>\n",
-		       (int)count, buf, count);
+	if (copy_from_user(buf, buffer, count))
 		return -EFAULT;
-	}
 
 	i = kstrtoint(buf, 10, &new_value);
 
-	if (i != 0) {
-		LOGERR("Failed to scan value for enable_ints, buf<<%.*s>>",
-		       (int)count, buf);
+	if (i != 0)
 		return -EFAULT;
-	}
 
 	/* set all counts to new_value usually 0 */
 	for (i = 0; i < VIRTHBASOPENMAX; i++) {
-		if (virthbas_open[i].virthbainfo != NULL) {
+		if (virthbas_open[i].virthbainfo) {
 			virthbainfo = virthbas_open[i].virthbainfo;
 			features_addr =
 				&virthbainfo->chinfo.queueinfo->chan->features;
@@ -1484,17 +1318,11 @@ virthba_serverup(struct virtpci_dev *virtpcidev)
 	    (struct virthba_info *)((struct Scsi_Host *)virtpcidev->scsi.
 				     scsihost)->hostdata;
 
-	DBGINF("virtpcidev bus_no<<%d>>devNo<<%d>>", virtpcidev->bus_no,
-	       virtpcidev->device_no);
-
-	if (!virthbainfo->serverdown) {
-		DBGINF("Server up message received while server is already up.\n");
+	if (!virthbainfo->serverdown)
 		return 1;
-	}
-	if (virthbainfo->serverchangingstate) {
-		LOGERR("Server already processing change state message\n");
+
+	if (virthbainfo->serverchangingstate)
 		return 0;
-	}
 
 	virthbainfo->serverchangingstate = true;
 	/* Must transition channel to ATTACHED state BEFORE we
@@ -1508,7 +1336,6 @@ virthba_serverup(struct virtpci_dev *virtpcidev)
 	if (!uisthread_start(&virthbainfo->chinfo.threadinfo,
 			     process_incoming_rsps,
 			     virthbainfo, "vhba_incoming")) {
-		LOGERR("uisthread_start rsp ****FAILED\n");
 		return 0;
 	}
 	virthbainfo->serverdown = false;
@@ -1543,18 +1370,16 @@ virthba_serverdown_complete(struct work_struct *work)
 		switch (pendingdel->cmdtype) {
 		case CMD_SCSI_TYPE:
 			scsicmd = (struct scsi_cmnd *)pendingdel->sent;
-			scsicmd->result = (DID_RESET << 16);
+			scsicmd->result = DID_RESET << 16;
 			if (scsicmd->scsi_done)
 				scsicmd->scsi_done(scsicmd);
 			break;
 		case CMD_SCSITASKMGMT_TYPE:
 			cmdrsp = (struct uiscmdrsp *)pendingdel->sent;
-			DBGINF("cmdrsp=0x%x, notify=0x%x\n", cmdrsp,
-			       cmdrsp->scsitaskmgmt.notify);
-			*(int *)cmdrsp->scsitaskmgmt.notifyresult =
-			    TASK_MGMT_FAILED;
 			wake_up_all((wait_queue_head_t *)
 				    cmdrsp->scsitaskmgmt.notify);
+			*(int *)cmdrsp->scsitaskmgmt.notifyresult =
+				TASK_MGMT_FAILED;
 			break;
 		case CMD_VDISKMGMT_TYPE:
 			cmdrsp = (struct uiscmdrsp *)pendingdel->sent;
@@ -1564,9 +1389,7 @@ virthba_serverdown_complete(struct work_struct *work)
 				    cmdrsp->vdiskmgmt.notify);
 			break;
 		default:
-			if (pendingdel->sent != NULL)
-				LOGERR("Unknown command type: 0x%x.  Only freeing list structure.\n",
-				     pendingdel->cmdtype);
+			break;
 		}
 		pendingdel->cmdtype = 0;
 		pendingdel->sent = NULL;
@@ -1575,8 +1398,6 @@ virthba_serverdown_complete(struct work_struct *work)
 
 	virtpcidev = virthbainfo->virtpcidev;
 
-	DBGINF("virtpcidev bus_no<<%d>>devNo<<%d>>", virtpcidev->bus_no,
-	       virtpcidev->device_no);
 	virthbainfo->serverdown = true;
 	virthbainfo->serverchangingstate = false;
 	/* Return the ServerDown response to Command */
@@ -1594,19 +1415,12 @@ virthba_serverdown(struct virtpci_dev *virtpcidev, u32 state)
 	    (struct virthba_info *)((struct Scsi_Host *)virtpcidev->scsi.
 				     scsihost)->hostdata;
 
-	DBGINF("virthba_serverdown");
-	DBGINF("virtpcidev bus_no<<%d>>devNo<<%d>>", virtpcidev->bus_no,
-	       virtpcidev->device_no);
-
 	if (!virthbainfo->serverdown && !virthbainfo->serverchangingstate) {
 		virthbainfo->serverchangingstate = true;
 		queue_work(virthba_serverdown_workqueue,
 			   &virthbainfo->serverdown_completion);
 	} else if (virthbainfo->serverchangingstate) {
-		LOGERR("Server already processing change state message\n");
 		stat = 0;
-	} else {
-		LOGERR("Server already down, but another server down message received.");
 	}
 
 	return stat;
@@ -1619,7 +1433,6 @@ virthba_serverdown(struct virtpci_dev *virtpcidev, u32 state)
 static int __init
 virthba_parse_line(char *str)
 {
-	DBGINF("In virthba_parse_line %s\n", str);
 	return 1;
 }
 
@@ -1629,14 +1442,13 @@ virthba_parse_options(char *line)
 	char *next = line;
 
 	POSTCODE_LINUX_2(VHBA_CREATE_ENTRY_PC, POSTCODE_SEVERITY_INFO);
-	if (line == NULL || !*line)
+	if (!line || !*line)
 		return;
-	while ((line = next) != NULL) {
+	while ((line = next)) {
 		next = strchr(line, ' ');
-		if (next != NULL)
+		if (next)
 			*next++ = 0;
-		if (!virthba_parse_line(line))
-			DBGINF("Unknown option '%s'\n", line);
+		virthba_parse_line(line);
 	}
 
 	POSTCODE_LINUX_2(VHBA_CREATE_EXIT_PC, POSTCODE_SEVERITY_INFO);
@@ -1651,14 +1463,11 @@ virthba_mod_init(void)
 	if (!unisys_spar_platform)
 		return -ENODEV;
 
-	LOGINF("Entering virthba_mod_init...\n");
-
 	POSTCODE_LINUX_2(VHBA_CREATE_ENTRY_PC, POSTCODE_SEVERITY_INFO);
 	virthba_parse_options(virthba_options);
 
 	error = virtpci_register_driver(&virthba_driver);
 	if (error < 0) {
-		LOGERR("register ****FAILED 0x%x\n", error);
 		POSTCODE_LINUX_3(VHBA_CREATE_FAILURE_PC, error,
 				 POSTCODE_SEVERITY_ERR);
 	} else {
@@ -1681,8 +1490,7 @@ virthba_mod_init(void)
 		/* Initialize the serverdown workqueue */
 		virthba_serverdown_workqueue =
 		    create_singlethread_workqueue("virthba_serverdown");
-		if (virthba_serverdown_workqueue == NULL) {
-			LOGERR("**** FAILED virthba_serverdown_workqueue creation\n");
+		if (!virthba_serverdown_workqueue) {
 			POSTCODE_LINUX_2(VHBA_CREATE_FAILURE_PC,
 					 POSTCODE_SEVERITY_ERR);
 			error = -1;
@@ -1690,7 +1498,6 @@ virthba_mod_init(void)
 	}
 
 	POSTCODE_LINUX_2(VHBA_CREATE_EXIT_PC, POSTCODE_SEVERITY_INFO);
-	LOGINF("Leaving virthba_mod_init\n");
 	return error;
 }
 
@@ -1740,8 +1547,6 @@ static DEVICE_ATTRIBUTE *virthba_shost_attrs[] = {
 static void __exit
 virthba_mod_exit(void)
 {
-	LOGINF("entering virthba_mod_exit...\n");
-
 	virtpci_unregister_driver(&virthba_driver);
 	/* unregister is going to call virthba_remove */
 	/* destroy serverdown completion workqueue */
@@ -1751,7 +1556,6 @@ virthba_mod_exit(void)
 	}
 
 	debugfs_remove_recursive(virthba_debugfs_dir);
-	LOGINF("Leaving virthba_mod_exit\n");
 }
 
 /* specify function to be run at module insertion time */
diff --git a/drivers/staging/unisys/virtpci/Kconfig b/drivers/staging/unisys/virtpci/Kconfig
index e59efcbc4d3b..6d19482ce11b 100644
--- a/drivers/staging/unisys/virtpci/Kconfig
+++ b/drivers/staging/unisys/virtpci/Kconfig
@@ -4,7 +4,7 @@
 
 config UNISYS_VIRTPCI
 	tristate "Unisys virtpci driver"
-	depends on UNISYSSPAR && UNISYS_UISLIB
+	select UNISYS_UISLIB
 	---help---
 	If you say Y here, you will enable the Unisys virtpci driver.
 
diff --git a/drivers/staging/unisys/virtpci/virtpci.c b/drivers/staging/unisys/virtpci/virtpci.c
index 8fdfd6f3605f..d5ad01783c07 100644
--- a/drivers/staging/unisys/virtpci/virtpci.c
+++ b/drivers/staging/unisys/virtpci/virtpci.c
@@ -21,7 +21,6 @@
 #ifdef CONFIG_MODVERSIONS
 #include <config/modversions.h>
 #endif
-#include "uniklog.h"
 #include "diagnostics/appos_subsystems.h"
 #include "uisutils.h"
 #include "vbuschannel.h"
@@ -187,13 +186,11 @@ static int write_vbus_chp_info(struct spar_vbus_channel_protocol *chan,
 {
 	int off;
 
-	if (!chan) {
-		LOGERR("vbus channel not present");
+	if (!chan)
 		return -1;
-	}
+
 	off = sizeof(struct channel_header) + chan->hdr_info.chp_info_offset;
 	if (chan->hdr_info.chp_info_offset == 0) {
-		LOGERR("vbus channel not used, because chp_info_offset == 0");
 		return -1;
 	}
 	memcpy(((u8 *)(chan)) + off, info, sizeof(*info));
@@ -206,15 +203,12 @@ static int write_vbus_bus_info(struct spar_vbus_channel_protocol *chan,
 {
 	int off;
 
-	if (!chan) {
-		LOGERR("vbus channel not present");
+	if (!chan)
 		return -1;
-	}
+
 	off = sizeof(struct channel_header) + chan->hdr_info.bus_info_offset;
-	if (chan->hdr_info.bus_info_offset == 0) {
-		LOGERR("vbus channel not used, because bus_info_offset == 0");
+	if (chan->hdr_info.bus_info_offset == 0)
 		return -1;
-	}
 	memcpy(((u8 *)(chan)) + off, info, sizeof(*info));
 	return 0;
 }
@@ -228,18 +222,16 @@ write_vbus_dev_info(struct spar_vbus_channel_protocol *chan,
 {
 	int off;
 
-	if (!chan) {
-		LOGERR("vbus channel not present");
+	if (!chan)
 		return -1;
-	}
+
 	off =
 	    (sizeof(struct channel_header) +
 	     chan->hdr_info.dev_info_offset) +
 	    (chan->hdr_info.device_info_struct_bytes * devix);
-	if (chan->hdr_info.dev_info_offset == 0) {
-		LOGERR("vbus channel not used, because dev_info_offset == 0");
+	if (chan->hdr_info.dev_info_offset == 0)
 		return -1;
-	}
+
 	memcpy(((u8 *)(chan)) + off, info, sizeof(*info));
 	return 0;
 }
@@ -271,7 +263,6 @@ static int add_vbus(struct add_vbus_guestpart *addparams)
 	 */
 	ret = device_register(vbus);
 	if (ret) {
-		LOGERR("device_register FAILED:%d\n", ret);
 		POSTCODE_LINUX_2(VPCI_CREATE_FAILURE_PC, POSTCODE_SEVERITY_ERR);
 		return 0;
 	}
@@ -279,8 +270,6 @@ static int add_vbus(struct add_vbus_guestpart *addparams)
 			    &chipset_driver_info);
 	write_vbus_bus_info(vbus->platform_data /* chanptr */,
 			    &bus_driver_info);
-	LOGINF("Added vbus %d; device %s created successfully\n",
-	       addparams->bus_no, BUS_ID(vbus));
 	POSTCODE_LINUX_2(VPCI_CREATE_EXIT_PC, POSTCODE_SEVERITY_INFO);
 	return 1;
 }
@@ -312,7 +301,6 @@ static int add_vhba(struct add_virt_guestpart *addparams)
 	POSTCODE_LINUX_2(VPCI_CREATE_ENTRY_PC, POSTCODE_SEVERITY_INFO);
 	if (!WAIT_FOR_IO_CHANNEL
 	    ((struct spar_io_channel_protocol __iomem *)addparams->chanptr)) {
-		LOGERR("Timed out.  Channel not ready\n");
 		POSTCODE_LINUX_2(VPCI_CREATE_FAILURE_PC, POSTCODE_SEVERITY_ERR);
 		return 0;
 	}
@@ -323,19 +311,11 @@ static int add_vhba(struct add_virt_guestpart *addparams)
 	sprintf(busid, "vbus%d", addparams->bus_no);
 	vbus = bus_find_device(&virtpci_bus_type, NULL,
 			       (void *)busid, match_busid);
-	if (!vbus) {
-		LOGERR("**** FAILED to find vbus %s\n", busid);
+	if (!vbus)
 		return 0;
-	}
 
-	LOGINF("Adding vhba wwnn:%x:%x config:%d-%d-%d-%d chanptr:%p\n",
-	       scsi.wwnn.wwnn1, scsi.wwnn.wwnn2,
-	       scsi.max.max_channel, scsi.max.max_id, scsi.max.max_lun,
-	       scsi.max.cmd_per_lun, addparams->chanptr);
 	i = virtpci_device_add(vbus, VIRTHBA_TYPE, addparams, &scsi, NULL);
 	if (i) {
-		LOGINF("Added vhba wwnn:%x:%x chanptr:%p\n", scsi.wwnn.wwnn1,
-		       scsi.wwnn.wwnn2, addparams->chanptr);
 		POSTCODE_LINUX_3(VPCI_CREATE_EXIT_PC, i,
 				 POSTCODE_SEVERITY_INFO);
 	}
@@ -375,7 +355,6 @@ add_vnic(struct add_virt_guestpart *addparams)
 	POSTCODE_LINUX_2(VPCI_CREATE_ENTRY_PC, POSTCODE_SEVERITY_INFO);
 	if (!WAIT_FOR_IO_CHANNEL
 	    ((struct spar_io_channel_protocol __iomem *)addparams->chanptr)) {
-		LOGERR("Timed out, channel not ready\n");
 		POSTCODE_LINUX_2(VPCI_CREATE_FAILURE_PC, POSTCODE_SEVERITY_ERR);
 		return 0;
 	}
@@ -386,20 +365,11 @@ add_vnic(struct add_virt_guestpart *addparams)
 	sprintf(busid, "vbus%d", addparams->bus_no);
 	vbus = bus_find_device(&virtpci_bus_type, NULL,
 			       (void *)busid, match_busid);
-	if (!vbus) {
-		LOGERR("**** FAILED to find vbus %s\n", busid);
+	if (!vbus)
 		return 0;
-	}
 
-	LOGINF("Adding vnic macaddr:%02x:%02x:%02x:%02x:%02x:%02x rcvbufs:%d mtu:%d chanptr:%p%pUL\n",
-	       net.mac_addr[0], net.mac_addr[1], net.mac_addr[2],
-	       net.mac_addr[3], net.mac_addr[4], net.mac_addr[5],
-	       net.num_rcv_bufs, net.mtu, addparams->chanptr, &net.zone_uuid);
 	i = virtpci_device_add(vbus, VIRTNIC_TYPE, addparams, NULL, &net);
 	if (i) {
-		LOGINF("Added vnic macaddr:%02x:%02x:%02x:%02x:%02x:%02x\n",
-		       net.mac_addr[0], net.mac_addr[1], net.mac_addr[2],
-		       net.mac_addr[3], net.mac_addr[4], net.mac_addr[5]);
 		POSTCODE_LINUX_3(VPCI_CREATE_EXIT_PC, i,
 				 POSTCODE_SEVERITY_INFO);
 		return 1;
@@ -420,34 +390,24 @@ delete_vbus(struct del_vbus_guestpart *delparams)
 	sprintf(busid, "vbus%d", delparams->bus_no);
 	vbus = bus_find_device(&virtpci_bus_type, NULL,
 			       (void *)busid, match_busid);
-	if (!vbus) {
-		LOGERR("**** FAILED to find vbus %s\n", busid);
+	if (!vbus)
 		return 0;
-	}
 
 	/* ensure that bus has no devices? -- TBD */
-	LOGINF("Deleting %s\n", BUS_ID(vbus));
-	if (delete_vbus_device(vbus, NULL))
-		return 0;	/* failure */
-	LOGINF("Deleted vbus %d\n", delparams->bus_no);
 	return 1;
 }
 
 static int
 delete_vbus_device(struct device *vbus, void *data)
 {
-	int checkforroot = (data != NULL);
 	struct device *dev = &virtpci_rootbus_device;
 
-	if ((checkforroot) && match_busid(vbus, (void *)BUS_ID(dev))) {
+	if ((data) && match_busid(vbus, (void *)BUS_ID(dev))) {
 		/* skip it - don't delete root bus */
-		LOGINF("skipping root bus\n");
 		return 0;	/* pretend no error */
 	}
-	LOGINF("Calling unregister for %s\n", BUS_ID(vbus));
 	device_unregister(vbus);
 	kfree(vbus);
-	LOGINF("VBus unregister and freed\n");
 	return 0;		/* no error */
 }
 
@@ -461,12 +421,8 @@ static int pause_vhba(struct pause_virt_guestpart *pauseparams)
 
 	GET_SCSIADAPINFO_FROM_CHANPTR(pauseparams->chanptr);
 
-	LOGINF("Pausing vhba wwnn:%x:%x\n", scsi.wwnn.wwnn1, scsi.wwnn.wwnn2);
 	i = virtpci_device_serverdown(NULL /*no parent bus */, VIRTHBA_TYPE,
 				      &scsi.wwnn, NULL);
-	if (i)
-		LOGINF("Paused vhba wwnn:%x:%x\n", scsi.wwnn.wwnn1,
-		       scsi.wwnn.wwnn2);
 	return i;
 }
 
@@ -480,16 +436,8 @@ static int pause_vnic(struct pause_virt_guestpart *pauseparams)
 
 	GET_NETADAPINFO_FROM_CHANPTR(pauseparams->chanptr);
 
-	LOGINF("Pausing vnic macaddr:%02x:%02x:%02x:%02x:%02x:%02x\n",
-	       net.mac_addr[0], net.mac_addr[1], net.mac_addr[2],
-	       net.mac_addr[3], net.mac_addr[4], net.mac_addr[5]);
 	i = virtpci_device_serverdown(NULL /*no parent bus */, VIRTNIC_TYPE,
 				      NULL, net.mac_addr);
-	if (i) {
-		LOGINF(" Paused vnic macaddr:%02x:%02x:%02x:%02x:%02x:%02x\n",
-		       net.mac_addr[0], net.mac_addr[1], net.mac_addr[2],
-		       net.mac_addr[3], net.mac_addr[4], net.mac_addr[5]);
-	}
 	return i;
 }
 
@@ -503,12 +451,8 @@ static int resume_vhba(struct resume_virt_guestpart *resumeparams)
 
 	GET_SCSIADAPINFO_FROM_CHANPTR(resumeparams->chanptr);
 
-	LOGINF("Resuming vhba wwnn:%x:%x\n", scsi.wwnn.wwnn1, scsi.wwnn.wwnn2);
 	i = virtpci_device_serverup(NULL /*no parent bus */, VIRTHBA_TYPE,
 				    &scsi.wwnn, NULL);
-	if (i)
-		LOGINF("Resumed vhba wwnn:%x:%x\n", scsi.wwnn.wwnn1,
-		       scsi.wwnn.wwnn2);
 	return i;
 }
 
@@ -523,16 +467,8 @@ resume_vnic(struct resume_virt_guestpart *resumeparams)
 
 	GET_NETADAPINFO_FROM_CHANPTR(resumeparams->chanptr);
 
-	LOGINF("Resuming vnic macaddr:%02x:%02x:%02x:%02x:%02x:%02x\n",
-	       net.mac_addr[0], net.mac_addr[1], net.mac_addr[2],
-	       net.mac_addr[3], net.mac_addr[4], net.mac_addr[5]);
 	i = virtpci_device_serverup(NULL /*no parent bus */, VIRTNIC_TYPE,
 				    NULL, net.mac_addr);
-	if (i) {
-		LOGINF(" Resumed vnic macaddr:%02x:%02x:%02x:%02x:%02x:%02x\n",
-		       net.mac_addr[0], net.mac_addr[1], net.mac_addr[2],
-		       net.mac_addr[3], net.mac_addr[4], net.mac_addr[5]);
-	}
 	return i;
 }
 
@@ -546,12 +482,9 @@ static int delete_vhba(struct del_virt_guestpart *delparams)
 
 	GET_SCSIADAPINFO_FROM_CHANPTR(delparams->chanptr);
 
-	LOGINF("Deleting vhba wwnn:%x:%x\n", scsi.wwnn.wwnn1, scsi.wwnn.wwnn2);
 	i = virtpci_device_del(NULL /*no parent bus */, VIRTHBA_TYPE,
 			       &scsi.wwnn, NULL);
 	if (i) {
-		LOGINF("Deleted vhba wwnn:%x:%x\n", scsi.wwnn.wwnn1,
-		       scsi.wwnn.wwnn2);
 		return 1;
 	}
 	return 0;
@@ -567,23 +500,13 @@ static int delete_vnic(struct del_virt_guestpart *delparams)
 
 	GET_NETADAPINFO_FROM_CHANPTR(delparams->chanptr);
 
-	LOGINF("Deleting vnic macaddr:%02x:%02x:%02x:%02x:%02x:%02x\n",
-	       net.mac_addr[0], net.mac_addr[1], net.mac_addr[2],
-	       net.mac_addr[3], net.mac_addr[4], net.mac_addr[5]);
 	i = virtpci_device_del(NULL /*no parent bus */, VIRTNIC_TYPE, NULL,
 			       net.mac_addr);
-	if (i) {
-		LOGINF("Deleted vnic macaddr:%02x:%02x:%02x:%02x:%02x:%02x\n",
-		       net.mac_addr[0], net.mac_addr[1], net.mac_addr[2],
-		       net.mac_addr[3], net.mac_addr[4], net.mac_addr[5]);
-	}
 	return i;
 }
 
 #define DELETE_ONE_VPCIDEV(vpcidev) { \
-	LOGINF("calling device_unregister:%p\n", &vpcidev->generic_dev); \
 	device_unregister(&vpcidev->generic_dev); \
-	LOGINF("Deleted %p\n", vpcidev); \
 	kfree(vpcidev); \
 }
 
@@ -610,12 +533,10 @@ static void delete_all(void)
 		tmpvpcidev = nextvpcidev;
 		count++;
 	}
-	LOGINF("Deleted %d vhbas/vnics.\n", count);
 
 	/* now delete each vbus */
-	if (bus_for_each_dev
-	    (&virtpci_bus_type, NULL, (void *)1, delete_vbus_device))
-		LOGERR("delete of all vbus failed\n");
+	bus_for_each_dev(&virtpci_bus_type, NULL, (void *)1,
+			 delete_vbus_device);
 }
 
 /* deletes all vnics or vhbas
@@ -632,24 +553,14 @@ static int delete_all_virt(enum virtpci_dev_type devtype,
 	sprintf(busid, "vbus%d", delparams->bus_no);
 	vbus = bus_find_device(&virtpci_bus_type, NULL,
 			       (void *)busid, match_busid);
-	if (!vbus) {
-		LOGERR("**** FAILED to find vbus %s\n", busid);
+	if (!vbus)
 		return 0;
-	}
 
-	if ((devtype != VIRTHBA_TYPE) && (devtype != VIRTNIC_TYPE)) {
-		LOGERR("**** FAILED to delete all devices; devtype:%d not vhba:%d or vnic:%d\n",
-		       devtype, VIRTHBA_TYPE, VIRTNIC_TYPE);
+	if ((devtype != VIRTHBA_TYPE) && (devtype != VIRTNIC_TYPE))
 		return 0;
-	}
 
-	LOGINF("Deleting all %s in vbus %s\n",
-	       devtype == VIRTHBA_TYPE ? "vhbas" : "vnics", busid);
 	/* delete all vhbas/vnics */
 	i = virtpci_device_del(vbus, devtype, NULL, NULL);
-	if (i > 0)
-		LOGINF("Deleted %d %s\n", i,
-		       devtype == VIRTHBA_TYPE ? "vhbas" : "vnics");
 	return 1;
 }
 
@@ -684,7 +595,6 @@ static int virtpci_ctrlchan_func(struct guest_msgs *msg)
 	case GUEST_RESUME_VNIC:
 		return resume_vnic(&msg->resume_vnic);
 	default:
-		LOGERR("invalid message type %d.\n", msg->msgtype);
 		return 0;
 	}
 }
@@ -708,9 +618,6 @@ virtpci_match_device(const struct pci_device_id *ids,
 		     const struct virtpci_dev *dev)
 {
 	while (ids->vendor || ids->subvendor || ids->class_mask) {
-		DBGINF("ids->vendor:%x dev->vendor:%x ids->device:%x dev->device:%x\n",
-		       ids->vendor, dev->vendor, ids->device, dev->device);
-
 		if ((ids->vendor == dev->vendor) &&
 		    (ids->device == dev->device))
 			return ids;
@@ -731,20 +638,15 @@ static int virtpci_bus_match(struct device *dev, struct device_driver *drv)
 	struct virtpci_driver *virtpcidrv = driver_to_virtpci_driver(drv);
 	int match = 0;
 
-	DBGINF("In virtpci_bus_match dev->bus_id:%s drv->name:%s\n",
-	       dev->bus_id, drv->name);
-
 	/* check ids list for a match */
 	if (virtpci_match_device(virtpcidrv->id_table, virtpcidev))
 		match = 1;
 
-	DBGINF("returning match:%d\n", match);
 	return match;		/* 0 - no match; 1 - yes it matches */
 }
 
 static int virtpci_uevent(struct device *dev, struct kobj_uevent_env *env)
 {
-	DBGINF("In virtpci_hotplug\n");
 	/* add variables to the environment prior to the generation of
 	 * hotplug events to user space
 	 */
@@ -766,24 +668,19 @@ static void fix_vbus_dev_info(struct device *dev, int dev_no, int dev_type,
 	struct ultra_vbus_deviceinfo dev_info;
 	const char *stype;
 
-	if (!dev) {
-		LOGERR("%s dev is NULL", __func__);
+	if (!dev)
 		return;
-	}
-	if (!virtpcidrv) {
-		LOGERR("%s driver is NULL", __func__);
+	if (!virtpcidrv)
 		return;
-	}
+
 	vbus = dev->parent;
-	if (!vbus) {
-		LOGERR("%s dev has no parent bus", __func__);
+	if (!vbus)
 		return;
-	}
+
 	chan = vbus->platform_data;
-	if (!chan) {
-		LOGERR("%s dev bus has no channel", __func__);
+	if (!chan)
 		return;
-	}
+
 	switch (dev_type) {
 	case PCI_DEVICE_ID_VIRTHBA:
 		stype = "vHBA";
@@ -820,8 +717,6 @@ static int virtpci_device_probe(struct device *dev)
 	const struct pci_device_id *id;
 	int error = 0;
 
-	LOGINF("In virtpci_device_probe dev:%p virtpcidev:%p virtpcidrv:%p\n",
-	       dev, virtpcidev, virtpcidrv);	/* VERBOSE/DEBUG ? */
 	POSTCODE_LINUX_2(VPCI_PROBE_ENTRY_PC, POSTCODE_SEVERITY_INFO);
 	/* static match and static probe vs dynamic match & dynamic
 	 * probe - do we care?.
@@ -866,9 +761,6 @@ static int virtpci_device_remove(struct device *dev_)
 	struct virtpci_dev *virtpcidev = device_to_virtpci_dev(dev_);
 	struct virtpci_driver *virtpcidrv = virtpcidev->mydriver;
 
-	LOGINF("In virtpci_device_remove bus_id:%s dev_:%p virtpcidev:%p dev->driver:%p drivername:%s\n",
-	       BUS_ID(dev_), dev_, virtpcidev, dev_->driver,
-	       dev_->driver->name);	/* VERBOSE/DEBUG */
 	if (virtpcidrv) {
 		/* TEMP: assuming we have only one such driver for now */
 		if (virtpcidrv->remove)
@@ -876,10 +768,7 @@ static int virtpci_device_remove(struct device *dev_)
 		virtpcidev->mydriver = NULL;
 	}
 
-	DBGINF("calling putdevice\n");
 	put_device(dev_);
-
-	DBGINF("Leaving\n");
 	return 0;
 }
 
@@ -889,11 +778,6 @@ static int virtpci_device_remove(struct device *dev_)
 
 static void virtpci_bus_release(struct device *dev)
 {
-	/* this function is called when the last reference to the
-	 * device is removed
-	 */
-	DBGINF("In virtpci_bus_release\n");
-	/* what else is supposed to happen here? */
 }
 
 /*****************************************************/
@@ -915,14 +799,9 @@ static int virtpci_device_add(struct device *parentbus, int devtype,
 	struct spar_io_channel_protocol __iomem *io_chan = NULL;
 	struct device *dev;
 
-	LOGINF("virtpci_device_add parentbus:%p chanptr:%p\n", parentbus,
-	       addparams->chanptr);
-
 	POSTCODE_LINUX_2(VPCI_CREATE_ENTRY_PC, POSTCODE_SEVERITY_INFO);
 
 	if ((devtype != VIRTHBA_TYPE) && (devtype != VIRTNIC_TYPE)) {
-		LOGERR("**** FAILED to add device; devtype:%d not vhba:%d or vnic:%d\n",
-		       devtype, VIRTHBA_TYPE, VIRTNIC_TYPE);
 		POSTCODE_LINUX_3(VPCI_CREATE_FAILURE_PC, devtype,
 				 POSTCODE_SEVERITY_ERR);
 		return 0;
@@ -930,8 +809,7 @@ static int virtpci_device_add(struct device *parentbus, int devtype,
 
 	/* add a Virtual Device */
 	virtpcidev = kzalloc(sizeof(*virtpcidev), GFP_ATOMIC);
-	if (virtpcidev == NULL) {
-		LOGERR("can't add device - malloc FALLED\n");
+	if (!virtpcidev) {
 		POSTCODE_LINUX_2(MALLOC_FAILURE_PC, POSTCODE_SEVERITY_ERR);
 		return 0;
 	}
@@ -993,7 +871,6 @@ static int virtpci_device_add(struct device *parentbus, int devtype,
 		 */
 		write_unlock_irqrestore(&vpcidev_list_lock, flags);
 		kfree(virtpcidev);
-		LOGERR("**** FAILED vhba/vnic already exists in the list\n");
 		POSTCODE_LINUX_2(VPCI_CREATE_FAILURE_PC, POSTCODE_SEVERITY_ERR);
 		return 0;
 	}
@@ -1024,8 +901,6 @@ static int virtpci_device_add(struct device *parentbus, int devtype,
 	* list. Otherwise, a device_unregister from this function can
 	* cause a "scheduling while atomic".
 	*/
-	DBGINF("registering device:%p with bus_id:%s\n",
-	       &virtpcidev->generic_dev, virtpcidev->generic_dev.bus_id);
 	ret = device_register(&virtpcidev->generic_dev);
 	/* NOTE: THIS IS CALLING HOTPLUG virtpci_hotplug!!!
 	 * This call to device_register results in virtpci_bus_match
@@ -1037,7 +912,6 @@ static int virtpci_device_add(struct device *parentbus, int devtype,
 	 * virtpci_device_probe is successful
 	 */
 	if (ret) {
-		LOGERR("device_register returned %d\n", ret);
 		dev = &virtpcidev->generic_dev;
 		SPAR_CHANNEL_CLIENT_TRANSITION(addparams->chanptr,
 					       BUS_ID(dev),
@@ -1060,10 +934,6 @@ static int virtpci_device_add(struct device *parentbus, int devtype,
 		return 0;
 	}
 
-	LOGINF("Added %s:%d:%d &virtpcidev->generic_dev:%p\n",
-	       (devtype == VIRTHBA_TYPE) ? "virthba" : "virtnic",
-	       addparams->bus_no, addparams->device_no,
-	       &virtpcidev->generic_dev);
 	POSTCODE_LINUX_2(VPCI_CREATE_EXIT_PC, POSTCODE_SEVERITY_INFO);
 	return 1;
 }
@@ -1080,11 +950,8 @@ static int virtpci_device_serverdown(struct device *parentbus,
 	unsigned long flags;
 	int rc = 0;
 
-	if ((devtype != VIRTHBA_TYPE) && (devtype != VIRTNIC_TYPE)) {
-		LOGERR("**** FAILED to pause device; devtype:%d not vhba:%d or vnic:%d\n",
-		       devtype, VIRTHBA_TYPE, VIRTNIC_TYPE);
+	if ((devtype != VIRTHBA_TYPE) && (devtype != VIRTNIC_TYPE))
 		return 0;
-	}
 
 	/* find the vhba or vnic in virtpci device list */
 	write_lock_irqsave(&vpcidev_list_lock, flags);
@@ -1119,10 +986,8 @@ static int virtpci_device_serverdown(struct device *parentbus,
 	}
 	write_unlock_irqrestore(&vpcidev_list_lock, flags);
 
-	if (!found) {
-		LOGERR("**** FAILED to find vhba/vnic in the list\n");
+	if (!found)
 		return 0;
-	}
 
 	return rc;
 }
@@ -1139,11 +1004,9 @@ static int virtpci_device_serverup(struct device *parentbus,
 	unsigned long flags;
 	int rc = 0;
 
-	if ((devtype != VIRTHBA_TYPE) && (devtype != VIRTNIC_TYPE)) {
-		LOGERR("**** FAILED to resume device; devtype:%d not vhba:%d or vnic:%d\n",
-		       devtype, VIRTHBA_TYPE, VIRTNIC_TYPE);
+	if ((devtype != VIRTHBA_TYPE) && (devtype != VIRTNIC_TYPE))
 		return 0;
-	}
+
 
 	/* find the vhba or vnic in virtpci device list */
 	write_lock_irqsave(&vpcidev_list_lock, flags);
@@ -1187,10 +1050,8 @@ static int virtpci_device_serverup(struct device *parentbus,
 
 	write_unlock_irqrestore(&vpcidev_list_lock, flags);
 
-	if (!found) {
-		LOGERR("**** FAILED to find vhba/vnic in the list\n");
+	if (!found)
 		return 0;
-	}
 
 	return rc;
 }
@@ -1209,17 +1070,14 @@ static int virtpci_device_del(struct device *parentbus,
 	continue; \
 }
 
-	if ((devtype != VIRTHBA_TYPE) && (devtype != VIRTNIC_TYPE)) {
-		LOGERR("**** FAILED to delete device; devtype:%d not vhba:%d or vnic:%d\n",
-		       devtype, VIRTHBA_TYPE, VIRTNIC_TYPE);
+	if ((devtype != VIRTHBA_TYPE) && (devtype != VIRTNIC_TYPE))
 		return 0;
-	}
 
 	/* see if we are to delete all - NOTE: all implies we have a
 	 * valid parentbus
 	 */
-	all = ((devtype == VIRTHBA_TYPE) && (wwnn == NULL)) ||
-	    ((devtype == VIRTNIC_TYPE) && (macaddr == NULL));
+	all = ((devtype == VIRTHBA_TYPE) && (!wwnn)) ||
+	    ((devtype == VIRTNIC_TYPE) && (!macaddr));
 
 	/* find all the vhba or vnic or both in virtpci device list
 	* keep list of ones we are deleting so we can call
@@ -1280,10 +1138,8 @@ static int virtpci_device_del(struct device *parentbus,
 	}
 	write_unlock_irqrestore(&vpcidev_list_lock, flags);
 
-	if (!all && (count == 0)) {
-		LOGERR("**** FAILED to find vhba/vnic in the list\n");
+	if (!all && (count == 0))
 		return 0;
-	}
 
 	/* now delete each one from delete list */
 	while (dellist) {
@@ -1303,7 +1159,6 @@ static void virtpci_device_release(struct device *dev_)
 	/* this function is called when the last reference to the
 	 * device is removed
 	 */
-	LOGINF("In virtpci_device_release:%p - NOT YET IMPLEMENTED\n", dev_);
 }
 
 /*****************************************************/
@@ -1324,8 +1179,6 @@ static ssize_t virtpci_driver_attr_show(struct kobject *kobj,
 	struct driver_private *dprivate = to_driver(kobj);
 	struct device_driver *driver = dprivate->driver;
 
-	DBGINF("In virtpci_driver_attr_show driver->name:%s\n",	driver->name);
-
 	if (dattr->show)
 		ret = dattr->show(driver, buf);
 
@@ -1342,8 +1195,6 @@ static ssize_t virtpci_driver_attr_store(struct kobject *kobj,
 	struct driver_private *dprivate = to_driver(kobj);
 	struct device_driver *driver = dprivate->driver;
 
-	DBGINF("In virtpci_driver_attr_store driver->name:%s\n", driver->name);
-
 	if (dattr->store)
 		ret = dattr->store(driver, buf, count);
 
@@ -1355,12 +1206,8 @@ int virtpci_register_driver(struct virtpci_driver *drv)
 {
 	int result = 0;
 
-	DBGINF("In virtpci_register_driver\n");
-
-	if (drv->id_table == NULL) {
-		LOGERR("id_table missing\n");
+	if (!drv->id_table)
 		return 1;
-	}
 	/* initialize core driver fields needed to call driver_register */
 	drv->core_driver.name = drv->name;	/* name of driver in sysfs */
 	drv->core_driver.bus = &virtpci_bus_type;	/* type of bus this
@@ -1389,7 +1236,6 @@ EXPORT_SYMBOL_GPL(virtpci_register_driver);
 
 void virtpci_unregister_driver(struct virtpci_driver *drv)
 {
-	DBGINF("In virtpci_unregister_driver drv:%p\n", drv);
 	driver_unregister(&drv->core_driver);
 	/* driver_unregister calls bus_remove_driver
 	 * bus_remove_driver calls device_detach
@@ -1399,7 +1245,6 @@ void virtpci_unregister_driver(struct virtpci_driver *drv)
 	 * virtpci_device_remove
 	 * virtpci_device_remove calls virthba_remove
 	 */
-	DBGINF("Leaving\n");
 }
 EXPORT_SYMBOL_GPL(virtpci_unregister_driver);
 
@@ -1442,9 +1287,8 @@ static ssize_t info_debugfs_read(struct file *file, char __user *buf,
 	printparam.str_pos = &str_pos;
 	printparam.buf = vbuf;
 	printparam.len = &len;
-	if (bus_for_each_dev(&virtpci_bus_type, NULL,
-			     (void *)&printparam, print_vbus))
-		LOGERR("Failed to find bus\n");
+	bus_for_each_dev(&virtpci_bus_type, NULL, (void *)&printparam,
+			 print_vbus);
 
 	str_pos += scnprintf(vbuf + str_pos, len - str_pos,
 			"\n Virtual PCI devices\n");
@@ -1464,15 +1308,10 @@ static ssize_t info_debugfs_read(struct file *file, char __user *buf,
 					tmpvpcidev->scsi.max.cmd_per_lun);
 		} else {
 			str_pos += scnprintf(vbuf + str_pos, len - str_pos,
-					"[%d:%d] VNic:%02x:%02x:%02x:%02x:%02x:%02x num_rcv_bufs:%d mtu:%d",
+					"[%d:%d] VNic:%pM num_rcv_bufs:%d mtu:%d",
 					tmpvpcidev->bus_no,
 					tmpvpcidev->device_no,
-					tmpvpcidev->net.mac_addr[0],
-					tmpvpcidev->net.mac_addr[1],
-					tmpvpcidev->net.mac_addr[2],
-					tmpvpcidev->net.mac_addr[3],
-					tmpvpcidev->net.mac_addr[4],
-					tmpvpcidev->net.mac_addr[5],
+					tmpvpcidev->net.mac_addr,
 					tmpvpcidev->net.num_rcv_bufs,
 					tmpvpcidev->net.mtu);
 		}
@@ -1507,58 +1346,44 @@ static int __init virtpci_mod_init(void)
 	 * drivers directory
 	 */
 	if (ret) {
-		LOGERR("bus_register ****FAILED:%d\n", ret);
 		POSTCODE_LINUX_3(VPCI_CREATE_FAILURE_PC, ret,
 				 POSTCODE_SEVERITY_ERR);
 		return ret;
 	}
-	DBGINF("bus_register successful\n");
 	bus_device_info_init(&bus_driver_info, "clientbus", "virtpci",
 			     VERSION, NULL);
 
 	/* create a root bus used to parent all the virtpci buses. */
 	ret = device_register(&virtpci_rootbus_device);
 	if (ret) {
-		LOGERR("device_register FAILED:%d\n", ret);
 		bus_unregister(&virtpci_bus_type);
 		POSTCODE_LINUX_3(VPCI_CREATE_FAILURE_PC, ret,
 				 POSTCODE_SEVERITY_ERR);
 		return ret;
 	}
-	DBGINF("device_register successful ret:%x\n", ret);
 
 	if (!uisctrl_register_req_handler(2, (void *)&virtpci_ctrlchan_func,
 					  &chipset_driver_info)) {
-		LOGERR("uisctrl_register_req_handler ****FAILED.\n");
 		POSTCODE_LINUX_2(VPCI_CREATE_FAILURE_PC, POSTCODE_SEVERITY_ERR);
 		device_unregister(&virtpci_rootbus_device);
 		bus_unregister(&virtpci_bus_type);
 		return -1;
 	}
 
-	LOGINF("successfully registered virtpci_ctrlchan_func (0x%p) as callback.\n",
-	       (void *)&virtpci_ctrlchan_func);
 	/* create debugfs directory and info file inside. */
 	virtpci_debugfs_dir = debugfs_create_dir("virtpci", NULL);
 	debugfs_create_file("info", S_IRUSR, virtpci_debugfs_dir,
 			    NULL, &debugfs_info_fops);
-	LOGINF("Leaving\n");
 	POSTCODE_LINUX_2(VPCI_CREATE_EXIT_PC, POSTCODE_SEVERITY_INFO);
 	return 0;
 }
 
 static void __exit virtpci_mod_exit(void)
 {
-	LOGINF("virtpci_mod_exit...\n");
-
 	/* unregister the callback function */
-	if (!uisctrl_register_req_handler(2, NULL, NULL))
-		LOGERR("uisctrl_register_req_handler ****FAILED.\n");
-
 	device_unregister(&virtpci_rootbus_device);
 	bus_unregister(&virtpci_bus_type);
 	debugfs_remove_recursive(virtpci_debugfs_dir);
-	LOGINF("Leaving\n");
 }
 
 module_init(virtpci_mod_init);
diff --git a/drivers/staging/unisys/visorchannel/Kconfig b/drivers/staging/unisys/visorchannel/Kconfig
index 41c3b4b997eb..8d31bebf039a 100644
--- a/drivers/staging/unisys/visorchannel/Kconfig
+++ b/drivers/staging/unisys/visorchannel/Kconfig
@@ -4,7 +4,7 @@
 
 config UNISYS_VISORCHANNEL
 	tristate "Unisys visorchannel driver"
-	depends on UNISYSSPAR && UNISYS_VISORUTIL
+	select UNISYS_VISORUTIL
 	---help---
 	If you say Y here, you will enable the Unisys visorchannel driver.
 
diff --git a/drivers/staging/unisys/visorchannel/globals.h b/drivers/staging/unisys/visorchannel/globals.h
index 581ed83fe6d0..0ed8e1d8033a 100644
--- a/drivers/staging/unisys/visorchannel/globals.h
+++ b/drivers/staging/unisys/visorchannel/globals.h
@@ -18,7 +18,6 @@
 #ifndef __VISORCHANNEL_GLOBALS_H__
 #define __VISORCHANNEL_GLOBALS_H__
 
-#include "uniklog.h"
 #include "timskmod.h"
 #include "memregion.h"
 #include "version.h"
diff --git a/drivers/staging/unisys/visorchannel/visorchannel_funcs.c b/drivers/staging/unisys/visorchannel/visorchannel_funcs.c
index 0188ef866fdd..7a9a7242f75d 100644
--- a/drivers/staging/unisys/visorchannel/visorchannel_funcs.c
+++ b/drivers/staging/unisys/visorchannel/visorchannel_funcs.c
@@ -58,8 +58,7 @@ visorchannel_create_guts(HOSTADDRESS physaddr, ulong channel_bytes,
 	void *rc = NULL;
 
 	p = kmalloc(sizeof(*p), GFP_KERNEL|__GFP_NORETRY);
-	if (p == NULL) {
-		ERRDRV("allocation failed: (status=0)\n");
+	if (!p) {
 		rc = NULL;
 		goto cleanup;
 	}
@@ -69,7 +68,7 @@ visorchannel_create_guts(HOSTADDRESS physaddr, ulong channel_bytes,
 	spin_lock_init(&p->remove_lock);
 
 	/* prepare chan_hdr (abstraction to read/write channel memory) */
-	if (parent == NULL)
+	if (!parent)
 		p->memregion =
 		    visor_memregion_create(physaddr,
 					   sizeof(struct channel_header));
@@ -77,14 +76,12 @@ visorchannel_create_guts(HOSTADDRESS physaddr, ulong channel_bytes,
 		p->memregion =
 		    visor_memregion_create_overlapped(parent->memregion,
 				off, sizeof(struct channel_header));
-	if (p->memregion == NULL) {
-		ERRDRV("visor_memregion_create failed failed: (status=0)\n");
+	if (!p->memregion) {
 		rc = NULL;
 		goto cleanup;
 	}
 	if (visor_memregion_read(p->memregion, 0, &p->chan_hdr,
 				 sizeof(struct channel_header)) < 0) {
-		ERRDRV("visor_memregion_read failed: (status=0)\n");
 		rc = NULL;
 		goto cleanup;
 	}
@@ -95,7 +92,6 @@ visorchannel_create_guts(HOSTADDRESS physaddr, ulong channel_bytes,
 		/* we had better be a CLIENT of this channel */
 		guid = p->chan_hdr.chtype;
 	if (visor_memregion_resize(p->memregion, channel_bytes) < 0) {
-		ERRDRV("visor_memregion_resize failed: (status=0)\n");
 		rc = NULL;
 		goto cleanup;
 	}
@@ -105,8 +101,8 @@ visorchannel_create_guts(HOSTADDRESS physaddr, ulong channel_bytes,
 	rc = p;
 cleanup:
 
-	if (rc == NULL) {
-		if (p != NULL) {
+	if (!rc) {
+		if (!p) {
 			visorchannel_destroy(p);
 			p = NULL;
 		}
@@ -154,9 +150,9 @@ EXPORT_SYMBOL_GPL(visorchannel_create_overlapped_with_lock);
 void
 visorchannel_destroy(struct visorchannel *channel)
 {
-	if (channel == NULL)
+	if (!channel)
 		return;
-	if (channel->memregion != NULL) {
+	if (channel->memregion) {
 		visor_memregion_destroy(channel->memregion);
 		channel->memregion = NULL;
 	}
@@ -256,10 +252,9 @@ visorchannel_clear(struct visorchannel *channel, ulong offset, u8 ch,
 	int written = 0;
 	u8 *buf = vmalloc(bufsize);
 
-	if (buf == NULL) {
-		ERRDRV("%s failed memory allocation", __func__);
+	if (!buf)
 		goto cleanup;
-	}
+
 	memset(buf, ch, bufsize);
 	while (nbytes > 0) {
 		ulong thisbytes = bufsize;
@@ -279,7 +274,7 @@ visorchannel_clear(struct visorchannel *channel, ulong offset, u8 ch,
 	rc = 0;
 
 cleanup:
-	if (buf != NULL) {
+	if (buf) {
 		vfree(buf);
 		buf = NULL;
 	}
@@ -324,10 +319,8 @@ sig_read_header(struct visorchannel *channel, u32 queue,
 {
 	BOOL rc = FALSE;
 
-	if (channel->chan_hdr.ch_space_offset < sizeof(struct channel_header)) {
-		ERRDRV("oChannelSpace too small: (status=%d)\n", rc);
+	if (channel->chan_hdr.ch_space_offset < sizeof(struct channel_header))
 		goto cleanup;
-	}
 
 	/* Read the appropriate SIGNAL_QUEUE_HEADER into local memory. */
 
@@ -335,10 +328,6 @@ sig_read_header(struct visorchannel *channel, u32 queue,
 				 SIG_QUEUE_OFFSET(&channel->chan_hdr, queue),
 				 sig_hdr,
 				 sizeof(struct signal_queue_header)) < 0) {
-		ERRDRV("queue=%d SIG_QUEUE_OFFSET=%d",
-		       queue, (int)SIG_QUEUE_OFFSET(&channel->chan_hdr, queue));
-		ERRDRV("visor_memregion_read of signal queue failed: (status=%d)\n",
-		       rc);
 		goto cleanup;
 	}
 	rc = TRUE;
@@ -358,15 +347,11 @@ sig_do_data(struct visorchannel *channel, u32 queue,
 		if (visor_memregion_write(channel->memregion,
 					  signal_data_offset,
 					  data, sig_hdr->signal_size) < 0) {
-			ERRDRV("visor_memregion_write of signal data failed: (status=%d)\n",
-			       rc);
 			goto cleanup;
 		}
 	} else {
 		if (visor_memregion_read(channel->memregion, signal_data_offset,
 					 data, sig_hdr->signal_size) < 0) {
-			ERRDRV("visor_memregion_read of signal data failed: (status=%d)\n",
-			       rc);
 			goto cleanup;
 		}
 	}
@@ -404,8 +389,6 @@ safe_sig_queue_validate(struct signal_queue_header *psafe_sqh,
 		punsafe_sqh->head = *phead;
 		punsafe_sqh->tail = *ptail;
 
-		ERRDRV("safe_sig_queue_validate: head = 0x%x, tail = 0x%x, MaxSlots = 0x%x",
-		       *phead, *ptail, psafe_sqh->max_slots);
 		return 0;
 	}
 	return 1;
@@ -416,15 +399,13 @@ signalremove_inner(struct visorchannel *channel, u32 queue, void *msg)
 {
 	struct signal_queue_header sig_hdr;
 
-	if (!sig_read_header(channel, queue, &sig_hdr)) {
+	if (!sig_read_header(channel, queue, &sig_hdr))
 		return FALSE;
-	}
 	if (sig_hdr.head == sig_hdr.tail)
 		return FALSE;	/* no signals to remove */
 
 	sig_hdr.tail = (sig_hdr.tail + 1) % sig_hdr.max_slots;
 	if (!sig_read_data(channel, queue, &sig_hdr, sig_hdr.tail, msg)) {
-		ERRDRV("sig_read_data failed\n");
 		return FALSE;
 	}
 	sig_hdr.num_received++;
@@ -433,14 +414,10 @@ signalremove_inner(struct visorchannel *channel, u32 queue, void *msg)
 	 * update host memory.
 	 */
 	mb(); /* required for channel synch */
-	if (!SIG_WRITE_FIELD(channel, queue, &sig_hdr, tail)) {
-		ERRDRV("visor_memregion_write of Tail failed\n");
+	if (!SIG_WRITE_FIELD(channel, queue, &sig_hdr, tail))
 		return FALSE;
-	}
-	if (!SIG_WRITE_FIELD(channel, queue, &sig_hdr, num_received)) {
-		ERRDRV("visor_memregion_write of NumSignalsReceived failed\n");
+	if (!SIG_WRITE_FIELD(channel, queue, &sig_hdr, num_received))
 		return FALSE;
-	}
 	return TRUE;
 }
 
@@ -466,35 +443,34 @@ signalinsert_inner(struct visorchannel *channel, u32 queue, void *msg)
 {
 	struct signal_queue_header sig_hdr;
 
-	if (!sig_read_header(channel, queue, &sig_hdr)) {
+	if (!sig_read_header(channel, queue, &sig_hdr))
 		return FALSE;
-	}
 
 	sig_hdr.head = ((sig_hdr.head + 1) % sig_hdr.max_slots);
 	if (sig_hdr.head == sig_hdr.tail) {
 		sig_hdr.num_overflows++;
-		if (!SIG_WRITE_FIELD(channel, queue, &sig_hdr, num_overflows))
-			ERRDRV("visor_memregion_write of NumOverflows failed\n");
-
+		visor_memregion_write(channel->memregion,
+				      SIG_QUEUE_OFFSET(&channel->chan_hdr,
+						       queue) +
+				      offsetof(struct signal_queue_header,
+					       num_overflows),
+				      &(sig_hdr.num_overflows),
+				      sizeof(sig_hdr.num_overflows));
 		return FALSE;
 	}
 
-	if (!sig_write_data(channel, queue, &sig_hdr, sig_hdr.head, msg)) {
-		ERRDRV("sig_write_data failed\n");
+	if (!sig_write_data(channel, queue, &sig_hdr, sig_hdr.head, msg))
 		return FALSE;
-	}
+
 	sig_hdr.num_sent++;
 
 	/* For each data field in SIGNAL_QUEUE_HEADER that was modified,
 	 * update host memory.
 	 */
 	mb(); /* required for channel synch */
-	if (!SIG_WRITE_FIELD(channel, queue, &sig_hdr, head)) {
-		ERRDRV("visor_memregion_write of Head failed\n");
+	if (!SIG_WRITE_FIELD(channel, queue, &sig_hdr, head))
 		return FALSE;
-	}
 	if (!SIG_WRITE_FIELD(channel, queue, &sig_hdr, num_sent)) {
-		ERRDRV("visor_memregion_write of NumSignalsSent failed\n");
 		return FALSE;
 	}
 
@@ -584,15 +560,12 @@ visorchannel_debug(struct visorchannel *channel, int num_queues,
 	int i = 0;
 	int errcode = 0;
 
-	if (channel == NULL) {
-		ERRDRV("%s no channel", __func__);
+	if (!channel)
 		return;
-	}
 	memregion = channel->memregion;
-	if (memregion == NULL) {
-		ERRDRV("%s no memregion", __func__);
+	if (!memregion)
 		return;
-	}
+
 	addr = visor_memregion_get_physaddr(memregion);
 	nbytes_region = visor_memregion_get_nbytes(memregion);
 	errcode = visorchannel_read(channel, off,
@@ -672,11 +645,8 @@ visorchannel_dump_section(struct visorchannel *chan, char *s,
 		goto fmt_failed;
 
 	errcode = visorchannel_read(chan, off, buf, len);
-	if (errcode < 0) {
-		ERRDRV("%s failed to read %s from channel errcode=%d",
-		       s, __func__, errcode);
+	if (errcode < 0)
 		goto read_failed;
-	}
 	seq_printf(seq, "channel %s:\n", s);
 	tbuf = buf;
 	while (len > 0) {
diff --git a/drivers/staging/unisys/visorchannel/visorchannel_main.c b/drivers/staging/unisys/visorchannel/visorchannel_main.c
index f4be2e62c97d..787d4774b199 100644
--- a/drivers/staging/unisys/visorchannel/visorchannel_main.c
+++ b/drivers/staging/unisys/visorchannel/visorchannel_main.c
@@ -32,14 +32,12 @@ visorchannel_init(void)
 	if (!unisys_spar_platform)
 		return -ENODEV;
 
-	INFODRV("driver version %s loaded", VERSION);
 	return 0;
 }
 
 static void
 visorchannel_exit(void)
 {
-	INFODRV("driver unloaded");
 }
 
 module_init(visorchannel_init);
diff --git a/drivers/staging/unisys/visorchipset/Kconfig b/drivers/staging/unisys/visorchipset/Kconfig
index e86836f84243..b03bfc5c3043 100644
--- a/drivers/staging/unisys/visorchipset/Kconfig
+++ b/drivers/staging/unisys/visorchipset/Kconfig
@@ -4,7 +4,8 @@
 
 config UNISYS_VISORCHIPSET
 	tristate "Unisys visorchipset driver"
-	depends on UNISYSSPAR && UNISYS_VISORUTIL && UNISYS_VISORCHANNEL && HAS_IOMEM
+	select UNISYS_VISORUTIL
+	select UNISYS_VISORCHANNEL
 	---help---
 	If you say Y here, you will enable the Unisys visorchipset driver.
 
diff --git a/drivers/staging/unisys/visorchipset/file.c b/drivers/staging/unisys/visorchipset/file.c
index e51fd4e3fa2d..203de0b5f607 100644
--- a/drivers/staging/unisys/visorchipset/file.c
+++ b/drivers/staging/unisys/visorchipset/file.c
@@ -30,74 +30,14 @@
 
 static struct cdev file_cdev;
 static struct visorchannel **file_controlvm_channel;
-static dev_t majordev = -1; /**< indicates major num for device */
-static BOOL registered = FALSE;
-
-static int visorchipset_open(struct inode *inode, struct file *file);
-static int visorchipset_release(struct inode *inode, struct file *file);
-static int visorchipset_mmap(struct file *file, struct vm_area_struct *vma);
-long visorchipset_ioctl(struct file *file, unsigned int cmd, unsigned long arg);
-
-static const struct file_operations visorchipset_fops = {
-	.owner = THIS_MODULE,
-	.open = visorchipset_open,
-	.read = NULL,
-	.write = NULL,
-	.unlocked_ioctl = visorchipset_ioctl,
-	.release = visorchipset_release,
-	.mmap = visorchipset_mmap,
-};
-
-int
-visorchipset_file_init(dev_t major_dev, struct visorchannel **controlvm_channel)
-{
-	int rc = 0;
-
-	file_controlvm_channel = controlvm_channel;
-	majordev = major_dev;
-	cdev_init(&file_cdev, &visorchipset_fops);
-	file_cdev.owner = THIS_MODULE;
-	if (MAJOR(majordev) == 0) {
-		/* dynamic major device number registration required */
-		if (alloc_chrdev_region(&majordev, 0, 1, MYDRVNAME) < 0) {
-			ERRDRV("Unable to allocate+register char device %s",
-			       MYDRVNAME);
-			return -1;
-		}
-		registered = TRUE;
-		INFODRV("New major number %d registered\n", MAJOR(majordev));
-	} else {
-		/* static major device number registration required */
-		if (register_chrdev_region(majordev, 1, MYDRVNAME) < 0) {
-			ERRDRV("Unable to register char device %s", MYDRVNAME);
-			return -1;
-		}
-		registered = TRUE;
-		INFODRV("Static major number %d registered\n", MAJOR(majordev));
-	}
-	rc = cdev_add(&file_cdev, MKDEV(MAJOR(majordev), 0), 1);
-	if (rc  < 0) {
-		ERRDRV("failed to create char device: (status=%d)\n", rc);
-		return -1;
-	}
-	INFODRV("Registered char device for %s (major=%d)",
-		MYDRVNAME, MAJOR(majordev));
-	return 0;
-}
 
 void
-visorchipset_file_cleanup(void)
+visorchipset_file_cleanup(dev_t major_dev)
 {
 	if (file_cdev.ops != NULL)
 		cdev_del(&file_cdev);
 	file_cdev.ops = NULL;
-	if (registered) {
-		if (MAJOR(majordev) >= 0) {
-			unregister_chrdev_region(majordev, 1);
-			majordev = MKDEV(0, 0);
-		}
-		registered = FALSE;
-	}
+	unregister_chrdev_region(major_dev, 1);
 }
 
 static int
@@ -105,7 +45,6 @@ visorchipset_open(struct inode *inode, struct file *file)
 {
 	unsigned minor_number = iminor(inode);
 
-	DEBUGDRV("%s", __func__);
 	if (minor_number != 0)
 		return -ENODEV;
 	file->private_data = NULL;
@@ -115,7 +54,6 @@ visorchipset_open(struct inode *inode, struct file *file)
 static int
 visorchipset_release(struct inode *inode, struct file *file)
 {
-	DEBUGDRV("%s", __func__);
 	return 0;
 }
 
@@ -127,16 +65,13 @@ visorchipset_mmap(struct file *file, struct vm_area_struct *vma)
 	GUEST_PHYSICAL_ADDRESS addr = 0;
 
 	/* sv_enable_dfp(); */
-	DEBUGDRV("%s", __func__);
-	if (offset & (PAGE_SIZE - 1)) {
-		ERRDRV("%s virtual address NOT page-aligned!", __func__);
+	if (offset & (PAGE_SIZE - 1))
 		return -ENXIO;	/* need aligned offsets */
-	}
+
 	switch (offset) {
 	case VISORCHIPSET_MMAP_CONTROLCHANOFFSET:
 		vma->vm_flags |= VM_IO;
 		if (*file_controlvm_channel == NULL) {
-			ERRDRV("%s no controlvm channel yet", __func__);
 			return -ENXIO;
 		}
 		visorchannel_read(*file_controlvm_channel,
@@ -144,33 +79,29 @@ visorchipset_mmap(struct file *file, struct vm_area_struct *vma)
 				 gp_control_channel),
 			&addr, sizeof(addr));
 		if (addr == 0) {
-			ERRDRV("%s control channel address is 0", __func__);
 			return -ENXIO;
 		}
 		physaddr = (ulong)addr;
-		DEBUGDRV("mapping physical address = 0x%lx", physaddr);
 		if (remap_pfn_range(vma, vma->vm_start,
 				    physaddr >> PAGE_SHIFT,
 				    vma->vm_end - vma->vm_start,
 				    /*pgprot_noncached */
 				    (vma->vm_page_prot))) {
-			ERRDRV("%s remap_pfn_range failed", __func__);
 			return -EAGAIN;
 		}
 		break;
 	default:
 		return -ENOSYS;
 	}
-	DEBUGDRV("%s success!", __func__);
 	return 0;
 }
 
-long visorchipset_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
+static long visorchipset_ioctl(struct file *file, unsigned int cmd,
+				unsigned long arg)
 {
 	s64 adjustment;
 	s64 vrtc_offset;
 
-	DBGINF("entered visorchipset_ioctl, cmd=%d", cmd);
 	switch (cmd) {
 	case VMCALL_QUERY_GUEST_VIRTUAL_TIME_OFFSET:
 		/* get the physical rtc offset */
@@ -179,19 +110,51 @@ long visorchipset_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 		    ((void __user *)arg, &vrtc_offset, sizeof(vrtc_offset))) {
 			return -EFAULT;
 		}
-		DBGINF("insde visorchipset_ioctl, cmd=%d, vrtc_offset=%lld",
-		       cmd, vrtc_offset);
 		return SUCCESS;
 	case VMCALL_UPDATE_PHYSICAL_TIME:
 		if (copy_from_user
 		    (&adjustment, (void __user *)arg, sizeof(adjustment))) {
 			return -EFAULT;
 		}
-		DBGINF("insde visorchipset_ioctl, cmd=%d, adjustment=%lld", cmd,
-		       adjustment);
 		return issue_vmcall_update_physical_time(adjustment);
 	default:
-		LOGERR("visorchipset_ioctl received invalid command");
 		return -EFAULT;
 	}
 }
+
+static const struct file_operations visorchipset_fops = {
+	.owner = THIS_MODULE,
+	.open = visorchipset_open,
+	.read = NULL,
+	.write = NULL,
+	.unlocked_ioctl = visorchipset_ioctl,
+	.release = visorchipset_release,
+	.mmap = visorchipset_mmap,
+};
+
+int
+visorchipset_file_init(dev_t major_dev, struct visorchannel **controlvm_channel)
+{
+	int rc = 0;
+
+	file_controlvm_channel = controlvm_channel;
+	cdev_init(&file_cdev, &visorchipset_fops);
+	file_cdev.owner = THIS_MODULE;
+	if (MAJOR(major_dev) == 0) {
+		rc = alloc_chrdev_region(&major_dev, 0, 1, MYDRVNAME);
+		/* dynamic major device number registration required */
+		if (rc < 0)
+			return rc;
+	} else {
+		/* static major device number registration required */
+		rc = register_chrdev_region(major_dev, 1, MYDRVNAME);
+		if (rc < 0)
+			return rc;
+	}
+	rc = cdev_add(&file_cdev, MKDEV(MAJOR(major_dev), 0), 1);
+	if (rc < 0) {
+		unregister_chrdev_region(major_dev, 1);
+		return rc;
+	}
+	return 0;
+}
diff --git a/drivers/staging/unisys/visorchipset/file.h b/drivers/staging/unisys/visorchipset/file.h
index dc7a19556b3f..51f7699b744b 100644
--- a/drivers/staging/unisys/visorchipset/file.h
+++ b/drivers/staging/unisys/visorchipset/file.h
@@ -22,6 +22,6 @@
 
 int visorchipset_file_init(dev_t majorDev,
 			   struct visorchannel **pControlVm_channel);
-void visorchipset_file_cleanup(void);
+void visorchipset_file_cleanup(dev_t major_dev);
 
 #endif
diff --git a/drivers/staging/unisys/visorchipset/globals.h b/drivers/staging/unisys/visorchipset/globals.h
index a1d35d4bef2e..f76e498a36b5 100644
--- a/drivers/staging/unisys/visorchipset/globals.h
+++ b/drivers/staging/unisys/visorchipset/globals.h
@@ -18,7 +18,6 @@
 #ifndef __VISORCHIPSET_GLOBALS_H__
 #define __VISORCHIPSET_GLOBALS_H__
 
-#include "uniklog.h"
 #include "diagnostics/appos_subsystems.h"
 #include "timskmod.h"
 #include "visorchipset.h"
diff --git a/drivers/staging/unisys/visorchipset/parser.c b/drivers/staging/unisys/visorchipset/parser.c
index 9edbd3bbd186..d8a2d6f5a75d 100644
--- a/drivers/staging/unisys/visorchipset/parser.c
+++ b/drivers/staging/unisys/visorchipset/parser.c
@@ -29,9 +29,9 @@
  * incoming payloads.  This serves as a throttling mechanism.
  */
 #define MAX_CONTROLVM_PAYLOAD_BYTES (1024*128)
-static ulong Controlvm_Payload_Bytes_Buffered;
+static ulong controlvm_payload_bytes_buffered;
 
-struct PARSER_CONTEXT_Tag {
+struct parser_context {
 	ulong allocbytes;
 	ulong param_bytes;
 	u8 *curr;
@@ -40,41 +40,36 @@ struct PARSER_CONTEXT_Tag {
 	char data[0];
 };
 
-static PARSER_CONTEXT *
-parser_init_guts(u64 addr, u32 bytes, BOOL isLocal,
-		 BOOL hasStandardPayloadHeader, BOOL *tryAgain)
+static struct parser_context *
+parser_init_guts(u64 addr, u32 bytes, BOOL local,
+		 BOOL standard_payload_header, BOOL *retry)
 {
-	int allocbytes = sizeof(PARSER_CONTEXT) + bytes;
-	PARSER_CONTEXT *rc = NULL;
-	PARSER_CONTEXT *ctx = NULL;
+	int allocbytes = sizeof(struct parser_context) + bytes;
+	struct parser_context *rc = NULL;
+	struct parser_context *ctx = NULL;
 	struct memregion *rgn = NULL;
 	struct spar_controlvm_parameters_header *phdr = NULL;
 
-	if (tryAgain)
-		*tryAgain = FALSE;
-	if (!hasStandardPayloadHeader)
+	if (retry)
+		*retry = FALSE;
+	if (!standard_payload_header)
 		/* alloc and 0 extra byte to ensure payload is
 		 * '\0'-terminated
 		 */
 		allocbytes++;
-	if ((Controlvm_Payload_Bytes_Buffered + bytes)
+	if ((controlvm_payload_bytes_buffered + bytes)
 	    > MAX_CONTROLVM_PAYLOAD_BYTES) {
-		ERRDRV("%s (%s:%d) - prevented allocation of %d bytes to prevent exceeding throttling max (%d)",
-		       __func__, __FILE__, __LINE__, allocbytes,
-		       MAX_CONTROLVM_PAYLOAD_BYTES);
-		if (tryAgain)
-			*tryAgain = TRUE;
+		if (retry)
+			*retry = TRUE;
 		rc = NULL;
-		goto Away;
+		goto cleanup;
 	}
 	ctx = kzalloc(allocbytes, GFP_KERNEL|__GFP_NORETRY);
-	if (ctx == NULL) {
-		ERRDRV("%s (%s:%d) - failed to allocate %d bytes",
-		       __func__, __FILE__, __LINE__, allocbytes);
-		if (tryAgain)
-			*tryAgain = TRUE;
+	if (!ctx) {
+		if (retry)
+			*retry = TRUE;
 		rc = NULL;
-		goto Away;
+		goto cleanup;
 	}
 
 	ctx->allocbytes = allocbytes;
@@ -82,15 +77,12 @@ parser_init_guts(u64 addr, u32 bytes, BOOL isLocal,
 	ctx->curr = NULL;
 	ctx->bytes_remaining = 0;
 	ctx->byte_stream = FALSE;
-	if (isLocal) {
+	if (local) {
 		void *p;
 
 		if (addr > virt_to_phys(high_memory - 1)) {
-			ERRDRV("%s - bad local address (0x%-16.16Lx for %lu)",
-			       __func__,
-			       (unsigned long long) addr, (ulong) bytes);
 			rc = NULL;
-			goto Away;
+			goto cleanup;
 		}
 		p = __va((ulong) (addr));
 		memcpy(ctx->data, p, bytes);
@@ -98,54 +90,42 @@ parser_init_guts(u64 addr, u32 bytes, BOOL isLocal,
 		rgn = visor_memregion_create(addr, bytes);
 		if (!rgn) {
 			rc = NULL;
-			goto Away;
+			goto cleanup;
 		}
 		if (visor_memregion_read(rgn, 0, ctx->data, bytes) < 0) {
 			rc = NULL;
-			goto Away;
+			goto cleanup;
 		}
 	}
-	if (!hasStandardPayloadHeader) {
+	if (!standard_payload_header) {
 		ctx->byte_stream = TRUE;
 		rc = ctx;
-		goto Away;
+		goto cleanup;
 	}
 	phdr = (struct spar_controlvm_parameters_header *)(ctx->data);
 	if (phdr->total_length != bytes) {
-		ERRDRV("%s - bad total length %lu (should be %lu)",
-		       __func__,
-		       (ulong) (phdr->total_length), (ulong) (bytes));
 		rc = NULL;
-		goto Away;
+		goto cleanup;
 	}
 	if (phdr->total_length < phdr->header_length) {
-		ERRDRV("%s - total length < header length (%lu < %lu)",
-		       __func__,
-		       (ulong) (phdr->total_length),
-		       (ulong) (phdr->header_length));
 		rc = NULL;
-		goto Away;
+		goto cleanup;
 	}
 	if (phdr->header_length <
 	    sizeof(struct spar_controlvm_parameters_header)) {
-		ERRDRV("%s - header is too small (%lu < %lu)",
-		       __func__,
-		       (ulong) (phdr->header_length),
-		       (ulong)(sizeof(
-				struct spar_controlvm_parameters_header)));
 		rc = NULL;
-		goto Away;
+		goto cleanup;
 	}
 
 	rc = ctx;
-Away:
+cleanup:
 	if (rgn) {
 		visor_memregion_destroy(rgn);
 		rgn = NULL;
 	}
-	if (rc)
-		Controlvm_Payload_Bytes_Buffered += ctx->param_bytes;
-	else {
+	if (rc) {
+		controlvm_payload_bytes_buffered += ctx->param_bytes;
+	} else {
 		if (ctx) {
 			parser_done(ctx);
 			ctx = NULL;
@@ -154,10 +134,10 @@ Away:
 	return rc;
 }
 
-PARSER_CONTEXT *
-parser_init(u64 addr, u32 bytes, BOOL isLocal, BOOL *tryAgain)
+struct parser_context *
+parser_init(u64 addr, u32 bytes, BOOL local, BOOL *retry)
 {
-	return parser_init_guts(addr, bytes, isLocal, TRUE, tryAgain);
+	return parser_init_guts(addr, bytes, local, TRUE, retry);
 }
 
 /* Call this instead of parser_init() if the payload area consists of just
@@ -165,16 +145,16 @@ parser_init(u64 addr, u32 bytes, BOOL isLocal, BOOL *tryAgain)
  * structures.  Afterwards, you can call parser_simpleString_get() or
  * parser_byteStream_get() to obtain the data.
  */
-PARSER_CONTEXT *
-parser_init_byteStream(u64 addr, u32 bytes, BOOL isLocal, BOOL *tryAgain)
+struct parser_context *
+parser_init_byte_stream(u64 addr, u32 bytes, BOOL local, BOOL *retry)
 {
-	return parser_init_guts(addr, bytes, isLocal, FALSE, tryAgain);
+	return parser_init_guts(addr, bytes, local, FALSE, retry);
 }
 
 /* Obtain '\0'-terminated copy of string in payload area.
  */
 char *
-parser_simpleString_get(PARSER_CONTEXT *ctx)
+parser_simpleString_get(struct parser_context *ctx)
 {
 	if (!ctx->byte_stream)
 		return NULL;
@@ -185,40 +165,33 @@ parser_simpleString_get(PARSER_CONTEXT *ctx)
 
 /* Obtain a copy of the buffer in the payload area.
  */
-void *
-parser_byteStream_get(PARSER_CONTEXT *ctx, ulong *nbytes)
+void *parser_byte_stream_get(struct parser_context *ctx, ulong *nbytes)
 {
 	if (!ctx->byte_stream)
 		return NULL;
 	if (nbytes)
 		*nbytes = ctx->param_bytes;
-	return (void *) ctx->data;
+	return (void *)ctx->data;
 }
 
 uuid_le
-parser_id_get(PARSER_CONTEXT *ctx)
+parser_id_get(struct parser_context *ctx)
 {
 	struct spar_controlvm_parameters_header *phdr = NULL;
 
-	if (ctx == NULL) {
-		ERRDRV("%s (%s:%d) - no context",
-		       __func__, __FILE__, __LINE__);
+	if (ctx == NULL)
 		return NULL_UUID_LE;
-	}
 	phdr = (struct spar_controlvm_parameters_header *)(ctx->data);
 	return phdr->id;
 }
 
 void
-parser_param_start(PARSER_CONTEXT *ctx, PARSER_WHICH_STRING which_string)
+parser_param_start(struct parser_context *ctx, PARSER_WHICH_STRING which_string)
 {
 	struct spar_controlvm_parameters_header *phdr = NULL;
 
-	if (ctx == NULL) {
-		ERRDRV("%s (%s:%d) - no context",
-		       __func__, __FILE__, __LINE__);
+	if (ctx == NULL)
 		goto Away;
-	}
 	phdr = (struct spar_controlvm_parameters_header *)(ctx->data);
 	switch (which_string) {
 	case PARSERSTRING_INITIATOR:
@@ -238,7 +211,6 @@ parser_param_start(PARSER_CONTEXT *ctx, PARSER_WHICH_STRING which_string)
 		ctx->bytes_remaining = phdr->name_length;
 		break;
 	default:
-		ERRDRV("%s - bad which_string %d", __func__, which_string);
 		break;
 	}
 
@@ -247,11 +219,11 @@ Away:
 }
 
 void
-parser_done(PARSER_CONTEXT *ctx)
+parser_done(struct parser_context *ctx)
 {
 	if (!ctx)
 		return;
-	Controlvm_Payload_Bytes_Buffered -= ctx->param_bytes;
+	controlvm_payload_bytes_buffered -= ctx->param_bytes;
 	kfree(ctx);
 }
 
@@ -290,7 +262,7 @@ string_length_no_trail(char *s, int len)
  *    parameter
  */
 void *
-parser_param_get(PARSER_CONTEXT *ctx, char *nam, int namesize)
+parser_param_get(struct parser_context *ctx, char *nam, int namesize)
 {
 	u8 *pscan, *pnam = nam;
 	ulong nscan;
@@ -321,25 +293,18 @@ parser_param_get(PARSER_CONTEXT *ctx, char *nam, int namesize)
 	}
 
 	while (*pscan != ':') {
-		if (namesize <= 0) {
-			ERRDRV("%s - name too big", __func__);
+		if (namesize <= 0)
 			return NULL;
-		}
 		*pnam = toupper(*pscan);
 		pnam++;
 		namesize--;
 		pscan++;
 		nscan--;
-		if (nscan == 0) {
-			ERRDRV("%s - unexpected end of input parsing name",
-			       __func__);
+		if (nscan == 0)
 			return NULL;
-		}
 	}
-	if (namesize <= 0) {
-		ERRDRV("%s - name too big", __func__);
+	if (namesize <= 0)
 		return NULL;
-	}
 	*pnam = '\0';
 	nam[string_length_no_trail(nam, strlen(nam))] = '\0';
 
@@ -350,26 +315,17 @@ parser_param_get(PARSER_CONTEXT *ctx, char *nam, int namesize)
 	while (isspace(*pscan)) {
 		pscan++;
 		nscan--;
-		if (nscan == 0) {
-			ERRDRV("%s - unexpected end of input looking for value",
-			       __func__);
+		if (nscan == 0)
 			return NULL;
-		}
 	}
-	if (nscan == 0) {
-		ERRDRV("%s - unexpected end of input looking for value",
-		       __func__);
+	if (nscan == 0)
 		return NULL;
-	}
 	if (*pscan == '\'' || *pscan == '"') {
 		closing_quote = *pscan;
 		pscan++;
 		nscan--;
-		if (nscan == 0) {
-			ERRDRV("%s - unexpected end of input after %c",
-			       __func__, closing_quote);
+		if (nscan == 0)
 			return NULL;
-		}
 	}
 
 	/* look for a separator character, terminator character, or
@@ -377,10 +333,8 @@ parser_param_get(PARSER_CONTEXT *ctx, char *nam, int namesize)
 	 */
 	for (i = 0, value_length = -1; i < nscan; i++) {
 		if (closing_quote) {
-			if (pscan[i] == '\0') {
-				ERRDRV("%s - unexpected end of input parsing quoted value", __func__);
+			if (pscan[i] == '\0')
 				return NULL;
-			}
 			if (pscan[i] == closing_quote) {
 				value_length = i;
 				break;
@@ -393,10 +347,8 @@ parser_param_get(PARSER_CONTEXT *ctx, char *nam, int namesize)
 		}
 	}
 	if (value_length < 0) {
-		if (closing_quote) {
-			ERRDRV("%s - unexpected end of input parsing quoted value", __func__);
+		if (closing_quote)
 			return NULL;
-		}
 		value_length = nscan;
 	}
 	orig_value_length = value_length;
@@ -433,7 +385,6 @@ parser_param_get(PARSER_CONTEXT *ctx, char *nam, int namesize)
 				pscan++;
 				nscan--;
 			} else if (*pscan != '\0') {
-				ERRDRV("%s - missing separator after quoted string", __func__);
 				kfree(value);
 				value = NULL;
 				return NULL;
@@ -446,7 +397,7 @@ parser_param_get(PARSER_CONTEXT *ctx, char *nam, int namesize)
 }
 
 void *
-parser_string_get(PARSER_CONTEXT *ctx)
+parser_string_get(struct parser_context *ctx)
 {
 	u8 *pscan;
 	ulong nscan;
diff --git a/drivers/staging/unisys/visorchipset/parser.h b/drivers/staging/unisys/visorchipset/parser.h
index 9fbe3b5b7cc3..2b903f1beff2 100644
--- a/drivers/staging/unisys/visorchipset/parser.h
+++ b/drivers/staging/unisys/visorchipset/parser.h
@@ -20,7 +20,6 @@
 
 #include <linux/uuid.h>
 
-#include "uniklog.h"
 #include "timskmod.h"
 #include "channel.h"
 
@@ -31,17 +30,17 @@ typedef enum {
 	PARSERSTRING_NAME,
 } PARSER_WHICH_STRING;
 
-typedef struct PARSER_CONTEXT_Tag PARSER_CONTEXT;
-
-PARSER_CONTEXT *parser_init(u64 addr, u32 bytes, BOOL isLocal, BOOL *tryAgain);
-PARSER_CONTEXT *parser_init_byteStream(u64 addr, u32 bytes, BOOL isLocal,
-				       BOOL *tryAgain);
-void parser_param_start(PARSER_CONTEXT *ctx, PARSER_WHICH_STRING which_string);
-void *parser_param_get(PARSER_CONTEXT *ctx, char *nam, int namesize);
-void *parser_string_get(PARSER_CONTEXT *ctx);
-uuid_le parser_id_get(PARSER_CONTEXT *ctx);
-char *parser_simpleString_get(PARSER_CONTEXT *ctx);
-void *parser_byteStream_get(PARSER_CONTEXT *ctx, ulong *nbytes);
-void parser_done(PARSER_CONTEXT *ctx);
+struct parser_context *parser_init(u64 addr, u32 bytes, BOOL isLocal,
+				   BOOL *tryAgain);
+struct parser_context *parser_init_byte_stream(u64 addr, u32 bytes, BOOL local,
+				       BOOL *retry);
+void parser_param_start(struct parser_context *ctx,
+			PARSER_WHICH_STRING which_string);
+void *parser_param_get(struct parser_context *ctx, char *nam, int namesize);
+void *parser_string_get(struct parser_context *ctx);
+uuid_le parser_id_get(struct parser_context *ctx);
+char *parser_simpleString_get(struct parser_context *ctx);
+void *parser_byte_stream_get(struct parser_context *ctx, ulong *nbytes);
+void parser_done(struct parser_context *ctx);
 
 #endif
diff --git a/drivers/staging/unisys/visorchipset/visorchipset.h b/drivers/staging/unisys/visorchipset/visorchipset.h
index 98f3ba4c13ac..bd46df9ef45a 100644
--- a/drivers/staging/unisys/visorchipset/visorchipset.h
+++ b/drivers/staging/unisys/visorchipset/visorchipset.h
@@ -133,7 +133,6 @@ struct visorchipset_bus_info {
 	u8 *description;	/* UTF8 */
 	u64 reserved1;
 	u32 reserved2;
-	MYPROCOBJECT *proc_object;
 	struct {
 		u32 server:1;
 		/* Add new fields above. */
diff --git a/drivers/staging/unisys/visorchipset/visorchipset_main.c b/drivers/staging/unisys/visorchipset/visorchipset_main.c
index f606ee9e0de9..f2663d2c7530 100644
--- a/drivers/staging/unisys/visorchipset/visorchipset_main.c
+++ b/drivers/staging/unisys/visorchipset/visorchipset_main.c
@@ -22,7 +22,6 @@
 #include "periodic_work.h"
 #include "file.h"
 #include "parser.h"
-#include "uniklog.h"
 #include "uisutils.h"
 #include "controlvmcompletionstatus.h"
 #include "guestlinuxdebug.h"
@@ -49,8 +48,8 @@
 * message, we switch back to fast polling mode.
 */
 #define MIN_IDLE_SECONDS 10
-static ulong Poll_jiffies = POLLJIFFIES_CONTROLVMCHANNEL_FAST;
-static ulong Most_recent_message_jiffies;	/* when we got our last
+static ulong poll_jiffies = POLLJIFFIES_CONTROLVMCHANNEL_FAST;
+static ulong most_recent_message_jiffies;	/* when we got our last
 						 * controlvm message */
 static inline char *
 NONULLSTR(char *s)
@@ -66,83 +65,74 @@ static int clientregistered;
 #define MAX_CHIPSET_EVENTS 2
 static u8 chipset_events[MAX_CHIPSET_EVENTS] = { 0, 0 };
 
-static struct delayed_work Periodic_controlvm_work;
-static struct workqueue_struct *Periodic_controlvm_workqueue;
-static DEFINE_SEMAPHORE(NotifierLock);
+static struct delayed_work periodic_controlvm_work;
+static struct workqueue_struct *periodic_controlvm_workqueue;
+static DEFINE_SEMAPHORE(notifier_lock);
 
-typedef struct {
-	struct controlvm_message message;
-	unsigned int crc;
-} MESSAGE_ENVELOPE;
-
-static struct controlvm_message_header g_DiagMsgHdr;
-static struct controlvm_message_header g_ChipSetMsgHdr;
-static struct controlvm_message_header g_DelDumpMsgHdr;
-static const uuid_le UltraDiagPoolChannelProtocolGuid =
+static struct controlvm_message_header g_diag_msg_hdr;
+static struct controlvm_message_header g_chipset_msg_hdr;
+static struct controlvm_message_header g_del_dump_msg_hdr;
+static const uuid_le spar_diag_pool_channel_protocol_uuid =
 	SPAR_DIAG_POOL_CHANNEL_PROTOCOL_UUID;
 /* 0xffffff is an invalid Bus/Device number */
-static ulong g_diagpoolBusNo = 0xffffff;
-static ulong g_diagpoolDevNo = 0xffffff;
-static struct controlvm_message_packet g_DeviceChangeStatePacket;
+static ulong g_diagpool_bus_no = 0xffffff;
+static ulong g_diagpool_dev_no = 0xffffff;
+static struct controlvm_message_packet g_devicechangestate_packet;
 
 /* Only VNIC and VHBA channels are sent to visorclientbus (aka
  * "visorhackbus")
  */
 #define FOR_VISORHACKBUS(channel_type_guid) \
 	(((uuid_le_cmp(channel_type_guid,\
-		       spar_vnic_channel_protocol_uuid) == 0)\
-	|| (uuid_le_cmp(channel_type_guid,\
+		       spar_vnic_channel_protocol_uuid) == 0) ||\
+	(uuid_le_cmp(channel_type_guid,\
 			spar_vhba_channel_protocol_uuid) == 0)))
 #define FOR_VISORBUS(channel_type_guid) (!(FOR_VISORHACKBUS(channel_type_guid)))
 
 #define is_diagpool_channel(channel_type_guid) \
-	 (uuid_le_cmp(channel_type_guid, UltraDiagPoolChannelProtocolGuid) == 0)
+	(uuid_le_cmp(channel_type_guid,\
+		     spar_diag_pool_channel_protocol_uuid) == 0)
 
-static LIST_HEAD(BusInfoList);
-static LIST_HEAD(DevInfoList);
+static LIST_HEAD(bus_info_list);
+static LIST_HEAD(dev_info_list);
 
-static struct visorchannel *ControlVm_channel;
+static struct visorchannel *controlvm_channel;
 
-typedef struct {
+/* Manages the request payload in the controlvm channel */
+static struct controlvm_payload_info {
 	u8 __iomem *ptr;	/* pointer to base address of payload pool */
 	u64 offset;		/* offset from beginning of controlvm
 				 * channel to beginning of payload * pool */
 	u32 bytes;		/* number of bytes in payload pool */
-} CONTROLVM_PAYLOAD_INFO;
-
-/* Manages the request payload in the controlvm channel */
-static CONTROLVM_PAYLOAD_INFO ControlVm_payload_info;
+} controlvm_payload_info;
 
-static struct channel_header *Test_Vnic_channel;
-
-typedef struct {
-	struct controlvm_message_header Dumpcapture_header;
-	struct controlvm_message_header Gettextdump_header;
-	struct controlvm_message_header Dumpcomplete_header;
-	BOOL Gettextdump_outstanding;
+/* Manages the info for a CONTROLVM_DUMP_CAPTURESTATE /
+ * CONTROLVM_DUMP_GETTEXTDUMP / CONTROLVM_DUMP_COMPLETE conversation.
+ */
+static struct livedump_info {
+	struct controlvm_message_header dumpcapture_header;
+	struct controlvm_message_header gettextdump_header;
+	struct controlvm_message_header dumpcomplete_header;
+	BOOL gettextdump_outstanding;
 	u32 crc32;
 	ulong length;
 	atomic_t buffers_in_use;
 	ulong destination;
-} LIVEDUMP_INFO;
-/* Manages the info for a CONTROLVM_DUMP_CAPTURESTATE /
- * CONTROLVM_DUMP_GETTEXTDUMP / CONTROLVM_DUMP_COMPLETE conversation.
- */
-static LIVEDUMP_INFO LiveDump_info;
+} livedump_info;
 
 /* The following globals are used to handle the scenario where we are unable to
  * offload the payload from a controlvm message due to memory requirements.  In
  * this scenario, we simply stash the controlvm message, then attempt to
  * process it again the next time controlvm_periodic_work() runs.
  */
-static struct controlvm_message ControlVm_Pending_Msg;
-static BOOL ControlVm_Pending_Msg_Valid = FALSE;
+static struct controlvm_message controlvm_pending_msg;
+static BOOL controlvm_pending_msg_valid = FALSE;
 
 /* Pool of struct putfile_buffer_entry, for keeping track of pending (incoming)
  * TRANSMIT_FILE PutFile payloads.
  */
-static struct kmem_cache *Putfile_buffer_list_pool;
-static const char Putfile_buffer_list_pool_name[] =
+static struct kmem_cache *putfile_buffer_list_pool;
+static const char putfile_buffer_list_pool_name[] =
 	"controlvm_putfile_buffer_list_pool";
 
 /* This identifies a data buffer that has been received via a controlvm messages
@@ -150,14 +140,14 @@ static const char Putfile_buffer_list_pool_name[] =
  */
 struct putfile_buffer_entry {
 	struct list_head next;	/* putfile_buffer_entry list */
-	PARSER_CONTEXT *parser_ctx; /* points to buffer containing input data */
+	struct parser_context *parser_ctx; /* points to input data buffer */
 };
 
 /* List of struct putfile_request *, via next_putfile_request member.
  * Each entry in this list identifies an outstanding TRANSMIT_FILE
  * conversation.
  */
-static LIST_HEAD(Putfile_request_list);
+static LIST_HEAD(putfile_request_list);
 
 /* This describes a buffer and its current state of transfer (e.g., how many
  * bytes have already been supplied as putfile data, and how many bytes are
@@ -165,7 +155,7 @@ static LIST_HEAD(Putfile_request_list);
  */
 struct putfile_active_buffer {
 	/* a payload from a controlvm message, containing a file data buffer */
-	PARSER_CONTEXT *parser_ctx;
+	struct parser_context *parser_ctx;
 	/* points within data area of parser_ctx to next byte of data */
 	u8 *pnext;
 	/* # bytes left from <pnext> to the end of this data buffer */
@@ -213,7 +203,7 @@ struct putfile_request {
 	int completion_status;
 };
 
-static atomic_t Visorchipset_cache_buffers_in_use = ATOMIC_INIT(0);
+static atomic_t visorchipset_cache_buffers_in_use = ATOMIC_INIT(0);
 
 struct parahotplug_request {
 	struct list_head list;
@@ -222,23 +212,23 @@ struct parahotplug_request {
 	struct controlvm_message msg;
 };
 
-static LIST_HEAD(Parahotplug_request_list);
-static DEFINE_SPINLOCK(Parahotplug_request_list_lock);	/* lock for above */
+static LIST_HEAD(parahotplug_request_list);
+static DEFINE_SPINLOCK(parahotplug_request_list_lock);	/* lock for above */
 static void parahotplug_process_list(void);
 
 /* Manages the info for a CONTROLVM_DUMP_CAPTURESTATE /
  * CONTROLVM_REPORTEVENT.
  */
-static struct visorchipset_busdev_notifiers BusDev_Server_Notifiers;
-static struct visorchipset_busdev_notifiers BusDev_Client_Notifiers;
+static struct visorchipset_busdev_notifiers busdev_server_notifiers;
+static struct visorchipset_busdev_notifiers busdev_client_notifiers;
 
-static void bus_create_response(ulong busNo, int response);
-static void bus_destroy_response(ulong busNo, int response);
-static void device_create_response(ulong busNo, ulong devNo, int response);
-static void device_destroy_response(ulong busNo, ulong devNo, int response);
-static void device_resume_response(ulong busNo, ulong devNo, int response);
+static void bus_create_response(ulong bus_no, int response);
+static void bus_destroy_response(ulong bus_no, int response);
+static void device_create_response(ulong bus_no, ulong dev_no, int response);
+static void device_destroy_response(ulong bus_no, ulong dev_no, int response);
+static void device_resume_response(ulong bus_no, ulong dev_no, int response);
 
-static struct visorchipset_busdev_responders BusDev_Responders = {
+static struct visorchipset_busdev_responders busdev_responders = {
 	.bus_create = bus_create_response,
 	.bus_destroy = bus_destroy_response,
 	.device_create = device_create_response,
@@ -248,49 +238,55 @@ static struct visorchipset_busdev_responders BusDev_Responders = {
 };
 
 /* info for /dev/visorchipset */
-static dev_t MajorDev = -1; /**< indicates major num for device */
+static dev_t major_dev = -1; /**< indicates major num for device */
 
 /* prototypes for attributes */
 static ssize_t toolaction_show(struct device *dev,
-	struct device_attribute *attr, char *buf);
+			       struct device_attribute *attr, char *buf);
 static ssize_t toolaction_store(struct device *dev,
-	struct device_attribute *attr, const char *buf, size_t count);
+				struct device_attribute *attr,
+				const char *buf, size_t count);
 static DEVICE_ATTR_RW(toolaction);
 
 static ssize_t boottotool_show(struct device *dev,
-	struct device_attribute *attr, char *buf);
+			       struct device_attribute *attr, char *buf);
 static ssize_t boottotool_store(struct device *dev,
-	struct device_attribute *attr, const char *buf, size_t count);
+				struct device_attribute *attr, const char *buf,
+				size_t count);
 static DEVICE_ATTR_RW(boottotool);
 
 static ssize_t error_show(struct device *dev, struct device_attribute *attr,
-	char *buf);
+			  char *buf);
 static ssize_t error_store(struct device *dev, struct device_attribute *attr,
-	const char *buf, size_t count);
+			   const char *buf, size_t count);
 static DEVICE_ATTR_RW(error);
 
 static ssize_t textid_show(struct device *dev, struct device_attribute *attr,
-	char *buf);
+			   char *buf);
 static ssize_t textid_store(struct device *dev, struct device_attribute *attr,
-	const char *buf, size_t count);
+			    const char *buf, size_t count);
 static DEVICE_ATTR_RW(textid);
 
 static ssize_t remaining_steps_show(struct device *dev,
-	struct device_attribute *attr, char *buf);
+				    struct device_attribute *attr, char *buf);
 static ssize_t remaining_steps_store(struct device *dev,
-	struct device_attribute *attr, const char *buf, size_t count);
+				     struct device_attribute *attr,
+				     const char *buf, size_t count);
 static DEVICE_ATTR_RW(remaining_steps);
 
 static ssize_t chipsetready_store(struct device *dev,
-		struct device_attribute *attr, const char *buf, size_t count);
+				  struct device_attribute *attr,
+				  const char *buf, size_t count);
 static DEVICE_ATTR_WO(chipsetready);
 
 static ssize_t devicedisabled_store(struct device *dev,
-		struct device_attribute *attr, const char *buf, size_t count);
+				    struct device_attribute *attr,
+				    const char *buf, size_t count);
 static DEVICE_ATTR_WO(devicedisabled);
 
 static ssize_t deviceenabled_store(struct device *dev,
-		struct device_attribute *attr, const char *buf, size_t count);
+				   struct device_attribute *attr,
+				   const char *buf, size_t count);
 static DEVICE_ATTR_WO(deviceenabled);
 
 static struct attribute *visorchipset_install_attrs[] = {
@@ -336,47 +332,48 @@ static const struct attribute_group *visorchipset_dev_groups[] = {
 };
 
 /* /sys/devices/platform/visorchipset */
-static struct platform_device Visorchipset_platform_device = {
+static struct platform_device visorchipset_platform_device = {
 	.name = "visorchipset",
 	.id = -1,
 	.dev.groups = visorchipset_dev_groups,
 };
 
 /* Function prototypes */
-static void controlvm_respond(struct controlvm_message_header *msgHdr,
+static void controlvm_respond(struct controlvm_message_header *msg_hdr,
 			      int response);
 static void controlvm_respond_chipset_init(
-		struct controlvm_message_header *msgHdr, int response,
+		struct controlvm_message_header *msg_hdr, int response,
 		enum ultra_chipset_feature features);
 static void controlvm_respond_physdev_changestate(
-		struct controlvm_message_header *msgHdr, int response,
+		struct controlvm_message_header *msg_hdr, int response,
 		struct spar_segment_state state);
 
 static ssize_t toolaction_show(struct device *dev,
 			       struct device_attribute *attr,
 			       char *buf)
 {
-	u8 toolAction;
+	u8 tool_action;
 
-	visorchannel_read(ControlVm_channel,
+	visorchannel_read(controlvm_channel,
 		offsetof(struct spar_controlvm_channel_protocol,
-			   tool_action), &toolAction, sizeof(u8));
-	return scnprintf(buf, PAGE_SIZE, "%u\n", toolAction);
+			 tool_action), &tool_action, sizeof(u8));
+	return scnprintf(buf, PAGE_SIZE, "%u\n", tool_action);
 }
 
 static ssize_t toolaction_store(struct device *dev,
 				struct device_attribute *attr,
 				const char *buf, size_t count)
 {
-	u8 toolAction;
+	u8 tool_action;
 	int ret;
 
-	if (kstrtou8(buf, 10, &toolAction) != 0)
+	if (kstrtou8(buf, 10, &tool_action) != 0)
 		return -EINVAL;
 
-	ret = visorchannel_write(ControlVm_channel,
-		offsetof(struct spar_controlvm_channel_protocol, tool_action),
-		&toolAction, sizeof(u8));
+	ret = visorchannel_write(controlvm_channel,
+		offsetof(struct spar_controlvm_channel_protocol,
+			 tool_action),
+		&tool_action, sizeof(u8));
 
 	if (ret)
 		return ret;
@@ -387,14 +384,14 @@ static ssize_t boottotool_show(struct device *dev,
 			       struct device_attribute *attr,
 			       char *buf)
 {
-	struct efi_spar_indication efiSparIndication;
+	struct efi_spar_indication efi_spar_indication;
 
-	visorchannel_read(ControlVm_channel,
-		offsetof(struct spar_controlvm_channel_protocol,
-			efi_spar_ind), &efiSparIndication,
-		sizeof(struct efi_spar_indication));
+	visorchannel_read(controlvm_channel,
+			  offsetof(struct spar_controlvm_channel_protocol,
+				   efi_spar_ind), &efi_spar_indication,
+			  sizeof(struct efi_spar_indication));
 	return scnprintf(buf, PAGE_SIZE, "%u\n",
-			efiSparIndication.boot_to_tool);
+			 efi_spar_indication.boot_to_tool);
 }
 
 static ssize_t boottotool_store(struct device *dev,
@@ -402,17 +399,16 @@ static ssize_t boottotool_store(struct device *dev,
 				const char *buf, size_t count)
 {
 	int val, ret;
-	struct efi_spar_indication efiSparIndication;
+	struct efi_spar_indication efi_spar_indication;
 
 	if (kstrtoint(buf, 10, &val) != 0)
 		return -EINVAL;
 
-	efiSparIndication.boot_to_tool = val;
-	ret = visorchannel_write(ControlVm_channel,
+	efi_spar_indication.boot_to_tool = val;
+	ret = visorchannel_write(controlvm_channel,
 			offsetof(struct spar_controlvm_channel_protocol,
-				efi_spar_ind),
-			&(efiSparIndication),
-		sizeof(struct efi_spar_indication));
+				 efi_spar_ind), &(efi_spar_indication),
+				 sizeof(struct efi_spar_indication));
 
 	if (ret)
 		return ret;
@@ -420,18 +416,19 @@ static ssize_t boottotool_store(struct device *dev,
 }
 
 static ssize_t error_show(struct device *dev, struct device_attribute *attr,
-		char *buf)
+			  char *buf)
 {
 	u32 error;
 
-	visorchannel_read(ControlVm_channel, offsetof(
-		struct spar_controlvm_channel_protocol, installation_error),
-		&error, sizeof(u32));
+	visorchannel_read(controlvm_channel,
+			  offsetof(struct spar_controlvm_channel_protocol,
+				   installation_error),
+			  &error, sizeof(u32));
 	return scnprintf(buf, PAGE_SIZE, "%i\n", error);
 }
 
 static ssize_t error_store(struct device *dev, struct device_attribute *attr,
-		const char *buf, size_t count)
+			   const char *buf, size_t count)
 {
 	u32 error;
 	int ret;
@@ -439,114 +436,81 @@ static ssize_t error_store(struct device *dev, struct device_attribute *attr,
 	if (kstrtou32(buf, 10, &error) != 0)
 		return -EINVAL;
 
-	ret = visorchannel_write(ControlVm_channel,
-			offsetof(struct spar_controlvm_channel_protocol,
-				installation_error),
-			&error, sizeof(u32));
+	ret = visorchannel_write(controlvm_channel,
+		offsetof(struct spar_controlvm_channel_protocol,
+			 installation_error),
+		&error, sizeof(u32));
 	if (ret)
 		return ret;
 	return count;
 }
 
 static ssize_t textid_show(struct device *dev, struct device_attribute *attr,
-		char *buf)
+			   char *buf)
 {
-	u32 textId;
+	u32 text_id;
 
-	visorchannel_read(ControlVm_channel, offsetof(
-		struct spar_controlvm_channel_protocol, installation_text_id),
-		&textId, sizeof(u32));
-	return scnprintf(buf, PAGE_SIZE, "%i\n", textId);
+	visorchannel_read(controlvm_channel,
+			  offsetof(struct spar_controlvm_channel_protocol,
+				   installation_text_id),
+			  &text_id, sizeof(u32));
+	return scnprintf(buf, PAGE_SIZE, "%i\n", text_id);
 }
 
 static ssize_t textid_store(struct device *dev, struct device_attribute *attr,
-		const char *buf, size_t count)
+			    const char *buf, size_t count)
 {
-	u32 textId;
+	u32 text_id;
 	int ret;
 
-	if (kstrtou32(buf, 10, &textId) != 0)
+	if (kstrtou32(buf, 10, &text_id) != 0)
 		return -EINVAL;
 
-	ret = visorchannel_write(ControlVm_channel,
-			offsetof(struct spar_controlvm_channel_protocol,
-				installation_text_id),
-			&textId, sizeof(u32));
+	ret = visorchannel_write(controlvm_channel,
+		offsetof(struct spar_controlvm_channel_protocol,
+			 installation_text_id),
+		&text_id, sizeof(u32));
 	if (ret)
 		return ret;
 	return count;
 }
 
-
 static ssize_t remaining_steps_show(struct device *dev,
-	struct device_attribute *attr, char *buf)
+				    struct device_attribute *attr, char *buf)
 {
-	u16 remainingSteps;
+	u16 remaining_steps;
 
-	visorchannel_read(ControlVm_channel,
-		offsetof(struct spar_controlvm_channel_protocol,
-			installation_remaining_steps),
-		&remainingSteps,
-		sizeof(u16));
-	return scnprintf(buf, PAGE_SIZE, "%hu\n", remainingSteps);
+	visorchannel_read(controlvm_channel,
+			  offsetof(struct spar_controlvm_channel_protocol,
+				   installation_remaining_steps),
+			  &remaining_steps, sizeof(u16));
+	return scnprintf(buf, PAGE_SIZE, "%hu\n", remaining_steps);
 }
 
 static ssize_t remaining_steps_store(struct device *dev,
-	struct device_attribute *attr, const char *buf, size_t count)
+				     struct device_attribute *attr,
+				     const char *buf, size_t count)
 {
-	u16 remainingSteps;
+	u16 remaining_steps;
 	int ret;
 
-	if (kstrtou16(buf, 10, &remainingSteps) != 0)
+	if (kstrtou16(buf, 10, &remaining_steps) != 0)
 		return -EINVAL;
 
-	ret = visorchannel_write(ControlVm_channel,
-			offsetof(struct spar_controlvm_channel_protocol,
-				installation_remaining_steps),
-			&remainingSteps, sizeof(u16));
+	ret = visorchannel_write(controlvm_channel,
+		offsetof(struct spar_controlvm_channel_protocol,
+			 installation_remaining_steps),
+		&remaining_steps, sizeof(u16));
 	if (ret)
 		return ret;
 	return count;
 }
 
-#if 0
-static void
-testUnicode(void)
-{
-	wchar_t unicodeString[] = { 'a', 'b', 'c', 0 };
-	char s[sizeof(unicodeString) * NLS_MAX_CHARSET_SIZE];
-	wchar_t unicode2[99];
-
-	/* NOTE: Either due to a bug, or feature I don't understand, the
-	 *       kernel utf8_mbstowcs() and utf_wcstombs() do NOT copy the
-	 *       trailed NUL byte!!   REALLY!!!!!    Arrrrgggghhhhh
-	 */
-
-	LOGINF("sizeof(wchar_t) = %d", sizeof(wchar_t));
-	LOGINF("utf8_wcstombs=%d",
-	       chrs = utf8_wcstombs(s, unicodeString, sizeof(s)));
-	if (chrs >= 0)
-		s[chrs] = '\0';	/* GRRRRRRRR */
-	LOGINF("s='%s'", s);
-	LOGINF("utf8_mbstowcs=%d", chrs = utf8_mbstowcs(unicode2, s, 100));
-	if (chrs >= 0)
-		unicode2[chrs] = 0;	/* GRRRRRRRR */
-	if (memcmp(unicodeString, unicode2, sizeof(unicodeString)) == 0)
-		LOGINF("strings match... good");
-	else
-		LOGINF("strings did not match!!");
-}
-#endif
-
 static void
-busInfo_clear(void *v)
+bus_info_clear(void *v)
 {
 	struct visorchipset_bus_info *p = (struct visorchipset_bus_info *) (v);
 
-	if (p->proc_object) {
-		visor_proc_DestroyObject(p->proc_object);
-		p->proc_object = NULL;
-	}
 	kfree(p->name);
 	p->name = NULL;
 
@@ -558,7 +522,7 @@ busInfo_clear(void *v)
 }
 
 static void
-devInfo_clear(void *v)
+dev_info_clear(void *v)
 {
 	struct visorchipset_device_info *p =
 			(struct visorchipset_device_info *)(v);
@@ -593,22 +557,22 @@ visorchipset_register_busdev_server(
 			struct visorchipset_busdev_responders *responders,
 			struct ultra_vbus_deviceinfo *driver_info)
 {
-	down(&NotifierLock);
-	if (notifiers == NULL) {
-		memset(&BusDev_Server_Notifiers, 0,
-		       sizeof(BusDev_Server_Notifiers));
+	down(&notifier_lock);
+	if (!notifiers) {
+		memset(&busdev_server_notifiers, 0,
+		       sizeof(busdev_server_notifiers));
 		serverregistered = 0;	/* clear flag */
 	} else {
-		BusDev_Server_Notifiers = *notifiers;
+		busdev_server_notifiers = *notifiers;
 		serverregistered = 1;	/* set flag */
 	}
 	if (responders)
-		*responders = BusDev_Responders;
+		*responders = busdev_responders;
 	if (driver_info)
 		bus_device_info_init(driver_info, "chipset", "visorchipset",
-				   VERSION, NULL);
+				     VERSION, NULL);
 
-	up(&NotifierLock);
+	up(&notifier_lock);
 }
 EXPORT_SYMBOL_GPL(visorchipset_register_busdev_server);
 
@@ -618,21 +582,21 @@ visorchipset_register_busdev_client(
 			struct visorchipset_busdev_responders *responders,
 			struct ultra_vbus_deviceinfo *driver_info)
 {
-	down(&NotifierLock);
-	if (notifiers == NULL) {
-		memset(&BusDev_Client_Notifiers, 0,
-		       sizeof(BusDev_Client_Notifiers));
+	down(&notifier_lock);
+	if (!notifiers) {
+		memset(&busdev_client_notifiers, 0,
+		       sizeof(busdev_client_notifiers));
 		clientregistered = 0;	/* clear flag */
 	} else {
-		BusDev_Client_Notifiers = *notifiers;
+		busdev_client_notifiers = *notifiers;
 		clientregistered = 1;	/* set flag */
 	}
 	if (responders)
-		*responders = BusDev_Responders;
+		*responders = busdev_responders;
 	if (driver_info)
 		bus_device_info_init(driver_info, "chipset(bolts)",
 				     "visorchipset", VERSION, NULL);
-	up(&NotifierLock);
+	up(&notifier_lock);
 }
 EXPORT_SYMBOL_GPL(visorchipset_register_busdev_client);
 
@@ -642,14 +606,14 @@ cleanup_controlvm_structures(void)
 	struct visorchipset_bus_info *bi, *tmp_bi;
 	struct visorchipset_device_info *di, *tmp_di;
 
-	list_for_each_entry_safe(bi, tmp_bi, &BusInfoList, entry) {
-		busInfo_clear(bi);
+	list_for_each_entry_safe(bi, tmp_bi, &bus_info_list, entry) {
+		bus_info_clear(bi);
 		list_del(&bi->entry);
 		kfree(bi);
 	}
 
-	list_for_each_entry_safe(di, tmp_di, &DevInfoList, entry) {
-		devInfo_clear(di);
+	list_for_each_entry_safe(di, tmp_di, &dev_info_list, entry) {
+		dev_info_clear(di);
 		list_del(&di->entry);
 		kfree(di);
 	}
@@ -664,9 +628,8 @@ chipset_init(struct controlvm_message *inmsg)
 
 	POSTCODE_LINUX_2(CHIPSET_INIT_ENTRY_PC, POSTCODE_SEVERITY_INFO);
 	if (chipset_inited) {
-		LOGERR("CONTROLVM_CHIPSET_INIT Failed: Already Done.");
 		rc = -CONTROLVM_RESP_ERROR_ALREADY_DONE;
-		goto Away;
+		goto cleanup;
 	}
 	chipset_inited = 1;
 	POSTCODE_LINUX_2(CHIPSET_INIT_EXIT_PC, POSTCODE_SEVERITY_INFO);
@@ -681,7 +644,7 @@ chipset_init(struct controlvm_message *inmsg)
 	 * features-aware driver. */
 	features |= ULTRA_CHIPSET_FEATURE_REPLY;
 
-Away:
+cleanup:
 	if (rc < 0)
 		cleanup_controlvm_structures();
 	if (inmsg->hdr.flags.response_expected)
@@ -690,10 +653,10 @@ Away:
 
 static void
 controlvm_init_response(struct controlvm_message *msg,
-			struct controlvm_message_header *msgHdr, int response)
+			struct controlvm_message_header *msg_hdr, int response)
 {
 	memset(msg, 0, sizeof(struct controlvm_message));
-	memcpy(&msg->hdr, msgHdr, sizeof(struct controlvm_message_header));
+	memcpy(&msg->hdr, msg_hdr, sizeof(struct controlvm_message_header));
 	msg->hdr.payload_bytes = 0;
 	msg->hdr.payload_vm_offset = 0;
 	msg->hdr.payload_max_bytes = 0;
@@ -704,59 +667,54 @@ controlvm_init_response(struct controlvm_message *msg,
 }
 
 static void
-controlvm_respond(struct controlvm_message_header *msgHdr, int response)
+controlvm_respond(struct controlvm_message_header *msg_hdr, int response)
 {
 	struct controlvm_message outmsg;
 
-	controlvm_init_response(&outmsg, msgHdr, response);
+	controlvm_init_response(&outmsg, msg_hdr, response);
 	/* For DiagPool channel DEVICE_CHANGESTATE, we need to send
 	* back the deviceChangeState structure in the packet. */
-	if (msgHdr->id == CONTROLVM_DEVICE_CHANGESTATE
-	    && g_DeviceChangeStatePacket.device_change_state.bus_no ==
-	    g_diagpoolBusNo
-	    && g_DeviceChangeStatePacket.device_change_state.dev_no ==
-	    g_diagpoolDevNo)
-		outmsg.cmd = g_DeviceChangeStatePacket;
-	if (outmsg.hdr.flags.test_message == 1) {
-		LOGINF("%s controlvm_msg=0x%x response=%d for test message",
-		       __func__, outmsg.hdr.id, response);
+	if (msg_hdr->id == CONTROLVM_DEVICE_CHANGESTATE &&
+	    g_devicechangestate_packet.device_change_state.bus_no ==
+	    g_diagpool_bus_no &&
+	    g_devicechangestate_packet.device_change_state.dev_no ==
+	    g_diagpool_dev_no)
+		outmsg.cmd = g_devicechangestate_packet;
+	if (outmsg.hdr.flags.test_message == 1)
 		return;
-	}
-	if (!visorchannel_signalinsert(ControlVm_channel,
+
+	if (!visorchannel_signalinsert(controlvm_channel,
 				       CONTROLVM_QUEUE_REQUEST, &outmsg)) {
-		LOGERR("signalinsert failed!");
 		return;
 	}
 }
 
 static void
-controlvm_respond_chipset_init(struct controlvm_message_header *msgHdr,
+controlvm_respond_chipset_init(struct controlvm_message_header *msg_hdr,
 			       int response,
 			       enum ultra_chipset_feature features)
 {
 	struct controlvm_message outmsg;
 
-	controlvm_init_response(&outmsg, msgHdr, response);
+	controlvm_init_response(&outmsg, msg_hdr, response);
 	outmsg.cmd.init_chipset.features = features;
-	if (!visorchannel_signalinsert(ControlVm_channel,
+	if (!visorchannel_signalinsert(controlvm_channel,
 				       CONTROLVM_QUEUE_REQUEST, &outmsg)) {
-		LOGERR("signalinsert failed!");
 		return;
 	}
 }
 
 static void controlvm_respond_physdev_changestate(
-		struct controlvm_message_header *msgHdr, int response,
+		struct controlvm_message_header *msg_hdr, int response,
 		struct spar_segment_state state)
 {
 	struct controlvm_message outmsg;
 
-	controlvm_init_response(&outmsg, msgHdr, response);
+	controlvm_init_response(&outmsg, msg_hdr, response);
 	outmsg.cmd.device_change_state.state = state;
 	outmsg.cmd.device_change_state.flags.phys_device = 1;
-	if (!visorchannel_signalinsert(ControlVm_channel,
+	if (!visorchannel_signalinsert(controlvm_channel,
 				       CONTROLVM_QUEUE_REQUEST, &outmsg)) {
-		LOGERR("signalinsert failed!");
 		return;
 	}
 }
@@ -765,56 +723,50 @@ void
 visorchipset_save_message(struct controlvm_message *msg,
 			  enum crash_obj_type type)
 {
-	u32 localSavedCrashMsgOffset;
-	u16 localSavedCrashMsgCount;
+	u32 crash_msg_offset;
+	u16 crash_msg_count;
 
 	/* get saved message count */
-	if (visorchannel_read(ControlVm_channel,
+	if (visorchannel_read(controlvm_channel,
 			      offsetof(struct spar_controlvm_channel_protocol,
 				       saved_crash_message_count),
-			      &localSavedCrashMsgCount, sizeof(u16)) < 0) {
-		LOGERR("failed to get Saved Message Count");
+			      &crash_msg_count, sizeof(u16)) < 0) {
 		POSTCODE_LINUX_2(CRASH_DEV_CTRL_RD_FAILURE_PC,
 				 POSTCODE_SEVERITY_ERR);
 		return;
 	}
 
-	if (localSavedCrashMsgCount != CONTROLVM_CRASHMSG_MAX) {
-		LOGERR("Saved Message Count incorrect %d",
-		       localSavedCrashMsgCount);
+	if (crash_msg_count != CONTROLVM_CRASHMSG_MAX) {
 		POSTCODE_LINUX_3(CRASH_DEV_COUNT_FAILURE_PC,
-				 localSavedCrashMsgCount,
+				 crash_msg_count,
 				 POSTCODE_SEVERITY_ERR);
 		return;
 	}
 
 	/* get saved crash message offset */
-	if (visorchannel_read(ControlVm_channel,
+	if (visorchannel_read(controlvm_channel,
 			      offsetof(struct spar_controlvm_channel_protocol,
 				       saved_crash_message_offset),
-			      &localSavedCrashMsgOffset, sizeof(u32)) < 0) {
-		LOGERR("failed to get Saved Message Offset");
+			      &crash_msg_offset, sizeof(u32)) < 0) {
 		POSTCODE_LINUX_2(CRASH_DEV_CTRL_RD_FAILURE_PC,
 				 POSTCODE_SEVERITY_ERR);
 		return;
 	}
 
 	if (type == CRASH_BUS) {
-		if (visorchannel_write(ControlVm_channel,
-				       localSavedCrashMsgOffset,
+		if (visorchannel_write(controlvm_channel,
+				       crash_msg_offset,
 				       msg,
 				       sizeof(struct controlvm_message)) < 0) {
-			LOGERR("SAVE_MSG_BUS_FAILURE: Failed to write CrashCreateBusMsg!");
 			POSTCODE_LINUX_2(SAVE_MSG_BUS_FAILURE_PC,
 					 POSTCODE_SEVERITY_ERR);
 			return;
 		}
 	} else {
-		if (visorchannel_write(ControlVm_channel,
-				       localSavedCrashMsgOffset +
+		if (visorchannel_write(controlvm_channel,
+				       crash_msg_offset +
 				       sizeof(struct controlvm_message), msg,
 				       sizeof(struct controlvm_message)) < 0) {
-			LOGERR("SAVE_MSG_DEV_FAILURE: Failed to write CrashCreateDevMsg!");
 			POSTCODE_LINUX_2(SAVE_MSG_DEV_FAILURE_PC,
 					 POSTCODE_SEVERITY_ERR);
 			return;
@@ -824,134 +776,118 @@ visorchipset_save_message(struct controlvm_message *msg,
 EXPORT_SYMBOL_GPL(visorchipset_save_message);
 
 static void
-bus_responder(enum controlvm_id cmdId, ulong busNo, int response)
+bus_responder(enum controlvm_id cmd_id, ulong bus_no, int response)
 {
 	struct visorchipset_bus_info *p = NULL;
 	BOOL need_clear = FALSE;
 
-	p = findbus(&BusInfoList, busNo);
-	if (!p) {
-		LOGERR("internal error busNo=%lu", busNo);
+	p = findbus(&bus_info_list, bus_no);
+	if (!p)
 		return;
-	}
+
 	if (response < 0) {
-		if ((cmdId == CONTROLVM_BUS_CREATE) &&
+		if ((cmd_id == CONTROLVM_BUS_CREATE) &&
 		    (response != (-CONTROLVM_RESP_ERROR_ALREADY_DONE)))
 			/* undo the row we just created... */
-			delbusdevices(&DevInfoList, busNo);
+			delbusdevices(&dev_info_list, bus_no);
 	} else {
-		if (cmdId == CONTROLVM_BUS_CREATE)
+		if (cmd_id == CONTROLVM_BUS_CREATE)
 			p->state.created = 1;
-		if (cmdId == CONTROLVM_BUS_DESTROY)
+		if (cmd_id == CONTROLVM_BUS_DESTROY)
 			need_clear = TRUE;
 	}
 
-	if (p->pending_msg_hdr.id == CONTROLVM_INVALID) {
-		LOGERR("bus_responder no pending msg");
+	if (p->pending_msg_hdr.id == CONTROLVM_INVALID)
 		return;		/* no controlvm response needed */
-	}
-	if (p->pending_msg_hdr.id != (u32) cmdId) {
-		LOGERR("expected=%d, found=%d", cmdId, p->pending_msg_hdr.id);
+	if (p->pending_msg_hdr.id != (u32)cmd_id)
 		return;
-	}
 	controlvm_respond(&p->pending_msg_hdr, response);
 	p->pending_msg_hdr.id = CONTROLVM_INVALID;
 	if (need_clear) {
-		busInfo_clear(p);
-		delbusdevices(&DevInfoList, busNo);
+		bus_info_clear(p);
+		delbusdevices(&dev_info_list, bus_no);
 	}
 }
 
 static void
-device_changestate_responder(enum controlvm_id cmdId,
-			     ulong busNo, ulong devNo, int response,
-			     struct spar_segment_state responseState)
+device_changestate_responder(enum controlvm_id cmd_id,
+			     ulong bus_no, ulong dev_no, int response,
+			     struct spar_segment_state response_state)
 {
 	struct visorchipset_device_info *p = NULL;
 	struct controlvm_message outmsg;
 
-	p = finddevice(&DevInfoList, busNo, devNo);
-	if (!p) {
-		LOGERR("internal error; busNo=%lu, devNo=%lu", busNo, devNo);
+	p = finddevice(&dev_info_list, bus_no, dev_no);
+	if (!p)
 		return;
-	}
-	if (p->pending_msg_hdr.id == CONTROLVM_INVALID) {
-		LOGERR("device_responder no pending msg");
+	if (p->pending_msg_hdr.id == CONTROLVM_INVALID)
 		return;		/* no controlvm response needed */
-	}
-	if (p->pending_msg_hdr.id != cmdId) {
-		LOGERR("expected=%d, found=%d", cmdId, p->pending_msg_hdr.id);
+	if (p->pending_msg_hdr.id != cmd_id)
 		return;
-	}
 
 	controlvm_init_response(&outmsg, &p->pending_msg_hdr, response);
 
-	outmsg.cmd.device_change_state.bus_no = busNo;
-	outmsg.cmd.device_change_state.dev_no = devNo;
-	outmsg.cmd.device_change_state.state = responseState;
+	outmsg.cmd.device_change_state.bus_no = bus_no;
+	outmsg.cmd.device_change_state.dev_no = dev_no;
+	outmsg.cmd.device_change_state.state = response_state;
 
-	if (!visorchannel_signalinsert(ControlVm_channel,
-				       CONTROLVM_QUEUE_REQUEST, &outmsg)) {
-		LOGERR("signalinsert failed!");
+	if (!visorchannel_signalinsert(controlvm_channel,
+				       CONTROLVM_QUEUE_REQUEST, &outmsg))
 		return;
-	}
 
 	p->pending_msg_hdr.id = CONTROLVM_INVALID;
 }
 
 static void
-device_responder(enum controlvm_id cmdId, ulong busNo, ulong devNo,
+device_responder(enum controlvm_id cmd_id, ulong bus_no, ulong dev_no,
 		 int response)
 {
 	struct visorchipset_device_info *p = NULL;
 	BOOL need_clear = FALSE;
 
-	p = finddevice(&DevInfoList, busNo, devNo);
-	if (!p) {
-		LOGERR("internal error; busNo=%lu, devNo=%lu", busNo, devNo);
+	p = finddevice(&dev_info_list, bus_no, dev_no);
+	if (!p)
 		return;
-	}
 	if (response >= 0) {
-		if (cmdId == CONTROLVM_DEVICE_CREATE)
+		if (cmd_id == CONTROLVM_DEVICE_CREATE)
 			p->state.created = 1;
-		if (cmdId == CONTROLVM_DEVICE_DESTROY)
+		if (cmd_id == CONTROLVM_DEVICE_DESTROY)
 			need_clear = TRUE;
 	}
 
-	if (p->pending_msg_hdr.id == CONTROLVM_INVALID) {
-		LOGERR("device_responder no pending msg");
+	if (p->pending_msg_hdr.id == CONTROLVM_INVALID)
 		return;		/* no controlvm response needed */
-	}
-	if (p->pending_msg_hdr.id != (u32) cmdId) {
-		LOGERR("expected=%d, found=%d", cmdId, p->pending_msg_hdr.id);
+
+	if (p->pending_msg_hdr.id != (u32)cmd_id)
 		return;
-	}
+
 	controlvm_respond(&p->pending_msg_hdr, response);
 	p->pending_msg_hdr.id = CONTROLVM_INVALID;
 	if (need_clear)
-		devInfo_clear(p);
+		dev_info_clear(p);
 }
 
 static void
-bus_epilog(u32 busNo,
-	   u32 cmd, struct controlvm_message_header *msgHdr,
-	   int response, BOOL needResponse)
+bus_epilog(u32 bus_no,
+	   u32 cmd, struct controlvm_message_header *msg_hdr,
+	   int response, BOOL need_response)
 {
 	BOOL notified = FALSE;
 
-	struct visorchipset_bus_info *pBusInfo = findbus(&BusInfoList, busNo);
+	struct visorchipset_bus_info *bus_info = findbus(&bus_info_list,
+							 bus_no);
 
-	if (!pBusInfo) {
-		LOGERR("HUH? bad busNo=%d", busNo);
+	if (!bus_info)
 		return;
-	}
-	if (needResponse) {
-		memcpy(&pBusInfo->pending_msg_hdr, msgHdr,
+
+	if (need_response) {
+		memcpy(&bus_info->pending_msg_hdr, msg_hdr,
 		       sizeof(struct controlvm_message_header));
-	} else
-		pBusInfo->pending_msg_hdr.id = CONTROLVM_INVALID;
+	} else {
+		bus_info->pending_msg_hdr.id = CONTROLVM_INVALID;
+	}
 
-	down(&NotifierLock);
+	down(&notifier_lock);
 	if (response == CONTROLVM_RESP_SUCCESS) {
 		switch (cmd) {
 		case CONTROLVM_BUS_CREATE:
@@ -965,24 +901,24 @@ bus_epilog(u32 busNo,
 			* either server or client devices
 			* - BusDev_Client can handle ONLY client
 			* devices */
-			if (BusDev_Server_Notifiers.bus_create) {
-				(*BusDev_Server_Notifiers.bus_create) (busNo);
+			if (busdev_server_notifiers.bus_create) {
+				(*busdev_server_notifiers.bus_create) (bus_no);
 				notified = TRUE;
 			}
-			if ((!pBusInfo->flags.server) /*client */ &&
-			    BusDev_Client_Notifiers.bus_create) {
-				(*BusDev_Client_Notifiers.bus_create) (busNo);
+			if ((!bus_info->flags.server) /*client */ &&
+			    busdev_client_notifiers.bus_create) {
+				(*busdev_client_notifiers.bus_create) (bus_no);
 				notified = TRUE;
 			}
 			break;
 		case CONTROLVM_BUS_DESTROY:
-			if (BusDev_Server_Notifiers.bus_destroy) {
-				(*BusDev_Server_Notifiers.bus_destroy) (busNo);
+			if (busdev_server_notifiers.bus_destroy) {
+				(*busdev_server_notifiers.bus_destroy) (bus_no);
 				notified = TRUE;
 			}
-			if ((!pBusInfo->flags.server) /*client */ &&
-			    BusDev_Client_Notifiers.bus_destroy) {
-				(*BusDev_Client_Notifiers.bus_destroy) (busNo);
+			if ((!bus_info->flags.server) /*client */ &&
+			    busdev_client_notifiers.bus_destroy) {
+				(*busdev_client_notifiers.bus_destroy) (bus_no);
 				notified = TRUE;
 			}
 			break;
@@ -995,45 +931,45 @@ bus_epilog(u32 busNo,
 		 */
 		;
 	else
-		bus_responder(cmd, busNo, response);
-	up(&NotifierLock);
+		bus_responder(cmd, bus_no, response);
+	up(&notifier_lock);
 }
 
 static void
-device_epilog(u32 busNo, u32 devNo, struct spar_segment_state state, u32 cmd,
-	      struct controlvm_message_header *msgHdr, int response,
-	      BOOL needResponse, BOOL for_visorbus)
+device_epilog(u32 bus_no, u32 dev_no, struct spar_segment_state state, u32 cmd,
+	      struct controlvm_message_header *msg_hdr, int response,
+	      BOOL need_response, BOOL for_visorbus)
 {
 	struct visorchipset_busdev_notifiers *notifiers = NULL;
 	BOOL notified = FALSE;
 
-	struct visorchipset_device_info *pDevInfo =
-		finddevice(&DevInfoList, busNo, devNo);
+	struct visorchipset_device_info *dev_info =
+		finddevice(&dev_info_list, bus_no, dev_no);
 	char *envp[] = {
 		"SPARSP_DIAGPOOL_PAUSED_STATE = 1",
 		NULL
 	};
 
-	if (!pDevInfo) {
-		LOGERR("HUH? bad busNo=%d, devNo=%d", busNo, devNo);
+	if (!dev_info)
 		return;
-	}
+
 	if (for_visorbus)
-		notifiers = &BusDev_Server_Notifiers;
+		notifiers = &busdev_server_notifiers;
 	else
-		notifiers = &BusDev_Client_Notifiers;
-	if (needResponse) {
-		memcpy(&pDevInfo->pending_msg_hdr, msgHdr,
+		notifiers = &busdev_client_notifiers;
+	if (need_response) {
+		memcpy(&dev_info->pending_msg_hdr, msg_hdr,
 		       sizeof(struct controlvm_message_header));
-	} else
-		pDevInfo->pending_msg_hdr.id = CONTROLVM_INVALID;
+	} else {
+		dev_info->pending_msg_hdr.id = CONTROLVM_INVALID;
+	}
 
-	down(&NotifierLock);
+	down(&notifier_lock);
 	if (response >= 0) {
 		switch (cmd) {
 		case CONTROLVM_DEVICE_CREATE:
 			if (notifiers->device_create) {
-				(*notifiers->device_create) (busNo, devNo);
+				(*notifiers->device_create) (bus_no, dev_no);
 				notified = TRUE;
 			}
 			break;
@@ -1043,8 +979,8 @@ device_epilog(u32 busNo, u32 devNo, struct spar_segment_state state, u32 cmd,
 			    state.operating ==
 				segment_state_running.operating) {
 				if (notifiers->device_resume) {
-					(*notifiers->device_resume) (busNo,
-								     devNo);
+					(*notifiers->device_resume) (bus_no,
+								     dev_no);
 					notified = TRUE;
 				}
 			}
@@ -1056,8 +992,8 @@ device_epilog(u32 busNo, u32 devNo, struct spar_segment_state state, u32 cmd,
 				 * where server is lost
 				 */
 				if (notifiers->device_pause) {
-					(*notifiers->device_pause) (busNo,
-								    devNo);
+					(*notifiers->device_pause) (bus_no,
+								    dev_no);
 					notified = TRUE;
 				}
 			} else if (state.alive == segment_state_paused.alive &&
@@ -1066,22 +1002,20 @@ device_epilog(u32 busNo, u32 devNo, struct spar_segment_state state, u32 cmd,
 				/* this is lite pause where channel is
 				 * still valid just 'pause' of it
 				 */
-				if (busNo == g_diagpoolBusNo
-				    && devNo == g_diagpoolDevNo) {
-					LOGINF("DEVICE_CHANGESTATE(DiagpoolChannel busNo=%d devNo=%d is pausing...)",
-					     busNo, devNo);
+				if (bus_no == g_diagpool_bus_no &&
+				    dev_no == g_diagpool_dev_no) {
 					/* this will trigger the
 					 * diag_shutdown.sh script in
 					 * the visorchipset hotplug */
 					kobject_uevent_env
-					    (&Visorchipset_platform_device.dev.
+					    (&visorchipset_platform_device.dev.
 					     kobj, KOBJ_ONLINE, envp);
 				}
 			}
 			break;
 		case CONTROLVM_DEVICE_DESTROY:
 			if (notifiers->device_destroy) {
-				(*notifiers->device_destroy) (busNo, devNo);
+				(*notifiers->device_destroy) (bus_no, dev_no);
 				notified = TRUE;
 			}
 			break;
@@ -1094,62 +1028,57 @@ device_epilog(u32 busNo, u32 devNo, struct spar_segment_state state, u32 cmd,
 		 */
 		;
 	else
-		device_responder(cmd, busNo, devNo, response);
-	up(&NotifierLock);
+		device_responder(cmd, bus_no, dev_no, response);
+	up(&notifier_lock);
 }
 
 static void
 bus_create(struct controlvm_message *inmsg)
 {
 	struct controlvm_message_packet *cmd = &inmsg->cmd;
-	ulong busNo = cmd->create_bus.bus_no;
+	ulong bus_no = cmd->create_bus.bus_no;
 	int rc = CONTROLVM_RESP_SUCCESS;
-	struct visorchipset_bus_info *pBusInfo = NULL;
-
+	struct visorchipset_bus_info *bus_info = NULL;
 
-	pBusInfo = findbus(&BusInfoList, busNo);
-	if (pBusInfo && (pBusInfo->state.created == 1)) {
-		LOGERR("CONTROLVM_BUS_CREATE Failed: bus %lu already exists",
-		       busNo);
-		POSTCODE_LINUX_3(BUS_CREATE_FAILURE_PC, busNo,
+	bus_info = findbus(&bus_info_list, bus_no);
+	if (bus_info && (bus_info->state.created == 1)) {
+		POSTCODE_LINUX_3(BUS_CREATE_FAILURE_PC, bus_no,
 				 POSTCODE_SEVERITY_ERR);
 		rc = -CONTROLVM_RESP_ERROR_ALREADY_DONE;
-		goto Away;
+		goto cleanup;
 	}
-	pBusInfo = kzalloc(sizeof(struct visorchipset_bus_info), GFP_KERNEL);
-	if (pBusInfo == NULL) {
-		LOGERR("CONTROLVM_BUS_CREATE Failed: bus %lu kzalloc failed",
-		       busNo);
-		POSTCODE_LINUX_3(BUS_CREATE_FAILURE_PC, busNo,
+	bus_info = kzalloc(sizeof(*bus_info), GFP_KERNEL);
+	if (!bus_info) {
+		POSTCODE_LINUX_3(BUS_CREATE_FAILURE_PC, bus_no,
 				 POSTCODE_SEVERITY_ERR);
 		rc = -CONTROLVM_RESP_ERROR_KMALLOC_FAILED;
-		goto Away;
+		goto cleanup;
 	}
 
-	INIT_LIST_HEAD(&pBusInfo->entry);
-	pBusInfo->bus_no = busNo;
-	pBusInfo->dev_no = cmd->create_bus.dev_count;
+	INIT_LIST_HEAD(&bus_info->entry);
+	bus_info->bus_no = bus_no;
+	bus_info->dev_no = cmd->create_bus.dev_count;
 
-	POSTCODE_LINUX_3(BUS_CREATE_ENTRY_PC, busNo, POSTCODE_SEVERITY_INFO);
+	POSTCODE_LINUX_3(BUS_CREATE_ENTRY_PC, bus_no, POSTCODE_SEVERITY_INFO);
 
 	if (inmsg->hdr.flags.test_message == 1)
-		pBusInfo->chan_info.addr_type = ADDRTYPE_LOCALTEST;
+		bus_info->chan_info.addr_type = ADDRTYPE_LOCALTEST;
 	else
-		pBusInfo->chan_info.addr_type = ADDRTYPE_LOCALPHYSICAL;
+		bus_info->chan_info.addr_type = ADDRTYPE_LOCALPHYSICAL;
 
-	pBusInfo->flags.server = inmsg->hdr.flags.server;
-	pBusInfo->chan_info.channel_addr = cmd->create_bus.channel_addr;
-	pBusInfo->chan_info.n_channel_bytes = cmd->create_bus.channel_bytes;
-	pBusInfo->chan_info.channel_type_uuid =
+	bus_info->flags.server = inmsg->hdr.flags.server;
+	bus_info->chan_info.channel_addr = cmd->create_bus.channel_addr;
+	bus_info->chan_info.n_channel_bytes = cmd->create_bus.channel_bytes;
+	bus_info->chan_info.channel_type_uuid =
 			cmd->create_bus.bus_data_type_uuid;
-	pBusInfo->chan_info.channel_inst_uuid = cmd->create_bus.bus_inst_uuid;
+	bus_info->chan_info.channel_inst_uuid = cmd->create_bus.bus_inst_uuid;
 
-	list_add(&pBusInfo->entry, &BusInfoList);
+	list_add(&bus_info->entry, &bus_info_list);
 
-	POSTCODE_LINUX_3(BUS_CREATE_EXIT_PC, busNo, POSTCODE_SEVERITY_INFO);
+	POSTCODE_LINUX_3(BUS_CREATE_EXIT_PC, bus_no, POSTCODE_SEVERITY_INFO);
 
-Away:
-	bus_epilog(busNo, CONTROLVM_BUS_CREATE, &inmsg->hdr,
+cleanup:
+	bus_epilog(bus_no, CONTROLVM_BUS_CREATE, &inmsg->hdr,
 		   rc, inmsg->hdr.flags.response_expected == 1);
 }
 
@@ -1157,76 +1086,58 @@ static void
 bus_destroy(struct controlvm_message *inmsg)
 {
 	struct controlvm_message_packet *cmd = &inmsg->cmd;
-	ulong busNo = cmd->destroy_bus.bus_no;
-	struct visorchipset_bus_info *pBusInfo;
+	ulong bus_no = cmd->destroy_bus.bus_no;
+	struct visorchipset_bus_info *bus_info;
 	int rc = CONTROLVM_RESP_SUCCESS;
 
-	pBusInfo = findbus(&BusInfoList, busNo);
-	if (!pBusInfo) {
-		LOGERR("CONTROLVM_BUS_DESTROY Failed: bus %lu invalid", busNo);
+	bus_info = findbus(&bus_info_list, bus_no);
+	if (!bus_info)
 		rc = -CONTROLVM_RESP_ERROR_BUS_INVALID;
-		goto Away;
-	}
-	if (pBusInfo->state.created == 0) {
-		LOGERR("CONTROLVM_BUS_DESTROY Failed: bus %lu already destroyed",
-		     busNo);
+	else if (bus_info->state.created == 0)
 		rc = -CONTROLVM_RESP_ERROR_ALREADY_DONE;
-		goto Away;
-	}
 
-Away:
-	bus_epilog(busNo, CONTROLVM_BUS_DESTROY, &inmsg->hdr,
+	bus_epilog(bus_no, CONTROLVM_BUS_DESTROY, &inmsg->hdr,
 		   rc, inmsg->hdr.flags.response_expected == 1);
 }
 
 static void
-bus_configure(struct controlvm_message *inmsg, PARSER_CONTEXT *parser_ctx)
+bus_configure(struct controlvm_message *inmsg,
+	      struct parser_context *parser_ctx)
 {
 	struct controlvm_message_packet *cmd = &inmsg->cmd;
-	ulong busNo = cmd->configure_bus.bus_no;
-	struct visorchipset_bus_info *pBusInfo = NULL;
+	ulong bus_no = cmd->configure_bus.bus_no;
+	struct visorchipset_bus_info *bus_info = NULL;
 	int rc = CONTROLVM_RESP_SUCCESS;
 	char s[99];
 
-	busNo = cmd->configure_bus.bus_no;
-	POSTCODE_LINUX_3(BUS_CONFIGURE_ENTRY_PC, busNo, POSTCODE_SEVERITY_INFO);
+	bus_no = cmd->configure_bus.bus_no;
+	POSTCODE_LINUX_3(BUS_CONFIGURE_ENTRY_PC, bus_no,
+			 POSTCODE_SEVERITY_INFO);
 
-	pBusInfo = findbus(&BusInfoList, busNo);
-	if (!pBusInfo) {
-		LOGERR("CONTROLVM_BUS_CONFIGURE Failed: bus %lu invalid",
-		       busNo);
-		POSTCODE_LINUX_3(BUS_CONFIGURE_FAILURE_PC, busNo,
+	bus_info = findbus(&bus_info_list, bus_no);
+	if (!bus_info) {
+		POSTCODE_LINUX_3(BUS_CONFIGURE_FAILURE_PC, bus_no,
 				 POSTCODE_SEVERITY_ERR);
 		rc = -CONTROLVM_RESP_ERROR_BUS_INVALID;
-		goto Away;
-	}
-	if (pBusInfo->state.created == 0) {
-		LOGERR("CONTROLVM_BUS_CONFIGURE Failed: Invalid bus %lu - not created yet",
-		     busNo);
-		POSTCODE_LINUX_3(BUS_CONFIGURE_FAILURE_PC, busNo,
+	} else if (bus_info->state.created == 0) {
+		POSTCODE_LINUX_3(BUS_CONFIGURE_FAILURE_PC, bus_no,
 				 POSTCODE_SEVERITY_ERR);
 		rc = -CONTROLVM_RESP_ERROR_BUS_INVALID;
-		goto Away;
-	}
-	/* TBD - add this check to other commands also... */
-	if (pBusInfo->pending_msg_hdr.id != CONTROLVM_INVALID) {
-		LOGERR("CONTROLVM_BUS_CONFIGURE Failed: bus %lu MsgId=%u outstanding",
-		     busNo, (uint) pBusInfo->pending_msg_hdr.id);
-		POSTCODE_LINUX_3(BUS_CONFIGURE_FAILURE_PC, busNo,
+	} else if (bus_info->pending_msg_hdr.id != CONTROLVM_INVALID) {
+		POSTCODE_LINUX_3(BUS_CONFIGURE_FAILURE_PC, bus_no,
 				 POSTCODE_SEVERITY_ERR);
 		rc = -CONTROLVM_RESP_ERROR_MESSAGE_ID_INVALID_FOR_CLIENT;
-		goto Away;
-	}
-
-	pBusInfo->partition_handle = cmd->configure_bus.guest_handle;
-	pBusInfo->partition_uuid = parser_id_get(parser_ctx);
-	parser_param_start(parser_ctx, PARSERSTRING_NAME);
-	pBusInfo->name = parser_string_get(parser_ctx);
+	} else {
+		bus_info->partition_handle = cmd->configure_bus.guest_handle;
+		bus_info->partition_uuid = parser_id_get(parser_ctx);
+		parser_param_start(parser_ctx, PARSERSTRING_NAME);
+		bus_info->name = parser_string_get(parser_ctx);
 
-	visorchannel_uuid_id(&pBusInfo->partition_uuid, s);
-	POSTCODE_LINUX_3(BUS_CONFIGURE_EXIT_PC, busNo, POSTCODE_SEVERITY_INFO);
-Away:
-	bus_epilog(busNo, CONTROLVM_BUS_CONFIGURE, &inmsg->hdr,
+		visorchannel_uuid_id(&bus_info->partition_uuid, s);
+		POSTCODE_LINUX_3(BUS_CONFIGURE_EXIT_PC, bus_no,
+				 POSTCODE_SEVERITY_INFO);
+	}
+	bus_epilog(bus_no, CONTROLVM_BUS_CONFIGURE, &inmsg->hdr,
 		   rc, inmsg->hdr.flags.response_expected == 1);
 }
 
@@ -1234,191 +1145,158 @@ static void
 my_device_create(struct controlvm_message *inmsg)
 {
 	struct controlvm_message_packet *cmd = &inmsg->cmd;
-	ulong busNo = cmd->create_device.bus_no;
-	ulong devNo = cmd->create_device.dev_no;
-	struct visorchipset_device_info *pDevInfo = NULL;
-	struct visorchipset_bus_info *pBusInfo = NULL;
+	ulong bus_no = cmd->create_device.bus_no;
+	ulong dev_no = cmd->create_device.dev_no;
+	struct visorchipset_device_info *dev_info = NULL;
+	struct visorchipset_bus_info *bus_info = NULL;
 	int rc = CONTROLVM_RESP_SUCCESS;
 
-	pDevInfo = finddevice(&DevInfoList, busNo, devNo);
-	if (pDevInfo && (pDevInfo->state.created == 1)) {
-		LOGERR("CONTROLVM_DEVICE_CREATE Failed: busNo=%lu, devNo=%lu already exists",
-		     busNo, devNo);
-		POSTCODE_LINUX_4(DEVICE_CREATE_FAILURE_PC, devNo, busNo,
+	dev_info = finddevice(&dev_info_list, bus_no, dev_no);
+	if (dev_info && (dev_info->state.created == 1)) {
+		POSTCODE_LINUX_4(DEVICE_CREATE_FAILURE_PC, dev_no, bus_no,
 				 POSTCODE_SEVERITY_ERR);
 		rc = -CONTROLVM_RESP_ERROR_ALREADY_DONE;
-		goto Away;
+		goto cleanup;
 	}
-	pBusInfo = findbus(&BusInfoList, busNo);
-	if (!pBusInfo) {
-		LOGERR("CONTROLVM_DEVICE_CREATE Failed: Invalid bus %lu - out of range",
-		     busNo);
-		POSTCODE_LINUX_4(DEVICE_CREATE_FAILURE_PC, devNo, busNo,
+	bus_info = findbus(&bus_info_list, bus_no);
+	if (!bus_info) {
+		POSTCODE_LINUX_4(DEVICE_CREATE_FAILURE_PC, dev_no, bus_no,
 				 POSTCODE_SEVERITY_ERR);
 		rc = -CONTROLVM_RESP_ERROR_BUS_INVALID;
-		goto Away;
+		goto cleanup;
 	}
-	if (pBusInfo->state.created == 0) {
-		LOGERR("CONTROLVM_DEVICE_CREATE Failed: Invalid bus %lu - not created yet",
-		     busNo);
-		POSTCODE_LINUX_4(DEVICE_CREATE_FAILURE_PC, devNo, busNo,
+	if (bus_info->state.created == 0) {
+		POSTCODE_LINUX_4(DEVICE_CREATE_FAILURE_PC, dev_no, bus_no,
 				 POSTCODE_SEVERITY_ERR);
 		rc = -CONTROLVM_RESP_ERROR_BUS_INVALID;
-		goto Away;
+		goto cleanup;
 	}
-	pDevInfo = kzalloc(sizeof(struct visorchipset_device_info), GFP_KERNEL);
-	if (pDevInfo == NULL) {
-		LOGERR("CONTROLVM_DEVICE_CREATE Failed: busNo=%lu, devNo=%lu kmaloc failed",
-		     busNo, devNo);
-		POSTCODE_LINUX_4(DEVICE_CREATE_FAILURE_PC, devNo, busNo,
+	dev_info = kzalloc(sizeof(*dev_info), GFP_KERNEL);
+	if (!dev_info) {
+		POSTCODE_LINUX_4(DEVICE_CREATE_FAILURE_PC, dev_no, bus_no,
 				 POSTCODE_SEVERITY_ERR);
 		rc = -CONTROLVM_RESP_ERROR_KMALLOC_FAILED;
-		goto Away;
+		goto cleanup;
 	}
 
-	INIT_LIST_HEAD(&pDevInfo->entry);
-	pDevInfo->bus_no = busNo;
-	pDevInfo->dev_no = devNo;
-	pDevInfo->dev_inst_uuid = cmd->create_device.dev_inst_uuid;
-	POSTCODE_LINUX_4(DEVICE_CREATE_ENTRY_PC, devNo, busNo,
+	INIT_LIST_HEAD(&dev_info->entry);
+	dev_info->bus_no = bus_no;
+	dev_info->dev_no = dev_no;
+	dev_info->dev_inst_uuid = cmd->create_device.dev_inst_uuid;
+	POSTCODE_LINUX_4(DEVICE_CREATE_ENTRY_PC, dev_no, bus_no,
 			 POSTCODE_SEVERITY_INFO);
 
 	if (inmsg->hdr.flags.test_message == 1)
-		pDevInfo->chan_info.addr_type = ADDRTYPE_LOCALTEST;
+		dev_info->chan_info.addr_type = ADDRTYPE_LOCALTEST;
 	else
-		pDevInfo->chan_info.addr_type = ADDRTYPE_LOCALPHYSICAL;
-	pDevInfo->chan_info.channel_addr = cmd->create_device.channel_addr;
-	pDevInfo->chan_info.n_channel_bytes = cmd->create_device.channel_bytes;
-	pDevInfo->chan_info.channel_type_uuid =
+		dev_info->chan_info.addr_type = ADDRTYPE_LOCALPHYSICAL;
+	dev_info->chan_info.channel_addr = cmd->create_device.channel_addr;
+	dev_info->chan_info.n_channel_bytes = cmd->create_device.channel_bytes;
+	dev_info->chan_info.channel_type_uuid =
 			cmd->create_device.data_type_uuid;
-	pDevInfo->chan_info.intr = cmd->create_device.intr;
-	list_add(&pDevInfo->entry, &DevInfoList);
-	POSTCODE_LINUX_4(DEVICE_CREATE_EXIT_PC, devNo, busNo,
+	dev_info->chan_info.intr = cmd->create_device.intr;
+	list_add(&dev_info->entry, &dev_info_list);
+	POSTCODE_LINUX_4(DEVICE_CREATE_EXIT_PC, dev_no, bus_no,
 			 POSTCODE_SEVERITY_INFO);
-Away:
+cleanup:
 	/* get the bus and devNo for DiagPool channel */
-	if (pDevInfo &&
-	    is_diagpool_channel(pDevInfo->chan_info.channel_type_uuid)) {
-		g_diagpoolBusNo = busNo;
-		g_diagpoolDevNo = devNo;
-		LOGINF("CONTROLVM_DEVICE_CREATE for DiagPool channel: busNo=%lu, devNo=%lu",
-		     g_diagpoolBusNo, g_diagpoolDevNo);
-	}
-	device_epilog(busNo, devNo, segment_state_running,
+	if (dev_info &&
+	    is_diagpool_channel(dev_info->chan_info.channel_type_uuid)) {
+		g_diagpool_bus_no = bus_no;
+		g_diagpool_dev_no = dev_no;
+	}
+	device_epilog(bus_no, dev_no, segment_state_running,
 		      CONTROLVM_DEVICE_CREATE, &inmsg->hdr, rc,
 		      inmsg->hdr.flags.response_expected == 1,
-		      FOR_VISORBUS(pDevInfo->chan_info.channel_type_uuid));
+		      FOR_VISORBUS(dev_info->chan_info.channel_type_uuid));
 }
 
 static void
 my_device_changestate(struct controlvm_message *inmsg)
 {
 	struct controlvm_message_packet *cmd = &inmsg->cmd;
-	ulong busNo = cmd->device_change_state.bus_no;
-	ulong devNo = cmd->device_change_state.dev_no;
+	ulong bus_no = cmd->device_change_state.bus_no;
+	ulong dev_no = cmd->device_change_state.dev_no;
 	struct spar_segment_state state = cmd->device_change_state.state;
-	struct visorchipset_device_info *pDevInfo = NULL;
+	struct visorchipset_device_info *dev_info = NULL;
 	int rc = CONTROLVM_RESP_SUCCESS;
 
-	pDevInfo = finddevice(&DevInfoList, busNo, devNo);
-	if (!pDevInfo) {
-		LOGERR("CONTROLVM_DEVICE_CHANGESTATE Failed: busNo=%lu, devNo=%lu invalid (doesn't exist)",
-		     busNo, devNo);
-		POSTCODE_LINUX_4(DEVICE_CHANGESTATE_FAILURE_PC, devNo, busNo,
+	dev_info = finddevice(&dev_info_list, bus_no, dev_no);
+	if (!dev_info) {
+		POSTCODE_LINUX_4(DEVICE_CHANGESTATE_FAILURE_PC, dev_no, bus_no,
 				 POSTCODE_SEVERITY_ERR);
 		rc = -CONTROLVM_RESP_ERROR_DEVICE_INVALID;
-		goto Away;
-	}
-	if (pDevInfo->state.created == 0) {
-		LOGERR("CONTROLVM_DEVICE_CHANGESTATE Failed: busNo=%lu, devNo=%lu invalid (not created)",
-		     busNo, devNo);
-		POSTCODE_LINUX_4(DEVICE_CHANGESTATE_FAILURE_PC, devNo, busNo,
+	} else if (dev_info->state.created == 0) {
+		POSTCODE_LINUX_4(DEVICE_CHANGESTATE_FAILURE_PC, dev_no, bus_no,
 				 POSTCODE_SEVERITY_ERR);
 		rc = -CONTROLVM_RESP_ERROR_DEVICE_INVALID;
 	}
-Away:
-	if ((rc >= CONTROLVM_RESP_SUCCESS) && pDevInfo)
-		device_epilog(busNo, devNo, state, CONTROLVM_DEVICE_CHANGESTATE,
-			      &inmsg->hdr, rc,
+	if ((rc >= CONTROLVM_RESP_SUCCESS) && dev_info)
+		device_epilog(bus_no, dev_no, state,
+			      CONTROLVM_DEVICE_CHANGESTATE, &inmsg->hdr, rc,
 			      inmsg->hdr.flags.response_expected == 1,
 			      FOR_VISORBUS(
-					pDevInfo->chan_info.channel_type_uuid));
+					dev_info->chan_info.channel_type_uuid));
 }
 
 static void
 my_device_destroy(struct controlvm_message *inmsg)
 {
 	struct controlvm_message_packet *cmd = &inmsg->cmd;
-	ulong busNo = cmd->destroy_device.bus_no;
-	ulong devNo = cmd->destroy_device.dev_no;
-	struct visorchipset_device_info *pDevInfo = NULL;
+	ulong bus_no = cmd->destroy_device.bus_no;
+	ulong dev_no = cmd->destroy_device.dev_no;
+	struct visorchipset_device_info *dev_info = NULL;
 	int rc = CONTROLVM_RESP_SUCCESS;
 
-	pDevInfo = finddevice(&DevInfoList, busNo, devNo);
-	if (!pDevInfo) {
-		LOGERR("CONTROLVM_DEVICE_DESTROY Failed: busNo=%lu, devNo=%lu invalid",
-		     busNo, devNo);
+	dev_info = finddevice(&dev_info_list, bus_no, dev_no);
+	if (!dev_info)
 		rc = -CONTROLVM_RESP_ERROR_DEVICE_INVALID;
-		goto Away;
-	}
-	if (pDevInfo->state.created == 0) {
-		LOGERR("CONTROLVM_DEVICE_DESTROY Failed: busNo=%lu, devNo=%lu already destroyed",
-		     busNo, devNo);
+	else if (dev_info->state.created == 0)
 		rc = -CONTROLVM_RESP_ERROR_ALREADY_DONE;
-	}
 
-Away:
-	if ((rc >= CONTROLVM_RESP_SUCCESS) && pDevInfo)
-		device_epilog(busNo, devNo, segment_state_running,
+	if ((rc >= CONTROLVM_RESP_SUCCESS) && dev_info)
+		device_epilog(bus_no, dev_no, segment_state_running,
 			      CONTROLVM_DEVICE_DESTROY, &inmsg->hdr, rc,
 			      inmsg->hdr.flags.response_expected == 1,
 			      FOR_VISORBUS(
-					pDevInfo->chan_info.channel_type_uuid));
+					dev_info->chan_info.channel_type_uuid));
 }
 
 /* When provided with the physical address of the controlvm channel
  * (phys_addr), the offset to the payload area we need to manage
  * (offset), and the size of this payload area (bytes), fills in the
- * CONTROLVM_PAYLOAD_INFO struct.  Returns TRUE for success or FALSE
+ * controlvm_payload_info struct.  Returns TRUE for success or FALSE
  * for failure.
  */
 static int
 initialize_controlvm_payload_info(HOSTADDRESS phys_addr, u64 offset, u32 bytes,
-				  CONTROLVM_PAYLOAD_INFO *info)
+				  struct controlvm_payload_info *info)
 {
 	u8 __iomem *payload = NULL;
 	int rc = CONTROLVM_RESP_SUCCESS;
 
-	if (info == NULL) {
-		LOGERR("HUH ? CONTROLVM_PAYLOAD_INIT Failed : Programmer check at %s:%d",
-		     __FILE__, __LINE__);
+	if (!info) {
 		rc = -CONTROLVM_RESP_ERROR_PAYLOAD_INVALID;
-		goto Away;
+		goto cleanup;
 	}
-	memset(info, 0, sizeof(CONTROLVM_PAYLOAD_INFO));
+	memset(info, 0, sizeof(struct controlvm_payload_info));
 	if ((offset == 0) || (bytes == 0)) {
-		LOGERR("CONTROLVM_PAYLOAD_INIT Failed: request_payload_offset=%llu request_payload_bytes=%llu!",
-		     (u64) offset, (u64) bytes);
 		rc = -CONTROLVM_RESP_ERROR_PAYLOAD_INVALID;
-		goto Away;
+		goto cleanup;
 	}
 	payload = ioremap_cache(phys_addr + offset, bytes);
-	if (payload == NULL) {
-		LOGERR("CONTROLVM_PAYLOAD_INIT Failed: ioremap_cache %llu for %llu bytes failed",
-		     (u64) offset, (u64) bytes);
+	if (!payload) {
 		rc = -CONTROLVM_RESP_ERROR_IOREMAP_FAILED;
-		goto Away;
+		goto cleanup;
 	}
 
 	info->offset = offset;
 	info->bytes = bytes;
 	info->ptr = payload;
-	LOGINF("offset=%llu, bytes=%lu, ptr=%p",
-	       (u64) (info->offset), (ulong) (info->bytes), info->ptr);
 
-Away:
+cleanup:
 	if (rc < 0) {
-		if (payload != NULL) {
+		if (payload) {
 			iounmap(payload);
 			payload = NULL;
 		}
@@ -1427,43 +1305,41 @@ Away:
 }
 
 static void
-destroy_controlvm_payload_info(CONTROLVM_PAYLOAD_INFO *info)
+destroy_controlvm_payload_info(struct controlvm_payload_info *info)
 {
-	if (info->ptr != NULL) {
+	if (info->ptr) {
 		iounmap(info->ptr);
 		info->ptr = NULL;
 	}
-	memset(info, 0, sizeof(CONTROLVM_PAYLOAD_INFO));
+	memset(info, 0, sizeof(struct controlvm_payload_info));
 }
 
 static void
 initialize_controlvm_payload(void)
 {
-	HOSTADDRESS phys_addr = visorchannel_get_physaddr(ControlVm_channel);
-	u64 payloadOffset = 0;
-	u32 payloadBytes = 0;
+	HOSTADDRESS phys_addr = visorchannel_get_physaddr(controlvm_channel);
+	u64 payload_offset = 0;
+	u32 payload_bytes = 0;
 
-	if (visorchannel_read(ControlVm_channel,
+	if (visorchannel_read(controlvm_channel,
 			      offsetof(struct spar_controlvm_channel_protocol,
 				       request_payload_offset),
-			      &payloadOffset, sizeof(payloadOffset)) < 0) {
-		LOGERR("CONTROLVM_PAYLOAD_INIT Failed to read controlvm channel!");
+			      &payload_offset, sizeof(payload_offset)) < 0) {
 		POSTCODE_LINUX_2(CONTROLVM_INIT_FAILURE_PC,
 				 POSTCODE_SEVERITY_ERR);
 		return;
 	}
-	if (visorchannel_read(ControlVm_channel,
+	if (visorchannel_read(controlvm_channel,
 			      offsetof(struct spar_controlvm_channel_protocol,
 				       request_payload_bytes),
-			      &payloadBytes, sizeof(payloadBytes)) < 0) {
-		LOGERR("CONTROLVM_PAYLOAD_INIT Failed to read controlvm channel!");
+			      &payload_bytes, sizeof(payload_bytes)) < 0) {
 		POSTCODE_LINUX_2(CONTROLVM_INIT_FAILURE_PC,
 				 POSTCODE_SEVERITY_ERR);
 		return;
 	}
 	initialize_controlvm_payload_info(phys_addr,
-					  payloadOffset, payloadBytes,
-					  &ControlVm_payload_info);
+					  payload_offset, payload_bytes,
+					  &controlvm_payload_info);
 }
 
 /*  Send ACTION=online for DEVPATH=/sys/devices/platform/visorchipset.
@@ -1472,7 +1348,7 @@ initialize_controlvm_payload(void)
 int
 visorchipset_chipset_ready(void)
 {
-	kobject_uevent(&Visorchipset_platform_device.dev.kobj, KOBJ_ONLINE);
+	kobject_uevent(&visorchipset_platform_device.dev.kobj, KOBJ_ONLINE);
 	return CONTROLVM_RESP_SUCCESS;
 }
 EXPORT_SYMBOL_GPL(visorchipset_chipset_ready);
@@ -1484,7 +1360,7 @@ visorchipset_chipset_selftest(void)
 	char *envp[] = { env_selftest, NULL };
 
 	sprintf(env_selftest, "SPARSP_SELFTEST=%d", 1);
-	kobject_uevent_env(&Visorchipset_platform_device.dev.kobj, KOBJ_CHANGE,
+	kobject_uevent_env(&visorchipset_platform_device.dev.kobj, KOBJ_CHANGE,
 			   envp);
 	return CONTROLVM_RESP_SUCCESS;
 }
@@ -1496,49 +1372,48 @@ EXPORT_SYMBOL_GPL(visorchipset_chipset_selftest);
 int
 visorchipset_chipset_notready(void)
 {
-	kobject_uevent(&Visorchipset_platform_device.dev.kobj, KOBJ_OFFLINE);
+	kobject_uevent(&visorchipset_platform_device.dev.kobj, KOBJ_OFFLINE);
 	return CONTROLVM_RESP_SUCCESS;
 }
 EXPORT_SYMBOL_GPL(visorchipset_chipset_notready);
 
 static void
-chipset_ready(struct controlvm_message_header *msgHdr)
+chipset_ready(struct controlvm_message_header *msg_hdr)
 {
 	int rc = visorchipset_chipset_ready();
 
 	if (rc != CONTROLVM_RESP_SUCCESS)
 		rc = -rc;
-	if (msgHdr->flags.response_expected && !visorchipset_holdchipsetready)
-		controlvm_respond(msgHdr, rc);
-	if (msgHdr->flags.response_expected && visorchipset_holdchipsetready) {
+	if (msg_hdr->flags.response_expected && !visorchipset_holdchipsetready)
+		controlvm_respond(msg_hdr, rc);
+	if (msg_hdr->flags.response_expected && visorchipset_holdchipsetready) {
 		/* Send CHIPSET_READY response when all modules have been loaded
 		 * and disks mounted for the partition
 		 */
-		g_ChipSetMsgHdr = *msgHdr;
-		LOGINF("Holding CHIPSET_READY response");
+		g_chipset_msg_hdr = *msg_hdr;
 	}
 }
 
 static void
-chipset_selftest(struct controlvm_message_header *msgHdr)
+chipset_selftest(struct controlvm_message_header *msg_hdr)
 {
 	int rc = visorchipset_chipset_selftest();
 
 	if (rc != CONTROLVM_RESP_SUCCESS)
 		rc = -rc;
-	if (msgHdr->flags.response_expected)
-		controlvm_respond(msgHdr, rc);
+	if (msg_hdr->flags.response_expected)
+		controlvm_respond(msg_hdr, rc);
 }
 
 static void
-chipset_notready(struct controlvm_message_header *msgHdr)
+chipset_notready(struct controlvm_message_header *msg_hdr)
 {
 	int rc = visorchipset_chipset_notready();
 
 	if (rc != CONTROLVM_RESP_SUCCESS)
 		rc = -rc;
-	if (msgHdr->flags.response_expected)
-		controlvm_respond(msgHdr, rc);
+	if (msg_hdr->flags.response_expected)
+		controlvm_respond(msg_hdr, rc);
 }
 
 /* This is your "one-stop" shop for grabbing the next message from the
@@ -1547,14 +1422,11 @@ chipset_notready(struct controlvm_message_header *msgHdr)
 static BOOL
 read_controlvm_event(struct controlvm_message *msg)
 {
-	if (visorchannel_signalremove(ControlVm_channel,
+	if (visorchannel_signalremove(controlvm_channel,
 				      CONTROLVM_QUEUE_EVENT, msg)) {
 		/* got a message */
-		if (msg->hdr.flags.test_message == 1) {
-			LOGERR("ignoring bad CONTROLVM_QUEUE_EVENT msg with controlvm_msg_id=0x%x because Flags.testMessage is nonsensical (=1)",
-			       msg->hdr.id);
+		if (msg->hdr.flags.test_message == 1)
 			return FALSE;
-		}
 		return TRUE;
 	}
 	return FALSE;
@@ -1604,10 +1476,10 @@ parahotplug_next_expiration(void)
 static struct parahotplug_request *
 parahotplug_request_create(struct controlvm_message *msg)
 {
-	struct parahotplug_request *req =
-	    kmalloc(sizeof(struct parahotplug_request),
-		    GFP_KERNEL|__GFP_NORETRY);
-	if (req == NULL)
+	struct parahotplug_request *req;
+
+	req = kmalloc(sizeof(*req), GFP_KERNEL | __GFP_NORETRY);
+	if (!req)
 		return NULL;
 
 	req->id = parahotplug_next_id();
@@ -1652,13 +1524,7 @@ parahotplug_request_kickoff(struct parahotplug_request *req)
 	sprintf(env_func, "SPAR_PARAHOTPLUG_FUNCTION=%d",
 		cmd->device_change_state.dev_no & 0x7);
 
-	LOGINF("parahotplug_request_kickoff: state=%d, bdf=%d/%d/%d, id=%u\n",
-	       cmd->device_change_state.state.active,
-	       cmd->device_change_state.bus_no,
-	       cmd->device_change_state.dev_no >> 3,
-	       cmd->device_change_state.dev_no & 7, req->id);
-
-	kobject_uevent_env(&Visorchipset_platform_device.dev.kobj, KOBJ_CHANGE,
+	kobject_uevent_env(&visorchipset_platform_device.dev.kobj, KOBJ_CHANGE,
 			   envp);
 }
 
@@ -1672,23 +1538,25 @@ parahotplug_process_list(void)
 	struct list_head *pos = NULL;
 	struct list_head *tmp = NULL;
 
-	spin_lock(&Parahotplug_request_list_lock);
+	spin_lock(&parahotplug_request_list_lock);
 
-	list_for_each_safe(pos, tmp, &Parahotplug_request_list) {
+	list_for_each_safe(pos, tmp, &parahotplug_request_list) {
 		struct parahotplug_request *req =
 		    list_entry(pos, struct parahotplug_request, list);
-		if (time_after_eq(jiffies, req->expiration)) {
-			list_del(pos);
-			if (req->msg.hdr.flags.response_expected)
-				controlvm_respond_physdev_changestate(
-					&req->msg.hdr,
-					CONTROLVM_RESP_ERROR_DEVICE_UDEV_TIMEOUT,
-					req->msg.cmd.device_change_state.state);
-			parahotplug_request_destroy(req);
-		}
+
+		if (!time_after_eq(jiffies, req->expiration))
+			continue;
+
+		list_del(pos);
+		if (req->msg.hdr.flags.response_expected)
+			controlvm_respond_physdev_changestate(
+				&req->msg.hdr,
+				CONTROLVM_RESP_ERROR_DEVICE_UDEV_TIMEOUT,
+				req->msg.cmd.device_change_state.state);
+		parahotplug_request_destroy(req);
 	}
 
-	spin_unlock(&Parahotplug_request_list_lock);
+	spin_unlock(&parahotplug_request_list_lock);
 }
 
 /*
@@ -1702,10 +1570,10 @@ parahotplug_request_complete(int id, u16 active)
 	struct list_head *pos = NULL;
 	struct list_head *tmp = NULL;
 
-	spin_lock(&Parahotplug_request_list_lock);
+	spin_lock(&parahotplug_request_list_lock);
 
 	/* Look for a request matching "id". */
-	list_for_each_safe(pos, tmp, &Parahotplug_request_list) {
+	list_for_each_safe(pos, tmp, &parahotplug_request_list) {
 		struct parahotplug_request *req =
 		    list_entry(pos, struct parahotplug_request, list);
 		if (req->id == id) {
@@ -1713,7 +1581,7 @@ parahotplug_request_complete(int id, u16 active)
 			 * respond.
 			 */
 			list_del(pos);
-			spin_unlock(&Parahotplug_request_list_lock);
+			spin_unlock(&parahotplug_request_list_lock);
 			req->msg.cmd.device_change_state.state.active = active;
 			if (req->msg.hdr.flags.response_expected)
 				controlvm_respond_physdev_changestate(
@@ -1724,7 +1592,7 @@ parahotplug_request_complete(int id, u16 active)
 		}
 	}
 
-	spin_unlock(&Parahotplug_request_list_lock);
+	spin_unlock(&parahotplug_request_list_lock);
 	return -1;
 }
 
@@ -1738,10 +1606,8 @@ parahotplug_process_message(struct controlvm_message *inmsg)
 
 	req = parahotplug_request_create(inmsg);
 
-	if (req == NULL) {
-		LOGERR("parahotplug_process_message: couldn't allocate request");
+	if (!req)
 		return;
-	}
 
 	if (inmsg->cmd.device_change_state.state.active) {
 		/* For enable messages, just respond with success
@@ -1755,8 +1621,8 @@ parahotplug_process_message(struct controlvm_message *inmsg)
 		*/
 		parahotplug_request_kickoff(req);
 		controlvm_respond_physdev_changestate(&inmsg->hdr,
-				CONTROLVM_RESP_SUCCESS, inmsg->cmd.
-				device_change_state.state);
+			CONTROLVM_RESP_SUCCESS,
+			inmsg->cmd.device_change_state.state);
 		parahotplug_request_destroy(req);
 	} else {
 		/* For disable messages, add the request to the
@@ -1764,9 +1630,9 @@ parahotplug_process_message(struct controlvm_message *inmsg)
 		* won't get responded to until the script has
 		* indicated it's done.
 		*/
-		spin_lock(&Parahotplug_request_list_lock);
-		list_add_tail(&(req->list), &Parahotplug_request_list);
-		spin_unlock(&Parahotplug_request_list_lock);
+		spin_lock(&parahotplug_request_list_lock);
+		list_add_tail(&req->list, &parahotplug_request_list);
+		spin_unlock(&parahotplug_request_list_lock);
 
 		parahotplug_request_kickoff(req);
 	}
@@ -1787,135 +1653,94 @@ static BOOL
 handle_command(struct controlvm_message inmsg, HOSTADDRESS channel_addr)
 {
 	struct controlvm_message_packet *cmd = &inmsg.cmd;
-	u64 parametersAddr = 0;
-	u32 parametersBytes = 0;
-	PARSER_CONTEXT *parser_ctx = NULL;
-	BOOL isLocalAddr = FALSE;
+	u64 parm_addr = 0;
+	u32 parm_bytes = 0;
+	struct parser_context *parser_ctx = NULL;
+	bool local_addr = false;
 	struct controlvm_message ackmsg;
 
 	/* create parsing context if necessary */
-	isLocalAddr = (inmsg.hdr.flags.test_message == 1);
-	if (channel_addr == 0) {
-		LOGERR("HUH? channel_addr is 0!");
+	local_addr = (inmsg.hdr.flags.test_message == 1);
+	if (channel_addr == 0)
 		return TRUE;
-	}
-	parametersAddr = channel_addr + inmsg.hdr.payload_vm_offset;
-	parametersBytes = inmsg.hdr.payload_bytes;
+	parm_addr = channel_addr + inmsg.hdr.payload_vm_offset;
+	parm_bytes = inmsg.hdr.payload_bytes;
 
 	/* Parameter and channel addresses within test messages actually lie
 	 * within our OS-controlled memory.  We need to know that, because it
 	 * makes a difference in how we compute the virtual address.
 	 */
-	if (parametersAddr != 0 && parametersBytes != 0) {
+	if (parm_addr != 0 && parm_bytes != 0) {
 		BOOL retry = FALSE;
 
 		parser_ctx =
-		    parser_init_byteStream(parametersAddr, parametersBytes,
-					   isLocalAddr, &retry);
-		if (!parser_ctx) {
-			if (retry) {
-				LOGWRN("throttling to copy payload");
-				return FALSE;
-			}
-			LOGWRN("parsing failed");
-			LOGWRN("inmsg.hdr.Id=0x%lx", (ulong) inmsg.hdr.id);
-			LOGWRN("parametersAddr=0x%llx", (u64) parametersAddr);
-			LOGWRN("parametersBytes=%lu", (ulong) parametersBytes);
-			LOGWRN("isLocalAddr=%d", isLocalAddr);
-		}
+		    parser_init_byte_stream(parm_addr, parm_bytes,
+					    local_addr, &retry);
+		if (!parser_ctx && retry)
+			return FALSE;
 	}
 
-	if (!isLocalAddr) {
+	if (!local_addr) {
 		controlvm_init_response(&ackmsg, &inmsg.hdr,
 					CONTROLVM_RESP_SUCCESS);
-		if ((ControlVm_channel)
-		    &&
-		    (!visorchannel_signalinsert
-		     (ControlVm_channel, CONTROLVM_QUEUE_ACK, &ackmsg)))
-			LOGWRN("failed to send ACK failed");
+		if (controlvm_channel)
+			visorchannel_signalinsert(controlvm_channel,
+						  CONTROLVM_QUEUE_ACK,
+						  &ackmsg);
 	}
 	switch (inmsg.hdr.id) {
 	case CONTROLVM_CHIPSET_INIT:
-		LOGINF("CHIPSET_INIT(#busses=%lu,#switches=%lu)",
-		       (ulong) inmsg.cmd.init_chipset.bus_count,
-		       (ulong) inmsg.cmd.init_chipset.switch_count);
 		chipset_init(&inmsg);
 		break;
 	case CONTROLVM_BUS_CREATE:
-		LOGINF("BUS_CREATE(%lu,#devs=%lu)",
-		       (ulong) cmd->create_bus.bus_no,
-		       (ulong) cmd->create_bus.dev_count);
 		bus_create(&inmsg);
 		break;
 	case CONTROLVM_BUS_DESTROY:
-		LOGINF("BUS_DESTROY(%lu)", (ulong) cmd->destroy_bus.bus_no);
 		bus_destroy(&inmsg);
 		break;
 	case CONTROLVM_BUS_CONFIGURE:
-		LOGINF("BUS_CONFIGURE(%lu)", (ulong) cmd->configure_bus.bus_no);
 		bus_configure(&inmsg, parser_ctx);
 		break;
 	case CONTROLVM_DEVICE_CREATE:
-		LOGINF("DEVICE_CREATE(%lu,%lu)",
-		       (ulong) cmd->create_device.bus_no,
-		       (ulong) cmd->create_device.dev_no);
 		my_device_create(&inmsg);
 		break;
 	case CONTROLVM_DEVICE_CHANGESTATE:
 		if (cmd->device_change_state.flags.phys_device) {
-			LOGINF("DEVICE_CHANGESTATE for physical device (%lu,%lu, active=%lu)",
-			     (ulong) cmd->device_change_state.bus_no,
-			     (ulong) cmd->device_change_state.dev_no,
-			     (ulong) cmd->device_change_state.state.active);
 			parahotplug_process_message(&inmsg);
 		} else {
-			LOGINF("DEVICE_CHANGESTATE for virtual device (%lu,%lu, state.Alive=0x%lx)",
-			     (ulong) cmd->device_change_state.bus_no,
-			     (ulong) cmd->device_change_state.dev_no,
-			     (ulong) cmd->device_change_state.state.alive);
 			/* save the hdr and cmd structures for later use */
 			/* when sending back the response to Command */
 			my_device_changestate(&inmsg);
-			g_DiagMsgHdr = inmsg.hdr;
-			g_DeviceChangeStatePacket = inmsg.cmd;
+			g_diag_msg_hdr = inmsg.hdr;
+			g_devicechangestate_packet = inmsg.cmd;
 			break;
 		}
 		break;
 	case CONTROLVM_DEVICE_DESTROY:
-		LOGINF("DEVICE_DESTROY(%lu,%lu)",
-		       (ulong) cmd->destroy_device.bus_no,
-		       (ulong) cmd->destroy_device.dev_no);
 		my_device_destroy(&inmsg);
 		break;
 	case CONTROLVM_DEVICE_CONFIGURE:
-		LOGINF("DEVICE_CONFIGURE(%lu,%lu)",
-		       (ulong) cmd->configure_device.bus_no,
-		       (ulong) cmd->configure_device.dev_no);
 		/* no op for now, just send a respond that we passed */
 		if (inmsg.hdr.flags.response_expected)
 			controlvm_respond(&inmsg.hdr, CONTROLVM_RESP_SUCCESS);
 		break;
 	case CONTROLVM_CHIPSET_READY:
-		LOGINF("CHIPSET_READY");
 		chipset_ready(&inmsg.hdr);
 		break;
 	case CONTROLVM_CHIPSET_SELFTEST:
-		LOGINF("CHIPSET_SELFTEST");
 		chipset_selftest(&inmsg.hdr);
 		break;
 	case CONTROLVM_CHIPSET_STOP:
-		LOGINF("CHIPSET_STOP");
 		chipset_notready(&inmsg.hdr);
 		break;
 	default:
-		LOGERR("unrecognized controlvm cmd=%d", (int) inmsg.hdr.id);
 		if (inmsg.hdr.flags.response_expected)
 			controlvm_respond(&inmsg.hdr,
-					  -CONTROLVM_RESP_ERROR_MESSAGE_ID_UNKNOWN);
+				-CONTROLVM_RESP_ERROR_MESSAGE_ID_UNKNOWN);
 		break;
 	}
 
-	if (parser_ctx != NULL) {
+	if (parser_ctx) {
 		parser_done(parser_ctx);
 		parser_ctx = NULL;
 	}
@@ -1927,12 +1752,9 @@ static HOSTADDRESS controlvm_get_channel_address(void)
 	u64 addr = 0;
 	u32 size = 0;
 
-	if (!VMCALL_SUCCESSFUL(issue_vmcall_io_controlvm_addr(&addr, &size))) {
-		ERRDRV("%s - vmcall to determine controlvm channel addr failed",
-		       __func__);
+	if (!VMCALL_SUCCESSFUL(issue_vmcall_io_controlvm_addr(&addr, &size)))
 		return 0;
-	}
-	INFODRV("controlvm addr=%Lx", addr);
+
 	return addr;
 }
 
@@ -1940,69 +1762,63 @@ static void
 controlvm_periodic_work(struct work_struct *work)
 {
 	struct controlvm_message inmsg;
-	BOOL gotACommand = FALSE;
+	BOOL got_command = FALSE;
 	BOOL handle_command_failed = FALSE;
-	static u64 Poll_Count;
+	static u64 poll_count;
 
 	/* make sure visorbus server is registered for controlvm callbacks */
 	if (visorchipset_serverregwait && !serverregistered)
-		goto Away;
+		goto cleanup;
 	/* make sure visorclientbus server is regsitered for controlvm
 	 * callbacks
 	 */
 	if (visorchipset_clientregwait && !clientregistered)
-		goto Away;
+		goto cleanup;
 
-	Poll_Count++;
-	if (Poll_Count >= 250)
+	poll_count++;
+	if (poll_count >= 250)
 		;	/* keep going */
 	else
-		goto Away;
+		goto cleanup;
 
 	/* Check events to determine if response to CHIPSET_READY
 	 * should be sent
 	 */
-	if (visorchipset_holdchipsetready
-	    && (g_ChipSetMsgHdr.id != CONTROLVM_INVALID)) {
+	if (visorchipset_holdchipsetready &&
+	    (g_chipset_msg_hdr.id != CONTROLVM_INVALID)) {
 		if (check_chipset_events() == 1) {
-			LOGINF("Sending CHIPSET_READY response");
-			controlvm_respond(&g_ChipSetMsgHdr, 0);
+			controlvm_respond(&g_chipset_msg_hdr, 0);
 			clear_chipset_events();
-			memset(&g_ChipSetMsgHdr, 0,
+			memset(&g_chipset_msg_hdr, 0,
 			       sizeof(struct controlvm_message_header));
 		}
 	}
 
-	while (visorchannel_signalremove(ControlVm_channel,
+	while (visorchannel_signalremove(controlvm_channel,
 					 CONTROLVM_QUEUE_RESPONSE,
-					 &inmsg)) {
-		if (inmsg.hdr.payload_max_bytes != 0) {
-			LOGERR("Payload of size %lu returned @%lu with unexpected message id %d.",
-			     (ulong) inmsg.hdr.payload_max_bytes,
-			     (ulong) inmsg.hdr.payload_vm_offset,
-			     inmsg.hdr.id);
-		}
-	}
-	if (!gotACommand) {
-		if (ControlVm_Pending_Msg_Valid) {
+					 &inmsg))
+		;
+	if (!got_command) {
+		if (controlvm_pending_msg_valid) {
 			/* we throttled processing of a prior
 			* msg, so try to process it again
 			* rather than reading a new one
 			*/
-			inmsg = ControlVm_Pending_Msg;
-			ControlVm_Pending_Msg_Valid = FALSE;
-			gotACommand = TRUE;
-		} else
-			gotACommand = read_controlvm_event(&inmsg);
+			inmsg = controlvm_pending_msg;
+			controlvm_pending_msg_valid = FALSE;
+			got_command = true;
+		} else {
+			got_command = read_controlvm_event(&inmsg);
+		}
 	}
 
 	handle_command_failed = FALSE;
-	while (gotACommand && (!handle_command_failed)) {
-		Most_recent_message_jiffies = jiffies;
+	while (got_command && (!handle_command_failed)) {
+		most_recent_message_jiffies = jiffies;
 		if (handle_command(inmsg,
 				   visorchannel_get_physaddr
-				   (ControlVm_channel)))
-			gotACommand = read_controlvm_event(&inmsg);
+				   (controlvm_channel)))
+			got_command = read_controlvm_event(&inmsg);
 		else {
 			/* this is a scenario where throttling
 			* is required, but probably NOT an
@@ -2011,56 +1827,51 @@ controlvm_periodic_work(struct work_struct *work)
 			* reprocess it on our next loop
 			*/
 			handle_command_failed = TRUE;
-			ControlVm_Pending_Msg = inmsg;
-			ControlVm_Pending_Msg_Valid = TRUE;
+			controlvm_pending_msg = inmsg;
+			controlvm_pending_msg_valid = TRUE;
 		}
 	}
 
 	/* parahotplug_worker */
 	parahotplug_process_list();
 
-Away:
+cleanup:
 
 	if (time_after(jiffies,
-		       Most_recent_message_jiffies + (HZ * MIN_IDLE_SECONDS))) {
+		       most_recent_message_jiffies + (HZ * MIN_IDLE_SECONDS))) {
 		/* it's been longer than MIN_IDLE_SECONDS since we
 		* processed our last controlvm message; slow down the
 		* polling
 		*/
-		if (Poll_jiffies != POLLJIFFIES_CONTROLVMCHANNEL_SLOW) {
-			LOGINF("switched to slow controlvm polling");
-			Poll_jiffies = POLLJIFFIES_CONTROLVMCHANNEL_SLOW;
-		}
+		if (poll_jiffies != POLLJIFFIES_CONTROLVMCHANNEL_SLOW)
+			poll_jiffies = POLLJIFFIES_CONTROLVMCHANNEL_SLOW;
 	} else {
-		if (Poll_jiffies != POLLJIFFIES_CONTROLVMCHANNEL_FAST) {
-			Poll_jiffies = POLLJIFFIES_CONTROLVMCHANNEL_FAST;
-			LOGINF("switched to fast controlvm polling");
-		}
+		if (poll_jiffies != POLLJIFFIES_CONTROLVMCHANNEL_FAST)
+			poll_jiffies = POLLJIFFIES_CONTROLVMCHANNEL_FAST;
 	}
 
-	queue_delayed_work(Periodic_controlvm_workqueue,
-			   &Periodic_controlvm_work, Poll_jiffies);
+	queue_delayed_work(periodic_controlvm_workqueue,
+			   &periodic_controlvm_work, poll_jiffies);
 }
 
 static void
 setup_crash_devices_work_queue(struct work_struct *work)
 {
-
-	struct controlvm_message localCrashCreateBusMsg;
-	struct controlvm_message localCrashCreateDevMsg;
+	struct controlvm_message local_crash_bus_msg;
+	struct controlvm_message local_crash_dev_msg;
 	struct controlvm_message msg;
-	u32 localSavedCrashMsgOffset;
-	u16 localSavedCrashMsgCount;
+	u32 local_crash_msg_offset;
+	u16 local_crash_msg_count;
 
 	/* make sure visorbus server is registered for controlvm callbacks */
 	if (visorchipset_serverregwait && !serverregistered)
-		goto Away;
+		goto cleanup;
 
 	/* make sure visorclientbus server is regsitered for controlvm
 	 * callbacks
 	 */
 	if (visorchipset_clientregwait && !clientregistered)
-		goto Away;
+		goto cleanup;
 
 	POSTCODE_LINUX_2(CRASH_DEV_ENTRY_PC, POSTCODE_SEVERITY_INFO);
 
@@ -2072,118 +1883,108 @@ setup_crash_devices_work_queue(struct work_struct *work)
 	chipset_init(&msg);
 
 	/* get saved message count */
-	if (visorchannel_read(ControlVm_channel,
+	if (visorchannel_read(controlvm_channel,
 			      offsetof(struct spar_controlvm_channel_protocol,
 				       saved_crash_message_count),
-			      &localSavedCrashMsgCount, sizeof(u16)) < 0) {
-		LOGERR("failed to get Saved Message Count");
+			      &local_crash_msg_count, sizeof(u16)) < 0) {
 		POSTCODE_LINUX_2(CRASH_DEV_CTRL_RD_FAILURE_PC,
 				 POSTCODE_SEVERITY_ERR);
 		return;
 	}
 
-	if (localSavedCrashMsgCount != CONTROLVM_CRASHMSG_MAX) {
-		LOGERR("Saved Message Count incorrect %d",
-		       localSavedCrashMsgCount);
+	if (local_crash_msg_count != CONTROLVM_CRASHMSG_MAX) {
 		POSTCODE_LINUX_3(CRASH_DEV_COUNT_FAILURE_PC,
-				 localSavedCrashMsgCount,
+				 local_crash_msg_count,
 				 POSTCODE_SEVERITY_ERR);
 		return;
 	}
 
 	/* get saved crash message offset */
-	if (visorchannel_read(ControlVm_channel,
+	if (visorchannel_read(controlvm_channel,
 			      offsetof(struct spar_controlvm_channel_protocol,
 				       saved_crash_message_offset),
-			      &localSavedCrashMsgOffset, sizeof(u32)) < 0) {
-		LOGERR("failed to get Saved Message Offset");
+			      &local_crash_msg_offset, sizeof(u32)) < 0) {
 		POSTCODE_LINUX_2(CRASH_DEV_CTRL_RD_FAILURE_PC,
 				 POSTCODE_SEVERITY_ERR);
 		return;
 	}
 
 	/* read create device message for storage bus offset */
-	if (visorchannel_read(ControlVm_channel,
-			      localSavedCrashMsgOffset,
-			      &localCrashCreateBusMsg,
+	if (visorchannel_read(controlvm_channel,
+			      local_crash_msg_offset,
+			      &local_crash_bus_msg,
 			      sizeof(struct controlvm_message)) < 0) {
-		LOGERR("CRASH_DEV_RD_BUS_FAIULRE: Failed to read CrashCreateBusMsg!");
 		POSTCODE_LINUX_2(CRASH_DEV_RD_BUS_FAIULRE_PC,
 				 POSTCODE_SEVERITY_ERR);
 		return;
 	}
 
 	/* read create device message for storage device */
-	if (visorchannel_read(ControlVm_channel,
-			      localSavedCrashMsgOffset +
+	if (visorchannel_read(controlvm_channel,
+			      local_crash_msg_offset +
 			      sizeof(struct controlvm_message),
-			      &localCrashCreateDevMsg,
+			      &local_crash_dev_msg,
 			      sizeof(struct controlvm_message)) < 0) {
-		LOGERR("CRASH_DEV_RD_DEV_FAIULRE: Failed to read CrashCreateDevMsg!");
 		POSTCODE_LINUX_2(CRASH_DEV_RD_DEV_FAIULRE_PC,
 				 POSTCODE_SEVERITY_ERR);
 		return;
 	}
 
 	/* reuse IOVM create bus message */
-	if (localCrashCreateBusMsg.cmd.create_bus.channel_addr != 0)
-		bus_create(&localCrashCreateBusMsg);
-	else {
-		LOGERR("CrashCreateBusMsg is null, no dump will be taken");
+	if (local_crash_bus_msg.cmd.create_bus.channel_addr != 0) {
+		bus_create(&local_crash_bus_msg);
+	} else {
 		POSTCODE_LINUX_2(CRASH_DEV_BUS_NULL_FAILURE_PC,
 				 POSTCODE_SEVERITY_ERR);
 		return;
 	}
 
 	/* reuse create device message for storage device */
-	if (localCrashCreateDevMsg.cmd.create_device.channel_addr != 0)
-		my_device_create(&localCrashCreateDevMsg);
-	else {
-		LOGERR("CrashCreateDevMsg is null, no dump will be taken");
+	if (local_crash_dev_msg.cmd.create_device.channel_addr != 0) {
+		my_device_create(&local_crash_dev_msg);
+	} else {
 		POSTCODE_LINUX_2(CRASH_DEV_DEV_NULL_FAILURE_PC,
 				 POSTCODE_SEVERITY_ERR);
 		return;
 	}
-	LOGINF("Bus and device ready for dumping");
 	POSTCODE_LINUX_2(CRASH_DEV_EXIT_PC, POSTCODE_SEVERITY_INFO);
 	return;
 
-Away:
+cleanup:
 
-	Poll_jiffies = POLLJIFFIES_CONTROLVMCHANNEL_SLOW;
+	poll_jiffies = POLLJIFFIES_CONTROLVMCHANNEL_SLOW;
 
-	queue_delayed_work(Periodic_controlvm_workqueue,
-			   &Periodic_controlvm_work, Poll_jiffies);
+	queue_delayed_work(periodic_controlvm_workqueue,
+			   &periodic_controlvm_work, poll_jiffies);
 }
 
 static void
-bus_create_response(ulong busNo, int response)
+bus_create_response(ulong bus_no, int response)
 {
-	bus_responder(CONTROLVM_BUS_CREATE, busNo, response);
+	bus_responder(CONTROLVM_BUS_CREATE, bus_no, response);
 }
 
 static void
-bus_destroy_response(ulong busNo, int response)
+bus_destroy_response(ulong bus_no, int response)
 {
-	bus_responder(CONTROLVM_BUS_DESTROY, busNo, response);
+	bus_responder(CONTROLVM_BUS_DESTROY, bus_no, response);
 }
 
 static void
-device_create_response(ulong busNo, ulong devNo, int response)
+device_create_response(ulong bus_no, ulong dev_no, int response)
 {
-	device_responder(CONTROLVM_DEVICE_CREATE, busNo, devNo, response);
+	device_responder(CONTROLVM_DEVICE_CREATE, bus_no, dev_no, response);
 }
 
 static void
-device_destroy_response(ulong busNo, ulong devNo, int response)
+device_destroy_response(ulong bus_no, ulong dev_no, int response)
 {
-	device_responder(CONTROLVM_DEVICE_DESTROY, busNo, devNo, response);
+	device_responder(CONTROLVM_DEVICE_DESTROY, bus_no, dev_no, response);
 }
 
 void
 visorchipset_device_pause_response(ulong bus_no, ulong dev_no, int response)
 {
-
 	device_changestate_responder(CONTROLVM_DEVICE_CHANGESTATE,
 				     bus_no, dev_no, response,
 				     segment_state_standby);
@@ -2191,22 +1992,20 @@ visorchipset_device_pause_response(ulong bus_no, ulong dev_no, int response)
 EXPORT_SYMBOL_GPL(visorchipset_device_pause_response);
 
 static void
-device_resume_response(ulong busNo, ulong devNo, int response)
+device_resume_response(ulong bus_no, ulong dev_no, int response)
 {
 	device_changestate_responder(CONTROLVM_DEVICE_CHANGESTATE,
-				     busNo, devNo, response,
+				     bus_no, dev_no, response,
 				     segment_state_running);
 }
 
 BOOL
 visorchipset_get_bus_info(ulong bus_no, struct visorchipset_bus_info *bus_info)
 {
-	void *p = findbus(&BusInfoList, bus_no);
+	void *p = findbus(&bus_info_list, bus_no);
 
-	if (!p) {
-		LOGERR("(%lu) failed", bus_no);
+	if (!p)
 		return FALSE;
-	}
 	memcpy(bus_info, p, sizeof(struct visorchipset_bus_info));
 	return TRUE;
 }
@@ -2215,12 +2014,10 @@ EXPORT_SYMBOL_GPL(visorchipset_get_bus_info);
 BOOL
 visorchipset_set_bus_context(ulong bus_no, void *context)
 {
-	struct visorchipset_bus_info *p = findbus(&BusInfoList, bus_no);
+	struct visorchipset_bus_info *p = findbus(&bus_info_list, bus_no);
 
-	if (!p) {
-		LOGERR("(%lu) failed", bus_no);
+	if (!p)
 		return FALSE;
-	}
 	p->bus_driver_context = context;
 	return TRUE;
 }
@@ -2230,12 +2027,10 @@ BOOL
 visorchipset_get_device_info(ulong bus_no, ulong dev_no,
 			     struct visorchipset_device_info *dev_info)
 {
-	void *p = finddevice(&DevInfoList, bus_no, dev_no);
+	void *p = finddevice(&dev_info_list, bus_no, dev_no);
 
-	if (!p) {
-		LOGERR("(%lu,%lu) failed", bus_no, dev_no);
+	if (!p)
 		return FALSE;
-	}
 	memcpy(dev_info, p, sizeof(struct visorchipset_device_info));
 	return TRUE;
 }
@@ -2245,12 +2040,10 @@ BOOL
 visorchipset_set_device_context(ulong bus_no, ulong dev_no, void *context)
 {
 	struct visorchipset_device_info *p =
-			finddevice(&DevInfoList, bus_no, dev_no);
+			finddevice(&dev_info_list, bus_no, dev_no);
 
-	if (!p) {
-		LOGERR("(%lu,%lu) failed", bus_no, dev_no);
+	if (!p)
 		return FALSE;
-	}
 	p->bus_driver_context = context;
 	return TRUE;
 }
@@ -2278,11 +2071,10 @@ visorchipset_cache_alloc(struct kmem_cache *pool, BOOL ok_to_block,
 	 */
 	gfp |= __GFP_NORETRY;
 	p = kmem_cache_alloc(pool, gfp);
-	if (!p) {
-		LOGERR("kmem_cache_alloc failed early @%s:%d\n", fn, ln);
+	if (!p)
 		return NULL;
-	}
-	atomic_inc(&Visorchipset_cache_buffers_in_use);
+
+	atomic_inc(&visorchipset_cache_buffers_in_use);
 	return p;
 }
 
@@ -2291,16 +2083,16 @@ visorchipset_cache_alloc(struct kmem_cache *pool, BOOL ok_to_block,
 void
 visorchipset_cache_free(struct kmem_cache *pool, void *p, char *fn, int ln)
 {
-	if (!p) {
-		LOGERR("NULL pointer @%s:%d\n", fn, ln);
+	if (!p)
 		return;
-	}
-	atomic_dec(&Visorchipset_cache_buffers_in_use);
+
+	atomic_dec(&visorchipset_cache_buffers_in_use);
 	kmem_cache_free(pool, p);
 }
 
 static ssize_t chipsetready_store(struct device *dev,
-	struct device_attribute *attr, const char *buf, size_t count)
+				  struct device_attribute *attr,
+				  const char *buf, size_t count)
 {
 	char msgtype[64];
 
@@ -2322,7 +2114,8 @@ static ssize_t chipsetready_store(struct device *dev,
  * and then passed back when the device has been removed.
  */
 static ssize_t devicedisabled_store(struct device *dev,
-	struct device_attribute *attr, const char *buf, size_t count)
+				    struct device_attribute *attr,
+				    const char *buf, size_t count)
 {
 	uint id;
 
@@ -2338,7 +2131,8 @@ static ssize_t devicedisabled_store(struct device *dev,
  * and then passed back when the device has been brought back up.
  */
 static ssize_t deviceenabled_store(struct device *dev,
-	struct device_attribute *attr, const char *buf, size_t count)
+				   struct device_attribute *attr,
+				   const char *buf, size_t count)
 {
 	uint id;
 
@@ -2353,133 +2147,102 @@ static int __init
 visorchipset_init(void)
 {
 	int rc = 0, x = 0;
-	char s[64];
 	HOSTADDRESS addr;
 
 	if (!unisys_spar_platform)
 		return -ENODEV;
 
-	LOGINF("chipset driver version %s loaded", VERSION);
-	/* process module options */
-	POSTCODE_LINUX_2(DRIVER_ENTRY_PC, POSTCODE_SEVERITY_INFO);
-
-	LOGINF("option - testvnic=%d", visorchipset_testvnic);
-	LOGINF("option - testvnicclient=%d", visorchipset_testvnicclient);
-	LOGINF("option - testmsg=%d", visorchipset_testmsg);
-	LOGINF("option - testteardown=%d", visorchipset_testteardown);
-	LOGINF("option - major=%d", visorchipset_major);
-	LOGINF("option - serverregwait=%d", visorchipset_serverregwait);
-	LOGINF("option - clientregwait=%d", visorchipset_clientregwait);
-	LOGINF("option - holdchipsetready=%d", visorchipset_holdchipsetready);
-
-	memset(&BusDev_Server_Notifiers, 0, sizeof(BusDev_Server_Notifiers));
-	memset(&BusDev_Client_Notifiers, 0, sizeof(BusDev_Client_Notifiers));
-	memset(&ControlVm_payload_info, 0, sizeof(ControlVm_payload_info));
-	memset(&LiveDump_info, 0, sizeof(LiveDump_info));
-	atomic_set(&LiveDump_info.buffers_in_use, 0);
+	memset(&busdev_server_notifiers, 0, sizeof(busdev_server_notifiers));
+	memset(&busdev_client_notifiers, 0, sizeof(busdev_client_notifiers));
+	memset(&controlvm_payload_info, 0, sizeof(controlvm_payload_info));
+	memset(&livedump_info, 0, sizeof(livedump_info));
+	atomic_set(&livedump_info.buffers_in_use, 0);
 
 	if (visorchipset_testvnic) {
-		ERRDRV("testvnic option no longer supported: (status = %d)\n",
-		       x);
 		POSTCODE_LINUX_3(CHIPSET_INIT_FAILURE_PC, x, DIAG_SEVERITY_ERR);
 		rc = x;
-		goto Away;
+		goto cleanup;
 	}
 
 	addr = controlvm_get_channel_address();
 	if (addr != 0) {
-		ControlVm_channel =
+		controlvm_channel =
 		    visorchannel_create_with_lock
 		    (addr,
 		     sizeof(struct spar_controlvm_channel_protocol),
 		     spar_controlvm_channel_protocol_uuid);
 		if (SPAR_CONTROLVM_CHANNEL_OK_CLIENT(
-				visorchannel_get_header(ControlVm_channel))) {
-			LOGINF("Channel %s (ControlVm) discovered",
-			       visorchannel_id(ControlVm_channel, s));
+				visorchannel_get_header(controlvm_channel))) {
 			initialize_controlvm_payload();
 		} else {
-			LOGERR("controlvm channel is invalid");
-			visorchannel_destroy(ControlVm_channel);
-			ControlVm_channel = NULL;
+			visorchannel_destroy(controlvm_channel);
+			controlvm_channel = NULL;
 			return -ENODEV;
 		}
 	} else {
-		LOGERR("no controlvm channel discovered");
 		return -ENODEV;
 	}
 
-	MajorDev = MKDEV(visorchipset_major, 0);
-	rc = visorchipset_file_init(MajorDev, &ControlVm_channel);
+	major_dev = MKDEV(visorchipset_major, 0);
+	rc = visorchipset_file_init(major_dev, &controlvm_channel);
 	if (rc < 0) {
-		ERRDRV("visorchipset_file_init(MajorDev, &ControlVm_channel): error (status=%d)\n", rc);
 		POSTCODE_LINUX_2(CHIPSET_INIT_FAILURE_PC, DIAG_SEVERITY_ERR);
-		goto Away;
+		goto cleanup;
 	}
 
-	memset(&g_DiagMsgHdr, 0, sizeof(struct controlvm_message_header));
+	memset(&g_diag_msg_hdr, 0, sizeof(struct controlvm_message_header));
 
-	memset(&g_ChipSetMsgHdr, 0, sizeof(struct controlvm_message_header));
+	memset(&g_chipset_msg_hdr, 0, sizeof(struct controlvm_message_header));
 
-	memset(&g_DelDumpMsgHdr, 0, sizeof(struct controlvm_message_header));
+	memset(&g_del_dump_msg_hdr, 0, sizeof(struct controlvm_message_header));
 
-	Putfile_buffer_list_pool =
-	    kmem_cache_create(Putfile_buffer_list_pool_name,
+	putfile_buffer_list_pool =
+	    kmem_cache_create(putfile_buffer_list_pool_name,
 			      sizeof(struct putfile_buffer_entry),
 			      0, SLAB_HWCACHE_ALIGN, NULL);
-	if (!Putfile_buffer_list_pool) {
-		ERRDRV("failed to alloc Putfile_buffer_list_pool: (status=-1)\n");
+	if (!putfile_buffer_list_pool) {
 		POSTCODE_LINUX_2(CHIPSET_INIT_FAILURE_PC, DIAG_SEVERITY_ERR);
 		rc = -1;
-		goto Away;
+		goto cleanup;
 	}
-	if (visorchipset_disable_controlvm) {
-		LOGINF("visorchipset_init:controlvm disabled");
-	} else {
+	if (!visorchipset_disable_controlvm) {
 		/* if booting in a crash kernel */
 		if (visorchipset_crash_kernel)
-			INIT_DELAYED_WORK(&Periodic_controlvm_work,
+			INIT_DELAYED_WORK(&periodic_controlvm_work,
 					  setup_crash_devices_work_queue);
 		else
-			INIT_DELAYED_WORK(&Periodic_controlvm_work,
+			INIT_DELAYED_WORK(&periodic_controlvm_work,
 					  controlvm_periodic_work);
-		Periodic_controlvm_workqueue =
+		periodic_controlvm_workqueue =
 		    create_singlethread_workqueue("visorchipset_controlvm");
 
-		if (Periodic_controlvm_workqueue == NULL) {
-			ERRDRV("cannot create controlvm workqueue: (status=%d)\n",
-			       -ENOMEM);
+		if (!periodic_controlvm_workqueue) {
 			POSTCODE_LINUX_2(CREATE_WORKQUEUE_FAILED_PC,
 					 DIAG_SEVERITY_ERR);
 			rc = -ENOMEM;
-			goto Away;
+			goto cleanup;
 		}
-		Most_recent_message_jiffies = jiffies;
-		Poll_jiffies = POLLJIFFIES_CONTROLVMCHANNEL_FAST;
-		rc = queue_delayed_work(Periodic_controlvm_workqueue,
-					&Periodic_controlvm_work, Poll_jiffies);
+		most_recent_message_jiffies = jiffies;
+		poll_jiffies = POLLJIFFIES_CONTROLVMCHANNEL_FAST;
+		rc = queue_delayed_work(periodic_controlvm_workqueue,
+					&periodic_controlvm_work, poll_jiffies);
 		if (rc < 0) {
-			ERRDRV("queue_delayed_work(Periodic_controlvm_workqueue, &Periodic_controlvm_work, Poll_jiffies): error (status=%d)\n", rc);
 			POSTCODE_LINUX_2(QUEUE_DELAYED_WORK_PC,
 					 DIAG_SEVERITY_ERR);
-			goto Away;
+			goto cleanup;
 		}
-
 	}
 
-	Visorchipset_platform_device.dev.devt = MajorDev;
-	if (platform_device_register(&Visorchipset_platform_device) < 0) {
-		ERRDRV("platform_device_register(visorchipset) failed: (status=-1)\n");
+	visorchipset_platform_device.dev.devt = major_dev;
+	if (platform_device_register(&visorchipset_platform_device) < 0) {
 		POSTCODE_LINUX_2(DEVICE_REGISTER_FAILURE_PC, DIAG_SEVERITY_ERR);
 		rc = -1;
-		goto Away;
+		goto cleanup;
 	}
-	LOGINF("visorchipset device created");
 	POSTCODE_LINUX_2(CHIPSET_INIT_SUCCESS_PC, POSTCODE_SEVERITY_INFO);
 	rc = 0;
-Away:
+cleanup:
 	if (rc) {
-		LOGERR("visorchipset_init failed");
 		POSTCODE_LINUX_3(CHIPSET_INIT_FAILURE_PC, rc,
 				 POSTCODE_SEVERITY_ERR);
 	}
@@ -2489,40 +2252,34 @@ Away:
 static void
 visorchipset_exit(void)
 {
-	char s[99];
-
 	POSTCODE_LINUX_2(DRIVER_EXIT_PC, POSTCODE_SEVERITY_INFO);
 
 	if (visorchipset_disable_controlvm) {
 		;
 	} else {
-		cancel_delayed_work(&Periodic_controlvm_work);
-		flush_workqueue(Periodic_controlvm_workqueue);
-		destroy_workqueue(Periodic_controlvm_workqueue);
-		Periodic_controlvm_workqueue = NULL;
-		destroy_controlvm_payload_info(&ControlVm_payload_info);
+		cancel_delayed_work(&periodic_controlvm_work);
+		flush_workqueue(periodic_controlvm_workqueue);
+		destroy_workqueue(periodic_controlvm_workqueue);
+		periodic_controlvm_workqueue = NULL;
+		destroy_controlvm_payload_info(&controlvm_payload_info);
 	}
-	Test_Vnic_channel = NULL;
-	if (Putfile_buffer_list_pool) {
-		kmem_cache_destroy(Putfile_buffer_list_pool);
-		Putfile_buffer_list_pool = NULL;
+	if (putfile_buffer_list_pool) {
+		kmem_cache_destroy(putfile_buffer_list_pool);
+		putfile_buffer_list_pool = NULL;
 	}
 
 	cleanup_controlvm_structures();
 
-	memset(&g_DiagMsgHdr, 0, sizeof(struct controlvm_message_header));
+	memset(&g_diag_msg_hdr, 0, sizeof(struct controlvm_message_header));
 
-	memset(&g_ChipSetMsgHdr, 0, sizeof(struct controlvm_message_header));
+	memset(&g_chipset_msg_hdr, 0, sizeof(struct controlvm_message_header));
 
-	memset(&g_DelDumpMsgHdr, 0, sizeof(struct controlvm_message_header));
+	memset(&g_del_dump_msg_hdr, 0, sizeof(struct controlvm_message_header));
 
-	LOGINF("Channel %s (ControlVm) disconnected",
-	       visorchannel_id(ControlVm_channel, s));
-	visorchannel_destroy(ControlVm_channel);
+	visorchannel_destroy(controlvm_channel);
 
-	visorchipset_file_cleanup();
+	visorchipset_file_cleanup(visorchipset_platform_device.dev.devt);
 	POSTCODE_LINUX_2(DRIVER_EXIT_PC, POSTCODE_SEVERITY_INFO);
-	LOGINF("chipset driver unloaded");
 }
 
 module_param_named(testvnic, visorchipset_testvnic, int, S_IRUGO);
diff --git a/drivers/staging/unisys/visorutil/Kconfig b/drivers/staging/unisys/visorutil/Kconfig
index 74b474eac252..be9c2cf890cc 100644
--- a/drivers/staging/unisys/visorutil/Kconfig
+++ b/drivers/staging/unisys/visorutil/Kconfig
@@ -4,7 +4,6 @@
 
 config UNISYS_VISORUTIL
 	tristate "Unisys visorutil driver"
-	depends on UNISYSSPAR && HAS_IOMEM
 	---help---
 	If you say Y here, you will enable the Unisys visorutil driver.
 
diff --git a/drivers/staging/unisys/visorutil/Makefile b/drivers/staging/unisys/visorutil/Makefile
index d871bbb78cef..d9ab5a36e3bf 100644
--- a/drivers/staging/unisys/visorutil/Makefile
+++ b/drivers/staging/unisys/visorutil/Makefile
@@ -4,7 +4,6 @@
 
 obj-$(CONFIG_UNISYS_VISORUTIL)	+= visorutil.o
 
-visorutil-y := charqueue.o  easyproc.o  periodic_work.o  procobjecttree.o  \
-		memregion_direct.o visorkmodutils.o
+visorutil-y := charqueue.o  periodic_work.o  memregion_direct.o visorkmodutils.o
 
 ccflags-y += -Idrivers/staging/unisys/include
diff --git a/drivers/staging/unisys/visorutil/charqueue.c b/drivers/staging/unisys/visorutil/charqueue.c
index ac7acb7c5b79..c91752a2d06b 100644
--- a/drivers/staging/unisys/visorutil/charqueue.c
+++ b/drivers/staging/unisys/visorutil/charqueue.c
@@ -36,13 +36,11 @@ struct charqueue {
 struct charqueue *visor_charqueue_create(ulong nslots)
 {
 	int alloc_size = sizeof(struct charqueue) + nslots + 1;
-	struct charqueue *cq = kmalloc(alloc_size, GFP_KERNEL|__GFP_NORETRY);
+	struct charqueue *cq;
 
-	if (cq == NULL) {
-		ERRDRV("visor_charqueue_create allocation failed (alloc_size=%d)",
-		       alloc_size);
+	cq = kmalloc(alloc_size, GFP_KERNEL|__GFP_NORETRY);
+	if (cq == NULL)
 		return NULL;
-	}
 	cq->alloc_size = alloc_size;
 	cq->nslots = nslots;
 	cq->head = 0;
diff --git a/drivers/staging/unisys/visorutil/charqueue.h b/drivers/staging/unisys/visorutil/charqueue.h
index 56c1f79a54b0..f46a776b935b 100644
--- a/drivers/staging/unisys/visorutil/charqueue.h
+++ b/drivers/staging/unisys/visorutil/charqueue.h
@@ -18,7 +18,6 @@
 #ifndef __CHARQUEUE_H__
 #define __CHARQUEUE_H__
 
-#include "uniklog.h"
 #include "timskmod.h"
 
 /* struct charqueue is an opaque structure to users.
diff --git a/drivers/staging/unisys/visorutil/easyproc.c b/drivers/staging/unisys/visorutil/easyproc.c
deleted file mode 100644
index 40f1ae9a155c..000000000000
--- a/drivers/staging/unisys/visorutil/easyproc.c
+++ /dev/null
@@ -1,377 +0,0 @@
-/* Copyright (C) 2010 - 2013 UNISYS CORPORATION
- * All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or (at
- * your option) any later version.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE, GOOD TITLE or
- * NON INFRINGEMENT.  See the GNU General Public License for more
- * details.
- */
-
-/** @file *********************************************************************
- *
- *  Handle procfs-specific tasks.
- *  Note that this file does not know about any module-specific things, nor
- *  does it know anything about what information to reveal as part of the proc
- *  entries.  The 2 functions that take care of displaying device and
- *  driver specific information are passed as parameters to
- *  visor_easyproc_InitDriver().
- *
- *      void show_device_info(struct seq_file *seq, void *p);
- *      void show_driver_info(struct seq_file *seq);
- *
- *  The second parameter to show_device_info is actually a pointer to the
- *  device-specific info to show.  It is the context that was originally
- *  passed to visor_easyproc_InitDevice().
- *
- ******************************************************************************
- */
-
-#include <linux/proc_fs.h>
-
-#include "uniklog.h"
-#include "timskmod.h"
-#include "easyproc.h"
-
-#define MYDRVNAME "easyproc"
-
-
-
-/*
- *   /proc/<ProcId>                              ProcDir
- *   /proc/<ProcId>/driver                       ProcDriverDir
- *   /proc/<ProcId>/driver/diag                  ProcDriverDiagFile
- *   /proc/<ProcId>/device                       ProcDeviceDir
- *   /proc/<ProcId>/device/0                     procDevicexDir
- *   /proc/<ProcId>/device/0/diag                procDevicexDiagFile
- */
-
-
-static ssize_t proc_write_device(struct file *file, const char __user *buffer,
-				 size_t count, loff_t *ppos);
-static ssize_t proc_write_driver(struct file *file, const char __user *buffer,
-				 size_t count, loff_t *ppos);
-
-static struct proc_dir_entry *
-	createProcDir(char *name, struct proc_dir_entry *parent)
-{
-	struct proc_dir_entry *p = proc_mkdir_mode(name, S_IFDIR, parent);
-
-	if (p == NULL)
-		ERRDRV("failed to create /proc directory %s", name);
-	return p;
-}
-
-static int seq_show_driver(struct seq_file *seq, void *offset);
-static int proc_open_driver(struct inode *inode, struct file *file)
-{
-	return single_open(file, seq_show_driver, PDE_DATA(inode));
-}
-static const struct file_operations proc_fops_driver = {
-	.open = proc_open_driver,
-	.read = seq_read,
-	.write = proc_write_driver,
-	.llseek = seq_lseek,
-	.release = single_release,
-};
-
-static int seq_show_device(struct seq_file *seq, void *offset);
-static int seq_show_device_property(struct seq_file *seq, void *offset);
-static int proc_open_device(struct inode *inode, struct file *file)
-{
-	return single_open(file, seq_show_device, PDE_DATA(inode));
-}
-static const struct file_operations proc_fops_device = {
-	.open = proc_open_device,
-	.read = seq_read,
-	.write = proc_write_device,
-	.llseek = seq_lseek,
-	.release = single_release,
-};
-static int proc_open_device_property(struct inode *inode, struct file *file)
-{
-	return single_open(file, seq_show_device_property, PDE_DATA(inode));
-}
-static const struct file_operations proc_fops_device_property = {
-	.open = proc_open_device_property,
-	.read = seq_read,
-	.llseek = seq_lseek,
-	.release = single_release,
-};
-
-
-
-void visor_easyproc_InitDriver(struct easyproc_driver_info *pdriver,
-			       char *procId,
-			       void (*show_driver_info)(struct seq_file *),
-			       void (*show_device_info)(struct seq_file *,
-							void *))
-{
-	memset(pdriver, 0, sizeof(struct easyproc_driver_info));
-	pdriver->ProcId = procId;
-	if (pdriver->ProcId == NULL)
-		ERRDRV("ProcId cannot be NULL (trouble ahead)!");
-	pdriver->Show_driver_info = show_driver_info;
-	pdriver->Show_device_info = show_device_info;
-	if (pdriver->ProcDir == NULL)
-		pdriver->ProcDir = createProcDir(pdriver->ProcId, NULL);
-	if ((pdriver->ProcDir != NULL) && (pdriver->ProcDriverDir == NULL))
-		pdriver->ProcDriverDir = createProcDir("driver",
-						       pdriver->ProcDir);
-	if ((pdriver->ProcDir != NULL) && (pdriver->ProcDeviceDir == NULL))
-		pdriver->ProcDeviceDir = createProcDir("device",
-						       pdriver->ProcDir);
-	if ((pdriver->ProcDriverDir != NULL) &&
-	    (pdriver->ProcDriverDiagFile == NULL)) {
-		pdriver->ProcDriverDiagFile =
-			proc_create_data("diag", 0,
-					 pdriver->ProcDriverDir,
-					 &proc_fops_driver, pdriver);
-		if (pdriver->ProcDriverDiagFile == NULL)
-			ERRDRV("failed to register /proc/%s/driver/diag entry",
-			       pdriver->ProcId);
-	}
-}
-EXPORT_SYMBOL_GPL(visor_easyproc_InitDriver);
-
-
-
-void visor_easyproc_InitDriverEx(struct easyproc_driver_info *pdriver,
-				 char *procId,
-				 void (*show_driver_info)(struct seq_file *),
-				 void (*show_device_info)(struct seq_file *,
-							  void *),
-				 void (*write_driver_info)(char *buf,
-							   size_t count,
-							   loff_t *ppos),
-				 void (*write_device_info)(char *buf,
-							   size_t count,
-							   loff_t *ppos,
-							   void *p))
-{
-	visor_easyproc_InitDriver(pdriver, procId,
-				  show_driver_info, show_device_info);
-	pdriver->Write_driver_info = write_driver_info;
-	pdriver->Write_device_info = write_device_info;
-}
-EXPORT_SYMBOL_GPL(visor_easyproc_InitDriverEx);
-
-
-
-void visor_easyproc_DeInitDriver(struct easyproc_driver_info *pdriver)
-{
-	if (pdriver->ProcDriverDiagFile != NULL) {
-		remove_proc_entry("diag", pdriver->ProcDriverDir);
-		pdriver->ProcDriverDiagFile = NULL;
-	}
-	if (pdriver->ProcDriverDir != NULL) {
-		remove_proc_entry("driver", pdriver->ProcDir);
-		pdriver->ProcDriverDir = NULL;
-	}
-	if (pdriver->ProcDeviceDir != NULL) {
-		remove_proc_entry("device", pdriver->ProcDir);
-		pdriver->ProcDeviceDir = NULL;
-	}
-	if (pdriver->ProcDir != NULL) {
-		remove_proc_entry(pdriver->ProcId, NULL);
-		pdriver->ProcDir = NULL;
-	}
-	pdriver->ProcId = NULL;
-	pdriver->Show_driver_info = NULL;
-	pdriver->Show_device_info = NULL;
-	pdriver->Write_driver_info = NULL;
-	pdriver->Write_device_info = NULL;
-}
-EXPORT_SYMBOL_GPL(visor_easyproc_DeInitDriver);
-
-
-
-void visor_easyproc_InitDevice(struct easyproc_driver_info *pdriver,
-			       struct easyproc_device_info *p, int devno,
-			       void *devdata)
-{
-	if ((pdriver->ProcDeviceDir != NULL) && (p->procDevicexDir == NULL)) {
-		char s[29];
-
-		sprintf(s, "%d", devno);
-		p->procDevicexDir = createProcDir(s, pdriver->ProcDeviceDir);
-		p->devno = devno;
-	}
-	p->devdata = devdata;
-	p->pdriver = pdriver;
-	p->devno = devno;
-	if ((p->procDevicexDir != NULL) && (p->procDevicexDiagFile == NULL)) {
-		p->procDevicexDiagFile =
-			proc_create_data("diag", 0, p->procDevicexDir,
-					 &proc_fops_device, p);
-		if (p->procDevicexDiagFile == NULL)
-			ERRDEVX(devno, "failed to register /proc/%s/device/%d/diag entry",
-				pdriver->ProcId, devno
-			       );
-	}
-	memset(&(p->device_property_info[0]), 0,
-	       sizeof(p->device_property_info));
-}
-EXPORT_SYMBOL_GPL(visor_easyproc_InitDevice);
-
-
-
-void visor_easyproc_CreateDeviceProperty(struct easyproc_device_info *p,
-					 void (*show_property_info)
-					 (struct seq_file *, void *),
-					 char *property_name)
-{
-	size_t i;
-	struct easyproc_device_property_info *px = NULL;
-
-	if (p->procDevicexDir == NULL) {
-		ERRDRV("state error");
-		return;
-	}
-	for (i = 0; i < ARRAY_SIZE(p->device_property_info); i++) {
-		if (p->device_property_info[i].procEntry == NULL) {
-			px = &(p->device_property_info[i]);
-			break;
-		}
-	}
-	if (!px) {
-		ERRDEVX(p->devno, "too many device properties");
-		return;
-	}
-	px->devdata = p->devdata;
-	px->pdriver = p->pdriver;
-	px->procEntry = proc_create_data(property_name, 0, p->procDevicexDir,
-					 &proc_fops_device_property, px);
-	if (strlen(property_name)+1 > sizeof(px->property_name)) {
-		ERRDEVX(p->devno, "device property name %s too long",
-			property_name);
-		return;
-	}
-	strcpy(px->property_name, property_name);
-	if (px->procEntry == NULL) {
-		ERRDEVX(p->devno,
-			"failed to register /proc/%s/device/%d/%s entry",
-			p->pdriver->ProcId, p->devno, property_name);
-		return;
-	}
-	px->show_device_property_info = show_property_info;
-}
-EXPORT_SYMBOL_GPL(visor_easyproc_CreateDeviceProperty);
-
-
-
-void visor_easyproc_DeInitDevice(struct easyproc_driver_info *pdriver,
-				 struct easyproc_device_info *p, int devno)
-{
-	size_t i;
-
-	for (i = 0; i < ARRAY_SIZE(p->device_property_info); i++) {
-		if (p->device_property_info[i].procEntry != NULL) {
-			struct easyproc_device_property_info *px =
-				&(p->device_property_info[i]);
-			remove_proc_entry(px->property_name, p->procDevicexDir);
-			px->procEntry = NULL;
-		}
-	}
-	if (p->procDevicexDiagFile != NULL) {
-		remove_proc_entry("diag", p->procDevicexDir);
-		p->procDevicexDiagFile = NULL;
-	}
-	if (p->procDevicexDir != NULL) {
-		char s[29];
-
-		sprintf(s, "%d", devno);
-		remove_proc_entry(s, pdriver->ProcDeviceDir);
-		p->procDevicexDir = NULL;
-	}
-	p->devdata = NULL;
-	p->pdriver = NULL;
-}
-EXPORT_SYMBOL_GPL(visor_easyproc_DeInitDevice);
-
-
-
-static int seq_show_driver(struct seq_file *seq, void *offset)
-{
-	struct easyproc_driver_info *p =
-		(struct easyproc_driver_info *)(seq->private);
-	if (!p)
-		return 0;
-	(*(p->Show_driver_info))(seq);
-	return 0;
-}
-
-
-
-static int seq_show_device(struct seq_file *seq, void *offset)
-{
-	struct easyproc_device_info *p =
-		(struct easyproc_device_info *)(seq->private);
-	if ((!p) || (!(p->pdriver)))
-		return 0;
-	(*(p->pdriver->Show_device_info))(seq, p->devdata);
-	return 0;
-}
-
-
-
-static int seq_show_device_property(struct seq_file *seq, void *offset)
-{
-	struct easyproc_device_property_info *p =
-		(struct easyproc_device_property_info *)(seq->private);
-	if ((!p) || (!(p->show_device_property_info)))
-		return 0;
-	(*(p->show_device_property_info))(seq, p->devdata);
-	return 0;
-}
-
-
-
-static ssize_t proc_write_driver(struct file *file, const char __user *buffer,
-				 size_t count, loff_t *ppos)
-{
-	struct seq_file *seq = (struct seq_file *)file->private_data;
-	struct easyproc_driver_info *p = NULL;
-	char local_buf[256];
-
-	if (seq == NULL)
-		return 0;
-	p = (struct easyproc_driver_info *)(seq->private);
-	if ((!p) || (!(p->Write_driver_info)))
-		return 0;
-	if (count >= sizeof(local_buf))
-		return -ENOMEM;
-	if (copy_from_user(local_buf, buffer, count))
-		return -EFAULT;
-	local_buf[count] = '\0';  /* be friendly */
-	(*(p->Write_driver_info))(local_buf, count, ppos);
-	return count;
-}
-
-
-
-static ssize_t proc_write_device(struct file *file, const char __user *buffer,
-				 size_t count, loff_t *ppos)
-{
-	struct seq_file *seq = (struct seq_file *)file->private_data;
-	struct easyproc_device_info *p = NULL;
-	char local_buf[256];
-
-	if (seq == NULL)
-		return 0;
-	p = (struct easyproc_device_info *)(seq->private);
-	if ((!p) || (!(p->pdriver)) || (!(p->pdriver->Write_device_info)))
-		return 0;
-	if (count >= sizeof(local_buf))
-		return -ENOMEM;
-	if (copy_from_user(local_buf, buffer, count))
-		return -EFAULT;
-	local_buf[count] = '\0';  /* be friendly */
-	(*(p->pdriver->Write_device_info))(local_buf, count, ppos, p->devdata);
-	return count;
-}
diff --git a/drivers/staging/unisys/visorutil/easyproc.h b/drivers/staging/unisys/visorutil/easyproc.h
deleted file mode 100644
index 6ce7d5eb6ad1..000000000000
--- a/drivers/staging/unisys/visorutil/easyproc.h
+++ /dev/null
@@ -1,92 +0,0 @@
-/* easyproc.h
- *
- * Copyright (C) 2010 - 2013 UNISYS CORPORATION
- * All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or (at
- * your option) any later version.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE, GOOD TITLE or
- * NON INFRINGEMENT.  See the GNU General Public License for more
- * details.
- */
-
-/** @file *********************************************************************
- *
- *  This describes the interfaces necessary for a simple /proc file
- *  implementation for a driver.
- *
- ******************************************************************************
- */
-
-#ifndef __EASYPROC_H__
-#define __EASYPROC_H__
-
-#include "timskmod.h"
-
-
-struct easyproc_driver_info {
-	struct proc_dir_entry *ProcDir;
-	struct proc_dir_entry *ProcDriverDir;
-	struct proc_dir_entry *ProcDriverDiagFile;
-	struct proc_dir_entry *ProcDeviceDir;
-	char *ProcId;
-	void (*Show_device_info)(struct seq_file *seq, void *p);
-	void (*Show_driver_info)(struct seq_file *seq);
-	void (*Write_device_info)(char *buf, size_t count,
-				  loff_t *ppos, void *p);
-	void (*Write_driver_info)(char *buf, size_t count, loff_t *ppos);
-};
-
-/* property is a file under /proc/<x>/device/<x>/<property_name> */
-struct easyproc_device_property_info {
-	char property_name[25];
-	struct proc_dir_entry *procEntry;
-	struct easyproc_driver_info *pdriver;
-	void *devdata;
-	void (*show_device_property_info)(struct seq_file *seq, void *p);
-};
-
-struct easyproc_device_info {
-	struct proc_dir_entry *procDevicexDir;
-	struct proc_dir_entry *procDevicexDiagFile;
-	struct easyproc_driver_info *pdriver;
-	void *devdata;
-	int devno;
-	/*  allow for a number of custom properties for each device: */
-	struct easyproc_device_property_info device_property_info[10];
-};
-
-void visor_easyproc_InitDevice(struct easyproc_driver_info *pdriver,
-			       struct easyproc_device_info *p, int devno,
-			       void *devdata);
-void visor_easyproc_DeInitDevice(struct easyproc_driver_info *pdriver,
-				 struct easyproc_device_info *p, int devno);
-void visor_easyproc_InitDriver(struct easyproc_driver_info *pdriver,
-			       char *procId,
-			       void (*show_driver_info)(struct seq_file *),
-			       void (*show_device_info)(struct seq_file *,
-							void *));
-void visor_easyproc_InitDriverEx(struct easyproc_driver_info *pdriver,
-				 char *procId,
-				 void (*show_driver_info)(struct seq_file *),
-				 void (*show_device_info)(struct seq_file *,
-							  void *),
-				 void (*Write_driver_info)(char *buf,
-							   size_t count,
-							   loff_t *ppos),
-				 void (*Write_device_info)(char *buf,
-							   size_t count,
-							   loff_t *ppos,
-							   void *p));
-void visor_easyproc_DeInitDriver(struct easyproc_driver_info *pdriver);
-void visor_easyproc_CreateDeviceProperty(struct easyproc_device_info *p,
-					 void (*show_property_info)
-					 (struct seq_file *, void *),
-					 char *property_name);
-
-#endif
diff --git a/drivers/staging/unisys/visorutil/memregion_direct.c b/drivers/staging/unisys/visorutil/memregion_direct.c
index 33522cc8c22c..eb7422fbe20f 100644
--- a/drivers/staging/unisys/visorutil/memregion_direct.c
+++ b/drivers/staging/unisys/visorutil/memregion_direct.c
@@ -20,7 +20,6 @@
  *  channel memory (in main memory of the host system) from code running in
  *  a virtual partition.
  */
-#include "uniklog.h"
 #include "timskmod.h"
 #include "memregion.h"
 
@@ -41,12 +40,12 @@ struct memregion *
 visor_memregion_create(HOSTADDRESS physaddr, ulong nbytes)
 {
 	struct memregion *rc = NULL;
-	struct memregion *memregion = kzalloc(sizeof(*memregion),
-					      GFP_KERNEL | __GFP_NORETRY);
-	if (memregion == NULL) {
-		ERRDRV("visor_memregion_create allocation failed");
+	struct memregion *memregion;
+
+	memregion = kzalloc(sizeof(*memregion), GFP_KERNEL | __GFP_NORETRY);
+	if (memregion == NULL)
 		return NULL;
-	}
+
 	memregion->physaddr = physaddr;
 	memregion->nbytes = nbytes;
 	memregion->overlapped = FALSE;
@@ -70,25 +69,19 @@ visor_memregion_create_overlapped(struct memregion *parent, ulong offset,
 {
 	struct memregion *memregion = NULL;
 
-	if (parent == NULL) {
-		ERRDRV("%s parent is NULL", __func__);
+	if (parent == NULL)
 		return NULL;
-	}
-	if (parent->mapped == NULL) {
-		ERRDRV("%s parent is not mapped!", __func__);
+
+	if (parent->mapped == NULL)
 		return NULL;
-	}
+
 	if ((offset >= parent->nbytes) ||
-	    ((offset + nbytes) >= parent->nbytes)) {
-		ERRDRV("%s range (%lu,%lu) out of parent range",
-		       __func__, offset, nbytes);
+	    ((offset + nbytes) >= parent->nbytes))
 		return NULL;
-	}
+
 	memregion = kzalloc(sizeof(*memregion), GFP_KERNEL|__GFP_NORETRY);
-	if (memregion == NULL) {
-		ERRDRV("%s allocation failed", __func__);
+	if (memregion == NULL)
 		return NULL;
-	}
 
 	memregion->physaddr = parent->physaddr + offset;
 	memregion->nbytes = nbytes;
@@ -106,17 +99,11 @@ mapit(struct memregion *memregion)
 	ulong nbytes = memregion->nbytes;
 
 	memregion->requested = FALSE;
-	if (!request_mem_region(physaddr, nbytes, MYDRVNAME))
-		ERRDRV("cannot reserve channel memory @0x%lx for 0x%lx-- no big deal",
-		       physaddr, nbytes);
-	else
+	if (request_mem_region(physaddr, nbytes, MYDRVNAME))
 		memregion->requested = TRUE;
 	memregion->mapped = ioremap_cache(physaddr, nbytes);
-	if (memregion->mapped == NULL) {
-		ERRDRV("cannot ioremap_cache channel memory @0x%lx for 0x%lx",
-		       physaddr, nbytes);
+	if (!memregion->mapped)
 		return FALSE;
-	}
 	return TRUE;
 }
 
@@ -180,10 +167,9 @@ memregion_readwrite(BOOL is_write,
 		    struct memregion *memregion, ulong offset,
 		    void *local, ulong nbytes)
 {
-	if (offset + nbytes > memregion->nbytes) {
-		ERRDRV("memregion_readwrite offset out of range!!");
+	if (offset + nbytes > memregion->nbytes)
 		return -EIO;
-	}
+
 	if (is_write)
 		memcpy_toio(memregion->mapped + offset, local, nbytes);
 	else
diff --git a/drivers/staging/unisys/visorutil/periodic_work.c b/drivers/staging/unisys/visorutil/periodic_work.c
index 0908bf929401..abbfb48894f3 100644
--- a/drivers/staging/unisys/visorutil/periodic_work.c
+++ b/drivers/staging/unisys/visorutil/periodic_work.c
@@ -19,7 +19,6 @@
  *  Helper functions to schedule periodic work in Linux kernel mode.
  */
 
-#include "uniklog.h"
 #include "timskmod.h"
 #include "periodic_work.h"
 
@@ -90,7 +89,6 @@ BOOL visor_periodic_work_nextperiod(struct periodic_work *pw)
 		goto unlock;
 	} else if (queue_delayed_work(pw->workqueue, &pw->work,
 				      pw->jiffy_interval) < 0) {
-		ERRDEV(pw->devnam, "queue_delayed_work failed!");
 		pw->is_scheduled = FALSE;
 		rc = FALSE;
 		goto unlock;
@@ -116,15 +114,12 @@ BOOL visor_periodic_work_start(struct periodic_work *pw)
 		goto unlock;
 	}
 	if (pw->want_to_stop) {
-		ERRDEV(pw->devnam,
-		       "dev_start_periodic_work failed!");
 		rc = FALSE;
 		goto unlock;
 	}
 	INIT_DELAYED_WORK(&pw->work, &periodic_work_func);
 	if (queue_delayed_work(pw->workqueue, &pw->work,
 			       pw->jiffy_interval) < 0) {
-		ERRDEV(pw->devnam, "%s queue_delayed_work failed!", __func__);
 		rc = FALSE;
 		goto unlock;
 	}
@@ -182,7 +177,7 @@ BOOL visor_periodic_work_stop(struct periodic_work *pw)
 			/* We get here if the delayed work was pending as
 			 * delayed work, but was NOT run.
 			 */
-			ASSERT(pw->is_scheduled);
+			WARN_ON(!pw->is_scheduled);
 			pw->is_scheduled = FALSE;
 		} else {
 			/* If we get here, either the delayed work:
@@ -197,14 +192,6 @@ BOOL visor_periodic_work_stop(struct periodic_work *pw)
 		}
 		if (pw->is_scheduled) {
 			write_unlock(&pw->lock);
-			WARNDEV(pw->devnam,
-				"waiting for delayed work...");
-			/* We rely on the delayed work function running here,
-			 * and eventually calling
-			 * visor_periodic_work_nextperiod(),
-			 * which will see that want_to_stop is set, and
-			 * subsequently clear is_scheduled.
-			 */
 			SLEEPJIFFIES(10);
 			write_lock(&pw->lock);
 		} else {
diff --git a/drivers/staging/unisys/visorutil/procobjecttree.c b/drivers/staging/unisys/visorutil/procobjecttree.c
deleted file mode 100644
index 82279ca5fbe1..000000000000
--- a/drivers/staging/unisys/visorutil/procobjecttree.c
+++ /dev/null
@@ -1,353 +0,0 @@
-/* procobjecttree.c
- *
- * Copyright (C) 2010 - 2013 UNISYS CORPORATION
- * All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or (at
- * your option) any later version.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE, GOOD TITLE or
- * NON INFRINGEMENT.  See the GNU General Public License for more
- * details.
- */
-
-#include "procobjecttree.h"
-
-#define MYDRVNAME "procobjecttree"
-
-
-
-/** This is context info that we stash in each /proc file entry, which we
- *  need in order to call the callback function that supplies the /proc read
- *  info for that file.
- */
-struct proc_dir_entry_context {
-	void (*show_property)(struct seq_file *, void *, int);
-	MYPROCOBJECT *procObject;
-	int propertyIndex;
-
-};
-
-/** This describes the attributes of a tree rooted at
- *  <procDirRoot>/<name[0]>/<name[1]>/...
- *  Properties for each object of this type will be located under
- *  <procDirRoot>/<name[0]>/<name[1]>/.../<objectName>/<propertyName>.
- */
-struct MYPROCTYPE_Tag {
-	const char **name;  /**< node names for this type, ending with NULL */
-	int nNames;         /**< num of node names in <name> */
-
-	/** root dir for this type tree in /proc */
-	struct proc_dir_entry *procDirRoot;
-
-	struct proc_dir_entry **procDirs;  /**< for each node in <name> */
-
-	/** bottom dir where objects will be rooted; i.e., this is
-	 *  <procDirRoot>/<name[0]>/<name[1]>/.../, which is the same as the
-	 *  last entry in the <procDirs> array. */
-	struct proc_dir_entry *procDir;
-
-	/** name for each property that objects of this type can have */
-	const char **propertyNames;
-
-	int nProperties;       /**< num of names in <propertyNames> */
-
-	/** Call this, passing MYPROCOBJECT.context and the property index
-	 *  whenever someone reads the proc entry */
-	void (*show_property)(struct seq_file *, void *, int);
-};
-
-
-
-struct MYPROCOBJECT_Tag {
-	MYPROCTYPE *type;
-
-	/** This is the name of the dir node in /proc under which the
-	 *  properties of this object will appear as files. */
-	char *name;
-
-	int namesize;   /**< number of bytes allocated for name */
-	void *context;  /**< passed to MYPROCTYPE.show_property */
-
-	/** <type.procDirRoot>/<type.name[0]>/<type.name[1]>/.../<name> */
-	struct proc_dir_entry *procDir;
-
-	/** a proc dir entry for each of the properties of the object;
-	 *  properties are identified in MYPROCTYPE.propertyNames, so each of
-	 *  the <procDirProperties> describes a single file like
-	 *  <type.procDirRoot>/<type.name[0]>/<type.name[1]>/...
-	 *           /<name>/<propertyName>
-	 */
-	struct proc_dir_entry **procDirProperties;
-
-	/** this is a holding area for the context information that is needed
-	 *  to run the /proc callback function */
-	struct proc_dir_entry_context *procDirPropertyContexts;
-};
-
-
-
-static struct proc_dir_entry *
-createProcDir(const char *name, struct proc_dir_entry *parent)
-{
-	struct proc_dir_entry *p = proc_mkdir_mode(name, S_IFDIR, parent);
-
-	if (p == NULL)
-		ERRDRV("failed to create /proc directory %s", name);
-	return p;
-}
-
-static struct proc_dir_entry *
-createProcFile(const char *name, struct proc_dir_entry *parent,
-	       const struct file_operations *fops, void *data)
-{
-	struct proc_dir_entry *p = proc_create_data(name, 0, parent,
-						    fops, data);
-	if (p == NULL)
-		ERRDRV("failed to create /proc file %s", name);
-	return p;
-}
-
-static int seq_show(struct seq_file *seq, void *offset);
-static int proc_open(struct inode *inode, struct file *file)
-{
-	return single_open(file, seq_show, PDE_DATA(inode));
-}
-
-static const struct file_operations proc_fops = {
-	.open = proc_open,
-	.read = seq_read,
-	.llseek = seq_lseek,
-	.release = single_release,
-};
-
-
-
-MYPROCTYPE *visor_proc_CreateType(struct proc_dir_entry *procDirRoot,
-				  const char **name,
-				  const char **propertyNames,
-				  void (*show_property)(struct seq_file *,
-							void *, int))
-{
-	int i = 0;
-	MYPROCTYPE *rc = NULL, *type = NULL;
-	struct proc_dir_entry *parent = NULL;
-
-	if (procDirRoot == NULL) {
-		ERRDRV("procDirRoot cannot be NULL!\n");
-		goto Away;
-	}
-	if (name == NULL || name[0] == NULL) {
-		ERRDRV("name must contain at least 1 node name!\n");
-		goto Away;
-	}
-	type = kzalloc(sizeof(MYPROCTYPE), GFP_KERNEL | __GFP_NORETRY);
-	if (type == NULL) {
-		ERRDRV("out of memory\n");
-		goto Away;
-	}
-	type->name = name;
-	type->propertyNames = propertyNames;
-	type->nProperties = 0;
-	type->nNames = 0;
-	type->show_property = show_property;
-	type->procDirRoot = procDirRoot;
-	if (type->propertyNames != NULL)
-		while (type->propertyNames[type->nProperties] != NULL)
-			type->nProperties++;
-	while (type->name[type->nNames] != NULL)
-		type->nNames++;
-	type->procDirs = kzalloc((type->nNames + 1) *
-				 sizeof(struct proc_dir_entry *),
-				 GFP_KERNEL | __GFP_NORETRY);
-	if (type->procDirs == NULL) {
-		ERRDRV("out of memory\n");
-		goto Away;
-	}
-	parent = procDirRoot;
-	for (i = 0; i < type->nNames; i++) {
-		type->procDirs[i] = createProcDir(type->name[i], parent);
-		if (type->procDirs[i] == NULL) {
-			rc = NULL;
-			goto Away;
-		}
-		parent = type->procDirs[i];
-	}
-	type->procDir = type->procDirs[type->nNames-1];
-	rc = type;
-Away:
-	if (rc == NULL) {
-		if (type != NULL) {
-			visor_proc_DestroyType(type);
-			type = NULL;
-		}
-	}
-	return rc;
-}
-EXPORT_SYMBOL_GPL(visor_proc_CreateType);
-
-
-
-void visor_proc_DestroyType(MYPROCTYPE *type)
-{
-	if (type == NULL)
-		return;
-	if (type->procDirs != NULL) {
-		int i = type->nNames-1;
-
-		while (i >= 0) {
-			if (type->procDirs[i] != NULL) {
-				struct proc_dir_entry *parent = NULL;
-
-				if (i == 0)
-					parent = type->procDirRoot;
-				else
-					parent = type->procDirs[i-1];
-				remove_proc_entry(type->name[i], parent);
-			}
-			i--;
-		}
-		kfree(type->procDirs);
-		type->procDirs = NULL;
-	}
-	kfree(type);
-}
-EXPORT_SYMBOL_GPL(visor_proc_DestroyType);
-
-
-
-MYPROCOBJECT *visor_proc_CreateObject(MYPROCTYPE *type,
-				      const char *name, void *context)
-{
-	MYPROCOBJECT *obj = NULL, *rc = NULL;
-	int i = 0;
-
-	if (type == NULL) {
-		ERRDRV("type cannot be NULL\n");
-		goto Away;
-	}
-	obj = kzalloc(sizeof(MYPROCOBJECT), GFP_KERNEL | __GFP_NORETRY);
-	if (obj == NULL) {
-		ERRDRV("out of memory\n");
-		goto Away;
-	}
-	obj->type = type;
-	obj->context = context;
-	if (name == NULL) {
-		obj->name = NULL;
-		obj->procDir = type->procDir;
-	} else {
-		obj->namesize = strlen(name)+1;
-		obj->name = kmalloc(obj->namesize, GFP_KERNEL | __GFP_NORETRY);
-		if (obj->name == NULL) {
-			obj->namesize = 0;
-			ERRDRV("out of memory\n");
-			goto Away;
-		}
-		strcpy(obj->name, name);
-		obj->procDir = createProcDir(obj->name, type->procDir);
-		if (obj->procDir == NULL)
-			goto Away;
-	}
-	obj->procDirPropertyContexts =
-		kzalloc((type->nProperties + 1) *
-			sizeof(struct proc_dir_entry_context),
-			GFP_KERNEL | __GFP_NORETRY);
-	if (obj->procDirPropertyContexts == NULL) {
-		ERRDRV("out of memory\n");
-		goto Away;
-	}
-	obj->procDirProperties = kzalloc((type->nProperties + 1) *
-					 sizeof(struct proc_dir_entry *),
-					 GFP_KERNEL | __GFP_NORETRY);
-	if (obj->procDirProperties == NULL) {
-		ERRDRV("out of memory\n");
-		goto Away;
-	}
-	for (i = 0; i < type->nProperties; i++) {
-		obj->procDirPropertyContexts[i].procObject = obj;
-		obj->procDirPropertyContexts[i].propertyIndex = i;
-		obj->procDirPropertyContexts[i].show_property =
-			type->show_property;
-		if (type->propertyNames[i][0] != '\0') {
-			/* only create properties that have names */
-			obj->procDirProperties[i] =
-				createProcFile(type->propertyNames[i],
-					obj->procDir, &proc_fops,
-					&obj->procDirPropertyContexts[i]);
-			if (obj->procDirProperties[i] == NULL) {
-				rc = NULL;
-				goto Away;
-			}
-		}
-	}
-	rc = obj;
-Away:
-	if (rc == NULL) {
-		if (obj != NULL) {
-			visor_proc_DestroyObject(obj);
-			obj = NULL;
-		}
-	}
-	return rc;
-}
-EXPORT_SYMBOL_GPL(visor_proc_CreateObject);
-
-
-
-void visor_proc_DestroyObject(MYPROCOBJECT *obj)
-{
-	MYPROCTYPE *type = NULL;
-
-	if (obj == NULL)
-		return;
-	type = obj->type;
-	if (type == NULL)
-		return;
-	if (obj->procDirProperties != NULL) {
-		int i = 0;
-
-		for (i = 0; i < type->nProperties; i++) {
-			if (obj->procDirProperties[i] != NULL) {
-				remove_proc_entry(type->propertyNames[i],
-						  obj->procDir);
-				obj->procDirProperties[i] = NULL;
-			}
-		}
-		kfree(obj->procDirProperties);
-		obj->procDirProperties = NULL;
-	}
-
-	kfree(obj->procDirPropertyContexts);
-	obj->procDirPropertyContexts = NULL;
-
-	if (obj->procDir != NULL) {
-		if (obj->name != NULL)
-			remove_proc_entry(obj->name, type->procDir);
-		obj->procDir = NULL;
-	}
-
-	kfree(obj->name);
-	obj->name = NULL;
-	kfree(obj);
-}
-EXPORT_SYMBOL_GPL(visor_proc_DestroyObject);
-
-
-
-static int seq_show(struct seq_file *seq, void *offset)
-{
-	struct proc_dir_entry_context *ctx = seq->private;
-
-	if (ctx == NULL) {
-		ERRDRV("I don't have a freakin' clue...");
-		return 0;
-	}
-	(*ctx->show_property)(seq, ctx->procObject->context,
-			      ctx->propertyIndex);
-	return 0;
-}
diff --git a/drivers/staging/unisys/visorutil/visorkmodutils.c b/drivers/staging/unisys/visorutil/visorkmodutils.c
index 556e2642d2d9..62f0f7046e17 100644
--- a/drivers/staging/unisys/visorutil/visorkmodutils.c
+++ b/drivers/staging/unisys/visorutil/visorkmodutils.c
@@ -15,7 +15,6 @@
  * details.
  */
 
-#include "uniklog.h"
 #include "timskmod.h"
 
 #define MYDRVNAME "timskmodutils"
diff --git a/drivers/staging/vme/devices/vme_pio2_core.c b/drivers/staging/vme/devices/vme_pio2_core.c
index 84c5a07e8f6a..eabbcc710a20 100644
--- a/drivers/staging/vme/devices/vme_pio2_core.c
+++ b/drivers/staging/vme/devices/vme_pio2_core.c
@@ -13,7 +13,6 @@
 #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
 
 #include <linux/module.h>
-#include <linux/moduleparam.h>
 #include <linux/types.h>
 #include <linux/kernel.h>
 #include <linux/errno.h>
diff --git a/drivers/staging/vme/devices/vme_pio2_gpio.c b/drivers/staging/vme/devices/vme_pio2_gpio.c
index da34d5529f51..77901b345a71 100644
--- a/drivers/staging/vme/devices/vme_pio2_gpio.c
+++ b/drivers/staging/vme/devices/vme_pio2_gpio.c
@@ -11,7 +11,6 @@
  */
 
 #include <linux/module.h>
-#include <linux/moduleparam.h>
 #include <linux/types.h>
 #include <linux/kernel.h>
 #include <linux/errno.h>
diff --git a/drivers/staging/vme/devices/vme_user.c b/drivers/staging/vme/devices/vme_user.c
index 8b1f53331433..19ba749bb122 100644
--- a/drivers/staging/vme/devices/vme_user.c
+++ b/drivers/staging/vme/devices/vme_user.c
@@ -17,6 +17,7 @@
 
 #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
 
+#include <linux/atomic.h>
 #include <linux/cdev.h>
 #include <linux/delay.h>
 #include <linux/device.h>
@@ -41,7 +42,6 @@
 
 #include "vme_user.h"
 
-static DEFINE_MUTEX(vme_user_mutex);
 static const char driver_name[] = "vme_user";
 
 static int bus[VME_USER_BUS_MAX];
@@ -100,6 +100,7 @@ struct image_desc {
 	struct device *device;	/* Sysfs device */
 	struct vme_resource *resource;	/* VME resource */
 	int users;		/* Number of current users */
+	int mmap_count;		/* Number of current mmap's */
 };
 static struct image_desc image[VME_DEVS];
 
@@ -135,6 +136,10 @@ static ssize_t vme_user_write(struct file *, const char __user *, size_t,
 	loff_t *);
 static loff_t vme_user_llseek(struct file *, loff_t, int);
 static long vme_user_unlocked_ioctl(struct file *, unsigned int, unsigned long);
+static int vme_user_mmap(struct file *file, struct vm_area_struct *vma);
+
+static void vme_user_vm_open(struct vm_area_struct *vma);
+static void vme_user_vm_close(struct vm_area_struct *vma);
 
 static int vme_user_match(struct vme_dev *);
 static int vme_user_probe(struct vme_dev *);
@@ -148,6 +153,17 @@ static const struct file_operations vme_user_fops = {
 	.llseek = vme_user_llseek,
 	.unlocked_ioctl = vme_user_unlocked_ioctl,
 	.compat_ioctl = vme_user_unlocked_ioctl,
+	.mmap = vme_user_mmap,
+};
+
+struct vme_user_vma_priv {
+	unsigned int minor;
+	atomic_t refcnt;
+};
+
+static const struct vm_operations_struct vme_user_vm_ops = {
+	.open = vme_user_vm_open,
+	.close = vme_user_vm_close,
 };
 
 
@@ -489,6 +505,11 @@ static int vme_user_ioctl(struct inode *inode, struct file *file,
 
 		case VME_SET_MASTER:
 
+			if (image[minor].mmap_count != 0) {
+				pr_warn("Can't adjust mapped window\n");
+				return -EPERM;
+			}
+
 			copied = copy_from_user(&master, argp, sizeof(master));
 			if (copied != 0) {
 				pr_warn("Partial copy from userspace\n");
@@ -555,14 +576,79 @@ static long
 vme_user_unlocked_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 {
 	int ret;
+	struct inode *inode = file_inode(file);
+	unsigned int minor = MINOR(inode->i_rdev);
 
-	mutex_lock(&vme_user_mutex);
-	ret = vme_user_ioctl(file_inode(file), file, cmd, arg);
-	mutex_unlock(&vme_user_mutex);
+	mutex_lock(&image[minor].mutex);
+	ret = vme_user_ioctl(inode, file, cmd, arg);
+	mutex_unlock(&image[minor].mutex);
 
 	return ret;
 }
 
+static void vme_user_vm_open(struct vm_area_struct *vma)
+{
+	struct vme_user_vma_priv *vma_priv = vma->vm_private_data;
+
+	atomic_inc(&vma_priv->refcnt);
+}
+
+static void vme_user_vm_close(struct vm_area_struct *vma)
+{
+	struct vme_user_vma_priv *vma_priv = vma->vm_private_data;
+	unsigned int minor = vma_priv->minor;
+
+	if (!atomic_dec_and_test(&vma_priv->refcnt))
+		return;
+
+	mutex_lock(&image[minor].mutex);
+	image[minor].mmap_count--;
+	mutex_unlock(&image[minor].mutex);
+
+	kfree(vma_priv);
+}
+
+static int vme_user_master_mmap(unsigned int minor, struct vm_area_struct *vma)
+{
+	int err;
+	struct vme_user_vma_priv *vma_priv;
+
+	mutex_lock(&image[minor].mutex);
+
+	err = vme_master_mmap(image[minor].resource, vma);
+	if (err) {
+		mutex_unlock(&image[minor].mutex);
+		return err;
+	}
+
+	vma_priv = kmalloc(sizeof(struct vme_user_vma_priv), GFP_KERNEL);
+	if (vma_priv == NULL) {
+		mutex_unlock(&image[minor].mutex);
+		return -ENOMEM;
+	}
+
+	vma_priv->minor = minor;
+	atomic_set(&vma_priv->refcnt, 1);
+	vma->vm_ops = &vme_user_vm_ops;
+	vma->vm_private_data = vma_priv;
+
+	image[minor].mmap_count++;
+
+	mutex_unlock(&image[minor].mutex);
+
+	return 0;
+}
+
+static int vme_user_mmap(struct file *file, struct vm_area_struct *vma)
+{
+	unsigned int minor = MINOR(file_inode(file)->i_rdev);
+
+	if (type[minor] == MASTER_MINOR)
+		return vme_user_master_mmap(minor, vma);
+
+	return -ENODEV;
+}
+
 
 /*
  * Unallocate a previously allocated buffer
diff --git a/drivers/staging/vt6655/baseband.c b/drivers/staging/vt6655/baseband.c
index 565ba189afb2..b0ea38f1911c 100644
--- a/drivers/staging/vt6655/baseband.c
+++ b/drivers/staging/vt6655/baseband.c
@@ -2021,14 +2021,20 @@ bool BBbVT3253Init(struct vnt_private *priv)
 	if (byRFType == RF_RFMD2959) {
 		if (byLocalID <= REV_ID_VT3253_A1) {
 			for (ii = 0; ii < CB_VT3253_INIT_FOR_RFMD; ii++)
-				bResult &= BBbWriteEmbedded(priv, byVT3253InitTab_RFMD[ii][0], byVT3253InitTab_RFMD[ii][1]);
+				bResult &= BBbWriteEmbedded(priv,
+					byVT3253InitTab_RFMD[ii][0],
+					byVT3253InitTab_RFMD[ii][1]);
 
 		} else {
 			for (ii = 0; ii < CB_VT3253B0_INIT_FOR_RFMD; ii++)
-				bResult &= BBbWriteEmbedded(priv, byVT3253B0_RFMD[ii][0], byVT3253B0_RFMD[ii][1]);
+				bResult &= BBbWriteEmbedded(priv,
+					byVT3253B0_RFMD[ii][0],
+					byVT3253B0_RFMD[ii][1]);
 
 			for (ii = 0; ii < CB_VT3253B0_AGC_FOR_RFMD2959; ii++)
-				bResult &= BBbWriteEmbedded(priv, byVT3253B0_AGC4_RFMD2959[ii][0], byVT3253B0_AGC4_RFMD2959[ii][1]);
+				bResult &= BBbWriteEmbedded(priv,
+					byVT3253B0_AGC4_RFMD2959[ii][0],
+					byVT3253B0_AGC4_RFMD2959[ii][1]);
 
 			VNSvOutPortD(dwIoBase + MAC_REG_ITRTMSET, 0x23);
 			MACvRegBitsOn(dwIoBase, MAC_REG_PAPEDELAY, BIT(0));
@@ -2043,10 +2049,13 @@ bool BBbVT3253Init(struct vnt_private *priv)
 		priv->ldBmThreshold[3] = 0;
 	} else if ((byRFType == RF_AIROHA) || (byRFType == RF_AL2230S)) {
 		for (ii = 0; ii < CB_VT3253B0_INIT_FOR_AIROHA2230; ii++)
-			bResult &= BBbWriteEmbedded(priv, byVT3253B0_AIROHA2230[ii][0], byVT3253B0_AIROHA2230[ii][1]);
+			bResult &= BBbWriteEmbedded(priv,
+				byVT3253B0_AIROHA2230[ii][0],
+				byVT3253B0_AIROHA2230[ii][1]);
 
 		for (ii = 0; ii < CB_VT3253B0_AGC; ii++)
-			bResult &= BBbWriteEmbedded(priv, byVT3253B0_AGC[ii][0], byVT3253B0_AGC[ii][1]);
+			bResult &= BBbWriteEmbedded(priv,
+				byVT3253B0_AGC[ii][0], byVT3253B0_AGC[ii][1]);
 
 		priv->abyBBVGA[0] = 0x1C;
 		priv->abyBBVGA[1] = 0x10;
@@ -2058,10 +2067,14 @@ bool BBbVT3253Init(struct vnt_private *priv)
 		priv->ldBmThreshold[3] = 0;
 	} else if (byRFType == RF_UW2451) {
 		for (ii = 0; ii < CB_VT3253B0_INIT_FOR_UW2451; ii++)
-			bResult &= BBbWriteEmbedded(priv, byVT3253B0_UW2451[ii][0], byVT3253B0_UW2451[ii][1]);
+			bResult &= BBbWriteEmbedded(priv,
+				byVT3253B0_UW2451[ii][0],
+				byVT3253B0_UW2451[ii][1]);
 
 		for (ii = 0; ii < CB_VT3253B0_AGC; ii++)
-			bResult &= BBbWriteEmbedded(priv, byVT3253B0_AGC[ii][0], byVT3253B0_AGC[ii][1]);
+			bResult &= BBbWriteEmbedded(priv,
+				byVT3253B0_AGC[ii][0],
+				byVT3253B0_AGC[ii][1]);
 
 		VNSvOutPortB(dwIoBase + MAC_REG_ITRTMSET, 0x23);
 		MACvRegBitsOn(dwIoBase, MAC_REG_PAPEDELAY, BIT(0));
@@ -2076,7 +2089,9 @@ bool BBbVT3253Init(struct vnt_private *priv)
 		priv->ldBmThreshold[3] = 0;
 	} else if (byRFType == RF_UW2452) {
 		for (ii = 0; ii < CB_VT3253B0_INIT_FOR_UW2451; ii++)
-			bResult &= BBbWriteEmbedded(priv, byVT3253B0_UW2451[ii][0], byVT3253B0_UW2451[ii][1]);
+			bResult &= BBbWriteEmbedded(priv,
+				byVT3253B0_UW2451[ii][0],
+				byVT3253B0_UW2451[ii][1]);
 
 		/* Init ANT B select,TX Config CR09 = 0x61->0x45, 0x45->0x41(VC1/VC2 define, make the ANT_A, ANT_B inverted) */
 		/*bResult &= BBbWriteEmbedded(dwIoBase,0x09,0x41);*/
@@ -2097,7 +2112,8 @@ bool BBbVT3253Init(struct vnt_private *priv)
 		bResult &= BBbWriteEmbedded(priv, 0xb0, 0x58);
 
 		for (ii = 0; ii < CB_VT3253B0_AGC; ii++)
-			bResult &= BBbWriteEmbedded(priv, byVT3253B0_AGC[ii][0], byVT3253B0_AGC[ii][1]);
+			bResult &= BBbWriteEmbedded(priv,
+				byVT3253B0_AGC[ii][0], byVT3253B0_AGC[ii][1]);
 
 		priv->abyBBVGA[0] = 0x14;
 		priv->abyBBVGA[1] = 0x0A;
@@ -2111,10 +2127,13 @@ bool BBbVT3253Init(struct vnt_private *priv)
 
 	} else if (byRFType == RF_VT3226) {
 		for (ii = 0; ii < CB_VT3253B0_INIT_FOR_AIROHA2230; ii++)
-			bResult &= BBbWriteEmbedded(priv, byVT3253B0_AIROHA2230[ii][0], byVT3253B0_AIROHA2230[ii][1]);
+			bResult &= BBbWriteEmbedded(priv,
+				byVT3253B0_AIROHA2230[ii][0],
+				byVT3253B0_AIROHA2230[ii][1]);
 
 		for (ii = 0; ii < CB_VT3253B0_AGC; ii++)
-			bResult &= BBbWriteEmbedded(priv, byVT3253B0_AGC[ii][0], byVT3253B0_AGC[ii][1]);
+			bResult &= BBbWriteEmbedded(priv,
+				byVT3253B0_AGC[ii][0], byVT3253B0_AGC[ii][1]);
 
 		priv->abyBBVGA[0] = 0x1C;
 		priv->abyBBVGA[1] = 0x10;
@@ -2129,7 +2148,9 @@ bool BBbVT3253Init(struct vnt_private *priv)
 		/* {{ RobertYu: 20050104 */
 	} else if (byRFType == RF_AIROHA7230) {
 		for (ii = 0; ii < CB_VT3253B0_INIT_FOR_AIROHA2230; ii++)
-			bResult &= BBbWriteEmbedded(priv, byVT3253B0_AIROHA2230[ii][0], byVT3253B0_AIROHA2230[ii][1]);
+			bResult &= BBbWriteEmbedded(priv,
+				byVT3253B0_AIROHA2230[ii][0],
+				byVT3253B0_AIROHA2230[ii][1]);
 
 
 		/* {{ RobertYu:20050223, request by JerryChung */
@@ -2142,7 +2163,8 @@ bool BBbVT3253Init(struct vnt_private *priv)
 		/* }} */
 
 		for (ii = 0; ii < CB_VT3253B0_AGC; ii++)
-			bResult &= BBbWriteEmbedded(priv, byVT3253B0_AGC[ii][0], byVT3253B0_AGC[ii][1]);
+			bResult &= BBbWriteEmbedded(priv,
+				byVT3253B0_AGC[ii][0], byVT3253B0_AGC[ii][1]);
 
 		priv->abyBBVGA[0] = 0x1C;
 		priv->abyBBVGA[1] = 0x10;
diff --git a/drivers/staging/vt6655/channel.c b/drivers/staging/vt6655/channel.c
index 3c17725d5910..7a717828fa09 100644
--- a/drivers/staging/vt6655/channel.c
+++ b/drivers/staging/vt6655/channel.c
@@ -193,7 +193,8 @@ bool set_channel(void *pDeviceHandler, struct ieee80211_channel *ch)
 	/* clear NAV */
 	MACvRegBitsOn(pDevice->PortOffset, MAC_REG_MACCR, MACCR_CLRNAV);
 
-	/* TX_PE will reserve 3 us for MAX2829 A mode only, it is for better TX throughput */
+	/* TX_PE will reserve 3 us for MAX2829 A mode only,
+	   it is for better TX throughput */
 
 	if (pDevice->byRFType == RF_AIROHA7230)
 		RFbAL7230SelectChannelPostProcess(pDevice, pDevice->byCurrentCh,
@@ -217,9 +218,11 @@ bool set_channel(void *pDeviceHandler, struct ieee80211_channel *ch)
 		/* set HW default power register */
 		MACvSelectPage1(pDevice->PortOffset);
 		RFbSetPower(pDevice, RATE_1M, pDevice->byCurrentCh);
-		VNSvOutPortB(pDevice->PortOffset + MAC_REG_PWRCCK, pDevice->byCurPwr);
+		VNSvOutPortB(pDevice->PortOffset + MAC_REG_PWRCCK,
+			     pDevice->byCurPwr);
 		RFbSetPower(pDevice, RATE_6M, pDevice->byCurrentCh);
-		VNSvOutPortB(pDevice->PortOffset + MAC_REG_PWROFDM, pDevice->byCurPwr);
+		VNSvOutPortB(pDevice->PortOffset + MAC_REG_PWROFDM,
+			     pDevice->byCurPwr);
 		MACvSelectPage0(pDevice->PortOffset);
 
 		spin_unlock_irqrestore(&pDevice->lock, flags);
diff --git a/drivers/staging/vt6655/device_main.c b/drivers/staging/vt6655/device_main.c
index 4324282afe49..4bb4f8ee4132 100644
--- a/drivers/staging/vt6655/device_main.c
+++ b/drivers/staging/vt6655/device_main.c
@@ -64,9 +64,9 @@
 #include <linux/slab.h>
 
 /*---------------------  Static Definitions -------------------------*/
-//
-// Define module options
-//
+/*
+ * Define module options
+ */
 MODULE_AUTHOR("VIA Networking Technologies, Inc., <lyndonchen@vntek.com.tw>");
 MODULE_LICENSE("GPL");
 MODULE_DESCRIPTION("VIA Networking Solomon-A/B/G Wireless LAN Adapter Driver");
@@ -126,9 +126,9 @@ DEVICE_PARAM(LongRetryLimit, "long frame retry limits");
 
 DEVICE_PARAM(BasebandType, "baseband type");
 
-//
-// Static vars definitions
-//
+/*
+ * Static vars definitions
+ */
 static CHIP_INFO chip_info_table[] = {
 	{ VT3253,       "VIA Networking Solomon-A/B/G Wireless LAN Adapter ",
 	  256, 1,     DEVICE_FLAGS_IP_ALIGN|DEVICE_FLAGS_TX_ALIGN },
@@ -231,9 +231,9 @@ device_set_options(struct vnt_private *pDevice)
 	pr_debug(" byBBType= %d\n", (int)pDevice->byBBType);
 }
 
-//
-// Initialisation of MAC & BBP registers
-//
+/*
+ * Initialisation of MAC & BBP registers
+ */
 
 static void device_init_registers(struct vnt_private *pDevice)
 {
@@ -330,16 +330,6 @@ static void device_init_registers(struct vnt_private *pDevice)
 	/* zonetype initial */
 	pDevice->byOriginalZonetype = pDevice->abyEEPROM[EEP_OFS_ZONETYPE];
 
-	/* Get RFType */
-	pDevice->byRFType = SROMbyReadEmbedded(pDevice->PortOffset, EEP_OFS_RFTYPE);
-
-	/* force change RevID for VT3253 emu */
-	if ((pDevice->byRFType & RF_EMU) != 0)
-			pDevice->byRevId = 0x80;
-
-	pDevice->byRFType &= RF_MASK;
-	pr_debug("pDevice->byRFType = %x\n", pDevice->byRFType);
-
 	if (!pDevice->bZoneRegExist)
 		pDevice->byZoneType = pDevice->abyEEPROM[EEP_OFS_ZONETYPE];
 
@@ -540,12 +530,12 @@ static bool device_init_rings(struct vnt_private *pDevice)
 	void *vir_pool;
 
 	/*allocate all RD/TD rings a single pool*/
-	vir_pool = pci_zalloc_consistent(pDevice->pcid,
+	vir_pool = dma_zalloc_coherent(&pDevice->pcid->dev,
 					 pDevice->sOpts.nRxDescs0 * sizeof(SRxDesc) +
 					 pDevice->sOpts.nRxDescs1 * sizeof(SRxDesc) +
 					 pDevice->sOpts.nTxDescs[0] * sizeof(STxDesc) +
 					 pDevice->sOpts.nTxDescs[1] * sizeof(STxDesc),
-					 &pDevice->pool_dma);
+					 &pDevice->pool_dma, GFP_ATOMIC);
 	if (vir_pool == NULL) {
 		dev_err(&pDevice->pcid->dev, "allocate desc dma memory failed\n");
 		return false;
@@ -559,16 +549,17 @@ static bool device_init_rings(struct vnt_private *pDevice)
 	pDevice->rd1_pool_dma = pDevice->rd0_pool_dma +
 		pDevice->sOpts.nRxDescs0 * sizeof(SRxDesc);
 
-	pDevice->tx0_bufs = pci_zalloc_consistent(pDevice->pcid,
+	pDevice->tx0_bufs = dma_zalloc_coherent(&pDevice->pcid->dev,
 						  pDevice->sOpts.nTxDescs[0] * PKT_BUF_SZ +
 						  pDevice->sOpts.nTxDescs[1] * PKT_BUF_SZ +
 						  CB_BEACON_BUF_SIZE +
 						  CB_MAX_BUF_SIZE,
-						  &pDevice->tx_bufs_dma0);
+						  &pDevice->tx_bufs_dma0,
+						  GFP_ATOMIC);
 	if (pDevice->tx0_bufs == NULL) {
 		dev_err(&pDevice->pcid->dev, "allocate buf dma memory failed\n");
 
-		pci_free_consistent(pDevice->pcid,
+		dma_free_coherent(&pDevice->pcid->dev,
 				    pDevice->sOpts.nRxDescs0 * sizeof(SRxDesc) +
 				    pDevice->sOpts.nRxDescs1 * sizeof(SRxDesc) +
 				    pDevice->sOpts.nTxDescs[0] * sizeof(STxDesc) +
@@ -584,7 +575,7 @@ static bool device_init_rings(struct vnt_private *pDevice)
 	pDevice->td1_pool_dma = pDevice->td0_pool_dma +
 		pDevice->sOpts.nTxDescs[0] * sizeof(STxDesc);
 
-	// vir_pool: pvoid type
+	/* vir_pool: pvoid type */
 	pDevice->apTD0Rings = vir_pool
 		+ pDevice->sOpts.nRxDescs0 * sizeof(SRxDesc)
 		+ pDevice->sOpts.nRxDescs1 * sizeof(SRxDesc);
@@ -614,7 +605,7 @@ static bool device_init_rings(struct vnt_private *pDevice)
 
 static void device_free_rings(struct vnt_private *pDevice)
 {
-	pci_free_consistent(pDevice->pcid,
+	dma_free_coherent(&pDevice->pcid->dev,
 			    pDevice->sOpts.nRxDescs0 * sizeof(SRxDesc) +
 			    pDevice->sOpts.nRxDescs1 * sizeof(SRxDesc) +
 			    pDevice->sOpts.nTxDescs[0] * sizeof(STxDesc) +
@@ -624,7 +615,7 @@ static void device_free_rings(struct vnt_private *pDevice)
 		);
 
 	if (pDevice->tx0_bufs)
-		pci_free_consistent(pDevice->pcid,
+		dma_free_coherent(&pDevice->pcid->dev,
 				    pDevice->sOpts.nTxDescs[0] * PKT_BUF_SZ +
 				    pDevice->sOpts.nTxDescs[1] * PKT_BUF_SZ +
 				    CB_BEACON_BUF_SIZE +
@@ -689,8 +680,8 @@ static void device_free_rd0_ring(struct vnt_private *pDevice)
 		PSRxDesc        pDesc = &(pDevice->aRD0Ring[i]);
 		PDEVICE_RD_INFO  pRDInfo = pDesc->pRDInfo;
 
-		pci_unmap_single(pDevice->pcid, pRDInfo->skb_dma,
-				 pDevice->rx_buf_sz, PCI_DMA_FROMDEVICE);
+		dma_unmap_single(&pDevice->pcid->dev, pRDInfo->skb_dma,
+				 pDevice->rx_buf_sz, DMA_FROM_DEVICE);
 
 		dev_kfree_skb(pRDInfo->skb);
 
@@ -706,8 +697,8 @@ static void device_free_rd1_ring(struct vnt_private *pDevice)
 		PSRxDesc        pDesc = &(pDevice->aRD1Ring[i]);
 		PDEVICE_RD_INFO  pRDInfo = pDesc->pRDInfo;
 
-		pci_unmap_single(pDevice->pcid, pRDInfo->skb_dma,
-				 pDevice->rx_buf_sz, PCI_DMA_FROMDEVICE);
+		dma_unmap_single(&pDevice->pcid->dev, pRDInfo->skb_dma,
+				 pDevice->rx_buf_sz, DMA_FROM_DEVICE);
 
 		dev_kfree_skb(pRDInfo->skb);
 
@@ -775,8 +766,8 @@ static void device_free_td0_ring(struct vnt_private *pDevice)
 		PDEVICE_TD_INFO  pTDInfo = pDesc->pTDInfo;
 
 		if (pTDInfo->skb_dma && (pTDInfo->skb_dma != pTDInfo->buf_dma))
-			pci_unmap_single(pDevice->pcid, pTDInfo->skb_dma,
-					 pTDInfo->skb->len, PCI_DMA_TODEVICE);
+			dma_unmap_single(&pDevice->pcid->dev, pTDInfo->skb_dma,
+					 pTDInfo->skb->len, DMA_TO_DEVICE);
 
 		if (pTDInfo->skb)
 			dev_kfree_skb(pTDInfo->skb);
@@ -794,8 +785,8 @@ static void device_free_td1_ring(struct vnt_private *pDevice)
 		PDEVICE_TD_INFO  pTDInfo = pDesc->pTDInfo;
 
 		if (pTDInfo->skb_dma && (pTDInfo->skb_dma != pTDInfo->buf_dma))
-			pci_unmap_single(pDevice->pcid, pTDInfo->skb_dma,
-					 pTDInfo->skb->len, PCI_DMA_TODEVICE);
+			dma_unmap_single(&pDevice->pcid->dev, pTDInfo->skb_dma,
+					 pTDInfo->skb->len, DMA_TO_DEVICE);
 
 		if (pTDInfo->skb)
 			dev_kfree_skb(pTDInfo->skb);
@@ -841,9 +832,9 @@ static bool device_alloc_rx_buf(struct vnt_private *pDevice, PSRxDesc pRD)
 	ASSERT(pRDInfo->skb);
 
 	pRDInfo->skb_dma =
-		pci_map_single(pDevice->pcid,
+		dma_map_single(&pDevice->pcid->dev,
 			       skb_put(pRDInfo->skb, skb_tailroom(pRDInfo->skb)),
-			       pDevice->rx_buf_sz, PCI_DMA_FROMDEVICE);
+			       pDevice->rx_buf_sz, DMA_FROM_DEVICE);
 
 	*((unsigned int *)&(pRD->m_rd0RD0)) = 0; /* FIX cast */
 
@@ -943,7 +934,7 @@ static int device_tx_srv(struct vnt_private *pDevice, unsigned int uIdx)
 		byTsr0 = pTD->m_td0TD0.byTSR0;
 		byTsr1 = pTD->m_td0TD0.byTSR1;
 
-		//Only the status of first TD in the chain is correct
+		/* Only the status of first TD in the chain is correct */
 		if (pTD->m_td1TD1.byTCR & TCR_STP) {
 			if ((pTD->pTDInfo->byFlags & TD_FLAGS_NETIF_SKB) != 0) {
 
@@ -992,10 +983,10 @@ static void device_free_tx_buf(struct vnt_private *pDevice, PSTxDesc pDesc)
 	PDEVICE_TD_INFO  pTDInfo = pDesc->pTDInfo;
 	struct sk_buff *skb = pTDInfo->skb;
 
-	// pre-allocated buf_dma can't be unmapped.
+	/* pre-allocated buf_dma can't be unmapped. */
 	if (pTDInfo->skb_dma && (pTDInfo->skb_dma != pTDInfo->buf_dma)) {
-		pci_unmap_single(pDevice->pcid, pTDInfo->skb_dma, skb->len,
-				 PCI_DMA_TODEVICE);
+		dma_unmap_single(&pDevice->pcid->dev, pTDInfo->skb_dma,
+				 skb->len, DMA_TO_DEVICE);
 	}
 
 	if (pTDInfo->byFlags & TD_FLAGS_NETIF_SKB)
@@ -1084,7 +1075,7 @@ static  irqreturn_t  device_intr(int irq,  void *dev_instance)
 
 	spin_lock_irqsave(&pDevice->lock, flags);
 
-	//Make sure current page is 0
+	/* Make sure current page is 0 */
 	VNSvInPortB(pDevice->PortOffset + MAC_REG_PAGE1SEL, &byOrgPageSel);
 	if (byOrgPageSel == 1)
 		MACvSelectPage0(pDevice->PortOffset);
@@ -1092,10 +1083,12 @@ static  irqreturn_t  device_intr(int irq,  void *dev_instance)
 		byOrgPageSel = 0;
 
 	MACvReadMIBCounter(pDevice->PortOffset, &dwMIBCounter);
-	// TBD....
-	// Must do this after doing rx/tx, cause ISR bit is slow
-	// than RD/TD write back
-	// update ISR counter
+	/*
+	 * TBD....
+	 * Must do this after doing rx/tx, cause ISR bit is slow
+	 * than RD/TD write back
+	 * update ISR counter
+	 */
 	STAvUpdate802_11Counter(&pDevice->s802_11Counter, &pDevice->scStatistic, dwMIBCounter);
 	while (pDevice->dwIsr != 0) {
 		STAvUpdateIsrStatCounter(&pDevice->scStatistic, pDevice->dwIsr);
@@ -1187,12 +1180,14 @@ static int vnt_tx_packet(struct vnt_private *priv, struct sk_buff *skb)
 {
 	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)skb->data;
 	PSTxDesc head_td;
-	u32 dma_idx = TYPE_AC0DMA;
+	u32 dma_idx;
 	unsigned long flags;
 
 	spin_lock_irqsave(&priv->lock, flags);
 
-	if (!ieee80211_is_data(hdr->frame_control))
+	if (ieee80211_is_data(hdr->frame_control))
+		dma_idx = TYPE_AC0DMA;
+	else
 		dma_idx = TYPE_TXDMA0;
 
 	if (AVAIL_TD(priv, dma_idx) < 1) {
@@ -1206,6 +1201,9 @@ static int vnt_tx_packet(struct vnt_private *priv, struct sk_buff *skb)
 
 	head_td->pTDInfo->skb = skb;
 
+	if (dma_idx == TYPE_AC0DMA)
+		head_td->pTDInfo->byFlags = TD_FLAGS_NETIF_SKB;
+
 	priv->iTDUsed[dma_idx]++;
 
 	/* Take ownership */
@@ -1234,13 +1232,10 @@ static int vnt_tx_packet(struct vnt_private *priv, struct sk_buff *skb)
 
 	head_td->buff_addr = cpu_to_le32(head_td->pTDInfo->skb_dma);
 
-	if (dma_idx == TYPE_AC0DMA) {
-		head_td->pTDInfo->byFlags = TD_FLAGS_NETIF_SKB;
-
+	if (head_td->pTDInfo->byFlags & TD_FLAGS_NETIF_SKB)
 		MACvTransmitAC0(priv->PortOffset);
-	} else {
+	else
 		MACvTransmit0(priv->PortOffset);
-	}
 
 	spin_unlock_irqrestore(&priv->lock, flags);
 
@@ -1778,6 +1773,12 @@ vt6655_probe(struct pci_dev *pcid, const struct pci_device_id *ent)
 	MACvInitialize(priv->PortOffset);
 	MACvReadEtherAddress(priv->PortOffset, priv->abyCurrentNetAddr);
 
+	/* Get RFType */
+	priv->byRFType = SROMbyReadEmbedded(priv->PortOffset, EEP_OFS_RFTYPE);
+	priv->byRFType &= RF_MASK;
+
+	dev_dbg(&pcid->dev, "RF Type = %x\n", priv->byRFType);
+
 	device_get_options(priv);
 	device_set_options(priv);
 	/* Mask out the options cannot be set to the chip */
diff --git a/drivers/staging/vt6655/dpc.c b/drivers/staging/vt6655/dpc.c
index 3c5b87ffdcac..b25ee962558d 100644
--- a/drivers/staging/vt6655/dpc.c
+++ b/drivers/staging/vt6655/dpc.c
@@ -140,8 +140,8 @@ bool vnt_receive_frame(struct vnt_private *priv, PSRxDesc curr_rd)
 
 	skb = rd_info->skb;
 
-	pci_unmap_single(priv->pcid, rd_info->skb_dma,
-			 priv->rx_buf_sz, PCI_DMA_FROMDEVICE);
+	dma_unmap_single(&priv->pcid->dev, rd_info->skb_dma,
+			 priv->rx_buf_sz, DMA_FROM_DEVICE);
 
 	frame_size = le16_to_cpu(curr_rd->m_rd1RD1.wReqCount)
 			- cpu_to_le16(curr_rd->m_rd0RD0.wResCount);
diff --git a/drivers/staging/vt6655/key.h b/drivers/staging/vt6655/key.h
index c01d4afb6ab8..261f8181d410 100644
--- a/drivers/staging/vt6655/key.h
+++ b/drivers/staging/vt6655/key.h
@@ -66,4 +66,4 @@ int vnt_key_init_table(struct vnt_private *);
 int vnt_set_keys(struct ieee80211_hw *hw, struct ieee80211_sta *sta,
 		 struct ieee80211_vif *vif, struct ieee80211_key_conf *key);
 
-#endif // __KEY_H__
+#endif /* __KEY_H__ */
diff --git a/drivers/staging/vt6655/mac.c b/drivers/staging/vt6655/mac.c
index 3653a2bd1e36..8048b3263360 100644
--- a/drivers/staging/vt6655/mac.c
+++ b/drivers/staging/vt6655/mac.c
@@ -141,7 +141,7 @@ bool MACbIsIntDisable(void __iomem *dwIoBase)
  */
 void MACvSetShortRetryLimit(void __iomem *dwIoBase, unsigned char byRetryLimit)
 {
-	// set SRT
+	/* set SRT */
 	VNSvOutPortB(dwIoBase + MAC_REG_SRT, byRetryLimit);
 }
 
@@ -162,7 +162,7 @@ void MACvSetShortRetryLimit(void __iomem *dwIoBase, unsigned char byRetryLimit)
  */
 void MACvSetLongRetryLimit(void __iomem *dwIoBase, unsigned char byRetryLimit)
 {
-	// set LRT
+	/* set LRT */
 	VNSvOutPortB(dwIoBase + MAC_REG_LRT, byRetryLimit);
 }
 
@@ -186,7 +186,7 @@ void MACvSetLoopbackMode(void __iomem *dwIoBase, unsigned char byLoopbackMode)
 
 	ASSERT(byLoopbackMode < 3);
 	byLoopbackMode <<= 6;
-	// set TCR
+	/* set TCR */
 	VNSvInPortB(dwIoBase + MAC_REG_TEST, &byOrgValue);
 	byOrgValue = byOrgValue & 0x3F;
 	byOrgValue = byOrgValue | byLoopbackMode;
@@ -210,13 +210,13 @@ void MACvSaveContext(void __iomem *dwIoBase, unsigned char *pbyCxtBuf)
 {
 	int         ii;
 
-	// read page0 register
+	/* read page0 register */
 	for (ii = 0; ii < MAC_MAX_CONTEXT_SIZE_PAGE0; ii++)
 		VNSvInPortB((dwIoBase + ii), (pbyCxtBuf + ii));
 
 	MACvSelectPage1(dwIoBase);
 
-	// read page1 register
+	/* read page1 register */
 	for (ii = 0; ii < MAC_MAX_CONTEXT_SIZE_PAGE1; ii++)
 		VNSvInPortB((dwIoBase + ii), (pbyCxtBuf + MAC_MAX_CONTEXT_SIZE_PAGE0 + ii));
 
@@ -242,27 +242,27 @@ void MACvRestoreContext(void __iomem *dwIoBase, unsigned char *pbyCxtBuf)
 	int         ii;
 
 	MACvSelectPage1(dwIoBase);
-	// restore page1
+	/* restore page1 */
 	for (ii = 0; ii < MAC_MAX_CONTEXT_SIZE_PAGE1; ii++)
 		VNSvOutPortB((dwIoBase + ii), *(pbyCxtBuf + MAC_MAX_CONTEXT_SIZE_PAGE0 + ii));
 
 	MACvSelectPage0(dwIoBase);
 
-	// restore RCR,TCR,IMR...
+	/* restore RCR,TCR,IMR... */
 	for (ii = MAC_REG_RCR; ii < MAC_REG_ISR; ii++)
 		VNSvOutPortB(dwIoBase + ii, *(pbyCxtBuf + ii));
 
-	// restore MAC Config.
+	/* restore MAC Config. */
 	for (ii = MAC_REG_LRT; ii < MAC_REG_PAGE1SEL; ii++)
 		VNSvOutPortB(dwIoBase + ii, *(pbyCxtBuf + ii));
 
 	VNSvOutPortB(dwIoBase + MAC_REG_CFG, *(pbyCxtBuf + MAC_REG_CFG));
 
-	// restore PS Config.
+	/* restore PS Config. */
 	for (ii = MAC_REG_PSCFG; ii < MAC_REG_BBREGCTL; ii++)
 		VNSvOutPortB(dwIoBase + ii, *(pbyCxtBuf + ii));
 
-	// restore CURR_RX_DESC_ADDR, CURR_TX_DESC_ADDR
+	/* restore CURR_RX_DESC_ADDR, CURR_TX_DESC_ADDR */
 	VNSvOutPortD(dwIoBase + MAC_REG_TXDMAPTR0, *(unsigned long *)(pbyCxtBuf + MAC_REG_TXDMAPTR0));
 	VNSvOutPortD(dwIoBase + MAC_REG_AC0DMAPTR, *(unsigned long *)(pbyCxtBuf + MAC_REG_AC0DMAPTR));
 	VNSvOutPortD(dwIoBase + MAC_REG_BCNDMAPTR, *(unsigned long *)(pbyCxtBuf + MAC_REG_BCNDMAPTR));
@@ -290,7 +290,7 @@ bool MACbSoftwareReset(void __iomem *dwIoBase)
 	unsigned char byData;
 	unsigned short ww;
 
-	// turn on HOSTCR_SOFTRST, just write 0x01 to reset
+	/* turn on HOSTCR_SOFTRST, just write 0x01 to reset */
 	VNSvOutPortB(dwIoBase + MAC_REG_HOSTCR, 0x01);
 
 	for (ww = 0; ww < W_MAX_TIMEOUT; ww++) {
@@ -321,15 +321,15 @@ bool MACbSafeSoftwareReset(void __iomem *dwIoBase)
 	unsigned char abyTmpRegData[MAC_MAX_CONTEXT_SIZE_PAGE0+MAC_MAX_CONTEXT_SIZE_PAGE1];
 	bool bRetVal;
 
-	// PATCH....
-	// save some important register's value, then do
-	// reset, then restore register's value
-
-	// save MAC context
+	/* PATCH....
+	 * save some important register's value, then do
+	 * reset, then restore register's value
+	 */
+	/* save MAC context */
 	MACvSaveContext(dwIoBase, abyTmpRegData);
-	// do reset
+	/* do reset */
 	bRetVal = MACbSoftwareReset(dwIoBase);
-	// restore MAC context, except CR0
+	/* restore MAC context, except CR0 */
 	MACvRestoreContext(dwIoBase, abyTmpRegData);
 
 	return bRetVal;
@@ -354,9 +354,9 @@ bool MACbSafeRxOff(void __iomem *dwIoBase)
 	unsigned long dwData;
 	unsigned char byData;
 
-	// turn off wow temp for turn off Rx safely
+	/* turn off wow temp for turn off Rx safely */
 
-	// Clear RX DMA0,1
+	/* Clear RX DMA0,1 */
 	VNSvOutPortD(dwIoBase + MAC_REG_RXDMACTL0, DMACTL_CLRRUN);
 	VNSvOutPortD(dwIoBase + MAC_REG_RXDMACTL1, DMACTL_CLRRUN);
 	for (ww = 0; ww < W_MAX_TIMEOUT; ww++) {
@@ -380,9 +380,9 @@ bool MACbSafeRxOff(void __iomem *dwIoBase)
 		return false;
 	}
 
-	// try to safe shutdown RX
+	/* try to safe shutdown RX */
 	MACvRegBitsOff(dwIoBase, MAC_REG_HOSTCR, HOSTCR_RXON);
-	// W_MAX_TIMEOUT is the timeout period
+	/* W_MAX_TIMEOUT is the timeout period */
 	for (ww = 0; ww < W_MAX_TIMEOUT; ww++) {
 		VNSvInPortB(dwIoBase + MAC_REG_HOSTCR, &byData);
 		if (!(byData & HOSTCR_RXONST))
@@ -415,10 +415,10 @@ bool MACbSafeTxOff(void __iomem *dwIoBase)
 	unsigned long dwData;
 	unsigned char byData;
 
-	// Clear TX DMA
-	//Tx0
+	/* Clear TX DMA */
+	/* Tx0 */
 	VNSvOutPortD(dwIoBase + MAC_REG_TXDMACTL0, DMACTL_CLRRUN);
-	//AC0
+	/* AC0 */
 	VNSvOutPortD(dwIoBase + MAC_REG_AC0DMACTL, DMACTL_CLRRUN);
 
 	for (ww = 0; ww < W_MAX_TIMEOUT; ww++) {
@@ -442,10 +442,10 @@ bool MACbSafeTxOff(void __iomem *dwIoBase)
 		return false;
 	}
 
-	// try to safe shutdown TX
+	/* try to safe shutdown TX */
 	MACvRegBitsOff(dwIoBase, MAC_REG_HOSTCR, HOSTCR_TXON);
 
-	// W_MAX_TIMEOUT is the timeout period
+	/* W_MAX_TIMEOUT is the timeout period */
 	for (ww = 0; ww < W_MAX_TIMEOUT; ww++) {
 		VNSvInPortB(dwIoBase + MAC_REG_HOSTCR, &byData);
 		if (!(byData & HOSTCR_TXONST))
@@ -509,10 +509,10 @@ bool MACbSafeStop(void __iomem *dwIoBase)
  */
 bool MACbShutdown(void __iomem *dwIoBase)
 {
-	// disable MAC IMR
+	/* disable MAC IMR */
 	MACvIntDisable(dwIoBase);
 	MACvSetLoopbackMode(dwIoBase, MAC_LB_INTERNAL);
-	// stop the adapter
+	/* stop the adapter */
 	if (!MACbSafeStop(dwIoBase)) {
 		MACvSetLoopbackMode(dwIoBase, MAC_LB_NONE);
 		return false;
@@ -536,18 +536,18 @@ bool MACbShutdown(void __iomem *dwIoBase)
  */
 void MACvInitialize(void __iomem *dwIoBase)
 {
-	// clear sticky bits
+	/* clear sticky bits */
 	MACvClearStckDS(dwIoBase);
-	// disable force PME-enable
+	/* disable force PME-enable */
 	VNSvOutPortB(dwIoBase + MAC_REG_PMC1, PME_OVR);
-	// only 3253 A
+	/* only 3253 A */
 
-	// do reset
+	/* do reset */
 	MACbSoftwareReset(dwIoBase);
 
-	// reset TSF counter
+	/* reset TSF counter */
 	VNSvOutPortB(dwIoBase + MAC_REG_TFTCTL, TFTCTL_TSFCNTRST);
-	// enable TSF counter
+	/* enable TSF counter */
 	VNSvOutPortB(dwIoBase + MAC_REG_TFTCTL, TFTCTL_TSFCNTREN);
 }
 
@@ -678,7 +678,7 @@ void MACvSetCurrTx0DescAddrEx(void __iomem *dwIoBase, unsigned long dwCurrDescAd
  * Return Value: none
  *
  */
-//TxDMA1 = AC0DMA
+/* TxDMA1 = AC0DMA */
 void MACvSetCurrAC0DescAddrEx(void __iomem *dwIoBase, unsigned long dwCurrDescAddr)
 {
 	unsigned short ww;
@@ -733,7 +733,7 @@ void MACvTimer0MicroSDelay(void __iomem *dwIoBase, unsigned int uDelay)
 	VNSvOutPortB(dwIoBase + MAC_REG_TMCTL0, 0);
 	VNSvOutPortD(dwIoBase + MAC_REG_TMDATA0, uDelay);
 	VNSvOutPortB(dwIoBase + MAC_REG_TMCTL0, (TMCTL_TMD | TMCTL_TE));
-	for (ii = 0; ii < 66; ii++) {  // assume max PCI clock is 66Mhz
+	for (ii = 0; ii < 66; ii++) {  /* assume max PCI clock is 66Mhz */
 		for (uu = 0; uu < uDelay; uu++) {
 			VNSvInPortB(dwIoBase + MAC_REG_TMCTL0, &byValue);
 			if ((byValue == 0) ||
@@ -780,14 +780,14 @@ bool MACbPSWakeup(void __iomem *dwIoBase)
 {
 	unsigned char byOrgValue;
 	unsigned int ww;
-	// Read PSCTL
+	/* Read PSCTL */
 	if (MACbIsRegBitsOff(dwIoBase, MAC_REG_PSCTL, PSCTL_PS))
 		return true;
 
-	// Disable PS
+	/* Disable PS */
 	MACvRegBitsOff(dwIoBase, MAC_REG_PSCTL, PSCTL_PSEN);
 
-	// Check if SyncFlushOK
+	/* Check if SyncFlushOK */
 	for (ww = 0; ww < W_MAX_TIMEOUT; ww++) {
 		VNSvInPortB(dwIoBase + MAC_REG_PSCTL, &byOrgValue);
 		if (byOrgValue & PSCTL_WAKEDONE)
@@ -859,7 +859,7 @@ void MACvSetKeyEntry(void __iomem *dwIoBase, unsigned short wKeyCtl, unsigned in
 
 	wOffset += (uKeyIdx * 4);
 	for (ii = 0; ii < 4; ii++) {
-		// always push 128 bits
+		/* always push 128 bits */
 		pr_debug("3.(%d) wOffset: %d, Data: %X\n",
 			 ii, wOffset+ii, *pdwKey);
 		VNSvOutPortW(dwIoBase + MAC_REG_MISCFFNDEX, wOffset+ii);
diff --git a/drivers/staging/vt6655/mib.c b/drivers/staging/vt6655/mib.c
index d2f351d19ff8..d55c762027ed 100644
--- a/drivers/staging/vt6655/mib.c
+++ b/drivers/staging/vt6655/mib.c
@@ -63,51 +63,51 @@ void STAvUpdateIsrStatCounter(PSStatCounter pStatistic, unsigned long dwIsr)
 	/**********************/
 	/* ABNORMAL interrupt */
 	/**********************/
-	// not any IMR bit invoke irq
+	/* not any IMR bit invoke irq */
 
 	if (dwIsr == 0) {
 		pStatistic->ISRStat.dwIsrUnknown++;
 		return;
 	}
 
-//Added by Kyle
-	if (dwIsr & ISR_TXDMA0)               // ISR, bit0
-		pStatistic->ISRStat.dwIsrTx0OK++;             // TXDMA0 successful
+/* Added by Kyle */
+	if (dwIsr & ISR_TXDMA0)               /* ISR, bit0 */
+		pStatistic->ISRStat.dwIsrTx0OK++;             /* TXDMA0 successful */
 
-	if (dwIsr & ISR_AC0DMA)               // ISR, bit1
-		pStatistic->ISRStat.dwIsrAC0TxOK++;           // AC0DMA successful
+	if (dwIsr & ISR_AC0DMA)               /* ISR, bit1 */
+		pStatistic->ISRStat.dwIsrAC0TxOK++;           /* AC0DMA successful */
 
-	if (dwIsr & ISR_BNTX)                 // ISR, bit2
-		pStatistic->ISRStat.dwIsrBeaconTxOK++;        // BeaconTx successful
+	if (dwIsr & ISR_BNTX)                 /* ISR, bit2 */
+		pStatistic->ISRStat.dwIsrBeaconTxOK++;        /* BeaconTx successful */
 
-	if (dwIsr & ISR_RXDMA0)               // ISR, bit3
-		pStatistic->ISRStat.dwIsrRx0OK++;             // Rx0 successful
+	if (dwIsr & ISR_RXDMA0)               /* ISR, bit3 */
+		pStatistic->ISRStat.dwIsrRx0OK++;             /* Rx0 successful */
 
-	if (dwIsr & ISR_TBTT)                 // ISR, bit4
-		pStatistic->ISRStat.dwIsrTBTTInt++;           // TBTT successful
+	if (dwIsr & ISR_TBTT)                 /* ISR, bit4 */
+		pStatistic->ISRStat.dwIsrTBTTInt++;           /* TBTT successful */
 
-	if (dwIsr & ISR_SOFTTIMER)            // ISR, bit6
+	if (dwIsr & ISR_SOFTTIMER)            /* ISR, bit6 */
 		pStatistic->ISRStat.dwIsrSTIMERInt++;
 
-	if (dwIsr & ISR_WATCHDOG)             // ISR, bit7
+	if (dwIsr & ISR_WATCHDOG)             /* ISR, bit7 */
 		pStatistic->ISRStat.dwIsrWatchDog++;
 
-	if (dwIsr & ISR_FETALERR)             // ISR, bit8
+	if (dwIsr & ISR_FETALERR)             /* ISR, bit8 */
 		pStatistic->ISRStat.dwIsrUnrecoverableError++;
 
-	if (dwIsr & ISR_SOFTINT)              // ISR, bit9
-		pStatistic->ISRStat.dwIsrSoftInterrupt++;     // software interrupt
+	if (dwIsr & ISR_SOFTINT)              /* ISR, bit9 */
+		pStatistic->ISRStat.dwIsrSoftInterrupt++;     /* software interrupt */
 
-	if (dwIsr & ISR_MIBNEARFULL)          // ISR, bit10
+	if (dwIsr & ISR_MIBNEARFULL)          /* ISR, bit10 */
 		pStatistic->ISRStat.dwIsrMIBNearfull++;
 
-	if (dwIsr & ISR_RXNOBUF)              // ISR, bit11
-		pStatistic->ISRStat.dwIsrRxNoBuf++;           // Rx No Buff
+	if (dwIsr & ISR_RXNOBUF)              /* ISR, bit11 */
+		pStatistic->ISRStat.dwIsrRxNoBuf++;           /* Rx No Buff */
 
-	if (dwIsr & ISR_RXDMA1)               // ISR, bit12
-		pStatistic->ISRStat.dwIsrRx1OK++;             // Rx1 successful
+	if (dwIsr & ISR_RXDMA1)               /* ISR, bit12 */
+		pStatistic->ISRStat.dwIsrRx1OK++;             /* Rx1 successful */
 
-	if (dwIsr & ISR_SOFTTIMER1)           // ISR, bit21
+	if (dwIsr & ISR_SOFTTIMER1)           /* ISR, bit21 */
 		pStatistic->ISRStat.dwIsrSTIMER1Int++;
 }
 
diff --git a/drivers/staging/vt6655/rf.c b/drivers/staging/vt6655/rf.c
index 941b2adca95a..7626f635f160 100644
--- a/drivers/staging/vt6655/rf.c
+++ b/drivers/staging/vt6655/rf.c
@@ -794,6 +794,7 @@ bool RFbSetPower(
 		break;
 	case RATE_6M:
 	case RATE_9M:
+	case RATE_12M:
 	case RATE_18M:
 		byPwr = priv->abyOFDMPwrTbl[uCH];
 		if (priv->byRFType == RF_UW2452)
diff --git a/drivers/staging/vt6655/rxtx.c b/drivers/staging/vt6655/rxtx.c
index 07ce3fd88e70..74687761bd2e 100644
--- a/drivers/staging/vt6655/rxtx.c
+++ b/drivers/staging/vt6655/rxtx.c
@@ -64,21 +64,21 @@
 /*---------------------  Static Functions  --------------------------*/
 
 /*---------------------  Static Definitions -------------------------*/
-#define CRITICAL_PACKET_LEN      256    // if packet size < 256 -> in-direct send
-                                        //    packet size >= 256 -> direct send
+#define CRITICAL_PACKET_LEN      256    /* if packet size < 256 -> in-direct send
+                                            packet size >= 256 -> direct send */
 
 static const unsigned short wTimeStampOff[2][MAX_RATE] = {
-	{384, 288, 226, 209, 54, 43, 37, 31, 28, 25, 24, 23}, // Long Preamble
-	{384, 192, 130, 113, 54, 43, 37, 31, 28, 25, 24, 23}, // Short Preamble
+	{384, 288, 226, 209, 54, 43, 37, 31, 28, 25, 24, 23}, /* Long Preamble */
+	{384, 192, 130, 113, 54, 43, 37, 31, 28, 25, 24, 23}, /* Short Preamble */
 };
 
 static const unsigned short wFB_Opt0[2][5] = {
-	{RATE_12M, RATE_18M, RATE_24M, RATE_36M, RATE_48M}, // fallback_rate0
-	{RATE_12M, RATE_12M, RATE_18M, RATE_24M, RATE_36M}, // fallback_rate1
+	{RATE_12M, RATE_18M, RATE_24M, RATE_36M, RATE_48M}, /* fallback_rate0 */
+	{RATE_12M, RATE_12M, RATE_18M, RATE_24M, RATE_36M}, /* fallback_rate1 */
 };
 static const unsigned short wFB_Opt1[2][5] = {
-	{RATE_12M, RATE_18M, RATE_24M, RATE_24M, RATE_36M}, // fallback_rate0
-	{RATE_6M , RATE_6M,  RATE_12M, RATE_12M, RATE_18M}, // fallback_rate1
+	{RATE_12M, RATE_18M, RATE_24M, RATE_24M, RATE_36M}, /* fallback_rate0 */
+	{RATE_6M , RATE_6M,  RATE_12M, RATE_12M, RATE_18M}, /* fallback_rate1 */
 };
 
 #define RTSDUR_BB       0
@@ -116,7 +116,7 @@ void
 s_vGenerateTxParameter(
 	struct vnt_private *pDevice,
 	unsigned char byPktType,
-	void *pTxBufHead,
+	struct vnt_tx_fifo_head *,
 	void *pvRrvTime,
 	void *pvRTS,
 	void *pvCTS,
@@ -176,9 +176,9 @@ s_uGetTxRsvTime(
 	unsigned int uDataTime, uAckTime;
 
 	uDataTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, cbFrameLength, wRate);
-	if (byPktType == PK_TYPE_11B) //llb,CCK mode
+	if (byPktType == PK_TYPE_11B) /* llb,CCK mode */
 		uAckTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 14, (unsigned short)pDevice->byTopCCKBasicRate);
-	else //11g 2.4G OFDM mode & 11a 5G OFDM mode
+	else /* 11g 2.4G OFDM mode & 11a 5G OFDM mode */
 		uAckTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 14, (unsigned short)pDevice->byTopOFDMBasicRate);
 
 	if (bNeedAck)
@@ -194,7 +194,7 @@ static __le16 vnt_rxtx_rsvtime_le16(struct vnt_private *priv, u8 pkt_type,
 						frame_length, rate, need_ack));
 }
 
-//byFreqType: 0=>5GHZ 1=>2.4GHZ
+/* byFreqType: 0=>5GHZ 1=>2.4GHZ */
 static
 __le16
 s_uGetRTSCTSRsvTime(
@@ -210,29 +210,29 @@ s_uGetRTSCTSRsvTime(
 	uRrvTime = uRTSTime = uCTSTime = uAckTime = uDataTime = 0;
 
 	uDataTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, cbFrameLength, wCurrentRate);
-	if (byRTSRsvType == 0) { //RTSTxRrvTime_bb
+	if (byRTSRsvType == 0) { /* RTSTxRrvTime_bb */
 		uRTSTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 20, pDevice->byTopCCKBasicRate);
 		uCTSTime = uAckTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 14, pDevice->byTopCCKBasicRate);
-	} else if (byRTSRsvType == 1) { //RTSTxRrvTime_ba, only in 2.4GHZ
+	} else if (byRTSRsvType == 1) { /* RTSTxRrvTime_ba, only in 2.4GHZ */
 		uRTSTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 20, pDevice->byTopCCKBasicRate);
 		uCTSTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 14, pDevice->byTopCCKBasicRate);
 		uAckTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 14, pDevice->byTopOFDMBasicRate);
-	} else if (byRTSRsvType == 2) { //RTSTxRrvTime_aa
+	} else if (byRTSRsvType == 2) { /* RTSTxRrvTime_aa */
 		uRTSTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 20, pDevice->byTopOFDMBasicRate);
 		uCTSTime = uAckTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 14, pDevice->byTopOFDMBasicRate);
-	} else if (byRTSRsvType == 3) { //CTSTxRrvTime_ba, only in 2.4GHZ
+	} else if (byRTSRsvType == 3) { /* CTSTxRrvTime_ba, only in 2.4GHZ */
 		uCTSTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 14, pDevice->byTopCCKBasicRate);
 		uAckTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 14, pDevice->byTopOFDMBasicRate);
 		uRrvTime = uCTSTime + uAckTime + uDataTime + 2*pDevice->uSIFS;
 		return cpu_to_le16((u16)uRrvTime);
 	}
 
-	//RTSRrvTime
+	/* RTSRrvTime */
 	uRrvTime = uRTSTime + uCTSTime + uAckTime + uDataTime + 3*pDevice->uSIFS;
 	return cpu_to_le16((u16)uRrvTime);
 }
 
-//byFreqType 0: 5GHz, 1:2.4Ghz
+/* byFreqType 0: 5GHz, 1:2.4Ghz */
 static
 unsigned int
 s_uGetDataDuration(
@@ -248,22 +248,22 @@ s_uGetDataDuration(
 	unsigned char byFBOption
 )
 {
-	bool bLastFrag = 0;
+	bool bLastFrag = false;
 	unsigned int uAckTime = 0, uNextPktTime = 0;
 
 	if (uFragIdx == (uMACfragNum-1))
-		bLastFrag = 1;
+		bLastFrag = true;
 
 	switch (byDurType) {
-	case DATADUR_B:    //DATADUR_B
-		if (((uMACfragNum == 1)) || bLastFrag) {//Non Frag or Last Frag
+	case DATADUR_B:    /* DATADUR_B */
+		if (((uMACfragNum == 1)) || bLastFrag) {/* Non Frag or Last Frag */
 			if (bNeedAck) {
 				uAckTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 14, pDevice->byTopCCKBasicRate);
 				return pDevice->uSIFS + uAckTime;
 			} else {
 				return 0;
 			}
-		} else {//First Frag or Mid Frag
+		} else {/* First Frag or Mid Frag */
 			if (uFragIdx == (uMACfragNum-2))
 				uNextPktTime = s_uGetTxRsvTime(pDevice, byPktType, cbLastFragmentSize, wRate, bNeedAck);
 			else
@@ -278,15 +278,15 @@ s_uGetDataDuration(
 		}
 		break;
 
-	case DATADUR_A:    //DATADUR_A
-		if (((uMACfragNum == 1)) || bLastFrag) {//Non Frag or Last Frag
+	case DATADUR_A:    /* DATADUR_A */
+		if (((uMACfragNum == 1)) || bLastFrag) {/* Non Frag or Last Frag */
 			if (bNeedAck) {
 				uAckTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 14, pDevice->byTopOFDMBasicRate);
 				return pDevice->uSIFS + uAckTime;
 			} else {
 				return 0;
 			}
-		} else {//First Frag or Mid Frag
+		} else {/* First Frag or Mid Frag */
 			if (uFragIdx == (uMACfragNum-2))
 				uNextPktTime = s_uGetTxRsvTime(pDevice, byPktType, cbLastFragmentSize, wRate, bNeedAck);
 			else
@@ -301,15 +301,15 @@ s_uGetDataDuration(
 		}
 		break;
 
-	case DATADUR_A_F0:    //DATADUR_A_F0
-		if (((uMACfragNum == 1)) || bLastFrag) {//Non Frag or Last Frag
+	case DATADUR_A_F0:    /* DATADUR_A_F0 */
+		if (((uMACfragNum == 1)) || bLastFrag) {/* Non Frag or Last Frag */
 			if (bNeedAck) {
 				uAckTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 14, pDevice->byTopOFDMBasicRate);
 				return pDevice->uSIFS + uAckTime;
 			} else {
 				return 0;
 			}
-		} else { //First Frag or Mid Frag
+		} else { /* First Frag or Mid Frag */
 			if (byFBOption == AUTO_FB_0) {
 				if (wRate < RATE_18M)
 					wRate = RATE_18M;
@@ -321,7 +321,7 @@ s_uGetDataDuration(
 				else
 					uNextPktTime = s_uGetTxRsvTime(pDevice, byPktType, cbFrameLength, wFB_Opt0[FB_RATE0][wRate-RATE_18M], bNeedAck);
 
-			} else { // (byFBOption == AUTO_FB_1)
+			} else { /* (byFBOption == AUTO_FB_1) */
 				if (wRate < RATE_18M)
 					wRate = RATE_18M;
 				else if (wRate > RATE_54M)
@@ -343,15 +343,15 @@ s_uGetDataDuration(
 		}
 		break;
 
-	case DATADUR_A_F1:    //DATADUR_A_F1
-		if (((uMACfragNum == 1)) || bLastFrag) {//Non Frag or Last Frag
+	case DATADUR_A_F1:    /* DATADUR_A_F1 */
+		if (((uMACfragNum == 1)) || bLastFrag) { /* Non Frag or Last Frag */
 			if (bNeedAck) {
 				uAckTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 14, pDevice->byTopOFDMBasicRate);
 				return pDevice->uSIFS + uAckTime;
 			} else {
 				return 0;
 			}
-		} else { //First Frag or Mid Frag
+		} else { /* First Frag or Mid Frag */
 			if (byFBOption == AUTO_FB_0) {
 				if (wRate < RATE_18M)
 					wRate = RATE_18M;
@@ -363,7 +363,7 @@ s_uGetDataDuration(
 				else
 					uNextPktTime = s_uGetTxRsvTime(pDevice, byPktType, cbFrameLength, wFB_Opt0[FB_RATE1][wRate-RATE_18M], bNeedAck);
 
-			} else { // (byFBOption == AUTO_FB_1)
+			} else { /* (byFBOption == AUTO_FB_1) */
 				if (wRate < RATE_18M)
 					wRate = RATE_18M;
 				else if (wRate > RATE_54M)
@@ -391,7 +391,7 @@ s_uGetDataDuration(
 	return 0;
 }
 
-//byFreqType: 0=>5GHZ 1=>2.4GHZ
+/* byFreqType: 0=>5GHZ 1=>2.4GHZ */
 static
 __le16
 s_uGetRTSCTSDuration(
@@ -407,26 +407,26 @@ s_uGetRTSCTSDuration(
 	unsigned int uCTSTime = 0, uDurTime = 0;
 
 	switch (byDurType) {
-	case RTSDUR_BB:    //RTSDuration_bb
+	case RTSDUR_BB:    /* RTSDuration_bb */
 		uCTSTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 14, pDevice->byTopCCKBasicRate);
 		uDurTime = uCTSTime + 2*pDevice->uSIFS + s_uGetTxRsvTime(pDevice, byPktType, cbFrameLength, wRate, bNeedAck);
 		break;
 
-	case RTSDUR_BA:    //RTSDuration_ba
+	case RTSDUR_BA:    /* RTSDuration_ba */
 		uCTSTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 14, pDevice->byTopCCKBasicRate);
 		uDurTime = uCTSTime + 2*pDevice->uSIFS + s_uGetTxRsvTime(pDevice, byPktType, cbFrameLength, wRate, bNeedAck);
 		break;
 
-	case RTSDUR_AA:    //RTSDuration_aa
+	case RTSDUR_AA:    /* RTSDuration_aa */
 		uCTSTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 14, pDevice->byTopOFDMBasicRate);
 		uDurTime = uCTSTime + 2*pDevice->uSIFS + s_uGetTxRsvTime(pDevice, byPktType, cbFrameLength, wRate, bNeedAck);
 		break;
 
-	case CTSDUR_BA:    //CTSDuration_ba
+	case CTSDUR_BA:    /* CTSDuration_ba */
 		uDurTime = pDevice->uSIFS + s_uGetTxRsvTime(pDevice, byPktType, cbFrameLength, wRate, bNeedAck);
 		break;
 
-	case RTSDUR_BA_F0: //RTSDuration_ba_f0
+	case RTSDUR_BA_F0: /* RTSDuration_ba_f0 */
 		uCTSTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 14, pDevice->byTopCCKBasicRate);
 		if ((byFBOption == AUTO_FB_0) && (wRate >= RATE_18M) && (wRate <= RATE_54M))
 			uDurTime = uCTSTime + 2 * pDevice->uSIFS + s_uGetTxRsvTime(pDevice, byPktType, cbFrameLength, wFB_Opt0[FB_RATE0][wRate-RATE_18M], bNeedAck);
@@ -435,7 +435,7 @@ s_uGetRTSCTSDuration(
 
 		break;
 
-	case RTSDUR_AA_F0: //RTSDuration_aa_f0
+	case RTSDUR_AA_F0: /* RTSDuration_aa_f0 */
 		uCTSTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 14, pDevice->byTopOFDMBasicRate);
 		if ((byFBOption == AUTO_FB_0) && (wRate >= RATE_18M) && (wRate <= RATE_54M))
 			uDurTime = uCTSTime + 2*pDevice->uSIFS + s_uGetTxRsvTime(pDevice, byPktType, cbFrameLength, wFB_Opt0[FB_RATE0][wRate-RATE_18M], bNeedAck);
@@ -444,7 +444,7 @@ s_uGetRTSCTSDuration(
 
 		break;
 
-	case RTSDUR_BA_F1: //RTSDuration_ba_f1
+	case RTSDUR_BA_F1: /* RTSDuration_ba_f1 */
 		uCTSTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 14, pDevice->byTopCCKBasicRate);
 		if ((byFBOption == AUTO_FB_0) && (wRate >= RATE_18M) && (wRate <= RATE_54M))
 			uDurTime = uCTSTime + 2*pDevice->uSIFS + s_uGetTxRsvTime(pDevice, byPktType, cbFrameLength, wFB_Opt0[FB_RATE1][wRate-RATE_18M], bNeedAck);
@@ -453,7 +453,7 @@ s_uGetRTSCTSDuration(
 
 		break;
 
-	case RTSDUR_AA_F1: //RTSDuration_aa_f1
+	case RTSDUR_AA_F1: /* RTSDuration_aa_f1 */
 		uCTSTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 14, pDevice->byTopOFDMBasicRate);
 		if ((byFBOption == AUTO_FB_0) && (wRate >= RATE_18M) && (wRate <= RATE_54M))
 			uDurTime = uCTSTime + 2*pDevice->uSIFS + s_uGetTxRsvTime(pDevice, byPktType, cbFrameLength, wFB_Opt0[FB_RATE1][wRate-RATE_18M], bNeedAck);
@@ -462,7 +462,7 @@ s_uGetRTSCTSDuration(
 
 		break;
 
-	case CTSDUR_BA_F0: //CTSDuration_ba_f0
+	case CTSDUR_BA_F0: /* CTSDuration_ba_f0 */
 		if ((byFBOption == AUTO_FB_0) && (wRate >= RATE_18M) && (wRate <= RATE_54M))
 			uDurTime = pDevice->uSIFS + s_uGetTxRsvTime(pDevice, byPktType, cbFrameLength, wFB_Opt0[FB_RATE0][wRate-RATE_18M], bNeedAck);
 		else if ((byFBOption == AUTO_FB_1) && (wRate >= RATE_18M) && (wRate <= RATE_54M))
@@ -470,7 +470,7 @@ s_uGetRTSCTSDuration(
 
 		break;
 
-	case CTSDUR_BA_F1: //CTSDuration_ba_f1
+	case CTSDUR_BA_F1: /* CTSDuration_ba_f1 */
 		if ((byFBOption == AUTO_FB_0) && (wRate >= RATE_18M) && (wRate <= RATE_54M))
 			uDurTime = pDevice->uSIFS + s_uGetTxRsvTime(pDevice, byPktType, cbFrameLength, wFB_Opt0[FB_RATE1][wRate-RATE_18M], bNeedAck);
 		else if ((byFBOption == AUTO_FB_1) && (wRate >= RATE_18M) && (wRate <= RATE_54M))
@@ -565,7 +565,7 @@ s_uFillDataHead(
 			buf->time_stamp_off_b = vnt_time_stamp_off(pDevice, pDevice->byTopCCKBasicRate);
 
 			return buf->duration_a;
-		} //if (byFBOption == AUTO_FB_NONE)
+		} /* if (byFBOption == AUTO_FB_NONE) */
 	} else if (byPktType == PK_TYPE_11A) {
 		if ((byFBOption != AUTO_FB_NONE)) {
 			/* Auto Fallback */
@@ -651,13 +651,13 @@ s_vFillRTSHead(
 		return;
 
 	if (bDisCRC) {
-		// When CRCDIS bit is on, H/W forgot to generate FCS for RTS frame,
-		// in this case we need to decrease its length by 4.
+		/* When CRCDIS bit is on, H/W forgot to generate FCS for RTS frame,
+		 in this case we need to decrease its length by 4. */
 		uRTSFrameLen -= 4;
 	}
 
-	// Note: So far RTSHead dosen't appear in ATIM & Beacom DMA, so we don't need to take them into account.
-	//       Otherwise, we need to modify codes for them.
+	/* Note: So far RTSHead dosen't appear in ATIM & Beacom DMA, so we don't need to take them into account.
+	       Otherwise, we need to modify codes for them. */
 	if (byPktType == PK_TYPE_11GB || byPktType == PK_TYPE_11GA) {
 		if (byFBOption == AUTO_FB_NONE) {
 			struct vnt_rts_g *buf = pvRTS;
@@ -748,7 +748,7 @@ s_vFillRTSHead(
 
 			ether_addr_copy(buf->data.ra, hdr->addr1);
 			ether_addr_copy(buf->data.ta, hdr->addr2);
-		} // if (byFBOption == AUTO_FB_NONE)
+		} /* if (byFBOption == AUTO_FB_NONE) */
 	} else if (byPktType == PK_TYPE_11A) {
 		if (byFBOption == AUTO_FB_NONE) {
 			struct vnt_rts_ab *buf = pvRTS;
@@ -843,14 +843,14 @@ s_vFillCTSHead(
 		return;
 
 	if (bDisCRC) {
-		// When CRCDIS bit is on, H/W forgot to generate FCS for CTS frame,
-		// in this case we need to decrease its length by 4.
+		/* When CRCDIS bit is on, H/W forgot to generate FCS for CTS frame,
+		 in this case we need to decrease its length by 4. */
 		uCTSFrameLen -= 4;
 	}
 
 	if (byPktType == PK_TYPE_11GB || byPktType == PK_TYPE_11GA) {
 		if (byFBOption != AUTO_FB_NONE && uDMAIdx != TYPE_ATIMDMA && uDMAIdx != TYPE_BEACONDMA) {
-			// Auto Fall back
+			/* Auto Fall back */
 			struct vnt_cts_fb *buf = pvCTS;
 			/* Get SignalField, ServiceField & Length */
 			vnt_get_phy_field(pDevice, uCTSFrameLen,
@@ -888,7 +888,7 @@ s_vFillCTSHead(
 
 			ether_addr_copy(buf->data.ra,
 					pDevice->abyCurrentNetAddr);
-		} else { //if (byFBOption != AUTO_FB_NONE && uDMAIdx != TYPE_ATIMDMA && uDMAIdx != TYPE_BEACONDMA)
+		} else { /* if (byFBOption != AUTO_FB_NONE && uDMAIdx != TYPE_ATIMDMA && uDMAIdx != TYPE_BEACONDMA) */
 			struct vnt_cts *buf = pvCTS;
 			/* Get SignalField, ServiceField & Length */
 			vnt_get_phy_field(pDevice, uCTSFrameLen,
@@ -937,14 +937,14 @@ s_vFillCTSHead(
  *
  * Return Value: none
  *
- -*/
-// unsigned int cbFrameSize,//Hdr+Payload+FCS
+ -
+ * unsigned int cbFrameSize, Hdr+Payload+FCS */
 static
 void
 s_vGenerateTxParameter(
 	struct vnt_private *pDevice,
 	unsigned char byPktType,
-	void *pTxBufHead,
+	struct vnt_tx_fifo_head *tx_buffer_head,
 	void *pvRrvTime,
 	void *pvRTS,
 	void *pvCTS,
@@ -955,29 +955,26 @@ s_vGenerateTxParameter(
 	unsigned short wCurrentRate
 )
 {
-	unsigned short wFifoCtl;
+	u16 fifo_ctl = le16_to_cpu(tx_buffer_head->fifo_ctl);
 	bool bDisCRC = false;
 	unsigned char byFBOption = AUTO_FB_NONE;
 
-	PSTxBufHead pFifoHead = (PSTxBufHead)pTxBufHead;
-
-	pFifoHead->wReserved = wCurrentRate;
-	wFifoCtl = pFifoHead->wFIFOCtl;
+	tx_buffer_head->current_rate = cpu_to_le16(wCurrentRate);
 
-	if (wFifoCtl & FIFOCTL_CRCDIS)
+	if (fifo_ctl & FIFOCTL_CRCDIS)
 		bDisCRC = true;
 
-	if (wFifoCtl & FIFOCTL_AUTO_FB_0)
+	if (fifo_ctl & FIFOCTL_AUTO_FB_0)
 		byFBOption = AUTO_FB_0;
-	else if (wFifoCtl & FIFOCTL_AUTO_FB_1)
+	else if (fifo_ctl & FIFOCTL_AUTO_FB_1)
 		byFBOption = AUTO_FB_1;
 
 	if (!pvRrvTime)
 		return;
 
 	if (byPktType == PK_TYPE_11GB || byPktType == PK_TYPE_11GA) {
-		if (pvRTS != NULL) { //RTS_need
-			/* Fill RsvTime */
+		if (pvRTS != NULL) { /* RTS_need
+			 Fill RsvTime */
 			struct vnt_rrv_time_rts *buf = pvRrvTime;
 
 			buf->rts_rrv_time_aa = s_uGetRTSCTSRsvTime(pDevice, 2, byPktType, cbFrameSize, wCurrentRate);
@@ -987,40 +984,40 @@ s_vGenerateTxParameter(
 			buf->rrv_time_b = vnt_rxtx_rsvtime_le16(pDevice, PK_TYPE_11B, cbFrameSize, pDevice->byTopCCKBasicRate, bNeedACK);
 
 			s_vFillRTSHead(pDevice, byPktType, pvRTS, cbFrameSize, bNeedACK, bDisCRC, psEthHeader, wCurrentRate, byFBOption);
-		} else {//RTS_needless, PCF mode
+		} else {/* RTS_needless, PCF mode */
 			struct vnt_rrv_time_cts *buf = pvRrvTime;
 
 			buf->rrv_time_a = vnt_rxtx_rsvtime_le16(pDevice, byPktType, cbFrameSize, wCurrentRate, bNeedACK);
 			buf->rrv_time_b = vnt_rxtx_rsvtime_le16(pDevice, PK_TYPE_11B, cbFrameSize, pDevice->byTopCCKBasicRate, bNeedACK);
 			buf->cts_rrv_time_ba = s_uGetRTSCTSRsvTime(pDevice, 3, byPktType, cbFrameSize, wCurrentRate);
 
-			//Fill CTS
+			/* Fill CTS */
 			s_vFillCTSHead(pDevice, uDMAIdx, byPktType, pvCTS, cbFrameSize, bNeedACK, bDisCRC, wCurrentRate, byFBOption);
 		}
 	} else if (byPktType == PK_TYPE_11A) {
-		if (pvRTS != NULL) {//RTS_need, non PCF mode
+		if (pvRTS != NULL) {/* RTS_need, non PCF mode */
 			struct vnt_rrv_time_ab *buf = pvRrvTime;
 
 			buf->rts_rrv_time = s_uGetRTSCTSRsvTime(pDevice, 2, byPktType, cbFrameSize, wCurrentRate);
 			buf->rrv_time = vnt_rxtx_rsvtime_le16(pDevice, byPktType, cbFrameSize, wCurrentRate, bNeedACK);
 
-			//Fill RTS
+			/* Fill RTS */
 			s_vFillRTSHead(pDevice, byPktType, pvRTS, cbFrameSize, bNeedACK, bDisCRC, psEthHeader, wCurrentRate, byFBOption);
-		} else if (pvRTS == NULL) {//RTS_needless, non PCF mode
+		} else if (pvRTS == NULL) {/* RTS_needless, non PCF mode */
 			struct vnt_rrv_time_ab *buf = pvRrvTime;
 
 			buf->rrv_time = vnt_rxtx_rsvtime_le16(pDevice, PK_TYPE_11A, cbFrameSize, wCurrentRate, bNeedACK);
 		}
 	} else if (byPktType == PK_TYPE_11B) {
-		if ((pvRTS != NULL)) {//RTS_need, non PCF mode
+		if ((pvRTS != NULL)) {/* RTS_need, non PCF mode */
 			struct vnt_rrv_time_ab *buf = pvRrvTime;
 
 			buf->rts_rrv_time = s_uGetRTSCTSRsvTime(pDevice, 0, byPktType, cbFrameSize, wCurrentRate);
 			buf->rrv_time = vnt_rxtx_rsvtime_le16(pDevice, PK_TYPE_11B, cbFrameSize, wCurrentRate, bNeedACK);
 
-			//Fill RTS
+			/* Fill RTS */
 			s_vFillRTSHead(pDevice, byPktType, pvRTS, cbFrameSize, bNeedACK, bDisCRC, psEthHeader, wCurrentRate, byFBOption);
-		} else { //RTS_needless, non PCF mode
+		} else { /* RTS_needless, non PCF mode */
 			struct vnt_rrv_time_ab *buf = pvRrvTime;
 
 			buf->rrv_time = vnt_rxtx_rsvtime_le16(pDevice, PK_TYPE_11B, cbFrameSize, wCurrentRate, bNeedACK);
@@ -1058,7 +1055,7 @@ s_cbFillTxBufHead(struct vnt_private *pDevice, unsigned char byPktType,
 	void *pvRTS;
 	void *pvCTS;
 	void *pvTxDataHd;
-	unsigned short wTxBufSize;   // FFinfo size
+	unsigned short wTxBufSize;   /* FFinfo size */
 	unsigned char byFBOption = AUTO_FB_NONE;
 
 	pvRrvTime = pMICHDR = pvRTS = pvCTS = pvTxDataHd = NULL;
@@ -1076,27 +1073,27 @@ s_cbFillTxBufHead(struct vnt_private *pDevice, unsigned char byPktType,
 		cbFrameSize += info->control.hw_key->icv_len;
 
 		if (pDevice->byLocalID > REV_ID_VT3253_A1) {
-			//MAC Header should be padding 0 to DW alignment.
+			/* MAC Header should be padding 0 to DW alignment. */
 			uPadding = 4 - (ieee80211_get_hdrlen_from_skb(skb) % 4);
 			uPadding %= 4;
 		}
 	}
 
-	//
-	// Use for AUTO FALL BACK
-	//
+	/*
+	* Use for AUTO FALL BACK
+	*/
 	if (fifo_ctl & FIFOCTL_AUTO_FB_0)
 		byFBOption = AUTO_FB_0;
 	else if (fifo_ctl & FIFOCTL_AUTO_FB_1)
 		byFBOption = AUTO_FB_1;
 
-	//////////////////////////////////////////////////////
-	//Set RrvTime/RTS/CTS Buffer
+
+	/* Set RrvTime/RTS/CTS Buffer */
 	wTxBufSize = sizeof(STxBufHead);
-	if (byPktType == PK_TYPE_11GB || byPktType == PK_TYPE_11GA) {//802.11g packet
+	if (byPktType == PK_TYPE_11GB || byPktType == PK_TYPE_11GA) {/* 802.11g packet */
 
 		if (byFBOption == AUTO_FB_NONE) {
-			if (bRTS == true) {//RTS_need
+			if (bRTS == true) {/* RTS_need */
 				pvRrvTime = (void *)(pbyTxBufferAddr + wTxBufSize);
 				pMICHDR = (struct vnt_mic_hdr *)(pbyTxBufferAddr + wTxBufSize + sizeof(struct vnt_rrv_time_rts));
 				pvRTS = (void *)(pbyTxBufferAddr + wTxBufSize + sizeof(struct vnt_rrv_time_rts) + cbMICHDR);
@@ -1106,7 +1103,7 @@ s_cbFillTxBufHead(struct vnt_private *pDevice, unsigned char byPktType,
 				cbHeaderLength = wTxBufSize + sizeof(struct vnt_rrv_time_rts) +
 							cbMICHDR + sizeof(struct vnt_rts_g) +
 							sizeof(struct vnt_tx_datahead_g);
-			} else { //RTS_needless
+			} else { /* RTS_needless */
 				pvRrvTime = (void *)(pbyTxBufferAddr + wTxBufSize);
 				pMICHDR = (struct vnt_mic_hdr *) (pbyTxBufferAddr + wTxBufSize + sizeof(struct vnt_rrv_time_cts));
 				pvRTS = NULL;
@@ -1117,8 +1114,8 @@ s_cbFillTxBufHead(struct vnt_private *pDevice, unsigned char byPktType,
 							cbMICHDR + sizeof(struct vnt_cts) + sizeof(struct vnt_tx_datahead_g);
 			}
 		} else {
-			// Auto Fall Back
-			if (bRTS == true) {//RTS_need
+			/* Auto Fall Back */
+			if (bRTS == true) {/* RTS_need */
 				pvRrvTime = (void *)(pbyTxBufferAddr + wTxBufSize);
 				pMICHDR = (struct vnt_mic_hdr *) (pbyTxBufferAddr + wTxBufSize + sizeof(struct vnt_rrv_time_rts));
 				pvRTS = (void *) (pbyTxBufferAddr + wTxBufSize + sizeof(struct vnt_rrv_time_rts) + cbMICHDR);
@@ -1127,7 +1124,7 @@ s_cbFillTxBufHead(struct vnt_private *pDevice, unsigned char byPktType,
 					cbMICHDR + sizeof(struct vnt_rts_g_fb));
 				cbHeaderLength = wTxBufSize + sizeof(struct vnt_rrv_time_rts) +
 					cbMICHDR + sizeof(struct vnt_rts_g_fb) + sizeof(struct vnt_tx_datahead_g_fb);
-			} else { //RTS_needless
+			} else { /* RTS_needless */
 				pvRrvTime = (void *)(pbyTxBufferAddr + wTxBufSize);
 				pMICHDR = (struct vnt_mic_hdr *) (pbyTxBufferAddr + wTxBufSize + sizeof(struct vnt_rrv_time_cts));
 				pvRTS = NULL;
@@ -1137,8 +1134,8 @@ s_cbFillTxBufHead(struct vnt_private *pDevice, unsigned char byPktType,
 				cbHeaderLength = wTxBufSize + sizeof(struct vnt_rrv_time_cts) +
 					cbMICHDR + sizeof(struct vnt_cts_fb) + sizeof(struct vnt_tx_datahead_g_fb);
 			}
-		} // Auto Fall Back
-	} else {//802.11a/b packet
+		} /* Auto Fall Back */
+	} else {/* 802.11a/b packet */
 
 		if (byFBOption == AUTO_FB_NONE) {
 			if (bRTS == true) {
@@ -1150,7 +1147,7 @@ s_cbFillTxBufHead(struct vnt_private *pDevice, unsigned char byPktType,
 					sizeof(struct vnt_rrv_time_ab) + cbMICHDR + sizeof(struct vnt_rts_ab));
 				cbHeaderLength = wTxBufSize + sizeof(struct vnt_rrv_time_ab) +
 					cbMICHDR + sizeof(struct vnt_rts_ab) + sizeof(struct vnt_tx_datahead_ab);
-			} else { //RTS_needless, need MICHDR
+			} else { /* RTS_needless, need MICHDR */
 				pvRrvTime = (void *)(pbyTxBufferAddr + wTxBufSize);
 				pMICHDR = (struct vnt_mic_hdr *) (pbyTxBufferAddr + wTxBufSize + sizeof(struct vnt_rrv_time_ab));
 				pvRTS = NULL;
@@ -1160,8 +1157,8 @@ s_cbFillTxBufHead(struct vnt_private *pDevice, unsigned char byPktType,
 					cbMICHDR + sizeof(struct vnt_tx_datahead_ab);
 			}
 		} else {
-			// Auto Fall Back
-			if (bRTS == true) {//RTS_need
+			/* Auto Fall Back */
+			if (bRTS == true) { /* RTS_need */
 				pvRrvTime = (void *)(pbyTxBufferAddr + wTxBufSize);
 				pMICHDR = (struct vnt_mic_hdr *) (pbyTxBufferAddr + wTxBufSize + sizeof(struct vnt_rrv_time_ab));
 				pvRTS = (void *)(pbyTxBufferAddr + wTxBufSize + sizeof(struct vnt_rrv_time_ab) + cbMICHDR);
@@ -1170,7 +1167,7 @@ s_cbFillTxBufHead(struct vnt_private *pDevice, unsigned char byPktType,
 					sizeof(struct vnt_rrv_time_ab) + cbMICHDR + sizeof(struct vnt_rts_a_fb));
 				cbHeaderLength = wTxBufSize + sizeof(struct vnt_rrv_time_ab) +
 					cbMICHDR + sizeof(struct vnt_rts_a_fb) + sizeof(struct vnt_tx_datahead_a_fb);
-			} else { //RTS_needless
+			} else { /* RTS_needless */
 				pvRrvTime = (void *)(pbyTxBufferAddr + wTxBufSize);
 				pMICHDR = (struct vnt_mic_hdr *)(pbyTxBufferAddr + wTxBufSize + sizeof(struct vnt_rrv_time_ab));
 				pvRTS = NULL;
@@ -1179,7 +1176,7 @@ s_cbFillTxBufHead(struct vnt_private *pDevice, unsigned char byPktType,
 				cbHeaderLength = wTxBufSize + sizeof(struct vnt_rrv_time_ab) +
 					cbMICHDR + sizeof(struct vnt_tx_datahead_a_fb);
 			}
-		} // Auto Fall Back
+		} /* Auto Fall Back */
 	}
 
 	td_info->mic_hdr = pMICHDR;
@@ -1308,10 +1305,18 @@ int vnt_generate_fifo_header(struct vnt_private *priv, u32 dma_idx,
 			    priv->hw->conf.chandef.chan->hw_value);
 	}
 
-	if (current_rate > RATE_11M)
-		pkt_type = (u8)priv->byPacketType;
-	else
+	if (current_rate > RATE_11M) {
+		if (info->band == IEEE80211_BAND_5GHZ) {
+			pkt_type = PK_TYPE_11A;
+		} else {
+			if (tx_rate->flags & IEEE80211_TX_RC_USE_CTS_PROTECT)
+				pkt_type = PK_TYPE_11GB;
+			else
+				pkt_type = PK_TYPE_11GA;
+		}
+	} else {
 		pkt_type = PK_TYPE_11B;
+	}
 
 	/*Set fifo controls */
 	if (pkt_type == PK_TYPE_11A)
@@ -1505,8 +1510,6 @@ int vnt_beacon_make(struct vnt_private *priv, struct ieee80211_vif *vif)
 int vnt_beacon_enable(struct vnt_private *priv, struct ieee80211_vif *vif,
 		      struct ieee80211_bss_conf *conf)
 {
-	int ret;
-
 	VNSvOutPortB(priv->PortOffset + MAC_REG_TFTCTL, TFTCTL_TSFCNTRST);
 
 	VNSvOutPortB(priv->PortOffset + MAC_REG_TFTCTL, TFTCTL_TSFCNTREN);
@@ -1515,7 +1518,5 @@ int vnt_beacon_enable(struct vnt_private *priv, struct ieee80211_vif *vif,
 
 	CARDbSetBeaconPeriod(priv, conf->beacon_int);
 
-	ret = vnt_beacon_make(priv, vif);
-
-	return ret;
+	return vnt_beacon_make(priv, vif);
 }
diff --git a/drivers/staging/vt6655/srom.h b/drivers/staging/vt6655/srom.h
index 7d3e3ef9f17f..531bf0069373 100644
--- a/drivers/staging/vt6655/srom.h
+++ b/drivers/staging/vt6655/srom.h
@@ -24,7 +24,6 @@
  * Author: Jerry Chen
  *
  * Date: Jan 29, 2003
- *
  */
 
 #ifndef __SROM_H__
@@ -34,56 +33,50 @@
 
 #define EEP_MAX_CONTEXT_SIZE    256
 
-#define CB_EEPROM_READBYTE_WAIT 900     //us
+#define CB_EEPROM_READBYTE_WAIT 900     /* us */
 
 #define W_MAX_I2CRETRY          0x0fff
 
-//
-// Contents in the EEPROM
-//
-#define EEP_OFS_PAR         0x00        // physical address
+/* Contents in the EEPROM */
+#define EEP_OFS_PAR         0x00        /* physical address */
 #define EEP_OFS_ANTENNA     0x16
 #define EEP_OFS_RADIOCTL    0x17
-#define EEP_OFS_RFTYPE      0x1B        // for select RF
-#define EEP_OFS_MINCHANNEL  0x1C        // Min Channel #
-#define EEP_OFS_MAXCHANNEL  0x1D        // Max Channel #
-#define EEP_OFS_SIGNATURE   0x1E        //
-#define EEP_OFS_ZONETYPE    0x1F        //
-#define EEP_OFS_RFTABLE     0x20        // RF POWER TABLE
+#define EEP_OFS_RFTYPE      0x1B        /* for select RF */
+#define EEP_OFS_MINCHANNEL  0x1C        /* Min Channel # */
+#define EEP_OFS_MAXCHANNEL  0x1D        /* Max Channel # */
+#define EEP_OFS_SIGNATURE   0x1E
+#define EEP_OFS_ZONETYPE    0x1F
+#define EEP_OFS_RFTABLE     0x20        /* RF POWER TABLE */
 #define EEP_OFS_PWR_CCK     0x20
 #define EEP_OFS_SETPT_CCK   0x21
 #define EEP_OFS_PWR_OFDMG   0x23
 #define EEP_OFS_SETPT_OFDMG 0x24
-#define EEP_OFS_PWR_FORMULA_OST  0x26   //
+#define EEP_OFS_PWR_FORMULA_OST  0x26
 #define EEP_OFS_MAJOR_VER 0x2E
 #define EEP_OFS_MINOR_VER 0x2F
 #define EEP_OFS_CCK_PWR_TBL     0x30
 #define EEP_OFS_CCK_PWR_dBm     0x3F
 #define EEP_OFS_OFDM_PWR_TBL    0x40
 #define EEP_OFS_OFDM_PWR_dBm    0x4F
-//{{ RobertYu: 20041124
+/*{{ RobertYu: 20041124 */
 #define EEP_OFS_SETPT_OFDMA         0x4E
 #define EEP_OFS_OFDMA_PWR_TBL       0x50
-//}}
+/*}}*/
 #define EEP_OFS_OFDMA_PWR_dBm       0xD2
 
-//----------need to remove --------------------
-#define EEP_OFS_BBTAB_LEN   0x70        // BB Table Length
-#define EEP_OFS_BBTAB_ADR   0x71        // BB Table Offset
-#define EEP_OFS_CHECKSUM    0xFF        // reserved area for baseband 28h ~ 78h
+/*----------need to remove --------------------*/
+#define EEP_OFS_BBTAB_LEN   0x70        /* BB Table Length */
+#define EEP_OFS_BBTAB_ADR   0x71        /* BB Table Offset */
+#define EEP_OFS_CHECKSUM    0xFF        /* reserved area for baseband 28h~78h */
 
-#define EEP_I2C_DEV_ID      0x50        // EEPROM device address on the I2C bus
+#define EEP_I2C_DEV_ID      0x50        /* EEPROM device address on I2C bus */
 
-//
-// Bits in EEP_OFS_ANTENNA
-//
+/* Bits in EEP_OFS_ANTENNA */
 #define EEP_ANTENNA_MAIN    0x01
 #define EEP_ANTENNA_AUX     0x02
 #define EEP_ANTINV          0x04
 
-//
-// Bits in EEP_OFS_RADIOCTL
-//
+/* Bits in EEP_OFS_RADIOCTL */
 #define EEP_RADIOCTL_ENABLE 0x80
 #define EEP_RADIOCTL_INV    0x01
 
@@ -97,10 +90,12 @@
 
 /*---------------------  Export Functions  --------------------------*/
 
-unsigned char SROMbyReadEmbedded(void __iomem *dwIoBase, unsigned char byContntOffset);
+unsigned char SROMbyReadEmbedded(void __iomem *dwIoBase,
+				 unsigned char byContntOffset);
 
 void SROMvReadAllContents(void __iomem *dwIoBase, unsigned char *pbyEepromRegs);
 
-void SROMvReadEtherAddress(void __iomem *dwIoBase, unsigned char *pbyEtherAddress);
+void SROMvReadEtherAddress(void __iomem *dwIoBase,
+			   unsigned char *pbyEtherAddress);
 
-#endif // __EEPROM_H__
+#endif /* __EEPROM_H__*/
diff --git a/drivers/staging/vt6655/tmacro.h b/drivers/staging/vt6655/tmacro.h
index 607b78f7a6a0..597efefc017f 100644
--- a/drivers/staging/vt6655/tmacro.h
+++ b/drivers/staging/vt6655/tmacro.h
@@ -55,4 +55,4 @@
 #define MAKEDWORD(lw, hw)   ((unsigned long)(((unsigned short)(lw)) | (((unsigned long)((unsigned short)(hw))) << 16)))
 #endif
 
-#endif // __TMACRO_H__
+#endif /* __TMACRO_H__ */
diff --git a/drivers/staging/vt6656/firmware.c b/drivers/staging/vt6656/firmware.c
index a177645af83e..d440f284bf18 100644
--- a/drivers/staging/vt6656/firmware.c
+++ b/drivers/staging/vt6656/firmware.c
@@ -61,7 +61,7 @@ int vnt_download_firmware(struct vnt_private *priv)
 
 	buffer = kmalloc(FIRMWARE_CHUNK_SIZE, GFP_KERNEL);
 	if (!buffer)
-		goto out;
+		goto free_fw;
 
 	for (ii = 0; ii < fw->size; ii += FIRMWARE_CHUNK_SIZE) {
 		length = min_t(int, fw->size - ii, FIRMWARE_CHUNK_SIZE);
diff --git a/drivers/staging/vt6656/mac.c b/drivers/staging/vt6656/mac.c
index bb37e33b9ffa..5dfac05b9cf1 100644
--- a/drivers/staging/vt6656/mac.c
+++ b/drivers/staging/vt6656/mac.c
@@ -30,6 +30,8 @@
  * Revision History:
  */
 
+#include <linux/etherdevice.h>
+
 #include "desc.h"
 #include "mac.h"
 #include "usbpipe.h"
@@ -126,7 +128,7 @@ void vnt_mac_set_keyentry(struct vnt_private *priv, u16 key_ctl, u32 entry_idx,
 	offset += (entry_idx * MISCFIFO_KEYENTRYSIZE);
 
 	set_key.u.write.key_ctl = cpu_to_le16(key_ctl);
-	memcpy(set_key.u.write.addr, addr, ETH_ALEN);
+	ether_addr_copy(set_key.u.write.addr, addr);
 
 	/* swap over swap[0] and swap[1] to get correct write order */
 	swap(set_key.u.swap[0], set_key.u.swap[1]);
diff --git a/drivers/staging/vt6656/main_usb.c b/drivers/staging/vt6656/main_usb.c
index 71adc1f61838..ab3ab84cb0a7 100644
--- a/drivers/staging/vt6656/main_usb.c
+++ b/drivers/staging/vt6656/main_usb.c
@@ -963,6 +963,7 @@ vt6656_probe(struct usb_interface *intf, const struct usb_device_id *id)
 	hw = ieee80211_alloc_hw(sizeof(struct vnt_private), &vnt_mac_ops);
 	if (!hw) {
 		dev_err(&udev->dev, "could not register ieee80211_hw\n");
+		rc = -ENOMEM;
 		goto err_nomem;
 	}
 
diff --git a/drivers/staging/vt6656/rf.c b/drivers/staging/vt6656/rf.c
index c42cde59f598..c4286ccac320 100644
--- a/drivers/staging/vt6656/rf.c
+++ b/drivers/staging/vt6656/rf.c
@@ -640,6 +640,7 @@ int vnt_rf_setpower(struct vnt_private *priv, u32 rate, u32 channel)
 		break;
 	case RATE_6M:
 	case RATE_9M:
+	case RATE_12M:
 	case RATE_18M:
 	case RATE_24M:
 	case RATE_36M:
diff --git a/drivers/staging/vt6656/rxtx.c b/drivers/staging/vt6656/rxtx.c
index 33baf26de4b5..f6c2cf8590c4 100644
--- a/drivers/staging/vt6656/rxtx.c
+++ b/drivers/staging/vt6656/rxtx.c
@@ -755,9 +755,9 @@ static void vnt_fill_txkey(struct vnt_usb_send_context *tx_context,
 		else
 			mic_hdr->hlen = cpu_to_be16(22);
 
-		memcpy(mic_hdr->addr1, hdr->addr1, ETH_ALEN);
-		memcpy(mic_hdr->addr2, hdr->addr2, ETH_ALEN);
-		memcpy(mic_hdr->addr3, hdr->addr3, ETH_ALEN);
+		ether_addr_copy(mic_hdr->addr1, hdr->addr1);
+		ether_addr_copy(mic_hdr->addr2, hdr->addr2);
+		ether_addr_copy(mic_hdr->addr3, hdr->addr3);
 
 		mic_hdr->frame_control = cpu_to_le16(
 			le16_to_cpu(hdr->frame_control) & 0xc78f);
@@ -765,7 +765,7 @@ static void vnt_fill_txkey(struct vnt_usb_send_context *tx_context,
 				le16_to_cpu(hdr->seq_ctrl) & 0xf);
 
 		if (ieee80211_has_a4(hdr->frame_control))
-			memcpy(mic_hdr->addr4, hdr->addr4, ETH_ALEN);
+			ether_addr_copy(mic_hdr->addr4, hdr->addr4);
 
 
 		memcpy(key_buffer, tx_key->key, WLAN_KEY_LEN_CCMP);
diff --git a/drivers/staging/wlan-ng/hfa384x.h b/drivers/staging/wlan-ng/hfa384x.h
index 8f2091070491..8dfe4381ddf7 100644
--- a/drivers/staging/wlan-ng/hfa384x.h
+++ b/drivers/staging/wlan-ng/hfa384x.h
@@ -1204,19 +1204,19 @@ typedef struct hfa484x_metacmd {
 #define WLAN_ACCESS_DENY	3   /* Do not authenticate "denied" stations. */
 
 /* XXX These are going away ASAP */
-typedef struct prism2sta_authlist {
+struct prism2sta_authlist {
 	unsigned int cnt;
 	u8 addr[WLAN_AUTH_MAX][ETH_ALEN];
 	u8 assoc[WLAN_AUTH_MAX];
-} prism2sta_authlist_t;
+};
 
-typedef struct prism2sta_accesslist {
+struct prism2sta_accesslist {
 	unsigned int modify;
 	unsigned int cnt;
 	u8 addr[WLAN_ACCESS_MAX][ETH_ALEN];
 	unsigned int cnt1;
 	u8 addr1[WLAN_ACCESS_MAX][ETH_ALEN];
-} prism2sta_accesslist_t;
+};
 
 typedef struct hfa384x {
 	/* USB support data */
@@ -1348,10 +1348,10 @@ typedef struct hfa384x {
 
 	hfa384x_InfFrame_t *scanresults;
 
-	prism2sta_authlist_t authlist;	/* Authenticated station list. */
-	unsigned int accessmode;	/* Access mode. */
-	prism2sta_accesslist_t allow;	/* Allowed station list. */
-	prism2sta_accesslist_t deny;	/* Denied station list. */
+	struct prism2sta_authlist authlist;	/* Authenticated station list. */
+	unsigned int accessmode;		/* Access mode. */
+	struct prism2sta_accesslist allow;	/* Allowed station list. */
+	struct prism2sta_accesslist deny;	/* Denied station list. */
 
 } hfa384x_t;
 
@@ -1413,7 +1413,8 @@ int hfa384x_drvr_start(hfa384x_t *hw);
 int hfa384x_drvr_stop(hfa384x_t *hw);
 int
 hfa384x_drvr_txframe(hfa384x_t *hw, struct sk_buff *skb,
-		     union p80211_hdr *p80211_hdr, struct p80211_metawep *p80211_wep);
+		     union p80211_hdr *p80211_hdr,
+		     struct p80211_metawep *p80211_wep);
 void hfa384x_tx_timeout(wlandevice_t *wlandev);
 
 int hfa384x_cmd_initialize(hfa384x_t *hw);
diff --git a/drivers/staging/wlan-ng/hfa384x_usb.c b/drivers/staging/wlan-ng/hfa384x_usb.c
index 28cd1c4c02c8..e109a7fd422f 100644
--- a/drivers/staging/wlan-ng/hfa384x_usb.c
+++ b/drivers/staging/wlan-ng/hfa384x_usb.c
@@ -557,17 +557,13 @@ void hfa384x_create(hfa384x_t *hw, struct usb_device *usb)
 	INIT_WORK(&hw->link_bh, prism2sta_processing_defer);
 	INIT_WORK(&hw->usb_work, hfa384x_usb_defer);
 
-	init_timer(&hw->throttle);
-	hw->throttle.function = hfa384x_usb_throttlefn;
-	hw->throttle.data = (unsigned long)hw;
+	setup_timer(&hw->throttle, hfa384x_usb_throttlefn, (unsigned long)hw);
 
-	init_timer(&hw->resptimer);
-	hw->resptimer.function = hfa384x_usbctlx_resptimerfn;
-	hw->resptimer.data = (unsigned long)hw;
+	setup_timer(&hw->resptimer, hfa384x_usbctlx_resptimerfn,
+		    (unsigned long)hw);
 
-	init_timer(&hw->reqtimer);
-	hw->reqtimer.function = hfa384x_usbctlx_reqtimerfn;
-	hw->reqtimer.data = (unsigned long)hw;
+	setup_timer(&hw->reqtimer, hfa384x_usbctlx_reqtimerfn,
+		    (unsigned long)hw);
 
 	usb_init_urb(&hw->rx_urb);
 	usb_init_urb(&hw->tx_urb);
@@ -577,9 +573,8 @@ void hfa384x_create(hfa384x_t *hw, struct usb_device *usb)
 	hw->state = HFA384x_STATE_INIT;
 
 	INIT_WORK(&hw->commsqual_bh, prism2sta_commsqual_defer);
-	init_timer(&hw->commsqual_timer);
-	hw->commsqual_timer.data = (unsigned long)hw;
-	hw->commsqual_timer.function = prism2sta_commsqual_timer;
+	setup_timer(&hw->commsqual_timer, prism2sta_commsqual_timer,
+		    (unsigned long)hw);
 }
 
 /*----------------------------------------------------------------
@@ -624,11 +619,10 @@ static hfa384x_usbctlx_t *usbctlx_alloc(void)
 {
 	hfa384x_usbctlx_t *ctlx;
 
-	ctlx = kmalloc(sizeof(*ctlx), in_interrupt() ? GFP_ATOMIC : GFP_KERNEL);
-	if (ctlx != NULL) {
-		memset(ctlx, 0, sizeof(*ctlx));
+	ctlx = kzalloc(sizeof(*ctlx),
+		       in_interrupt() ? GFP_ATOMIC : GFP_KERNEL);
+	if (ctlx != NULL)
 		init_completion(&ctlx->done);
-	}
 
 	return ctlx;
 }
diff --git a/drivers/staging/wlan-ng/prism2mib.c b/drivers/staging/wlan-ng/prism2mib.c
index 0163e062b650..b4a15ef3a405 100644
--- a/drivers/staging/wlan-ng/prism2mib.c
+++ b/drivers/staging/wlan-ng/prism2mib.c
@@ -582,8 +582,6 @@ static int prism2mib_privacyinvoked(struct mibrec *mib,
 				    struct p80211msg_dot11req_mibset *msg,
 				    void *data)
 {
-	int result;
-
 	if (wlandev->hostwep & HOSTWEP_DECRYPT) {
 		if (wlandev->hostwep & HOSTWEP_DECRYPT)
 			mib->parm2 |= HFA384x_WEPFLAGS_DISABLE_RXCRYPT;
@@ -591,9 +589,7 @@ static int prism2mib_privacyinvoked(struct mibrec *mib,
 			mib->parm2 |= HFA384x_WEPFLAGS_DISABLE_TXCRYPT;
 	}
 
-	result = prism2mib_flag(mib, isget, wlandev, hw, msg, data);
-
-	return result;
+	return prism2mib_flag(mib, isget, wlandev, hw, msg, data);
 }
 
 /*----------------------------------------------------------------
@@ -628,11 +624,8 @@ static int prism2mib_excludeunencrypted(struct mibrec *mib,
 					struct p80211msg_dot11req_mibset *msg,
 					void *data)
 {
-	int result;
-
-	result = prism2mib_flag(mib, isget, wlandev, hw, msg, data);
 
-	return result;
+	return prism2mib_flag(mib, isget, wlandev, hw, msg, data);
 }
 
 /*----------------------------------------------------------------
diff --git a/drivers/staging/wlan-ng/prism2sta.c b/drivers/staging/wlan-ng/prism2sta.c
index 10ad24a89ddd..ddb294e7044f 100644
--- a/drivers/staging/wlan-ng/prism2sta.c
+++ b/drivers/staging/wlan-ng/prism2sta.c
@@ -51,7 +51,6 @@
 */
 
 #include <linux/module.h>
-#include <linux/moduleparam.h>
 #include <linux/kernel.h>
 #include <linux/sched.h>
 #include <linux/types.h>
@@ -60,6 +59,7 @@
 #include <linux/netdevice.h>
 #include <linux/workqueue.h>
 #include <linux/byteorder/generic.h>
+#include <linux/etherdevice.h>
 
 #include <linux/io.h>
 #include <linux/delay.h>
@@ -243,7 +243,6 @@ static int prism2sta_txframe(wlandevice_t *wlandev, struct sk_buff *skb,
 			     struct p80211_metawep *p80211_wep)
 {
 	hfa384x_t *hw = (hfa384x_t *) wlandev->priv;
-	int result;
 
 	/* If necessary, set the 802.11 WEP bit */
 	if ((wlandev->hostwep & (HOSTWEP_PRIVACYINVOKED | HOSTWEP_ENCRYPT)) ==
@@ -251,9 +250,7 @@ static int prism2sta_txframe(wlandevice_t *wlandev, struct sk_buff *skb,
 		p80211_hdr->a3.fc |= cpu_to_le16(WLAN_SET_FC_ISWEP(1));
 	}
 
-	result = hfa384x_drvr_txframe(hw, skb, p80211_hdr, p80211_wep);
-
-	return result;
+	return hfa384x_drvr_txframe(hw, skb, p80211_hdr, p80211_wep);
 }
 
 /*----------------------------------------------------------------
@@ -1548,7 +1545,7 @@ static void prism2sta_inf_authreq_defer(wlandevice_t *wlandev,
 	 ** authentication.
 	 */
 
-	memcpy(rec.address, inf->info.authreq.sta_addr, ETH_ALEN);
+	ether_addr_copy(rec.address, inf->info.authreq.sta_addr);
 	rec.status = P80211ENUM_status_unspec_failure;
 
 	/*
@@ -1661,8 +1658,8 @@ static void prism2sta_inf_authreq_defer(wlandevice_t *wlandev,
 			if (hw->authlist.cnt >= WLAN_AUTH_MAX) {
 				rec.status = P80211ENUM_status_ap_full;
 			} else {
-				memcpy(hw->authlist.addr[hw->authlist.cnt],
-				       rec.address, ETH_ALEN);
+				ether_addr_copy(hw->authlist.addr[hw->authlist.cnt],
+						rec.address);
 				hw->authlist.cnt++;
 				added = 1;
 			}
diff --git a/drivers/staging/xgifb/XGI_main_26.c b/drivers/staging/xgifb/XGI_main_26.c
index 935c714f592a..74e88200726c 100644
--- a/drivers/staging/xgifb/XGI_main_26.c
+++ b/drivers/staging/xgifb/XGI_main_26.c
@@ -106,7 +106,7 @@ static int XGIfb_mode_rate_to_ddata(struct vb_device_info *XGI_Pr,
 
 	sr_data = XGI_CRT1Table[index].CR[5];
 
-	HDE = (XGI330_RefIndex[RefreshRateTableIndex].XRes >> 3);
+	HDE = XGI330_RefIndex[RefreshRateTableIndex].XRes >> 3;
 
 	cr_data = XGI_CRT1Table[index].CR[3];
 
@@ -1011,8 +1011,8 @@ static int XGIfb_do_set_var(struct fb_var_screeninfo *var, int isactive,
 			       XGIbios_mode[xgifb_info->mode_idx].mode_no);
 			return -EINVAL;
 		}
-		info->fix.line_length = ((info->var.xres_virtual
-				* info->var.bits_per_pixel) >> 6);
+		info->fix.line_length = (info->var.xres_virtual
+				* info->var.bits_per_pixel) >> 6;
 
 		xgifb_reg_set(XGISR, IND_SIS_PASSWORD, SIS_PASSWORD);
 
diff --git a/drivers/staging/xgifb/vb_setmode.c b/drivers/staging/xgifb/vb_setmode.c
index 1f6f699e238c..a47395e92d20 100644
--- a/drivers/staging/xgifb/vb_setmode.c
+++ b/drivers/staging/xgifb/vb_setmode.c
@@ -308,11 +308,11 @@ static void XGI_SetCRT1Timing_H(struct vb_device_info *pVBInfo,
 		data |= data1;
 		xgifb_reg_set(pVBInfo->P3d4, 0x05, data);
 		data = xgifb_reg_get(pVBInfo->P3c4, 0x0e);
-		data = data >> 5;
+		data >>= 5;
 		data = data + 3;
 		if (data > 7)
 			data = data - 7;
-		data = data << 5;
+		data <<= 5;
 		xgifb_reg_and_or(pVBInfo->P3c4, 0x0e, ~0xE0, data);
 	}
 }
@@ -347,7 +347,7 @@ static void XGI_SetCRT1Timing_V(unsigned short ModeIdIndex,
 
 	data = pVBInfo->TimingV.data[6];
 	data &= 0x80;
-	data = data >> 2;
+	data >>= 2;
 
 	i = XGI330_EModeIDTable[ModeIdIndex].Ext_ModeFlag;
 	i &= DoubleScanMode;
@@ -693,18 +693,18 @@ static void XGI_SetCRT1DE(unsigned short ModeIdIndex,
 	tempbx = XGI330_ModeResInfo[resindex].VTotal;
 
 	if (modeflag & HalfDCLK)
-		tempax = tempax >> 1;
+		tempax >>= 1;
 
 	if (modeflag & HalfDCLK)
-		tempax = tempax << 1;
+		tempax <<= 1;
 
 	temp = XGI330_RefIndex[RefreshRateTableIndex].Ext_InfoFlag;
 
 	if (temp & InterlaceMode)
-		tempbx = tempbx >> 1;
+		tempbx >>= 1;
 
 	if (modeflag & DoubleScanMode)
-		tempbx = tempbx << 1;
+		tempbx <<= 1;
 
 	tempcx = 8;
 
@@ -721,7 +721,7 @@ static void XGI_SetCRT1DE(unsigned short ModeIdIndex,
 			(unsigned short) ((tempcx & 0x0ff00) >> 10));
 	xgifb_reg_set(pVBInfo->P3d4, 0x12, (unsigned short) (tempbx & 0xff));
 	tempax = 0;
-	tempbx = tempbx >> 8;
+	tempbx >>= 8;
 
 	if (tempbx & 0x01)
 		tempax |= 0x02;
@@ -750,14 +750,14 @@ static void XGI_SetCRT1Offset(unsigned short ModeNo,
 
 	/* GetOffset */
 	temp = XGI330_EModeIDTable[ModeIdIndex].Ext_ModeInfo;
-	temp = temp >> 8;
+	temp >>= 8;
 	temp = XGI330_ScreenOffset[temp];
 
 	temp2 = XGI330_RefIndex[RefreshRateTableIndex].Ext_InfoFlag;
 	temp2 &= InterlaceMode;
 
 	if (temp2)
-		temp = temp << 1;
+		temp <<= 1;
 
 	temp2 = pVBInfo->ModeType - ModeEGA;
 
@@ -792,7 +792,7 @@ static void XGI_SetCRT1Offset(unsigned short ModeNo,
 	/* SetOffset */
 	DisplayUnit = temp;
 	temp2 = temp;
-	temp = temp >> 8; /* ah */
+	temp >>= 8; /* ah */
 	temp &= 0x0F;
 	i = xgifb_reg_get(pVBInfo->P3c4, 0x0E);
 	i &= 0xF0;
@@ -809,7 +809,7 @@ static void XGI_SetCRT1Offset(unsigned short ModeNo,
 	if (temp2)
 		DisplayUnit >>= 1;
 
-	DisplayUnit = DisplayUnit << 5;
+	DisplayUnit <<= 5;
 	ah = (DisplayUnit & 0xff00) >> 8;
 	al = DisplayUnit & 0x00ff;
 	if (al == 0)
@@ -912,7 +912,7 @@ static void XGI_SetCRT1VCLK(unsigned short ModeIdIndex,
 			index = data;
 			index &= 0xE0;
 			data &= 0x1F;
-			data = data << 1;
+			data <<= 1;
 			data += 1;
 			data |= index;
 			xgifb_reg_set(pVBInfo->P3c4, 0x2C, data);
@@ -1011,7 +1011,7 @@ static void XGI_SetCRT1ModeRegs(struct xgi_hw_device_info *HwDeviceExtension,
 	data2 = 0;
 	data2 |= 0x02;
 	data3 = pVBInfo->ModeType - ModeVGA;
-	data3 = data3 << 2;
+	data3 <<= 2;
 	data2 |= data3;
 	data &= InterlaceMode;
 
@@ -1126,7 +1126,7 @@ static void XGI_LoadDAC(struct vb_device_info *pVBInfo)
 				data2 += 0x15;
 
 			outb(data2, pVBInfo->P3c9);
-			data = data >> 2;
+			data >>= 2;
 		}
 	}
 
@@ -1185,10 +1185,10 @@ static void XGI_GetLVDSResInfo(unsigned short ModeIdIndex,
 	yres = XGI330_ModeResInfo[resindex].VTotal;
 
 	if (modeflag & HalfDCLK)
-		xres = xres << 1;
+		xres <<= 1;
 
 	if (modeflag & DoubleScanMode)
-		yres = yres << 1;
+		yres <<= 1;
 
 	if (xres == 720)
 		xres = 640;
@@ -1450,8 +1450,8 @@ static void XGI_SetLVDSRegs(unsigned short ModeIdIndex,
 
 	xgifb_reg_set(pVBInfo->Part1Port, 0x1A, tempbx & 0x07);
 
-	tempcx = tempcx >> 3;
-	tempbx = tempbx >> 3;
+	tempcx >>= 3;
+	tempbx >>= 3;
 
 	xgifb_reg_set(pVBInfo->Part1Port, 0x16,
 			(unsigned short) (tempbx & 0xff));
@@ -1473,9 +1473,9 @@ static void XGI_SetLVDSRegs(unsigned short ModeIdIndex,
 		tempcx -= tempax;
 
 	tempax = tempbx & 0x07;
-	tempax = tempax >> 5;
-	tempcx = tempcx >> 3;
-	tempbx = tempbx >> 3;
+	tempax >>= 5;
+	tempcx >>= 3;
+	tempbx >>= 3;
 
 	tempcx &= 0x1f;
 	tempax |= tempcx;
@@ -1600,7 +1600,7 @@ static void XGI_SetLVDSRegs(unsigned short ModeIdIndex,
 	temp1 = pVBInfo->VGAHDE << 16;
 
 	temp1 /= temp3;
-	temp3 = temp3 << 16;
+	temp3 <<= 16;
 	temp1 -= 1;
 
 	temp3 = (temp3 & 0xffff0000) + (temp1 & 0xffff);
@@ -1622,10 +1622,10 @@ static void XGI_SetLVDSRegs(unsigned short ModeIdIndex,
 	xgifb_reg_set(pVBInfo->Part1Port, 0x21,
 			(unsigned short) (tempbx & 0xff));
 
-	temp3 = temp3 >> 16;
+	temp3 >>= 16;
 
 	if (modeflag & HalfDCLK)
-		temp3 = temp3 >> 1;
+		temp3 >>= 1;
 
 	xgifb_reg_set(pVBInfo->Part1Port, 0x22,
 			(unsigned short) ((temp3 >> 8) & 0xff));
@@ -1909,7 +1909,7 @@ static void XGI_GetVBInfo(unsigned short ModeIdIndex,
 	tempbx = tempbx | temp;
 	temp = xgifb_reg_get(pVBInfo->P3d4, 0x31);
 	push = temp;
-	push = push << 8;
+	push <<= 8;
 	tempax = temp << 8;
 	tempbx = tempbx | tempax;
 	temp = (SetCRT2ToDualEdge | SetCRT2ToYPbPr525750 | XGI_SetCRT2ToLCDA
@@ -2107,7 +2107,7 @@ static unsigned char XGI_GetLCDInfo(unsigned short ModeIdIndex,
 			if (pVBInfo->VBInfo & XGI_SetCRT2ToLCDA)
 				tempax &= 0x0F;
 			else
-				tempax = tempax >> 4;
+				tempax >>= 4;
 
 			if ((resinfo == 6) || (resinfo == 9)) {
 				if (tempax >= 3)
@@ -2182,7 +2182,7 @@ static unsigned char XG21GPIODataTransfer(unsigned char ujDate)
 	unsigned char i = 0;
 
 	for (i = 0; i < 8; i++) {
-		ujRet = ujRet << 1;
+		ujRet <<= 1;
 		ujRet |= (ujDate >> i) & 1;
 	}
 
@@ -2494,7 +2494,7 @@ static void XGI_GetRAMDAC2DATA(unsigned short ModeIdIndex,
 	tempcx = (unsigned short)
 			XGI_CRT1Table[CRT1Index].CR[14] << 8;
 	tempcx &= 0x0100;
-	tempcx = tempcx << 2;
+	tempcx <<= 2;
 	tempbx |= tempcx;
 	temp1 = (unsigned short) XGI_CRT1Table[CRT1Index].CR[9];
 
@@ -2745,7 +2745,7 @@ static unsigned short XGI_GetOffset(unsigned short ModeNo,
 	temp = XGI330_ScreenOffset[index];
 
 	if (infoflag & InterlaceMode)
-		temp = temp << 1;
+		temp <<= 1;
 
 	colordepth = XGI_GetColorDepth(ModeIdIndex);
 
@@ -2754,7 +2754,7 @@ static unsigned short XGI_GetOffset(unsigned short ModeNo,
 		colordepth = ColorDepth[temp];
 		temp = 0x6B;
 		if (infoflag & InterlaceMode)
-			temp = temp << 1;
+			temp <<= 1;
 	}
 	return temp * colordepth;
 }
@@ -2826,7 +2826,7 @@ static void XGI_SetGroup1(unsigned short ModeIdIndex,
 		xgifb_reg_set(pVBInfo->Part1Port, 0x0A, temp);
 		tempcx = ((pVBInfo->VGAHT - pVBInfo->VGAHDE) / 2) >> 2;
 		pushbx = pVBInfo->VGAHDE / 2 + 16;
-		tempcx = tempcx >> 1;
+		tempcx >>= 1;
 		tempbx = pushbx + tempcx; /* bx BTVGA@HRS 0x0B,0x0C */
 		tempcx += tempbx;
 
@@ -2861,7 +2861,7 @@ static void XGI_SetGroup1(unsigned short ModeIdIndex,
 		xgifb_reg_set(pVBInfo->Part1Port, 0x0A, temp);
 		tempcx = (pVBInfo->VGAHT - pVBInfo->VGAHDE) >> 2; /* cx */
 		pushbx = pVBInfo->VGAHDE + 16;
-		tempcx = tempcx >> 1;
+		tempcx >>= 1;
 		tempbx = pushbx + tempcx; /* bx BTVGA@HRS 0x0B,0x0C */
 		tempcx += tempbx;
 
@@ -2980,7 +2980,7 @@ static void XGI_SetLockRegs(unsigned short ModeNo, unsigned short ModeIdIndex,
 	tempax = pVBInfo->VGAHDE; /* 0x04 Horizontal Display End */
 
 	if (modeflag & HalfDCLK)
-		tempax = tempax >> 1;
+		tempax >>= 1;
 
 	tempax = (tempax / tempcx) - 1;
 	tempbx |= ((tempax & 0x00FF) << 8);
@@ -3015,7 +3015,7 @@ static void XGI_SetLockRegs(unsigned short ModeNo, unsigned short ModeIdIndex,
 		tempax = pVBInfo->VGAHT;
 
 	if (modeflag & HalfDCLK)
-		tempax = tempax >> 1;
+		tempax >>= 1;
 
 	tempax = (tempax / tempcx) - 5;
 	tempcx = tempax; /* 20030401 0x07 horizontal Retrace Start */
@@ -3142,11 +3142,11 @@ static void XGI_SetLockRegs(unsigned short ModeNo, unsigned short ModeIdIndex,
 
 	tempax = push1;
 	tempax -= tempbx; /* 0x0C Vertical Retrace Start */
-	tempax = tempax >> 2;
+	tempax >>= 2;
 	push1 = tempax; /* push ax */
 
 	if (resinfo != 0x09) {
-		tempax = tempax << 1;
+		tempax <<= 1;
 		tempbx += tempax;
 	}
 
@@ -3179,7 +3179,7 @@ static void XGI_SetLockRegs(unsigned short ModeNo, unsigned short ModeIdIndex,
 		}
 	}
 	tempax = push1;
-	tempax = tempax >> 2;
+	tempax >>= 2;
 	tempax++;
 	tempax += tempbx;
 	push1 = tempax; /* push ax */
@@ -3332,7 +3332,7 @@ static void XGI_SetGroup2(unsigned short ModeNo, unsigned short ModeIdIndex,
 
 	if (pVBInfo->VDE <= tempax) {
 		tempax -= pVBInfo->VDE;
-		tempax = tempax >> 2;
+		tempax >>= 2;
 		tempax = (tempax & 0x00FF) | ((tempax & 0x00FF) << 8);
 		push1 = tempax;
 		temp = (tempax & 0xFF00) >> 8;
@@ -3377,7 +3377,7 @@ static void XGI_SetGroup2(unsigned short ModeNo, unsigned short ModeIdIndex,
 	tempcx = pVBInfo->HT;
 
 	if (XGI_IsLCDDualLink(pVBInfo))
-		tempcx = tempcx >> 1;
+		tempcx >>= 1;
 
 	tempcx -= 2;
 	temp = tempcx & 0x00FF;
@@ -3394,7 +3394,7 @@ static void XGI_SetGroup2(unsigned short ModeNo, unsigned short ModeIdIndex,
 		tempcx -= 4;
 
 	temp = tempcx & 0x00FF;
-	temp = temp << 4;
+	temp <<= 4;
 	xgifb_reg_and_or(pVBInfo->Part2Port, 0x22, 0x0F, temp);
 
 	tempbx = TimingPoint[j] | ((TimingPoint[j + 1]) << 8);
@@ -3403,7 +3403,7 @@ static void XGI_SetGroup2(unsigned short ModeNo, unsigned short ModeIdIndex,
 	temp = tempbx & 0x00FF;
 	xgifb_reg_set(pVBInfo->Part2Port, 0x24, temp);
 	temp = (tempbx & 0xFF00) >> 8;
-	temp = temp << 4;
+	temp <<= 4;
 	xgifb_reg_and_or(pVBInfo->Part2Port, 0x25, 0x0F, temp);
 
 	tempbx = push2;
@@ -3428,7 +3428,7 @@ static void XGI_SetGroup2(unsigned short ModeNo, unsigned short ModeIdIndex,
 		tempcx -= 4;
 
 	temp = tempcx & 0xFF;
-	temp = temp << 4;
+	temp <<= 4;
 	xgifb_reg_and_or(pVBInfo->Part2Port, 0x2A, 0x0F, temp);
 
 	tempcx = push1; /* pop cx */
@@ -3436,7 +3436,7 @@ static void XGI_SetGroup2(unsigned short ModeNo, unsigned short ModeIdIndex,
 	temp = TimingPoint[j] | ((TimingPoint[j + 1]) << 8);
 	tempcx -= temp;
 	temp = tempcx & 0x00FF;
-	temp = temp << 4;
+	temp <<= 4;
 	xgifb_reg_and_or(pVBInfo->Part2Port, 0x2D, 0x0F, temp);
 
 	tempcx -= 11;
@@ -3462,9 +3462,9 @@ static void XGI_SetGroup2(unsigned short ModeNo, unsigned short ModeIdIndex,
 		    (VB_SIS301LV | VB_SIS302LV | VB_XGI301C)) {
 			if (!(pVBInfo->TVInfo &
 			    (TVSetYPbPr525p | TVSetYPbPr750p)))
-				tempbx = tempbx >> 1;
+				tempbx >>= 1;
 		} else
-			tempbx = tempbx >> 1;
+			tempbx >>= 1;
 	}
 
 	tempbx -= 2;
@@ -3514,7 +3514,7 @@ static void XGI_SetGroup2(unsigned short ModeNo, unsigned short ModeIdIndex,
 		if (pVBInfo->VBInfo & SetCRT2ToTV) {
 			if (!(pVBInfo->TVInfo & (TVSetYPbPr525p
 					| TVSetYPbPr750p)))
-				tempbx = tempbx >> 1;
+				tempbx >>= 1;
 		}
 
 		if (pVBInfo->VBType & (VB_SIS302LV | VB_XGI301C)) {
@@ -3627,7 +3627,7 @@ static void XGI_SetGroup2(unsigned short ModeNo, unsigned short ModeIdIndex,
 	xgifb_reg_set(pVBInfo->Part2Port, 0x4c, temp);
 
 	temp = ((tempcx & 0xFF00) >> 8) & 0x03;
-	temp = temp << 2;
+	temp <<= 2;
 	temp |= ((tempbx & 0xFF00) >> 8) & 0x03;
 
 	if (pVBInfo->VBInfo & SetCRT2ToYPbPr525750) {
@@ -3691,13 +3691,13 @@ static void XGI_SetLCDRegs(unsigned short ModeIdIndex,
 	tempbx = pVBInfo->HDE; /* RHACTE=HDE-1 */
 
 	if (XGI_IsLCDDualLink(pVBInfo))
-		tempbx = tempbx >> 1;
+		tempbx >>= 1;
 
 	tempbx -= 1;
 	temp = tempbx & 0x00FF;
 	xgifb_reg_set(pVBInfo->Part2Port, 0x2C, temp);
 	temp = (tempbx & 0xFF00) >> 8;
-	temp = temp << 4;
+	temp <<= 4;
 	xgifb_reg_and_or(pVBInfo->Part2Port, 0x2B, 0x0F, temp);
 	temp = 0x01;
 
@@ -3713,7 +3713,7 @@ static void XGI_SetLCDRegs(unsigned short ModeIdIndex,
 	temp = tempcx & 0x00FF; /* RVTVT=VT-1 */
 	xgifb_reg_set(pVBInfo->Part2Port, 0x19, temp);
 	temp = (tempcx & 0xFF00) >> 8;
-	temp = temp << 5;
+	temp <<= 5;
 	xgifb_reg_set(pVBInfo->Part2Port, 0x1A, temp);
 	xgifb_reg_and_or(pVBInfo->Part2Port, 0x09, 0xF0, 0x00);
 	xgifb_reg_and_or(pVBInfo->Part2Port, 0x0A, 0xF0, 0x00);
@@ -3770,7 +3770,7 @@ static void XGI_SetLCDRegs(unsigned short ModeIdIndex,
 	tempch = ((tempcx & 0xFF00) >> 8) & 0x07;
 	tempbh = ((tempbx & 0xFF00) >> 8) & 0x07;
 	tempah = tempch;
-	tempah = tempah << 3;
+	tempah <<= 3;
 	tempah |= tempbh;
 	xgifb_reg_set(pVBInfo->Part2Port, 0x02, tempah);
 
@@ -3787,7 +3787,7 @@ static void XGI_SetLCDRegs(unsigned short ModeIdIndex,
 	temp = tempbx & 0x00FF; /* RTVACTEE=lcdvrs */
 	xgifb_reg_set(pVBInfo->Part2Port, 0x04, temp);
 	temp = (tempbx & 0xFF00) >> 8;
-	temp = temp << 4;
+	temp <<= 4;
 	temp |= (tempcx & 0x000F);
 	xgifb_reg_set(pVBInfo->Part2Port, 0x01, temp);
 	tempcx = pushbx;
@@ -3796,9 +3796,9 @@ static void XGI_SetLCDRegs(unsigned short ModeIdIndex,
 	tempbx &= 0x0FFF;
 
 	if (XGI_IsLCDDualLink(pVBInfo)) {
-		tempax = tempax >> 1;
-		tempbx = tempbx >> 1;
-		tempcx = tempcx >> 1;
+		tempax >>= 1;
+		tempbx >>= 1;
+		tempcx >>= 1;
 	}
 
 	if (pVBInfo->VBType & VB_SIS302LV)
@@ -3826,9 +3826,9 @@ static void XGI_SetLCDRegs(unsigned short ModeIdIndex,
 	tempax = pVBInfo->HT;
 	tempbx = pVBInfo->LCDHRS;
 	if (XGI_IsLCDDualLink(pVBInfo)) {
-		tempax = tempax >> 1;
-		tempbx = tempbx >> 1;
-		tempcx = tempcx >> 1;
+		tempax >>= 1;
+		tempbx >>= 1;
+		tempcx >>= 1;
 	}
 
 	if (pVBInfo->VBType & VB_SIS302LV)
@@ -3843,7 +3843,7 @@ static void XGI_SetLCDRegs(unsigned short ModeIdIndex,
 	xgifb_reg_set(pVBInfo->Part2Port, 0x1C, temp);
 
 	temp = (tempbx & 0xFF00) >> 8;
-	temp = temp << 4;
+	temp <<= 4;
 	xgifb_reg_and_or(pVBInfo->Part2Port, 0x1D, ~0x0F0, temp);
 	temp = tempcx & 0x00FF; /* RHSYEXP2S=lcdhre */
 	xgifb_reg_set(pVBInfo->Part2Port, 0x21, temp);
@@ -4044,10 +4044,10 @@ static void XGI_SetGroup4(unsigned short ModeIdIndex,
 	tempbx = pVBInfo->VGAHDE;
 
 	if (modeflag & HalfDCLK)
-		tempbx = tempbx >> 1;
+		tempbx >>= 1;
 
 	if (XGI_IsLCDDualLink(pVBInfo))
-		tempbx = tempbx >> 1;
+		tempbx >>= 1;
 
 	if (tempcx & SetCRT2ToHiVision) {
 		temp = 0;
@@ -4107,7 +4107,7 @@ static void XGI_SetGroup4(unsigned short ModeIdIndex,
 	xgifb_reg_set(pVBInfo->Part4Port, 0x1A, temp);
 	tempbx = (unsigned short) (tempebx >> 16);
 	temp = tempbx & 0x00FF;
-	temp = temp << 4;
+	temp <<= 4;
 	temp |= ((tempcx & 0xFF00) >> 8);
 	xgifb_reg_set(pVBInfo->Part4Port, 0x19, temp);
 
@@ -4118,10 +4118,10 @@ static void XGI_SetGroup4(unsigned short ModeIdIndex,
 		xgifb_reg_set(pVBInfo->Part4Port, 0x1C, temp);
 		tempax = pVBInfo->VGAHDE;
 		if (modeflag & HalfDCLK)
-			tempax = tempax >> 1;
+			tempax >>= 1;
 
 		if (XGI_IsLCDDualLink(pVBInfo))
-			tempax = tempax >> 1;
+			tempax >>= 1;
 
 		if (pVBInfo->VBInfo & SetCRT2ToLCD) {
 			if (tempax > 800)
@@ -4162,7 +4162,7 @@ static void XGI_SetGroup4(unsigned short ModeIdIndex,
 		xgifb_reg_and_or(pVBInfo->Part4Port, 0x1F, 0x00C0, temp);
 		tempbx = pVBInfo->HT;
 		if (XGI_IsLCDDualLink(pVBInfo))
-			tempbx = tempbx >> 1;
+			tempbx >>= 1;
 		tempbx = (tempbx >> 1) - 2;
 		temp = ((tempbx & 0x0700) >> 8) << 3;
 		xgifb_reg_and_or(pVBInfo->Part4Port, 0x21, 0x00C0, temp);
@@ -4622,7 +4622,7 @@ static void XGI_SetDelayComp(struct vb_device_info *pVBInfo)
 			tempbl = XGI301TVDelay;
 
 			if (pVBInfo->VBInfo & SetCRT2ToDualEdge)
-				tempbl = tempbl >> 4;
+				tempbl >>= 4;
 			if (pVBInfo->VBInfo &
 			    (SetCRT2ToLCD | XGI_SetCRT2ToLCDA)) {
 				tempbh = XGI301LCDDelay;
@@ -4785,7 +4785,7 @@ static void XGI_SetAntiFlicker(struct vb_device_info *pVBInfo)
 	tempbx = XGI_GetTVPtrIndex(pVBInfo);
 	tempbx &= 0xFE;
 	tempah = TVAntiFlickList[tempbx];
-	tempah = tempah << 4;
+	tempah <<= 4;
 
 	xgifb_reg_and_or(pVBInfo->Part2Port, 0x0A, 0x8F, tempah);
 }
@@ -4799,7 +4799,7 @@ static void XGI_SetEdgeEnhance(struct vb_device_info *pVBInfo)
 	tempbx = XGI_GetTVPtrIndex(pVBInfo);
 	tempbx &= 0xFE;
 	tempah = TVEdgeList[tempbx];
-	tempah = tempah << 5;
+	tempah <<= 5;
 
 	xgifb_reg_and_or(pVBInfo->Part2Port, 0x3A, 0x1F, tempah);
 }
@@ -5101,7 +5101,7 @@ unsigned short XGI_GetRatePtrCRT2(struct xgi_hw_device_info *pXGIHWDE,
 	unsigned short RefreshRateTableIndex, i, index, temp;
 
 	index = xgifb_reg_get(pVBInfo->P3d4, 0x33);
-	index = index >> pVBInfo->SelectCRT2Rate;
+	index >>= pVBInfo->SelectCRT2Rate;
 	index &= 0x0F;
 
 	if (pVBInfo->LCDInfo & LCDNonExpanding)
diff --git a/drivers/target/iscsi/iscsi_target.c b/drivers/target/iscsi/iscsi_target.c
index 50bad55a0c42..77d64251af40 100644
--- a/drivers/target/iscsi/iscsi_target.c
+++ b/drivers/target/iscsi/iscsi_target.c
@@ -1181,7 +1181,7 @@ iscsit_handle_scsi_cmd(struct iscsi_conn *conn, struct iscsi_cmd *cmd,
 	 * traditional iSCSI block I/O.
 	 */
 	if (iscsit_allocate_iovecs(cmd) < 0) {
-		return iscsit_add_reject_cmd(cmd,
+		return iscsit_reject_cmd(cmd,
 				ISCSI_REASON_BOOKMARK_NO_RESOURCES, buf);
 	}
 	immed_data = cmd->immediate_data;
@@ -3468,6 +3468,7 @@ iscsit_build_sendtargets_response(struct iscsi_cmd *cmd,
 						tpg_np_list) {
 				struct iscsi_np *np = tpg_np->tpg_np;
 				bool inaddr_any = iscsit_check_inaddr_any(np);
+				char *fmt_str;
 
 				if (np->np_network_transport != network_transport)
 					continue;
@@ -3495,8 +3496,12 @@ iscsit_build_sendtargets_response(struct iscsi_cmd *cmd,
 					}
 				}
 
-				len = sprintf(buf, "TargetAddress="
-					"%s:%hu,%hu",
+				if (np->np_sockaddr.ss_family == AF_INET6)
+					fmt_str = "TargetAddress=[%s]:%hu,%hu";
+				else
+					fmt_str = "TargetAddress=%s:%hu,%hu";
+
+				len = sprintf(buf, fmt_str,
 					inaddr_any ? conn->local_ip : np->np_ip,
 					np->np_port,
 					tpg->tpgt);
@@ -4256,11 +4261,17 @@ int iscsit_close_connection(
 	pr_debug("Closing iSCSI connection CID %hu on SID:"
 		" %u\n", conn->cid, sess->sid);
 	/*
-	 * Always up conn_logout_comp just in case the RX Thread is sleeping
-	 * and the logout response never got sent because the connection
-	 * failed.
+	 * Always up conn_logout_comp for the traditional TCP case just in case
+	 * the RX Thread in iscsi_target_rx_opcode() is sleeping and the logout
+	 * response never got sent because the connection failed.
+	 *
+	 * However for iser-target, isert_wait4logout() is using conn_logout_comp
+	 * to signal logout response TX interrupt completion.  Go ahead and skip
+	 * this for iser since isert_rx_opcode() does not wait on logout failure,
+	 * and to avoid iscsi_conn pointer dereference in iser-target code.
 	 */
-	complete(&conn->conn_logout_comp);
+	if (conn->conn_transport->transport_type == ISCSI_TCP)
+		complete(&conn->conn_logout_comp);
 
 	iscsi_release_thread_set(conn);
 
diff --git a/drivers/target/iscsi/iscsi_target_erl0.c b/drivers/target/iscsi/iscsi_target_erl0.c
index 1c197bad6132..bdd8731a4daa 100644
--- a/drivers/target/iscsi/iscsi_target_erl0.c
+++ b/drivers/target/iscsi/iscsi_target_erl0.c
@@ -22,7 +22,6 @@
 #include <target/target_core_fabric.h>
 
 #include <target/iscsi/iscsi_target_core.h>
-#include <target/iscsi/iscsi_transport.h>
 #include "iscsi_target_seq_pdu_list.h"
 #include "iscsi_target_tq.h"
 #include "iscsi_target_erl0.h"
@@ -940,8 +939,7 @@ void iscsit_take_action_for_connection_exit(struct iscsi_conn *conn)
 
 	if (conn->conn_state == TARG_CONN_STATE_IN_LOGOUT) {
 		spin_unlock_bh(&conn->state_lock);
-		if (conn->conn_transport->transport_type == ISCSI_TCP)
-			iscsit_close_connection(conn);
+		iscsit_close_connection(conn);
 		return;
 	}
 
diff --git a/drivers/target/loopback/tcm_loop.c b/drivers/target/loopback/tcm_loop.c
index 6b3c32954689..c36bd7c29136 100644
--- a/drivers/target/loopback/tcm_loop.c
+++ b/drivers/target/loopback/tcm_loop.c
@@ -953,11 +953,8 @@ static int tcm_loop_make_nexus(
 		transport_free_session(tl_nexus->se_sess);
 		goto out;
 	}
-	/*
-	 * Now, register the SAS I_T Nexus as active with the call to
-	 * transport_register_session()
-	 */
-	__transport_register_session(se_tpg, tl_nexus->se_sess->se_node_acl,
+	/* Now, register the SAS I_T Nexus as active. */
+	transport_register_session(se_tpg, tl_nexus->se_sess->se_node_acl,
 			tl_nexus->se_sess, tl_nexus);
 	tl_tpg->tl_nexus = tl_nexus;
 	pr_debug("TCM_Loop_ConfigFS: Established I_T Nexus to emulated"
diff --git a/drivers/target/target_core_device.c b/drivers/target/target_core_device.c
index 58f49ff69b14..7faa6aef9a4d 100644
--- a/drivers/target/target_core_device.c
+++ b/drivers/target/target_core_device.c
@@ -650,6 +650,18 @@ static u32 se_dev_align_max_sectors(u32 max_sectors, u32 block_size)
 	return aligned_max_sectors;
 }
 
+bool se_dev_check_wce(struct se_device *dev)
+{
+	bool wce = false;
+
+	if (dev->transport->get_write_cache)
+		wce = dev->transport->get_write_cache(dev);
+	else if (dev->dev_attrib.emulate_write_cache > 0)
+		wce = true;
+
+	return wce;
+}
+
 int se_dev_set_max_unmap_lba_count(
 	struct se_device *dev,
 	u32 max_unmap_lba_count)
@@ -767,6 +779,16 @@ int se_dev_set_emulate_fua_write(struct se_device *dev, int flag)
 		pr_err("Illegal value %d\n", flag);
 		return -EINVAL;
 	}
+	if (flag &&
+	    dev->transport->get_write_cache) {
+		pr_warn("emulate_fua_write not supported for this device, ignoring\n");
+		return 0;
+	}
+	if (dev->export_count) {
+		pr_err("emulate_fua_write cannot be changed with active"
+		       " exports: %d\n", dev->export_count);
+		return -EINVAL;
+	}
 	dev->dev_attrib.emulate_fua_write = flag;
 	pr_debug("dev[%p]: SE Device Forced Unit Access WRITEs: %d\n",
 			dev, dev->dev_attrib.emulate_fua_write);
@@ -801,7 +823,11 @@ int se_dev_set_emulate_write_cache(struct se_device *dev, int flag)
 		pr_err("emulate_write_cache not supported for this device\n");
 		return -EINVAL;
 	}
-
+	if (dev->export_count) {
+		pr_err("emulate_write_cache cannot be changed with active"
+		       " exports: %d\n", dev->export_count);
+		return -EINVAL;
+	}
 	dev->dev_attrib.emulate_write_cache = flag;
 	pr_debug("dev[%p]: SE Device WRITE_CACHE_EMULATION flag: %d\n",
 			dev, dev->dev_attrib.emulate_write_cache);
@@ -1534,8 +1560,6 @@ int target_configure_device(struct se_device *dev)
 	ret = dev->transport->configure_device(dev);
 	if (ret)
 		goto out;
-	dev->dev_flags |= DF_CONFIGURED;
-
 	/*
 	 * XXX: there is not much point to have two different values here..
 	 */
@@ -1597,6 +1621,8 @@ int target_configure_device(struct se_device *dev)
 	list_add_tail(&dev->g_dev_node, &g_device_list);
 	mutex_unlock(&g_device_mutex);
 
+	dev->dev_flags |= DF_CONFIGURED;
+
 	return 0;
 
 out_free_alua:
diff --git a/drivers/target/target_core_pscsi.c b/drivers/target/target_core_pscsi.c
index 1045dcd7bf65..f6c954c4635f 100644
--- a/drivers/target/target_core_pscsi.c
+++ b/drivers/target/target_core_pscsi.c
@@ -1121,7 +1121,7 @@ static u32 pscsi_get_device_type(struct se_device *dev)
 	struct pscsi_dev_virt *pdv = PSCSI_DEV(dev);
 	struct scsi_device *sd = pdv->pdv_sd;
 
-	return sd->type;
+	return (sd) ? sd->type : TYPE_NO_LUN;
 }
 
 static sector_t pscsi_get_blocks(struct se_device *dev)
diff --git a/drivers/target/target_core_sbc.c b/drivers/target/target_core_sbc.c
index 9a2f9d3a6e70..3e7297411110 100644
--- a/drivers/target/target_core_sbc.c
+++ b/drivers/target/target_core_sbc.c
@@ -708,8 +708,7 @@ sbc_check_dpofua(struct se_device *dev, struct se_cmd *cmd, unsigned char *cdb)
 		}
 	}
 	if (cdb[1] & 0x8) {
-		if (!dev->dev_attrib.emulate_fua_write ||
-		    !dev->dev_attrib.emulate_write_cache) {
+		if (!dev->dev_attrib.emulate_fua_write || !se_dev_check_wce(dev)) {
 			pr_err("Got CDB: 0x%02x with FUA bit set, but device"
 			       " does not advertise support for FUA write\n",
 			       cdb[0]);
diff --git a/drivers/target/target_core_spc.c b/drivers/target/target_core_spc.c
index 460e93109473..6c8bd6bc175c 100644
--- a/drivers/target/target_core_spc.c
+++ b/drivers/target/target_core_spc.c
@@ -454,19 +454,6 @@ check_scsi_name:
 }
 EXPORT_SYMBOL(spc_emulate_evpd_83);
 
-static bool
-spc_check_dev_wce(struct se_device *dev)
-{
-	bool wce = false;
-
-	if (dev->transport->get_write_cache)
-		wce = dev->transport->get_write_cache(dev);
-	else if (dev->dev_attrib.emulate_write_cache > 0)
-		wce = true;
-
-	return wce;
-}
-
 /* Extended INQUIRY Data VPD Page */
 static sense_reason_t
 spc_emulate_evpd_86(struct se_cmd *cmd, unsigned char *buf)
@@ -490,7 +477,7 @@ spc_emulate_evpd_86(struct se_cmd *cmd, unsigned char *buf)
 	buf[5] = 0x07;
 
 	/* If WriteCache emulation is enabled, set V_SUP */
-	if (spc_check_dev_wce(dev))
+	if (se_dev_check_wce(dev))
 		buf[6] = 0x01;
 	/* If an LBA map is present set R_SUP */
 	spin_lock(&cmd->se_dev->t10_alua.lba_map_lock);
@@ -897,7 +884,7 @@ static int spc_modesense_caching(struct se_cmd *cmd, u8 pc, u8 *p)
 	if (pc == 1)
 		goto out;
 
-	if (spc_check_dev_wce(dev))
+	if (se_dev_check_wce(dev))
 		p[2] = 0x04; /* Write Cache Enable */
 	p[12] = 0x20; /* Disabled Read Ahead */
 
@@ -1009,7 +996,7 @@ static sense_reason_t spc_emulate_modesense(struct se_cmd *cmd)
 	     (cmd->se_deve->lun_flags & TRANSPORT_LUNFLAGS_READ_ONLY)))
 		spc_modesense_write_protect(&buf[length], type);
 
-	if ((spc_check_dev_wce(dev)) &&
+	if ((se_dev_check_wce(dev)) &&
 	    (dev->dev_attrib.emulate_fua_write > 0))
 		spc_modesense_dpofua(&buf[length], type);
 
diff --git a/drivers/target/target_core_transport.c b/drivers/target/target_core_transport.c
index 0adc0f650213..ac3cbabdbdf0 100644
--- a/drivers/target/target_core_transport.c
+++ b/drivers/target/target_core_transport.c
@@ -2389,6 +2389,10 @@ int target_get_sess_cmd(struct se_session *se_sess, struct se_cmd *se_cmd,
 	list_add_tail(&se_cmd->se_cmd_list, &se_sess->sess_cmd_list);
 out:
 	spin_unlock_irqrestore(&se_sess->sess_cmd_lock, flags);
+
+	if (ret && ack_kref)
+		target_put_sess_cmd(se_sess, se_cmd);
+
 	return ret;
 }
 EXPORT_SYMBOL(target_get_sess_cmd);
diff --git a/drivers/target/tcm_fc/tfc_io.c b/drivers/target/tcm_fc/tfc_io.c
index 97b486c3dda1..583e755d8091 100644
--- a/drivers/target/tcm_fc/tfc_io.c
+++ b/drivers/target/tcm_fc/tfc_io.c
@@ -359,7 +359,7 @@ void ft_invl_hw_context(struct ft_cmd *cmd)
 		ep = fc_seq_exch(seq);
 		if (ep) {
 			lport = ep->lp;
-			if (lport && (ep->xid <= lport->lro_xid))
+			if (lport && (ep->xid <= lport->lro_xid)) {
 				/*
 				 * "ddp_done" trigger invalidation of HW
 				 * specific DDP context
@@ -374,6 +374,7 @@ void ft_invl_hw_context(struct ft_cmd *cmd)
 				 * identified using ep->xid)
 				 */
 				cmd->was_ddp_setup = 0;
+			}
 		}
 	}
 }
diff --git a/drivers/thermal/int340x_thermal/int3400_thermal.c b/drivers/thermal/int340x_thermal/int3400_thermal.c
index 25d244cbbe8f..031018e7a65b 100644
--- a/drivers/thermal/int340x_thermal/int3400_thermal.c
+++ b/drivers/thermal/int340x_thermal/int3400_thermal.c
@@ -262,13 +262,12 @@ static int int3400_thermal_probe(struct platform_device *pdev)
 	result = acpi_parse_art(priv->adev->handle, &priv->art_count,
 				&priv->arts, true);
 	if (result)
-		goto free_priv;
-
+		dev_dbg(&pdev->dev, "_ART table parsing error\n");
 
 	result = acpi_parse_trt(priv->adev->handle, &priv->trt_count,
 				&priv->trts, true);
 	if (result)
-		goto free_art;
+		dev_dbg(&pdev->dev, "_TRT table parsing error\n");
 
 	platform_set_drvdata(pdev, priv);
 
@@ -281,7 +280,7 @@ static int int3400_thermal_probe(struct platform_device *pdev)
 						&int3400_thermal_params, 0, 0);
 	if (IS_ERR(priv->thermal)) {
 		result = PTR_ERR(priv->thermal);
-		goto free_trt;
+		goto free_art_trt;
 	}
 
 	priv->rel_misc_dev_res = acpi_thermal_rel_misc_device_add(
@@ -295,9 +294,8 @@ static int int3400_thermal_probe(struct platform_device *pdev)
 
 free_zone:
 	thermal_zone_device_unregister(priv->thermal);
-free_trt:
+free_art_trt:
 	kfree(priv->trts);
-free_art:
 	kfree(priv->arts);
 free_priv:
 	kfree(priv);
diff --git a/drivers/thermal/int340x_thermal/int340x_thermal_zone.c b/drivers/thermal/int340x_thermal/int340x_thermal_zone.c
index f88b08877025..1e25133d35e2 100644
--- a/drivers/thermal/int340x_thermal/int340x_thermal_zone.c
+++ b/drivers/thermal/int340x_thermal/int340x_thermal_zone.c
@@ -208,7 +208,7 @@ struct int34x_thermal_zone *int340x_thermal_zone_add(struct acpi_device *adev,
 				trip_cnt, GFP_KERNEL);
 		if (!int34x_thermal_zone->aux_trips) {
 			ret = -ENOMEM;
-			goto free_mem;
+			goto err_trip_alloc;
 		}
 		trip_mask = BIT(trip_cnt) - 1;
 		int34x_thermal_zone->aux_trip_nr = trip_cnt;
@@ -248,14 +248,15 @@ struct int34x_thermal_zone *int340x_thermal_zone_add(struct acpi_device *adev,
 						0, 0);
 	if (IS_ERR(int34x_thermal_zone->zone)) {
 		ret = PTR_ERR(int34x_thermal_zone->zone);
-		goto free_lpat;
+		goto err_thermal_zone;
 	}
 
 	return int34x_thermal_zone;
 
-free_lpat:
+err_thermal_zone:
 	acpi_lpat_free_conversion_table(int34x_thermal_zone->lpat_table);
-free_mem:
+	kfree(int34x_thermal_zone->aux_trips);
+err_trip_alloc:
 	kfree(int34x_thermal_zone);
 	return ERR_PTR(ret);
 }
@@ -266,6 +267,7 @@ void int340x_thermal_zone_remove(struct int34x_thermal_zone
 {
 	thermal_zone_device_unregister(int34x_thermal_zone->zone);
 	acpi_lpat_free_conversion_table(int34x_thermal_zone->lpat_table);
+	kfree(int34x_thermal_zone->aux_trips);
 	kfree(int34x_thermal_zone);
 }
 EXPORT_SYMBOL_GPL(int340x_thermal_zone_remove);
diff --git a/drivers/thermal/intel_powerclamp.c b/drivers/thermal/intel_powerclamp.c
index 6ceebd659dd4..12623bc02f46 100644
--- a/drivers/thermal/intel_powerclamp.c
+++ b/drivers/thermal/intel_powerclamp.c
@@ -688,6 +688,7 @@ static const struct x86_cpu_id intel_powerclamp_ids[] = {
 	{ X86_VENDOR_INTEL, 6, 0x45},
 	{ X86_VENDOR_INTEL, 6, 0x46},
 	{ X86_VENDOR_INTEL, 6, 0x4c},
+	{ X86_VENDOR_INTEL, 6, 0x4d},
 	{ X86_VENDOR_INTEL, 6, 0x56},
 	{}
 };
diff --git a/drivers/thermal/rcar_thermal.c b/drivers/thermal/rcar_thermal.c
index 2580a4872f90..fe4e767018c4 100644
--- a/drivers/thermal/rcar_thermal.c
+++ b/drivers/thermal/rcar_thermal.c
@@ -387,21 +387,9 @@ static int rcar_thermal_probe(struct platform_device *pdev)
 
 	irq = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
 	if (irq) {
-		int ret;
-
 		/*
 		 * platform has IRQ support.
 		 * Then, driver uses common registers
-		 */
-
-		ret = devm_request_irq(dev, irq->start, rcar_thermal_irq, 0,
-				       dev_name(dev), common);
-		if (ret) {
-			dev_err(dev, "irq request failed\n ");
-			return ret;
-		}
-
-		/*
 		 * rcar_has_irq_support() will be enabled
 		 */
 		res = platform_get_resource(pdev, IORESOURCE_MEM, mres++);
@@ -456,8 +444,16 @@ static int rcar_thermal_probe(struct platform_device *pdev)
 	}
 
 	/* enable temperature comparation */
-	if (irq)
+	if (irq) {
+		ret = devm_request_irq(dev, irq->start, rcar_thermal_irq, 0,
+				       dev_name(dev), common);
+		if (ret) {
+			dev_err(dev, "irq request failed\n ");
+			goto error_unregister;
+		}
+
 		rcar_thermal_common_write(common, ENR, enr_bits);
+	}
 
 	platform_set_drvdata(pdev, common);
 
@@ -467,9 +463,9 @@ static int rcar_thermal_probe(struct platform_device *pdev)
 
 error_unregister:
 	rcar_thermal_for_each_priv(priv, common) {
-		thermal_zone_device_unregister(priv->zone);
 		if (rcar_has_irq_support(priv))
 			rcar_thermal_irq_disable(priv);
+		thermal_zone_device_unregister(priv->zone);
 	}
 
 	pm_runtime_put(dev);
@@ -485,9 +481,9 @@ static int rcar_thermal_remove(struct platform_device *pdev)
 	struct rcar_thermal_priv *priv;
 
 	rcar_thermal_for_each_priv(priv, common) {
-		thermal_zone_device_unregister(priv->zone);
 		if (rcar_has_irq_support(priv))
 			rcar_thermal_irq_disable(priv);
+		thermal_zone_device_unregister(priv->zone);
 	}
 
 	pm_runtime_put(dev);
diff --git a/drivers/thermal/samsung/exynos_tmu.c b/drivers/thermal/samsung/exynos_tmu.c
index 933cd80a6bc5..1d30b0975651 100644
--- a/drivers/thermal/samsung/exynos_tmu.c
+++ b/drivers/thermal/samsung/exynos_tmu.c
@@ -682,6 +682,7 @@ static void exynos7_tmu_control(struct platform_device *pdev, bool on)
 
 	if (on) {
 		con |= (1 << EXYNOS_TMU_CORE_EN_SHIFT);
+		con |= (1 << EXYNOS7_PD_DET_EN_SHIFT);
 		interrupt_en =
 			(of_thermal_is_trip_valid(tz, 7)
 			<< EXYNOS7_TMU_INTEN_RISE7_SHIFT) |
@@ -704,9 +705,9 @@ static void exynos7_tmu_control(struct platform_device *pdev, bool on)
 			interrupt_en << EXYNOS_TMU_INTEN_FALL0_SHIFT;
 	} else {
 		con &= ~(1 << EXYNOS_TMU_CORE_EN_SHIFT);
+		con &= ~(1 << EXYNOS7_PD_DET_EN_SHIFT);
 		interrupt_en = 0; /* Disable all interrupts */
 	}
-	con |= 1 << EXYNOS7_PD_DET_EN_SHIFT;
 
 	writel(interrupt_en, data->base + EXYNOS7_TMU_REG_INTEN);
 	writel(con, data->base + EXYNOS_TMU_REG_CONTROL);
@@ -918,34 +919,16 @@ static irqreturn_t exynos_tmu_irq(int irq, void *id)
 }
 
 static const struct of_device_id exynos_tmu_match[] = {
-	{
-		.compatible = "samsung,exynos3250-tmu",
-	},
-	{
-		.compatible = "samsung,exynos4210-tmu",
-	},
-	{
-		.compatible = "samsung,exynos4412-tmu",
-	},
-	{
-		.compatible = "samsung,exynos5250-tmu",
-	},
-	{
-		.compatible = "samsung,exynos5260-tmu",
-	},
-	{
-		.compatible = "samsung,exynos5420-tmu",
-	},
-	{
-		.compatible = "samsung,exynos5420-tmu-ext-triminfo",
-	},
-	{
-		.compatible = "samsung,exynos5440-tmu",
-	},
-	{
-		.compatible = "samsung,exynos7-tmu",
-	},
-	{},
+	{ .compatible = "samsung,exynos3250-tmu", },
+	{ .compatible = "samsung,exynos4210-tmu", },
+	{ .compatible = "samsung,exynos4412-tmu", },
+	{ .compatible = "samsung,exynos5250-tmu", },
+	{ .compatible = "samsung,exynos5260-tmu", },
+	{ .compatible = "samsung,exynos5420-tmu", },
+	{ .compatible = "samsung,exynos5420-tmu-ext-triminfo", },
+	{ .compatible = "samsung,exynos5440-tmu", },
+	{ .compatible = "samsung,exynos7-tmu", },
+	{ /* sentinel */ },
 };
 MODULE_DEVICE_TABLE(of, exynos_tmu_match);
 
diff --git a/drivers/thermal/st/st_thermal.c b/drivers/thermal/st/st_thermal.c
index d1ec5804c0bb..76c515dd802b 100644
--- a/drivers/thermal/st/st_thermal.c
+++ b/drivers/thermal/st/st_thermal.c
@@ -25,7 +25,7 @@
  * Function to allocate regfields which are common
  * between syscfg and memory mapped based sensors
  */
-int st_thermal_alloc_regfields(struct st_thermal_sensor *sensor)
+static int st_thermal_alloc_regfields(struct st_thermal_sensor *sensor)
 {
 	struct device *dev = sensor->dev;
 	struct regmap *regmap = sensor->regmap;
diff --git a/drivers/thermal/st/st_thermal_memmap.c b/drivers/thermal/st/st_thermal_memmap.c
index 067bfcdb91d6..fc0c9e198710 100644
--- a/drivers/thermal/st/st_thermal_memmap.c
+++ b/drivers/thermal/st/st_thermal_memmap.c
@@ -157,7 +157,7 @@ static const struct st_thermal_sensor_ops st_mmap_sensor_ops = {
 };
 
 /* Compatible device data stih416 mpe thermal sensor */
-const struct st_thermal_compat_data st_416mpe_cdata = {
+static const struct st_thermal_compat_data st_416mpe_cdata = {
 	.reg_fields		= st_mmap_thermal_regfields,
 	.ops			= &st_mmap_sensor_ops,
 	.calibration_val	= 14,
@@ -166,7 +166,7 @@ const struct st_thermal_compat_data st_416mpe_cdata = {
 };
 
 /* Compatible device data stih407 thermal sensor */
-const struct st_thermal_compat_data st_407_cdata = {
+static const struct st_thermal_compat_data st_407_cdata = {
 	.reg_fields		= st_mmap_thermal_regfields,
 	.ops			= &st_mmap_sensor_ops,
 	.calibration_val	= 16,
@@ -174,19 +174,19 @@ const struct st_thermal_compat_data st_407_cdata = {
 	.crit_temp		= 120,
 };
 
-static struct of_device_id st_mmap_thermal_of_match[] = {
+static const struct of_device_id st_mmap_thermal_of_match[] = {
 	{ .compatible = "st,stih416-mpe-thermal", .data = &st_416mpe_cdata },
 	{ .compatible = "st,stih407-thermal",     .data = &st_407_cdata },
 	{ /* sentinel */ }
 };
 MODULE_DEVICE_TABLE(of, st_mmap_thermal_of_match);
 
-int st_mmap_probe(struct platform_device *pdev)
+static int st_mmap_probe(struct platform_device *pdev)
 {
 	return st_thermal_register(pdev,  st_mmap_thermal_of_match);
 }
 
-int st_mmap_remove(struct platform_device *pdev)
+static int st_mmap_remove(struct platform_device *pdev)
 {
 	return st_thermal_unregister(pdev);
 }
diff --git a/drivers/thermal/st/st_thermal_syscfg.c b/drivers/thermal/st/st_thermal_syscfg.c
index 26d36a242bb8..3df5b7890703 100644
--- a/drivers/thermal/st/st_thermal_syscfg.c
+++ b/drivers/thermal/st/st_thermal_syscfg.c
@@ -104,7 +104,7 @@ static const struct st_thermal_sensor_ops st_syscfg_sensor_ops = {
 };
 
 /* Compatible device data for stih415 sas thermal sensor */
-const struct st_thermal_compat_data st_415sas_cdata = {
+static const struct st_thermal_compat_data st_415sas_cdata = {
 	.sys_compat		= "st,stih415-front-syscfg",
 	.reg_fields		= st_415sas_regfields,
 	.ops			= &st_syscfg_sensor_ops,
@@ -114,7 +114,7 @@ const struct st_thermal_compat_data st_415sas_cdata = {
 };
 
 /* Compatible device data for stih415 mpe thermal sensor */
-const struct st_thermal_compat_data st_415mpe_cdata = {
+static const struct st_thermal_compat_data st_415mpe_cdata = {
 	.sys_compat		= "st,stih415-system-syscfg",
 	.reg_fields		= st_415mpe_regfields,
 	.ops			= &st_syscfg_sensor_ops,
@@ -124,7 +124,7 @@ const struct st_thermal_compat_data st_415mpe_cdata = {
 };
 
 /* Compatible device data for stih416 sas thermal sensor */
-const struct st_thermal_compat_data st_416sas_cdata = {
+static const struct st_thermal_compat_data st_416sas_cdata = {
 	.sys_compat		= "st,stih416-front-syscfg",
 	.reg_fields		= st_416sas_regfields,
 	.ops			= &st_syscfg_sensor_ops,
@@ -134,7 +134,7 @@ const struct st_thermal_compat_data st_416sas_cdata = {
 };
 
 /* Compatible device data for stid127 thermal sensor */
-const struct st_thermal_compat_data st_127_cdata = {
+static const struct st_thermal_compat_data st_127_cdata = {
 	.sys_compat		= "st,stid127-cpu-syscfg",
 	.reg_fields		= st_127_regfields,
 	.ops			= &st_syscfg_sensor_ops,
@@ -143,7 +143,7 @@ const struct st_thermal_compat_data st_127_cdata = {
 	.crit_temp		= 120,
 };
 
-static struct of_device_id st_syscfg_thermal_of_match[] = {
+static const struct of_device_id st_syscfg_thermal_of_match[] = {
 	{ .compatible = "st,stih415-sas-thermal", .data = &st_415sas_cdata },
 	{ .compatible = "st,stih415-mpe-thermal", .data = &st_415mpe_cdata },
 	{ .compatible = "st,stih416-sas-thermal", .data = &st_416sas_cdata },
@@ -152,12 +152,12 @@ static struct of_device_id st_syscfg_thermal_of_match[] = {
 };
 MODULE_DEVICE_TABLE(of, st_syscfg_thermal_of_match);
 
-int st_syscfg_probe(struct platform_device *pdev)
+static int st_syscfg_probe(struct platform_device *pdev)
 {
 	return st_thermal_register(pdev, st_syscfg_thermal_of_match);
 }
 
-int st_syscfg_remove(struct platform_device *pdev)
+static int st_syscfg_remove(struct platform_device *pdev)
 {
 	return st_thermal_unregister(pdev);
 }
diff --git a/drivers/thermal/thermal_core.c b/drivers/thermal/thermal_core.c
index 48491d1a81d6..4108db7e10c1 100644
--- a/drivers/thermal/thermal_core.c
+++ b/drivers/thermal/thermal_core.c
@@ -458,8 +458,10 @@ static void update_temperature(struct thermal_zone_device *tz)
 
 	ret = thermal_zone_get_temp(tz, &temp);
 	if (ret) {
-		dev_warn(&tz->device, "failed to read out thermal zone %d\n",
-			 tz->id);
+		if (ret != -EAGAIN)
+			dev_warn(&tz->device,
+				 "failed to read out thermal zone (%d)\n",
+				 ret);
 		return;
 	}
 
@@ -899,6 +901,22 @@ thermal_cooling_device_trip_point_show(struct device *dev,
 		return sprintf(buf, "%d\n", instance->trip);
 }
 
+static struct attribute *cooling_device_attrs[] = {
+	&dev_attr_cdev_type.attr,
+	&dev_attr_max_state.attr,
+	&dev_attr_cur_state.attr,
+	NULL,
+};
+
+static const struct attribute_group cooling_device_attr_group = {
+	.attrs = cooling_device_attrs,
+};
+
+static const struct attribute_group *cooling_device_attr_groups[] = {
+	&cooling_device_attr_group,
+	NULL,
+};
+
 /* Device management */
 
 /**
@@ -1130,6 +1148,7 @@ __thermal_cooling_device_register(struct device_node *np,
 	cdev->ops = ops;
 	cdev->updated = false;
 	cdev->device.class = &thermal_class;
+	cdev->device.groups = cooling_device_attr_groups;
 	cdev->devdata = devdata;
 	dev_set_name(&cdev->device, "cooling_device%d", cdev->id);
 	result = device_register(&cdev->device);
@@ -1139,21 +1158,6 @@ __thermal_cooling_device_register(struct device_node *np,
 		return ERR_PTR(result);
 	}
 
-	/* sys I/F */
-	if (type) {
-		result = device_create_file(&cdev->device, &dev_attr_cdev_type);
-		if (result)
-			goto unregister;
-	}
-
-	result = device_create_file(&cdev->device, &dev_attr_max_state);
-	if (result)
-		goto unregister;
-
-	result = device_create_file(&cdev->device, &dev_attr_cur_state);
-	if (result)
-		goto unregister;
-
 	/* Add 'this' new cdev to the global cdev list */
 	mutex_lock(&thermal_list_lock);
 	list_add(&cdev->node, &thermal_cdev_list);
@@ -1163,11 +1167,6 @@ __thermal_cooling_device_register(struct device_node *np,
 	bind_cdev(cdev);
 
 	return cdev;
-
-unregister:
-	release_idr(&thermal_cdev_idr, &thermal_idr_lock, cdev->id);
-	device_unregister(&cdev->device);
-	return ERR_PTR(result);
 }
 
 /**
diff --git a/drivers/thermal/ti-soc-thermal/ti-bandgap.c b/drivers/thermal/ti-soc-thermal/ti-bandgap.c
index 634b6ce0e63a..62a5d449c388 100644
--- a/drivers/thermal/ti-soc-thermal/ti-bandgap.c
+++ b/drivers/thermal/ti-soc-thermal/ti-bandgap.c
@@ -1402,7 +1402,7 @@ int ti_bandgap_remove(struct platform_device *pdev)
 	return 0;
 }
 
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 static int ti_bandgap_save_ctxt(struct ti_bandgap *bgp)
 {
 	int i;
diff --git a/drivers/thermal/ti-soc-thermal/ti-thermal-common.c b/drivers/thermal/ti-soc-thermal/ti-thermal-common.c
index 3fb054a10f6a..a38c1756442a 100644
--- a/drivers/thermal/ti-soc-thermal/ti-thermal-common.c
+++ b/drivers/thermal/ti-soc-thermal/ti-thermal-common.c
@@ -429,7 +429,7 @@ int ti_thermal_unregister_cpu_cooling(struct ti_bandgap *bgp, int id)
 
 	data = ti_bandgap_get_sensor_data(bgp, id);
 
-	if (data && data->cool_dev)
+	if (data)
 		cpufreq_cooling_unregister(data->cool_dev);
 
 	return 0;
diff --git a/drivers/tty/bfin_jtag_comm.c b/drivers/tty/bfin_jtag_comm.c
index d7b198c400c7..ce24182f8514 100644
--- a/drivers/tty/bfin_jtag_comm.c
+++ b/drivers/tty/bfin_jtag_comm.c
@@ -210,18 +210,6 @@ bfin_jc_chars_in_buffer(struct tty_struct *tty)
 	return circ_cnt(&bfin_jc_write_buf);
 }
 
-static void
-bfin_jc_wait_until_sent(struct tty_struct *tty, int timeout)
-{
-	unsigned long expire = jiffies + timeout;
-	while (!circ_empty(&bfin_jc_write_buf)) {
-		if (signal_pending(current))
-			break;
-		if (time_after(jiffies, expire))
-			break;
-	}
-}
-
 static const struct tty_operations bfin_jc_ops = {
 	.open            = bfin_jc_open,
 	.close           = bfin_jc_close,
@@ -230,7 +218,6 @@ static const struct tty_operations bfin_jc_ops = {
 	.flush_chars     = bfin_jc_flush_chars,
 	.write_room      = bfin_jc_write_room,
 	.chars_in_buffer = bfin_jc_chars_in_buffer,
-	.wait_until_sent = bfin_jc_wait_until_sent,
 };
 
 static int __init bfin_jc_init(void)
diff --git a/drivers/tty/serial/8250/8250_core.c b/drivers/tty/serial/8250/8250_core.c
index e3b9570a1eff..deae122c9c4b 100644
--- a/drivers/tty/serial/8250/8250_core.c
+++ b/drivers/tty/serial/8250/8250_core.c
@@ -2138,8 +2138,8 @@ int serial8250_do_startup(struct uart_port *port)
 	/*
 	 * Clear the interrupt registers.
 	 */
-	if (serial_port_in(port, UART_LSR) & UART_LSR_DR)
-		serial_port_in(port, UART_RX);
+	serial_port_in(port, UART_LSR);
+	serial_port_in(port, UART_RX);
 	serial_port_in(port, UART_IIR);
 	serial_port_in(port, UART_MSR);
 
@@ -2300,8 +2300,8 @@ dont_test_tx_en:
 	 * saved flags to avoid getting false values from polling
 	 * routines or the previous session.
 	 */
-	if (serial_port_in(port, UART_LSR) & UART_LSR_DR)
-		serial_port_in(port, UART_RX);
+	serial_port_in(port, UART_LSR);
+	serial_port_in(port, UART_RX);
 	serial_port_in(port, UART_IIR);
 	serial_port_in(port, UART_MSR);
 	up->lsr_saved_flags = 0;
@@ -2394,8 +2394,7 @@ void serial8250_do_shutdown(struct uart_port *port)
 	 * Read data port to reset things, and then unlink from
 	 * the IRQ chain.
 	 */
-	if (serial_port_in(port, UART_LSR) & UART_LSR_DR)
-		serial_port_in(port, UART_RX);
+	serial_port_in(port, UART_RX);
 	serial8250_rpm_put(up);
 
 	del_timer_sync(&up->timer);
diff --git a/drivers/tty/serial/8250/8250_dw.c b/drivers/tty/serial/8250/8250_dw.c
index e60116235836..6ae5b8560e4d 100644
--- a/drivers/tty/serial/8250/8250_dw.c
+++ b/drivers/tty/serial/8250/8250_dw.c
@@ -59,6 +59,8 @@ struct dw8250_data {
 	u8			usr_reg;
 	int			last_mcr;
 	int			line;
+	int			msr_mask_on;
+	int			msr_mask_off;
 	struct clk		*clk;
 	struct clk		*pclk;
 	struct reset_control	*rst;
@@ -81,6 +83,12 @@ static inline int dw8250_modify_msr(struct uart_port *p, int offset, int value)
 		value &= ~UART_MSR_DCTS;
 	}
 
+	/* Override any modem control signals if needed */
+	if (offset == UART_MSR) {
+		value |= d->msr_mask_on;
+		value &= ~d->msr_mask_off;
+	}
+
 	return value;
 }
 
@@ -111,7 +119,10 @@ static void dw8250_serial_out(struct uart_port *p, int offset, int value)
 			dw8250_force_idle(p);
 			writeb(value, p->membase + (UART_LCR << p->regshift));
 		}
-		dev_err(p->dev, "Couldn't set LCR to %d\n", value);
+		/*
+		 * FIXME: this deadlocks if port->lock is already held
+		 * dev_err(p->dev, "Couldn't set LCR to %d\n", value);
+		 */
 	}
 }
 
@@ -155,7 +166,10 @@ static void dw8250_serial_outq(struct uart_port *p, int offset, int value)
 			__raw_writeq(value & 0xff,
 				     p->membase + (UART_LCR << p->regshift));
 		}
-		dev_err(p->dev, "Couldn't set LCR to %d\n", value);
+		/*
+		 * FIXME: this deadlocks if port->lock is already held
+		 * dev_err(p->dev, "Couldn't set LCR to %d\n", value);
+		 */
 	}
 }
 #endif /* CONFIG_64BIT */
@@ -179,7 +193,10 @@ static void dw8250_serial_out32(struct uart_port *p, int offset, int value)
 			dw8250_force_idle(p);
 			writel(value, p->membase + (UART_LCR << p->regshift));
 		}
-		dev_err(p->dev, "Couldn't set LCR to %d\n", value);
+		/*
+		 * FIXME: this deadlocks if port->lock is already held
+		 * dev_err(p->dev, "Couldn't set LCR to %d\n", value);
+		 */
 	}
 }
 
@@ -334,6 +351,30 @@ static int dw8250_probe_of(struct uart_port *p,
 	if (id >= 0)
 		p->line = id;
 
+	if (of_property_read_bool(np, "dcd-override")) {
+		/* Always report DCD as active */
+		data->msr_mask_on |= UART_MSR_DCD;
+		data->msr_mask_off |= UART_MSR_DDCD;
+	}
+
+	if (of_property_read_bool(np, "dsr-override")) {
+		/* Always report DSR as active */
+		data->msr_mask_on |= UART_MSR_DSR;
+		data->msr_mask_off |= UART_MSR_DDSR;
+	}
+
+	if (of_property_read_bool(np, "cts-override")) {
+		/* Always report DSR as active */
+		data->msr_mask_on |= UART_MSR_DSR;
+		data->msr_mask_off |= UART_MSR_DDSR;
+	}
+
+	if (of_property_read_bool(np, "ri-override")) {
+		/* Always report Ring indicator as inactive */
+		data->msr_mask_off |= UART_MSR_RI;
+		data->msr_mask_off |= UART_MSR_TERI;
+	}
+
 	/* clock got configured through clk api, all done */
 	if (p->uartclk)
 		return 0;
diff --git a/drivers/tty/serial/8250/8250_pci.c b/drivers/tty/serial/8250/8250_pci.c
index daf2c82984e9..892eb32cdef4 100644
--- a/drivers/tty/serial/8250/8250_pci.c
+++ b/drivers/tty/serial/8250/8250_pci.c
@@ -69,7 +69,7 @@ static void moan_device(const char *str, struct pci_dev *dev)
 	       "Please send the output of lspci -vv, this\n"
 	       "message (0x%04x,0x%04x,0x%04x,0x%04x), the\n"
 	       "manufacturer and name of serial board or\n"
-	       "modem board to rmk+serial@arm.linux.org.uk.\n",
+	       "modem board to <linux-serial@vger.kernel.org>.\n",
 	       pci_name(dev), str, dev->vendor, dev->device,
 	       dev->subsystem_vendor, dev->subsystem_device);
 }
@@ -1989,13 +1989,6 @@ static struct pci_serial_quirk pci_serial_quirks[] __refdata = {
 	},
 	{
 		.vendor		= PCI_VENDOR_ID_INTEL,
-		.device		= PCI_DEVICE_ID_INTEL_QRK_UART,
-		.subvendor	= PCI_ANY_ID,
-		.subdevice	= PCI_ANY_ID,
-		.setup		= pci_default_setup,
-	},
-	{
-		.vendor		= PCI_VENDOR_ID_INTEL,
 		.device		= PCI_DEVICE_ID_INTEL_BSW_UART1,
 		.subvendor	= PCI_ANY_ID,
 		.subdevice	= PCI_ANY_ID,
@@ -2201,13 +2194,6 @@ static struct pci_serial_quirk pci_serial_quirks[] __refdata = {
 	 */
 	{
 		.vendor		= PCI_VENDOR_ID_PLX,
-		.device		= PCI_DEVICE_ID_PLX_9030,
-		.subvendor	= PCI_SUBVENDOR_ID_PERLE,
-		.subdevice	= PCI_ANY_ID,
-		.setup		= pci_default_setup,
-	},
-	{
-		.vendor		= PCI_VENDOR_ID_PLX,
 		.device		= PCI_DEVICE_ID_PLX_9050,
 		.subvendor	= PCI_SUBVENDOR_ID_EXSYS,
 		.subdevice	= PCI_SUBDEVICE_ID_EXSYS_4055,
@@ -5415,10 +5401,6 @@ static struct pci_device_id serial_pci_tbl[] = {
 		PCI_ANY_ID, PCI_ANY_ID,
 		0, 0, pbn_b0_bt_2_115200 },
 
-	{	PCI_VENDOR_ID_WCH, PCI_DEVICE_ID_WCH_CH352_2S,
-		PCI_ANY_ID, PCI_ANY_ID,
-		0, 0, pbn_b0_bt_2_115200 },
-
 	{	PCIE_VENDOR_ID_WCH, PCIE_DEVICE_ID_WCH_CH384_4S,
 		PCI_ANY_ID, PCI_ANY_ID,
 		0, 0, pbn_wch384_4 },
diff --git a/drivers/tty/serial/atmel_serial.c b/drivers/tty/serial/atmel_serial.c
index 846552bff67d..4e959c43f680 100644
--- a/drivers/tty/serial/atmel_serial.c
+++ b/drivers/tty/serial/atmel_serial.c
@@ -47,6 +47,7 @@
 #include <linux/gpio/consumer.h>
 #include <linux/err.h>
 #include <linux/irq.h>
+#include <linux/suspend.h>
 
 #include <asm/io.h>
 #include <asm/ioctls.h>
@@ -173,6 +174,12 @@ struct atmel_uart_port {
 	bool			ms_irq_enabled;
 	bool			is_usart;	/* usart or uart */
 	struct timer_list	uart_timer;	/* uart timer */
+
+	bool			suspended;
+	unsigned int		pending;
+	unsigned int		pending_status;
+	spinlock_t		lock_suspended;
+
 	int (*prepare_rx)(struct uart_port *port);
 	int (*prepare_tx)(struct uart_port *port);
 	void (*schedule_rx)(struct uart_port *port);
@@ -1179,12 +1186,15 @@ static irqreturn_t atmel_interrupt(int irq, void *dev_id)
 {
 	struct uart_port *port = dev_id;
 	struct atmel_uart_port *atmel_port = to_atmel_uart_port(port);
-	unsigned int status, pending, pass_counter = 0;
+	unsigned int status, pending, mask, pass_counter = 0;
 	bool gpio_handled = false;
 
+	spin_lock(&atmel_port->lock_suspended);
+
 	do {
 		status = atmel_get_lines_status(port);
-		pending = status & UART_GET_IMR(port);
+		mask = UART_GET_IMR(port);
+		pending = status & mask;
 		if (!gpio_handled) {
 			/*
 			 * Dealing with GPIO interrupt
@@ -1206,11 +1216,21 @@ static irqreturn_t atmel_interrupt(int irq, void *dev_id)
 		if (!pending)
 			break;
 
+		if (atmel_port->suspended) {
+			atmel_port->pending |= pending;
+			atmel_port->pending_status = status;
+			UART_PUT_IDR(port, mask);
+			pm_system_wakeup();
+			break;
+		}
+
 		atmel_handle_receive(port, pending);
 		atmel_handle_status(port, pending, status);
 		atmel_handle_transmit(port, pending);
 	} while (pass_counter++ < ATMEL_ISR_PASS_LIMIT);
 
+	spin_unlock(&atmel_port->lock_suspended);
+
 	return pass_counter ? IRQ_HANDLED : IRQ_NONE;
 }
 
@@ -1742,7 +1762,8 @@ static int atmel_startup(struct uart_port *port)
 	/*
 	 * Allocate the IRQ
 	 */
-	retval = request_irq(port->irq, atmel_interrupt, IRQF_SHARED,
+	retval = request_irq(port->irq, atmel_interrupt,
+			IRQF_SHARED | IRQF_COND_SUSPEND,
 			tty ? tty->name : "atmel_serial", port);
 	if (retval) {
 		dev_err(port->dev, "atmel_startup - Can't get irq\n");
@@ -2513,8 +2534,14 @@ static int atmel_serial_suspend(struct platform_device *pdev,
 
 	/* we can not wake up if we're running on slow clock */
 	atmel_port->may_wakeup = device_may_wakeup(&pdev->dev);
-	if (atmel_serial_clk_will_stop())
+	if (atmel_serial_clk_will_stop()) {
+		unsigned long flags;
+
+		spin_lock_irqsave(&atmel_port->lock_suspended, flags);
+		atmel_port->suspended = true;
+		spin_unlock_irqrestore(&atmel_port->lock_suspended, flags);
 		device_set_wakeup_enable(&pdev->dev, 0);
+	}
 
 	uart_suspend_port(&atmel_uart, port);
 
@@ -2525,6 +2552,18 @@ static int atmel_serial_resume(struct platform_device *pdev)
 {
 	struct uart_port *port = platform_get_drvdata(pdev);
 	struct atmel_uart_port *atmel_port = to_atmel_uart_port(port);
+	unsigned long flags;
+
+	spin_lock_irqsave(&atmel_port->lock_suspended, flags);
+	if (atmel_port->pending) {
+		atmel_handle_receive(port, atmel_port->pending);
+		atmel_handle_status(port, atmel_port->pending,
+				    atmel_port->pending_status);
+		atmel_handle_transmit(port, atmel_port->pending);
+		atmel_port->pending = 0;
+	}
+	atmel_port->suspended = false;
+	spin_unlock_irqrestore(&atmel_port->lock_suspended, flags);
 
 	uart_resume_port(&atmel_uart, port);
 	device_set_wakeup_enable(&pdev->dev, atmel_port->may_wakeup);
@@ -2593,6 +2632,8 @@ static int atmel_serial_probe(struct platform_device *pdev)
 	port->backup_imr = 0;
 	port->uart.line = ret;
 
+	spin_lock_init(&port->lock_suspended);
+
 	ret = atmel_init_gpios(port, &pdev->dev);
 	if (ret < 0)
 		dev_err(&pdev->dev, "%s",
diff --git a/drivers/tty/serial/fsl_lpuart.c b/drivers/tty/serial/fsl_lpuart.c
index b1893f3f88f1..3ad1458bfeb0 100644
--- a/drivers/tty/serial/fsl_lpuart.c
+++ b/drivers/tty/serial/fsl_lpuart.c
@@ -921,6 +921,9 @@ static void lpuart_setup_watermark(struct lpuart_port *sport)
 	writeb(val | UARTPFIFO_TXFE | UARTPFIFO_RXFE,
 			sport->port.membase + UARTPFIFO);
 
+	/* explicitly clear RDRF */
+	readb(sport->port.membase + UARTSR1);
+
 	/* flush Tx and Rx FIFO */
 	writeb(UARTCFIFO_TXFLUSH | UARTCFIFO_RXFLUSH,
 			sport->port.membase + UARTCFIFO);
@@ -1076,6 +1079,8 @@ static int lpuart_startup(struct uart_port *port)
 	sport->txfifo_size = 0x1 << (((temp >> UARTPFIFO_TXSIZE_OFF) &
 		UARTPFIFO_FIFOSIZE_MASK) + 1);
 
+	sport->port.fifosize = sport->txfifo_size;
+
 	sport->rxfifo_size = 0x1 << (((temp >> UARTPFIFO_RXSIZE_OFF) &
 		UARTPFIFO_FIFOSIZE_MASK) + 1);
 
diff --git a/drivers/tty/serial/of_serial.c b/drivers/tty/serial/of_serial.c
index 7ff61e24a195..33fb94f78967 100644
--- a/drivers/tty/serial/of_serial.c
+++ b/drivers/tty/serial/of_serial.c
@@ -133,10 +133,6 @@ static int of_platform_serial_setup(struct platform_device *ofdev,
 	if (of_find_property(np, "no-loopback-test", NULL))
 		port->flags |= UPF_SKIP_TEST;
 
-	ret = of_alias_get_id(np, "serial");
-	if (ret >= 0)
-		port->line = ret;
-
 	port->dev = &ofdev->dev;
 
 	switch (type) {
diff --git a/drivers/tty/serial/samsung.c b/drivers/tty/serial/samsung.c
index af821a908720..cf08876922f1 100644
--- a/drivers/tty/serial/samsung.c
+++ b/drivers/tty/serial/samsung.c
@@ -963,6 +963,7 @@ static void s3c24xx_serial_shutdown(struct uart_port *port)
 			free_irq(ourport->tx_irq, ourport);
 		tx_enabled(port) = 0;
 		ourport->tx_claimed = 0;
+		ourport->tx_mode = 0;
 	}
 
 	if (ourport->rx_claimed) {
diff --git a/drivers/tty/serial/sprd_serial.c b/drivers/tty/serial/sprd_serial.c
index 594b63331ef4..bca975f5093b 100644
--- a/drivers/tty/serial/sprd_serial.c
+++ b/drivers/tty/serial/sprd_serial.c
@@ -293,8 +293,10 @@ static irqreturn_t sprd_handle_irq(int irq, void *dev_id)
 
 	ims = serial_in(port, SPRD_IMSR);
 
-	if (!ims)
+	if (!ims) {
+		spin_unlock(&port->lock);
 		return IRQ_NONE;
+	}
 
 	serial_out(port, SPRD_ICLR, ~0);
 
diff --git a/drivers/tty/sysrq.c b/drivers/tty/sysrq.c
index 259a4d5a4e8f..843f2cdc280b 100644
--- a/drivers/tty/sysrq.c
+++ b/drivers/tty/sysrq.c
@@ -275,6 +275,7 @@ static struct sysrq_key_op sysrq_showregs_op = {
 static void sysrq_handle_showstate(int key)
 {
 	show_state();
+	show_workqueue_state();
 }
 static struct sysrq_key_op sysrq_showstate_op = {
 	.handler	= sysrq_handle_showstate,
diff --git a/drivers/tty/tty_io.c b/drivers/tty/tty_io.c
index 51f066aa375e..2bb4dfc02873 100644
--- a/drivers/tty/tty_io.c
+++ b/drivers/tty/tty_io.c
@@ -1028,8 +1028,8 @@ EXPORT_SYMBOL(start_tty);
 /* We limit tty time update visibility to every 8 seconds or so. */
 static void tty_update_time(struct timespec *time)
 {
-	unsigned long sec = get_seconds() & ~7;
-	if ((long)(sec - time->tv_sec) > 0)
+	unsigned long sec = get_seconds();
+	if (abs(sec - time->tv_sec) & ~7)
 		time->tv_sec = sec;
 }
 
diff --git a/drivers/tty/tty_ioctl.c b/drivers/tty/tty_ioctl.c
index a5cf253b2544..632fc8152061 100644
--- a/drivers/tty/tty_ioctl.c
+++ b/drivers/tty/tty_ioctl.c
@@ -217,11 +217,17 @@ void tty_wait_until_sent(struct tty_struct *tty, long timeout)
 #endif
 	if (!timeout)
 		timeout = MAX_SCHEDULE_TIMEOUT;
-	if (wait_event_interruptible_timeout(tty->write_wait,
-			!tty_chars_in_buffer(tty), timeout) >= 0) {
-		if (tty->ops->wait_until_sent)
-			tty->ops->wait_until_sent(tty, timeout);
-	}
+
+	timeout = wait_event_interruptible_timeout(tty->write_wait,
+			!tty_chars_in_buffer(tty), timeout);
+	if (timeout <= 0)
+		return;
+
+	if (timeout == MAX_SCHEDULE_TIMEOUT)
+		timeout = 0;
+
+	if (tty->ops->wait_until_sent)
+		tty->ops->wait_until_sent(tty, timeout);
 }
 EXPORT_SYMBOL(tty_wait_until_sent);
 
diff --git a/drivers/usb/Makefile b/drivers/usb/Makefile
index 2f1e2aa42b44..d8926c6cd2a8 100644
--- a/drivers/usb/Makefile
+++ b/drivers/usb/Makefile
@@ -5,6 +5,7 @@
 # Object files in subdirectories
 
 obj-$(CONFIG_USB)		+= core/
+obj-$(CONFIG_USB_SUPPORT)	+= phy/
 
 obj-$(CONFIG_USB_DWC3)		+= dwc3/
 obj-$(CONFIG_USB_DWC2)		+= dwc2/
@@ -48,7 +49,6 @@ obj-$(CONFIG_USB_MICROTEK)	+= image/
 obj-$(CONFIG_USB_SERIAL)	+= serial/
 
 obj-$(CONFIG_USB)		+= misc/
-obj-$(CONFIG_USB_SUPPORT)	+= phy/
 obj-$(CONFIG_EARLY_PRINTK_DBGP)	+= early/
 
 obj-$(CONFIG_USB_ATM)		+= atm/
diff --git a/drivers/usb/atm/ueagle-atm.c b/drivers/usb/atm/ueagle-atm.c
index 5a459377574b..888998a7fe31 100644
--- a/drivers/usb/atm/ueagle-atm.c
+++ b/drivers/usb/atm/ueagle-atm.c
@@ -952,7 +952,7 @@ static void uea_load_page_e1(struct work_struct *work)
 	int i;
 
 	/* reload firmware when reboot start and it's loaded already */
-	if (ovl == 0 && pageno == 0 && sc->dsp_firm) {
+	if (ovl == 0 && pageno == 0) {
 		release_firmware(sc->dsp_firm);
 		sc->dsp_firm = NULL;
 	}
@@ -1074,7 +1074,7 @@ static void uea_load_page_e4(struct work_struct *work)
 	uea_dbg(INS_TO_USBDEV(sc), "sending DSP page %u\n", pageno);
 
 	/* reload firmware when reboot start and it's loaded already */
-	if (pageno == 0 && sc->dsp_firm) {
+	if (pageno == 0) {
 		release_firmware(sc->dsp_firm);
 		sc->dsp_firm = NULL;
 	}
diff --git a/drivers/usb/c67x00/c67x00-hcd.c b/drivers/usb/c67x00/c67x00-hcd.c
index 20ec4eee1ac8..c2d13968da82 100644
--- a/drivers/usb/c67x00/c67x00-hcd.c
+++ b/drivers/usb/c67x00/c67x00-hcd.c
@@ -34,7 +34,7 @@
 
 static __u8 c67x00_hub_des[] = {
 	0x09,			/*  __u8  bLength; */
-	0x29,			/*  __u8  bDescriptorType; Hub-descriptor */
+	USB_DT_HUB,		/*  __u8  bDescriptorType; Hub-descriptor */
 	0x02,			/*  __u8  bNbrPorts; */
 	0x00,			/* __u16  wHubCharacteristics; */
 	0x00,			/*   (per-port OC, no power switching) */
diff --git a/drivers/usb/chipidea/Kconfig b/drivers/usb/chipidea/Kconfig
index 77b47d82c9a6..5ce3f1d6a6ed 100644
--- a/drivers/usb/chipidea/Kconfig
+++ b/drivers/usb/chipidea/Kconfig
@@ -10,6 +10,17 @@ config USB_CHIPIDEA
 
 if USB_CHIPIDEA
 
+config USB_CHIPIDEA_OF
+	tristate
+	depends on OF
+	default USB_CHIPIDEA
+
+config USB_CHIPIDEA_PCI
+	tristate
+	depends on PCI
+	depends on NOP_USB_XCEIV
+	default USB_CHIPIDEA
+
 config USB_CHIPIDEA_UDC
 	bool "ChipIdea device controller"
 	depends on USB_GADGET
diff --git a/drivers/usb/chipidea/Makefile b/drivers/usb/chipidea/Makefile
index 1fc86a2ca22d..4decb12f2578 100644
--- a/drivers/usb/chipidea/Makefile
+++ b/drivers/usb/chipidea/Makefile
@@ -14,11 +14,6 @@ obj-$(CONFIG_USB_CHIPIDEA)	+= ci_hdrc_usb2.o
 obj-$(CONFIG_USB_CHIPIDEA)	+= ci_hdrc_msm.o
 obj-$(CONFIG_USB_CHIPIDEA)	+= ci_hdrc_zevio.o
 
-# PCI doesn't provide stubs, need to check
-ifneq ($(CONFIG_PCI),)
-	obj-$(CONFIG_USB_CHIPIDEA)	+= ci_hdrc_pci.o
-endif
+obj-$(CONFIG_USB_CHIPIDEA_PCI)	+= ci_hdrc_pci.o
 
-ifneq ($(CONFIG_OF),)
-	obj-$(CONFIG_USB_CHIPIDEA)	+= usbmisc_imx.o ci_hdrc_imx.o
-endif
+obj-$(CONFIG_USB_CHIPIDEA_OF)	+= usbmisc_imx.o ci_hdrc_imx.o
diff --git a/drivers/usb/chipidea/bits.h b/drivers/usb/chipidea/bits.h
index ca57e3dcd3d5..3cb9bda51ddf 100644
--- a/drivers/usb/chipidea/bits.h
+++ b/drivers/usb/chipidea/bits.h
@@ -15,6 +15,16 @@
 
 #include <linux/usb/ehci_def.h>
 
+/*
+ * ID
+ * For 1.x revision, bit24 - bit31 are reserved
+ * For 2.x revision, bit25 - bit28 are 0x2
+ */
+#define TAG		      (0x1F << 16)
+#define REVISION	      (0xF << 21)
+#define VERSION		      (0xF << 25)
+#define CIVERSION	      (0x7 << 29)
+
 /* HCCPARAMS */
 #define HCCPARAMS_LEN         BIT(17)
 
@@ -53,6 +63,7 @@
 #define PORTSC_HSP            BIT(9)
 #define PORTSC_PP             BIT(12)
 #define PORTSC_PTC            (0x0FUL << 16)
+#define PORTSC_WKCN           BIT(20)
 #define PORTSC_PHCD(d)	      ((d) ? BIT(22) : BIT(23))
 /* PTS and PTW for non lpm version only */
 #define PORTSC_PFSC           BIT(24)
diff --git a/drivers/usb/chipidea/ci.h b/drivers/usb/chipidea/ci.h
index 65913d48f0c8..6d6200e37b71 100644
--- a/drivers/usb/chipidea/ci.h
+++ b/drivers/usb/chipidea/ci.h
@@ -29,6 +29,15 @@
 /******************************************************************************
  * REGISTERS
  *****************************************************************************/
+/* Identification Registers */
+#define ID_ID				0x0
+#define ID_HWGENERAL			0x4
+#define ID_HWHOST			0x8
+#define ID_HWDEVICE			0xc
+#define ID_HWTXBUF			0x10
+#define ID_HWRXBUF			0x14
+#define ID_SBUSCFG			0x90
+
 /* register indices */
 enum ci_hw_regs {
 	CAP_CAPLENGTH,
@@ -97,6 +106,18 @@ enum ci_role {
 	CI_ROLE_END,
 };
 
+enum ci_revision {
+	CI_REVISION_1X = 10,	/* Revision 1.x */
+	CI_REVISION_20 = 20, /* Revision 2.0 */
+	CI_REVISION_21, /* Revision 2.1 */
+	CI_REVISION_22, /* Revision 2.2 */
+	CI_REVISION_23, /* Revision 2.3 */
+	CI_REVISION_24, /* Revision 2.4 */
+	CI_REVISION_25, /* Revision 2.5 */
+	CI_REVISION_25_PLUS, /* Revision above than 2.5 */
+	CI_REVISION_UNKNOWN = 99, /* Unknown Revision */
+};
+
 /**
  * struct ci_role_driver - host/gadget role driver
  * @start: start this role
@@ -141,7 +162,10 @@ struct hw_bank {
  * @role: current role
  * @is_otg: if the device is otg-capable
  * @fsm: otg finite state machine
- * @fsm_timer: pointer to timer list of otg fsm
+ * @otg_fsm_hrtimer: hrtimer for otg fsm timers
+ * @hr_timeouts: time out list for active otg fsm timers
+ * @enabled_otg_timer_bits: bits of enabled otg timers
+ * @next_otg_timer: next nearest enabled timer to be expired
  * @work: work for role changing
  * @wq: workqueue thread
  * @qh_pool: allocation pool for queue heads
@@ -169,6 +193,10 @@ struct hw_bank {
  * @b_sess_valid_event: indicates there is a vbus event, and handled
  * at ci_otg_work
  * @imx28_write_fix: Freescale imx28 needs swp instruction for writing
+ * @supports_runtime_pm: if runtime pm is supported
+ * @in_lpm: if the core in low power mode
+ * @wakeup_int: if wakeup interrupt occur
+ * @rev: The revision number for controller
  */
 struct ci_hdrc {
 	struct device			*dev;
@@ -180,7 +208,10 @@ struct ci_hdrc {
 	bool				is_otg;
 	struct usb_otg			otg;
 	struct otg_fsm			fsm;
-	struct ci_otg_fsm_timer_list	*fsm_timer;
+	struct hrtimer			otg_fsm_hrtimer;
+	ktime_t				hr_timeouts[NUM_OTG_FSM_TIMERS];
+	unsigned			enabled_otg_timer_bits;
+	enum otg_fsm_timer		next_otg_timer;
 	struct work_struct		work;
 	struct workqueue_struct		*wq;
 
@@ -211,6 +242,10 @@ struct ci_hdrc {
 	bool				id_event;
 	bool				b_sess_valid_event;
 	bool				imx28_write_fix;
+	bool				supports_runtime_pm;
+	bool				in_lpm;
+	bool				wakeup_int;
+	enum ci_revision		rev;
 };
 
 static inline struct ci_role_driver *ci_role(struct ci_hdrc *ci)
@@ -248,6 +283,36 @@ static inline void ci_role_stop(struct ci_hdrc *ci)
 }
 
 /**
+ * hw_read_id_reg: reads from a identification register
+ * @ci: the controller
+ * @offset: offset from the beginning of identification registers region
+ * @mask: bitfield mask
+ *
+ * This function returns register contents
+ */
+static inline u32 hw_read_id_reg(struct ci_hdrc *ci, u32 offset, u32 mask)
+{
+	return ioread32(ci->hw_bank.abs + offset) & mask;
+}
+
+/**
+ * hw_write_id_reg: writes to a identification register
+ * @ci: the controller
+ * @offset: offset from the beginning of identification registers region
+ * @mask: bitfield mask
+ * @data: new value
+ */
+static inline void hw_write_id_reg(struct ci_hdrc *ci, u32 offset,
+			    u32 mask, u32 data)
+{
+	if (~mask)
+		data = (ioread32(ci->hw_bank.abs + offset) & ~mask)
+			| (data & mask);
+
+	iowrite32(data, ci->hw_bank.abs + offset);
+}
+
+/**
  * hw_read: reads from a hw register
  * @ci: the controller
  * @reg:  register index
diff --git a/drivers/usb/chipidea/ci_hdrc_imx.c b/drivers/usb/chipidea/ci_hdrc_imx.c
index 0f05de7c6b6c..389f0e034259 100644
--- a/drivers/usb/chipidea/ci_hdrc_imx.c
+++ b/drivers/usb/chipidea/ci_hdrc_imx.c
@@ -23,22 +23,40 @@
 #include "ci.h"
 #include "ci_hdrc_imx.h"
 
-#define CI_HDRC_IMX_IMX28_WRITE_FIX BIT(0)
-
 struct ci_hdrc_imx_platform_flag {
 	unsigned int flags;
+	bool runtime_pm;
 };
 
 static const struct ci_hdrc_imx_platform_flag imx27_usb_data = {
 };
 
 static const struct ci_hdrc_imx_platform_flag imx28_usb_data = {
-	.flags = CI_HDRC_IMX_IMX28_WRITE_FIX,
+	.flags = CI_HDRC_IMX28_WRITE_FIX |
+		CI_HDRC_TURN_VBUS_EARLY_ON,
+};
+
+static const struct ci_hdrc_imx_platform_flag imx6q_usb_data = {
+	.flags = CI_HDRC_SUPPORTS_RUNTIME_PM |
+		CI_HDRC_TURN_VBUS_EARLY_ON,
+};
+
+static const struct ci_hdrc_imx_platform_flag imx6sl_usb_data = {
+	.flags = CI_HDRC_SUPPORTS_RUNTIME_PM |
+		CI_HDRC_TURN_VBUS_EARLY_ON,
+};
+
+static const struct ci_hdrc_imx_platform_flag imx6sx_usb_data = {
+	.flags = CI_HDRC_SUPPORTS_RUNTIME_PM |
+		CI_HDRC_TURN_VBUS_EARLY_ON,
 };
 
 static const struct of_device_id ci_hdrc_imx_dt_ids[] = {
 	{ .compatible = "fsl,imx28-usb", .data = &imx28_usb_data},
 	{ .compatible = "fsl,imx27-usb", .data = &imx27_usb_data},
+	{ .compatible = "fsl,imx6q-usb", .data = &imx6q_usb_data},
+	{ .compatible = "fsl,imx6sl-usb", .data = &imx6sl_usb_data},
+	{ .compatible = "fsl,imx6sx-usb", .data = &imx6sl_usb_data},
 	{ /* sentinel */ }
 };
 MODULE_DEVICE_TABLE(of, ci_hdrc_imx_dt_ids);
@@ -48,6 +66,8 @@ struct ci_hdrc_imx_data {
 	struct platform_device *ci_pdev;
 	struct clk *clk;
 	struct imx_usbmisc_data *usbmisc_data;
+	bool supports_runtime_pm;
+	bool in_lpm;
 };
 
 /* Common functions shared by usbmisc drivers */
@@ -145,21 +165,18 @@ static int ci_hdrc_imx_probe(struct platform_device *pdev)
 	}
 
 	pdata.usb_phy = data->phy;
-
-	if (imx_platform_flag->flags & CI_HDRC_IMX_IMX28_WRITE_FIX)
-		pdata.flags |= CI_HDRC_IMX28_WRITE_FIX;
+	pdata.flags |= imx_platform_flag->flags;
+	if (pdata.flags & CI_HDRC_SUPPORTS_RUNTIME_PM)
+		data->supports_runtime_pm = true;
 
 	ret = dma_coerce_mask_and_coherent(&pdev->dev, DMA_BIT_MASK(32));
 	if (ret)
 		goto err_clk;
 
-	if (data->usbmisc_data) {
-		ret = imx_usbmisc_init(data->usbmisc_data);
-		if (ret) {
-			dev_err(&pdev->dev, "usbmisc init failed, ret=%d\n",
-					ret);
-			goto err_clk;
-		}
+	ret = imx_usbmisc_init(data->usbmisc_data);
+	if (ret) {
+		dev_err(&pdev->dev, "usbmisc init failed, ret=%d\n", ret);
+		goto err_clk;
 	}
 
 	data->ci_pdev = ci_hdrc_add_device(&pdev->dev,
@@ -173,19 +190,20 @@ static int ci_hdrc_imx_probe(struct platform_device *pdev)
 		goto err_clk;
 	}
 
-	if (data->usbmisc_data) {
-		ret = imx_usbmisc_init_post(data->usbmisc_data);
-		if (ret) {
-			dev_err(&pdev->dev, "usbmisc post failed, ret=%d\n",
-					ret);
-			goto disable_device;
-		}
+	ret = imx_usbmisc_init_post(data->usbmisc_data);
+	if (ret) {
+		dev_err(&pdev->dev, "usbmisc post failed, ret=%d\n", ret);
+		goto disable_device;
 	}
 
 	platform_set_drvdata(pdev, data);
 
-	pm_runtime_no_callbacks(&pdev->dev);
-	pm_runtime_enable(&pdev->dev);
+	if (data->supports_runtime_pm) {
+		pm_runtime_set_active(&pdev->dev);
+		pm_runtime_enable(&pdev->dev);
+	}
+
+	device_set_wakeup_capable(&pdev->dev, true);
 
 	return 0;
 
@@ -200,14 +218,18 @@ static int ci_hdrc_imx_remove(struct platform_device *pdev)
 {
 	struct ci_hdrc_imx_data *data = platform_get_drvdata(pdev);
 
-	pm_runtime_disable(&pdev->dev);
+	if (data->supports_runtime_pm) {
+		pm_runtime_get_sync(&pdev->dev);
+		pm_runtime_disable(&pdev->dev);
+		pm_runtime_put_noidle(&pdev->dev);
+	}
 	ci_hdrc_remove_device(data->ci_pdev);
 	clk_disable_unprepare(data->clk);
 
 	return 0;
 }
 
-#ifdef CONFIG_PM_SLEEP
+#ifdef CONFIG_PM
 static int imx_controller_suspend(struct device *dev)
 {
 	struct ci_hdrc_imx_data *data = dev_get_drvdata(dev);
@@ -215,6 +237,7 @@ static int imx_controller_suspend(struct device *dev)
 	dev_dbg(dev, "at %s\n", __func__);
 
 	clk_disable_unprepare(data->clk);
+	data->in_lpm = true;
 
 	return 0;
 }
@@ -222,25 +245,103 @@ static int imx_controller_suspend(struct device *dev)
 static int imx_controller_resume(struct device *dev)
 {
 	struct ci_hdrc_imx_data *data = dev_get_drvdata(dev);
+	int ret = 0;
 
 	dev_dbg(dev, "at %s\n", __func__);
 
-	return clk_prepare_enable(data->clk);
+	if (!data->in_lpm) {
+		WARN_ON(1);
+		return 0;
+	}
+
+	ret = clk_prepare_enable(data->clk);
+	if (ret)
+		return ret;
+
+	data->in_lpm = false;
+
+	ret = imx_usbmisc_set_wakeup(data->usbmisc_data, false);
+	if (ret) {
+		dev_err(dev, "usbmisc set_wakeup failed, ret=%d\n", ret);
+		goto clk_disable;
+	}
+
+	return 0;
+
+clk_disable:
+	clk_disable_unprepare(data->clk);
+	return ret;
 }
 
+#ifdef CONFIG_PM_SLEEP
 static int ci_hdrc_imx_suspend(struct device *dev)
 {
+	int ret;
+
+	struct ci_hdrc_imx_data *data = dev_get_drvdata(dev);
+
+	if (data->in_lpm)
+		/* The core's suspend doesn't run */
+		return 0;
+
+	if (device_may_wakeup(dev)) {
+		ret = imx_usbmisc_set_wakeup(data->usbmisc_data, true);
+		if (ret) {
+			dev_err(dev, "usbmisc set_wakeup failed, ret=%d\n",
+					ret);
+			return ret;
+		}
+	}
+
 	return imx_controller_suspend(dev);
 }
 
 static int ci_hdrc_imx_resume(struct device *dev)
 {
-	return imx_controller_resume(dev);
+	struct ci_hdrc_imx_data *data = dev_get_drvdata(dev);
+	int ret;
+
+	ret = imx_controller_resume(dev);
+	if (!ret && data->supports_runtime_pm) {
+		pm_runtime_disable(dev);
+		pm_runtime_set_active(dev);
+		pm_runtime_enable(dev);
+	}
+
+	return ret;
 }
 #endif /* CONFIG_PM_SLEEP */
 
+static int ci_hdrc_imx_runtime_suspend(struct device *dev)
+{
+	struct ci_hdrc_imx_data *data = dev_get_drvdata(dev);
+	int ret;
+
+	if (data->in_lpm) {
+		WARN_ON(1);
+		return 0;
+	}
+
+	ret = imx_usbmisc_set_wakeup(data->usbmisc_data, true);
+	if (ret) {
+		dev_err(dev, "usbmisc set_wakeup failed, ret=%d\n", ret);
+		return ret;
+	}
+
+	return imx_controller_suspend(dev);
+}
+
+static int ci_hdrc_imx_runtime_resume(struct device *dev)
+{
+	return imx_controller_resume(dev);
+}
+
+#endif /* CONFIG_PM */
+
 static const struct dev_pm_ops ci_hdrc_imx_pm_ops = {
 	SET_SYSTEM_SLEEP_PM_OPS(ci_hdrc_imx_suspend, ci_hdrc_imx_resume)
+	SET_RUNTIME_PM_OPS(ci_hdrc_imx_runtime_suspend,
+			ci_hdrc_imx_runtime_resume, NULL)
 };
 static struct platform_driver ci_hdrc_imx_driver = {
 	.probe = ci_hdrc_imx_probe,
diff --git a/drivers/usb/chipidea/ci_hdrc_imx.h b/drivers/usb/chipidea/ci_hdrc_imx.h
index 4ed828f75a1e..635717e9354a 100644
--- a/drivers/usb/chipidea/ci_hdrc_imx.h
+++ b/drivers/usb/chipidea/ci_hdrc_imx.h
@@ -22,5 +22,6 @@ struct imx_usbmisc_data {
 
 int imx_usbmisc_init(struct imx_usbmisc_data *);
 int imx_usbmisc_init_post(struct imx_usbmisc_data *);
+int imx_usbmisc_set_wakeup(struct imx_usbmisc_data *, bool);
 
 #endif /* __DRIVER_USB_CHIPIDEA_CI_HDRC_IMX_H */
diff --git a/drivers/usb/chipidea/ci_hdrc_pci.c b/drivers/usb/chipidea/ci_hdrc_pci.c
index 4df669437211..773d150512fa 100644
--- a/drivers/usb/chipidea/ci_hdrc_pci.c
+++ b/drivers/usb/chipidea/ci_hdrc_pci.c
@@ -16,10 +16,16 @@
 #include <linux/interrupt.h>
 #include <linux/usb/gadget.h>
 #include <linux/usb/chipidea.h>
+#include <linux/usb/usb_phy_generic.h>
 
 /* driver name */
 #define UDC_DRIVER_NAME   "ci_hdrc_pci"
 
+struct ci_hdrc_pci {
+	struct platform_device	*ci;
+	struct platform_device	*phy;
+};
+
 /******************************************************************************
  * PCI block
  *****************************************************************************/
@@ -52,7 +58,7 @@ static int ci_hdrc_pci_probe(struct pci_dev *pdev,
 				       const struct pci_device_id *id)
 {
 	struct ci_hdrc_platform_data *platdata = (void *)id->driver_data;
-	struct platform_device *plat_ci;
+	struct ci_hdrc_pci *ci;
 	struct resource res[3];
 	int retval = 0, nres = 2;
 
@@ -61,6 +67,10 @@ static int ci_hdrc_pci_probe(struct pci_dev *pdev,
 		return -ENODEV;
 	}
 
+	ci = devm_kzalloc(&pdev->dev, sizeof(*ci), GFP_KERNEL);
+	if (!ci)
+		return -ENOMEM;
+
 	retval = pcim_enable_device(pdev);
 	if (retval)
 		return retval;
@@ -73,6 +83,11 @@ static int ci_hdrc_pci_probe(struct pci_dev *pdev,
 	pci_set_master(pdev);
 	pci_try_set_mwi(pdev);
 
+	/* register a nop PHY */
+	ci->phy = usb_phy_generic_register();
+	if (!ci->phy)
+		return -ENOMEM;
+
 	memset(res, 0, sizeof(res));
 	res[0].start	= pci_resource_start(pdev, 0);
 	res[0].end	= pci_resource_end(pdev, 0);
@@ -80,13 +95,14 @@ static int ci_hdrc_pci_probe(struct pci_dev *pdev,
 	res[1].start	= pdev->irq;
 	res[1].flags	= IORESOURCE_IRQ;
 
-	plat_ci = ci_hdrc_add_device(&pdev->dev, res, nres, platdata);
-	if (IS_ERR(plat_ci)) {
+	ci->ci = ci_hdrc_add_device(&pdev->dev, res, nres, platdata);
+	if (IS_ERR(ci->ci)) {
 		dev_err(&pdev->dev, "ci_hdrc_add_device failed!\n");
-		return PTR_ERR(plat_ci);
+		usb_phy_generic_unregister(ci->phy);
+		return PTR_ERR(ci->ci);
 	}
 
-	pci_set_drvdata(pdev, plat_ci);
+	pci_set_drvdata(pdev, ci);
 
 	return 0;
 }
@@ -101,9 +117,10 @@ static int ci_hdrc_pci_probe(struct pci_dev *pdev,
  */
 static void ci_hdrc_pci_remove(struct pci_dev *pdev)
 {
-	struct platform_device *plat_ci = pci_get_drvdata(pdev);
+	struct ci_hdrc_pci *ci = pci_get_drvdata(pdev);
 
-	ci_hdrc_remove_device(plat_ci);
+	ci_hdrc_remove_device(ci->ci);
+	usb_phy_generic_unregister(ci->phy);
 }
 
 /**
diff --git a/drivers/usb/chipidea/ci_hdrc_zevio.c b/drivers/usb/chipidea/ci_hdrc_zevio.c
index d976fc1db73a..1264de505527 100644
--- a/drivers/usb/chipidea/ci_hdrc_zevio.c
+++ b/drivers/usb/chipidea/ci_hdrc_zevio.c
@@ -18,7 +18,7 @@
 
 static struct ci_hdrc_platform_data ci_hdrc_zevio_platdata = {
 	.name			= "ci_hdrc_zevio",
-	.flags			= CI_HDRC_REGS_SHARED,
+	.flags			= CI_HDRC_REGS_SHARED | CI_HDRC_FORCE_FULLSPEED,
 	.capoffset		= DEF_CAPOFFSET,
 };
 
diff --git a/drivers/usb/chipidea/core.c b/drivers/usb/chipidea/core.c
index a57dc8866fc5..74fea4fa41b1 100644
--- a/drivers/usb/chipidea/core.c
+++ b/drivers/usb/chipidea/core.c
@@ -137,6 +137,22 @@ static int hw_alloc_regmap(struct ci_hdrc *ci, bool is_lpm)
 	return 0;
 }
 
+static enum ci_revision ci_get_revision(struct ci_hdrc *ci)
+{
+	int ver = hw_read_id_reg(ci, ID_ID, VERSION) >> __ffs(VERSION);
+	enum ci_revision rev = CI_REVISION_UNKNOWN;
+
+	if (ver == 0x2) {
+		rev = hw_read_id_reg(ci, ID_ID, REVISION)
+			>> __ffs(REVISION);
+		rev += CI_REVISION_20;
+	} else if (ver == 0x0) {
+		rev = CI_REVISION_1X;
+	}
+
+	return rev;
+}
+
 /**
  * hw_read_intr_enable: returns interrupt enable register
  *
@@ -251,8 +267,11 @@ static int hw_device_init(struct ci_hdrc *ci, void __iomem *base)
 	/* Clear all interrupts status bits*/
 	hw_write(ci, OP_USBSTS, 0xffffffff, 0xffffffff);
 
-	dev_dbg(ci->dev, "ChipIdea HDRC found, lpm: %d; cap: %p op: %p\n",
-		ci->hw_bank.lpm, ci->hw_bank.cap, ci->hw_bank.op);
+	ci->rev = ci_get_revision(ci);
+
+	dev_dbg(ci->dev,
+		"ChipIdea HDRC found, revision: %d, lpm: %d; cap: %p op: %p\n",
+		ci->rev, ci->hw_bank.lpm, ci->hw_bank.cap, ci->hw_bank.op);
 
 	/* setup lock mode ? */
 
@@ -491,6 +510,13 @@ static irqreturn_t ci_irq(int irq, void *data)
 	irqreturn_t ret = IRQ_NONE;
 	u32 otgsc = 0;
 
+	if (ci->in_lpm) {
+		disable_irq_nosync(irq);
+		ci->wakeup_int = true;
+		pm_runtime_get(ci->dev);
+		return IRQ_HANDLED;
+	}
+
 	if (ci->is_otg) {
 		otgsc = hw_read_otgsc(ci, ~0);
 		if (ci_otg_is_fsm_mode(ci)) {
@@ -642,8 +668,12 @@ static void ci_get_otg_capable(struct ci_hdrc *ci)
 		ci->is_otg = (hw_read(ci, CAP_DCCPARAMS,
 				DCCPARAMS_DC | DCCPARAMS_HC)
 					== (DCCPARAMS_DC | DCCPARAMS_HC));
-	if (ci->is_otg)
+	if (ci->is_otg) {
 		dev_dbg(ci->dev, "It is OTG capable controller\n");
+		/* Disable and clear all OTG irq */
+		hw_write_otgsc(ci, OTGSC_INT_EN_BITS | OTGSC_INT_STATUS_BITS,
+							OTGSC_INT_STATUS_BITS);
+	}
 }
 
 static int ci_hdrc_probe(struct platform_device *pdev)
@@ -673,6 +703,8 @@ static int ci_hdrc_probe(struct platform_device *pdev)
 	ci->platdata = dev_get_platdata(dev);
 	ci->imx28_write_fix = !!(ci->platdata->flags &
 		CI_HDRC_IMX28_WRITE_FIX);
+	ci->supports_runtime_pm = !!(ci->platdata->flags &
+		CI_HDRC_SUPPORTS_RUNTIME_PM);
 
 	ret = hw_device_init(ci, base);
 	if (ret < 0) {
@@ -740,9 +772,6 @@ static int ci_hdrc_probe(struct platform_device *pdev)
 	}
 
 	if (ci->is_otg && ci->roles[CI_ROLE_GADGET]) {
-		/* Disable and clear all OTG irq */
-		hw_write_otgsc(ci, OTGSC_INT_EN_BITS | OTGSC_INT_STATUS_BITS,
-							OTGSC_INT_STATUS_BITS);
 		ret = ci_hdrc_otg_init(ci);
 		if (ret) {
 			dev_err(dev, "init otg fails, ret = %d\n", ret);
@@ -769,11 +798,11 @@ static int ci_hdrc_probe(struct platform_device *pdev)
 			: CI_ROLE_GADGET;
 	}
 
-	/* only update vbus status for peripheral */
-	if (ci->role == CI_ROLE_GADGET)
-		ci_handle_vbus_change(ci);
-
 	if (!ci_otg_is_fsm_mode(ci)) {
+		/* only update vbus status for peripheral */
+		if (ci->role == CI_ROLE_GADGET)
+			ci_handle_vbus_change(ci);
+
 		ret = ci_role_start(ci, ci->role);
 		if (ret) {
 			dev_err(dev, "can't start %s role\n",
@@ -788,9 +817,19 @@ static int ci_hdrc_probe(struct platform_device *pdev)
 	if (ret)
 		goto stop;
 
+	if (ci->supports_runtime_pm) {
+		pm_runtime_set_active(&pdev->dev);
+		pm_runtime_enable(&pdev->dev);
+		pm_runtime_set_autosuspend_delay(&pdev->dev, 2000);
+		pm_runtime_mark_last_busy(ci->dev);
+		pm_runtime_use_autosuspend(&pdev->dev);
+	}
+
 	if (ci_otg_is_fsm_mode(ci))
 		ci_hdrc_otg_fsm_start(ci);
 
+	device_set_wakeup_capable(&pdev->dev, true);
+
 	ret = dbg_create_files(ci);
 	if (!ret)
 		return 0;
@@ -807,6 +846,12 @@ static int ci_hdrc_remove(struct platform_device *pdev)
 {
 	struct ci_hdrc *ci = platform_get_drvdata(pdev);
 
+	if (ci->supports_runtime_pm) {
+		pm_runtime_get_sync(&pdev->dev);
+		pm_runtime_disable(&pdev->dev);
+		pm_runtime_put_noidle(&pdev->dev);
+	}
+
 	dbg_remove_files(ci);
 	ci_role_destroy(ci);
 	ci_hdrc_enter_lpm(ci, true);
@@ -815,13 +860,41 @@ static int ci_hdrc_remove(struct platform_device *pdev)
 	return 0;
 }
 
-#ifdef CONFIG_PM_SLEEP
+#ifdef CONFIG_PM
+/* Prepare wakeup by SRP before suspend */
+static void ci_otg_fsm_suspend_for_srp(struct ci_hdrc *ci)
+{
+	if ((ci->fsm.otg->state == OTG_STATE_A_IDLE) &&
+				!hw_read_otgsc(ci, OTGSC_ID)) {
+		hw_write(ci, OP_PORTSC, PORTSC_W1C_BITS | PORTSC_PP,
+								PORTSC_PP);
+		hw_write(ci, OP_PORTSC, PORTSC_W1C_BITS | PORTSC_WKCN,
+								PORTSC_WKCN);
+	}
+}
+
+/* Handle SRP when wakeup by data pulse */
+static void ci_otg_fsm_wakeup_by_srp(struct ci_hdrc *ci)
+{
+	if ((ci->fsm.otg->state == OTG_STATE_A_IDLE) &&
+		(ci->fsm.a_bus_drop == 1) && (ci->fsm.a_bus_req == 0)) {
+		if (!hw_read_otgsc(ci, OTGSC_ID)) {
+			ci->fsm.a_srp_det = 1;
+			ci->fsm.a_bus_drop = 0;
+		} else {
+			ci->fsm.id = 1;
+		}
+		ci_otg_queue_work(ci);
+	}
+}
+
 static void ci_controller_suspend(struct ci_hdrc *ci)
 {
+	disable_irq(ci->irq);
 	ci_hdrc_enter_lpm(ci, true);
-
-	if (ci->usb_phy)
-		usb_phy_set_suspend(ci->usb_phy, 1);
+	usb_phy_set_suspend(ci->usb_phy, 1);
+	ci->in_lpm = true;
+	enable_irq(ci->irq);
 }
 
 static int ci_controller_resume(struct device *dev)
@@ -830,23 +903,59 @@ static int ci_controller_resume(struct device *dev)
 
 	dev_dbg(dev, "at %s\n", __func__);
 
-	ci_hdrc_enter_lpm(ci, false);
+	if (!ci->in_lpm) {
+		WARN_ON(1);
+		return 0;
+	}
 
+	ci_hdrc_enter_lpm(ci, false);
 	if (ci->usb_phy) {
 		usb_phy_set_suspend(ci->usb_phy, 0);
 		usb_phy_set_wakeup(ci->usb_phy, false);
 		hw_wait_phy_stable();
 	}
 
+	ci->in_lpm = false;
+	if (ci->wakeup_int) {
+		ci->wakeup_int = false;
+		pm_runtime_mark_last_busy(ci->dev);
+		pm_runtime_put_autosuspend(ci->dev);
+		enable_irq(ci->irq);
+		if (ci_otg_is_fsm_mode(ci))
+			ci_otg_fsm_wakeup_by_srp(ci);
+	}
+
 	return 0;
 }
 
+#ifdef CONFIG_PM_SLEEP
 static int ci_suspend(struct device *dev)
 {
 	struct ci_hdrc *ci = dev_get_drvdata(dev);
 
 	if (ci->wq)
 		flush_workqueue(ci->wq);
+	/*
+	 * Controller needs to be active during suspend, otherwise the core
+	 * may run resume when the parent is at suspend if other driver's
+	 * suspend fails, it occurs before parent's suspend has not started,
+	 * but the core suspend has finished.
+	 */
+	if (ci->in_lpm)
+		pm_runtime_resume(dev);
+
+	if (ci->in_lpm) {
+		WARN_ON(1);
+		return 0;
+	}
+
+	if (device_may_wakeup(dev)) {
+		if (ci_otg_is_fsm_mode(ci))
+			ci_otg_fsm_suspend_for_srp(ci);
+
+		usb_phy_set_wakeup(ci->usb_phy, true);
+		enable_irq_wake(ci->irq);
+	}
 
 	ci_controller_suspend(ci);
 
@@ -855,13 +964,57 @@ static int ci_suspend(struct device *dev)
 
 static int ci_resume(struct device *dev)
 {
-	return ci_controller_resume(dev);
+	struct ci_hdrc *ci = dev_get_drvdata(dev);
+	int ret;
+
+	if (device_may_wakeup(dev))
+		disable_irq_wake(ci->irq);
+
+	ret = ci_controller_resume(dev);
+	if (ret)
+		return ret;
+
+	if (ci->supports_runtime_pm) {
+		pm_runtime_disable(dev);
+		pm_runtime_set_active(dev);
+		pm_runtime_enable(dev);
+	}
+
+	return ret;
 }
 #endif /* CONFIG_PM_SLEEP */
 
+static int ci_runtime_suspend(struct device *dev)
+{
+	struct ci_hdrc *ci = dev_get_drvdata(dev);
+
+	dev_dbg(dev, "at %s\n", __func__);
+
+	if (ci->in_lpm) {
+		WARN_ON(1);
+		return 0;
+	}
+
+	if (ci_otg_is_fsm_mode(ci))
+		ci_otg_fsm_suspend_for_srp(ci);
+
+	usb_phy_set_wakeup(ci->usb_phy, true);
+	ci_controller_suspend(ci);
+
+	return 0;
+}
+
+static int ci_runtime_resume(struct device *dev)
+{
+	return ci_controller_resume(dev);
+}
+
+#endif /* CONFIG_PM */
 static const struct dev_pm_ops ci_pm_ops = {
 	SET_SYSTEM_SLEEP_PM_OPS(ci_suspend, ci_resume)
+	SET_RUNTIME_PM_OPS(ci_runtime_suspend, ci_runtime_resume, NULL)
 };
+
 static struct platform_driver ci_hdrc_driver = {
 	.probe	= ci_hdrc_probe,
 	.remove	= ci_hdrc_remove,
diff --git a/drivers/usb/chipidea/debug.c b/drivers/usb/chipidea/debug.c
index 268e4236e84c..dfb05edcdb96 100644
--- a/drivers/usb/chipidea/debug.c
+++ b/drivers/usb/chipidea/debug.c
@@ -336,8 +336,8 @@ static int ci_registers_show(struct seq_file *s, void *unused)
 	struct ci_hdrc *ci = s->private;
 	u32 tmp_reg;
 
-	if (!ci)
-		return 0;
+	if (!ci || ci->in_lpm)
+		return -EPERM;
 
 	/* ------ Registers ----- */
 	tmp_reg = hw_read_intr_enable(ci);
diff --git a/drivers/usb/chipidea/host.c b/drivers/usb/chipidea/host.c
index 48731d0bab35..21fe1a314313 100644
--- a/drivers/usb/chipidea/host.c
+++ b/drivers/usb/chipidea/host.c
@@ -33,6 +33,7 @@
 #include "host.h"
 
 static struct hc_driver __read_mostly ci_ehci_hc_driver;
+static int (*orig_bus_suspend)(struct usb_hcd *hcd);
 
 struct ehci_ci_priv {
 	struct regulator *reg_vbus;
@@ -43,11 +44,10 @@ static int ehci_ci_portpower(struct usb_hcd *hcd, int portnum, bool enable)
 	struct ehci_hcd *ehci = hcd_to_ehci(hcd);
 	struct ehci_ci_priv *priv = (struct ehci_ci_priv *)ehci->priv;
 	struct device *dev = hcd->self.controller;
-	struct ci_hdrc *ci = dev_get_drvdata(dev);
 	int ret = 0;
 	int port = HCS_N_PORTS(ehci->hcs_params);
 
-	if (priv->reg_vbus && !ci_otg_is_fsm_mode(ci)) {
+	if (priv->reg_vbus) {
 		if (port > 1) {
 			dev_warn(dev,
 				"Not support multi-port regulator control\n");
@@ -113,12 +113,23 @@ static int host_start(struct ci_hdrc *ci)
 	priv = (struct ehci_ci_priv *)ehci->priv;
 	priv->reg_vbus = NULL;
 
-	if (ci->platdata->reg_vbus)
-		priv->reg_vbus = ci->platdata->reg_vbus;
+	if (ci->platdata->reg_vbus && !ci_otg_is_fsm_mode(ci)) {
+		if (ci->platdata->flags & CI_HDRC_TURN_VBUS_EARLY_ON) {
+			ret = regulator_enable(ci->platdata->reg_vbus);
+			if (ret) {
+				dev_err(ci->dev,
+				"Failed to enable vbus regulator, ret=%d\n",
+									ret);
+				goto put_hcd;
+			}
+		} else {
+			priv->reg_vbus = ci->platdata->reg_vbus;
+		}
+	}
 
 	ret = usb_add_hcd(hcd, 0, 0);
 	if (ret) {
-		goto put_hcd;
+		goto disable_reg;
 	} else {
 		struct usb_otg *otg = &ci->otg;
 
@@ -133,8 +144,15 @@ static int host_start(struct ci_hdrc *ci)
 	if (ci->platdata->flags & CI_HDRC_DISABLE_STREAMING)
 		hw_write(ci, OP_USBMODE, USBMODE_CI_SDIS, USBMODE_CI_SDIS);
 
+	if (ci->platdata->flags & CI_HDRC_FORCE_FULLSPEED)
+		hw_write(ci, OP_PORTSC, PORTSC_PFSC, PORTSC_PFSC);
+
 	return ret;
 
+disable_reg:
+	if (ci->platdata->reg_vbus && !ci_otg_is_fsm_mode(ci) &&
+			(ci->platdata->flags & CI_HDRC_TURN_VBUS_EARLY_ON))
+		regulator_disable(ci->platdata->reg_vbus);
 put_hcd:
 	usb_put_hcd(hcd);
 
@@ -148,6 +166,9 @@ static void host_stop(struct ci_hdrc *ci)
 	if (hcd) {
 		usb_remove_hcd(hcd);
 		usb_put_hcd(hcd);
+		if (ci->platdata->reg_vbus && !ci_otg_is_fsm_mode(ci) &&
+			(ci->platdata->flags & CI_HDRC_TURN_VBUS_EARLY_ON))
+				regulator_disable(ci->platdata->reg_vbus);
 	}
 }
 
@@ -158,6 +179,47 @@ void ci_hdrc_host_destroy(struct ci_hdrc *ci)
 		host_stop(ci);
 }
 
+static int ci_ehci_bus_suspend(struct usb_hcd *hcd)
+{
+	struct ehci_hcd *ehci = hcd_to_ehci(hcd);
+	int port;
+	u32 tmp;
+
+	int ret = orig_bus_suspend(hcd);
+
+	if (ret)
+		return ret;
+
+	port = HCS_N_PORTS(ehci->hcs_params);
+	while (port--) {
+		u32 __iomem *reg = &ehci->regs->port_status[port];
+		u32 portsc = ehci_readl(ehci, reg);
+
+		if (portsc & PORT_CONNECT) {
+			/*
+			 * For chipidea, the resume signal will be ended
+			 * automatically, so for remote wakeup case, the
+			 * usbcmd.rs may not be set before the resume has
+			 * ended if other resume paths consumes too much
+			 * time (~24ms), in that case, the SOF will not
+			 * send out within 3ms after resume ends, then the
+			 * high speed device will enter full speed mode.
+			 */
+
+			tmp = ehci_readl(ehci, &ehci->regs->command);
+			tmp |= CMD_RUN;
+			ehci_writel(ehci, tmp, &ehci->regs->command);
+			/*
+			 * It needs a short delay between set RS bit and PHCD.
+			 */
+			usleep_range(150, 200);
+			break;
+		}
+	}
+
+	return 0;
+}
+
 int ci_hdrc_host_init(struct ci_hdrc *ci)
 {
 	struct ci_role_driver *rdrv;
@@ -176,6 +238,8 @@ int ci_hdrc_host_init(struct ci_hdrc *ci)
 	ci->roles[CI_ROLE_HOST] = rdrv;
 
 	ehci_init_driver(&ci_ehci_hc_driver, &ehci_ci_overrides);
+	orig_bus_suspend = ci_ehci_hc_driver.bus_suspend;
+	ci_ehci_hc_driver.bus_suspend = ci_ehci_bus_suspend;
 
 	return 0;
 }
diff --git a/drivers/usb/chipidea/otg.c b/drivers/usb/chipidea/otg.c
index a048b08b9d4d..ad6c87a4653c 100644
--- a/drivers/usb/chipidea/otg.c
+++ b/drivers/usb/chipidea/otg.c
@@ -96,6 +96,7 @@ static void ci_otg_work(struct work_struct *work)
 		return;
 	}
 
+	pm_runtime_get_sync(ci->dev);
 	if (ci->id_event) {
 		ci->id_event = false;
 		ci_handle_id_switch(ci);
@@ -104,6 +105,7 @@ static void ci_otg_work(struct work_struct *work)
 		ci_handle_vbus_change(ci);
 	} else
 		dev_err(ci->dev, "unexpected event occurs at %s\n", __func__);
+	pm_runtime_put_sync(ci->dev);
 
 	enable_irq(ci->irq);
 }
diff --git a/drivers/usb/chipidea/otg_fsm.c b/drivers/usb/chipidea/otg_fsm.c
index 562e581f6765..083acf45ad5a 100644
--- a/drivers/usb/chipidea/otg_fsm.c
+++ b/drivers/usb/chipidea/otg_fsm.c
@@ -30,22 +30,6 @@
 #include "otg.h"
 #include "otg_fsm.h"
 
-static struct ci_otg_fsm_timer *otg_timer_initializer
-(struct ci_hdrc *ci, void (*function)(void *, unsigned long),
-			unsigned long expires, unsigned long data)
-{
-	struct ci_otg_fsm_timer *timer;
-
-	timer = devm_kzalloc(ci->dev, sizeof(struct ci_otg_fsm_timer),
-								GFP_KERNEL);
-	if (!timer)
-		return NULL;
-	timer->function = function;
-	timer->expires = expires;
-	timer->data = data;
-	return timer;
-}
-
 /* Add for otg: interact with user space app */
 static ssize_t
 get_a_bus_req(struct device *dev, struct device_attribute *attr, char *buf)
@@ -204,229 +188,227 @@ static struct attribute_group inputs_attr_group = {
 };
 
 /*
+ * Keep this list in the same order as timers indexed
+ * by enum otg_fsm_timer in include/linux/usb/otg-fsm.h
+ */
+static unsigned otg_timer_ms[] = {
+	TA_WAIT_VRISE,
+	TA_WAIT_VFALL,
+	TA_WAIT_BCON,
+	TA_AIDL_BDIS,
+	TB_ASE0_BRST,
+	TA_BIDL_ADIS,
+	TB_SE0_SRP,
+	TB_SRP_FAIL,
+	0,
+	TB_DATA_PLS,
+	TB_SSEND_SRP,
+};
+
+/*
  * Add timer to active timer list
  */
-static void ci_otg_add_timer(struct ci_hdrc *ci, enum ci_otg_fsm_timer_index t)
+static void ci_otg_add_timer(struct ci_hdrc *ci, enum otg_fsm_timer t)
 {
-	struct ci_otg_fsm_timer *tmp_timer;
-	struct ci_otg_fsm_timer *timer = ci->fsm_timer->timer_list[t];
-	struct list_head *active_timers = &ci->fsm_timer->active_timers;
+	unsigned long flags, timer_sec, timer_nsec;
 
-	if (t >= NUM_CI_OTG_FSM_TIMERS)
+	if (t >= NUM_OTG_FSM_TIMERS)
 		return;
 
-	/*
-	 * Check if the timer is already in the active list,
-	 * if so update timer count
-	 */
-	list_for_each_entry(tmp_timer, active_timers, list)
-		if (tmp_timer == timer) {
-			timer->count = timer->expires;
-			return;
-		}
-
-	timer->count = timer->expires;
-	list_add_tail(&timer->list, active_timers);
-
-	/* Enable 1ms irq */
-	if (!(hw_read_otgsc(ci, OTGSC_1MSIE)))
-		hw_write_otgsc(ci, OTGSC_1MSIE, OTGSC_1MSIE);
+	spin_lock_irqsave(&ci->lock, flags);
+	timer_sec = otg_timer_ms[t] / MSEC_PER_SEC;
+	timer_nsec = (otg_timer_ms[t] % MSEC_PER_SEC) * NSEC_PER_MSEC;
+	ci->hr_timeouts[t] = ktime_add(ktime_get(),
+				ktime_set(timer_sec, timer_nsec));
+	ci->enabled_otg_timer_bits |= (1 << t);
+	if ((ci->next_otg_timer == NUM_OTG_FSM_TIMERS) ||
+			(ci->hr_timeouts[ci->next_otg_timer].tv64 >
+						ci->hr_timeouts[t].tv64)) {
+			ci->next_otg_timer = t;
+			hrtimer_start_range_ns(&ci->otg_fsm_hrtimer,
+					ci->hr_timeouts[t], NSEC_PER_MSEC,
+							HRTIMER_MODE_ABS);
+	}
+	spin_unlock_irqrestore(&ci->lock, flags);
 }
 
 /*
  * Remove timer from active timer list
  */
-static void ci_otg_del_timer(struct ci_hdrc *ci, enum ci_otg_fsm_timer_index t)
+static void ci_otg_del_timer(struct ci_hdrc *ci, enum otg_fsm_timer t)
 {
-	struct ci_otg_fsm_timer *tmp_timer, *del_tmp;
-	struct ci_otg_fsm_timer *timer = ci->fsm_timer->timer_list[t];
-	struct list_head *active_timers = &ci->fsm_timer->active_timers;
+	unsigned long flags, enabled_timer_bits;
+	enum otg_fsm_timer cur_timer, next_timer = NUM_OTG_FSM_TIMERS;
 
-	if (t >= NUM_CI_OTG_FSM_TIMERS)
+	if ((t >= NUM_OTG_FSM_TIMERS) ||
+			!(ci->enabled_otg_timer_bits & (1 << t)))
 		return;
 
-	list_for_each_entry_safe(tmp_timer, del_tmp, active_timers, list)
-		if (tmp_timer == timer)
-			list_del(&timer->list);
-
-	/* Disable 1ms irq if there is no any active timer */
-	if (list_empty(active_timers))
-		hw_write_otgsc(ci, OTGSC_1MSIE, 0);
-}
-
-/*
- * Reduce timer count by 1, and find timeout conditions.
- * Called by otg 1ms timer interrupt
- */
-static inline int ci_otg_tick_timer(struct ci_hdrc *ci)
-{
-	struct ci_otg_fsm_timer *tmp_timer, *del_tmp;
-	struct list_head *active_timers = &ci->fsm_timer->active_timers;
-	int expired = 0;
-
-	list_for_each_entry_safe(tmp_timer, del_tmp, active_timers, list) {
-		tmp_timer->count--;
-		/* check if timer expires */
-		if (!tmp_timer->count) {
-			list_del(&tmp_timer->list);
-			tmp_timer->function(ci, tmp_timer->data);
-			expired = 1;
+	spin_lock_irqsave(&ci->lock, flags);
+	ci->enabled_otg_timer_bits &= ~(1 << t);
+	if (ci->next_otg_timer == t) {
+		if (ci->enabled_otg_timer_bits == 0) {
+			/* No enabled timers after delete it */
+			hrtimer_cancel(&ci->otg_fsm_hrtimer);
+			ci->next_otg_timer = NUM_OTG_FSM_TIMERS;
+		} else {
+			/* Find the next timer */
+			enabled_timer_bits = ci->enabled_otg_timer_bits;
+			for_each_set_bit(cur_timer, &enabled_timer_bits,
+							NUM_OTG_FSM_TIMERS) {
+				if ((next_timer == NUM_OTG_FSM_TIMERS) ||
+					(ci->hr_timeouts[next_timer].tv64 <
+					ci->hr_timeouts[cur_timer].tv64))
+					next_timer = cur_timer;
+			}
 		}
 	}
-
-	/* disable 1ms irq if there is no any timer active */
-	if ((expired == 1) && list_empty(active_timers))
-		hw_write_otgsc(ci, OTGSC_1MSIE, 0);
-
-	return expired;
+	if (next_timer != NUM_OTG_FSM_TIMERS) {
+		ci->next_otg_timer = next_timer;
+		hrtimer_start_range_ns(&ci->otg_fsm_hrtimer,
+			ci->hr_timeouts[next_timer], NSEC_PER_MSEC,
+							HRTIMER_MODE_ABS);
+	}
+	spin_unlock_irqrestore(&ci->lock, flags);
 }
 
-/* The timeout callback function to set time out bit */
-static void set_tmout(void *ptr, unsigned long indicator)
+/* OTG FSM timer handlers */
+static int a_wait_vrise_tmout(struct ci_hdrc *ci)
 {
-	*(int *)indicator = 1;
+	ci->fsm.a_wait_vrise_tmout = 1;
+	return 0;
 }
 
-static void set_tmout_and_fsm(void *ptr, unsigned long indicator)
+static int a_wait_vfall_tmout(struct ci_hdrc *ci)
 {
-	struct ci_hdrc *ci = (struct ci_hdrc *)ptr;
-
-	set_tmout(ci, indicator);
-
-	ci_otg_queue_work(ci);
+	ci->fsm.a_wait_vfall_tmout = 1;
+	return 0;
 }
 
-static void a_wait_vfall_tmout_func(void *ptr, unsigned long indicator)
+static int a_wait_bcon_tmout(struct ci_hdrc *ci)
 {
-	struct ci_hdrc *ci = (struct ci_hdrc *)ptr;
-
-	set_tmout(ci, indicator);
-	/* Disable port power */
-	hw_write(ci, OP_PORTSC, PORTSC_W1C_BITS | PORTSC_PP, 0);
-	/* Clear existing DP irq */
-	hw_write_otgsc(ci, OTGSC_DPIS, OTGSC_DPIS);
-	/* Enable data pulse irq */
-	hw_write_otgsc(ci, OTGSC_DPIE, OTGSC_DPIE);
-	ci_otg_queue_work(ci);
+	ci->fsm.a_wait_bcon_tmout = 1;
+	return 0;
 }
 
-static void b_ase0_brst_tmout_func(void *ptr, unsigned long indicator)
+static int a_aidl_bdis_tmout(struct ci_hdrc *ci)
 {
-	struct ci_hdrc *ci = (struct ci_hdrc *)ptr;
-
-	set_tmout(ci, indicator);
-	if (!hw_read_otgsc(ci, OTGSC_BSV))
-		ci->fsm.b_sess_vld = 0;
-
-	ci_otg_queue_work(ci);
+	ci->fsm.a_aidl_bdis_tmout = 1;
+	return 0;
 }
 
-static void b_ssend_srp_tmout_func(void *ptr, unsigned long indicator)
+static int b_ase0_brst_tmout(struct ci_hdrc *ci)
 {
-	struct ci_hdrc *ci = (struct ci_hdrc *)ptr;
-
-	set_tmout(ci, indicator);
-
-	/* only vbus fall below B_sess_vld in b_idle state */
-	if (ci->fsm.otg->state == OTG_STATE_B_IDLE)
-		ci_otg_queue_work(ci);
+	ci->fsm.b_ase0_brst_tmout = 1;
+	return 0;
 }
 
-static void b_sess_vld_tmout_func(void *ptr, unsigned long indicator)
+static int a_bidl_adis_tmout(struct ci_hdrc *ci)
 {
-	struct ci_hdrc *ci = (struct ci_hdrc *)ptr;
+	ci->fsm.a_bidl_adis_tmout = 1;
+	return 0;
+}
 
-	/* Check if A detached */
-	if (!(hw_read_otgsc(ci, OTGSC_BSV))) {
-		ci->fsm.b_sess_vld = 0;
-		ci_otg_add_timer(ci, B_SSEND_SRP);
-		ci_otg_queue_work(ci);
-	}
+static int b_se0_srp_tmout(struct ci_hdrc *ci)
+{
+	ci->fsm.b_se0_srp = 1;
+	return 0;
 }
 
-static void b_data_pulse_end(void *ptr, unsigned long indicator)
+static int b_srp_fail_tmout(struct ci_hdrc *ci)
 {
-	struct ci_hdrc *ci = (struct ci_hdrc *)ptr;
+	ci->fsm.b_srp_done = 1;
+	return 1;
+}
 
+static int b_data_pls_tmout(struct ci_hdrc *ci)
+{
 	ci->fsm.b_srp_done = 1;
 	ci->fsm.b_bus_req = 0;
 	if (ci->fsm.power_up)
 		ci->fsm.power_up = 0;
-
 	hw_write_otgsc(ci, OTGSC_HABA, 0);
+	pm_runtime_put(ci->dev);
+	return 0;
+}
 
-	ci_otg_queue_work(ci);
+static int b_ssend_srp_tmout(struct ci_hdrc *ci)
+{
+	ci->fsm.b_ssend_srp = 1;
+	/* only vbus fall below B_sess_vld in b_idle state */
+	if (ci->fsm.otg->state == OTG_STATE_B_IDLE)
+		return 0;
+	else
+		return 1;
+}
+
+/*
+ * Keep this list in the same order as timers indexed
+ * by enum otg_fsm_timer in include/linux/usb/otg-fsm.h
+ */
+static int (*otg_timer_handlers[])(struct ci_hdrc *) = {
+	a_wait_vrise_tmout,	/* A_WAIT_VRISE */
+	a_wait_vfall_tmout,	/* A_WAIT_VFALL */
+	a_wait_bcon_tmout,	/* A_WAIT_BCON */
+	a_aidl_bdis_tmout,	/* A_AIDL_BDIS */
+	b_ase0_brst_tmout,	/* B_ASE0_BRST */
+	a_bidl_adis_tmout,	/* A_BIDL_ADIS */
+	b_se0_srp_tmout,	/* B_SE0_SRP */
+	b_srp_fail_tmout,	/* B_SRP_FAIL */
+	NULL,			/* A_WAIT_ENUM */
+	b_data_pls_tmout,	/* B_DATA_PLS */
+	b_ssend_srp_tmout,	/* B_SSEND_SRP */
+};
+
+/*
+ * Enable the next nearest enabled timer if have
+ */
+static enum hrtimer_restart ci_otg_hrtimer_func(struct hrtimer *t)
+{
+	struct ci_hdrc *ci = container_of(t, struct ci_hdrc, otg_fsm_hrtimer);
+	ktime_t	now, *timeout;
+	unsigned long   enabled_timer_bits;
+	unsigned long   flags;
+	enum otg_fsm_timer cur_timer, next_timer = NUM_OTG_FSM_TIMERS;
+	int ret = -EINVAL;
+
+	spin_lock_irqsave(&ci->lock, flags);
+	enabled_timer_bits = ci->enabled_otg_timer_bits;
+	ci->next_otg_timer = NUM_OTG_FSM_TIMERS;
+
+	now = ktime_get();
+	for_each_set_bit(cur_timer, &enabled_timer_bits, NUM_OTG_FSM_TIMERS) {
+		if (now.tv64 >= ci->hr_timeouts[cur_timer].tv64) {
+			ci->enabled_otg_timer_bits &= ~(1 << cur_timer);
+			if (otg_timer_handlers[cur_timer])
+				ret = otg_timer_handlers[cur_timer](ci);
+		} else {
+			if ((next_timer == NUM_OTG_FSM_TIMERS) ||
+				(ci->hr_timeouts[cur_timer].tv64 <
+					ci->hr_timeouts[next_timer].tv64))
+				next_timer = cur_timer;
+		}
+	}
+	/* Enable the next nearest timer */
+	if (next_timer < NUM_OTG_FSM_TIMERS) {
+		timeout = &ci->hr_timeouts[next_timer];
+		hrtimer_start_range_ns(&ci->otg_fsm_hrtimer, *timeout,
+					NSEC_PER_MSEC, HRTIMER_MODE_ABS);
+		ci->next_otg_timer = next_timer;
+	}
+	spin_unlock_irqrestore(&ci->lock, flags);
+
+	if (!ret)
+		ci_otg_queue_work(ci);
+
+	return HRTIMER_NORESTART;
 }
 
 /* Initialize timers */
 static int ci_otg_init_timers(struct ci_hdrc *ci)
 {
-	struct otg_fsm *fsm = &ci->fsm;
-
-	/* FSM used timers */
-	ci->fsm_timer->timer_list[A_WAIT_VRISE] =
-		otg_timer_initializer(ci, &set_tmout_and_fsm, TA_WAIT_VRISE,
-			(unsigned long)&fsm->a_wait_vrise_tmout);
-	if (ci->fsm_timer->timer_list[A_WAIT_VRISE] == NULL)
-		return -ENOMEM;
-
-	ci->fsm_timer->timer_list[A_WAIT_VFALL] =
-		otg_timer_initializer(ci, &a_wait_vfall_tmout_func,
-		TA_WAIT_VFALL, (unsigned long)&fsm->a_wait_vfall_tmout);
-	if (ci->fsm_timer->timer_list[A_WAIT_VFALL] == NULL)
-		return -ENOMEM;
-
-	ci->fsm_timer->timer_list[A_WAIT_BCON] =
-		otg_timer_initializer(ci, &set_tmout_and_fsm, TA_WAIT_BCON,
-				(unsigned long)&fsm->a_wait_bcon_tmout);
-	if (ci->fsm_timer->timer_list[A_WAIT_BCON] == NULL)
-		return -ENOMEM;
-
-	ci->fsm_timer->timer_list[A_AIDL_BDIS] =
-		otg_timer_initializer(ci, &set_tmout_and_fsm, TA_AIDL_BDIS,
-				(unsigned long)&fsm->a_aidl_bdis_tmout);
-	if (ci->fsm_timer->timer_list[A_AIDL_BDIS] == NULL)
-		return -ENOMEM;
-
-	ci->fsm_timer->timer_list[A_BIDL_ADIS] =
-		otg_timer_initializer(ci, &set_tmout_and_fsm, TA_BIDL_ADIS,
-				(unsigned long)&fsm->a_bidl_adis_tmout);
-	if (ci->fsm_timer->timer_list[A_BIDL_ADIS] == NULL)
-		return -ENOMEM;
-
-	ci->fsm_timer->timer_list[B_ASE0_BRST] =
-		otg_timer_initializer(ci, &b_ase0_brst_tmout_func, TB_ASE0_BRST,
-					(unsigned long)&fsm->b_ase0_brst_tmout);
-	if (ci->fsm_timer->timer_list[B_ASE0_BRST] == NULL)
-		return -ENOMEM;
-
-	ci->fsm_timer->timer_list[B_SE0_SRP] =
-		otg_timer_initializer(ci, &set_tmout_and_fsm, TB_SE0_SRP,
-					(unsigned long)&fsm->b_se0_srp);
-	if (ci->fsm_timer->timer_list[B_SE0_SRP] == NULL)
-		return -ENOMEM;
-
-	ci->fsm_timer->timer_list[B_SSEND_SRP] =
-		otg_timer_initializer(ci, &b_ssend_srp_tmout_func, TB_SSEND_SRP,
-					(unsigned long)&fsm->b_ssend_srp);
-	if (ci->fsm_timer->timer_list[B_SSEND_SRP] == NULL)
-		return -ENOMEM;
-
-	ci->fsm_timer->timer_list[B_SRP_FAIL] =
-		otg_timer_initializer(ci, &set_tmout, TB_SRP_FAIL,
-				(unsigned long)&fsm->b_srp_done);
-	if (ci->fsm_timer->timer_list[B_SRP_FAIL] == NULL)
-		return -ENOMEM;
-
-	ci->fsm_timer->timer_list[B_DATA_PLS] =
-		otg_timer_initializer(ci, &b_data_pulse_end, TB_DATA_PLS, 0);
-	if (ci->fsm_timer->timer_list[B_DATA_PLS] == NULL)
-		return -ENOMEM;
-
-	ci->fsm_timer->timer_list[B_SESS_VLD] =	otg_timer_initializer(ci,
-					&b_sess_vld_tmout_func, TB_SESS_VLD, 0);
-	if (ci->fsm_timer->timer_list[B_SESS_VLD] == NULL)
-		return -ENOMEM;
+	hrtimer_init(&ci->otg_fsm_hrtimer, CLOCK_MONOTONIC, HRTIMER_MODE_ABS);
+	ci->otg_fsm_hrtimer.function = ci_otg_hrtimer_func;
 
 	return 0;
 }
@@ -530,6 +512,7 @@ static void ci_otg_start_pulse(struct otg_fsm *fsm)
 	/* Hardware Assistant Data pulse */
 	hw_write_otgsc(ci, OTGSC_HADP, OTGSC_HADP);
 
+	pm_runtime_get(ci->dev);
 	ci_otg_add_timer(ci, B_DATA_PLS);
 }
 
@@ -585,6 +568,7 @@ int ci_otg_fsm_work(struct ci_hdrc *ci)
 		ci->fsm.otg->state < OTG_STATE_A_IDLE)
 		return 0;
 
+	pm_runtime_get_sync(ci->dev);
 	if (otg_statemachine(&ci->fsm)) {
 		if (ci->fsm.otg->state == OTG_STATE_A_IDLE) {
 			/*
@@ -596,8 +580,15 @@ int ci_otg_fsm_work(struct ci_hdrc *ci)
 			 * a_idle to a_wait_vrise when power up
 			 */
 			if ((ci->fsm.id) || (ci->id_event) ||
-						(ci->fsm.power_up))
+						(ci->fsm.power_up)) {
 				ci_otg_queue_work(ci);
+			} else {
+				/* Enable data pulse irq */
+				hw_write(ci, OP_PORTSC, PORTSC_W1C_BITS |
+								PORTSC_PP, 0);
+				hw_write_otgsc(ci, OTGSC_DPIS, OTGSC_DPIS);
+				hw_write_otgsc(ci, OTGSC_DPIE, OTGSC_DPIE);
+			}
 			if (ci->id_event)
 				ci->id_event = false;
 		} else if (ci->fsm.otg->state == OTG_STATE_B_IDLE) {
@@ -609,8 +600,13 @@ int ci_otg_fsm_work(struct ci_hdrc *ci)
 				 */
 				ci_otg_queue_work(ci);
 			}
+		} else if (ci->fsm.otg->state == OTG_STATE_A_HOST) {
+			pm_runtime_mark_last_busy(ci->dev);
+			pm_runtime_put_autosuspend(ci->dev);
+			return 0;
 		}
 	}
+	pm_runtime_put_sync(ci->dev);
 	return 0;
 }
 
@@ -655,7 +651,6 @@ static void ci_otg_fsm_event(struct ci_hdrc *ci)
 			fsm->a_conn = 0;
 			fsm->b_bus_req = 0;
 			ci_otg_queue_work(ci);
-			ci_otg_add_timer(ci, B_SESS_VLD);
 		}
 		break;
 	case OTG_STATE_A_PERIPHERAL:
@@ -725,11 +720,7 @@ irqreturn_t ci_otg_fsm_irq(struct ci_hdrc *ci)
 	fsm->id = (otgsc & OTGSC_ID) ? 1 : 0;
 
 	if (otg_int_src) {
-		if (otg_int_src & OTGSC_1MSIS) {
-			hw_write_otgsc(ci, OTGSC_1MSIS, OTGSC_1MSIS);
-			retval = ci_otg_tick_timer(ci);
-			return IRQ_HANDLED;
-		} else if (otg_int_src & OTGSC_DPIS) {
+		if (otg_int_src & OTGSC_DPIS) {
 			hw_write_otgsc(ci, OTGSC_DPIS, OTGSC_DPIS);
 			fsm->a_srp_det = 1;
 			fsm->a_bus_drop = 0;
@@ -793,17 +784,13 @@ int ci_hdrc_otg_fsm_init(struct ci_hdrc *ci)
 
 	mutex_init(&ci->fsm.lock);
 
-	ci->fsm_timer = devm_kzalloc(ci->dev,
-			sizeof(struct ci_otg_fsm_timer_list), GFP_KERNEL);
-	if (!ci->fsm_timer)
-		return -ENOMEM;
-
-	INIT_LIST_HEAD(&ci->fsm_timer->active_timers);
 	retval = ci_otg_init_timers(ci);
 	if (retval) {
 		dev_err(ci->dev, "Couldn't init OTG timers\n");
 		return retval;
 	}
+	ci->enabled_otg_timer_bits = 0;
+	ci->next_otg_timer = NUM_OTG_FSM_TIMERS;
 
 	retval = sysfs_create_group(&ci->dev->kobj, &inputs_attr_group);
 	if (retval < 0) {
diff --git a/drivers/usb/chipidea/otg_fsm.h b/drivers/usb/chipidea/otg_fsm.h
index 94c085f456a9..2689375ae5da 100644
--- a/drivers/usb/chipidea/otg_fsm.h
+++ b/drivers/usb/chipidea/otg_fsm.h
@@ -62,33 +62,6 @@
 /* SSEND time before SRP */
 #define TB_SSEND_SRP         (1500)	/* minimum 1.5 sec, section:5.1.2 */
 
-#define TB_SESS_VLD          (1000)
-
-enum ci_otg_fsm_timer_index {
-	/*
-	 * CI specific timers, start from the end
-	 * of standard and auxiliary OTG timers
-	 */
-	B_DATA_PLS = NUM_OTG_FSM_TIMERS,
-	B_SSEND_SRP,
-	B_SESS_VLD,
-
-	NUM_CI_OTG_FSM_TIMERS,
-};
-
-struct ci_otg_fsm_timer {
-	unsigned long expires;  /* Number of count increase to timeout */
-	unsigned long count;    /* Tick counter */
-	void (*function)(void *, unsigned long);        /* Timeout function */
-	unsigned long data;     /* Data passed to function */
-	struct list_head list;
-};
-
-struct ci_otg_fsm_timer_list {
-	struct ci_otg_fsm_timer *timer_list[NUM_CI_OTG_FSM_TIMERS];
-	struct list_head active_timers;
-};
-
 #ifdef CONFIG_USB_OTG_FSM
 
 int ci_hdrc_otg_fsm_init(struct ci_hdrc *ci);
diff --git a/drivers/usb/chipidea/udc.c b/drivers/usb/chipidea/udc.c
index ff451048c1ac..764f668d45a9 100644
--- a/drivers/usb/chipidea/udc.c
+++ b/drivers/usb/chipidea/udc.c
@@ -86,10 +86,8 @@ static int hw_device_state(struct ci_hdrc *ci, u32 dma)
 		/* interrupt, error, port change, reset, sleep/suspend */
 		hw_write(ci, OP_USBINTR, ~0,
 			     USBi_UI|USBi_UEI|USBi_PCI|USBi_URI|USBi_SLI);
-		hw_write(ci, OP_USBCMD, USBCMD_RS, USBCMD_RS);
 	} else {
 		hw_write(ci, OP_USBINTR, ~0, 0);
-		hw_write(ci, OP_USBCMD, USBCMD_RS, 0);
 	}
 	return 0;
 }
@@ -522,6 +520,20 @@ static void free_pending_td(struct ci_hw_ep *hwep)
 	kfree(pending);
 }
 
+static int reprime_dtd(struct ci_hdrc *ci, struct ci_hw_ep *hwep,
+					   struct td_node *node)
+{
+	hwep->qh.ptr->td.next = node->dma;
+	hwep->qh.ptr->td.token &=
+		cpu_to_le32(~(TD_STATUS_HALTED | TD_STATUS_ACTIVE));
+
+	/* Synchronize before ep prime */
+	wmb();
+
+	return hw_ep_prime(ci, hwep->num, hwep->dir,
+				hwep->type == USB_ENDPOINT_XFER_CONTROL);
+}
+
 /**
  * _hardware_dequeue: handles a request at hardware level
  * @gadget: gadget
@@ -535,6 +547,7 @@ static int _hardware_dequeue(struct ci_hw_ep *hwep, struct ci_hw_req *hwreq)
 	struct td_node *node, *tmpnode;
 	unsigned remaining_length;
 	unsigned actual = hwreq->req.length;
+	struct ci_hdrc *ci = hwep->ci;
 
 	if (hwreq->req.status != -EALREADY)
 		return -EINVAL;
@@ -544,6 +557,11 @@ static int _hardware_dequeue(struct ci_hw_ep *hwep, struct ci_hw_req *hwreq)
 	list_for_each_entry_safe(node, tmpnode, &hwreq->tds, td) {
 		tmptoken = le32_to_cpu(node->ptr->token);
 		if ((TD_STATUS_ACTIVE & tmptoken) != 0) {
+			int n = hw_ep_bit(hwep->num, hwep->dir);
+
+			if (ci->rev == CI_REVISION_24)
+				if (!hw_read(ci, OP_ENDPTSTAT, BIT(n)))
+					reprime_dtd(ci, hwep, node);
 			hwreq->req.status = -EALREADY;
 			return -EBUSY;
 		}
@@ -929,6 +947,13 @@ __acquires(hwep->lock)
 	return retval;
 }
 
+static int otg_a_alt_hnp_support(struct ci_hdrc *ci)
+{
+	dev_warn(&ci->gadget.dev,
+		"connect the device to an alternate port if you want HNP\n");
+	return isr_setup_status_phase(ci);
+}
+
 /**
  * isr_setup_packet_handler: setup packet handler
  * @ci: UDC descriptor
@@ -1061,6 +1086,10 @@ __acquires(ci->lock)
 							ci);
 				}
 				break;
+			case USB_DEVICE_A_ALT_HNP_SUPPORT:
+				if (ci_otg_is_fsm_mode(ci))
+					err = otg_a_alt_hnp_support(ci);
+				break;
 			default:
 				goto delegate;
 			}
@@ -1151,10 +1180,13 @@ static int ep_enable(struct usb_ep *ep,
 
 	/* only internal SW should enable ctrl endpts */
 
-	hwep->ep.desc = desc;
-
-	if (!list_empty(&hwep->qh.queue))
+	if (!list_empty(&hwep->qh.queue)) {
 		dev_warn(hwep->ci->dev, "enabling a non-empty endpoint!\n");
+		spin_unlock_irqrestore(hwep->lock, flags);
+		return -EBUSY;
+	}
+
+	hwep->ep.desc = desc;
 
 	hwep->dir  = usb_endpoint_dir_in(desc) ? TX : RX;
 	hwep->num  = usb_endpoint_num(desc);
@@ -1474,7 +1506,9 @@ static int ci_udc_vbus_session(struct usb_gadget *_gadget, int is_active)
 			hw_device_reset(ci);
 			hw_device_state(ci, ci->ep0out->qh.dma);
 			usb_gadget_set_state(_gadget, USB_STATE_POWERED);
+			usb_udc_vbus_handler(_gadget, true);
 		} else {
+			usb_udc_vbus_handler(_gadget, false);
 			if (ci->driver)
 				ci->driver->disconnect(&ci->gadget);
 			hw_device_state(ci, 0);
@@ -1540,13 +1574,16 @@ static int ci_udc_pullup(struct usb_gadget *_gadget, int is_on)
 {
 	struct ci_hdrc *ci = container_of(_gadget, struct ci_hdrc, gadget);
 
-	if (!ci->vbus_active)
-		return -EOPNOTSUPP;
+	/* Data+ pullup controlled by OTG state machine in OTG fsm mode */
+	if (ci_otg_is_fsm_mode(ci))
+		return 0;
 
+	pm_runtime_get_sync(&ci->gadget.dev);
 	if (is_on)
 		hw_write(ci, OP_USBCMD, USBCMD_RS, USBCMD_RS);
 	else
 		hw_write(ci, OP_USBCMD, USBCMD_RS, 0);
+	pm_runtime_put_sync(&ci->gadget.dev);
 
 	return 0;
 }
@@ -1676,6 +1713,7 @@ static int ci_udc_start(struct usb_gadget *gadget,
 		spin_lock_irqsave(&ci->lock, flags);
 		hw_device_reset(ci);
 	} else {
+		usb_udc_vbus_handler(&ci->gadget, false);
 		pm_runtime_put_sync(&ci->gadget.dev);
 		return retval;
 	}
diff --git a/drivers/usb/chipidea/usbmisc_imx.c b/drivers/usb/chipidea/usbmisc_imx.c
index c3c6225b8acf..140945cb124f 100644
--- a/drivers/usb/chipidea/usbmisc_imx.c
+++ b/drivers/usb/chipidea/usbmisc_imx.c
@@ -11,7 +11,6 @@
 
 #include <linux/module.h>
 #include <linux/of_platform.h>
-#include <linux/clk.h>
 #include <linux/err.h>
 #include <linux/io.h>
 #include <linux/delay.h>
@@ -56,6 +55,19 @@
 #define MX53_USB_PLL_DIV_24_MHZ		0x01
 
 #define MX6_BM_OVER_CUR_DIS		BIT(7)
+#define MX6_BM_WAKEUP_ENABLE		BIT(10)
+#define MX6_BM_ID_WAKEUP		BIT(16)
+#define MX6_BM_VBUS_WAKEUP		BIT(17)
+#define MX6SX_BM_DPDM_WAKEUP_EN		BIT(29)
+#define MX6_BM_WAKEUP_INTR		BIT(31)
+#define MX6_USB_OTG1_PHY_CTRL		0x18
+/* For imx6dql, it is host-only controller, for later imx6, it is otg's */
+#define MX6_USB_OTG2_PHY_CTRL		0x1c
+#define MX6SX_USB_VBUS_WAKEUP_SOURCE(v)	(v << 8)
+#define MX6SX_USB_VBUS_WAKEUP_SOURCE_VBUS	MX6SX_USB_VBUS_WAKEUP_SOURCE(0)
+#define MX6SX_USB_VBUS_WAKEUP_SOURCE_AVALID	MX6SX_USB_VBUS_WAKEUP_SOURCE(1)
+#define MX6SX_USB_VBUS_WAKEUP_SOURCE_BVALID	MX6SX_USB_VBUS_WAKEUP_SOURCE(2)
+#define MX6SX_USB_VBUS_WAKEUP_SOURCE_SESS_END	MX6SX_USB_VBUS_WAKEUP_SOURCE(3)
 
 #define VF610_OVER_CUR_DIS		BIT(7)
 
@@ -64,12 +76,13 @@ struct usbmisc_ops {
 	int (*init)(struct imx_usbmisc_data *data);
 	/* It's called once after adding a usb device */
 	int (*post)(struct imx_usbmisc_data *data);
+	/* It's called when we need to enable/disable usb wakeup */
+	int (*set_wakeup)(struct imx_usbmisc_data *data, bool enabled);
 };
 
 struct imx_usbmisc {
 	void __iomem *base;
 	spinlock_t lock;
-	struct clk *clk;
 	const struct usbmisc_ops *ops;
 };
 
@@ -204,6 +217,35 @@ static int usbmisc_imx53_init(struct imx_usbmisc_data *data)
 	return 0;
 }
 
+static int usbmisc_imx6q_set_wakeup
+	(struct imx_usbmisc_data *data, bool enabled)
+{
+	struct imx_usbmisc *usbmisc = dev_get_drvdata(data->dev);
+	unsigned long flags;
+	u32 val;
+	u32 wakeup_setting = (MX6_BM_WAKEUP_ENABLE |
+		MX6_BM_VBUS_WAKEUP | MX6_BM_ID_WAKEUP);
+	int ret = 0;
+
+	if (data->index > 3)
+		return -EINVAL;
+
+	spin_lock_irqsave(&usbmisc->lock, flags);
+	val = readl(usbmisc->base + data->index * 4);
+	if (enabled) {
+		val |= wakeup_setting;
+		writel(val, usbmisc->base + data->index * 4);
+	} else {
+		if (val & MX6_BM_WAKEUP_INTR)
+			pr_debug("wakeup int at ci_hdrc.%d\n", data->index);
+		val &= ~wakeup_setting;
+		writel(val, usbmisc->base + data->index * 4);
+	}
+	spin_unlock_irqrestore(&usbmisc->lock, flags);
+
+	return ret;
+}
+
 static int usbmisc_imx6q_init(struct imx_usbmisc_data *data)
 {
 	struct imx_usbmisc *usbmisc = dev_get_drvdata(data->dev);
@@ -221,6 +263,36 @@ static int usbmisc_imx6q_init(struct imx_usbmisc_data *data)
 		spin_unlock_irqrestore(&usbmisc->lock, flags);
 	}
 
+	usbmisc_imx6q_set_wakeup(data, false);
+
+	return 0;
+}
+
+static int usbmisc_imx6sx_init(struct imx_usbmisc_data *data)
+{
+	void __iomem *reg = NULL;
+	unsigned long flags;
+	struct imx_usbmisc *usbmisc = dev_get_drvdata(data->dev);
+	u32 val;
+
+	usbmisc_imx6q_init(data);
+
+	if (data->index == 0 || data->index == 1) {
+		reg = usbmisc->base + MX6_USB_OTG1_PHY_CTRL + data->index * 4;
+		spin_lock_irqsave(&usbmisc->lock, flags);
+		/* Set vbus wakeup source as bvalid */
+		val = readl(reg);
+		writel(val | MX6SX_USB_VBUS_WAKEUP_SOURCE_BVALID, reg);
+		/*
+		 * Disable dp/dm wakeup in device mode when vbus is
+		 * not there.
+		 */
+		val = readl(usbmisc->base + data->index * 4);
+		writel(val & ~MX6SX_BM_DPDM_WAKEUP_EN,
+			usbmisc->base + data->index * 4);
+		spin_unlock_irqrestore(&usbmisc->lock, flags);
+	}
+
 	return 0;
 }
 
@@ -258,6 +330,7 @@ static const struct usbmisc_ops imx53_usbmisc_ops = {
 };
 
 static const struct usbmisc_ops imx6q_usbmisc_ops = {
+	.set_wakeup = usbmisc_imx6q_set_wakeup,
 	.init = usbmisc_imx6q_init,
 };
 
@@ -265,10 +338,19 @@ static const struct usbmisc_ops vf610_usbmisc_ops = {
 	.init = usbmisc_vf610_init,
 };
 
+static const struct usbmisc_ops imx6sx_usbmisc_ops = {
+	.set_wakeup = usbmisc_imx6q_set_wakeup,
+	.init = usbmisc_imx6sx_init,
+};
+
 int imx_usbmisc_init(struct imx_usbmisc_data *data)
 {
-	struct imx_usbmisc *usbmisc = dev_get_drvdata(data->dev);
+	struct imx_usbmisc *usbmisc;
+
+	if (!data)
+		return 0;
 
+	usbmisc = dev_get_drvdata(data->dev);
 	if (!usbmisc->ops->init)
 		return 0;
 	return usbmisc->ops->init(data);
@@ -277,14 +359,32 @@ EXPORT_SYMBOL_GPL(imx_usbmisc_init);
 
 int imx_usbmisc_init_post(struct imx_usbmisc_data *data)
 {
-	struct imx_usbmisc *usbmisc = dev_get_drvdata(data->dev);
+	struct imx_usbmisc *usbmisc;
 
+	if (!data)
+		return 0;
+
+	usbmisc = dev_get_drvdata(data->dev);
 	if (!usbmisc->ops->post)
 		return 0;
 	return usbmisc->ops->post(data);
 }
 EXPORT_SYMBOL_GPL(imx_usbmisc_init_post);
 
+int imx_usbmisc_set_wakeup(struct imx_usbmisc_data *data, bool enabled)
+{
+	struct imx_usbmisc *usbmisc;
+
+	if (!data)
+		return 0;
+
+	usbmisc = dev_get_drvdata(data->dev);
+	if (!usbmisc->ops->set_wakeup)
+		return 0;
+	return usbmisc->ops->set_wakeup(data, enabled);
+}
+EXPORT_SYMBOL_GPL(imx_usbmisc_set_wakeup);
+
 static const struct of_device_id usbmisc_imx_dt_ids[] = {
 	{
 		.compatible = "fsl,imx25-usbmisc",
@@ -314,6 +414,10 @@ static const struct of_device_id usbmisc_imx_dt_ids[] = {
 		.compatible = "fsl,vf610-usbmisc",
 		.data = &vf610_usbmisc_ops,
 	},
+	{
+		.compatible = "fsl,imx6sx-usbmisc",
+		.data = &imx6sx_usbmisc_ops,
+	},
 	{ /* sentinel */ }
 };
 MODULE_DEVICE_TABLE(of, usbmisc_imx_dt_ids);
@@ -322,7 +426,6 @@ static int usbmisc_imx_probe(struct platform_device *pdev)
 {
 	struct resource	*res;
 	struct imx_usbmisc *data;
-	int ret;
 	struct of_device_id *tmp_dev;
 
 	data = devm_kzalloc(&pdev->dev, sizeof(*data), GFP_KERNEL);
@@ -336,20 +439,6 @@ static int usbmisc_imx_probe(struct platform_device *pdev)
 	if (IS_ERR(data->base))
 		return PTR_ERR(data->base);
 
-	data->clk = devm_clk_get(&pdev->dev, NULL);
-	if (IS_ERR(data->clk)) {
-		dev_err(&pdev->dev,
-			"failed to get clock, err=%ld\n", PTR_ERR(data->clk));
-		return PTR_ERR(data->clk);
-	}
-
-	ret = clk_prepare_enable(data->clk);
-	if (ret) {
-		dev_err(&pdev->dev,
-			"clk_prepare_enable failed, err=%d\n", ret);
-		return ret;
-	}
-
 	tmp_dev = (struct of_device_id *)
 		of_match_device(usbmisc_imx_dt_ids, &pdev->dev);
 	data->ops = (const struct usbmisc_ops *)tmp_dev->data;
@@ -360,8 +449,6 @@ static int usbmisc_imx_probe(struct platform_device *pdev)
 
 static int usbmisc_imx_remove(struct platform_device *pdev)
 {
-	struct imx_usbmisc *usbmisc = dev_get_drvdata(&pdev->dev);
-	clk_disable_unprepare(usbmisc->clk);
 	return 0;
 }
 
diff --git a/drivers/usb/class/cdc-acm.c b/drivers/usb/class/cdc-acm.c
index e78720b59d67..3e15add665e2 100644
--- a/drivers/usb/class/cdc-acm.c
+++ b/drivers/usb/class/cdc-acm.c
@@ -360,7 +360,7 @@ static void acm_ctrl_irq(struct urb *urb)
 	}
 exit:
 	retval = usb_submit_urb(urb, GFP_ATOMIC);
-	if (retval)
+	if (retval && retval != -EPERM)
 		dev_err(&acm->control->dev, "%s - usb_submit_urb failed: %d\n",
 							__func__, retval);
 }
@@ -417,25 +417,33 @@ static void acm_read_bulk_callback(struct urb *urb)
 	struct acm_rb *rb = urb->context;
 	struct acm *acm = rb->instance;
 	unsigned long flags;
+	int status = urb->status;
 
 	dev_vdbg(&acm->data->dev, "%s - urb %d, len %d\n", __func__,
 					rb->index, urb->actual_length);
-	set_bit(rb->index, &acm->read_urbs_free);
 
 	if (!acm->dev) {
+		set_bit(rb->index, &acm->read_urbs_free);
 		dev_dbg(&acm->data->dev, "%s - disconnected\n", __func__);
 		return;
 	}
 
-	if (urb->status) {
+	if (status) {
+		set_bit(rb->index, &acm->read_urbs_free);
 		dev_dbg(&acm->data->dev, "%s - non-zero urb status: %d\n",
-							__func__, urb->status);
+							__func__, status);
 		return;
 	}
 
 	usb_mark_last_busy(acm->dev);
 
 	acm_process_read_urb(acm, urb);
+	/*
+	 * Unthrottle may run on another CPU which needs to see events
+	 * in the same order. Submission has an implict barrier
+	 */
+	smp_mb__before_atomic();
+	set_bit(rb->index, &acm->read_urbs_free);
 
 	/* throttle device if requested by tty */
 	spin_lock_irqsave(&acm->read_lock, flags);
@@ -454,13 +462,14 @@ static void acm_write_bulk(struct urb *urb)
 	struct acm_wb *wb = urb->context;
 	struct acm *acm = wb->instance;
 	unsigned long flags;
+	int status = urb->status;
 
-	if (urb->status	|| (urb->actual_length != urb->transfer_buffer_length))
+	if (status || (urb->actual_length != urb->transfer_buffer_length))
 		dev_vdbg(&acm->data->dev, "%s - len %d/%d, status %d\n",
 			__func__,
 			urb->actual_length,
 			urb->transfer_buffer_length,
-			urb->status);
+			status);
 
 	spin_lock_irqsave(&acm->write_lock, flags);
 	acm_write_done(acm, wb);
@@ -1650,6 +1659,8 @@ static int acm_reset_resume(struct usb_interface *intf)
 
 static const struct usb_device_id acm_ids[] = {
 	/* quirky and broken devices */
+	{ USB_DEVICE(0x076d, 0x0006), /* Denso Cradle CU-321 */
+	.driver_info = NO_UNION_NORMAL, },/* has no union descriptor */
 	{ USB_DEVICE(0x17ef, 0x7000), /* Lenovo USB modem */
 	.driver_info = NO_UNION_NORMAL, },/* has no union descriptor */
 	{ USB_DEVICE(0x0870, 0x0001), /* Metricom GS Modem */
diff --git a/drivers/usb/class/cdc-wdm.c b/drivers/usb/class/cdc-wdm.c
index a051a7a2b1bd..61ea87917433 100644
--- a/drivers/usb/class/cdc-wdm.c
+++ b/drivers/usb/class/cdc-wdm.c
@@ -245,7 +245,7 @@ static void wdm_int_callback(struct urb *urb)
 	case USB_CDC_NOTIFY_RESPONSE_AVAILABLE:
 		dev_dbg(&desc->intf->dev,
 			"NOTIFY_RESPONSE_AVAILABLE received: index %d len %d",
-			dr->wIndex, dr->wLength);
+			le16_to_cpu(dr->wIndex), le16_to_cpu(dr->wLength));
 		break;
 
 	case USB_CDC_NOTIFY_NETWORK_CONNECTION:
@@ -262,7 +262,9 @@ static void wdm_int_callback(struct urb *urb)
 		clear_bit(WDM_POLL_RUNNING, &desc->flags);
 		dev_err(&desc->intf->dev,
 			"unknown notification %d received: index %d len %d\n",
-			dr->bNotificationType, dr->wIndex, dr->wLength);
+			dr->bNotificationType,
+			le16_to_cpu(dr->wIndex),
+			le16_to_cpu(dr->wLength));
 		goto exit;
 	}
 
@@ -339,7 +341,7 @@ static ssize_t wdm_write
 	desc->werr = 0;
 	spin_unlock_irq(&desc->iuspin);
 	if (we < 0)
-		return -EIO;
+		return usb_translate_errors(we);
 
 	buf = kmalloc(count, GFP_KERNEL);
 	if (!buf) {
@@ -349,30 +351,25 @@ static ssize_t wdm_write
 
 	r = copy_from_user(buf, buffer, count);
 	if (r > 0) {
-		kfree(buf);
 		rv = -EFAULT;
-		goto outnl;
+		goto out_free_mem;
 	}
 
 	/* concurrent writes and disconnect */
 	r = mutex_lock_interruptible(&desc->wlock);
 	rv = -ERESTARTSYS;
-	if (r) {
-		kfree(buf);
-		goto outnl;
-	}
+	if (r)
+		goto out_free_mem;
 
 	if (test_bit(WDM_DISCONNECTING, &desc->flags)) {
-		kfree(buf);
 		rv = -ENODEV;
-		goto outnp;
+		goto out_free_mem_lock;
 	}
 
 	r = usb_autopm_get_interface(desc->intf);
 	if (r < 0) {
-		kfree(buf);
 		rv = usb_translate_errors(r);
-		goto outnp;
+		goto out_free_mem_lock;
 	}
 
 	if (!(file->f_flags & O_NONBLOCK))
@@ -386,9 +383,8 @@ static ssize_t wdm_write
 		r = -EIO;
 
 	if (r < 0) {
-		kfree(buf);
 		rv = r;
-		goto out;
+		goto out_free_mem_pm;
 	}
 
 	req = desc->orq;
@@ -408,28 +404,35 @@ static ssize_t wdm_write
 			     USB_RECIP_INTERFACE);
 	req->bRequest = USB_CDC_SEND_ENCAPSULATED_COMMAND;
 	req->wValue = 0;
-	req->wIndex = desc->inum;
+	req->wIndex = desc->inum; /* already converted */
 	req->wLength = cpu_to_le16(count);
 	set_bit(WDM_IN_USE, &desc->flags);
 	desc->outbuf = buf;
 
 	rv = usb_submit_urb(desc->command, GFP_KERNEL);
 	if (rv < 0) {
-		kfree(buf);
 		desc->outbuf = NULL;
 		clear_bit(WDM_IN_USE, &desc->flags);
 		dev_err(&desc->intf->dev, "Tx URB error: %d\n", rv);
 		rv = usb_translate_errors(rv);
+		goto out_free_mem_pm;
 	} else {
 		dev_dbg(&desc->intf->dev, "Tx URB has been submitted index=%d",
-			req->wIndex);
+			le16_to_cpu(req->wIndex));
 	}
-out:
+
 	usb_autopm_put_interface(desc->intf);
-outnp:
 	mutex_unlock(&desc->wlock);
 outnl:
 	return rv < 0 ? rv : count;
+
+out_free_mem_pm:
+	usb_autopm_put_interface(desc->intf);
+out_free_mem_lock:
+	mutex_unlock(&desc->wlock);
+out_free_mem:
+	kfree(buf);
+	return rv;
 }
 
 /*
@@ -519,9 +522,9 @@ retry:
 		spin_lock_irq(&desc->iuspin);
 
 		if (desc->rerr) { /* read completed, error happened */
+			rv = usb_translate_errors(desc->rerr);
 			desc->rerr = 0;
 			spin_unlock_irq(&desc->iuspin);
-			rv = -EIO;
 			goto err;
 		}
 		/*
@@ -820,7 +823,7 @@ static int wdm_create(struct usb_interface *intf, struct usb_endpoint_descriptor
 	desc->irq->bRequestType = (USB_DIR_IN | USB_TYPE_CLASS | USB_RECIP_INTERFACE);
 	desc->irq->bRequest = USB_CDC_GET_ENCAPSULATED_RESPONSE;
 	desc->irq->wValue = 0;
-	desc->irq->wIndex = desc->inum;
+	desc->irq->wIndex = desc->inum; /* already converted */
 	desc->irq->wLength = cpu_to_le16(desc->wMaxCommand);
 
 	usb_fill_control_urb(
diff --git a/drivers/usb/common/usb-otg-fsm.c b/drivers/usb/common/usb-otg-fsm.c
index c6b35b77dab7..61d538aa2346 100644
--- a/drivers/usb/common/usb-otg-fsm.c
+++ b/drivers/usb/common/usb-otg-fsm.c
@@ -150,9 +150,9 @@ static int otg_set_state(struct otg_fsm *fsm, enum usb_otg_state new_state)
 		break;
 	case OTG_STATE_B_PERIPHERAL:
 		otg_chrg_vbus(fsm, 0);
-		otg_loc_conn(fsm, 1);
 		otg_loc_sof(fsm, 0);
 		otg_set_protocol(fsm, PROTO_GADGET);
+		otg_loc_conn(fsm, 1);
 		break;
 	case OTG_STATE_B_WAIT_ACON:
 		otg_chrg_vbus(fsm, 0);
@@ -213,10 +213,10 @@ static int otg_set_state(struct otg_fsm *fsm, enum usb_otg_state new_state)
 
 		break;
 	case OTG_STATE_A_PERIPHERAL:
-		otg_loc_conn(fsm, 1);
 		otg_loc_sof(fsm, 0);
 		otg_set_protocol(fsm, PROTO_GADGET);
 		otg_drv_vbus(fsm, 1);
+		otg_loc_conn(fsm, 1);
 		otg_add_timer(fsm, A_BIDL_ADIS);
 		break;
 	case OTG_STATE_A_WAIT_VFALL:
diff --git a/drivers/usb/core/devio.c b/drivers/usb/core/devio.c
index 66abdbcfbfa5..4b0448c26810 100644
--- a/drivers/usb/core/devio.c
+++ b/drivers/usb/core/devio.c
@@ -501,6 +501,7 @@ static void async_completed(struct urb *urb)
 	as->status = urb->status;
 	signr = as->signr;
 	if (signr) {
+		memset(&sinfo, 0, sizeof(sinfo));
 		sinfo.si_signo = as->signr;
 		sinfo.si_errno = as->status;
 		sinfo.si_code = SI_ASYNCIO;
@@ -2382,6 +2383,7 @@ static void usbdev_remove(struct usb_device *udev)
 		wake_up_all(&ps->wait);
 		list_del_init(&ps->list);
 		if (ps->discsignr) {
+			memset(&sinfo, 0, sizeof(sinfo));
 			sinfo.si_signo = ps->discsignr;
 			sinfo.si_errno = EPIPE;
 			sinfo.si_code = SI_ASYNCIO;
@@ -2406,7 +2408,7 @@ static int usbdev_notify(struct notifier_block *self,
 }
 
 static struct notifier_block usbdev_nb = {
-	.notifier_call = 	usbdev_notify,
+	.notifier_call =	usbdev_notify,
 };
 
 static struct cdev usb_device_cdev;
diff --git a/drivers/usb/core/hub.c b/drivers/usb/core/hub.c
index d7c3d5a35946..3b7151687776 100644
--- a/drivers/usb/core/hub.c
+++ b/drivers/usb/core/hub.c
@@ -3406,10 +3406,10 @@ int usb_port_resume(struct usb_device *udev, pm_message_t msg)
 	if (status) {
 		dev_dbg(&port_dev->dev, "can't resume, status %d\n", status);
 	} else {
-		/* drive resume for at least 20 msec */
+		/* drive resume for USB_RESUME_TIMEOUT msec */
 		dev_dbg(&udev->dev, "usb %sresume\n",
 				(PMSG_IS_AUTO(msg) ? "auto-" : ""));
-		msleep(25);
+		msleep(USB_RESUME_TIMEOUT);
 
 		/* Virtual root hubs can trigger on GET_PORT_STATUS to
 		 * stop resume signaling.  Then finish the resume
diff --git a/drivers/usb/core/usb.c b/drivers/usb/core/usb.c
index b1fb9aef0f5b..8d5b2f4113cd 100644
--- a/drivers/usb/core/usb.c
+++ b/drivers/usb/core/usb.c
@@ -49,6 +49,22 @@ const char *usbcore_name = "usbcore";
 
 static bool nousb;	/* Disable USB when built into kernel image */
 
+/* To disable USB, kernel command line is 'nousb' not 'usbcore.nousb' */
+#ifdef MODULE
+module_param(nousb, bool, 0444);
+#else
+core_param(nousb, nousb, bool, 0444);
+#endif
+
+/*
+ * for external read access to <nousb>
+ */
+int usb_disabled(void)
+{
+	return nousb;
+}
+EXPORT_SYMBOL_GPL(usb_disabled);
+
 #ifdef	CONFIG_PM
 static int usb_autosuspend_delay = 2;		/* Default delay value,
 						 * in seconds */
@@ -964,22 +980,6 @@ void usb_buffer_unmap_sg(const struct usb_device *dev, int is_in,
 EXPORT_SYMBOL_GPL(usb_buffer_unmap_sg);
 #endif
 
-/* To disable USB, kernel command line is 'nousb' not 'usbcore.nousb' */
-#ifdef MODULE
-module_param(nousb, bool, 0444);
-#else
-core_param(nousb, nousb, bool, 0444);
-#endif
-
-/*
- * for external read access to <nousb>
- */
-int usb_disabled(void)
-{
-	return nousb;
-}
-EXPORT_SYMBOL_GPL(usb_disabled);
-
 /*
  * Notifications of device and interface registration
  */
@@ -1045,7 +1045,7 @@ static void usb_debugfs_cleanup(void)
 static int __init usb_init(void)
 {
 	int retval;
-	if (nousb) {
+	if (usb_disabled()) {
 		pr_info("%s: USB support disabled\n", usbcore_name);
 		return 0;
 	}
@@ -1102,7 +1102,7 @@ out:
 static void __exit usb_exit(void)
 {
 	/* This will matter if shutdown/reboot does exitcalls. */
-	if (nousb)
+	if (usb_disabled())
 		return;
 
 	usb_deregister_device_driver(&usb_generic_driver);
diff --git a/drivers/usb/dwc2/Kconfig b/drivers/usb/dwc2/Kconfig
index 76b9ba4dc925..1bcb36ae6505 100644
--- a/drivers/usb/dwc2/Kconfig
+++ b/drivers/usb/dwc2/Kconfig
@@ -59,11 +59,13 @@ config USB_DWC2_PLATFORM
 
 config USB_DWC2_PCI
 	tristate "DWC2 PCI"
-	depends on USB_DWC2_HOST && PCI
-	default USB_DWC2_HOST
+	depends on PCI
+	default n
+	select USB_DWC2_PLATFORM
+	select NOP_USB_XCEIV
 	help
 	  The Designware USB2.0 PCI interface module for controllers
-	  connected to a PCI bus. This is only used for host mode.
+	  connected to a PCI bus.
 
 config USB_DWC2_DEBUG
 	bool "Enable Debugging Messages"
diff --git a/drivers/usb/dwc2/Makefile b/drivers/usb/dwc2/Makefile
index 8f752679752a..f07b425eaff3 100644
--- a/drivers/usb/dwc2/Makefile
+++ b/drivers/usb/dwc2/Makefile
@@ -19,10 +19,8 @@ endif
 # mode. The PCI bus interface module will called dwc2_pci.ko and the platform
 # interface module will be called dwc2_platform.ko.
 
-ifneq ($(CONFIG_USB_DWC2_PCI),)
-	obj-$(CONFIG_USB_DWC2)		+= dwc2_pci.o
-	dwc2_pci-y			:= pci.o
-endif
+obj-$(CONFIG_USB_DWC2_PCI)		+= dwc2_pci.o
+dwc2_pci-y				:= pci.o
 
 obj-$(CONFIG_USB_DWC2_PLATFORM)		+= dwc2_platform.o
 dwc2_platform-y				:= platform.o
diff --git a/drivers/usb/dwc2/core.h b/drivers/usb/dwc2/core.h
index f74304b12652..836c012c7707 100644
--- a/drivers/usb/dwc2/core.h
+++ b/drivers/usb/dwc2/core.h
@@ -593,6 +593,8 @@ struct dwc2_hsotg {
 	struct dwc2_core_params *core_params;
 	enum usb_otg_state op_state;
 	enum usb_dr_mode dr_mode;
+	unsigned int hcd_enabled:1;
+	unsigned int gadget_enabled:1;
 
 	struct phy *phy;
 	struct usb_phy *uphy;
diff --git a/drivers/usb/dwc2/core_intr.c b/drivers/usb/dwc2/core_intr.c
index 02e3e2d4ea56..6cf047878dba 100644
--- a/drivers/usb/dwc2/core_intr.c
+++ b/drivers/usb/dwc2/core_intr.c
@@ -377,6 +377,9 @@ static void dwc2_handle_disconnect_intr(struct dwc2_hsotg *hsotg)
 		dwc2_is_host_mode(hsotg) ? "Host" : "Device",
 		dwc2_op_state_str(hsotg));
 
+	if (hsotg->op_state == OTG_STATE_A_HOST)
+		dwc2_hcd_disconnect(hsotg);
+
 	/* Change to L3 (OFF) state */
 	hsotg->lx_state = DWC2_L3;
 
diff --git a/drivers/usb/dwc2/hcd.c b/drivers/usb/dwc2/hcd.c
index c78c8740db1d..fbbbac2150a5 100644
--- a/drivers/usb/dwc2/hcd.c
+++ b/drivers/usb/dwc2/hcd.c
@@ -257,6 +257,14 @@ static void dwc2_hcd_cleanup_channels(struct dwc2_hsotg *hsotg)
 		 */
 		channel->qh = NULL;
 	}
+	/* All channels have been freed, mark them available */
+	if (hsotg->core_params->uframe_sched > 0) {
+		hsotg->available_host_channels =
+			hsotg->core_params->host_channels;
+	} else {
+		hsotg->non_periodic_channels = 0;
+		hsotg->periodic_channels = 0;
+	}
 }
 
 /**
@@ -1527,7 +1535,7 @@ static int dwc2_hcd_hub_control(struct dwc2_hsotg *hsotg, u16 typereq,
 			hprt0 |= HPRT0_RES;
 			writel(hprt0, hsotg->regs + HPRT0);
 			hprt0 &= ~HPRT0_SUSP;
-			usleep_range(100000, 150000);
+			msleep(USB_RESUME_TIMEOUT);
 
 			hprt0 &= ~HPRT0_RES;
 			writel(hprt0, hsotg->regs + HPRT0);
@@ -1608,7 +1616,7 @@ static int dwc2_hcd_hub_control(struct dwc2_hsotg *hsotg, u16 typereq,
 		dev_dbg(hsotg->dev, "GetHubDescriptor\n");
 		hub_desc = (struct usb_hub_descriptor *)buf;
 		hub_desc->bDescLength = 9;
-		hub_desc->bDescriptorType = 0x29;
+		hub_desc->bDescriptorType = USB_DT_HUB;
 		hub_desc->bNbrPorts = 1;
 		hub_desc->wHubCharacteristics =
 			cpu_to_le16(HUB_CHAR_COMMON_LPSM |
diff --git a/drivers/usb/dwc2/pci.c b/drivers/usb/dwc2/pci.c
index a4e724b0a62e..ae419615a176 100644
--- a/drivers/usb/dwc2/pci.c
+++ b/drivers/usb/dwc2/pci.c
@@ -50,113 +50,97 @@
 
 #include <linux/usb/hcd.h>
 #include <linux/usb/ch11.h>
+#include <linux/platform_device.h>
+#include <linux/usb/usb_phy_generic.h>
 
-#include "core.h"
-#include "hcd.h"
-
-#define PCI_VENDOR_ID_SYNOPSYS		0x16c3
 #define PCI_PRODUCT_ID_HAPS_HSOTG	0xabc0
 
-static const char dwc2_driver_name[] = "dwc2";
-
-static const struct dwc2_core_params dwc2_module_params = {
-	.otg_cap			= -1,
-	.otg_ver			= -1,
-	.dma_enable			= -1,
-	.dma_desc_enable		= 0,
-	.speed				= -1,
-	.enable_dynamic_fifo		= -1,
-	.en_multiple_tx_fifo		= -1,
-	.host_rx_fifo_size		= 1024,
-	.host_nperio_tx_fifo_size	= 256,
-	.host_perio_tx_fifo_size	= 1024,
-	.max_transfer_size		= 65535,
-	.max_packet_count		= 511,
-	.host_channels			= -1,
-	.phy_type			= -1,
-	.phy_utmi_width			= -1,
-	.phy_ulpi_ddr			= -1,
-	.phy_ulpi_ext_vbus		= -1,
-	.i2c_enable			= -1,
-	.ulpi_fs_ls			= -1,
-	.host_support_fs_ls_low_power	= -1,
-	.host_ls_low_power_phy_clk	= -1,
-	.ts_dline			= -1,
-	.reload_ctl			= -1,
-	.ahbcfg				= -1,
-	.uframe_sched			= -1,
+static const char dwc2_driver_name[] = "dwc2-pci";
+
+struct dwc2_pci_glue {
+	struct platform_device *dwc2;
+	struct platform_device *phy;
 };
 
-/**
- * dwc2_driver_remove() - Called when the DWC_otg core is unregistered with the
- * DWC_otg driver
- *
- * @dev: Bus device
- *
- * This routine is called, for example, when the rmmod command is executed. The
- * device may or may not be electrically present. If it is present, the driver
- * stops device processing. Any resources used on behalf of this device are
- * freed.
- */
-static void dwc2_driver_remove(struct pci_dev *dev)
+static void dwc2_pci_remove(struct pci_dev *pci)
 {
-	struct dwc2_hsotg *hsotg = pci_get_drvdata(dev);
+	struct dwc2_pci_glue *glue = pci_get_drvdata(pci);
 
-	dwc2_hcd_remove(hsotg);
-	pci_disable_device(dev);
+	platform_device_unregister(glue->dwc2);
+	usb_phy_generic_unregister(glue->phy);
+	kfree(glue);
+	pci_set_drvdata(pci, NULL);
 }
 
-/**
- * dwc2_driver_probe() - Called when the DWC_otg core is bound to the DWC_otg
- * driver
- *
- * @dev: Bus device
- *
- * This routine creates the driver components required to control the device
- * (core, HCD, and PCD) and initializes the device. The driver components are
- * stored in a dwc2_hsotg structure. A reference to the dwc2_hsotg is saved
- * in the device private data. This allows the driver to access the dwc2_hsotg
- * structure on subsequent calls to driver methods for this device.
- */
-static int dwc2_driver_probe(struct pci_dev *dev,
-			     const struct pci_device_id *id)
+static int dwc2_pci_probe(struct pci_dev *pci,
+		const struct pci_device_id *id)
 {
-	struct dwc2_hsotg *hsotg;
-	int retval;
+	struct resource		res[2];
+	struct platform_device	*dwc2;
+	struct platform_device	*phy;
+	int			ret;
+	struct device		*dev = &pci->dev;
+	struct dwc2_pci_glue	*glue;
+
+	ret = pcim_enable_device(pci);
+	if (ret) {
+		dev_err(dev, "failed to enable pci device\n");
+		return -ENODEV;
+	}
+
+	pci_set_master(pci);
 
-	hsotg = devm_kzalloc(&dev->dev, sizeof(*hsotg), GFP_KERNEL);
-	if (!hsotg)
+	dwc2 = platform_device_alloc("dwc2", PLATFORM_DEVID_AUTO);
+	if (!dwc2) {
+		dev_err(dev, "couldn't allocate dwc2 device\n");
 		return -ENOMEM;
+	}
 
-	hsotg->dev = &dev->dev;
-	hsotg->regs = devm_ioremap_resource(&dev->dev, &dev->resource[0]);
-	if (IS_ERR(hsotg->regs))
-		return PTR_ERR(hsotg->regs);
+	memset(res, 0x00, sizeof(struct resource) * ARRAY_SIZE(res));
 
-	dev_dbg(&dev->dev, "mapped PA %08lx to VA %p\n",
-		(unsigned long)pci_resource_start(dev, 0), hsotg->regs);
+	res[0].start	= pci_resource_start(pci, 0);
+	res[0].end	= pci_resource_end(pci, 0);
+	res[0].name	= "dwc2";
+	res[0].flags	= IORESOURCE_MEM;
 
-	if (pci_enable_device(dev) < 0)
-		return -ENODEV;
+	res[1].start	= pci->irq;
+	res[1].name	= "dwc2";
+	res[1].flags	= IORESOURCE_IRQ;
 
-	pci_set_master(dev);
+	ret = platform_device_add_resources(dwc2, res, ARRAY_SIZE(res));
+	if (ret) {
+		dev_err(dev, "couldn't add resources to dwc2 device\n");
+		return ret;
+	}
 
-	retval = devm_request_irq(hsotg->dev, dev->irq,
-				  dwc2_handle_common_intr, IRQF_SHARED,
-				  dev_name(hsotg->dev), hsotg);
-	if (retval)
-		return retval;
+	dwc2->dev.parent = dev;
 
-	spin_lock_init(&hsotg->lock);
-	retval = dwc2_hcd_init(hsotg, dev->irq, &dwc2_module_params);
-	if (retval) {
-		pci_disable_device(dev);
-		return retval;
+	phy = usb_phy_generic_register();
+	if (IS_ERR(phy)) {
+		dev_err(dev, "error registering generic PHY (%ld)\n",
+			PTR_ERR(phy));
+		return PTR_ERR(phy);
 	}
 
-	pci_set_drvdata(dev, hsotg);
+	ret = platform_device_add(dwc2);
+	if (ret) {
+		dev_err(dev, "failed to register dwc2 device\n");
+		goto err;
+	}
+
+	glue = kzalloc(sizeof(*glue), GFP_KERNEL);
+	if (!glue)
+		return -ENOMEM;
+
+	glue->phy = phy;
+	glue->dwc2 = dwc2;
+	pci_set_drvdata(pci, glue);
 
-	return retval;
+	return 0;
+err:
+	usb_phy_generic_unregister(phy);
+	platform_device_put(dwc2);
+	return ret;
 }
 
 static const struct pci_device_id dwc2_pci_ids[] = {
@@ -174,8 +158,8 @@ MODULE_DEVICE_TABLE(pci, dwc2_pci_ids);
 static struct pci_driver dwc2_pci_driver = {
 	.name = dwc2_driver_name,
 	.id_table = dwc2_pci_ids,
-	.probe = dwc2_driver_probe,
-	.remove = dwc2_driver_remove,
+	.probe = dwc2_pci_probe,
+	.remove = dwc2_pci_remove,
 };
 
 module_pci_driver(dwc2_pci_driver);
diff --git a/drivers/usb/dwc2/platform.c b/drivers/usb/dwc2/platform.c
index ae095f009b4f..185663e0b5f4 100644
--- a/drivers/usb/dwc2/platform.c
+++ b/drivers/usb/dwc2/platform.c
@@ -121,8 +121,10 @@ static int dwc2_driver_remove(struct platform_device *dev)
 {
 	struct dwc2_hsotg *hsotg = platform_get_drvdata(dev);
 
-	dwc2_hcd_remove(hsotg);
-	s3c_hsotg_remove(hsotg);
+	if (hsotg->hcd_enabled)
+		dwc2_hcd_remove(hsotg);
+	if (hsotg->gadget_enabled)
+		s3c_hsotg_remove(hsotg);
 
 	return 0;
 }
@@ -234,12 +236,23 @@ static int dwc2_driver_probe(struct platform_device *dev)
 
 	spin_lock_init(&hsotg->lock);
 	mutex_init(&hsotg->init_mutex);
-	retval = dwc2_gadget_init(hsotg, irq);
-	if (retval)
-		return retval;
-	retval = dwc2_hcd_init(hsotg, irq, params);
-	if (retval)
-		return retval;
+
+	if (hsotg->dr_mode != USB_DR_MODE_HOST) {
+		retval = dwc2_gadget_init(hsotg, irq);
+		if (retval)
+			return retval;
+		hsotg->gadget_enabled = 1;
+	}
+
+	if (hsotg->dr_mode != USB_DR_MODE_PERIPHERAL) {
+		retval = dwc2_hcd_init(hsotg, irq, params);
+		if (retval) {
+			if (hsotg->gadget_enabled)
+				s3c_hsotg_remove(hsotg);
+			return retval;
+		}
+		hsotg->hcd_enabled = 1;
+	}
 
 	platform_set_drvdata(dev, hsotg);
 
diff --git a/drivers/usb/dwc3/Kconfig b/drivers/usb/dwc3/Kconfig
index edbf9c85af7e..827c4f80379f 100644
--- a/drivers/usb/dwc3/Kconfig
+++ b/drivers/usb/dwc3/Kconfig
@@ -104,11 +104,4 @@ config USB_DWC3_DEBUG
 	help
 	  Say Y here to enable debugging messages on DWC3 Driver.
 
-config DWC3_HOST_USB3_LPM_ENABLE
-	bool "Enable USB3 LPM Capability"
-	depends on USB_DWC3_HOST=y || USB_DWC3_DUAL_ROLE=y
-	default n
-	help
-	  Select this when you want to enable USB3 LPM with dwc3 xhci host.
-
 endif
diff --git a/drivers/usb/dwc3/core.c b/drivers/usb/dwc3/core.c
index 9f0e209b8f6c..2bbab3d86fff 100644
--- a/drivers/usb/dwc3/core.c
+++ b/drivers/usb/dwc3/core.c
@@ -774,17 +774,13 @@ static int dwc3_probe(struct platform_device *pdev)
 	 * since it will be requested by the xhci-plat driver.
 	 */
 	regs = devm_ioremap_resource(dev, res);
-	if (IS_ERR(regs))
-		return PTR_ERR(regs);
+	if (IS_ERR(regs)) {
+		ret = PTR_ERR(regs);
+		goto err0;
+	}
 
 	dwc->regs	= regs;
 	dwc->regs_size	= resource_size(res);
-	/*
-	 * restore res->start back to its original value so that,
-	 * in case the probe is deferred, we don't end up getting error in
-	 * request the memory region the next time probe is called.
-	 */
-	res->start -= DWC3_GLOBALS_REGS_START;
 
 	/* default to highest possible threshold */
 	lpm_nyet_threshold = 0xff;
@@ -808,6 +804,8 @@ static int dwc3_probe(struct platform_device *pdev)
 				"snps,is-utmi-l1-suspend");
 		of_property_read_u8(node, "snps,hird-threshold",
 				&hird_threshold);
+		dwc->usb3_lpm_capable = of_property_read_bool(node,
+				"snps,usb3_lpm_capable");
 
 		dwc->needs_fifo_resize = of_property_read_bool(node,
 				"tx-fifo-resize");
@@ -848,6 +846,7 @@ static int dwc3_probe(struct platform_device *pdev)
 			hird_threshold = pdata->hird_threshold;
 
 		dwc->needs_fifo_resize = pdata->tx_fifo_resize;
+		dwc->usb3_lpm_capable = pdata->usb3_lpm_capable;
 		dwc->dr_mode = pdata->dr_mode;
 
 		dwc->disable_scramble_quirk = pdata->disable_scramble_quirk;
@@ -878,7 +877,7 @@ static int dwc3_probe(struct platform_device *pdev)
 
 	ret = dwc3_core_get_phy(dwc);
 	if (ret)
-		return ret;
+		goto err0;
 
 	spin_lock_init(&dwc->lock);
 	platform_set_drvdata(pdev, dwc);
@@ -899,7 +898,7 @@ static int dwc3_probe(struct platform_device *pdev)
 	if (ret) {
 		dev_err(dwc->dev, "failed to allocate event buffers\n");
 		ret = -ENOMEM;
-		goto err0;
+		goto err1;
 	}
 
 	if (IS_ENABLED(CONFIG_USB_DWC3_HOST))
@@ -913,65 +912,81 @@ static int dwc3_probe(struct platform_device *pdev)
 	ret = dwc3_core_init(dwc);
 	if (ret) {
 		dev_err(dev, "failed to initialize core\n");
-		goto err0;
+		goto err1;
 	}
 
 	usb_phy_set_suspend(dwc->usb2_phy, 0);
 	usb_phy_set_suspend(dwc->usb3_phy, 0);
 	ret = phy_power_on(dwc->usb2_generic_phy);
 	if (ret < 0)
-		goto err1;
+		goto err2;
 
 	ret = phy_power_on(dwc->usb3_generic_phy);
 	if (ret < 0)
-		goto err_usb2phy_power;
+		goto err3;
 
 	ret = dwc3_event_buffers_setup(dwc);
 	if (ret) {
 		dev_err(dwc->dev, "failed to setup event buffers\n");
-		goto err_usb3phy_power;
+		goto err4;
 	}
 
 	ret = dwc3_core_init_mode(dwc);
 	if (ret)
-		goto err2;
+		goto err5;
 
 	ret = dwc3_debugfs_init(dwc);
 	if (ret) {
 		dev_err(dev, "failed to initialize debugfs\n");
-		goto err3;
+		goto err6;
 	}
 
 	pm_runtime_allow(dev);
 
 	return 0;
 
-err3:
+err6:
 	dwc3_core_exit_mode(dwc);
 
-err2:
+err5:
 	dwc3_event_buffers_cleanup(dwc);
 
-err_usb3phy_power:
+err4:
 	phy_power_off(dwc->usb3_generic_phy);
 
-err_usb2phy_power:
+err3:
 	phy_power_off(dwc->usb2_generic_phy);
 
-err1:
+err2:
 	usb_phy_set_suspend(dwc->usb2_phy, 1);
 	usb_phy_set_suspend(dwc->usb3_phy, 1);
 	dwc3_core_exit(dwc);
 
-err0:
+err1:
 	dwc3_free_event_buffers(dwc);
 
+err0:
+	/*
+	 * restore res->start back to its original value so that, in case the
+	 * probe is deferred, we don't end up getting error in request the
+	 * memory region the next time probe is called.
+	 */
+	res->start -= DWC3_GLOBALS_REGS_START;
+
 	return ret;
 }
 
 static int dwc3_remove(struct platform_device *pdev)
 {
 	struct dwc3	*dwc = platform_get_drvdata(pdev);
+	struct resource *res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+
+	/*
+	 * restore res->start back to its original value so that, in case the
+	 * probe is deferred, we don't end up getting error in request the
+	 * memory region the next time probe is called.
+	 */
+	res->start -= DWC3_GLOBALS_REGS_START;
 
 	dwc3_debugfs_exit(dwc);
 	dwc3_core_exit_mode(dwc);
diff --git a/drivers/usb/dwc3/core.h b/drivers/usb/dwc3/core.h
index d201910b892f..fdab715a0631 100644
--- a/drivers/usb/dwc3/core.h
+++ b/drivers/usb/dwc3/core.h
@@ -689,6 +689,7 @@ struct dwc3_scratchpad_array {
  * @setup_packet_pending: true when there's a Setup Packet in FIFO. Workaround
  * @start_config_issued: true when StartConfig command has been issued
  * @three_stage_setup: set if we perform a three phase setup
+ * @usb3_lpm_capable: set if hadrware supports Link Power Management
  * @disable_scramble_quirk: set if we enable the disable scramble quirk
  * @u2exit_lfps_quirk: set if we enable u2exit lfps quirk
  * @u2ss_inp3_quirk: set if we enable P3 OK for U2/SS Inactive quirk
@@ -812,6 +813,7 @@ struct dwc3 {
 	unsigned		setup_packet_pending:1;
 	unsigned		start_config_issued:1;
 	unsigned		three_stage_setup:1;
+	unsigned		usb3_lpm_capable:1;
 
 	unsigned		disable_scramble_quirk:1;
 	unsigned		u2exit_lfps_quirk:1;
diff --git a/drivers/usb/dwc3/dwc3-omap.c b/drivers/usb/dwc3/dwc3-omap.c
index 172d64e585b6..edba5348be18 100644
--- a/drivers/usb/dwc3/dwc3-omap.c
+++ b/drivers/usb/dwc3/dwc3-omap.c
@@ -205,6 +205,18 @@ static void dwc3_omap_write_irq0_set(struct dwc3_omap *omap, u32 value)
 						omap->irq0_offset, value);
 }
 
+static void dwc3_omap_write_irqmisc_clr(struct dwc3_omap *omap, u32 value)
+{
+	dwc3_omap_writel(omap->base, USBOTGSS_IRQENABLE_CLR_MISC +
+						omap->irqmisc_offset, value);
+}
+
+static void dwc3_omap_write_irq0_clr(struct dwc3_omap *omap, u32 value)
+{
+	dwc3_omap_writel(omap->base, USBOTGSS_IRQENABLE_CLR_0 -
+						omap->irq0_offset, value);
+}
+
 static void dwc3_omap_set_mailbox(struct dwc3_omap *omap,
 	enum omap_dwc3_vbus_id_status status)
 {
@@ -313,15 +325,6 @@ static irqreturn_t dwc3_omap_interrupt(int irq, void *_omap)
 	return IRQ_HANDLED;
 }
 
-static int dwc3_omap_remove_core(struct device *dev, void *c)
-{
-	struct platform_device *pdev = to_platform_device(dev);
-
-	of_device_unregister(pdev);
-
-	return 0;
-}
-
 static void dwc3_omap_enable_irqs(struct dwc3_omap *omap)
 {
 	u32			reg;
@@ -345,9 +348,23 @@ static void dwc3_omap_enable_irqs(struct dwc3_omap *omap)
 
 static void dwc3_omap_disable_irqs(struct dwc3_omap *omap)
 {
+	u32			reg;
+
 	/* disable all IRQs */
-	dwc3_omap_write_irqmisc_set(omap, 0x00);
-	dwc3_omap_write_irq0_set(omap, 0x00);
+	reg = USBOTGSS_IRQO_COREIRQ_ST;
+	dwc3_omap_write_irq0_clr(omap, reg);
+
+	reg = (USBOTGSS_IRQMISC_OEVT |
+			USBOTGSS_IRQMISC_DRVVBUS_RISE |
+			USBOTGSS_IRQMISC_CHRGVBUS_RISE |
+			USBOTGSS_IRQMISC_DISCHRGVBUS_RISE |
+			USBOTGSS_IRQMISC_IDPULLUP_RISE |
+			USBOTGSS_IRQMISC_DRVVBUS_FALL |
+			USBOTGSS_IRQMISC_CHRGVBUS_FALL |
+			USBOTGSS_IRQMISC_DISCHRGVBUS_FALL |
+			USBOTGSS_IRQMISC_IDPULLUP_FALL);
+
+	dwc3_omap_write_irqmisc_clr(omap, reg);
 }
 
 static u64 dwc3_omap_dma_mask = DMA_BIT_MASK(32);
@@ -574,7 +591,7 @@ static int dwc3_omap_remove(struct platform_device *pdev)
 	if (omap->extcon_id_dev.edev)
 		extcon_unregister_interest(&omap->extcon_id_dev);
 	dwc3_omap_disable_irqs(omap);
-	device_for_each_child(&pdev->dev, NULL, dwc3_omap_remove_core);
+	of_platform_depopulate(omap->dev);
 	pm_runtime_put_sync(&pdev->dev);
 	pm_runtime_disable(&pdev->dev);
 
diff --git a/drivers/usb/dwc3/dwc3-pci.c b/drivers/usb/dwc3/dwc3-pci.c
index 8d950569d557..b773fb53d6a7 100644
--- a/drivers/usb/dwc3/dwc3-pci.c
+++ b/drivers/usb/dwc3/dwc3-pci.c
@@ -24,8 +24,6 @@
 
 #include "platform_data.h"
 
-/* FIXME define these in <linux/pci_ids.h> */
-#define PCI_VENDOR_ID_SYNOPSYS		0x16c3
 #define PCI_DEVICE_ID_SYNOPSYS_HAPSUSB3	0xabcd
 #define PCI_DEVICE_ID_INTEL_BYT		0x0f37
 #define PCI_DEVICE_ID_INTEL_MRFLD	0x119e
diff --git a/drivers/usb/dwc3/gadget.c b/drivers/usb/dwc3/gadget.c
index a03a485205c7..8946c32047e9 100644
--- a/drivers/usb/dwc3/gadget.c
+++ b/drivers/usb/dwc3/gadget.c
@@ -1855,32 +1855,27 @@ static int dwc3_cleanup_done_reqs(struct dwc3 *dwc, struct dwc3_ep *dep,
 	unsigned int		i;
 	int			ret;
 
+	req = next_request(&dep->req_queued);
+	if (!req) {
+		WARN_ON_ONCE(1);
+		return 1;
+	}
+	i = 0;
 	do {
-		req = next_request(&dep->req_queued);
-		if (!req) {
-			WARN_ON_ONCE(1);
-			return 1;
-		}
-		i = 0;
-		do {
-			slot = req->start_slot + i;
-			if ((slot == DWC3_TRB_NUM - 1) &&
+		slot = req->start_slot + i;
+		if ((slot == DWC3_TRB_NUM - 1) &&
 				usb_endpoint_xfer_isoc(dep->endpoint.desc))
-				slot++;
-			slot %= DWC3_TRB_NUM;
-			trb = &dep->trb_pool[slot];
-
-			ret = __dwc3_cleanup_done_trbs(dwc, dep, req, trb,
-					event, status);
-			if (ret)
-				break;
-		}while (++i < req->request.num_mapped_sgs);
-
-		dwc3_gadget_giveback(dep, req, status);
+			slot++;
+		slot %= DWC3_TRB_NUM;
+		trb = &dep->trb_pool[slot];
 
+		ret = __dwc3_cleanup_done_trbs(dwc, dep, req, trb,
+				event, status);
 		if (ret)
 			break;
-	} while (1);
+	} while (++i < req->request.num_mapped_sgs);
+
+	dwc3_gadget_giveback(dep, req, status);
 
 	if (usb_endpoint_xfer_isoc(dep->endpoint.desc) &&
 			list_empty(&dep->req_queued)) {
diff --git a/drivers/usb/dwc3/host.c b/drivers/usb/dwc3/host.c
index 12bfd3c5405e..c679f63783ae 100644
--- a/drivers/usb/dwc3/host.c
+++ b/drivers/usb/dwc3/host.c
@@ -49,9 +49,7 @@ int dwc3_host_init(struct dwc3 *dwc)
 
 	memset(&pdata, 0, sizeof(pdata));
 
-#ifdef CONFIG_DWC3_HOST_USB3_LPM_ENABLE
-	pdata.usb3_lpm_capable = 1;
-#endif
+	pdata.usb3_lpm_capable = dwc->usb3_lpm_capable;
 
 	ret = platform_device_add_data(xhci, &pdata, sizeof(pdata));
 	if (ret) {
diff --git a/drivers/usb/dwc3/platform_data.h b/drivers/usb/dwc3/platform_data.h
index a3a3b6d5668c..a2bd464be828 100644
--- a/drivers/usb/dwc3/platform_data.h
+++ b/drivers/usb/dwc3/platform_data.h
@@ -24,6 +24,7 @@ struct dwc3_platform_data {
 	enum usb_device_speed maximum_speed;
 	enum usb_dr_mode dr_mode;
 	bool tx_fifo_resize;
+	bool usb3_lpm_capable;
 
 	unsigned is_utmi_l1_suspend:1;
 	u8 hird_threshold;
diff --git a/drivers/usb/gadget/Kconfig b/drivers/usb/gadget/Kconfig
index b454d05be583..bcf83c0a6e62 100644
--- a/drivers/usb/gadget/Kconfig
+++ b/drivers/usb/gadget/Kconfig
@@ -196,6 +196,9 @@ config USB_F_MIDI
 config USB_F_HID
 	tristate
 
+config USB_F_PRINTER
+	tristate
+
 choice
 	tristate "USB Gadget Drivers"
 	default USB_ETH
@@ -434,6 +437,20 @@ config USB_CONFIGFS_F_UVC
 	  device. It provides a userspace API to process UVC control requests
 	  and stream video data to the host.
 
+config USB_CONFIGFS_F_PRINTER
+	bool "Printer function"
+	select USB_F_PRINTER
+	depends on USB_CONFIGFS
+	help
+	  The Printer function channels data between the USB host and a
+	  userspace program driving the print engine. The user space
+	  program reads and writes the device file /dev/g_printer<X> to
+	  receive or send printer data. It can use ioctl calls to
+	  the device file to get or set printer status.
+
+	  For more information, see Documentation/usb/gadget_printer.txt
+	  which includes sample code for accessing the device file.
+
 source "drivers/usb/gadget/legacy/Kconfig"
 
 endchoice
diff --git a/drivers/usb/gadget/composite.c b/drivers/usb/gadget/composite.c
index 13adfd1a3f54..4e3447bbd097 100644
--- a/drivers/usb/gadget/composite.c
+++ b/drivers/usb/gadget/composite.c
@@ -1161,11 +1161,11 @@ static struct usb_gadget_string_container *copy_gadget_strings(
  * This function will create a deep copy of usb_gadget_strings and usb_string
  * and attach it to the cdev. The actual string (usb_string.s) will not be
  * copied but only a referenced will be made. The struct usb_gadget_strings
- * array may contain multiple languges and should be NULL terminated.
+ * array may contain multiple languages and should be NULL terminated.
  * The ->language pointer of each struct usb_gadget_strings has to contain the
  * same amount of entries.
  * For instance: sp[0] is en-US, sp[1] is es-ES. It is expected that the first
- * usb_string entry of es-ES containts the translation of the first usb_string
+ * usb_string entry of es-ES contains the translation of the first usb_string
  * entry of en-US. Therefore both entries become the same id assign.
  */
 struct usb_string *usb_gstrings_attach(struct usb_composite_dev *cdev,
@@ -1472,6 +1472,13 @@ composite_setup(struct usb_gadget *gadget, const struct usb_ctrlrequest *ctrl)
 	req->length = 0;
 	gadget->ep0->driver_data = cdev;
 
+	/*
+	 * Don't let non-standard requests match any of the cases below
+	 * by accident.
+	 */
+	if ((ctrl->bRequestType & USB_TYPE_MASK) != USB_TYPE_STANDARD)
+		goto unknown;
+
 	switch (ctrl->bRequest) {
 
 	/* we handle all standard USB descriptors */
@@ -1751,6 +1758,10 @@ unknown:
 		 * take such requests too, if that's ever needed:  to work
 		 * in config 0, etc.
 		 */
+		list_for_each_entry(f, &cdev->config->functions, list)
+			if (f->req_match && f->req_match(f, ctrl))
+				goto try_fun_setup;
+		f = NULL;
 		switch (ctrl->bRequestType & USB_RECIP_MASK) {
 		case USB_RECIP_INTERFACE:
 			if (!cdev->config || intf >= MAX_CONFIG_INTERFACES)
@@ -1768,7 +1779,7 @@ unknown:
 				f = NULL;
 			break;
 		}
-
+try_fun_setup:
 		if (f && f->setup)
 			value = f->setup(f, ctrl);
 		else {
diff --git a/drivers/usb/gadget/configfs.c b/drivers/usb/gadget/configfs.c
index 75648145dc1b..c42765b3a060 100644
--- a/drivers/usb/gadget/configfs.c
+++ b/drivers/usb/gadget/configfs.c
@@ -1161,7 +1161,6 @@ static ssize_t interf_grp_compatible_id_store(struct usb_os_desc *desc,
 	if (desc->opts_mutex)
 		mutex_lock(desc->opts_mutex);
 	memcpy(desc->ext_compat_id, page, l);
-	desc->ext_compat_id[l] = '\0';
 
 	if (desc->opts_mutex)
 		mutex_unlock(desc->opts_mutex);
@@ -1192,7 +1191,6 @@ static ssize_t interf_grp_sub_compatible_id_store(struct usb_os_desc *desc,
 	if (desc->opts_mutex)
 		mutex_lock(desc->opts_mutex);
 	memcpy(desc->ext_compat_id + 8, page, l);
-	desc->ext_compat_id[l + 8] = '\0';
 
 	if (desc->opts_mutex)
 		mutex_unlock(desc->opts_mutex);
diff --git a/drivers/usb/gadget/function/Makefile b/drivers/usb/gadget/function/Makefile
index f71b1aaa0edf..bd7def576955 100644
--- a/drivers/usb/gadget/function/Makefile
+++ b/drivers/usb/gadget/function/Makefile
@@ -42,3 +42,5 @@ usb_f_midi-y			:= f_midi.o
 obj-$(CONFIG_USB_F_MIDI)	+= usb_f_midi.o
 usb_f_hid-y			:= f_hid.o
 obj-$(CONFIG_USB_F_HID)		+= usb_f_hid.o
+usb_f_printer-y			:= f_printer.o
+obj-$(CONFIG_USB_F_PRINTER)	+= usb_f_printer.o
diff --git a/drivers/usb/gadget/function/f_fs.c b/drivers/usb/gadget/function/f_fs.c
index af98b096af2f..175c9956cbe3 100644
--- a/drivers/usb/gadget/function/f_fs.c
+++ b/drivers/usb/gadget/function/f_fs.c
@@ -144,10 +144,9 @@ struct ffs_io_data {
 	bool read;
 
 	struct kiocb *kiocb;
-	const struct iovec *iovec;
-	unsigned long nr_segs;
-	char __user *buf;
-	size_t len;
+	struct iov_iter data;
+	const void *to_free;
+	char *buf;
 
 	struct mm_struct *mm;
 	struct work_struct work;
@@ -649,29 +648,10 @@ static void ffs_user_copy_worker(struct work_struct *work)
 					 io_data->req->actual;
 
 	if (io_data->read && ret > 0) {
-		int i;
-		size_t pos = 0;
-
-		/*
-		 * Since req->length may be bigger than io_data->len (after
-		 * being rounded up to maxpacketsize), we may end up with more
-		 * data then user space has space for.
-		 */
-		ret = min_t(int, ret, io_data->len);
-
 		use_mm(io_data->mm);
-		for (i = 0; i < io_data->nr_segs; i++) {
-			size_t len = min_t(size_t, ret - pos,
-					io_data->iovec[i].iov_len);
-			if (!len)
-				break;
-			if (unlikely(copy_to_user(io_data->iovec[i].iov_base,
-						 &io_data->buf[pos], len))) {
-				ret = -EFAULT;
-				break;
-			}
-			pos += len;
-		}
+		ret = copy_to_iter(io_data->buf, ret, &io_data->data);
+		if (iov_iter_count(&io_data->data))
+			ret = -EFAULT;
 		unuse_mm(io_data->mm);
 	}
 
@@ -684,7 +664,7 @@ static void ffs_user_copy_worker(struct work_struct *work)
 
 	io_data->kiocb->private = NULL;
 	if (io_data->read)
-		kfree(io_data->iovec);
+		kfree(io_data->to_free);
 	kfree(io_data->buf);
 	kfree(io_data);
 }
@@ -743,6 +723,7 @@ static ssize_t ffs_epfile_io(struct file *file, struct ffs_io_data *io_data)
 		 * before the waiting completes, so do not assign to 'gadget' earlier
 		 */
 		struct usb_gadget *gadget = epfile->ffs->gadget;
+		size_t copied;
 
 		spin_lock_irq(&epfile->ffs->eps_lock);
 		/* In the meantime, endpoint got disabled or changed. */
@@ -750,34 +731,21 @@ static ssize_t ffs_epfile_io(struct file *file, struct ffs_io_data *io_data)
 			spin_unlock_irq(&epfile->ffs->eps_lock);
 			return -ESHUTDOWN;
 		}
+		data_len = iov_iter_count(&io_data->data);
 		/*
 		 * Controller may require buffer size to be aligned to
 		 * maxpacketsize of an out endpoint.
 		 */
-		data_len = io_data->read ?
-			   usb_ep_align_maybe(gadget, ep->ep, io_data->len) :
-			   io_data->len;
+		if (io_data->read)
+			data_len = usb_ep_align_maybe(gadget, ep->ep, data_len);
 		spin_unlock_irq(&epfile->ffs->eps_lock);
 
 		data = kmalloc(data_len, GFP_KERNEL);
 		if (unlikely(!data))
 			return -ENOMEM;
-		if (io_data->aio && !io_data->read) {
-			int i;
-			size_t pos = 0;
-			for (i = 0; i < io_data->nr_segs; i++) {
-				if (unlikely(copy_from_user(&data[pos],
-					     io_data->iovec[i].iov_base,
-					     io_data->iovec[i].iov_len))) {
-					ret = -EFAULT;
-					goto error;
-				}
-				pos += io_data->iovec[i].iov_len;
-			}
-		} else {
-			if (!io_data->read &&
-			    unlikely(__copy_from_user(data, io_data->buf,
-						      io_data->len))) {
+		if (!io_data->read) {
+			copied = copy_from_iter(data, data_len, &io_data->data);
+			if (copied != data_len) {
 				ret = -EFAULT;
 				goto error;
 			}
@@ -876,10 +844,8 @@ static ssize_t ffs_epfile_io(struct file *file, struct ffs_io_data *io_data)
 				 */
 				ret = ep->status;
 				if (io_data->read && ret > 0) {
-					ret = min_t(size_t, ret, io_data->len);
-
-					if (unlikely(copy_to_user(io_data->buf,
-						data, ret)))
+					ret = copy_to_iter(data, ret, &io_data->data);
+					if (unlikely(iov_iter_count(&io_data->data)))
 						ret = -EFAULT;
 				}
 			}
@@ -898,37 +864,6 @@ error:
 	return ret;
 }
 
-static ssize_t
-ffs_epfile_write(struct file *file, const char __user *buf, size_t len,
-		 loff_t *ptr)
-{
-	struct ffs_io_data io_data;
-
-	ENTER();
-
-	io_data.aio = false;
-	io_data.read = false;
-	io_data.buf = (char * __user)buf;
-	io_data.len = len;
-
-	return ffs_epfile_io(file, &io_data);
-}
-
-static ssize_t
-ffs_epfile_read(struct file *file, char __user *buf, size_t len, loff_t *ptr)
-{
-	struct ffs_io_data io_data;
-
-	ENTER();
-
-	io_data.aio = false;
-	io_data.read = true;
-	io_data.buf = buf;
-	io_data.len = len;
-
-	return ffs_epfile_io(file, &io_data);
-}
-
 static int
 ffs_epfile_open(struct inode *inode, struct file *file)
 {
@@ -965,67 +900,86 @@ static int ffs_aio_cancel(struct kiocb *kiocb)
 	return value;
 }
 
-static ssize_t ffs_epfile_aio_write(struct kiocb *kiocb,
-				    const struct iovec *iovec,
-				    unsigned long nr_segs, loff_t loff)
+static ssize_t ffs_epfile_write_iter(struct kiocb *kiocb, struct iov_iter *from)
 {
-	struct ffs_io_data *io_data;
+	struct ffs_io_data io_data, *p = &io_data;
+	ssize_t res;
 
 	ENTER();
 
-	io_data = kmalloc(sizeof(*io_data), GFP_KERNEL);
-	if (unlikely(!io_data))
-		return -ENOMEM;
+	if (!is_sync_kiocb(kiocb)) {
+		p = kmalloc(sizeof(io_data), GFP_KERNEL);
+		if (unlikely(!p))
+			return -ENOMEM;
+		p->aio = true;
+	} else {
+		p->aio = false;
+	}
 
-	io_data->aio = true;
-	io_data->read = false;
-	io_data->kiocb = kiocb;
-	io_data->iovec = iovec;
-	io_data->nr_segs = nr_segs;
-	io_data->len = kiocb->ki_nbytes;
-	io_data->mm = current->mm;
+	p->read = false;
+	p->kiocb = kiocb;
+	p->data = *from;
+	p->mm = current->mm;
 
-	kiocb->private = io_data;
+	kiocb->private = p;
 
 	kiocb_set_cancel_fn(kiocb, ffs_aio_cancel);
 
-	return ffs_epfile_io(kiocb->ki_filp, io_data);
+	res = ffs_epfile_io(kiocb->ki_filp, p);
+	if (res == -EIOCBQUEUED)
+		return res;
+	if (p->aio)
+		kfree(p);
+	else
+		*from = p->data;
+	return res;
 }
 
-static ssize_t ffs_epfile_aio_read(struct kiocb *kiocb,
-				   const struct iovec *iovec,
-				   unsigned long nr_segs, loff_t loff)
+static ssize_t ffs_epfile_read_iter(struct kiocb *kiocb, struct iov_iter *to)
 {
-	struct ffs_io_data *io_data;
-	struct iovec *iovec_copy;
+	struct ffs_io_data io_data, *p = &io_data;
+	ssize_t res;
 
 	ENTER();
 
-	iovec_copy = kmalloc_array(nr_segs, sizeof(*iovec_copy), GFP_KERNEL);
-	if (unlikely(!iovec_copy))
-		return -ENOMEM;
-
-	memcpy(iovec_copy, iovec, sizeof(struct iovec)*nr_segs);
-
-	io_data = kmalloc(sizeof(*io_data), GFP_KERNEL);
-	if (unlikely(!io_data)) {
-		kfree(iovec_copy);
-		return -ENOMEM;
+	if (!is_sync_kiocb(kiocb)) {
+		p = kmalloc(sizeof(io_data), GFP_KERNEL);
+		if (unlikely(!p))
+			return -ENOMEM;
+		p->aio = true;
+	} else {
+		p->aio = false;
 	}
 
-	io_data->aio = true;
-	io_data->read = true;
-	io_data->kiocb = kiocb;
-	io_data->iovec = iovec_copy;
-	io_data->nr_segs = nr_segs;
-	io_data->len = kiocb->ki_nbytes;
-	io_data->mm = current->mm;
+	p->read = true;
+	p->kiocb = kiocb;
+	if (p->aio) {
+		p->to_free = dup_iter(&p->data, to, GFP_KERNEL);
+		if (!p->to_free) {
+			kfree(p);
+			return -ENOMEM;
+		}
+	} else {
+		p->data = *to;
+		p->to_free = NULL;
+	}
+	p->mm = current->mm;
 
-	kiocb->private = io_data;
+	kiocb->private = p;
 
 	kiocb_set_cancel_fn(kiocb, ffs_aio_cancel);
 
-	return ffs_epfile_io(kiocb->ki_filp, io_data);
+	res = ffs_epfile_io(kiocb->ki_filp, p);
+	if (res == -EIOCBQUEUED)
+		return res;
+
+	if (p->aio) {
+		kfree(p->to_free);
+		kfree(p);
+	} else {
+		*to = p->data;
+	}
+	return res;
 }
 
 static int
@@ -1105,10 +1059,10 @@ static const struct file_operations ffs_epfile_operations = {
 	.llseek =	no_llseek,
 
 	.open =		ffs_epfile_open,
-	.write =	ffs_epfile_write,
-	.read =		ffs_epfile_read,
-	.aio_write =	ffs_epfile_aio_write,
-	.aio_read =	ffs_epfile_aio_read,
+	.write =	new_sync_write,
+	.read =		new_sync_read,
+	.write_iter =	ffs_epfile_write_iter,
+	.read_iter =	ffs_epfile_read_iter,
 	.release =	ffs_epfile_release,
 	.unlocked_ioctl =	ffs_epfile_ioctl,
 };
diff --git a/drivers/usb/gadget/function/f_hid.c b/drivers/usb/gadget/function/f_hid.c
index 426d69a9c018..13dfc9915b1d 100644
--- a/drivers/usb/gadget/function/f_hid.c
+++ b/drivers/usb/gadget/function/f_hid.c
@@ -569,7 +569,7 @@ fail:
 	return status;
 }
 
-const struct file_operations f_hidg_fops = {
+static const struct file_operations f_hidg_fops = {
 	.owner		= THIS_MODULE,
 	.open		= f_hidg_open,
 	.release	= f_hidg_release,
@@ -908,7 +908,6 @@ static void hidg_unbind(struct usb_configuration *c, struct usb_function *f)
 
 	/* disable/free request and end point */
 	usb_ep_disable(hidg->in_ep);
-	usb_ep_dequeue(hidg->in_ep, hidg->req);
 	kfree(hidg->req->buf);
 	usb_ep_free_request(hidg->in_ep, hidg->req);
 
diff --git a/drivers/usb/gadget/function/f_loopback.c b/drivers/usb/gadget/function/f_loopback.c
index 298b46112b1a..39f49f1ad22f 100644
--- a/drivers/usb/gadget/function/f_loopback.c
+++ b/drivers/usb/gadget/function/f_loopback.c
@@ -289,8 +289,7 @@ static void disable_loopback(struct f_loopback *loop)
 	struct usb_composite_dev	*cdev;
 
 	cdev = loop->function.config->cdev;
-	disable_endpoints(cdev, loop->in_ep, loop->out_ep, NULL, NULL, NULL,
-			NULL);
+	disable_endpoints(cdev, loop->in_ep, loop->out_ep, NULL, NULL);
 	VDBG(cdev, "%s disabled\n", loop->function.name);
 }
 
diff --git a/drivers/usb/gadget/function/f_mass_storage.c b/drivers/usb/gadget/function/f_mass_storage.c
index 811929cd4c9e..3cc109f3c9c8 100644
--- a/drivers/usb/gadget/function/f_mass_storage.c
+++ b/drivers/usb/gadget/function/f_mass_storage.c
@@ -1085,7 +1085,7 @@ static int do_inquiry(struct fsg_common *common, struct fsg_buffhd *bh)
 	if (!curlun) {		/* Unsupported LUNs are okay */
 		common->bad_lun_okay = 1;
 		memset(buf, 0, 36);
-		buf[0] = 0x7f;		/* Unsupported, no device-type */
+		buf[0] = TYPE_NO_LUN;	/* Unsupported, no device-type */
 		buf[4] = 31;		/* Additional length */
 		return 36;
 	}
@@ -2624,13 +2624,10 @@ static ssize_t file_store(struct device *dev, struct device_attribute *attr,
 	return fsg_store_file(curlun, filesem, buf, count);
 }
 
-static DEVICE_ATTR_RW(ro);
 static DEVICE_ATTR_RW(nofua);
-static DEVICE_ATTR_RW(file);
-
-static struct device_attribute dev_attr_ro_cdrom = __ATTR_RO(ro);
-static struct device_attribute dev_attr_file_nonremovable = __ATTR_RO(file);
-
+/* mode wil be set in fsg_lun_attr_is_visible() */
+static DEVICE_ATTR(ro, 0, ro_show, ro_store);
+static DEVICE_ATTR(file, 0, file_show, file_store);
 
 /****************************** FSG COMMON ******************************/
 
@@ -2745,40 +2742,10 @@ error_release:
 }
 EXPORT_SYMBOL_GPL(fsg_common_set_num_buffers);
 
-static inline void fsg_common_remove_sysfs(struct fsg_lun *lun)
-{
-	device_remove_file(&lun->dev, &dev_attr_nofua);
-	/*
-	 * device_remove_file() =>
-	 *
-	 * here the attr (e.g. dev_attr_ro) is only used to be passed to:
-	 *
-	 *	sysfs_remove_file() =>
-	 *
-	 *	here e.g. both dev_attr_ro_cdrom and dev_attr_ro are in
-	 *	the same namespace and
-	 *	from here only attr->name is passed to:
-	 *
-	 *		sysfs_hash_and_remove()
-	 *
-	 *		attr->name is the same for dev_attr_ro_cdrom and
-	 *		dev_attr_ro
-	 *		attr->name is the same for dev_attr_file and
-	 *		dev_attr_file_nonremovable
-	 *
-	 * so we don't differentiate between removing e.g. dev_attr_ro_cdrom
-	 * and dev_attr_ro
-	 */
-	device_remove_file(&lun->dev, &dev_attr_ro);
-	device_remove_file(&lun->dev, &dev_attr_file);
-}
-
 void fsg_common_remove_lun(struct fsg_lun *lun, bool sysfs)
 {
-	if (sysfs) {
-		fsg_common_remove_sysfs(lun);
+	if (sysfs)
 		device_unregister(&lun->dev);
-	}
 	fsg_lun_close(lun);
 	kfree(lun);
 }
@@ -2877,41 +2844,35 @@ int fsg_common_set_cdev(struct fsg_common *common,
 }
 EXPORT_SYMBOL_GPL(fsg_common_set_cdev);
 
-static inline int fsg_common_add_sysfs(struct fsg_common *common,
-				       struct fsg_lun *lun)
-{
-	int rc;
+static struct attribute *fsg_lun_dev_attrs[] = {
+	&dev_attr_ro.attr,
+	&dev_attr_file.attr,
+	&dev_attr_nofua.attr,
+	NULL
+};
 
-	rc = device_register(&lun->dev);
-	if (rc) {
-		put_device(&lun->dev);
-		return rc;
-	}
+static umode_t fsg_lun_dev_is_visible(struct kobject *kobj,
+				      struct attribute *attr, int idx)
+{
+	struct device *dev = kobj_to_dev(kobj);
+	struct fsg_lun *lun = fsg_lun_from_dev(dev);
 
-	rc = device_create_file(&lun->dev,
-				lun->cdrom
-			      ? &dev_attr_ro_cdrom
-			      : &dev_attr_ro);
-	if (rc)
-		goto error;
-	rc = device_create_file(&lun->dev,
-				lun->removable
-			      ? &dev_attr_file
-			      : &dev_attr_file_nonremovable);
-	if (rc)
-		goto error;
-	rc = device_create_file(&lun->dev, &dev_attr_nofua);
-	if (rc)
-		goto error;
+	if (attr == &dev_attr_ro.attr)
+		return lun->cdrom ? S_IRUGO : (S_IWUSR | S_IRUGO);
+	if (attr == &dev_attr_file.attr)
+		return lun->removable ? (S_IWUSR | S_IRUGO) : S_IRUGO;
+	return attr->mode;
+}
 
-	return 0;
+static const struct attribute_group fsg_lun_dev_group = {
+	.attrs = fsg_lun_dev_attrs,
+	.is_visible = fsg_lun_dev_is_visible,
+};
 
-error:
-	/* removing nonexistent files is a no-op */
-	fsg_common_remove_sysfs(lun);
-	device_unregister(&lun->dev);
-	return rc;
-}
+static const struct attribute_group *fsg_lun_dev_groups[] = {
+	&fsg_lun_dev_group,
+	NULL
+};
 
 int fsg_common_create_lun(struct fsg_common *common, struct fsg_lun_config *cfg,
 			  unsigned int id, const char *name,
@@ -2949,13 +2910,15 @@ int fsg_common_create_lun(struct fsg_common *common, struct fsg_lun_config *cfg,
 	} else {
 		lun->dev.release = fsg_lun_release;
 		lun->dev.parent = &common->gadget->dev;
+		lun->dev.groups = fsg_lun_dev_groups;
 		dev_set_drvdata(&lun->dev, &common->filesem);
 		dev_set_name(&lun->dev, "%s", name);
 		lun->name = dev_name(&lun->dev);
 
-		rc = fsg_common_add_sysfs(common, lun);
+		rc = device_register(&lun->dev);
 		if (rc) {
 			pr_info("failed to register LUN%d: %d\n", id, rc);
+			put_device(&lun->dev);
 			goto error_sysfs;
 		}
 	}
@@ -2988,10 +2951,8 @@ int fsg_common_create_lun(struct fsg_common *common, struct fsg_lun_config *cfg,
 	return 0;
 
 error_lun:
-	if (common->sysfs) {
-		fsg_common_remove_sysfs(lun);
+	if (common->sysfs)
 		device_unregister(&lun->dev);
-	}
 	fsg_lun_close(lun);
 	common->luns[id] = NULL;
 error_sysfs:
@@ -3077,8 +3038,6 @@ static void fsg_common_release(struct kref *ref)
 			struct fsg_lun *lun = *lun_it;
 			if (!lun)
 				continue;
-			if (common->sysfs)
-				fsg_common_remove_sysfs(lun);
 			fsg_lun_close(lun);
 			if (common->sysfs)
 				device_unregister(&lun->dev);
diff --git a/drivers/usb/gadget/function/f_phonet.c b/drivers/usb/gadget/function/f_phonet.c
index c89e96cfa3e4..c0c3ef272714 100644
--- a/drivers/usb/gadget/function/f_phonet.c
+++ b/drivers/usb/gadget/function/f_phonet.c
@@ -417,7 +417,10 @@ static int pn_set_alt(struct usb_function *f, unsigned intf, unsigned alt)
 			return -EINVAL;
 
 		spin_lock(&port->lock);
-		__pn_reset(f);
+
+		if (fp->in_ep->driver_data)
+			__pn_reset(f);
+
 		if (alt == 1) {
 			int i;
 
diff --git a/drivers/usb/gadget/function/f_printer.c b/drivers/usb/gadget/function/f_printer.c
new file mode 100644
index 000000000000..44173df27273
--- /dev/null
+++ b/drivers/usb/gadget/function/f_printer.c
@@ -0,0 +1,1471 @@
+/*
+ * f_printer.c - USB printer function driver
+ *
+ * Copied from drivers/usb/gadget/legacy/printer.c,
+ * which was:
+ *
+ * printer.c -- Printer gadget driver
+ *
+ * Copyright (C) 2003-2005 David Brownell
+ * Copyright (C) 2006 Craig W. Nadler
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/delay.h>
+#include <linux/ioport.h>
+#include <linux/sched.h>
+#include <linux/slab.h>
+#include <linux/mutex.h>
+#include <linux/errno.h>
+#include <linux/init.h>
+#include <linux/idr.h>
+#include <linux/timer.h>
+#include <linux/list.h>
+#include <linux/interrupt.h>
+#include <linux/device.h>
+#include <linux/moduleparam.h>
+#include <linux/fs.h>
+#include <linux/poll.h>
+#include <linux/types.h>
+#include <linux/ctype.h>
+#include <linux/cdev.h>
+
+#include <asm/byteorder.h>
+#include <linux/io.h>
+#include <linux/irq.h>
+#include <linux/uaccess.h>
+#include <asm/unaligned.h>
+
+#include <linux/usb/ch9.h>
+#include <linux/usb/composite.h>
+#include <linux/usb/gadget.h>
+#include <linux/usb/g_printer.h>
+
+#include "u_printer.h"
+
+#define PNP_STRING_LEN		1024
+#define PRINTER_MINORS		4
+#define GET_DEVICE_ID		0
+#define GET_PORT_STATUS		1
+#define SOFT_RESET		2
+
+static int major, minors;
+static struct class *usb_gadget_class;
+static DEFINE_IDA(printer_ida);
+static DEFINE_MUTEX(printer_ida_lock); /* protects access do printer_ida */
+
+/*-------------------------------------------------------------------------*/
+
+struct printer_dev {
+	spinlock_t		lock;		/* lock this structure */
+	/* lock buffer lists during read/write calls */
+	struct mutex		lock_printer_io;
+	struct usb_gadget	*gadget;
+	s8			interface;
+	struct usb_ep		*in_ep, *out_ep;
+
+	struct list_head	rx_reqs;	/* List of free RX structs */
+	struct list_head	rx_reqs_active;	/* List of Active RX xfers */
+	struct list_head	rx_buffers;	/* List of completed xfers */
+	/* wait until there is data to be read. */
+	wait_queue_head_t	rx_wait;
+	struct list_head	tx_reqs;	/* List of free TX structs */
+	struct list_head	tx_reqs_active; /* List of Active TX xfers */
+	/* Wait until there are write buffers available to use. */
+	wait_queue_head_t	tx_wait;
+	/* Wait until all write buffers have been sent. */
+	wait_queue_head_t	tx_flush_wait;
+	struct usb_request	*current_rx_req;
+	size_t			current_rx_bytes;
+	u8			*current_rx_buf;
+	u8			printer_status;
+	u8			reset_printer;
+	int			minor;
+	struct cdev		printer_cdev;
+	u8			printer_cdev_open;
+	wait_queue_head_t	wait;
+	unsigned		q_len;
+	char			*pnp_string;	/* We don't own memory! */
+	struct usb_function	function;
+};
+
+static inline struct printer_dev *func_to_printer(struct usb_function *f)
+{
+	return container_of(f, struct printer_dev, function);
+}
+
+/*-------------------------------------------------------------------------*/
+
+/*
+ * DESCRIPTORS ... most are static, but strings and (full) configuration
+ * descriptors are built on demand.
+ */
+
+/* holds our biggest descriptor */
+#define USB_DESC_BUFSIZE		256
+#define USB_BUFSIZE			8192
+
+static struct usb_interface_descriptor intf_desc = {
+	.bLength =		sizeof(intf_desc),
+	.bDescriptorType =	USB_DT_INTERFACE,
+	.bNumEndpoints =	2,
+	.bInterfaceClass =	USB_CLASS_PRINTER,
+	.bInterfaceSubClass =	1,	/* Printer Sub-Class */
+	.bInterfaceProtocol =	2,	/* Bi-Directional */
+	.iInterface =		0
+};
+
+static struct usb_endpoint_descriptor fs_ep_in_desc = {
+	.bLength =		USB_DT_ENDPOINT_SIZE,
+	.bDescriptorType =	USB_DT_ENDPOINT,
+	.bEndpointAddress =	USB_DIR_IN,
+	.bmAttributes =		USB_ENDPOINT_XFER_BULK
+};
+
+static struct usb_endpoint_descriptor fs_ep_out_desc = {
+	.bLength =		USB_DT_ENDPOINT_SIZE,
+	.bDescriptorType =	USB_DT_ENDPOINT,
+	.bEndpointAddress =	USB_DIR_OUT,
+	.bmAttributes =		USB_ENDPOINT_XFER_BULK
+};
+
+static struct usb_descriptor_header *fs_printer_function[] = {
+	(struct usb_descriptor_header *) &intf_desc,
+	(struct usb_descriptor_header *) &fs_ep_in_desc,
+	(struct usb_descriptor_header *) &fs_ep_out_desc,
+	NULL
+};
+
+/*
+ * usb 2.0 devices need to expose both high speed and full speed
+ * descriptors, unless they only run at full speed.
+ */
+
+static struct usb_endpoint_descriptor hs_ep_in_desc = {
+	.bLength =		USB_DT_ENDPOINT_SIZE,
+	.bDescriptorType =	USB_DT_ENDPOINT,
+	.bmAttributes =		USB_ENDPOINT_XFER_BULK,
+	.wMaxPacketSize =	cpu_to_le16(512)
+};
+
+static struct usb_endpoint_descriptor hs_ep_out_desc = {
+	.bLength =		USB_DT_ENDPOINT_SIZE,
+	.bDescriptorType =	USB_DT_ENDPOINT,
+	.bmAttributes =		USB_ENDPOINT_XFER_BULK,
+	.wMaxPacketSize =	cpu_to_le16(512)
+};
+
+static struct usb_qualifier_descriptor dev_qualifier = {
+	.bLength =		sizeof(dev_qualifier),
+	.bDescriptorType =	USB_DT_DEVICE_QUALIFIER,
+	.bcdUSB =		cpu_to_le16(0x0200),
+	.bDeviceClass =		USB_CLASS_PRINTER,
+	.bNumConfigurations =	1
+};
+
+static struct usb_descriptor_header *hs_printer_function[] = {
+	(struct usb_descriptor_header *) &intf_desc,
+	(struct usb_descriptor_header *) &hs_ep_in_desc,
+	(struct usb_descriptor_header *) &hs_ep_out_desc,
+	NULL
+};
+
+/*
+ * Added endpoint descriptors for 3.0 devices
+ */
+
+static struct usb_endpoint_descriptor ss_ep_in_desc = {
+	.bLength =              USB_DT_ENDPOINT_SIZE,
+	.bDescriptorType =      USB_DT_ENDPOINT,
+	.bmAttributes =         USB_ENDPOINT_XFER_BULK,
+	.wMaxPacketSize =       cpu_to_le16(1024),
+};
+
+static struct usb_ss_ep_comp_descriptor ss_ep_in_comp_desc = {
+	.bLength =              sizeof(ss_ep_in_comp_desc),
+	.bDescriptorType =      USB_DT_SS_ENDPOINT_COMP,
+};
+
+static struct usb_endpoint_descriptor ss_ep_out_desc = {
+	.bLength =              USB_DT_ENDPOINT_SIZE,
+	.bDescriptorType =      USB_DT_ENDPOINT,
+	.bmAttributes =         USB_ENDPOINT_XFER_BULK,
+	.wMaxPacketSize =       cpu_to_le16(1024),
+};
+
+static struct usb_ss_ep_comp_descriptor ss_ep_out_comp_desc = {
+	.bLength =              sizeof(ss_ep_out_comp_desc),
+	.bDescriptorType =      USB_DT_SS_ENDPOINT_COMP,
+};
+
+static struct usb_descriptor_header *ss_printer_function[] = {
+	(struct usb_descriptor_header *) &intf_desc,
+	(struct usb_descriptor_header *) &ss_ep_in_desc,
+	(struct usb_descriptor_header *) &ss_ep_in_comp_desc,
+	(struct usb_descriptor_header *) &ss_ep_out_desc,
+	(struct usb_descriptor_header *) &ss_ep_out_comp_desc,
+	NULL
+};
+
+/* maxpacket and other transfer characteristics vary by speed. */
+static inline struct usb_endpoint_descriptor *ep_desc(struct usb_gadget *gadget,
+					struct usb_endpoint_descriptor *fs,
+					struct usb_endpoint_descriptor *hs,
+					struct usb_endpoint_descriptor *ss)
+{
+	switch (gadget->speed) {
+	case USB_SPEED_SUPER:
+		return ss;
+	case USB_SPEED_HIGH:
+		return hs;
+	default:
+		return fs;
+	}
+}
+
+/*-------------------------------------------------------------------------*/
+
+static struct usb_request *
+printer_req_alloc(struct usb_ep *ep, unsigned len, gfp_t gfp_flags)
+{
+	struct usb_request	*req;
+
+	req = usb_ep_alloc_request(ep, gfp_flags);
+
+	if (req != NULL) {
+		req->length = len;
+		req->buf = kmalloc(len, gfp_flags);
+		if (req->buf == NULL) {
+			usb_ep_free_request(ep, req);
+			return NULL;
+		}
+	}
+
+	return req;
+}
+
+static void
+printer_req_free(struct usb_ep *ep, struct usb_request *req)
+{
+	if (ep != NULL && req != NULL) {
+		kfree(req->buf);
+		usb_ep_free_request(ep, req);
+	}
+}
+
+/*-------------------------------------------------------------------------*/
+
+static void rx_complete(struct usb_ep *ep, struct usb_request *req)
+{
+	struct printer_dev	*dev = ep->driver_data;
+	int			status = req->status;
+	unsigned long		flags;
+
+	spin_lock_irqsave(&dev->lock, flags);
+
+	list_del_init(&req->list);	/* Remode from Active List */
+
+	switch (status) {
+
+	/* normal completion */
+	case 0:
+		if (req->actual > 0) {
+			list_add_tail(&req->list, &dev->rx_buffers);
+			DBG(dev, "G_Printer : rx length %d\n", req->actual);
+		} else {
+			list_add(&req->list, &dev->rx_reqs);
+		}
+		break;
+
+	/* software-driven interface shutdown */
+	case -ECONNRESET:		/* unlink */
+	case -ESHUTDOWN:		/* disconnect etc */
+		VDBG(dev, "rx shutdown, code %d\n", status);
+		list_add(&req->list, &dev->rx_reqs);
+		break;
+
+	/* for hardware automagic (such as pxa) */
+	case -ECONNABORTED:		/* endpoint reset */
+		DBG(dev, "rx %s reset\n", ep->name);
+		list_add(&req->list, &dev->rx_reqs);
+		break;
+
+	/* data overrun */
+	case -EOVERFLOW:
+		/* FALLTHROUGH */
+
+	default:
+		DBG(dev, "rx status %d\n", status);
+		list_add(&req->list, &dev->rx_reqs);
+		break;
+	}
+
+	wake_up_interruptible(&dev->rx_wait);
+	spin_unlock_irqrestore(&dev->lock, flags);
+}
+
+static void tx_complete(struct usb_ep *ep, struct usb_request *req)
+{
+	struct printer_dev	*dev = ep->driver_data;
+
+	switch (req->status) {
+	default:
+		VDBG(dev, "tx err %d\n", req->status);
+		/* FALLTHROUGH */
+	case -ECONNRESET:		/* unlink */
+	case -ESHUTDOWN:		/* disconnect etc */
+		break;
+	case 0:
+		break;
+	}
+
+	spin_lock(&dev->lock);
+	/* Take the request struct off the active list and put it on the
+	 * free list.
+	 */
+	list_del_init(&req->list);
+	list_add(&req->list, &dev->tx_reqs);
+	wake_up_interruptible(&dev->tx_wait);
+	if (likely(list_empty(&dev->tx_reqs_active)))
+		wake_up_interruptible(&dev->tx_flush_wait);
+
+	spin_unlock(&dev->lock);
+}
+
+/*-------------------------------------------------------------------------*/
+
+static int
+printer_open(struct inode *inode, struct file *fd)
+{
+	struct printer_dev	*dev;
+	unsigned long		flags;
+	int			ret = -EBUSY;
+
+	dev = container_of(inode->i_cdev, struct printer_dev, printer_cdev);
+
+	spin_lock_irqsave(&dev->lock, flags);
+
+	if (!dev->printer_cdev_open) {
+		dev->printer_cdev_open = 1;
+		fd->private_data = dev;
+		ret = 0;
+		/* Change the printer status to show that it's on-line. */
+		dev->printer_status |= PRINTER_SELECTED;
+	}
+
+	spin_unlock_irqrestore(&dev->lock, flags);
+
+	DBG(dev, "printer_open returned %x\n", ret);
+	return ret;
+}
+
+static int
+printer_close(struct inode *inode, struct file *fd)
+{
+	struct printer_dev	*dev = fd->private_data;
+	unsigned long		flags;
+
+	spin_lock_irqsave(&dev->lock, flags);
+	dev->printer_cdev_open = 0;
+	fd->private_data = NULL;
+	/* Change printer status to show that the printer is off-line. */
+	dev->printer_status &= ~PRINTER_SELECTED;
+	spin_unlock_irqrestore(&dev->lock, flags);
+
+	DBG(dev, "printer_close\n");
+
+	return 0;
+}
+
+/* This function must be called with interrupts turned off. */
+static void
+setup_rx_reqs(struct printer_dev *dev)
+{
+	struct usb_request              *req;
+
+	while (likely(!list_empty(&dev->rx_reqs))) {
+		int error;
+
+		req = container_of(dev->rx_reqs.next,
+				struct usb_request, list);
+		list_del_init(&req->list);
+
+		/* The USB Host sends us whatever amount of data it wants to
+		 * so we always set the length field to the full USB_BUFSIZE.
+		 * If the amount of data is more than the read() caller asked
+		 * for it will be stored in the request buffer until it is
+		 * asked for by read().
+		 */
+		req->length = USB_BUFSIZE;
+		req->complete = rx_complete;
+
+		/* here, we unlock, and only unlock, to avoid deadlock. */
+		spin_unlock(&dev->lock);
+		error = usb_ep_queue(dev->out_ep, req, GFP_ATOMIC);
+		spin_lock(&dev->lock);
+		if (error) {
+			DBG(dev, "rx submit --> %d\n", error);
+			list_add(&req->list, &dev->rx_reqs);
+			break;
+		}
+		/* if the req is empty, then add it into dev->rx_reqs_active. */
+		else if (list_empty(&req->list))
+			list_add(&req->list, &dev->rx_reqs_active);
+	}
+}
+
+static ssize_t
+printer_read(struct file *fd, char __user *buf, size_t len, loff_t *ptr)
+{
+	struct printer_dev		*dev = fd->private_data;
+	unsigned long			flags;
+	size_t				size;
+	size_t				bytes_copied;
+	struct usb_request		*req;
+	/* This is a pointer to the current USB rx request. */
+	struct usb_request		*current_rx_req;
+	/* This is the number of bytes in the current rx buffer. */
+	size_t				current_rx_bytes;
+	/* This is a pointer to the current rx buffer. */
+	u8				*current_rx_buf;
+
+	if (len == 0)
+		return -EINVAL;
+
+	DBG(dev, "printer_read trying to read %d bytes\n", (int)len);
+
+	mutex_lock(&dev->lock_printer_io);
+	spin_lock_irqsave(&dev->lock, flags);
+
+	/* We will use this flag later to check if a printer reset happened
+	 * after we turn interrupts back on.
+	 */
+	dev->reset_printer = 0;
+
+	setup_rx_reqs(dev);
+
+	bytes_copied = 0;
+	current_rx_req = dev->current_rx_req;
+	current_rx_bytes = dev->current_rx_bytes;
+	current_rx_buf = dev->current_rx_buf;
+	dev->current_rx_req = NULL;
+	dev->current_rx_bytes = 0;
+	dev->current_rx_buf = NULL;
+
+	/* Check if there is any data in the read buffers. Please note that
+	 * current_rx_bytes is the number of bytes in the current rx buffer.
+	 * If it is zero then check if there are any other rx_buffers that
+	 * are on the completed list. We are only out of data if all rx
+	 * buffers are empty.
+	 */
+	if ((current_rx_bytes == 0) &&
+			(likely(list_empty(&dev->rx_buffers)))) {
+		/* Turn interrupts back on before sleeping. */
+		spin_unlock_irqrestore(&dev->lock, flags);
+
+		/*
+		 * If no data is available check if this is a NON-Blocking
+		 * call or not.
+		 */
+		if (fd->f_flags & (O_NONBLOCK|O_NDELAY)) {
+			mutex_unlock(&dev->lock_printer_io);
+			return -EAGAIN;
+		}
+
+		/* Sleep until data is available */
+		wait_event_interruptible(dev->rx_wait,
+				(likely(!list_empty(&dev->rx_buffers))));
+		spin_lock_irqsave(&dev->lock, flags);
+	}
+
+	/* We have data to return then copy it to the caller's buffer.*/
+	while ((current_rx_bytes || likely(!list_empty(&dev->rx_buffers)))
+			&& len) {
+		if (current_rx_bytes == 0) {
+			req = container_of(dev->rx_buffers.next,
+					struct usb_request, list);
+			list_del_init(&req->list);
+
+			if (req->actual && req->buf) {
+				current_rx_req = req;
+				current_rx_bytes = req->actual;
+				current_rx_buf = req->buf;
+			} else {
+				list_add(&req->list, &dev->rx_reqs);
+				continue;
+			}
+		}
+
+		/* Don't leave irqs off while doing memory copies */
+		spin_unlock_irqrestore(&dev->lock, flags);
+
+		if (len > current_rx_bytes)
+			size = current_rx_bytes;
+		else
+			size = len;
+
+		size -= copy_to_user(buf, current_rx_buf, size);
+		bytes_copied += size;
+		len -= size;
+		buf += size;
+
+		spin_lock_irqsave(&dev->lock, flags);
+
+		/* We've disconnected or reset so return. */
+		if (dev->reset_printer) {
+			list_add(&current_rx_req->list, &dev->rx_reqs);
+			spin_unlock_irqrestore(&dev->lock, flags);
+			mutex_unlock(&dev->lock_printer_io);
+			return -EAGAIN;
+		}
+
+		/* If we not returning all the data left in this RX request
+		 * buffer then adjust the amount of data left in the buffer.
+		 * Othewise if we are done with this RX request buffer then
+		 * requeue it to get any incoming data from the USB host.
+		 */
+		if (size < current_rx_bytes) {
+			current_rx_bytes -= size;
+			current_rx_buf += size;
+		} else {
+			list_add(&current_rx_req->list, &dev->rx_reqs);
+			current_rx_bytes = 0;
+			current_rx_buf = NULL;
+			current_rx_req = NULL;
+		}
+	}
+
+	dev->current_rx_req = current_rx_req;
+	dev->current_rx_bytes = current_rx_bytes;
+	dev->current_rx_buf = current_rx_buf;
+
+	spin_unlock_irqrestore(&dev->lock, flags);
+	mutex_unlock(&dev->lock_printer_io);
+
+	DBG(dev, "printer_read returned %d bytes\n", (int)bytes_copied);
+
+	if (bytes_copied)
+		return bytes_copied;
+	else
+		return -EAGAIN;
+}
+
+static ssize_t
+printer_write(struct file *fd, const char __user *buf, size_t len, loff_t *ptr)
+{
+	struct printer_dev	*dev = fd->private_data;
+	unsigned long		flags;
+	size_t			size;	/* Amount of data in a TX request. */
+	size_t			bytes_copied = 0;
+	struct usb_request	*req;
+
+	DBG(dev, "printer_write trying to send %d bytes\n", (int)len);
+
+	if (len == 0)
+		return -EINVAL;
+
+	mutex_lock(&dev->lock_printer_io);
+	spin_lock_irqsave(&dev->lock, flags);
+
+	/* Check if a printer reset happens while we have interrupts on */
+	dev->reset_printer = 0;
+
+	/* Check if there is any available write buffers */
+	if (likely(list_empty(&dev->tx_reqs))) {
+		/* Turn interrupts back on before sleeping. */
+		spin_unlock_irqrestore(&dev->lock, flags);
+
+		/*
+		 * If write buffers are available check if this is
+		 * a NON-Blocking call or not.
+		 */
+		if (fd->f_flags & (O_NONBLOCK|O_NDELAY)) {
+			mutex_unlock(&dev->lock_printer_io);
+			return -EAGAIN;
+		}
+
+		/* Sleep until a write buffer is available */
+		wait_event_interruptible(dev->tx_wait,
+				(likely(!list_empty(&dev->tx_reqs))));
+		spin_lock_irqsave(&dev->lock, flags);
+	}
+
+	while (likely(!list_empty(&dev->tx_reqs)) && len) {
+
+		if (len > USB_BUFSIZE)
+			size = USB_BUFSIZE;
+		else
+			size = len;
+
+		req = container_of(dev->tx_reqs.next, struct usb_request,
+				list);
+		list_del_init(&req->list);
+
+		req->complete = tx_complete;
+		req->length = size;
+
+		/* Check if we need to send a zero length packet. */
+		if (len > size)
+			/* They will be more TX requests so no yet. */
+			req->zero = 0;
+		else
+			/* If the data amount is not a multiple of the
+			 * maxpacket size then send a zero length packet.
+			 */
+			req->zero = ((len % dev->in_ep->maxpacket) == 0);
+
+		/* Don't leave irqs off while doing memory copies */
+		spin_unlock_irqrestore(&dev->lock, flags);
+
+		if (copy_from_user(req->buf, buf, size)) {
+			list_add(&req->list, &dev->tx_reqs);
+			mutex_unlock(&dev->lock_printer_io);
+			return bytes_copied;
+		}
+
+		bytes_copied += size;
+		len -= size;
+		buf += size;
+
+		spin_lock_irqsave(&dev->lock, flags);
+
+		/* We've disconnected or reset so free the req and buffer */
+		if (dev->reset_printer) {
+			list_add(&req->list, &dev->tx_reqs);
+			spin_unlock_irqrestore(&dev->lock, flags);
+			mutex_unlock(&dev->lock_printer_io);
+			return -EAGAIN;
+		}
+
+		if (usb_ep_queue(dev->in_ep, req, GFP_ATOMIC)) {
+			list_add(&req->list, &dev->tx_reqs);
+			spin_unlock_irqrestore(&dev->lock, flags);
+			mutex_unlock(&dev->lock_printer_io);
+			return -EAGAIN;
+		}
+
+		list_add(&req->list, &dev->tx_reqs_active);
+
+	}
+
+	spin_unlock_irqrestore(&dev->lock, flags);
+	mutex_unlock(&dev->lock_printer_io);
+
+	DBG(dev, "printer_write sent %d bytes\n", (int)bytes_copied);
+
+	if (bytes_copied)
+		return bytes_copied;
+	else
+		return -EAGAIN;
+}
+
+static int
+printer_fsync(struct file *fd, loff_t start, loff_t end, int datasync)
+{
+	struct printer_dev	*dev = fd->private_data;
+	struct inode *inode = file_inode(fd);
+	unsigned long		flags;
+	int			tx_list_empty;
+
+	mutex_lock(&inode->i_mutex);
+	spin_lock_irqsave(&dev->lock, flags);
+	tx_list_empty = (likely(list_empty(&dev->tx_reqs)));
+	spin_unlock_irqrestore(&dev->lock, flags);
+
+	if (!tx_list_empty) {
+		/* Sleep until all data has been sent */
+		wait_event_interruptible(dev->tx_flush_wait,
+				(likely(list_empty(&dev->tx_reqs_active))));
+	}
+	mutex_unlock(&inode->i_mutex);
+
+	return 0;
+}
+
+static unsigned int
+printer_poll(struct file *fd, poll_table *wait)
+{
+	struct printer_dev	*dev = fd->private_data;
+	unsigned long		flags;
+	int			status = 0;
+
+	mutex_lock(&dev->lock_printer_io);
+	spin_lock_irqsave(&dev->lock, flags);
+	setup_rx_reqs(dev);
+	spin_unlock_irqrestore(&dev->lock, flags);
+	mutex_unlock(&dev->lock_printer_io);
+
+	poll_wait(fd, &dev->rx_wait, wait);
+	poll_wait(fd, &dev->tx_wait, wait);
+
+	spin_lock_irqsave(&dev->lock, flags);
+	if (likely(!list_empty(&dev->tx_reqs)))
+		status |= POLLOUT | POLLWRNORM;
+
+	if (likely(dev->current_rx_bytes) ||
+			likely(!list_empty(&dev->rx_buffers)))
+		status |= POLLIN | POLLRDNORM;
+
+	spin_unlock_irqrestore(&dev->lock, flags);
+
+	return status;
+}
+
+static long
+printer_ioctl(struct file *fd, unsigned int code, unsigned long arg)
+{
+	struct printer_dev	*dev = fd->private_data;
+	unsigned long		flags;
+	int			status = 0;
+
+	DBG(dev, "printer_ioctl: cmd=0x%4.4x, arg=%lu\n", code, arg);
+
+	/* handle ioctls */
+
+	spin_lock_irqsave(&dev->lock, flags);
+
+	switch (code) {
+	case GADGET_GET_PRINTER_STATUS:
+		status = (int)dev->printer_status;
+		break;
+	case GADGET_SET_PRINTER_STATUS:
+		dev->printer_status = (u8)arg;
+		break;
+	default:
+		/* could not handle ioctl */
+		DBG(dev, "printer_ioctl: ERROR cmd=0x%4.4xis not supported\n",
+				code);
+		status = -ENOTTY;
+	}
+
+	spin_unlock_irqrestore(&dev->lock, flags);
+
+	return status;
+}
+
+/* used after endpoint configuration */
+static const struct file_operations printer_io_operations = {
+	.owner =	THIS_MODULE,
+	.open =		printer_open,
+	.read =		printer_read,
+	.write =	printer_write,
+	.fsync =	printer_fsync,
+	.poll =		printer_poll,
+	.unlocked_ioctl = printer_ioctl,
+	.release =	printer_close,
+	.llseek =	noop_llseek,
+};
+
+/*-------------------------------------------------------------------------*/
+
+static int
+set_printer_interface(struct printer_dev *dev)
+{
+	int			result = 0;
+
+	dev->in_ep->desc = ep_desc(dev->gadget, &fs_ep_in_desc, &hs_ep_in_desc,
+				&ss_ep_in_desc);
+	dev->in_ep->driver_data = dev;
+
+	dev->out_ep->desc = ep_desc(dev->gadget, &fs_ep_out_desc,
+				    &hs_ep_out_desc, &ss_ep_out_desc);
+	dev->out_ep->driver_data = dev;
+
+	result = usb_ep_enable(dev->in_ep);
+	if (result != 0) {
+		DBG(dev, "enable %s --> %d\n", dev->in_ep->name, result);
+		goto done;
+	}
+
+	result = usb_ep_enable(dev->out_ep);
+	if (result != 0) {
+		DBG(dev, "enable %s --> %d\n", dev->in_ep->name, result);
+		goto done;
+	}
+
+done:
+	/* on error, disable any endpoints  */
+	if (result != 0) {
+		(void) usb_ep_disable(dev->in_ep);
+		(void) usb_ep_disable(dev->out_ep);
+		dev->in_ep->desc = NULL;
+		dev->out_ep->desc = NULL;
+	}
+
+	/* caller is responsible for cleanup on error */
+	return result;
+}
+
+static void printer_reset_interface(struct printer_dev *dev)
+{
+	if (dev->interface < 0)
+		return;
+
+	DBG(dev, "%s\n", __func__);
+
+	if (dev->in_ep->desc)
+		usb_ep_disable(dev->in_ep);
+
+	if (dev->out_ep->desc)
+		usb_ep_disable(dev->out_ep);
+
+	dev->in_ep->desc = NULL;
+	dev->out_ep->desc = NULL;
+	dev->interface = -1;
+}
+
+/* Change our operational Interface. */
+static int set_interface(struct printer_dev *dev, unsigned number)
+{
+	int			result = 0;
+
+	/* Free the current interface */
+	printer_reset_interface(dev);
+
+	result = set_printer_interface(dev);
+	if (result)
+		printer_reset_interface(dev);
+	else
+		dev->interface = number;
+
+	if (!result)
+		INFO(dev, "Using interface %x\n", number);
+
+	return result;
+}
+
+static void printer_soft_reset(struct printer_dev *dev)
+{
+	struct usb_request	*req;
+
+	INFO(dev, "Received Printer Reset Request\n");
+
+	if (usb_ep_disable(dev->in_ep))
+		DBG(dev, "Failed to disable USB in_ep\n");
+	if (usb_ep_disable(dev->out_ep))
+		DBG(dev, "Failed to disable USB out_ep\n");
+
+	if (dev->current_rx_req != NULL) {
+		list_add(&dev->current_rx_req->list, &dev->rx_reqs);
+		dev->current_rx_req = NULL;
+	}
+	dev->current_rx_bytes = 0;
+	dev->current_rx_buf = NULL;
+	dev->reset_printer = 1;
+
+	while (likely(!(list_empty(&dev->rx_buffers)))) {
+		req = container_of(dev->rx_buffers.next, struct usb_request,
+				list);
+		list_del_init(&req->list);
+		list_add(&req->list, &dev->rx_reqs);
+	}
+
+	while (likely(!(list_empty(&dev->rx_reqs_active)))) {
+		req = container_of(dev->rx_buffers.next, struct usb_request,
+				list);
+		list_del_init(&req->list);
+		list_add(&req->list, &dev->rx_reqs);
+	}
+
+	while (likely(!(list_empty(&dev->tx_reqs_active)))) {
+		req = container_of(dev->tx_reqs_active.next,
+				struct usb_request, list);
+		list_del_init(&req->list);
+		list_add(&req->list, &dev->tx_reqs);
+	}
+
+	if (usb_ep_enable(dev->in_ep))
+		DBG(dev, "Failed to enable USB in_ep\n");
+	if (usb_ep_enable(dev->out_ep))
+		DBG(dev, "Failed to enable USB out_ep\n");
+
+	wake_up_interruptible(&dev->rx_wait);
+	wake_up_interruptible(&dev->tx_wait);
+	wake_up_interruptible(&dev->tx_flush_wait);
+}
+
+/*-------------------------------------------------------------------------*/
+
+static bool gprinter_req_match(struct usb_function *f,
+			       const struct usb_ctrlrequest *ctrl)
+{
+	struct printer_dev	*dev = func_to_printer(f);
+	u16			w_index = le16_to_cpu(ctrl->wIndex);
+	u16			w_value = le16_to_cpu(ctrl->wValue);
+	u16			w_length = le16_to_cpu(ctrl->wLength);
+
+	if ((ctrl->bRequestType & USB_RECIP_MASK) != USB_RECIP_INTERFACE ||
+	    (ctrl->bRequestType & USB_TYPE_MASK) != USB_TYPE_CLASS)
+		return false;
+
+	switch (ctrl->bRequest) {
+	case GET_DEVICE_ID:
+		w_index >>= 8;
+		if (w_length <= PNP_STRING_LEN &&
+		    (USB_DIR_IN & ctrl->bRequestType))
+			break;
+		return false;
+	case GET_PORT_STATUS:
+		if (!w_value && w_length == 1 &&
+		    (USB_DIR_IN & ctrl->bRequestType))
+			break;
+		return false;
+	case SOFT_RESET:
+		if (!w_value && !w_length &&
+		   !(USB_DIR_IN & ctrl->bRequestType))
+			break;
+		/* fall through */
+	default:
+		return false;
+	}
+	return w_index == dev->interface;
+}
+
+/*
+ * The setup() callback implements all the ep0 functionality that's not
+ * handled lower down.
+ */
+static int printer_func_setup(struct usb_function *f,
+		const struct usb_ctrlrequest *ctrl)
+{
+	struct printer_dev *dev = func_to_printer(f);
+	struct usb_composite_dev *cdev = f->config->cdev;
+	struct usb_request	*req = cdev->req;
+	int			value = -EOPNOTSUPP;
+	u16			wIndex = le16_to_cpu(ctrl->wIndex);
+	u16			wValue = le16_to_cpu(ctrl->wValue);
+	u16			wLength = le16_to_cpu(ctrl->wLength);
+
+	DBG(dev, "ctrl req%02x.%02x v%04x i%04x l%d\n",
+		ctrl->bRequestType, ctrl->bRequest, wValue, wIndex, wLength);
+
+	switch (ctrl->bRequestType&USB_TYPE_MASK) {
+	case USB_TYPE_CLASS:
+		switch (ctrl->bRequest) {
+		case GET_DEVICE_ID: /* Get the IEEE-1284 PNP String */
+			/* Only one printer interface is supported. */
+			if ((wIndex>>8) != dev->interface)
+				break;
+
+			value = (dev->pnp_string[0] << 8) | dev->pnp_string[1];
+			memcpy(req->buf, dev->pnp_string, value);
+			DBG(dev, "1284 PNP String: %x %s\n", value,
+					&dev->pnp_string[2]);
+			break;
+
+		case GET_PORT_STATUS: /* Get Port Status */
+			/* Only one printer interface is supported. */
+			if (wIndex != dev->interface)
+				break;
+
+			*(u8 *)req->buf = dev->printer_status;
+			value = min_t(u16, wLength, 1);
+			break;
+
+		case SOFT_RESET: /* Soft Reset */
+			/* Only one printer interface is supported. */
+			if (wIndex != dev->interface)
+				break;
+
+			printer_soft_reset(dev);
+
+			value = 0;
+			break;
+
+		default:
+			goto unknown;
+		}
+		break;
+
+	default:
+unknown:
+		VDBG(dev,
+			"unknown ctrl req%02x.%02x v%04x i%04x l%d\n",
+			ctrl->bRequestType, ctrl->bRequest,
+			wValue, wIndex, wLength);
+		break;
+	}
+	/* host either stalls (value < 0) or reports success */
+	if (value >= 0) {
+		req->length = value;
+		req->zero = value < wLength;
+		value = usb_ep_queue(cdev->gadget->ep0, req, GFP_ATOMIC);
+		if (value < 0) {
+			ERROR(dev, "%s:%d Error!\n", __func__, __LINE__);
+			req->status = 0;
+		}
+	}
+	return value;
+}
+
+static int printer_func_bind(struct usb_configuration *c,
+		struct usb_function *f)
+{
+	struct usb_gadget *gadget = c->cdev->gadget;
+	struct printer_dev *dev = func_to_printer(f);
+	struct device *pdev;
+	struct usb_composite_dev *cdev = c->cdev;
+	struct usb_ep *in_ep;
+	struct usb_ep *out_ep = NULL;
+	struct usb_request *req;
+	dev_t devt;
+	int id;
+	int ret;
+	u32 i;
+
+	id = usb_interface_id(c, f);
+	if (id < 0)
+		return id;
+	intf_desc.bInterfaceNumber = id;
+
+	/* finish hookup to lower layer ... */
+	dev->gadget = gadget;
+
+	/* all we really need is bulk IN/OUT */
+	in_ep = usb_ep_autoconfig(cdev->gadget, &fs_ep_in_desc);
+	if (!in_ep) {
+autoconf_fail:
+		dev_err(&cdev->gadget->dev, "can't autoconfigure on %s\n",
+			cdev->gadget->name);
+		return -ENODEV;
+	}
+	in_ep->driver_data = in_ep;	/* claim */
+
+	out_ep = usb_ep_autoconfig(cdev->gadget, &fs_ep_out_desc);
+	if (!out_ep)
+		goto autoconf_fail;
+	out_ep->driver_data = out_ep;	/* claim */
+
+	/* assumes that all endpoints are dual-speed */
+	hs_ep_in_desc.bEndpointAddress = fs_ep_in_desc.bEndpointAddress;
+	hs_ep_out_desc.bEndpointAddress = fs_ep_out_desc.bEndpointAddress;
+	ss_ep_in_desc.bEndpointAddress = fs_ep_in_desc.bEndpointAddress;
+	ss_ep_out_desc.bEndpointAddress = fs_ep_out_desc.bEndpointAddress;
+
+	ret = usb_assign_descriptors(f, fs_printer_function,
+			hs_printer_function, ss_printer_function);
+	if (ret)
+		return ret;
+
+	dev->in_ep = in_ep;
+	dev->out_ep = out_ep;
+
+	ret = -ENOMEM;
+	for (i = 0; i < dev->q_len; i++) {
+		req = printer_req_alloc(dev->in_ep, USB_BUFSIZE, GFP_KERNEL);
+		if (!req)
+			goto fail_tx_reqs;
+		list_add(&req->list, &dev->tx_reqs);
+	}
+
+	for (i = 0; i < dev->q_len; i++) {
+		req = printer_req_alloc(dev->out_ep, USB_BUFSIZE, GFP_KERNEL);
+		if (!req)
+			goto fail_rx_reqs;
+		list_add(&req->list, &dev->rx_reqs);
+	}
+
+	/* Setup the sysfs files for the printer gadget. */
+	devt = MKDEV(major, dev->minor);
+	pdev = device_create(usb_gadget_class, NULL, devt,
+				  NULL, "g_printer%d", dev->minor);
+	if (IS_ERR(pdev)) {
+		ERROR(dev, "Failed to create device: g_printer\n");
+		ret = PTR_ERR(pdev);
+		goto fail_rx_reqs;
+	}
+
+	/*
+	 * Register a character device as an interface to a user mode
+	 * program that handles the printer specific functionality.
+	 */
+	cdev_init(&dev->printer_cdev, &printer_io_operations);
+	dev->printer_cdev.owner = THIS_MODULE;
+	ret = cdev_add(&dev->printer_cdev, devt, 1);
+	if (ret) {
+		ERROR(dev, "Failed to open char device\n");
+		goto fail_cdev_add;
+	}
+
+	return 0;
+
+fail_cdev_add:
+	device_destroy(usb_gadget_class, devt);
+
+fail_rx_reqs:
+	while (!list_empty(&dev->rx_reqs)) {
+		req = container_of(dev->rx_reqs.next, struct usb_request, list);
+		list_del(&req->list);
+		printer_req_free(dev->out_ep, req);
+	}
+
+fail_tx_reqs:
+	while (!list_empty(&dev->tx_reqs)) {
+		req = container_of(dev->tx_reqs.next, struct usb_request, list);
+		list_del(&req->list);
+		printer_req_free(dev->in_ep, req);
+	}
+
+	return ret;
+
+}
+
+static int printer_func_set_alt(struct usb_function *f,
+		unsigned intf, unsigned alt)
+{
+	struct printer_dev *dev = func_to_printer(f);
+	int ret = -ENOTSUPP;
+
+	if (!alt)
+		ret = set_interface(dev, intf);
+
+	return ret;
+}
+
+static void printer_func_disable(struct usb_function *f)
+{
+	struct printer_dev *dev = func_to_printer(f);
+	unsigned long		flags;
+
+	DBG(dev, "%s\n", __func__);
+
+	spin_lock_irqsave(&dev->lock, flags);
+	printer_reset_interface(dev);
+	spin_unlock_irqrestore(&dev->lock, flags);
+}
+
+static inline struct f_printer_opts
+*to_f_printer_opts(struct config_item *item)
+{
+	return container_of(to_config_group(item), struct f_printer_opts,
+			    func_inst.group);
+}
+
+CONFIGFS_ATTR_STRUCT(f_printer_opts);
+CONFIGFS_ATTR_OPS(f_printer_opts);
+
+static void printer_attr_release(struct config_item *item)
+{
+	struct f_printer_opts *opts = to_f_printer_opts(item);
+
+	usb_put_function_instance(&opts->func_inst);
+}
+
+static struct configfs_item_operations printer_item_ops = {
+	.release	= printer_attr_release,
+	.show_attribute	= f_printer_opts_attr_show,
+	.store_attribute = f_printer_opts_attr_store,
+};
+
+static ssize_t f_printer_opts_pnp_string_show(struct f_printer_opts *opts,
+					      char *page)
+{
+	int result;
+
+	mutex_lock(&opts->lock);
+	result = strlcpy(page, opts->pnp_string + 2, PNP_STRING_LEN - 2);
+	mutex_unlock(&opts->lock);
+
+	return result;
+}
+
+static ssize_t f_printer_opts_pnp_string_store(struct f_printer_opts *opts,
+					       const char *page, size_t len)
+{
+	int result, l;
+
+	mutex_lock(&opts->lock);
+	result = strlcpy(opts->pnp_string + 2, page, PNP_STRING_LEN - 2);
+	l = strlen(opts->pnp_string + 2) + 2;
+	opts->pnp_string[0] = (l >> 8) & 0xFF;
+	opts->pnp_string[1] = l & 0xFF;
+	mutex_unlock(&opts->lock);
+
+	return result;
+}
+
+static struct f_printer_opts_attribute f_printer_opts_pnp_string =
+	__CONFIGFS_ATTR(pnp_string, S_IRUGO | S_IWUSR,
+			f_printer_opts_pnp_string_show,
+			f_printer_opts_pnp_string_store);
+
+static ssize_t f_printer_opts_q_len_show(struct f_printer_opts *opts,
+					 char *page)
+{
+	int result;
+
+	mutex_lock(&opts->lock);
+	result = sprintf(page, "%d\n", opts->q_len);
+	mutex_unlock(&opts->lock);
+
+	return result;
+}
+
+static ssize_t f_printer_opts_q_len_store(struct f_printer_opts *opts,
+					  const char *page, size_t len)
+{
+	int ret;
+	u16 num;
+
+	mutex_lock(&opts->lock);
+	if (opts->refcnt) {
+		ret = -EBUSY;
+		goto end;
+	}
+
+	ret = kstrtou16(page, 0, &num);
+	if (ret)
+		goto end;
+
+	opts->q_len = (unsigned)num;
+	ret = len;
+end:
+	mutex_unlock(&opts->lock);
+	return ret;
+}
+
+static struct f_printer_opts_attribute f_printer_opts_q_len =
+	__CONFIGFS_ATTR(q_len, S_IRUGO | S_IWUSR, f_printer_opts_q_len_show,
+			f_printer_opts_q_len_store);
+
+static struct configfs_attribute *printer_attrs[] = {
+	&f_printer_opts_pnp_string.attr,
+	&f_printer_opts_q_len.attr,
+	NULL,
+};
+
+static struct config_item_type printer_func_type = {
+	.ct_item_ops	= &printer_item_ops,
+	.ct_attrs	= printer_attrs,
+	.ct_owner	= THIS_MODULE,
+};
+
+static inline int gprinter_get_minor(void)
+{
+	return ida_simple_get(&printer_ida, 0, 0, GFP_KERNEL);
+}
+
+static inline void gprinter_put_minor(int minor)
+{
+	ida_simple_remove(&printer_ida, minor);
+}
+
+static int gprinter_setup(int);
+static void gprinter_cleanup(void);
+
+static void gprinter_free_inst(struct usb_function_instance *f)
+{
+	struct f_printer_opts *opts;
+
+	opts = container_of(f, struct f_printer_opts, func_inst);
+
+	mutex_lock(&printer_ida_lock);
+
+	gprinter_put_minor(opts->minor);
+	if (idr_is_empty(&printer_ida.idr))
+		gprinter_cleanup();
+
+	mutex_unlock(&printer_ida_lock);
+
+	kfree(opts);
+}
+
+static struct usb_function_instance *gprinter_alloc_inst(void)
+{
+	struct f_printer_opts *opts;
+	struct usb_function_instance *ret;
+	int status = 0;
+
+	opts = kzalloc(sizeof(*opts), GFP_KERNEL);
+	if (!opts)
+		return ERR_PTR(-ENOMEM);
+
+	mutex_init(&opts->lock);
+	opts->func_inst.free_func_inst = gprinter_free_inst;
+	ret = &opts->func_inst;
+
+	mutex_lock(&printer_ida_lock);
+
+	if (idr_is_empty(&printer_ida.idr)) {
+		status = gprinter_setup(PRINTER_MINORS);
+		if (status) {
+			ret = ERR_PTR(status);
+			kfree(opts);
+			goto unlock;
+		}
+	}
+
+	opts->minor = gprinter_get_minor();
+	if (opts->minor < 0) {
+		ret = ERR_PTR(opts->minor);
+		kfree(opts);
+		if (idr_is_empty(&printer_ida.idr))
+			gprinter_cleanup();
+		goto unlock;
+	}
+	config_group_init_type_name(&opts->func_inst.group, "",
+				    &printer_func_type);
+
+unlock:
+	mutex_unlock(&printer_ida_lock);
+	return ret;
+}
+
+static void gprinter_free(struct usb_function *f)
+{
+	struct printer_dev *dev = func_to_printer(f);
+	struct f_printer_opts *opts;
+
+	opts = container_of(f->fi, struct f_printer_opts, func_inst);
+	kfree(dev);
+	mutex_lock(&opts->lock);
+	--opts->refcnt;
+	mutex_unlock(&opts->lock);
+}
+
+static void printer_func_unbind(struct usb_configuration *c,
+		struct usb_function *f)
+{
+	struct printer_dev	*dev;
+	struct usb_request	*req;
+
+	dev = func_to_printer(f);
+
+	device_destroy(usb_gadget_class, MKDEV(major, dev->minor));
+
+	/* Remove Character Device */
+	cdev_del(&dev->printer_cdev);
+
+	/* we must already have been disconnected ... no i/o may be active */
+	WARN_ON(!list_empty(&dev->tx_reqs_active));
+	WARN_ON(!list_empty(&dev->rx_reqs_active));
+
+	/* Free all memory for this driver. */
+	while (!list_empty(&dev->tx_reqs)) {
+		req = container_of(dev->tx_reqs.next, struct usb_request,
+				list);
+		list_del(&req->list);
+		printer_req_free(dev->in_ep, req);
+	}
+
+	if (dev->current_rx_req != NULL)
+		printer_req_free(dev->out_ep, dev->current_rx_req);
+
+	while (!list_empty(&dev->rx_reqs)) {
+		req = container_of(dev->rx_reqs.next,
+				struct usb_request, list);
+		list_del(&req->list);
+		printer_req_free(dev->out_ep, req);
+	}
+
+	while (!list_empty(&dev->rx_buffers)) {
+		req = container_of(dev->rx_buffers.next,
+				struct usb_request, list);
+		list_del(&req->list);
+		printer_req_free(dev->out_ep, req);
+	}
+	usb_free_all_descriptors(f);
+}
+
+static struct usb_function *gprinter_alloc(struct usb_function_instance *fi)
+{
+	struct printer_dev	*dev;
+	struct f_printer_opts	*opts;
+
+	opts = container_of(fi, struct f_printer_opts, func_inst);
+
+	mutex_lock(&opts->lock);
+	if (opts->minor >= minors) {
+		mutex_unlock(&opts->lock);
+		return ERR_PTR(-ENOENT);
+	}
+
+	dev = kzalloc(sizeof(*dev), GFP_KERNEL);
+	if (!dev) {
+		mutex_unlock(&opts->lock);
+		return ERR_PTR(-ENOMEM);
+	}
+
+	++opts->refcnt;
+	dev->minor = opts->minor;
+	dev->pnp_string = opts->pnp_string;
+	dev->q_len = opts->q_len;
+	mutex_unlock(&opts->lock);
+
+	dev->function.name = "printer";
+	dev->function.bind = printer_func_bind;
+	dev->function.setup = printer_func_setup;
+	dev->function.unbind = printer_func_unbind;
+	dev->function.set_alt = printer_func_set_alt;
+	dev->function.disable = printer_func_disable;
+	dev->function.req_match = gprinter_req_match;
+	dev->function.free_func = gprinter_free;
+
+	INIT_LIST_HEAD(&dev->tx_reqs);
+	INIT_LIST_HEAD(&dev->rx_reqs);
+	INIT_LIST_HEAD(&dev->rx_buffers);
+	INIT_LIST_HEAD(&dev->tx_reqs_active);
+	INIT_LIST_HEAD(&dev->rx_reqs_active);
+
+	spin_lock_init(&dev->lock);
+	mutex_init(&dev->lock_printer_io);
+	init_waitqueue_head(&dev->rx_wait);
+	init_waitqueue_head(&dev->tx_wait);
+	init_waitqueue_head(&dev->tx_flush_wait);
+
+	dev->interface = -1;
+	dev->printer_cdev_open = 0;
+	dev->printer_status = PRINTER_NOT_ERROR;
+	dev->current_rx_req = NULL;
+	dev->current_rx_bytes = 0;
+	dev->current_rx_buf = NULL;
+
+	return &dev->function;
+}
+
+DECLARE_USB_FUNCTION_INIT(printer, gprinter_alloc_inst, gprinter_alloc);
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Craig Nadler");
+
+static int gprinter_setup(int count)
+{
+	int status;
+	dev_t devt;
+
+	usb_gadget_class = class_create(THIS_MODULE, "usb_printer_gadget");
+	if (IS_ERR(usb_gadget_class)) {
+		status = PTR_ERR(usb_gadget_class);
+		usb_gadget_class = NULL;
+		pr_err("unable to create usb_gadget class %d\n", status);
+		return status;
+	}
+
+	status = alloc_chrdev_region(&devt, 0, count, "USB printer gadget");
+	if (status) {
+		pr_err("alloc_chrdev_region %d\n", status);
+		class_destroy(usb_gadget_class);
+		usb_gadget_class = NULL;
+		return status;
+	}
+
+	major = MAJOR(devt);
+	minors = count;
+
+	return status;
+}
+
+static void gprinter_cleanup(void)
+{
+	if (major) {
+		unregister_chrdev_region(MKDEV(major, 0), minors);
+		major = minors = 0;
+	}
+	class_destroy(usb_gadget_class);
+	usb_gadget_class = NULL;
+}
diff --git a/drivers/usb/gadget/function/f_sourcesink.c b/drivers/usb/gadget/function/f_sourcesink.c
index e07c50ced64d..3a5ae9900b1e 100644
--- a/drivers/usb/gadget/function/f_sourcesink.c
+++ b/drivers/usb/gadget/function/f_sourcesink.c
@@ -23,15 +23,6 @@
 #include "gadget_chips.h"
 #include "u_f.h"
 
-#define USB_MS_TO_SS_INTERVAL(x) USB_MS_TO_HS_INTERVAL(x)
-
-enum eptype {
-	EP_CONTROL = 0,
-	EP_BULK,
-	EP_ISOC,
-	EP_INTERRUPT,
-};
-
 /*
  * SOURCE/SINK FUNCTION ... a primary testing vehicle for USB peripheral
  * controller drivers.
@@ -64,8 +55,6 @@ struct f_sourcesink {
 	struct usb_ep		*out_ep;
 	struct usb_ep		*iso_in_ep;
 	struct usb_ep		*iso_out_ep;
-	struct usb_ep		*int_in_ep;
-	struct usb_ep		*int_out_ep;
 	int			cur_alt;
 };
 
@@ -79,10 +68,6 @@ static unsigned isoc_interval;
 static unsigned isoc_maxpacket;
 static unsigned isoc_mult;
 static unsigned isoc_maxburst;
-static unsigned int_interval; /* In ms */
-static unsigned int_maxpacket;
-static unsigned int_mult;
-static unsigned int_maxburst;
 static unsigned buflen;
 
 /*-------------------------------------------------------------------------*/
@@ -107,16 +92,6 @@ static struct usb_interface_descriptor source_sink_intf_alt1 = {
 	/* .iInterface		= DYNAMIC */
 };
 
-static struct usb_interface_descriptor source_sink_intf_alt2 = {
-	.bLength =		USB_DT_INTERFACE_SIZE,
-	.bDescriptorType =	USB_DT_INTERFACE,
-
-	.bAlternateSetting =	2,
-	.bNumEndpoints =	2,
-	.bInterfaceClass =	USB_CLASS_VENDOR_SPEC,
-	/* .iInterface		= DYNAMIC */
-};
-
 /* full speed support: */
 
 static struct usb_endpoint_descriptor fs_source_desc = {
@@ -155,26 +130,6 @@ static struct usb_endpoint_descriptor fs_iso_sink_desc = {
 	.bInterval =		4,
 };
 
-static struct usb_endpoint_descriptor fs_int_source_desc = {
-	.bLength =		USB_DT_ENDPOINT_SIZE,
-	.bDescriptorType =	USB_DT_ENDPOINT,
-
-	.bEndpointAddress =	USB_DIR_IN,
-	.bmAttributes =		USB_ENDPOINT_XFER_INT,
-	.wMaxPacketSize =	cpu_to_le16(64),
-	.bInterval =		GZERO_INT_INTERVAL,
-};
-
-static struct usb_endpoint_descriptor fs_int_sink_desc = {
-	.bLength =		USB_DT_ENDPOINT_SIZE,
-	.bDescriptorType =	USB_DT_ENDPOINT,
-
-	.bEndpointAddress =	USB_DIR_OUT,
-	.bmAttributes =		USB_ENDPOINT_XFER_INT,
-	.wMaxPacketSize =	cpu_to_le16(64),
-	.bInterval =		GZERO_INT_INTERVAL,
-};
-
 static struct usb_descriptor_header *fs_source_sink_descs[] = {
 	(struct usb_descriptor_header *) &source_sink_intf_alt0,
 	(struct usb_descriptor_header *) &fs_sink_desc,
@@ -185,10 +140,6 @@ static struct usb_descriptor_header *fs_source_sink_descs[] = {
 	(struct usb_descriptor_header *) &fs_source_desc,
 	(struct usb_descriptor_header *) &fs_iso_sink_desc,
 	(struct usb_descriptor_header *) &fs_iso_source_desc,
-	(struct usb_descriptor_header *) &source_sink_intf_alt2,
-#define FS_ALT_IFC_2_OFFSET	8
-	(struct usb_descriptor_header *) &fs_int_sink_desc,
-	(struct usb_descriptor_header *) &fs_int_source_desc,
 	NULL,
 };
 
@@ -228,24 +179,6 @@ static struct usb_endpoint_descriptor hs_iso_sink_desc = {
 	.bInterval =		4,
 };
 
-static struct usb_endpoint_descriptor hs_int_source_desc = {
-	.bLength =		USB_DT_ENDPOINT_SIZE,
-	.bDescriptorType =	USB_DT_ENDPOINT,
-
-	.bmAttributes =		USB_ENDPOINT_XFER_INT,
-	.wMaxPacketSize =	cpu_to_le16(1024),
-	.bInterval =		USB_MS_TO_HS_INTERVAL(GZERO_INT_INTERVAL),
-};
-
-static struct usb_endpoint_descriptor hs_int_sink_desc = {
-	.bLength =		USB_DT_ENDPOINT_SIZE,
-	.bDescriptorType =	USB_DT_ENDPOINT,
-
-	.bmAttributes =		USB_ENDPOINT_XFER_INT,
-	.wMaxPacketSize =	cpu_to_le16(1024),
-	.bInterval =		USB_MS_TO_HS_INTERVAL(GZERO_INT_INTERVAL),
-};
-
 static struct usb_descriptor_header *hs_source_sink_descs[] = {
 	(struct usb_descriptor_header *) &source_sink_intf_alt0,
 	(struct usb_descriptor_header *) &hs_source_desc,
@@ -256,10 +189,6 @@ static struct usb_descriptor_header *hs_source_sink_descs[] = {
 	(struct usb_descriptor_header *) &hs_sink_desc,
 	(struct usb_descriptor_header *) &hs_iso_source_desc,
 	(struct usb_descriptor_header *) &hs_iso_sink_desc,
-	(struct usb_descriptor_header *) &source_sink_intf_alt2,
-#define HS_ALT_IFC_2_OFFSET	8
-	(struct usb_descriptor_header *) &hs_int_source_desc,
-	(struct usb_descriptor_header *) &hs_int_sink_desc,
 	NULL,
 };
 
@@ -335,42 +264,6 @@ static struct usb_ss_ep_comp_descriptor ss_iso_sink_comp_desc = {
 	.wBytesPerInterval =	cpu_to_le16(1024),
 };
 
-static struct usb_endpoint_descriptor ss_int_source_desc = {
-	.bLength =		USB_DT_ENDPOINT_SIZE,
-	.bDescriptorType =	USB_DT_ENDPOINT,
-
-	.bmAttributes =		USB_ENDPOINT_XFER_INT,
-	.wMaxPacketSize =	cpu_to_le16(1024),
-	.bInterval =		USB_MS_TO_SS_INTERVAL(GZERO_INT_INTERVAL),
-};
-
-struct usb_ss_ep_comp_descriptor ss_int_source_comp_desc = {
-	.bLength =		USB_DT_SS_EP_COMP_SIZE,
-	.bDescriptorType =	USB_DT_SS_ENDPOINT_COMP,
-
-	.bMaxBurst =		0,
-	.bmAttributes =		0,
-	.wBytesPerInterval =	cpu_to_le16(1024),
-};
-
-static struct usb_endpoint_descriptor ss_int_sink_desc = {
-	.bLength =		USB_DT_ENDPOINT_SIZE,
-	.bDescriptorType =	USB_DT_ENDPOINT,
-
-	.bmAttributes =		USB_ENDPOINT_XFER_INT,
-	.wMaxPacketSize =	cpu_to_le16(1024),
-	.bInterval =		USB_MS_TO_SS_INTERVAL(GZERO_INT_INTERVAL),
-};
-
-struct usb_ss_ep_comp_descriptor ss_int_sink_comp_desc = {
-	.bLength =		USB_DT_SS_EP_COMP_SIZE,
-	.bDescriptorType =	USB_DT_SS_ENDPOINT_COMP,
-
-	.bMaxBurst =		0,
-	.bmAttributes =		0,
-	.wBytesPerInterval =	cpu_to_le16(1024),
-};
-
 static struct usb_descriptor_header *ss_source_sink_descs[] = {
 	(struct usb_descriptor_header *) &source_sink_intf_alt0,
 	(struct usb_descriptor_header *) &ss_source_desc,
@@ -387,12 +280,6 @@ static struct usb_descriptor_header *ss_source_sink_descs[] = {
 	(struct usb_descriptor_header *) &ss_iso_source_comp_desc,
 	(struct usb_descriptor_header *) &ss_iso_sink_desc,
 	(struct usb_descriptor_header *) &ss_iso_sink_comp_desc,
-	(struct usb_descriptor_header *) &source_sink_intf_alt2,
-#define SS_ALT_IFC_2_OFFSET	14
-	(struct usb_descriptor_header *) &ss_int_source_desc,
-	(struct usb_descriptor_header *) &ss_int_source_comp_desc,
-	(struct usb_descriptor_header *) &ss_int_sink_desc,
-	(struct usb_descriptor_header *) &ss_int_sink_comp_desc,
 	NULL,
 };
 
@@ -414,21 +301,6 @@ static struct usb_gadget_strings *sourcesink_strings[] = {
 };
 
 /*-------------------------------------------------------------------------*/
-static const char *get_ep_string(enum eptype ep_type)
-{
-	switch (ep_type) {
-	case EP_ISOC:
-		return "ISOC-";
-	case EP_INTERRUPT:
-		return "INTERRUPT-";
-	case EP_CONTROL:
-		return "CTRL-";
-	case EP_BULK:
-		return "BULK-";
-	default:
-		return "UNKNOWN-";
-	}
-}
 
 static inline struct usb_request *ss_alloc_ep_req(struct usb_ep *ep, int len)
 {
@@ -456,8 +328,7 @@ static void disable_ep(struct usb_composite_dev *cdev, struct usb_ep *ep)
 
 void disable_endpoints(struct usb_composite_dev *cdev,
 		struct usb_ep *in, struct usb_ep *out,
-		struct usb_ep *iso_in, struct usb_ep *iso_out,
-		struct usb_ep *int_in, struct usb_ep *int_out)
+		struct usb_ep *iso_in, struct usb_ep *iso_out)
 {
 	disable_ep(cdev, in);
 	disable_ep(cdev, out);
@@ -465,10 +336,6 @@ void disable_endpoints(struct usb_composite_dev *cdev,
 		disable_ep(cdev, iso_in);
 	if (iso_out)
 		disable_ep(cdev, iso_out);
-	if (int_in)
-		disable_ep(cdev, int_in);
-	if (int_out)
-		disable_ep(cdev, int_out);
 }
 
 static int
@@ -485,7 +352,6 @@ sourcesink_bind(struct usb_configuration *c, struct usb_function *f)
 		return id;
 	source_sink_intf_alt0.bInterfaceNumber = id;
 	source_sink_intf_alt1.bInterfaceNumber = id;
-	source_sink_intf_alt2.bInterfaceNumber = id;
 
 	/* allocate bulk endpoints */
 	ss->in_ep = usb_ep_autoconfig(cdev->gadget, &fs_source_desc);
@@ -546,55 +412,14 @@ no_iso:
 	if (isoc_maxpacket > 1024)
 		isoc_maxpacket = 1024;
 
-	/* sanity check the interrupt module parameters */
-	if (int_interval < 1)
-		int_interval = 1;
-	if (int_interval > 4096)
-		int_interval = 4096;
-	if (int_mult > 2)
-		int_mult = 2;
-	if (int_maxburst > 15)
-		int_maxburst = 15;
-
-	/* fill in the FS interrupt descriptors from the module parameters */
-	fs_int_source_desc.wMaxPacketSize = int_maxpacket > 64 ?
-						64 : int_maxpacket;
-	fs_int_source_desc.bInterval = int_interval > 255 ?
-						255 : int_interval;
-	fs_int_sink_desc.wMaxPacketSize = int_maxpacket > 64 ?
-						64 : int_maxpacket;
-	fs_int_sink_desc.bInterval = int_interval > 255 ?
-						255 : int_interval;
-
-	/* allocate int endpoints */
-	ss->int_in_ep = usb_ep_autoconfig(cdev->gadget, &fs_int_source_desc);
-	if (!ss->int_in_ep)
-		goto no_int;
-	ss->int_in_ep->driver_data = cdev;	/* claim */
-
-	ss->int_out_ep = usb_ep_autoconfig(cdev->gadget, &fs_int_sink_desc);
-	if (ss->int_out_ep) {
-		ss->int_out_ep->driver_data = cdev;	/* claim */
-	} else {
-		ss->int_in_ep->driver_data = NULL;
-		ss->int_in_ep = NULL;
-no_int:
-		fs_source_sink_descs[FS_ALT_IFC_2_OFFSET] = NULL;
-		hs_source_sink_descs[HS_ALT_IFC_2_OFFSET] = NULL;
-		ss_source_sink_descs[SS_ALT_IFC_2_OFFSET] = NULL;
-	}
-
-	if (int_maxpacket > 1024)
-		int_maxpacket = 1024;
-
 	/* support high speed hardware */
 	hs_source_desc.bEndpointAddress = fs_source_desc.bEndpointAddress;
 	hs_sink_desc.bEndpointAddress = fs_sink_desc.bEndpointAddress;
 
 	/*
-	 * Fill in the HS isoc and interrupt descriptors from the module
-	 * parameters. We assume that the user knows what they are doing and
-	 * won't give parameters that their UDC doesn't support.
+	 * Fill in the HS isoc descriptors from the module parameters.
+	 * We assume that the user knows what they are doing and won't
+	 * give parameters that their UDC doesn't support.
 	 */
 	hs_iso_source_desc.wMaxPacketSize = isoc_maxpacket;
 	hs_iso_source_desc.wMaxPacketSize |= isoc_mult << 11;
@@ -607,17 +432,6 @@ no_int:
 	hs_iso_sink_desc.bInterval = isoc_interval;
 	hs_iso_sink_desc.bEndpointAddress = fs_iso_sink_desc.bEndpointAddress;
 
-	hs_int_source_desc.wMaxPacketSize = int_maxpacket;
-	hs_int_source_desc.wMaxPacketSize |= int_mult << 11;
-	hs_int_source_desc.bInterval = USB_MS_TO_HS_INTERVAL(int_interval);
-	hs_int_source_desc.bEndpointAddress =
-		fs_int_source_desc.bEndpointAddress;
-
-	hs_int_sink_desc.wMaxPacketSize = int_maxpacket;
-	hs_int_sink_desc.wMaxPacketSize |= int_mult << 11;
-	hs_int_sink_desc.bInterval = USB_MS_TO_HS_INTERVAL(int_interval);
-	hs_int_sink_desc.bEndpointAddress = fs_int_sink_desc.bEndpointAddress;
-
 	/* support super speed hardware */
 	ss_source_desc.bEndpointAddress =
 		fs_source_desc.bEndpointAddress;
@@ -625,9 +439,9 @@ no_int:
 		fs_sink_desc.bEndpointAddress;
 
 	/*
-	 * Fill in the SS isoc and interrupt descriptors from the module
-	 * parameters. We assume that the user knows what they are doing and
-	 * won't give parameters that their UDC doesn't support.
+	 * Fill in the SS isoc descriptors from the module parameters.
+	 * We assume that the user knows what they are doing and won't
+	 * give parameters that their UDC doesn't support.
 	 */
 	ss_iso_source_desc.wMaxPacketSize = isoc_maxpacket;
 	ss_iso_source_desc.bInterval = isoc_interval;
@@ -646,37 +460,17 @@ no_int:
 		isoc_maxpacket * (isoc_mult + 1) * (isoc_maxburst + 1);
 	ss_iso_sink_desc.bEndpointAddress = fs_iso_sink_desc.bEndpointAddress;
 
-	ss_int_source_desc.wMaxPacketSize = int_maxpacket;
-	ss_int_source_desc.bInterval = USB_MS_TO_SS_INTERVAL(int_interval);
-	ss_int_source_comp_desc.bmAttributes = int_mult;
-	ss_int_source_comp_desc.bMaxBurst = int_maxburst;
-	ss_int_source_comp_desc.wBytesPerInterval =
-		int_maxpacket * (int_mult + 1) * (int_maxburst + 1);
-	ss_int_source_desc.bEndpointAddress =
-		fs_int_source_desc.bEndpointAddress;
-
-	ss_int_sink_desc.wMaxPacketSize = int_maxpacket;
-	ss_int_sink_desc.bInterval = USB_MS_TO_SS_INTERVAL(int_interval);
-	ss_int_sink_comp_desc.bmAttributes = int_mult;
-	ss_int_sink_comp_desc.bMaxBurst = int_maxburst;
-	ss_int_sink_comp_desc.wBytesPerInterval =
-		int_maxpacket * (int_mult + 1) * (int_maxburst + 1);
-	ss_int_sink_desc.bEndpointAddress = fs_int_sink_desc.bEndpointAddress;
-
 	ret = usb_assign_descriptors(f, fs_source_sink_descs,
 			hs_source_sink_descs, ss_source_sink_descs);
 	if (ret)
 		return ret;
 
-	DBG(cdev, "%s speed %s: IN/%s, OUT/%s, ISO-IN/%s, ISO-OUT/%s, "
-			"INT-IN/%s, INT-OUT/%s\n",
+	DBG(cdev, "%s speed %s: IN/%s, OUT/%s, ISO-IN/%s, ISO-OUT/%s\n",
 	    (gadget_is_superspeed(c->cdev->gadget) ? "super" :
 	     (gadget_is_dualspeed(c->cdev->gadget) ? "dual" : "full")),
 			f->name, ss->in_ep->name, ss->out_ep->name,
 			ss->iso_in_ep ? ss->iso_in_ep->name : "<none>",
-			ss->iso_out_ep ? ss->iso_out_ep->name : "<none>",
-			ss->int_in_ep ? ss->int_in_ep->name : "<none>",
-			ss->int_out_ep ? ss->int_out_ep->name : "<none>");
+			ss->iso_out_ep ? ss->iso_out_ep->name : "<none>");
 	return 0;
 }
 
@@ -807,15 +601,14 @@ static void source_sink_complete(struct usb_ep *ep, struct usb_request *req)
 }
 
 static int source_sink_start_ep(struct f_sourcesink *ss, bool is_in,
-		enum eptype ep_type, int speed)
+		bool is_iso, int speed)
 {
 	struct usb_ep		*ep;
 	struct usb_request	*req;
 	int			i, size, status;
 
 	for (i = 0; i < 8; i++) {
-		switch (ep_type) {
-		case EP_ISOC:
+		if (is_iso) {
 			switch (speed) {
 			case USB_SPEED_SUPER:
 				size = isoc_maxpacket * (isoc_mult + 1) *
@@ -831,28 +624,9 @@ static int source_sink_start_ep(struct f_sourcesink *ss, bool is_in,
 			}
 			ep = is_in ? ss->iso_in_ep : ss->iso_out_ep;
 			req = ss_alloc_ep_req(ep, size);
-			break;
-		case EP_INTERRUPT:
-			switch (speed) {
-			case USB_SPEED_SUPER:
-				size = int_maxpacket * (int_mult + 1) *
-						(int_maxburst + 1);
-				break;
-			case USB_SPEED_HIGH:
-				size = int_maxpacket * (int_mult + 1);
-				break;
-			default:
-				size = int_maxpacket > 1023 ?
-						1023 : int_maxpacket;
-				break;
-			}
-			ep = is_in ? ss->int_in_ep : ss->int_out_ep;
-			req = ss_alloc_ep_req(ep, size);
-			break;
-		default:
+		} else {
 			ep = is_in ? ss->in_ep : ss->out_ep;
 			req = ss_alloc_ep_req(ep, 0);
-			break;
 		}
 
 		if (!req)
@@ -870,12 +644,12 @@ static int source_sink_start_ep(struct f_sourcesink *ss, bool is_in,
 
 			cdev = ss->function.config->cdev;
 			ERROR(cdev, "start %s%s %s --> %d\n",
-				get_ep_string(ep_type), is_in ? "IN" : "OUT",
-				ep->name, status);
+			      is_iso ? "ISO-" : "", is_in ? "IN" : "OUT",
+			      ep->name, status);
 			free_ep_req(ep, req);
 		}
 
-		if (!(ep_type == EP_ISOC))
+		if (!is_iso)
 			break;
 	}
 
@@ -888,7 +662,7 @@ static void disable_source_sink(struct f_sourcesink *ss)
 
 	cdev = ss->function.config->cdev;
 	disable_endpoints(cdev, ss->in_ep, ss->out_ep, ss->iso_in_ep,
-			ss->iso_out_ep, ss->int_in_ep, ss->int_out_ep);
+			ss->iso_out_ep);
 	VDBG(cdev, "%s disabled\n", ss->function.name);
 }
 
@@ -900,62 +674,6 @@ enable_source_sink(struct usb_composite_dev *cdev, struct f_sourcesink *ss,
 	int					speed = cdev->gadget->speed;
 	struct usb_ep				*ep;
 
-	if (alt == 2) {
-		/* Configure for periodic interrupt endpoint */
-		ep = ss->int_in_ep;
-		if (ep) {
-			result = config_ep_by_speed(cdev->gadget,
-					&(ss->function), ep);
-			if (result)
-				return result;
-
-			result = usb_ep_enable(ep);
-			if (result < 0)
-				return result;
-
-			ep->driver_data = ss;
-			result = source_sink_start_ep(ss, true, EP_INTERRUPT,
-					speed);
-			if (result < 0) {
-fail1:
-				ep = ss->int_in_ep;
-				if (ep) {
-					usb_ep_disable(ep);
-					ep->driver_data = NULL;
-				}
-				return result;
-			}
-		}
-
-		/*
-		 * one interrupt endpoint reads (sinks) anything OUT (from the
-		 * host)
-		 */
-		ep = ss->int_out_ep;
-		if (ep) {
-			result = config_ep_by_speed(cdev->gadget,
-					&(ss->function), ep);
-			if (result)
-				goto fail1;
-
-			result = usb_ep_enable(ep);
-			if (result < 0)
-				goto fail1;
-
-			ep->driver_data = ss;
-			result = source_sink_start_ep(ss, false, EP_INTERRUPT,
-					speed);
-			if (result < 0) {
-				ep = ss->int_out_ep;
-				usb_ep_disable(ep);
-				ep->driver_data = NULL;
-				goto fail1;
-			}
-		}
-
-		goto out;
-	}
-
 	/* one bulk endpoint writes (sources) zeroes IN (to the host) */
 	ep = ss->in_ep;
 	result = config_ep_by_speed(cdev->gadget, &(ss->function), ep);
@@ -966,7 +684,7 @@ fail1:
 		return result;
 	ep->driver_data = ss;
 
-	result = source_sink_start_ep(ss, true, EP_BULK, speed);
+	result = source_sink_start_ep(ss, true, false, speed);
 	if (result < 0) {
 fail:
 		ep = ss->in_ep;
@@ -985,7 +703,7 @@ fail:
 		goto fail;
 	ep->driver_data = ss;
 
-	result = source_sink_start_ep(ss, false, EP_BULK, speed);
+	result = source_sink_start_ep(ss, false, false, speed);
 	if (result < 0) {
 fail2:
 		ep = ss->out_ep;
@@ -1008,7 +726,7 @@ fail2:
 			goto fail2;
 		ep->driver_data = ss;
 
-		result = source_sink_start_ep(ss, true, EP_ISOC, speed);
+		result = source_sink_start_ep(ss, true, true, speed);
 		if (result < 0) {
 fail3:
 			ep = ss->iso_in_ep;
@@ -1031,14 +749,13 @@ fail3:
 			goto fail3;
 		ep->driver_data = ss;
 
-		result = source_sink_start_ep(ss, false, EP_ISOC, speed);
+		result = source_sink_start_ep(ss, false, true, speed);
 		if (result < 0) {
 			usb_ep_disable(ep);
 			ep->driver_data = NULL;
 			goto fail3;
 		}
 	}
-
 out:
 	ss->cur_alt = alt;
 
@@ -1054,8 +771,6 @@ static int sourcesink_set_alt(struct usb_function *f,
 
 	if (ss->in_ep->driver_data)
 		disable_source_sink(ss);
-	else if (alt == 2 && ss->int_in_ep->driver_data)
-		disable_source_sink(ss);
 	return enable_source_sink(cdev, ss, alt);
 }
 
@@ -1168,10 +883,6 @@ static struct usb_function *source_sink_alloc_func(
 	isoc_maxpacket = ss_opts->isoc_maxpacket;
 	isoc_mult = ss_opts->isoc_mult;
 	isoc_maxburst = ss_opts->isoc_maxburst;
-	int_interval = ss_opts->int_interval;
-	int_maxpacket = ss_opts->int_maxpacket;
-	int_mult = ss_opts->int_mult;
-	int_maxburst = ss_opts->int_maxburst;
 	buflen = ss_opts->bulk_buflen;
 
 	ss->function.name = "source/sink";
@@ -1468,182 +1179,6 @@ static struct f_ss_opts_attribute f_ss_opts_bulk_buflen =
 			f_ss_opts_bulk_buflen_show,
 			f_ss_opts_bulk_buflen_store);
 
-static ssize_t f_ss_opts_int_interval_show(struct f_ss_opts *opts, char *page)
-{
-	int result;
-
-	mutex_lock(&opts->lock);
-	result = sprintf(page, "%u", opts->int_interval);
-	mutex_unlock(&opts->lock);
-
-	return result;
-}
-
-static ssize_t f_ss_opts_int_interval_store(struct f_ss_opts *opts,
-				       const char *page, size_t len)
-{
-	int ret;
-	u32 num;
-
-	mutex_lock(&opts->lock);
-	if (opts->refcnt) {
-		ret = -EBUSY;
-		goto end;
-	}
-
-	ret = kstrtou32(page, 0, &num);
-	if (ret)
-		goto end;
-
-	if (num > 4096) {
-		ret = -EINVAL;
-		goto end;
-	}
-
-	opts->int_interval = num;
-	ret = len;
-end:
-	mutex_unlock(&opts->lock);
-	return ret;
-}
-
-static struct f_ss_opts_attribute f_ss_opts_int_interval =
-	__CONFIGFS_ATTR(int_interval, S_IRUGO | S_IWUSR,
-			f_ss_opts_int_interval_show,
-			f_ss_opts_int_interval_store);
-
-static ssize_t f_ss_opts_int_maxpacket_show(struct f_ss_opts *opts, char *page)
-{
-	int result;
-
-	mutex_lock(&opts->lock);
-	result = sprintf(page, "%u", opts->int_maxpacket);
-	mutex_unlock(&opts->lock);
-
-	return result;
-}
-
-static ssize_t f_ss_opts_int_maxpacket_store(struct f_ss_opts *opts,
-				       const char *page, size_t len)
-{
-	int ret;
-	u16 num;
-
-	mutex_lock(&opts->lock);
-	if (opts->refcnt) {
-		ret = -EBUSY;
-		goto end;
-	}
-
-	ret = kstrtou16(page, 0, &num);
-	if (ret)
-		goto end;
-
-	if (num > 1024) {
-		ret = -EINVAL;
-		goto end;
-	}
-
-	opts->int_maxpacket = num;
-	ret = len;
-end:
-	mutex_unlock(&opts->lock);
-	return ret;
-}
-
-static struct f_ss_opts_attribute f_ss_opts_int_maxpacket =
-	__CONFIGFS_ATTR(int_maxpacket, S_IRUGO | S_IWUSR,
-			f_ss_opts_int_maxpacket_show,
-			f_ss_opts_int_maxpacket_store);
-
-static ssize_t f_ss_opts_int_mult_show(struct f_ss_opts *opts, char *page)
-{
-	int result;
-
-	mutex_lock(&opts->lock);
-	result = sprintf(page, "%u", opts->int_mult);
-	mutex_unlock(&opts->lock);
-
-	return result;
-}
-
-static ssize_t f_ss_opts_int_mult_store(struct f_ss_opts *opts,
-				       const char *page, size_t len)
-{
-	int ret;
-	u8 num;
-
-	mutex_lock(&opts->lock);
-	if (opts->refcnt) {
-		ret = -EBUSY;
-		goto end;
-	}
-
-	ret = kstrtou8(page, 0, &num);
-	if (ret)
-		goto end;
-
-	if (num > 2) {
-		ret = -EINVAL;
-		goto end;
-	}
-
-	opts->int_mult = num;
-	ret = len;
-end:
-	mutex_unlock(&opts->lock);
-	return ret;
-}
-
-static struct f_ss_opts_attribute f_ss_opts_int_mult =
-	__CONFIGFS_ATTR(int_mult, S_IRUGO | S_IWUSR,
-			f_ss_opts_int_mult_show,
-			f_ss_opts_int_mult_store);
-
-static ssize_t f_ss_opts_int_maxburst_show(struct f_ss_opts *opts, char *page)
-{
-	int result;
-
-	mutex_lock(&opts->lock);
-	result = sprintf(page, "%u", opts->int_maxburst);
-	mutex_unlock(&opts->lock);
-
-	return result;
-}
-
-static ssize_t f_ss_opts_int_maxburst_store(struct f_ss_opts *opts,
-				       const char *page, size_t len)
-{
-	int ret;
-	u8 num;
-
-	mutex_lock(&opts->lock);
-	if (opts->refcnt) {
-		ret = -EBUSY;
-		goto end;
-	}
-
-	ret = kstrtou8(page, 0, &num);
-	if (ret)
-		goto end;
-
-	if (num > 15) {
-		ret = -EINVAL;
-		goto end;
-	}
-
-	opts->int_maxburst = num;
-	ret = len;
-end:
-	mutex_unlock(&opts->lock);
-	return ret;
-}
-
-static struct f_ss_opts_attribute f_ss_opts_int_maxburst =
-	__CONFIGFS_ATTR(int_maxburst, S_IRUGO | S_IWUSR,
-			f_ss_opts_int_maxburst_show,
-			f_ss_opts_int_maxburst_store);
-
 static struct configfs_attribute *ss_attrs[] = {
 	&f_ss_opts_pattern.attr,
 	&f_ss_opts_isoc_interval.attr,
@@ -1651,10 +1186,6 @@ static struct configfs_attribute *ss_attrs[] = {
 	&f_ss_opts_isoc_mult.attr,
 	&f_ss_opts_isoc_maxburst.attr,
 	&f_ss_opts_bulk_buflen.attr,
-	&f_ss_opts_int_interval.attr,
-	&f_ss_opts_int_maxpacket.attr,
-	&f_ss_opts_int_mult.attr,
-	&f_ss_opts_int_maxburst.attr,
 	NULL,
 };
 
@@ -1684,8 +1215,6 @@ static struct usb_function_instance *source_sink_alloc_inst(void)
 	ss_opts->isoc_interval = GZERO_ISOC_INTERVAL;
 	ss_opts->isoc_maxpacket = GZERO_ISOC_MAXPACKET;
 	ss_opts->bulk_buflen = GZERO_BULK_BUFLEN;
-	ss_opts->int_interval = GZERO_INT_INTERVAL;
-	ss_opts->int_maxpacket = GZERO_INT_MAXPACKET;
 
 	config_group_init_type_name(&ss_opts->func_inst.group, "",
 				    &ss_func_type);
diff --git a/drivers/usb/gadget/function/f_uac2.c b/drivers/usb/gadget/function/f_uac2.c
index 33e16658e5cf..6d3eb8b00a48 100644
--- a/drivers/usb/gadget/function/f_uac2.c
+++ b/drivers/usb/gadget/function/f_uac2.c
@@ -54,7 +54,7 @@
 #define UNFLW_CTRL	8
 #define OVFLW_CTRL	10
 
-const char *uac2_name = "snd_uac2";
+static const char *uac2_name = "snd_uac2";
 
 struct uac2_req {
 	struct uac2_rtd_params *pp; /* parent param */
@@ -634,7 +634,7 @@ static struct usb_interface_descriptor std_ac_if_desc = {
 };
 
 /* Clock source for IN traffic */
-struct uac_clock_source_descriptor in_clk_src_desc = {
+static struct uac_clock_source_descriptor in_clk_src_desc = {
 	.bLength = sizeof in_clk_src_desc,
 	.bDescriptorType = USB_DT_CS_INTERFACE,
 
@@ -646,7 +646,7 @@ struct uac_clock_source_descriptor in_clk_src_desc = {
 };
 
 /* Clock source for OUT traffic */
-struct uac_clock_source_descriptor out_clk_src_desc = {
+static struct uac_clock_source_descriptor out_clk_src_desc = {
 	.bLength = sizeof out_clk_src_desc,
 	.bDescriptorType = USB_DT_CS_INTERFACE,
 
@@ -658,7 +658,7 @@ struct uac_clock_source_descriptor out_clk_src_desc = {
 };
 
 /* Input Terminal for USB_OUT */
-struct uac2_input_terminal_descriptor usb_out_it_desc = {
+static struct uac2_input_terminal_descriptor usb_out_it_desc = {
 	.bLength = sizeof usb_out_it_desc,
 	.bDescriptorType = USB_DT_CS_INTERFACE,
 
@@ -672,7 +672,7 @@ struct uac2_input_terminal_descriptor usb_out_it_desc = {
 };
 
 /* Input Terminal for I/O-In */
-struct uac2_input_terminal_descriptor io_in_it_desc = {
+static struct uac2_input_terminal_descriptor io_in_it_desc = {
 	.bLength = sizeof io_in_it_desc,
 	.bDescriptorType = USB_DT_CS_INTERFACE,
 
@@ -686,7 +686,7 @@ struct uac2_input_terminal_descriptor io_in_it_desc = {
 };
 
 /* Ouput Terminal for USB_IN */
-struct uac2_output_terminal_descriptor usb_in_ot_desc = {
+static struct uac2_output_terminal_descriptor usb_in_ot_desc = {
 	.bLength = sizeof usb_in_ot_desc,
 	.bDescriptorType = USB_DT_CS_INTERFACE,
 
@@ -700,7 +700,7 @@ struct uac2_output_terminal_descriptor usb_in_ot_desc = {
 };
 
 /* Ouput Terminal for I/O-Out */
-struct uac2_output_terminal_descriptor io_out_ot_desc = {
+static struct uac2_output_terminal_descriptor io_out_ot_desc = {
 	.bLength = sizeof io_out_ot_desc,
 	.bDescriptorType = USB_DT_CS_INTERFACE,
 
@@ -713,7 +713,7 @@ struct uac2_output_terminal_descriptor io_out_ot_desc = {
 	.bmControls = (CONTROL_RDWR << COPY_CTRL),
 };
 
-struct uac2_ac_header_descriptor ac_hdr_desc = {
+static struct uac2_ac_header_descriptor ac_hdr_desc = {
 	.bLength = sizeof ac_hdr_desc,
 	.bDescriptorType = USB_DT_CS_INTERFACE,
 
@@ -751,7 +751,7 @@ static struct usb_interface_descriptor std_as_out_if1_desc = {
 };
 
 /* Audio Stream OUT Intface Desc */
-struct uac2_as_header_descriptor as_out_hdr_desc = {
+static struct uac2_as_header_descriptor as_out_hdr_desc = {
 	.bLength = sizeof as_out_hdr_desc,
 	.bDescriptorType = USB_DT_CS_INTERFACE,
 
@@ -764,7 +764,7 @@ struct uac2_as_header_descriptor as_out_hdr_desc = {
 };
 
 /* Audio USB_OUT Format */
-struct uac2_format_type_i_descriptor as_out_fmt1_desc = {
+static struct uac2_format_type_i_descriptor as_out_fmt1_desc = {
 	.bLength = sizeof as_out_fmt1_desc,
 	.bDescriptorType = USB_DT_CS_INTERFACE,
 	.bDescriptorSubtype = UAC_FORMAT_TYPE,
@@ -772,7 +772,7 @@ struct uac2_format_type_i_descriptor as_out_fmt1_desc = {
 };
 
 /* STD AS ISO OUT Endpoint */
-struct usb_endpoint_descriptor fs_epout_desc = {
+static struct usb_endpoint_descriptor fs_epout_desc = {
 	.bLength = USB_DT_ENDPOINT_SIZE,
 	.bDescriptorType = USB_DT_ENDPOINT,
 
@@ -782,7 +782,7 @@ struct usb_endpoint_descriptor fs_epout_desc = {
 	.bInterval = 1,
 };
 
-struct usb_endpoint_descriptor hs_epout_desc = {
+static struct usb_endpoint_descriptor hs_epout_desc = {
 	.bLength = USB_DT_ENDPOINT_SIZE,
 	.bDescriptorType = USB_DT_ENDPOINT,
 
@@ -828,7 +828,7 @@ static struct usb_interface_descriptor std_as_in_if1_desc = {
 };
 
 /* Audio Stream IN Intface Desc */
-struct uac2_as_header_descriptor as_in_hdr_desc = {
+static struct uac2_as_header_descriptor as_in_hdr_desc = {
 	.bLength = sizeof as_in_hdr_desc,
 	.bDescriptorType = USB_DT_CS_INTERFACE,
 
@@ -841,7 +841,7 @@ struct uac2_as_header_descriptor as_in_hdr_desc = {
 };
 
 /* Audio USB_IN Format */
-struct uac2_format_type_i_descriptor as_in_fmt1_desc = {
+static struct uac2_format_type_i_descriptor as_in_fmt1_desc = {
 	.bLength = sizeof as_in_fmt1_desc,
 	.bDescriptorType = USB_DT_CS_INTERFACE,
 	.bDescriptorSubtype = UAC_FORMAT_TYPE,
@@ -849,7 +849,7 @@ struct uac2_format_type_i_descriptor as_in_fmt1_desc = {
 };
 
 /* STD AS ISO IN Endpoint */
-struct usb_endpoint_descriptor fs_epin_desc = {
+static struct usb_endpoint_descriptor fs_epin_desc = {
 	.bLength = USB_DT_ENDPOINT_SIZE,
 	.bDescriptorType = USB_DT_ENDPOINT,
 
@@ -859,7 +859,7 @@ struct usb_endpoint_descriptor fs_epin_desc = {
 	.bInterval = 1,
 };
 
-struct usb_endpoint_descriptor hs_epin_desc = {
+static struct usb_endpoint_descriptor hs_epin_desc = {
 	.bLength = USB_DT_ENDPOINT_SIZE,
 	.bDescriptorType = USB_DT_ENDPOINT,
 
@@ -1563,7 +1563,7 @@ static void afunc_unbind(struct usb_configuration *c, struct usb_function *f)
 		agdev->out_ep->driver_data = NULL;
 }
 
-struct usb_function *afunc_alloc(struct usb_function_instance *fi)
+static struct usb_function *afunc_alloc(struct usb_function_instance *fi)
 {
 	struct audio_dev *agdev;
 	struct f_uac2_opts *opts;
diff --git a/drivers/usb/gadget/function/g_zero.h b/drivers/usb/gadget/function/g_zero.h
index 2ce28b9d97cc..15f180904f8a 100644
--- a/drivers/usb/gadget/function/g_zero.h
+++ b/drivers/usb/gadget/function/g_zero.h
@@ -10,8 +10,6 @@
 #define GZERO_QLEN		32
 #define GZERO_ISOC_INTERVAL	4
 #define GZERO_ISOC_MAXPACKET	1024
-#define GZERO_INT_INTERVAL	1 /* Default interrupt interval = 1 ms */
-#define GZERO_INT_MAXPACKET	1024
 
 struct usb_zero_options {
 	unsigned pattern;
@@ -19,10 +17,6 @@ struct usb_zero_options {
 	unsigned isoc_maxpacket;
 	unsigned isoc_mult;
 	unsigned isoc_maxburst;
-	unsigned int_interval; /* In ms */
-	unsigned int_maxpacket;
-	unsigned int_mult;
-	unsigned int_maxburst;
 	unsigned bulk_buflen;
 	unsigned qlen;
 };
@@ -34,10 +28,6 @@ struct f_ss_opts {
 	unsigned isoc_maxpacket;
 	unsigned isoc_mult;
 	unsigned isoc_maxburst;
-	unsigned int_interval; /* In ms */
-	unsigned int_maxpacket;
-	unsigned int_mult;
-	unsigned int_maxburst;
 	unsigned bulk_buflen;
 
 	/*
@@ -72,7 +62,6 @@ int lb_modinit(void);
 void free_ep_req(struct usb_ep *ep, struct usb_request *req);
 void disable_endpoints(struct usb_composite_dev *cdev,
 		struct usb_ep *in, struct usb_ep *out,
-		struct usb_ep *iso_in, struct usb_ep *iso_out,
-		struct usb_ep *int_in, struct usb_ep *int_out);
+		struct usb_ep *iso_in, struct usb_ep *iso_out);
 
 #endif /* __G_ZERO_H */
diff --git a/drivers/usb/gadget/function/u_printer.h b/drivers/usb/gadget/function/u_printer.h
new file mode 100644
index 000000000000..0e2c49d4274e
--- /dev/null
+++ b/drivers/usb/gadget/function/u_printer.h
@@ -0,0 +1,37 @@
+/*
+ * u_printer.h
+ *
+ * Utility definitions for the printer function
+ *
+ * Copyright (c) 2015 Samsung Electronics Co., Ltd.
+ *		http://www.samsung.com
+ *
+ * Author: Andrzej Pietrasiewicz <andrzej.p@samsung.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef U_PRINTER_H
+#define U_PRINTER_H
+
+#include <linux/usb/composite.h>
+
+#define PNP_STRING_LEN			1024
+
+struct f_printer_opts {
+	struct usb_function_instance	func_inst;
+	int				minor;
+	char				pnp_string[PNP_STRING_LEN];
+	unsigned			q_len;
+
+	/*
+	 * Protect the data from concurrent access by read/write
+	 * and create symlink/remove symlink
+	 */
+	struct mutex			lock;
+	int				refcnt;
+};
+
+#endif /* U_PRINTER_H */
diff --git a/drivers/usb/gadget/function/u_serial.c b/drivers/usb/gadget/function/u_serial.c
index 491082aaf103..89179ab20c10 100644
--- a/drivers/usb/gadget/function/u_serial.c
+++ b/drivers/usb/gadget/function/u_serial.c
@@ -912,7 +912,7 @@ static int gs_put_char(struct tty_struct *tty, unsigned char ch)
 	unsigned long	flags;
 	int		status;
 
-	pr_vdebug("gs_put_char: (%d,%p) char=0x%x, called from %pf\n",
+	pr_vdebug("gs_put_char: (%d,%p) char=0x%x, called from %ps\n",
 		port->port_num, tty, ch, __builtin_return_address(0));
 
 	spin_lock_irqsave(&port->port_lock, flags);
diff --git a/drivers/usb/gadget/function/uvc_v4l2.c b/drivers/usb/gadget/function/uvc_v4l2.c
index 5aad7fededa5..8b818fd027b3 100644
--- a/drivers/usb/gadget/function/uvc_v4l2.c
+++ b/drivers/usb/gadget/function/uvc_v4l2.c
@@ -27,6 +27,7 @@
 #include "uvc.h"
 #include "uvc_queue.h"
 #include "uvc_video.h"
+#include "uvc_v4l2.h"
 
 /* --------------------------------------------------------------------------
  * Requests handling
diff --git a/drivers/usb/gadget/function/uvc_video.c b/drivers/usb/gadget/function/uvc_video.c
index 9cb86bc1a9a5..50a5e637ca35 100644
--- a/drivers/usb/gadget/function/uvc_video.c
+++ b/drivers/usb/gadget/function/uvc_video.c
@@ -21,6 +21,7 @@
 
 #include "uvc.h"
 #include "uvc_queue.h"
+#include "uvc_video.h"
 
 /* --------------------------------------------------------------------------
  * Video codecs
diff --git a/drivers/usb/gadget/legacy/Kconfig b/drivers/usb/gadget/legacy/Kconfig
index 113c87e22117..d5a7102de696 100644
--- a/drivers/usb/gadget/legacy/Kconfig
+++ b/drivers/usb/gadget/legacy/Kconfig
@@ -301,6 +301,7 @@ config USB_MIDI_GADGET
 config USB_G_PRINTER
 	tristate "Printer Gadget"
 	select USB_LIBCOMPOSITE
+	select USB_F_PRINTER
 	help
 	  The Printer Gadget channels data between the USB host and a
 	  userspace program driving the print engine. The user space
diff --git a/drivers/usb/gadget/legacy/g_ffs.c b/drivers/usb/gadget/legacy/g_ffs.c
index 06acfa55864a..b01b88e1b716 100644
--- a/drivers/usb/gadget/legacy/g_ffs.c
+++ b/drivers/usb/gadget/legacy/g_ffs.c
@@ -133,7 +133,9 @@ struct gfs_configuration {
 	struct usb_configuration c;
 	int (*eth)(struct usb_configuration *c);
 	int num;
-} gfs_configurations[] = {
+};
+
+static struct gfs_configuration gfs_configurations[] = {
 #ifdef CONFIG_USB_FUNCTIONFS_RNDIS
 	{
 		.eth		= bind_rndis_config,
@@ -278,7 +280,7 @@ static void *functionfs_acquire_dev(struct ffs_dev *dev)
 	if (!try_module_get(THIS_MODULE))
 		return ERR_PTR(-ENOENT);
 	
-	return 0;
+	return NULL;
 }
 
 static void functionfs_release_dev(struct ffs_dev *dev)
diff --git a/drivers/usb/gadget/legacy/inode.c b/drivers/usb/gadget/legacy/inode.c
index db49ec4c748e..200f9a584064 100644
--- a/drivers/usb/gadget/legacy/inode.c
+++ b/drivers/usb/gadget/legacy/inode.c
@@ -74,6 +74,8 @@ MODULE_DESCRIPTION (DRIVER_DESC);
 MODULE_AUTHOR ("David Brownell");
 MODULE_LICENSE ("GPL");
 
+static int ep_open(struct inode *, struct file *);
+
 
 /*----------------------------------------------------------------------*/
 
@@ -283,14 +285,15 @@ static void epio_complete (struct usb_ep *ep, struct usb_request *req)
  * still need dev->lock to use epdata->ep.
  */
 static int
-get_ready_ep (unsigned f_flags, struct ep_data *epdata)
+get_ready_ep (unsigned f_flags, struct ep_data *epdata, bool is_write)
 {
 	int	val;
 
 	if (f_flags & O_NONBLOCK) {
 		if (!mutex_trylock(&epdata->lock))
 			goto nonblock;
-		if (epdata->state != STATE_EP_ENABLED) {
+		if (epdata->state != STATE_EP_ENABLED &&
+		    (!is_write || epdata->state != STATE_EP_READY)) {
 			mutex_unlock(&epdata->lock);
 nonblock:
 			val = -EAGAIN;
@@ -305,18 +308,20 @@ nonblock:
 
 	switch (epdata->state) {
 	case STATE_EP_ENABLED:
+		return 0;
+	case STATE_EP_READY:			/* not configured yet */
+		if (is_write)
+			return 0;
+		// FALLTHRU
+	case STATE_EP_UNBOUND:			/* clean disconnect */
 		break;
 	// case STATE_EP_DISABLED:		/* "can't happen" */
-	// case STATE_EP_READY:			/* "can't happen" */
 	default:				/* error! */
 		pr_debug ("%s: ep %p not available, state %d\n",
 				shortname, epdata, epdata->state);
-		// FALLTHROUGH
-	case STATE_EP_UNBOUND:			/* clean disconnect */
-		val = -ENODEV;
-		mutex_unlock(&epdata->lock);
 	}
-	return val;
+	mutex_unlock(&epdata->lock);
+	return -ENODEV;
 }
 
 static ssize_t
@@ -363,97 +368,6 @@ ep_io (struct ep_data *epdata, void *buf, unsigned len)
 	return value;
 }
 
-
-/* handle a synchronous OUT bulk/intr/iso transfer */
-static ssize_t
-ep_read (struct file *fd, char __user *buf, size_t len, loff_t *ptr)
-{
-	struct ep_data		*data = fd->private_data;
-	void			*kbuf;
-	ssize_t			value;
-
-	if ((value = get_ready_ep (fd->f_flags, data)) < 0)
-		return value;
-
-	/* halt any endpoint by doing a "wrong direction" i/o call */
-	if (usb_endpoint_dir_in(&data->desc)) {
-		if (usb_endpoint_xfer_isoc(&data->desc)) {
-			mutex_unlock(&data->lock);
-			return -EINVAL;
-		}
-		DBG (data->dev, "%s halt\n", data->name);
-		spin_lock_irq (&data->dev->lock);
-		if (likely (data->ep != NULL))
-			usb_ep_set_halt (data->ep);
-		spin_unlock_irq (&data->dev->lock);
-		mutex_unlock(&data->lock);
-		return -EBADMSG;
-	}
-
-	/* FIXME readahead for O_NONBLOCK and poll(); careful with ZLPs */
-
-	value = -ENOMEM;
-	kbuf = kmalloc (len, GFP_KERNEL);
-	if (unlikely (!kbuf))
-		goto free1;
-
-	value = ep_io (data, kbuf, len);
-	VDEBUG (data->dev, "%s read %zu OUT, status %d\n",
-		data->name, len, (int) value);
-	if (value >= 0 && copy_to_user (buf, kbuf, value))
-		value = -EFAULT;
-
-free1:
-	mutex_unlock(&data->lock);
-	kfree (kbuf);
-	return value;
-}
-
-/* handle a synchronous IN bulk/intr/iso transfer */
-static ssize_t
-ep_write (struct file *fd, const char __user *buf, size_t len, loff_t *ptr)
-{
-	struct ep_data		*data = fd->private_data;
-	void			*kbuf;
-	ssize_t			value;
-
-	if ((value = get_ready_ep (fd->f_flags, data)) < 0)
-		return value;
-
-	/* halt any endpoint by doing a "wrong direction" i/o call */
-	if (!usb_endpoint_dir_in(&data->desc)) {
-		if (usb_endpoint_xfer_isoc(&data->desc)) {
-			mutex_unlock(&data->lock);
-			return -EINVAL;
-		}
-		DBG (data->dev, "%s halt\n", data->name);
-		spin_lock_irq (&data->dev->lock);
-		if (likely (data->ep != NULL))
-			usb_ep_set_halt (data->ep);
-		spin_unlock_irq (&data->dev->lock);
-		mutex_unlock(&data->lock);
-		return -EBADMSG;
-	}
-
-	/* FIXME writebehind for O_NONBLOCK and poll(), qlen = 1 */
-
-	value = -ENOMEM;
-	kbuf = memdup_user(buf, len);
-	if (IS_ERR(kbuf)) {
-		value = PTR_ERR(kbuf);
-		kbuf = NULL;
-		goto free1;
-	}
-
-	value = ep_io (data, kbuf, len);
-	VDEBUG (data->dev, "%s write %zu IN, status %d\n",
-		data->name, len, (int) value);
-free1:
-	mutex_unlock(&data->lock);
-	kfree (kbuf);
-	return value;
-}
-
 static int
 ep_release (struct inode *inode, struct file *fd)
 {
@@ -481,7 +395,7 @@ static long ep_ioctl(struct file *fd, unsigned code, unsigned long value)
 	struct ep_data		*data = fd->private_data;
 	int			status;
 
-	if ((status = get_ready_ep (fd->f_flags, data)) < 0)
+	if ((status = get_ready_ep (fd->f_flags, data, false)) < 0)
 		return status;
 
 	spin_lock_irq (&data->dev->lock);
@@ -517,8 +431,8 @@ struct kiocb_priv {
 	struct mm_struct	*mm;
 	struct work_struct	work;
 	void			*buf;
-	const struct iovec	*iv;
-	unsigned long		nr_segs;
+	struct iov_iter		to;
+	const void		*to_free;
 	unsigned		actual;
 };
 
@@ -541,35 +455,6 @@ static int ep_aio_cancel(struct kiocb *iocb)
 	return value;
 }
 
-static ssize_t ep_copy_to_user(struct kiocb_priv *priv)
-{
-	ssize_t			len, total;
-	void			*to_copy;
-	int			i;
-
-	/* copy stuff into user buffers */
-	total = priv->actual;
-	len = 0;
-	to_copy = priv->buf;
-	for (i=0; i < priv->nr_segs; i++) {
-		ssize_t this = min((ssize_t)(priv->iv[i].iov_len), total);
-
-		if (copy_to_user(priv->iv[i].iov_base, to_copy, this)) {
-			if (len == 0)
-				len = -EFAULT;
-			break;
-		}
-
-		total -= this;
-		len += this;
-		to_copy += this;
-		if (total == 0)
-			break;
-	}
-
-	return len;
-}
-
 static void ep_user_copy_worker(struct work_struct *work)
 {
 	struct kiocb_priv *priv = container_of(work, struct kiocb_priv, work);
@@ -578,13 +463,16 @@ static void ep_user_copy_worker(struct work_struct *work)
 	size_t ret;
 
 	use_mm(mm);
-	ret = ep_copy_to_user(priv);
+	ret = copy_to_iter(priv->buf, priv->actual, &priv->to);
 	unuse_mm(mm);
+	if (!ret)
+		ret = -EFAULT;
 
 	/* completing the iocb can drop the ctx and mm, don't touch mm after */
 	aio_complete(iocb, ret, ret);
 
 	kfree(priv->buf);
+	kfree(priv->to_free);
 	kfree(priv);
 }
 
@@ -603,8 +491,9 @@ static void ep_aio_complete(struct usb_ep *ep, struct usb_request *req)
 	 * don't need to copy anything to userspace, so we can
 	 * complete the aio request immediately.
 	 */
-	if (priv->iv == NULL || unlikely(req->actual == 0)) {
+	if (priv->to_free == NULL || unlikely(req->actual == 0)) {
 		kfree(req->buf);
+		kfree(priv->to_free);
 		kfree(priv);
 		iocb->private = NULL;
 		/* aio_complete() reports bytes-transferred _and_ faults */
@@ -618,6 +507,7 @@ static void ep_aio_complete(struct usb_ep *ep, struct usb_request *req)
 
 		priv->buf = req->buf;
 		priv->actual = req->actual;
+		INIT_WORK(&priv->work, ep_user_copy_worker);
 		schedule_work(&priv->work);
 	}
 	spin_unlock(&epdata->dev->lock);
@@ -626,38 +516,17 @@ static void ep_aio_complete(struct usb_ep *ep, struct usb_request *req)
 	put_ep(epdata);
 }
 
-static ssize_t
-ep_aio_rwtail(
-	struct kiocb	*iocb,
-	char		*buf,
-	size_t		len,
-	struct ep_data	*epdata,
-	const struct iovec *iv,
-	unsigned long	nr_segs
-)
+static ssize_t ep_aio(struct kiocb *iocb,
+		      struct kiocb_priv *priv,
+		      struct ep_data *epdata,
+		      char *buf,
+		      size_t len)
 {
-	struct kiocb_priv	*priv;
-	struct usb_request	*req;
-	ssize_t			value;
+	struct usb_request *req;
+	ssize_t value;
 
-	priv = kmalloc(sizeof *priv, GFP_KERNEL);
-	if (!priv) {
-		value = -ENOMEM;
-fail:
-		kfree(buf);
-		return value;
-	}
 	iocb->private = priv;
 	priv->iocb = iocb;
-	priv->iv = iv;
-	priv->nr_segs = nr_segs;
-	INIT_WORK(&priv->work, ep_user_copy_worker);
-
-	value = get_ready_ep(iocb->ki_filp->f_flags, epdata);
-	if (unlikely(value < 0)) {
-		kfree(priv);
-		goto fail;
-	}
 
 	kiocb_set_cancel_fn(iocb, ep_aio_cancel);
 	get_ep(epdata);
@@ -669,75 +538,154 @@ fail:
 	 * allocate or submit those if the host disconnected.
 	 */
 	spin_lock_irq(&epdata->dev->lock);
-	if (likely(epdata->ep)) {
-		req = usb_ep_alloc_request(epdata->ep, GFP_ATOMIC);
-		if (likely(req)) {
-			priv->req = req;
-			req->buf = buf;
-			req->length = len;
-			req->complete = ep_aio_complete;
-			req->context = iocb;
-			value = usb_ep_queue(epdata->ep, req, GFP_ATOMIC);
-			if (unlikely(0 != value))
-				usb_ep_free_request(epdata->ep, req);
-		} else
-			value = -EAGAIN;
-	} else
-		value = -ENODEV;
-	spin_unlock_irq(&epdata->dev->lock);
+	value = -ENODEV;
+	if (unlikely(epdata->ep))
+		goto fail;
 
-	mutex_unlock(&epdata->lock);
+	req = usb_ep_alloc_request(epdata->ep, GFP_ATOMIC);
+	value = -ENOMEM;
+	if (unlikely(!req))
+		goto fail;
 
-	if (unlikely(value)) {
-		kfree(priv);
-		put_ep(epdata);
-	} else
-		value = -EIOCBQUEUED;
+	priv->req = req;
+	req->buf = buf;
+	req->length = len;
+	req->complete = ep_aio_complete;
+	req->context = iocb;
+	value = usb_ep_queue(epdata->ep, req, GFP_ATOMIC);
+	if (unlikely(0 != value)) {
+		usb_ep_free_request(epdata->ep, req);
+		goto fail;
+	}
+	spin_unlock_irq(&epdata->dev->lock);
+	return -EIOCBQUEUED;
+
+fail:
+	spin_unlock_irq(&epdata->dev->lock);
+	kfree(priv->to_free);
+	kfree(priv);
+	put_ep(epdata);
 	return value;
 }
 
 static ssize_t
-ep_aio_read(struct kiocb *iocb, const struct iovec *iov,
-		unsigned long nr_segs, loff_t o)
+ep_read_iter(struct kiocb *iocb, struct iov_iter *to)
 {
-	struct ep_data		*epdata = iocb->ki_filp->private_data;
-	char			*buf;
+	struct file *file = iocb->ki_filp;
+	struct ep_data *epdata = file->private_data;
+	size_t len = iov_iter_count(to);
+	ssize_t value;
+	char *buf;
 
-	if (unlikely(usb_endpoint_dir_in(&epdata->desc)))
-		return -EINVAL;
+	if ((value = get_ready_ep(file->f_flags, epdata, false)) < 0)
+		return value;
 
-	buf = kmalloc(iocb->ki_nbytes, GFP_KERNEL);
-	if (unlikely(!buf))
-		return -ENOMEM;
+	/* halt any endpoint by doing a "wrong direction" i/o call */
+	if (usb_endpoint_dir_in(&epdata->desc)) {
+		if (usb_endpoint_xfer_isoc(&epdata->desc) ||
+		    !is_sync_kiocb(iocb)) {
+			mutex_unlock(&epdata->lock);
+			return -EINVAL;
+		}
+		DBG (epdata->dev, "%s halt\n", epdata->name);
+		spin_lock_irq(&epdata->dev->lock);
+		if (likely(epdata->ep != NULL))
+			usb_ep_set_halt(epdata->ep);
+		spin_unlock_irq(&epdata->dev->lock);
+		mutex_unlock(&epdata->lock);
+		return -EBADMSG;
+	}
 
-	return ep_aio_rwtail(iocb, buf, iocb->ki_nbytes, epdata, iov, nr_segs);
+	buf = kmalloc(len, GFP_KERNEL);
+	if (unlikely(!buf)) {
+		mutex_unlock(&epdata->lock);
+		return -ENOMEM;
+	}
+	if (is_sync_kiocb(iocb)) {
+		value = ep_io(epdata, buf, len);
+		if (value >= 0 && copy_to_iter(buf, value, to))
+			value = -EFAULT;
+	} else {
+		struct kiocb_priv *priv = kzalloc(sizeof *priv, GFP_KERNEL);
+		value = -ENOMEM;
+		if (!priv)
+			goto fail;
+		priv->to_free = dup_iter(&priv->to, to, GFP_KERNEL);
+		if (!priv->to_free) {
+			kfree(priv);
+			goto fail;
+		}
+		value = ep_aio(iocb, priv, epdata, buf, len);
+		if (value == -EIOCBQUEUED)
+			buf = NULL;
+	}
+fail:
+	kfree(buf);
+	mutex_unlock(&epdata->lock);
+	return value;
 }
 
+static ssize_t ep_config(struct ep_data *, const char *, size_t);
+
 static ssize_t
-ep_aio_write(struct kiocb *iocb, const struct iovec *iov,
-		unsigned long nr_segs, loff_t o)
+ep_write_iter(struct kiocb *iocb, struct iov_iter *from)
 {
-	struct ep_data		*epdata = iocb->ki_filp->private_data;
-	char			*buf;
-	size_t			len = 0;
-	int			i = 0;
+	struct file *file = iocb->ki_filp;
+	struct ep_data *epdata = file->private_data;
+	size_t len = iov_iter_count(from);
+	bool configured;
+	ssize_t value;
+	char *buf;
+
+	if ((value = get_ready_ep(file->f_flags, epdata, true)) < 0)
+		return value;
 
-	if (unlikely(!usb_endpoint_dir_in(&epdata->desc)))
-		return -EINVAL;
+	configured = epdata->state == STATE_EP_ENABLED;
 
-	buf = kmalloc(iocb->ki_nbytes, GFP_KERNEL);
-	if (unlikely(!buf))
+	/* halt any endpoint by doing a "wrong direction" i/o call */
+	if (configured && !usb_endpoint_dir_in(&epdata->desc)) {
+		if (usb_endpoint_xfer_isoc(&epdata->desc) ||
+		    !is_sync_kiocb(iocb)) {
+			mutex_unlock(&epdata->lock);
+			return -EINVAL;
+		}
+		DBG (epdata->dev, "%s halt\n", epdata->name);
+		spin_lock_irq(&epdata->dev->lock);
+		if (likely(epdata->ep != NULL))
+			usb_ep_set_halt(epdata->ep);
+		spin_unlock_irq(&epdata->dev->lock);
+		mutex_unlock(&epdata->lock);
+		return -EBADMSG;
+	}
+
+	buf = kmalloc(len, GFP_KERNEL);
+	if (unlikely(!buf)) {
+		mutex_unlock(&epdata->lock);
 		return -ENOMEM;
+	}
 
-	for (i=0; i < nr_segs; i++) {
-		if (unlikely(copy_from_user(&buf[len], iov[i].iov_base,
-				iov[i].iov_len) != 0)) {
-			kfree(buf);
-			return -EFAULT;
+	if (unlikely(copy_from_iter(buf, len, from) != len)) {
+		value = -EFAULT;
+		goto out;
+	}
+
+	if (unlikely(!configured)) {
+		value = ep_config(epdata, buf, len);
+	} else if (is_sync_kiocb(iocb)) {
+		value = ep_io(epdata, buf, len);
+	} else {
+		struct kiocb_priv *priv = kzalloc(sizeof *priv, GFP_KERNEL);
+		value = -ENOMEM;
+		if (priv) {
+			value = ep_aio(iocb, priv, epdata, buf, len);
+			if (value == -EIOCBQUEUED)
+				buf = NULL;
 		}
-		len += iov[i].iov_len;
 	}
-	return ep_aio_rwtail(iocb, buf, len, epdata, NULL, 0);
+out:
+	kfree(buf);
+	mutex_unlock(&epdata->lock);
+	return value;
 }
 
 /*----------------------------------------------------------------------*/
@@ -745,15 +693,15 @@ ep_aio_write(struct kiocb *iocb, const struct iovec *iov,
 /* used after endpoint configuration */
 static const struct file_operations ep_io_operations = {
 	.owner =	THIS_MODULE,
-	.llseek =	no_llseek,
 
-	.read =		ep_read,
-	.write =	ep_write,
-	.unlocked_ioctl = ep_ioctl,
+	.open =		ep_open,
 	.release =	ep_release,
-
-	.aio_read =	ep_aio_read,
-	.aio_write =	ep_aio_write,
+	.llseek =	no_llseek,
+	.read =		new_sync_read,
+	.write =	new_sync_write,
+	.unlocked_ioctl = ep_ioctl,
+	.read_iter =	ep_read_iter,
+	.write_iter =	ep_write_iter,
 };
 
 /* ENDPOINT INITIALIZATION
@@ -770,17 +718,12 @@ static const struct file_operations ep_io_operations = {
  * speed descriptor, then optional high speed descriptor.
  */
 static ssize_t
-ep_config (struct file *fd, const char __user *buf, size_t len, loff_t *ptr)
+ep_config (struct ep_data *data, const char *buf, size_t len)
 {
-	struct ep_data		*data = fd->private_data;
 	struct usb_ep		*ep;
 	u32			tag;
 	int			value, length = len;
 
-	value = mutex_lock_interruptible(&data->lock);
-	if (value < 0)
-		return value;
-
 	if (data->state != STATE_EP_READY) {
 		value = -EL2HLT;
 		goto fail;
@@ -791,9 +734,7 @@ ep_config (struct file *fd, const char __user *buf, size_t len, loff_t *ptr)
 		goto fail0;
 
 	/* we might need to change message format someday */
-	if (copy_from_user (&tag, buf, 4)) {
-		goto fail1;
-	}
+	memcpy(&tag, buf, 4);
 	if (tag != 1) {
 		DBG(data->dev, "config %s, bad tag %d\n", data->name, tag);
 		goto fail0;
@@ -806,19 +747,15 @@ ep_config (struct file *fd, const char __user *buf, size_t len, loff_t *ptr)
 	 */
 
 	/* full/low speed descriptor, then high speed */
-	if (copy_from_user (&data->desc, buf, USB_DT_ENDPOINT_SIZE)) {
-		goto fail1;
-	}
+	memcpy(&data->desc, buf, USB_DT_ENDPOINT_SIZE);
 	if (data->desc.bLength != USB_DT_ENDPOINT_SIZE
 			|| data->desc.bDescriptorType != USB_DT_ENDPOINT)
 		goto fail0;
 	if (len != USB_DT_ENDPOINT_SIZE) {
 		if (len != 2 * USB_DT_ENDPOINT_SIZE)
 			goto fail0;
-		if (copy_from_user (&data->hs_desc, buf + USB_DT_ENDPOINT_SIZE,
-					USB_DT_ENDPOINT_SIZE)) {
-			goto fail1;
-		}
+		memcpy(&data->hs_desc, buf + USB_DT_ENDPOINT_SIZE,
+			USB_DT_ENDPOINT_SIZE);
 		if (data->hs_desc.bLength != USB_DT_ENDPOINT_SIZE
 				|| data->hs_desc.bDescriptorType
 					!= USB_DT_ENDPOINT) {
@@ -840,24 +777,20 @@ ep_config (struct file *fd, const char __user *buf, size_t len, loff_t *ptr)
 	case USB_SPEED_LOW:
 	case USB_SPEED_FULL:
 		ep->desc = &data->desc;
-		value = usb_ep_enable(ep);
-		if (value == 0)
-			data->state = STATE_EP_ENABLED;
 		break;
 	case USB_SPEED_HIGH:
 		/* fails if caller didn't provide that descriptor... */
 		ep->desc = &data->hs_desc;
-		value = usb_ep_enable(ep);
-		if (value == 0)
-			data->state = STATE_EP_ENABLED;
 		break;
 	default:
 		DBG(data->dev, "unconnected, %s init abandoned\n",
 				data->name);
 		value = -EINVAL;
+		goto gone;
 	}
+	value = usb_ep_enable(ep);
 	if (value == 0) {
-		fd->f_op = &ep_io_operations;
+		data->state = STATE_EP_ENABLED;
 		value = length;
 	}
 gone:
@@ -867,14 +800,10 @@ fail:
 		data->desc.bDescriptorType = 0;
 		data->hs_desc.bDescriptorType = 0;
 	}
-	mutex_unlock(&data->lock);
 	return value;
 fail0:
 	value = -EINVAL;
 	goto fail;
-fail1:
-	value = -EFAULT;
-	goto fail;
 }
 
 static int
@@ -902,15 +831,6 @@ ep_open (struct inode *inode, struct file *fd)
 	return value;
 }
 
-/* used before endpoint configuration */
-static const struct file_operations ep_config_operations = {
-	.llseek =	no_llseek,
-
-	.open =		ep_open,
-	.write =	ep_config,
-	.release =	ep_release,
-};
-
 /*----------------------------------------------------------------------*/
 
 /* EP0 IMPLEMENTATION can be partly in userspace.
@@ -989,6 +909,10 @@ ep0_read (struct file *fd, char __user *buf, size_t len, loff_t *ptr)
 	enum ep0_state			state;
 
 	spin_lock_irq (&dev->lock);
+	if (dev->state <= STATE_DEV_OPENED) {
+		retval = -EINVAL;
+		goto done;
+	}
 
 	/* report fd mode change before acting on it */
 	if (dev->setup_abort) {
@@ -1187,8 +1111,6 @@ ep0_write (struct file *fd, const char __user *buf, size_t len, loff_t *ptr)
 	struct dev_data		*dev = fd->private_data;
 	ssize_t			retval = -ESRCH;
 
-	spin_lock_irq (&dev->lock);
-
 	/* report fd mode change before acting on it */
 	if (dev->setup_abort) {
 		dev->setup_abort = 0;
@@ -1234,7 +1156,6 @@ ep0_write (struct file *fd, const char __user *buf, size_t len, loff_t *ptr)
 	} else
 		DBG (dev, "fail %s, state %d\n", __func__, dev->state);
 
-	spin_unlock_irq (&dev->lock);
 	return retval;
 }
 
@@ -1281,6 +1202,9 @@ ep0_poll (struct file *fd, poll_table *wait)
        struct dev_data         *dev = fd->private_data;
        int                     mask = 0;
 
+	if (dev->state <= STATE_DEV_OPENED)
+		return DEFAULT_POLLMASK;
+
        poll_wait(fd, &dev->wait, wait);
 
        spin_lock_irq (&dev->lock);
@@ -1316,19 +1240,6 @@ static long dev_ioctl (struct file *fd, unsigned code, unsigned long value)
 	return ret;
 }
 
-/* used after device configuration */
-static const struct file_operations ep0_io_operations = {
-	.owner =	THIS_MODULE,
-	.llseek =	no_llseek,
-
-	.read =		ep0_read,
-	.write =	ep0_write,
-	.fasync =	ep0_fasync,
-	.poll =		ep0_poll,
-	.unlocked_ioctl =	dev_ioctl,
-	.release =	dev_release,
-};
-
 /*----------------------------------------------------------------------*/
 
 /* The in-kernel gadget driver handles most ep0 issues, in particular
@@ -1650,7 +1561,7 @@ static int activate_ep_files (struct dev_data *dev)
 			goto enomem1;
 
 		data->dentry = gadgetfs_create_file (dev->sb, data->name,
-				data, &ep_config_operations);
+				data, &ep_io_operations);
 		if (!data->dentry)
 			goto enomem2;
 		list_add_tail (&data->epfiles, &dev->epfiles);
@@ -1852,6 +1763,14 @@ dev_config (struct file *fd, const char __user *buf, size_t len, loff_t *ptr)
 	u32			tag;
 	char			*kbuf;
 
+	spin_lock_irq(&dev->lock);
+	if (dev->state > STATE_DEV_OPENED) {
+		value = ep0_write(fd, buf, len, ptr);
+		spin_unlock_irq(&dev->lock);
+		return value;
+	}
+	spin_unlock_irq(&dev->lock);
+
 	if (len < (USB_DT_CONFIG_SIZE + USB_DT_DEVICE_SIZE + 4))
 		return -EINVAL;
 
@@ -1925,7 +1844,6 @@ dev_config (struct file *fd, const char __user *buf, size_t len, loff_t *ptr)
 		 * on, they can work ... except in cleanup paths that
 		 * kick in after the ep0 descriptor is closed.
 		 */
-		fd->f_op = &ep0_io_operations;
 		value = len;
 	}
 	return value;
@@ -1956,12 +1874,14 @@ dev_open (struct inode *inode, struct file *fd)
 	return value;
 }
 
-static const struct file_operations dev_init_operations = {
+static const struct file_operations ep0_operations = {
 	.llseek =	no_llseek,
 
 	.open =		dev_open,
+	.read =		ep0_read,
 	.write =	dev_config,
 	.fasync =	ep0_fasync,
+	.poll =		ep0_poll,
 	.unlocked_ioctl = dev_ioctl,
 	.release =	dev_release,
 };
@@ -2077,7 +1997,7 @@ gadgetfs_fill_super (struct super_block *sb, void *opts, int silent)
 		goto Enomem;
 
 	dev->sb = sb;
-	dev->dentry = gadgetfs_create_file(sb, CHIP, dev, &dev_init_operations);
+	dev->dentry = gadgetfs_create_file(sb, CHIP, dev, &ep0_operations);
 	if (!dev->dentry) {
 		put_dev(dev);
 		goto Enomem;
diff --git a/drivers/usb/gadget/legacy/printer.c b/drivers/usb/gadget/legacy/printer.c
index 90545980542f..d5b6ee725a2a 100644
--- a/drivers/usb/gadget/legacy/printer.c
+++ b/drivers/usb/gadget/legacy/printer.c
@@ -12,29 +12,7 @@
 
 #include <linux/module.h>
 #include <linux/kernel.h>
-#include <linux/delay.h>
-#include <linux/ioport.h>
-#include <linux/sched.h>
-#include <linux/slab.h>
-#include <linux/mutex.h>
-#include <linux/errno.h>
-#include <linux/init.h>
-#include <linux/timer.h>
-#include <linux/list.h>
-#include <linux/interrupt.h>
-#include <linux/device.h>
-#include <linux/moduleparam.h>
-#include <linux/fs.h>
-#include <linux/poll.h>
-#include <linux/types.h>
-#include <linux/ctype.h>
-#include <linux/cdev.h>
-
 #include <asm/byteorder.h>
-#include <linux/io.h>
-#include <linux/irq.h>
-#include <linux/uaccess.h>
-#include <asm/unaligned.h>
 
 #include <linux/usb/ch9.h>
 #include <linux/usb/composite.h>
@@ -46,50 +24,12 @@
 USB_GADGET_COMPOSITE_OPTIONS();
 
 #define DRIVER_DESC		"Printer Gadget"
-#define DRIVER_VERSION		"2007 OCT 06"
+#define DRIVER_VERSION		"2015 FEB 17"
 
-static DEFINE_MUTEX(printer_mutex);
 static const char shortname [] = "printer";
 static const char driver_desc [] = DRIVER_DESC;
 
-static dev_t g_printer_devno;
-
-static struct class *usb_gadget_class;
-
-/*-------------------------------------------------------------------------*/
-
-struct printer_dev {
-	spinlock_t		lock;		/* lock this structure */
-	/* lock buffer lists during read/write calls */
-	struct mutex		lock_printer_io;
-	struct usb_gadget	*gadget;
-	s8			interface;
-	struct usb_ep		*in_ep, *out_ep;
-
-	struct list_head	rx_reqs;	/* List of free RX structs */
-	struct list_head	rx_reqs_active;	/* List of Active RX xfers */
-	struct list_head	rx_buffers;	/* List of completed xfers */
-	/* wait until there is data to be read. */
-	wait_queue_head_t	rx_wait;
-	struct list_head	tx_reqs;	/* List of free TX structs */
-	struct list_head	tx_reqs_active; /* List of Active TX xfers */
-	/* Wait until there are write buffers available to use. */
-	wait_queue_head_t	tx_wait;
-	/* Wait until all write buffers have been sent. */
-	wait_queue_head_t	tx_flush_wait;
-	struct usb_request	*current_rx_req;
-	size_t			current_rx_bytes;
-	u8			*current_rx_buf;
-	u8			printer_status;
-	u8			reset_printer;
-	struct cdev		printer_cdev;
-	struct device		*pdev;
-	u8			printer_cdev_open;
-	wait_queue_head_t	wait;
-	struct usb_function	function;
-};
-
-static struct printer_dev usb_printer_gadget;
+#include "u_printer.h"
 
 /*-------------------------------------------------------------------------*/
 
@@ -120,6 +60,9 @@ module_param(qlen, uint, S_IRUGO|S_IWUSR);
 
 #define QLEN	qlen
 
+static struct usb_function_instance *fi_printer;
+static struct usb_function *f_printer;
+
 /*-------------------------------------------------------------------------*/
 
 /*
@@ -127,10 +70,6 @@ module_param(qlen, uint, S_IRUGO|S_IWUSR);
  * descriptors are built on demand.
  */
 
-/* holds our biggest descriptor */
-#define USB_DESC_BUFSIZE		256
-#define USB_BUFSIZE			8192
-
 static struct usb_device_descriptor device_desc = {
 	.bLength =		sizeof device_desc,
 	.bDescriptorType =	USB_DT_DEVICE,
@@ -143,108 +82,6 @@ static struct usb_device_descriptor device_desc = {
 	.bNumConfigurations =	1
 };
 
-static struct usb_interface_descriptor intf_desc = {
-	.bLength =		sizeof intf_desc,
-	.bDescriptorType =	USB_DT_INTERFACE,
-	.bNumEndpoints =	2,
-	.bInterfaceClass =	USB_CLASS_PRINTER,
-	.bInterfaceSubClass =	1,	/* Printer Sub-Class */
-	.bInterfaceProtocol =	2,	/* Bi-Directional */
-	.iInterface =		0
-};
-
-static struct usb_endpoint_descriptor fs_ep_in_desc = {
-	.bLength =		USB_DT_ENDPOINT_SIZE,
-	.bDescriptorType =	USB_DT_ENDPOINT,
-	.bEndpointAddress =	USB_DIR_IN,
-	.bmAttributes =		USB_ENDPOINT_XFER_BULK
-};
-
-static struct usb_endpoint_descriptor fs_ep_out_desc = {
-	.bLength =		USB_DT_ENDPOINT_SIZE,
-	.bDescriptorType =	USB_DT_ENDPOINT,
-	.bEndpointAddress =	USB_DIR_OUT,
-	.bmAttributes =		USB_ENDPOINT_XFER_BULK
-};
-
-static struct usb_descriptor_header *fs_printer_function[] = {
-	(struct usb_descriptor_header *) &intf_desc,
-	(struct usb_descriptor_header *) &fs_ep_in_desc,
-	(struct usb_descriptor_header *) &fs_ep_out_desc,
-	NULL
-};
-
-/*
- * usb 2.0 devices need to expose both high speed and full speed
- * descriptors, unless they only run at full speed.
- */
-
-static struct usb_endpoint_descriptor hs_ep_in_desc = {
-	.bLength =		USB_DT_ENDPOINT_SIZE,
-	.bDescriptorType =	USB_DT_ENDPOINT,
-	.bmAttributes =		USB_ENDPOINT_XFER_BULK,
-	.wMaxPacketSize =	cpu_to_le16(512)
-};
-
-static struct usb_endpoint_descriptor hs_ep_out_desc = {
-	.bLength =		USB_DT_ENDPOINT_SIZE,
-	.bDescriptorType =	USB_DT_ENDPOINT,
-	.bmAttributes =		USB_ENDPOINT_XFER_BULK,
-	.wMaxPacketSize =	cpu_to_le16(512)
-};
-
-static struct usb_qualifier_descriptor dev_qualifier = {
-	.bLength =		sizeof dev_qualifier,
-	.bDescriptorType =	USB_DT_DEVICE_QUALIFIER,
-	.bcdUSB =		cpu_to_le16(0x0200),
-	.bDeviceClass =		USB_CLASS_PRINTER,
-	.bNumConfigurations =	1
-};
-
-static struct usb_descriptor_header *hs_printer_function[] = {
-	(struct usb_descriptor_header *) &intf_desc,
-	(struct usb_descriptor_header *) &hs_ep_in_desc,
-	(struct usb_descriptor_header *) &hs_ep_out_desc,
-	NULL
-};
-
-/*
- * Added endpoint descriptors for 3.0 devices
- */
-
-static struct usb_endpoint_descriptor ss_ep_in_desc = {
-	.bLength =              USB_DT_ENDPOINT_SIZE,
-	.bDescriptorType =      USB_DT_ENDPOINT,
-	.bmAttributes =         USB_ENDPOINT_XFER_BULK,
-	.wMaxPacketSize =       cpu_to_le16(1024),
-};
-
-static struct usb_ss_ep_comp_descriptor ss_ep_in_comp_desc = {
-	.bLength =              sizeof(ss_ep_in_comp_desc),
-	.bDescriptorType =      USB_DT_SS_ENDPOINT_COMP,
-};
-
-static struct usb_endpoint_descriptor ss_ep_out_desc = {
-	.bLength =              USB_DT_ENDPOINT_SIZE,
-	.bDescriptorType =      USB_DT_ENDPOINT,
-	.bmAttributes =         USB_ENDPOINT_XFER_BULK,
-	.wMaxPacketSize =       cpu_to_le16(1024),
-};
-
-static struct usb_ss_ep_comp_descriptor ss_ep_out_comp_desc = {
-	.bLength =              sizeof(ss_ep_out_comp_desc),
-	.bDescriptorType =      USB_DT_SS_ENDPOINT_COMP,
-};
-
-static struct usb_descriptor_header *ss_printer_function[] = {
-	(struct usb_descriptor_header *) &intf_desc,
-	(struct usb_descriptor_header *) &ss_ep_in_desc,
-	(struct usb_descriptor_header *) &ss_ep_in_comp_desc,
-	(struct usb_descriptor_header *) &ss_ep_out_desc,
-	(struct usb_descriptor_header *) &ss_ep_out_comp_desc,
-	NULL
-};
-
 static struct usb_otg_descriptor otg_descriptor = {
 	.bLength =              sizeof otg_descriptor,
 	.bDescriptorType =      USB_DT_OTG,
@@ -256,29 +93,13 @@ static const struct usb_descriptor_header *otg_desc[] = {
 	NULL,
 };
 
-/* maxpacket and other transfer characteristics vary by speed. */
-static inline struct usb_endpoint_descriptor *ep_desc(struct usb_gadget *gadget,
-					struct usb_endpoint_descriptor *fs,
-					struct usb_endpoint_descriptor *hs,
-					struct usb_endpoint_descriptor *ss)
-{
-	switch (gadget->speed) {
-	case USB_SPEED_SUPER:
-		return ss;
-	case USB_SPEED_HIGH:
-		return hs;
-	default:
-		return fs;
-	}
-}
-
 /*-------------------------------------------------------------------------*/
 
 /* descriptors that are built on-demand */
 
 static char				product_desc [40] = DRIVER_DESC;
 static char				serial_num [40] = "1";
-static char				pnp_string [1024] =
+static char				pnp_string[PNP_STRING_LEN] =
 	"XXMFG:linux;MDL:g_printer;CLS:PRINTER;SN:1;";
 
 /* static strings, in UTF-8 */
@@ -299,921 +120,19 @@ static struct usb_gadget_strings *dev_strings[] = {
 	NULL,
 };
 
-/*-------------------------------------------------------------------------*/
-
-static struct usb_request *
-printer_req_alloc(struct usb_ep *ep, unsigned len, gfp_t gfp_flags)
-{
-	struct usb_request	*req;
-
-	req = usb_ep_alloc_request(ep, gfp_flags);
-
-	if (req != NULL) {
-		req->length = len;
-		req->buf = kmalloc(len, gfp_flags);
-		if (req->buf == NULL) {
-			usb_ep_free_request(ep, req);
-			return NULL;
-		}
-	}
-
-	return req;
-}
-
-static void
-printer_req_free(struct usb_ep *ep, struct usb_request *req)
-{
-	if (ep != NULL && req != NULL) {
-		kfree(req->buf);
-		usb_ep_free_request(ep, req);
-	}
-}
-
-/*-------------------------------------------------------------------------*/
-
-static void rx_complete(struct usb_ep *ep, struct usb_request *req)
-{
-	struct printer_dev	*dev = ep->driver_data;
-	int			status = req->status;
-	unsigned long		flags;
-
-	spin_lock_irqsave(&dev->lock, flags);
-
-	list_del_init(&req->list);	/* Remode from Active List */
-
-	switch (status) {
-
-	/* normal completion */
-	case 0:
-		if (req->actual > 0) {
-			list_add_tail(&req->list, &dev->rx_buffers);
-			DBG(dev, "G_Printer : rx length %d\n", req->actual);
-		} else {
-			list_add(&req->list, &dev->rx_reqs);
-		}
-		break;
-
-	/* software-driven interface shutdown */
-	case -ECONNRESET:		/* unlink */
-	case -ESHUTDOWN:		/* disconnect etc */
-		VDBG(dev, "rx shutdown, code %d\n", status);
-		list_add(&req->list, &dev->rx_reqs);
-		break;
-
-	/* for hardware automagic (such as pxa) */
-	case -ECONNABORTED:		/* endpoint reset */
-		DBG(dev, "rx %s reset\n", ep->name);
-		list_add(&req->list, &dev->rx_reqs);
-		break;
-
-	/* data overrun */
-	case -EOVERFLOW:
-		/* FALLTHROUGH */
-
-	default:
-		DBG(dev, "rx status %d\n", status);
-		list_add(&req->list, &dev->rx_reqs);
-		break;
-	}
-
-	wake_up_interruptible(&dev->rx_wait);
-	spin_unlock_irqrestore(&dev->lock, flags);
-}
-
-static void tx_complete(struct usb_ep *ep, struct usb_request *req)
-{
-	struct printer_dev	*dev = ep->driver_data;
-
-	switch (req->status) {
-	default:
-		VDBG(dev, "tx err %d\n", req->status);
-		/* FALLTHROUGH */
-	case -ECONNRESET:		/* unlink */
-	case -ESHUTDOWN:		/* disconnect etc */
-		break;
-	case 0:
-		break;
-	}
-
-	spin_lock(&dev->lock);
-	/* Take the request struct off the active list and put it on the
-	 * free list.
-	 */
-	list_del_init(&req->list);
-	list_add(&req->list, &dev->tx_reqs);
-	wake_up_interruptible(&dev->tx_wait);
-	if (likely(list_empty(&dev->tx_reqs_active)))
-		wake_up_interruptible(&dev->tx_flush_wait);
-
-	spin_unlock(&dev->lock);
-}
-
-/*-------------------------------------------------------------------------*/
-
-static int
-printer_open(struct inode *inode, struct file *fd)
-{
-	struct printer_dev	*dev;
-	unsigned long		flags;
-	int			ret = -EBUSY;
-
-	mutex_lock(&printer_mutex);
-	dev = container_of(inode->i_cdev, struct printer_dev, printer_cdev);
-
-	spin_lock_irqsave(&dev->lock, flags);
-
-	if (!dev->printer_cdev_open) {
-		dev->printer_cdev_open = 1;
-		fd->private_data = dev;
-		ret = 0;
-		/* Change the printer status to show that it's on-line. */
-		dev->printer_status |= PRINTER_SELECTED;
-	}
-
-	spin_unlock_irqrestore(&dev->lock, flags);
-
-	DBG(dev, "printer_open returned %x\n", ret);
-	mutex_unlock(&printer_mutex);
-	return ret;
-}
-
-static int
-printer_close(struct inode *inode, struct file *fd)
-{
-	struct printer_dev	*dev = fd->private_data;
-	unsigned long		flags;
-
-	spin_lock_irqsave(&dev->lock, flags);
-	dev->printer_cdev_open = 0;
-	fd->private_data = NULL;
-	/* Change printer status to show that the printer is off-line. */
-	dev->printer_status &= ~PRINTER_SELECTED;
-	spin_unlock_irqrestore(&dev->lock, flags);
-
-	DBG(dev, "printer_close\n");
-
-	return 0;
-}
-
-/* This function must be called with interrupts turned off. */
-static void
-setup_rx_reqs(struct printer_dev *dev)
-{
-	struct usb_request              *req;
-
-	while (likely(!list_empty(&dev->rx_reqs))) {
-		int error;
-
-		req = container_of(dev->rx_reqs.next,
-				struct usb_request, list);
-		list_del_init(&req->list);
-
-		/* The USB Host sends us whatever amount of data it wants to
-		 * so we always set the length field to the full USB_BUFSIZE.
-		 * If the amount of data is more than the read() caller asked
-		 * for it will be stored in the request buffer until it is
-		 * asked for by read().
-		 */
-		req->length = USB_BUFSIZE;
-		req->complete = rx_complete;
-
-		/* here, we unlock, and only unlock, to avoid deadlock. */
-		spin_unlock(&dev->lock);
-		error = usb_ep_queue(dev->out_ep, req, GFP_ATOMIC);
-		spin_lock(&dev->lock);
-		if (error) {
-			DBG(dev, "rx submit --> %d\n", error);
-			list_add(&req->list, &dev->rx_reqs);
-			break;
-		}
-		/* if the req is empty, then add it into dev->rx_reqs_active. */
-		else if (list_empty(&req->list)) {
-			list_add(&req->list, &dev->rx_reqs_active);
-		}
-	}
-}
-
-static ssize_t
-printer_read(struct file *fd, char __user *buf, size_t len, loff_t *ptr)
-{
-	struct printer_dev		*dev = fd->private_data;
-	unsigned long			flags;
-	size_t				size;
-	size_t				bytes_copied;
-	struct usb_request		*req;
-	/* This is a pointer to the current USB rx request. */
-	struct usb_request		*current_rx_req;
-	/* This is the number of bytes in the current rx buffer. */
-	size_t				current_rx_bytes;
-	/* This is a pointer to the current rx buffer. */
-	u8				*current_rx_buf;
-
-	if (len == 0)
-		return -EINVAL;
-
-	DBG(dev, "printer_read trying to read %d bytes\n", (int)len);
-
-	mutex_lock(&dev->lock_printer_io);
-	spin_lock_irqsave(&dev->lock, flags);
-
-	/* We will use this flag later to check if a printer reset happened
-	 * after we turn interrupts back on.
-	 */
-	dev->reset_printer = 0;
-
-	setup_rx_reqs(dev);
-
-	bytes_copied = 0;
-	current_rx_req = dev->current_rx_req;
-	current_rx_bytes = dev->current_rx_bytes;
-	current_rx_buf = dev->current_rx_buf;
-	dev->current_rx_req = NULL;
-	dev->current_rx_bytes = 0;
-	dev->current_rx_buf = NULL;
-
-	/* Check if there is any data in the read buffers. Please note that
-	 * current_rx_bytes is the number of bytes in the current rx buffer.
-	 * If it is zero then check if there are any other rx_buffers that
-	 * are on the completed list. We are only out of data if all rx
-	 * buffers are empty.
-	 */
-	if ((current_rx_bytes == 0) &&
-			(likely(list_empty(&dev->rx_buffers)))) {
-		/* Turn interrupts back on before sleeping. */
-		spin_unlock_irqrestore(&dev->lock, flags);
-
-		/*
-		 * If no data is available check if this is a NON-Blocking
-		 * call or not.
-		 */
-		if (fd->f_flags & (O_NONBLOCK|O_NDELAY)) {
-			mutex_unlock(&dev->lock_printer_io);
-			return -EAGAIN;
-		}
-
-		/* Sleep until data is available */
-		wait_event_interruptible(dev->rx_wait,
-				(likely(!list_empty(&dev->rx_buffers))));
-		spin_lock_irqsave(&dev->lock, flags);
-	}
-
-	/* We have data to return then copy it to the caller's buffer.*/
-	while ((current_rx_bytes || likely(!list_empty(&dev->rx_buffers)))
-			&& len) {
-		if (current_rx_bytes == 0) {
-			req = container_of(dev->rx_buffers.next,
-					struct usb_request, list);
-			list_del_init(&req->list);
-
-			if (req->actual && req->buf) {
-				current_rx_req = req;
-				current_rx_bytes = req->actual;
-				current_rx_buf = req->buf;
-			} else {
-				list_add(&req->list, &dev->rx_reqs);
-				continue;
-			}
-		}
-
-		/* Don't leave irqs off while doing memory copies */
-		spin_unlock_irqrestore(&dev->lock, flags);
-
-		if (len > current_rx_bytes)
-			size = current_rx_bytes;
-		else
-			size = len;
-
-		size -= copy_to_user(buf, current_rx_buf, size);
-		bytes_copied += size;
-		len -= size;
-		buf += size;
-
-		spin_lock_irqsave(&dev->lock, flags);
-
-		/* We've disconnected or reset so return. */
-		if (dev->reset_printer) {
-			list_add(&current_rx_req->list, &dev->rx_reqs);
-			spin_unlock_irqrestore(&dev->lock, flags);
-			mutex_unlock(&dev->lock_printer_io);
-			return -EAGAIN;
-		}
-
-		/* If we not returning all the data left in this RX request
-		 * buffer then adjust the amount of data left in the buffer.
-		 * Othewise if we are done with this RX request buffer then
-		 * requeue it to get any incoming data from the USB host.
-		 */
-		if (size < current_rx_bytes) {
-			current_rx_bytes -= size;
-			current_rx_buf += size;
-		} else {
-			list_add(&current_rx_req->list, &dev->rx_reqs);
-			current_rx_bytes = 0;
-			current_rx_buf = NULL;
-			current_rx_req = NULL;
-		}
-	}
-
-	dev->current_rx_req = current_rx_req;
-	dev->current_rx_bytes = current_rx_bytes;
-	dev->current_rx_buf = current_rx_buf;
-
-	spin_unlock_irqrestore(&dev->lock, flags);
-	mutex_unlock(&dev->lock_printer_io);
-
-	DBG(dev, "printer_read returned %d bytes\n", (int)bytes_copied);
-
-	if (bytes_copied)
-		return bytes_copied;
-	else
-		return -EAGAIN;
-}
-
-static ssize_t
-printer_write(struct file *fd, const char __user *buf, size_t len, loff_t *ptr)
-{
-	struct printer_dev	*dev = fd->private_data;
-	unsigned long		flags;
-	size_t			size;	/* Amount of data in a TX request. */
-	size_t			bytes_copied = 0;
-	struct usb_request	*req;
-
-	DBG(dev, "printer_write trying to send %d bytes\n", (int)len);
-
-	if (len == 0)
-		return -EINVAL;
-
-	mutex_lock(&dev->lock_printer_io);
-	spin_lock_irqsave(&dev->lock, flags);
-
-	/* Check if a printer reset happens while we have interrupts on */
-	dev->reset_printer = 0;
-
-	/* Check if there is any available write buffers */
-	if (likely(list_empty(&dev->tx_reqs))) {
-		/* Turn interrupts back on before sleeping. */
-		spin_unlock_irqrestore(&dev->lock, flags);
-
-		/*
-		 * If write buffers are available check if this is
-		 * a NON-Blocking call or not.
-		 */
-		if (fd->f_flags & (O_NONBLOCK|O_NDELAY)) {
-			mutex_unlock(&dev->lock_printer_io);
-			return -EAGAIN;
-		}
-
-		/* Sleep until a write buffer is available */
-		wait_event_interruptible(dev->tx_wait,
-				(likely(!list_empty(&dev->tx_reqs))));
-		spin_lock_irqsave(&dev->lock, flags);
-	}
-
-	while (likely(!list_empty(&dev->tx_reqs)) && len) {
-
-		if (len > USB_BUFSIZE)
-			size = USB_BUFSIZE;
-		else
-			size = len;
-
-		req = container_of(dev->tx_reqs.next, struct usb_request,
-				list);
-		list_del_init(&req->list);
-
-		req->complete = tx_complete;
-		req->length = size;
-
-		/* Check if we need to send a zero length packet. */
-		if (len > size)
-			/* They will be more TX requests so no yet. */
-			req->zero = 0;
-		else
-			/* If the data amount is not a multple of the
-			 * maxpacket size then send a zero length packet.
-			 */
-			req->zero = ((len % dev->in_ep->maxpacket) == 0);
-
-		/* Don't leave irqs off while doing memory copies */
-		spin_unlock_irqrestore(&dev->lock, flags);
-
-		if (copy_from_user(req->buf, buf, size)) {
-			list_add(&req->list, &dev->tx_reqs);
-			mutex_unlock(&dev->lock_printer_io);
-			return bytes_copied;
-		}
-
-		bytes_copied += size;
-		len -= size;
-		buf += size;
-
-		spin_lock_irqsave(&dev->lock, flags);
-
-		/* We've disconnected or reset so free the req and buffer */
-		if (dev->reset_printer) {
-			list_add(&req->list, &dev->tx_reqs);
-			spin_unlock_irqrestore(&dev->lock, flags);
-			mutex_unlock(&dev->lock_printer_io);
-			return -EAGAIN;
-		}
-
-		if (usb_ep_queue(dev->in_ep, req, GFP_ATOMIC)) {
-			list_add(&req->list, &dev->tx_reqs);
-			spin_unlock_irqrestore(&dev->lock, flags);
-			mutex_unlock(&dev->lock_printer_io);
-			return -EAGAIN;
-		}
-
-		list_add(&req->list, &dev->tx_reqs_active);
-
-	}
-
-	spin_unlock_irqrestore(&dev->lock, flags);
-	mutex_unlock(&dev->lock_printer_io);
-
-	DBG(dev, "printer_write sent %d bytes\n", (int)bytes_copied);
-
-	if (bytes_copied) {
-		return bytes_copied;
-	} else {
-		return -EAGAIN;
-	}
-}
-
-static int
-printer_fsync(struct file *fd, loff_t start, loff_t end, int datasync)
-{
-	struct printer_dev	*dev = fd->private_data;
-	struct inode *inode = file_inode(fd);
-	unsigned long		flags;
-	int			tx_list_empty;
-
-	mutex_lock(&inode->i_mutex);
-	spin_lock_irqsave(&dev->lock, flags);
-	tx_list_empty = (likely(list_empty(&dev->tx_reqs)));
-	spin_unlock_irqrestore(&dev->lock, flags);
-
-	if (!tx_list_empty) {
-		/* Sleep until all data has been sent */
-		wait_event_interruptible(dev->tx_flush_wait,
-				(likely(list_empty(&dev->tx_reqs_active))));
-	}
-	mutex_unlock(&inode->i_mutex);
-
-	return 0;
-}
-
-static unsigned int
-printer_poll(struct file *fd, poll_table *wait)
-{
-	struct printer_dev	*dev = fd->private_data;
-	unsigned long		flags;
-	int			status = 0;
-
-	mutex_lock(&dev->lock_printer_io);
-	spin_lock_irqsave(&dev->lock, flags);
-	setup_rx_reqs(dev);
-	spin_unlock_irqrestore(&dev->lock, flags);
-	mutex_unlock(&dev->lock_printer_io);
-
-	poll_wait(fd, &dev->rx_wait, wait);
-	poll_wait(fd, &dev->tx_wait, wait);
-
-	spin_lock_irqsave(&dev->lock, flags);
-	if (likely(!list_empty(&dev->tx_reqs)))
-		status |= POLLOUT | POLLWRNORM;
-
-	if (likely(dev->current_rx_bytes) ||
-			likely(!list_empty(&dev->rx_buffers)))
-		status |= POLLIN | POLLRDNORM;
-
-	spin_unlock_irqrestore(&dev->lock, flags);
-
-	return status;
-}
-
-static long
-printer_ioctl(struct file *fd, unsigned int code, unsigned long arg)
-{
-	struct printer_dev	*dev = fd->private_data;
-	unsigned long		flags;
-	int			status = 0;
-
-	DBG(dev, "printer_ioctl: cmd=0x%4.4x, arg=%lu\n", code, arg);
-
-	/* handle ioctls */
-
-	spin_lock_irqsave(&dev->lock, flags);
-
-	switch (code) {
-	case GADGET_GET_PRINTER_STATUS:
-		status = (int)dev->printer_status;
-		break;
-	case GADGET_SET_PRINTER_STATUS:
-		dev->printer_status = (u8)arg;
-		break;
-	default:
-		/* could not handle ioctl */
-		DBG(dev, "printer_ioctl: ERROR cmd=0x%4.4xis not supported\n",
-				code);
-		status = -ENOTTY;
-	}
-
-	spin_unlock_irqrestore(&dev->lock, flags);
-
-	return status;
-}
-
-/* used after endpoint configuration */
-static const struct file_operations printer_io_operations = {
-	.owner =	THIS_MODULE,
-	.open =		printer_open,
-	.read =		printer_read,
-	.write =	printer_write,
-	.fsync =	printer_fsync,
-	.poll =		printer_poll,
-	.unlocked_ioctl = printer_ioctl,
-	.release =	printer_close,
-	.llseek =	noop_llseek,
-};
-
-/*-------------------------------------------------------------------------*/
-
-static int
-set_printer_interface(struct printer_dev *dev)
-{
-	int			result = 0;
-
-	dev->in_ep->desc = ep_desc(dev->gadget, &fs_ep_in_desc, &hs_ep_in_desc,
-				&ss_ep_in_desc);
-	dev->in_ep->driver_data = dev;
-
-	dev->out_ep->desc = ep_desc(dev->gadget, &fs_ep_out_desc,
-				    &hs_ep_out_desc, &ss_ep_out_desc);
-	dev->out_ep->driver_data = dev;
-
-	result = usb_ep_enable(dev->in_ep);
-	if (result != 0) {
-		DBG(dev, "enable %s --> %d\n", dev->in_ep->name, result);
-		goto done;
-	}
-
-	result = usb_ep_enable(dev->out_ep);
-	if (result != 0) {
-		DBG(dev, "enable %s --> %d\n", dev->in_ep->name, result);
-		goto done;
-	}
-
-done:
-	/* on error, disable any endpoints  */
-	if (result != 0) {
-		(void) usb_ep_disable(dev->in_ep);
-		(void) usb_ep_disable(dev->out_ep);
-		dev->in_ep->desc = NULL;
-		dev->out_ep->desc = NULL;
-	}
-
-	/* caller is responsible for cleanup on error */
-	return result;
-}
-
-static void printer_reset_interface(struct printer_dev *dev)
-{
-	if (dev->interface < 0)
-		return;
-
-	DBG(dev, "%s\n", __func__);
-
-	if (dev->in_ep->desc)
-		usb_ep_disable(dev->in_ep);
-
-	if (dev->out_ep->desc)
-		usb_ep_disable(dev->out_ep);
-
-	dev->in_ep->desc = NULL;
-	dev->out_ep->desc = NULL;
-	dev->interface = -1;
-}
-
-/* Change our operational Interface. */
-static int set_interface(struct printer_dev *dev, unsigned number)
-{
-	int			result = 0;
-
-	/* Free the current interface */
-	printer_reset_interface(dev);
-
-	result = set_printer_interface(dev);
-	if (result)
-		printer_reset_interface(dev);
-	else
-		dev->interface = number;
-
-	if (!result)
-		INFO(dev, "Using interface %x\n", number);
-
-	return result;
-}
-
-static void printer_soft_reset(struct printer_dev *dev)
-{
-	struct usb_request	*req;
-
-	INFO(dev, "Received Printer Reset Request\n");
-
-	if (usb_ep_disable(dev->in_ep))
-		DBG(dev, "Failed to disable USB in_ep\n");
-	if (usb_ep_disable(dev->out_ep))
-		DBG(dev, "Failed to disable USB out_ep\n");
-
-	if (dev->current_rx_req != NULL) {
-		list_add(&dev->current_rx_req->list, &dev->rx_reqs);
-		dev->current_rx_req = NULL;
-	}
-	dev->current_rx_bytes = 0;
-	dev->current_rx_buf = NULL;
-	dev->reset_printer = 1;
-
-	while (likely(!(list_empty(&dev->rx_buffers)))) {
-		req = container_of(dev->rx_buffers.next, struct usb_request,
-				list);
-		list_del_init(&req->list);
-		list_add(&req->list, &dev->rx_reqs);
-	}
-
-	while (likely(!(list_empty(&dev->rx_reqs_active)))) {
-		req = container_of(dev->rx_buffers.next, struct usb_request,
-				list);
-		list_del_init(&req->list);
-		list_add(&req->list, &dev->rx_reqs);
-	}
-
-	while (likely(!(list_empty(&dev->tx_reqs_active)))) {
-		req = container_of(dev->tx_reqs_active.next,
-				struct usb_request, list);
-		list_del_init(&req->list);
-		list_add(&req->list, &dev->tx_reqs);
-	}
-
-	if (usb_ep_enable(dev->in_ep))
-		DBG(dev, "Failed to enable USB in_ep\n");
-	if (usb_ep_enable(dev->out_ep))
-		DBG(dev, "Failed to enable USB out_ep\n");
-
-	wake_up_interruptible(&dev->rx_wait);
-	wake_up_interruptible(&dev->tx_wait);
-	wake_up_interruptible(&dev->tx_flush_wait);
-}
-
-/*-------------------------------------------------------------------------*/
-
-/*
- * The setup() callback implements all the ep0 functionality that's not
- * handled lower down.
- */
-static int printer_func_setup(struct usb_function *f,
-		const struct usb_ctrlrequest *ctrl)
-{
-	struct printer_dev *dev = container_of(f, struct printer_dev, function);
-	struct usb_composite_dev *cdev = f->config->cdev;
-	struct usb_request	*req = cdev->req;
-	int			value = -EOPNOTSUPP;
-	u16			wIndex = le16_to_cpu(ctrl->wIndex);
-	u16			wValue = le16_to_cpu(ctrl->wValue);
-	u16			wLength = le16_to_cpu(ctrl->wLength);
-
-	DBG(dev, "ctrl req%02x.%02x v%04x i%04x l%d\n",
-		ctrl->bRequestType, ctrl->bRequest, wValue, wIndex, wLength);
-
-	switch (ctrl->bRequestType&USB_TYPE_MASK) {
-	case USB_TYPE_CLASS:
-		switch (ctrl->bRequest) {
-		case 0: /* Get the IEEE-1284 PNP String */
-			/* Only one printer interface is supported. */
-			if ((wIndex>>8) != dev->interface)
-				break;
-
-			value = (pnp_string[0]<<8)|pnp_string[1];
-			memcpy(req->buf, pnp_string, value);
-			DBG(dev, "1284 PNP String: %x %s\n", value,
-					&pnp_string[2]);
-			break;
-
-		case 1: /* Get Port Status */
-			/* Only one printer interface is supported. */
-			if (wIndex != dev->interface)
-				break;
-
-			*(u8 *)req->buf = dev->printer_status;
-			value = min(wLength, (u16) 1);
-			break;
-
-		case 2: /* Soft Reset */
-			/* Only one printer interface is supported. */
-			if (wIndex != dev->interface)
-				break;
-
-			printer_soft_reset(dev);
-
-			value = 0;
-			break;
-
-		default:
-			goto unknown;
-		}
-		break;
-
-	default:
-unknown:
-		VDBG(dev,
-			"unknown ctrl req%02x.%02x v%04x i%04x l%d\n",
-			ctrl->bRequestType, ctrl->bRequest,
-			wValue, wIndex, wLength);
-		break;
-	}
-	/* host either stalls (value < 0) or reports success */
-	return value;
-}
-
-static int __init printer_func_bind(struct usb_configuration *c,
-		struct usb_function *f)
-{
-	struct printer_dev *dev = container_of(f, struct printer_dev, function);
-	struct usb_composite_dev *cdev = c->cdev;
-	struct usb_ep *in_ep;
-	struct usb_ep *out_ep = NULL;
-	int id;
-	int ret;
-
-	id = usb_interface_id(c, f);
-	if (id < 0)
-		return id;
-	intf_desc.bInterfaceNumber = id;
-
-	/* all we really need is bulk IN/OUT */
-	in_ep = usb_ep_autoconfig(cdev->gadget, &fs_ep_in_desc);
-	if (!in_ep) {
-autoconf_fail:
-		dev_err(&cdev->gadget->dev, "can't autoconfigure on %s\n",
-			cdev->gadget->name);
-		return -ENODEV;
-	}
-	in_ep->driver_data = in_ep;	/* claim */
-
-	out_ep = usb_ep_autoconfig(cdev->gadget, &fs_ep_out_desc);
-	if (!out_ep)
-		goto autoconf_fail;
-	out_ep->driver_data = out_ep;	/* claim */
-
-	/* assumes that all endpoints are dual-speed */
-	hs_ep_in_desc.bEndpointAddress = fs_ep_in_desc.bEndpointAddress;
-	hs_ep_out_desc.bEndpointAddress = fs_ep_out_desc.bEndpointAddress;
-	ss_ep_in_desc.bEndpointAddress = fs_ep_in_desc.bEndpointAddress;
-	ss_ep_out_desc.bEndpointAddress = fs_ep_out_desc.bEndpointAddress;
-
-	ret = usb_assign_descriptors(f, fs_printer_function,
-			hs_printer_function, ss_printer_function);
-	if (ret)
-		return ret;
-
-	dev->in_ep = in_ep;
-	dev->out_ep = out_ep;
-	return 0;
-}
-
-static void printer_func_unbind(struct usb_configuration *c,
-		struct usb_function *f)
-{
-	usb_free_all_descriptors(f);
-}
-
-static int printer_func_set_alt(struct usb_function *f,
-		unsigned intf, unsigned alt)
-{
-	struct printer_dev *dev = container_of(f, struct printer_dev, function);
-	int ret = -ENOTSUPP;
-
-	if (!alt)
-		ret = set_interface(dev, intf);
-
-	return ret;
-}
-
-static void printer_func_disable(struct usb_function *f)
-{
-	struct printer_dev *dev = container_of(f, struct printer_dev, function);
-	unsigned long		flags;
-
-	DBG(dev, "%s\n", __func__);
-
-	spin_lock_irqsave(&dev->lock, flags);
-	printer_reset_interface(dev);
-	spin_unlock_irqrestore(&dev->lock, flags);
-}
-
-static void printer_cfg_unbind(struct usb_configuration *c)
-{
-	struct printer_dev	*dev;
-	struct usb_request	*req;
-
-	dev = &usb_printer_gadget;
-
-	DBG(dev, "%s\n", __func__);
-
-	/* Remove sysfs files */
-	device_destroy(usb_gadget_class, g_printer_devno);
-
-	/* Remove Character Device */
-	cdev_del(&dev->printer_cdev);
-
-	/* we must already have been disconnected ... no i/o may be active */
-	WARN_ON(!list_empty(&dev->tx_reqs_active));
-	WARN_ON(!list_empty(&dev->rx_reqs_active));
-
-	/* Free all memory for this driver. */
-	while (!list_empty(&dev->tx_reqs)) {
-		req = container_of(dev->tx_reqs.next, struct usb_request,
-				list);
-		list_del(&req->list);
-		printer_req_free(dev->in_ep, req);
-	}
-
-	if (dev->current_rx_req != NULL)
-		printer_req_free(dev->out_ep, dev->current_rx_req);
-
-	while (!list_empty(&dev->rx_reqs)) {
-		req = container_of(dev->rx_reqs.next,
-				struct usb_request, list);
-		list_del(&req->list);
-		printer_req_free(dev->out_ep, req);
-	}
-
-	while (!list_empty(&dev->rx_buffers)) {
-		req = container_of(dev->rx_buffers.next,
-				struct usb_request, list);
-		list_del(&req->list);
-		printer_req_free(dev->out_ep, req);
-	}
-}
-
 static struct usb_configuration printer_cfg_driver = {
 	.label			= "printer",
-	.unbind			= printer_cfg_unbind,
 	.bConfigurationValue	= 1,
 	.bmAttributes		= USB_CONFIG_ATT_ONE | USB_CONFIG_ATT_SELFPOWER,
 };
 
-static int __init printer_bind_config(struct usb_configuration *c)
+static int __init printer_do_config(struct usb_configuration *c)
 {
 	struct usb_gadget	*gadget = c->cdev->gadget;
-	struct printer_dev	*dev;
-	int			status = -ENOMEM;
-	size_t			len;
-	u32			i;
-	struct usb_request	*req;
+	int			status = 0;
 
 	usb_ep_autoconfig_reset(gadget);
 
-	dev = &usb_printer_gadget;
-
-	dev->function.name = shortname;
-	dev->function.bind = printer_func_bind;
-	dev->function.setup = printer_func_setup;
-	dev->function.unbind = printer_func_unbind;
-	dev->function.set_alt = printer_func_set_alt;
-	dev->function.disable = printer_func_disable;
-
-	status = usb_add_function(c, &dev->function);
-	if (status)
-		return status;
-
-	/* Setup the sysfs files for the printer gadget. */
-	dev->pdev = device_create(usb_gadget_class, NULL, g_printer_devno,
-				  NULL, "g_printer");
-	if (IS_ERR(dev->pdev)) {
-		ERROR(dev, "Failed to create device: g_printer\n");
-		status = PTR_ERR(dev->pdev);
-		goto fail;
-	}
-
-	/*
-	 * Register a character device as an interface to a user mode
-	 * program that handles the printer specific functionality.
-	 */
-	cdev_init(&dev->printer_cdev, &printer_io_operations);
-	dev->printer_cdev.owner = THIS_MODULE;
-	status = cdev_add(&dev->printer_cdev, g_printer_devno, 1);
-	if (status) {
-		ERROR(dev, "Failed to open char device\n");
-		goto fail;
-	}
-
-	if (iPNPstring)
-		strlcpy(&pnp_string[2], iPNPstring, (sizeof pnp_string)-2);
-
-	len = strlen(pnp_string);
-	pnp_string[0] = (len >> 8) & 0xFF;
-	pnp_string[1] = len & 0xFF;
-
 	usb_gadget_set_selfpowered(gadget);
 
 	if (gadget_is_otg(gadget)) {
@@ -1222,86 +141,64 @@ static int __init printer_bind_config(struct usb_configuration *c)
 		printer_cfg_driver.bmAttributes |= USB_CONFIG_ATT_WAKEUP;
 	}
 
-	spin_lock_init(&dev->lock);
-	mutex_init(&dev->lock_printer_io);
-	INIT_LIST_HEAD(&dev->tx_reqs);
-	INIT_LIST_HEAD(&dev->tx_reqs_active);
-	INIT_LIST_HEAD(&dev->rx_reqs);
-	INIT_LIST_HEAD(&dev->rx_reqs_active);
-	INIT_LIST_HEAD(&dev->rx_buffers);
-	init_waitqueue_head(&dev->rx_wait);
-	init_waitqueue_head(&dev->tx_wait);
-	init_waitqueue_head(&dev->tx_flush_wait);
-
-	dev->interface = -1;
-	dev->printer_cdev_open = 0;
-	dev->printer_status = PRINTER_NOT_ERROR;
-	dev->current_rx_req = NULL;
-	dev->current_rx_bytes = 0;
-	dev->current_rx_buf = NULL;
-
-	for (i = 0; i < QLEN; i++) {
-		req = printer_req_alloc(dev->in_ep, USB_BUFSIZE, GFP_KERNEL);
-		if (!req) {
-			while (!list_empty(&dev->tx_reqs)) {
-				req = container_of(dev->tx_reqs.next,
-						struct usb_request, list);
-				list_del(&req->list);
-				printer_req_free(dev->in_ep, req);
-			}
-			return -ENOMEM;
-		}
-		list_add(&req->list, &dev->tx_reqs);
-	}
-
-	for (i = 0; i < QLEN; i++) {
-		req = printer_req_alloc(dev->out_ep, USB_BUFSIZE, GFP_KERNEL);
-		if (!req) {
-			while (!list_empty(&dev->rx_reqs)) {
-				req = container_of(dev->rx_reqs.next,
-						struct usb_request, list);
-				list_del(&req->list);
-				printer_req_free(dev->out_ep, req);
-			}
-			return -ENOMEM;
-		}
-		list_add(&req->list, &dev->rx_reqs);
-	}
-
-	/* finish hookup to lower layer ... */
-	dev->gadget = gadget;
+	f_printer = usb_get_function(fi_printer);
+	if (IS_ERR(f_printer))
+		return PTR_ERR(f_printer);
 
-	INFO(dev, "%s, version: " DRIVER_VERSION "\n", driver_desc);
-	return 0;
+	status = usb_add_function(c, f_printer);
+	if (status < 0)
+		usb_put_function(f_printer);
 
-fail:
-	printer_cfg_unbind(c);
 	return status;
 }
 
-static int printer_unbind(struct usb_composite_dev *cdev)
-{
-	return 0;
-}
-
 static int __init printer_bind(struct usb_composite_dev *cdev)
 {
-	int ret;
+	struct f_printer_opts *opts;
+	int ret, len;
+
+	fi_printer = usb_get_function_instance("printer");
+	if (IS_ERR(fi_printer))
+		return PTR_ERR(fi_printer);
+
+	if (iPNPstring)
+		strlcpy(&pnp_string[2], iPNPstring, PNP_STRING_LEN - 2);
+
+	len = strlen(pnp_string);
+	pnp_string[0] = (len >> 8) & 0xFF;
+	pnp_string[1] = len & 0xFF;
+
+	opts = container_of(fi_printer, struct f_printer_opts, func_inst);
+	opts->minor = 0;
+	memcpy(opts->pnp_string, pnp_string, PNP_STRING_LEN);
+	opts->q_len = QLEN;
 
 	ret = usb_string_ids_tab(cdev, strings);
-	if (ret < 0)
+	if (ret < 0) {
+		usb_put_function_instance(fi_printer);
 		return ret;
+	}
 	device_desc.iManufacturer = strings[USB_GADGET_MANUFACTURER_IDX].id;
 	device_desc.iProduct = strings[USB_GADGET_PRODUCT_IDX].id;
 	device_desc.iSerialNumber = strings[USB_GADGET_SERIAL_IDX].id;
 
-	ret = usb_add_config(cdev, &printer_cfg_driver, printer_bind_config);
-	if (ret)
+	ret = usb_add_config(cdev, &printer_cfg_driver, printer_do_config);
+	if (ret) {
+		usb_put_function_instance(fi_printer);
 		return ret;
+	}
 	usb_composite_overwrite_options(cdev, &coverwrite);
 	return ret;
 }
 
+static int __exit printer_unbind(struct usb_composite_dev *cdev)
+{
+	usb_put_function(f_printer);
+	usb_put_function_instance(fi_printer);
+
+	return 0;
+}
+
 static __refdata struct usb_composite_driver printer_driver = {
 	.name           = shortname,
 	.dev            = &device_desc,
@@ -1311,47 +208,7 @@ static __refdata struct usb_composite_driver printer_driver = {
 	.unbind		= printer_unbind,
 };
 
-static int __init
-init(void)
-{
-	int status;
-
-	usb_gadget_class = class_create(THIS_MODULE, "usb_printer_gadget");
-	if (IS_ERR(usb_gadget_class)) {
-		status = PTR_ERR(usb_gadget_class);
-		pr_err("unable to create usb_gadget class %d\n", status);
-		return status;
-	}
-
-	status = alloc_chrdev_region(&g_printer_devno, 0, 1,
-			"USB printer gadget");
-	if (status) {
-		pr_err("alloc_chrdev_region %d\n", status);
-		class_destroy(usb_gadget_class);
-		return status;
-	}
-
-	status = usb_composite_probe(&printer_driver);
-	if (status) {
-		class_destroy(usb_gadget_class);
-		unregister_chrdev_region(g_printer_devno, 1);
-		pr_err("usb_gadget_probe_driver %x\n", status);
-	}
-
-	return status;
-}
-module_init(init);
-
-static void __exit
-cleanup(void)
-{
-	mutex_lock(&usb_printer_gadget.lock_printer_io);
-	usb_composite_unregister(&printer_driver);
-	unregister_chrdev_region(g_printer_devno, 1);
-	class_destroy(usb_gadget_class);
-	mutex_unlock(&usb_printer_gadget.lock_printer_io);
-}
-module_exit(cleanup);
+module_usb_composite_driver(printer_driver);
 
 MODULE_DESCRIPTION(DRIVER_DESC);
 MODULE_AUTHOR("Craig Nadler");
diff --git a/drivers/usb/gadget/legacy/tcm_usb_gadget.c b/drivers/usb/gadget/legacy/tcm_usb_gadget.c
index 3a494168661e..6e0a019aad54 100644
--- a/drivers/usb/gadget/legacy/tcm_usb_gadget.c
+++ b/drivers/usb/gadget/legacy/tcm_usb_gadget.c
@@ -1740,10 +1740,9 @@ static int tcm_usbg_make_nexus(struct usbg_tpg *tpg, char *name)
 		goto err_session;
 	}
 	/*
-	 * Now register the TCM vHost virtual I_T Nexus as active with the
-	 * call to __transport_register_session()
+	 * Now register the TCM vHost virtual I_T Nexus as active.
 	 */
-	__transport_register_session(se_tpg, tv_nexus->tvn_se_sess->se_node_acl,
+	transport_register_session(se_tpg, tv_nexus->tvn_se_sess->se_node_acl,
 			tv_nexus->tvn_se_sess, tv_nexus);
 	tpg->tpg_nexus = tv_nexus;
 	mutex_unlock(&tpg->tpg_mutex);
diff --git a/drivers/usb/gadget/legacy/zero.c b/drivers/usb/gadget/legacy/zero.c
index ff97ac93ac03..5ee95152493c 100644
--- a/drivers/usb/gadget/legacy/zero.c
+++ b/drivers/usb/gadget/legacy/zero.c
@@ -68,8 +68,6 @@ static struct usb_zero_options gzero_options = {
 	.isoc_maxpacket = GZERO_ISOC_MAXPACKET,
 	.bulk_buflen = GZERO_BULK_BUFLEN,
 	.qlen = GZERO_QLEN,
-	.int_interval = GZERO_INT_INTERVAL,
-	.int_maxpacket = GZERO_INT_MAXPACKET,
 };
 
 /*-------------------------------------------------------------------------*/
@@ -268,21 +266,6 @@ module_param_named(isoc_maxburst, gzero_options.isoc_maxburst, uint,
 		S_IRUGO|S_IWUSR);
 MODULE_PARM_DESC(isoc_maxburst, "0 - 15 (ss only)");
 
-module_param_named(int_interval, gzero_options.int_interval, uint,
-		S_IRUGO|S_IWUSR);
-MODULE_PARM_DESC(int_interval, "1 - 16");
-
-module_param_named(int_maxpacket, gzero_options.int_maxpacket, uint,
-		S_IRUGO|S_IWUSR);
-MODULE_PARM_DESC(int_maxpacket, "0 - 1023 (fs), 0 - 1024 (hs/ss)");
-
-module_param_named(int_mult, gzero_options.int_mult, uint, S_IRUGO|S_IWUSR);
-MODULE_PARM_DESC(int_mult, "0 - 2 (hs/ss only)");
-
-module_param_named(int_maxburst, gzero_options.int_maxburst, uint,
-		S_IRUGO|S_IWUSR);
-MODULE_PARM_DESC(int_maxburst, "0 - 15 (ss only)");
-
 static struct usb_function *func_lb;
 static struct usb_function_instance *func_inst_lb;
 
@@ -318,10 +301,6 @@ static int __init zero_bind(struct usb_composite_dev *cdev)
 	ss_opts->isoc_maxpacket = gzero_options.isoc_maxpacket;
 	ss_opts->isoc_mult = gzero_options.isoc_mult;
 	ss_opts->isoc_maxburst = gzero_options.isoc_maxburst;
-	ss_opts->int_interval = gzero_options.int_interval;
-	ss_opts->int_maxpacket = gzero_options.int_maxpacket;
-	ss_opts->int_mult = gzero_options.int_mult;
-	ss_opts->int_maxburst = gzero_options.int_maxburst;
 	ss_opts->bulk_buflen = gzero_options.bulk_buflen;
 
 	func_ss = usb_get_function(func_inst_ss);
diff --git a/drivers/usb/gadget/udc/atmel_usba_udc.c b/drivers/usb/gadget/udc/atmel_usba_udc.c
index d79cb35dbf8a..4c01953a0869 100644
--- a/drivers/usb/gadget/udc/atmel_usba_udc.c
+++ b/drivers/usb/gadget/udc/atmel_usba_udc.c
@@ -152,7 +152,7 @@ static int regs_dbg_open(struct inode *inode, struct file *file)
 
 	spin_lock_irq(&udc->lock);
 	for (i = 0; i < inode->i_size / 4; i++)
-		data[i] = __raw_readl(udc->regs + i * 4);
+		data[i] = usba_io_readl(udc->regs + i * 4);
 	spin_unlock_irq(&udc->lock);
 
 	file->private_data = data;
@@ -1249,7 +1249,7 @@ static int handle_ep0_setup(struct usba_udc *udc, struct usba_ep *ep,
 		if (crq->wLength != cpu_to_le16(sizeof(status)))
 			goto stall;
 		ep->state = DATA_STAGE_IN;
-		__raw_writew(status, ep->fifo);
+		usba_io_writew(status, ep->fifo);
 		usba_ep_writel(ep, SET_STA, USBA_TX_PK_RDY);
 		break;
 	}
@@ -1739,7 +1739,72 @@ static irqreturn_t usba_udc_irq(int irq, void *devid)
 	return IRQ_HANDLED;
 }
 
-static irqreturn_t usba_vbus_irq(int irq, void *devid)
+static int start_clock(struct usba_udc *udc)
+{
+	int ret;
+
+	if (udc->clocked)
+		return 0;
+
+	ret = clk_prepare_enable(udc->pclk);
+	if (ret)
+		return ret;
+	ret = clk_prepare_enable(udc->hclk);
+	if (ret) {
+		clk_disable_unprepare(udc->pclk);
+		return ret;
+	}
+
+	udc->clocked = true;
+	return 0;
+}
+
+static void stop_clock(struct usba_udc *udc)
+{
+	if (!udc->clocked)
+		return;
+
+	clk_disable_unprepare(udc->hclk);
+	clk_disable_unprepare(udc->pclk);
+
+	udc->clocked = false;
+}
+
+static int usba_start(struct usba_udc *udc)
+{
+	unsigned long flags;
+	int ret;
+
+	ret = start_clock(udc);
+	if (ret)
+		return ret;
+
+	spin_lock_irqsave(&udc->lock, flags);
+	toggle_bias(udc, 1);
+	usba_writel(udc, CTRL, USBA_ENABLE_MASK);
+	usba_int_enb_set(udc, USBA_END_OF_RESET);
+	spin_unlock_irqrestore(&udc->lock, flags);
+
+	return 0;
+}
+
+static void usba_stop(struct usba_udc *udc)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&udc->lock, flags);
+	udc->gadget.speed = USB_SPEED_UNKNOWN;
+	reset_all_endpoints(udc);
+
+	/* This will also disable the DP pullup */
+	toggle_bias(udc, 0);
+	usba_writel(udc, CTRL, USBA_DISABLE_MASK);
+	spin_unlock_irqrestore(&udc->lock, flags);
+
+	stop_clock(udc);
+}
+
+static irqreturn_t usba_vbus_irq_thread(int irq, void *devid)
 {
 	struct usba_udc *udc = devid;
 	int vbus;
@@ -1747,35 +1812,22 @@ static irqreturn_t usba_vbus_irq(int irq, void *devid)
 	/* debounce */
 	udelay(10);
 
-	spin_lock(&udc->lock);
-
-	/* May happen if Vbus pin toggles during probe() */
-	if (!udc->driver)
-		goto out;
+	mutex_lock(&udc->vbus_mutex);
 
 	vbus = vbus_is_present(udc);
 	if (vbus != udc->vbus_prev) {
 		if (vbus) {
-			toggle_bias(udc, 1);
-			usba_writel(udc, CTRL, USBA_ENABLE_MASK);
-			usba_int_enb_set(udc, USBA_END_OF_RESET);
+			usba_start(udc);
 		} else {
-			udc->gadget.speed = USB_SPEED_UNKNOWN;
-			reset_all_endpoints(udc);
-			toggle_bias(udc, 0);
-			usba_writel(udc, CTRL, USBA_DISABLE_MASK);
-			if (udc->driver->disconnect) {
-				spin_unlock(&udc->lock);
+			usba_stop(udc);
+
+			if (udc->driver->disconnect)
 				udc->driver->disconnect(&udc->gadget);
-				spin_lock(&udc->lock);
-			}
 		}
 		udc->vbus_prev = vbus;
 	}
 
-out:
-	spin_unlock(&udc->lock);
-
+	mutex_unlock(&udc->vbus_mutex);
 	return IRQ_HANDLED;
 }
 
@@ -1787,55 +1839,47 @@ static int atmel_usba_start(struct usb_gadget *gadget,
 	unsigned long flags;
 
 	spin_lock_irqsave(&udc->lock, flags);
-
 	udc->devstatus = 1 << USB_DEVICE_SELF_POWERED;
 	udc->driver = driver;
 	spin_unlock_irqrestore(&udc->lock, flags);
 
-	ret = clk_prepare_enable(udc->pclk);
-	if (ret)
-		return ret;
-	ret = clk_prepare_enable(udc->hclk);
-	if (ret) {
-		clk_disable_unprepare(udc->pclk);
-		return ret;
-	}
+	mutex_lock(&udc->vbus_mutex);
 
-	udc->vbus_prev = 0;
 	if (gpio_is_valid(udc->vbus_pin))
 		enable_irq(gpio_to_irq(udc->vbus_pin));
 
 	/* If Vbus is present, enable the controller and wait for reset */
-	spin_lock_irqsave(&udc->lock, flags);
-	if (vbus_is_present(udc) && udc->vbus_prev == 0) {
-		toggle_bias(udc, 1);
-		usba_writel(udc, CTRL, USBA_ENABLE_MASK);
-		usba_int_enb_set(udc, USBA_END_OF_RESET);
+	udc->vbus_prev = vbus_is_present(udc);
+	if (udc->vbus_prev) {
+		ret = usba_start(udc);
+		if (ret)
+			goto err;
 	}
-	spin_unlock_irqrestore(&udc->lock, flags);
 
+	mutex_unlock(&udc->vbus_mutex);
 	return 0;
+
+err:
+	if (gpio_is_valid(udc->vbus_pin))
+		disable_irq(gpio_to_irq(udc->vbus_pin));
+
+	mutex_unlock(&udc->vbus_mutex);
+
+	spin_lock_irqsave(&udc->lock, flags);
+	udc->devstatus &= ~(1 << USB_DEVICE_SELF_POWERED);
+	udc->driver = NULL;
+	spin_unlock_irqrestore(&udc->lock, flags);
+	return ret;
 }
 
 static int atmel_usba_stop(struct usb_gadget *gadget)
 {
 	struct usba_udc *udc = container_of(gadget, struct usba_udc, gadget);
-	unsigned long flags;
 
 	if (gpio_is_valid(udc->vbus_pin))
 		disable_irq(gpio_to_irq(udc->vbus_pin));
 
-	spin_lock_irqsave(&udc->lock, flags);
-	udc->gadget.speed = USB_SPEED_UNKNOWN;
-	reset_all_endpoints(udc);
-	spin_unlock_irqrestore(&udc->lock, flags);
-
-	/* This will also disable the DP pullup */
-	toggle_bias(udc, 0);
-	usba_writel(udc, CTRL, USBA_DISABLE_MASK);
-
-	clk_disable_unprepare(udc->hclk);
-	clk_disable_unprepare(udc->pclk);
+	usba_stop(udc);
 
 	udc->driver = NULL;
 
@@ -2057,6 +2101,7 @@ static int usba_udc_probe(struct platform_device *pdev)
 		return PTR_ERR(hclk);
 
 	spin_lock_init(&udc->lock);
+	mutex_init(&udc->vbus_mutex);
 	udc->pdev = pdev;
 	udc->pclk = pclk;
 	udc->hclk = hclk;
@@ -2111,17 +2156,17 @@ static int usba_udc_probe(struct platform_device *pdev)
 
 	if (gpio_is_valid(udc->vbus_pin)) {
 		if (!devm_gpio_request(&pdev->dev, udc->vbus_pin, "atmel_usba_udc")) {
-			ret = devm_request_irq(&pdev->dev,
-					gpio_to_irq(udc->vbus_pin),
-					usba_vbus_irq, 0,
+			irq_set_status_flags(gpio_to_irq(udc->vbus_pin),
+					IRQ_NOAUTOEN);
+			ret = devm_request_threaded_irq(&pdev->dev,
+					gpio_to_irq(udc->vbus_pin), NULL,
+					usba_vbus_irq_thread, IRQF_ONESHOT,
 					"atmel_usba_udc", udc);
 			if (ret) {
 				udc->vbus_pin = -ENODEV;
 				dev_warn(&udc->pdev->dev,
 					 "failed to request vbus irq; "
 					 "assuming always on\n");
-			} else {
-				disable_irq(gpio_to_irq(udc->vbus_pin));
 			}
 		} else {
 			/* gpio_request fail so use -EINVAL for gpio_is_valid */
@@ -2132,6 +2177,7 @@ static int usba_udc_probe(struct platform_device *pdev)
 	ret = usb_add_gadget_udc(&pdev->dev, &udc->gadget);
 	if (ret)
 		return ret;
+	device_init_wakeup(&pdev->dev, 1);
 
 	usba_init_debugfs(udc);
 	for (i = 1; i < udc->num_ep; i++)
@@ -2147,6 +2193,7 @@ static int __exit usba_udc_remove(struct platform_device *pdev)
 
 	udc = platform_get_drvdata(pdev);
 
+	device_init_wakeup(&pdev->dev, 0);
 	usb_del_gadget_udc(&udc->gadget);
 
 	for (i = 1; i < udc->num_ep; i++)
@@ -2156,10 +2203,65 @@ static int __exit usba_udc_remove(struct platform_device *pdev)
 	return 0;
 }
 
+#ifdef CONFIG_PM
+static int usba_udc_suspend(struct device *dev)
+{
+	struct usba_udc *udc = dev_get_drvdata(dev);
+
+	/* Not started */
+	if (!udc->driver)
+		return 0;
+
+	mutex_lock(&udc->vbus_mutex);
+
+	if (!device_may_wakeup(dev)) {
+		usba_stop(udc);
+		goto out;
+	}
+
+	/*
+	 * Device may wake up. We stay clocked if we failed
+	 * to request vbus irq, assuming always on.
+	 */
+	if (gpio_is_valid(udc->vbus_pin)) {
+		usba_stop(udc);
+		enable_irq_wake(gpio_to_irq(udc->vbus_pin));
+	}
+
+out:
+	mutex_unlock(&udc->vbus_mutex);
+	return 0;
+}
+
+static int usba_udc_resume(struct device *dev)
+{
+	struct usba_udc *udc = dev_get_drvdata(dev);
+
+	/* Not started */
+	if (!udc->driver)
+		return 0;
+
+	if (device_may_wakeup(dev) && gpio_is_valid(udc->vbus_pin))
+		disable_irq_wake(gpio_to_irq(udc->vbus_pin));
+
+	/* If Vbus is present, enable the controller and wait for reset */
+	mutex_lock(&udc->vbus_mutex);
+	udc->vbus_prev = vbus_is_present(udc);
+	if (udc->vbus_prev)
+		usba_start(udc);
+	mutex_unlock(&udc->vbus_mutex);
+
+	return 0;
+}
+#endif
+
+static SIMPLE_DEV_PM_OPS(usba_udc_pm_ops, usba_udc_suspend, usba_udc_resume);
+
 static struct platform_driver udc_driver = {
 	.remove		= __exit_p(usba_udc_remove),
 	.driver		= {
 		.name		= "atmel_usba_udc",
+		.pm		= &usba_udc_pm_ops,
 		.of_match_table	= of_match_ptr(atmel_udc_dt_ids),
 	},
 };
diff --git a/drivers/usb/gadget/udc/atmel_usba_udc.h b/drivers/usb/gadget/udc/atmel_usba_udc.h
index 497cd18836f3..ea448a344767 100644
--- a/drivers/usb/gadget/udc/atmel_usba_udc.h
+++ b/drivers/usb/gadget/udc/atmel_usba_udc.h
@@ -191,18 +191,28 @@
 	 | USBA_BF(name, value))
 
 /* Register access macros */
+#ifdef CONFIG_AVR32
+#define usba_io_readl	__raw_readl
+#define usba_io_writel	__raw_writel
+#define usba_io_writew	__raw_writew
+#else
+#define usba_io_readl	readl_relaxed
+#define usba_io_writel	writel_relaxed
+#define usba_io_writew	writew_relaxed
+#endif
+
 #define usba_readl(udc, reg)					\
-	__raw_readl((udc)->regs + USBA_##reg)
+	usba_io_readl((udc)->regs + USBA_##reg)
 #define usba_writel(udc, reg, value)				\
-	__raw_writel((value), (udc)->regs + USBA_##reg)
+	usba_io_writel((value), (udc)->regs + USBA_##reg)
 #define usba_ep_readl(ep, reg)					\
-	__raw_readl((ep)->ep_regs + USBA_EPT_##reg)
+	usba_io_readl((ep)->ep_regs + USBA_EPT_##reg)
 #define usba_ep_writel(ep, reg, value)				\
-	__raw_writel((value), (ep)->ep_regs + USBA_EPT_##reg)
+	usba_io_writel((value), (ep)->ep_regs + USBA_EPT_##reg)
 #define usba_dma_readl(ep, reg)					\
-	__raw_readl((ep)->dma_regs + USBA_DMA_##reg)
+	usba_io_readl((ep)->dma_regs + USBA_DMA_##reg)
 #define usba_dma_writel(ep, reg, value)				\
-	__raw_writel((value), (ep)->dma_regs + USBA_DMA_##reg)
+	usba_io_writel((value), (ep)->dma_regs + USBA_DMA_##reg)
 
 /* Calculate base address for a given endpoint or DMA controller */
 #define USBA_EPT_BASE(x)	(0x100 + (x) * 0x20)
@@ -313,6 +323,9 @@ struct usba_udc {
 	/* Protect hw registers from concurrent modifications */
 	spinlock_t lock;
 
+	/* Mutex to prevent concurrent start or stop */
+	struct mutex vbus_mutex;
+
 	void __iomem *regs;
 	void __iomem *fifo;
 
@@ -328,6 +341,7 @@ struct usba_udc {
 	struct clk *hclk;
 	struct usba_ep *usba_ep;
 	bool bias_pulse_needed;
+	bool clocked;
 
 	u16 devstatus;
 
diff --git a/drivers/usb/gadget/udc/dummy_hcd.c b/drivers/usb/gadget/udc/dummy_hcd.c
index 8dda48445f6f..181112c88f43 100644
--- a/drivers/usb/gadget/udc/dummy_hcd.c
+++ b/drivers/usb/gadget/udc/dummy_hcd.c
@@ -1923,7 +1923,7 @@ static inline void
 ss_hub_descriptor(struct usb_hub_descriptor *desc)
 {
 	memset(desc, 0, sizeof *desc);
-	desc->bDescriptorType = 0x2a;
+	desc->bDescriptorType = USB_DT_SS_HUB;
 	desc->bDescLength = 12;
 	desc->wHubCharacteristics = cpu_to_le16(
 			HUB_CHAR_INDV_PORT_LPSM |
@@ -1936,7 +1936,7 @@ ss_hub_descriptor(struct usb_hub_descriptor *desc)
 static inline void hub_descriptor(struct usb_hub_descriptor *desc)
 {
 	memset(desc, 0, sizeof *desc);
-	desc->bDescriptorType = 0x29;
+	desc->bDescriptorType = USB_DT_HUB;
 	desc->bDescLength = 9;
 	desc->wHubCharacteristics = cpu_to_le16(
 			HUB_CHAR_INDV_PORT_LPSM |
@@ -2631,7 +2631,7 @@ static int __init init(void)
 		return -EINVAL;
 
 	if (mod_data.num < 1 || mod_data.num > MAX_NUM_UDC) {
-		pr_err("Number of emulated UDC must be in range of 1…%d\n",
+		pr_err("Number of emulated UDC must be in range of 1...%d\n",
 				MAX_NUM_UDC);
 		return -EINVAL;
 	}
diff --git a/drivers/usb/gadget/udc/goku_udc.c b/drivers/usb/gadget/udc/goku_udc.c
index 5b9176e7202a..9e8d842e8c08 100644
--- a/drivers/usb/gadget/udc/goku_udc.c
+++ b/drivers/usb/gadget/udc/goku_udc.c
@@ -1024,35 +1024,79 @@ static const char proc_node_name [] = "driver/udc";
 static void dump_intmask(struct seq_file *m, const char *label, u32 mask)
 {
 	/* int_status is the same format ... */
-	seq_printf(m,
-		"%s %05X =" FOURBITS EIGHTBITS EIGHTBITS "\n",
-		label, mask,
-		(mask & INT_PWRDETECT) ? " power" : "",
-		(mask & INT_SYSERROR) ? " sys" : "",
-		(mask & INT_MSTRDEND) ? " in-dma" : "",
-		(mask & INT_MSTWRTMOUT) ? " wrtmo" : "",
-
-		(mask & INT_MSTWREND) ? " out-dma" : "",
-		(mask & INT_MSTWRSET) ? " wrset" : "",
-		(mask & INT_ERR) ? " err" : "",
-		(mask & INT_SOF) ? " sof" : "",
-
-		(mask & INT_EP3NAK) ? " ep3nak" : "",
-		(mask & INT_EP2NAK) ? " ep2nak" : "",
-		(mask & INT_EP1NAK) ? " ep1nak" : "",
-		(mask & INT_EP3DATASET) ? " ep3" : "",
-
-		(mask & INT_EP2DATASET) ? " ep2" : "",
-		(mask & INT_EP1DATASET) ? " ep1" : "",
-		(mask & INT_STATUSNAK) ? " ep0snak" : "",
-		(mask & INT_STATUS) ? " ep0status" : "",
-
-		(mask & INT_SETUP) ? " setup" : "",
-		(mask & INT_ENDPOINT0) ? " ep0" : "",
-		(mask & INT_USBRESET) ? " reset" : "",
-		(mask & INT_SUSPEND) ? " suspend" : "");
+	seq_printf(m, "%s %05X =" FOURBITS EIGHTBITS EIGHTBITS "\n",
+		   label, mask,
+		   (mask & INT_PWRDETECT) ? " power" : "",
+		   (mask & INT_SYSERROR) ? " sys" : "",
+		   (mask & INT_MSTRDEND) ? " in-dma" : "",
+		   (mask & INT_MSTWRTMOUT) ? " wrtmo" : "",
+
+		   (mask & INT_MSTWREND) ? " out-dma" : "",
+		   (mask & INT_MSTWRSET) ? " wrset" : "",
+		   (mask & INT_ERR) ? " err" : "",
+		   (mask & INT_SOF) ? " sof" : "",
+
+		   (mask & INT_EP3NAK) ? " ep3nak" : "",
+		   (mask & INT_EP2NAK) ? " ep2nak" : "",
+		   (mask & INT_EP1NAK) ? " ep1nak" : "",
+		   (mask & INT_EP3DATASET) ? " ep3" : "",
+
+		   (mask & INT_EP2DATASET) ? " ep2" : "",
+		   (mask & INT_EP1DATASET) ? " ep1" : "",
+		   (mask & INT_STATUSNAK) ? " ep0snak" : "",
+		   (mask & INT_STATUS) ? " ep0status" : "",
+
+		   (mask & INT_SETUP) ? " setup" : "",
+		   (mask & INT_ENDPOINT0) ? " ep0" : "",
+		   (mask & INT_USBRESET) ? " reset" : "",
+		   (mask & INT_SUSPEND) ? " suspend" : "");
+}
+
+static const char *udc_ep_state(enum ep0state state)
+{
+	switch (state) {
+	case EP0_DISCONNECT:
+		return "ep0_disconnect";
+	case EP0_IDLE:
+		return "ep0_idle";
+	case EP0_IN:
+		return "ep0_in";
+	case EP0_OUT:
+		return "ep0_out";
+	case EP0_STATUS:
+		return "ep0_status";
+	case EP0_STALL:
+		return "ep0_stall";
+	case EP0_SUSPEND:
+		return "ep0_suspend";
+	}
+
+	return "ep0_?";
 }
 
+static const char *udc_ep_status(u32 status)
+{
+	switch (status & EPxSTATUS_EP_MASK) {
+	case EPxSTATUS_EP_READY:
+		return "ready";
+	case EPxSTATUS_EP_DATAIN:
+		return "packet";
+	case EPxSTATUS_EP_FULL:
+		return "full";
+	case EPxSTATUS_EP_TX_ERR:	/* host will retry */
+		return "tx_err";
+	case EPxSTATUS_EP_RX_ERR:
+		return "rx_err";
+	case EPxSTATUS_EP_BUSY:		/* ep0 only */
+		return "busy";
+	case EPxSTATUS_EP_STALL:
+		return "stall";
+	case EPxSTATUS_EP_INVALID:	/* these "can't happen" */
+		return "invalid";
+	}
+
+	return "?";
+}
 
 static int udc_proc_read(struct seq_file *m, void *v)
 {
@@ -1068,29 +1112,18 @@ static int udc_proc_read(struct seq_file *m, void *v)
 	tmp = readl(&regs->power_detect);
 	is_usb_connected = tmp & PW_DETECT;
 	seq_printf(m,
-		"%s - %s\n"
-		"%s version: %s %s\n"
-		"Gadget driver: %s\n"
-		"Host %s, %s\n"
-		"\n",
-		pci_name(dev->pdev), driver_desc,
-		driver_name, DRIVER_VERSION, dmastr(),
-		dev->driver ? dev->driver->driver.name : "(none)",
-		is_usb_connected
-			? ((tmp & PW_PULLUP) ? "full speed" : "powered")
-			: "disconnected",
-		({const char *state;
-		switch(dev->ep0state){
-		case EP0_DISCONNECT:	state = "ep0_disconnect"; break;
-		case EP0_IDLE:		state = "ep0_idle"; break;
-		case EP0_IN:		state = "ep0_in"; break;
-		case EP0_OUT:		state = "ep0_out"; break;
-		case EP0_STATUS:	state = "ep0_status"; break;
-		case EP0_STALL:		state = "ep0_stall"; break;
-		case EP0_SUSPEND:	state = "ep0_suspend"; break;
-		default:		state = "ep0_?"; break;
-		} state; })
-		);
+		   "%s - %s\n"
+		   "%s version: %s %s\n"
+		   "Gadget driver: %s\n"
+		   "Host %s, %s\n"
+		   "\n",
+		   pci_name(dev->pdev), driver_desc,
+		   driver_name, DRIVER_VERSION, dmastr(),
+		   dev->driver ? dev->driver->driver.name : "(none)",
+		   is_usb_connected
+			   ? ((tmp & PW_PULLUP) ? "full speed" : "powered")
+			   : "disconnected",
+		   udc_ep_state(dev->ep0state));
 
 	dump_intmask(m, "int_status", readl(&regs->int_status));
 	dump_intmask(m, "int_enable", readl(&regs->int_enable));
@@ -1099,31 +1132,30 @@ static int udc_proc_read(struct seq_file *m, void *v)
 		goto done;
 
 	/* registers for (active) device and ep0 */
-	if (seq_printf(m, "\nirqs %lu\ndataset %02x "
-			"single.bcs %02x.%02x state %x addr %u\n",
-			dev->irqs, readl(&regs->DataSet),
-			readl(&regs->EPxSingle), readl(&regs->EPxBCS),
-			readl(&regs->UsbState),
-			readl(&regs->address)) < 0)
+	seq_printf(m, "\nirqs %lu\ndataset %02x single.bcs %02x.%02x state %x addr %u\n",
+		   dev->irqs, readl(&regs->DataSet),
+		   readl(&regs->EPxSingle), readl(&regs->EPxBCS),
+		   readl(&regs->UsbState),
+		   readl(&regs->address));
+	if (seq_has_overflowed(m))
 		goto done;
 
 	tmp = readl(&regs->dma_master);
-	if (seq_printf(m,
-		"dma %03X =" EIGHTBITS "%s %s\n", tmp,
-		(tmp & MST_EOPB_DIS) ? " eopb-" : "",
-		(tmp & MST_EOPB_ENA) ? " eopb+" : "",
-		(tmp & MST_TIMEOUT_DIS) ? " tmo-" : "",
-		(tmp & MST_TIMEOUT_ENA) ? " tmo+" : "",
-
-		(tmp & MST_RD_EOPB) ? " eopb" : "",
-		(tmp & MST_RD_RESET) ? " in_reset" : "",
-		(tmp & MST_WR_RESET) ? " out_reset" : "",
-		(tmp & MST_RD_ENA) ? " IN" : "",
-
-		(tmp & MST_WR_ENA) ? " OUT" : "",
-		(tmp & MST_CONNECTION)
-			? "ep1in/ep2out"
-			: "ep1out/ep2in") < 0)
+	seq_printf(m, "dma %03X =" EIGHTBITS "%s %s\n",
+		   tmp,
+		   (tmp & MST_EOPB_DIS) ? " eopb-" : "",
+		   (tmp & MST_EOPB_ENA) ? " eopb+" : "",
+		   (tmp & MST_TIMEOUT_DIS) ? " tmo-" : "",
+		   (tmp & MST_TIMEOUT_ENA) ? " tmo+" : "",
+
+		   (tmp & MST_RD_EOPB) ? " eopb" : "",
+		   (tmp & MST_RD_RESET) ? " in_reset" : "",
+		   (tmp & MST_WR_RESET) ? " out_reset" : "",
+		   (tmp & MST_RD_ENA) ? " IN" : "",
+
+		   (tmp & MST_WR_ENA) ? " OUT" : "",
+		   (tmp & MST_CONNECTION) ? "ep1in/ep2out" : "ep1out/ep2in");
+	if (seq_has_overflowed(m))
 		goto done;
 
 	/* dump endpoint queues */
@@ -1135,44 +1167,23 @@ static int udc_proc_read(struct seq_file *m, void *v)
 			continue;
 
 		tmp = readl(ep->reg_status);
-		if (seq_printf(m,
-			"%s %s max %u %s, irqs %lu, "
-			"status %02x (%s) " FOURBITS "\n",
-			ep->ep.name,
-			ep->is_in ? "in" : "out",
-			ep->ep.maxpacket,
-			ep->dma ? "dma" : "pio",
-			ep->irqs,
-			tmp, ({ char *s;
-			switch (tmp & EPxSTATUS_EP_MASK) {
-			case EPxSTATUS_EP_READY:
-				s = "ready"; break;
-			case EPxSTATUS_EP_DATAIN:
-				s = "packet"; break;
-			case EPxSTATUS_EP_FULL:
-				s = "full"; break;
-			case EPxSTATUS_EP_TX_ERR:	// host will retry
-				s = "tx_err"; break;
-			case EPxSTATUS_EP_RX_ERR:
-				s = "rx_err"; break;
-			case EPxSTATUS_EP_BUSY:		/* ep0 only */
-				s = "busy"; break;
-			case EPxSTATUS_EP_STALL:
-				s = "stall"; break;
-			case EPxSTATUS_EP_INVALID:	// these "can't happen"
-				s = "invalid"; break;
-			default:
-				s = "?"; break;
-			} s; }),
-			(tmp & EPxSTATUS_TOGGLE) ? "data1" : "data0",
-			(tmp & EPxSTATUS_SUSPEND) ? " suspend" : "",
-			(tmp & EPxSTATUS_FIFO_DISABLE) ? " disable" : "",
-			(tmp & EPxSTATUS_STAGE_ERROR) ? " ep0stat" : ""
-			) < 0)
+		seq_printf(m, "%s %s max %u %s, irqs %lu, status %02x (%s) " FOURBITS "\n",
+			   ep->ep.name,
+			   ep->is_in ? "in" : "out",
+			   ep->ep.maxpacket,
+			   ep->dma ? "dma" : "pio",
+			   ep->irqs,
+			   tmp, udc_ep_status(tmp),
+			   (tmp & EPxSTATUS_TOGGLE) ? "data1" : "data0",
+			   (tmp & EPxSTATUS_SUSPEND) ? " suspend" : "",
+			   (tmp & EPxSTATUS_FIFO_DISABLE) ? " disable" : "",
+			   (tmp & EPxSTATUS_STAGE_ERROR) ? " ep0stat" : "");
+		if (seq_has_overflowed(m))
 			goto done;
 
 		if (list_empty(&ep->queue)) {
-			if (seq_puts(m, "\t(nothing queued)\n") < 0)
+			seq_puts(m, "\t(nothing queued)\n");
+			if (seq_has_overflowed(m))
 				goto done;
 			continue;
 		}
@@ -1187,10 +1198,10 @@ static int udc_proc_read(struct seq_file *m, void *v)
 			} else
 				tmp = req->req.actual;
 
-			if (seq_printf(m,
-				"\treq %p len %u/%u buf %p\n",
-				&req->req, tmp, req->req.length,
-				req->req.buf) < 0)
+			seq_printf(m, "\treq %p len %u/%u buf %p\n",
+				   &req->req, tmp, req->req.length,
+				   req->req.buf);
+			if (seq_has_overflowed(m))
 				goto done;
 		}
 	}
diff --git a/drivers/usb/gadget/udc/lpc32xx_udc.c b/drivers/usb/gadget/udc/lpc32xx_udc.c
index 27fd41333f71..3b6a7852822d 100644
--- a/drivers/usb/gadget/udc/lpc32xx_udc.c
+++ b/drivers/usb/gadget/udc/lpc32xx_udc.c
@@ -1803,23 +1803,14 @@ static int lpc32xx_ep_queue(struct usb_ep *_ep,
 	req = container_of(_req, struct lpc32xx_request, req);
 	ep = container_of(_ep, struct lpc32xx_ep, ep);
 
-	if (!_req || !_req->complete || !_req->buf ||
+	if (!_ep || !_req || !_req->complete || !_req->buf ||
 	    !list_empty(&req->queue))
 		return -EINVAL;
 
 	udc = ep->udc;
 
-	if (!_ep) {
-		dev_dbg(udc->dev, "invalid ep\n");
-		return -EINVAL;
-	}
-
-
-	if ((!udc) || (!udc->driver) ||
-	    (udc->gadget.speed == USB_SPEED_UNKNOWN)) {
-		dev_dbg(udc->dev, "invalid device\n");
-		return -EINVAL;
-	}
+	if (udc->gadget.speed == USB_SPEED_UNKNOWN)
+		return -EPIPE;
 
 	if (ep->lep) {
 		struct lpc32xx_usbd_dd_gad *dd;
diff --git a/drivers/usb/gadget/udc/net2280.c b/drivers/usb/gadget/udc/net2280.c
index d2c0bf65e345..9871b90195ad 100644
--- a/drivers/usb/gadget/udc/net2280.c
+++ b/drivers/usb/gadget/udc/net2280.c
@@ -80,6 +80,13 @@ static const char *const ep_name[] = {
 	"ep-e", "ep-f", "ep-g", "ep-h",
 };
 
+/* Endpoint names for usb3380 advance mode */
+static const char *const ep_name_adv[] = {
+	ep0name,
+	"ep1in", "ep2out", "ep3in", "ep4out",
+	"ep1out", "ep2in", "ep3out", "ep4in",
+};
+
 /* mode 0 == ep-{a,b,c,d} 1K fifo each
  * mode 1 == ep-{a,b} 2K fifo each, ep-{c,d} unavailable
  * mode 2 == ep-a 2K fifo, ep-{b,c} 1K each, ep-d unavailable
@@ -138,31 +145,44 @@ net2280_enable(struct usb_ep *_ep, const struct usb_endpoint_descriptor *desc)
 	u32			max, tmp;
 	unsigned long		flags;
 	static const u32 ep_key[9] = { 1, 0, 1, 0, 1, 1, 0, 1, 0 };
+	int ret = 0;
 
 	ep = container_of(_ep, struct net2280_ep, ep);
 	if (!_ep || !desc || ep->desc || _ep->name == ep0name ||
-			desc->bDescriptorType != USB_DT_ENDPOINT)
+			desc->bDescriptorType != USB_DT_ENDPOINT) {
+		pr_err("%s: failed at line=%d\n", __func__, __LINE__);
 		return -EINVAL;
+	}
 	dev = ep->dev;
-	if (!dev->driver || dev->gadget.speed == USB_SPEED_UNKNOWN)
-		return -ESHUTDOWN;
+	if (!dev->driver || dev->gadget.speed == USB_SPEED_UNKNOWN) {
+		ret = -ESHUTDOWN;
+		goto print_err;
+	}
 
 	/* erratum 0119 workaround ties up an endpoint number */
-	if ((desc->bEndpointAddress & 0x0f) == EP_DONTUSE)
-		return -EDOM;
+	if ((desc->bEndpointAddress & 0x0f) == EP_DONTUSE) {
+		ret = -EDOM;
+		goto print_err;
+	}
 
 	if (dev->quirks & PLX_SUPERSPEED) {
-		if ((desc->bEndpointAddress & 0x0f) >= 0x0c)
-			return -EDOM;
+		if ((desc->bEndpointAddress & 0x0f) >= 0x0c) {
+			ret = -EDOM;
+			goto print_err;
+		}
 		ep->is_in = !!usb_endpoint_dir_in(desc);
-		if (dev->enhanced_mode && ep->is_in && ep_key[ep->num])
-			return -EINVAL;
+		if (dev->enhanced_mode && ep->is_in && ep_key[ep->num]) {
+			ret = -EINVAL;
+			goto print_err;
+		}
 	}
 
 	/* sanity check ep-e/ep-f since their fifos are small */
 	max = usb_endpoint_maxp(desc) & 0x1fff;
-	if (ep->num > 4 && max > 64 && (dev->quirks & PLX_LEGACY))
-		return -ERANGE;
+	if (ep->num > 4 && max > 64 && (dev->quirks & PLX_LEGACY)) {
+		ret = -ERANGE;
+		goto print_err;
+	}
 
 	spin_lock_irqsave(&dev->lock, flags);
 	_ep->maxpacket = max & 0x7ff;
@@ -192,7 +212,8 @@ net2280_enable(struct usb_ep *_ep, const struct usb_endpoint_descriptor *desc)
 		    (dev->gadget.speed == USB_SPEED_HIGH && max != 512) ||
 		    (dev->gadget.speed == USB_SPEED_FULL && max > 64)) {
 			spin_unlock_irqrestore(&dev->lock, flags);
-			return -ERANGE;
+			ret = -ERANGE;
+			goto print_err;
 		}
 	}
 	ep->is_iso = (tmp == USB_ENDPOINT_XFER_ISOC);
@@ -271,7 +292,11 @@ net2280_enable(struct usb_ep *_ep, const struct usb_endpoint_descriptor *desc)
 
 	/* pci writes may still be posted */
 	spin_unlock_irqrestore(&dev->lock, flags);
-	return 0;
+	return ret;
+
+print_err:
+	dev_err(&ep->dev->pdev->dev, "%s: error=%d\n", __func__, ret);
+	return ret;
 }
 
 static int handshake(u32 __iomem *ptr, u32 mask, u32 done, int usec)
@@ -426,9 +451,10 @@ static int net2280_disable(struct usb_ep *_ep)
 	unsigned long		flags;
 
 	ep = container_of(_ep, struct net2280_ep, ep);
-	if (!_ep || !ep->desc || _ep->name == ep0name)
+	if (!_ep || !ep->desc || _ep->name == ep0name) {
+		pr_err("%s: Invalid ep=%p or ep->desc\n", __func__, _ep);
 		return -EINVAL;
-
+	}
 	spin_lock_irqsave(&ep->dev->lock, flags);
 	nuke(ep);
 
@@ -458,8 +484,10 @@ static struct usb_request
 	struct net2280_ep	*ep;
 	struct net2280_request	*req;
 
-	if (!_ep)
+	if (!_ep) {
+		pr_err("%s: Invalid ep\n", __func__);
 		return NULL;
+	}
 	ep = container_of(_ep, struct net2280_ep, ep);
 
 	req = kzalloc(sizeof(*req), gfp_flags);
@@ -491,8 +519,11 @@ static void net2280_free_request(struct usb_ep *_ep, struct usb_request *_req)
 	struct net2280_request	*req;
 
 	ep = container_of(_ep, struct net2280_ep, ep);
-	if (!_ep || !_req)
+	if (!_ep || !_req) {
+		dev_err(&ep->dev->pdev->dev, "%s: Inavlid ep=%p or req=%p\n",
+							__func__, _ep, _req);
 		return;
+	}
 
 	req = container_of(_req, struct net2280_request, req);
 	WARN_ON(!list_empty(&req->queue));
@@ -896,35 +927,44 @@ net2280_queue(struct usb_ep *_ep, struct usb_request *_req, gfp_t gfp_flags)
 	struct net2280_ep	*ep;
 	struct net2280		*dev;
 	unsigned long		flags;
+	int ret = 0;
 
 	/* we always require a cpu-view buffer, so that we can
 	 * always use pio (as fallback or whatever).
 	 */
-	req = container_of(_req, struct net2280_request, req);
-	if (!_req || !_req->complete || !_req->buf ||
-				!list_empty(&req->queue))
-		return -EINVAL;
-	if (_req->length > (~0 & DMA_BYTE_COUNT_MASK))
-		return -EDOM;
 	ep = container_of(_ep, struct net2280_ep, ep);
-	if (!_ep || (!ep->desc && ep->num != 0))
+	if (!_ep || (!ep->desc && ep->num != 0)) {
+		pr_err("%s: Invalid ep=%p or ep->desc\n", __func__, _ep);
 		return -EINVAL;
+	}
+	req = container_of(_req, struct net2280_request, req);
+	if (!_req || !_req->complete || !_req->buf ||
+				!list_empty(&req->queue)) {
+		ret = -EINVAL;
+		goto print_err;
+	}
+	if (_req->length > (~0 & DMA_BYTE_COUNT_MASK)) {
+		ret = -EDOM;
+		goto print_err;
+	}
 	dev = ep->dev;
-	if (!dev->driver || dev->gadget.speed == USB_SPEED_UNKNOWN)
-		return -ESHUTDOWN;
+	if (!dev->driver || dev->gadget.speed == USB_SPEED_UNKNOWN) {
+		ret = -ESHUTDOWN;
+		goto print_err;
+	}
 
 	/* FIXME implement PIO fallback for ZLPs with DMA */
-	if (ep->dma && _req->length == 0)
-		return -EOPNOTSUPP;
+	if (ep->dma && _req->length == 0) {
+		ret = -EOPNOTSUPP;
+		goto print_err;
+	}
 
 	/* set up dma mapping in case the caller didn't */
 	if (ep->dma) {
-		int ret;
-
 		ret = usb_gadget_map_request(&dev->gadget, _req,
 				ep->is_in);
 		if (ret)
-			return ret;
+			goto print_err;
 	}
 
 	ep_vdbg(dev, "%s queue req %p, len %d buf %p\n",
@@ -1013,7 +1053,11 @@ done:
 	spin_unlock_irqrestore(&dev->lock, flags);
 
 	/* pci writes may still be posted */
-	return 0;
+	return ret;
+
+print_err:
+	dev_err(&ep->dev->pdev->dev, "%s: error=%d\n", __func__, ret);
+	return ret;
 }
 
 static inline void
@@ -1134,8 +1178,11 @@ static int net2280_dequeue(struct usb_ep *_ep, struct usb_request *_req)
 	int			stopped;
 
 	ep = container_of(_ep, struct net2280_ep, ep);
-	if (!_ep || (!ep->desc && ep->num != 0) || !_req)
+	if (!_ep || (!ep->desc && ep->num != 0) || !_req) {
+		pr_err("%s: Invalid ep=%p or ep->desc or req=%p\n",
+						__func__, _ep, _req);
 		return -EINVAL;
+	}
 
 	spin_lock_irqsave(&ep->dev->lock, flags);
 	stopped = ep->stopped;
@@ -1157,6 +1204,8 @@ static int net2280_dequeue(struct usb_ep *_ep, struct usb_request *_req)
 	}
 	if (&req->req != _req) {
 		spin_unlock_irqrestore(&ep->dev->lock, flags);
+		dev_err(&ep->dev->pdev->dev, "%s: Request mismatch\n",
+								__func__);
 		return -EINVAL;
 	}
 
@@ -1214,20 +1263,28 @@ net2280_set_halt_and_wedge(struct usb_ep *_ep, int value, int wedged)
 	int			retval = 0;
 
 	ep = container_of(_ep, struct net2280_ep, ep);
-	if (!_ep || (!ep->desc && ep->num != 0))
+	if (!_ep || (!ep->desc && ep->num != 0)) {
+		pr_err("%s: Invalid ep=%p or ep->desc\n", __func__, _ep);
 		return -EINVAL;
-	if (!ep->dev->driver || ep->dev->gadget.speed == USB_SPEED_UNKNOWN)
-		return -ESHUTDOWN;
+	}
+	if (!ep->dev->driver || ep->dev->gadget.speed == USB_SPEED_UNKNOWN) {
+		retval = -ESHUTDOWN;
+		goto print_err;
+	}
 	if (ep->desc /* not ep0 */ && (ep->desc->bmAttributes & 0x03)
-						== USB_ENDPOINT_XFER_ISOC)
-		return -EINVAL;
+						== USB_ENDPOINT_XFER_ISOC) {
+		retval = -EINVAL;
+		goto print_err;
+	}
 
 	spin_lock_irqsave(&ep->dev->lock, flags);
-	if (!list_empty(&ep->queue))
+	if (!list_empty(&ep->queue)) {
 		retval = -EAGAIN;
-	else if (ep->is_in && value && net2280_fifo_status(_ep) != 0)
+		goto print_unlock;
+	} else if (ep->is_in && value && net2280_fifo_status(_ep) != 0) {
 		retval = -EAGAIN;
-	else {
+		goto print_unlock;
+	} else {
 		ep_vdbg(ep->dev, "%s %s %s\n", _ep->name,
 				value ? "set" : "clear",
 				wedged ? "wedge" : "halt");
@@ -1251,6 +1308,12 @@ net2280_set_halt_and_wedge(struct usb_ep *_ep, int value, int wedged)
 	spin_unlock_irqrestore(&ep->dev->lock, flags);
 
 	return retval;
+
+print_unlock:
+	spin_unlock_irqrestore(&ep->dev->lock, flags);
+print_err:
+	dev_err(&ep->dev->pdev->dev, "%s: error=%d\n", __func__, retval);
+	return retval;
 }
 
 static int net2280_set_halt(struct usb_ep *_ep, int value)
@@ -1260,8 +1323,10 @@ static int net2280_set_halt(struct usb_ep *_ep, int value)
 
 static int net2280_set_wedge(struct usb_ep *_ep)
 {
-	if (!_ep || _ep->name == ep0name)
+	if (!_ep || _ep->name == ep0name) {
+		pr_err("%s: Invalid ep=%p or ep0\n", __func__, _ep);
 		return -EINVAL;
+	}
 	return net2280_set_halt_and_wedge(_ep, 1, 1);
 }
 
@@ -1271,14 +1336,22 @@ static int net2280_fifo_status(struct usb_ep *_ep)
 	u32			avail;
 
 	ep = container_of(_ep, struct net2280_ep, ep);
-	if (!_ep || (!ep->desc && ep->num != 0))
+	if (!_ep || (!ep->desc && ep->num != 0)) {
+		pr_err("%s: Invalid ep=%p or ep->desc\n", __func__, _ep);
 		return -ENODEV;
-	if (!ep->dev->driver || ep->dev->gadget.speed == USB_SPEED_UNKNOWN)
+	}
+	if (!ep->dev->driver || ep->dev->gadget.speed == USB_SPEED_UNKNOWN) {
+		dev_err(&ep->dev->pdev->dev,
+			"%s: Invalid driver=%p or speed=%d\n",
+			__func__, ep->dev->driver, ep->dev->gadget.speed);
 		return -ESHUTDOWN;
+	}
 
 	avail = readl(&ep->regs->ep_avail) & (BIT(12) - 1);
-	if (avail > ep->fifo_size)
+	if (avail > ep->fifo_size) {
+		dev_err(&ep->dev->pdev->dev, "%s: Fifo overflow\n", __func__);
 		return -EOVERFLOW;
+	}
 	if (ep->is_in)
 		avail = ep->fifo_size - avail;
 	return avail;
@@ -1289,10 +1362,16 @@ static void net2280_fifo_flush(struct usb_ep *_ep)
 	struct net2280_ep	*ep;
 
 	ep = container_of(_ep, struct net2280_ep, ep);
-	if (!_ep || (!ep->desc && ep->num != 0))
+	if (!_ep || (!ep->desc && ep->num != 0)) {
+		pr_err("%s: Invalid ep=%p or ep->desc\n", __func__, _ep);
 		return;
-	if (!ep->dev->driver || ep->dev->gadget.speed == USB_SPEED_UNKNOWN)
+	}
+	if (!ep->dev->driver || ep->dev->gadget.speed == USB_SPEED_UNKNOWN) {
+		dev_err(&ep->dev->pdev->dev,
+			"%s: Invalid driver=%p or speed=%d\n",
+			__func__, ep->dev->driver, ep->dev->gadget.speed);
 		return;
+	}
 
 	writel(BIT(FIFO_FLUSH), &ep->regs->ep_stat);
 	(void) readl(&ep->regs->ep_rsp);
@@ -1977,7 +2056,7 @@ static void usb_reinit_338x(struct net2280 *dev)
 	for (i = 0; i < dev->n_ep; i++) {
 		struct net2280_ep *ep = &dev->ep[i];
 
-		ep->ep.name = ep_name[i];
+		ep->ep.name = dev->enhanced_mode ? ep_name_adv[i] : ep_name[i];
 		ep->dev = dev;
 		ep->num = i;
 
@@ -1989,11 +2068,9 @@ static void usb_reinit_338x(struct net2280 *dev)
 			ep->regs = (struct net2280_ep_regs __iomem *)
 				(((void __iomem *)&dev->epregs[ne[i]]) +
 				ep_reg_addr[i]);
-			ep->fiforegs = &dev->fiforegs[i];
 		} else {
 			ep->cfg = &dev->epregs[i];
 			ep->regs = &dev->epregs[i];
-			ep->fiforegs = &dev->fiforegs[i];
 		}
 
 		ep->fifo_size = (i != 0) ? 2048 : 512;
@@ -2186,7 +2263,6 @@ static int net2280_start(struct usb_gadget *_gadget,
 		dev->ep[i].irqs = 0;
 
 	/* hook up the driver ... */
-	dev->softconnect = 1;
 	driver->driver.bus = NULL;
 	dev->driver = driver;
 
@@ -3052,6 +3128,8 @@ next_endpoints:
 		BIT(PCI_RETRY_ABORT_INTERRUPT))
 
 static void handle_stat1_irqs(struct net2280 *dev, u32 stat)
+__releases(dev->lock)
+__acquires(dev->lock)
 {
 	struct net2280_ep	*ep;
 	u32			tmp, num, mask, scratch;
@@ -3373,8 +3451,6 @@ static int net2280_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 		u32 usbstat;
 		dev->usb_ext = (struct usb338x_usb_ext_regs __iomem *)
 							(base + 0x00b4);
-		dev->fiforegs = (struct usb338x_fifo_regs __iomem *)
-							(base + 0x0500);
 		dev->llregs = (struct usb338x_ll_regs __iomem *)
 							(base + 0x0700);
 		dev->ll_lfps_regs = (struct usb338x_ll_lfps_regs __iomem *)
diff --git a/drivers/usb/gadget/udc/net2280.h b/drivers/usb/gadget/udc/net2280.h
index ac8d5a20a378..4dff60d34f73 100644
--- a/drivers/usb/gadget/udc/net2280.h
+++ b/drivers/usb/gadget/udc/net2280.h
@@ -96,7 +96,6 @@ struct net2280_ep {
 	struct net2280_ep_regs			__iomem *regs;
 	struct net2280_dma_regs			__iomem *dma;
 	struct net2280_dma			*dummy;
-	struct usb338x_fifo_regs __iomem *fiforegs;
 	dma_addr_t				td_dma;	/* of dummy */
 	struct net2280				*dev;
 	unsigned long				irqs;
@@ -181,7 +180,6 @@ struct net2280 {
 	struct net2280_dma_regs		__iomem *dma;
 	struct net2280_dep_regs		__iomem *dep;
 	struct net2280_ep_regs		__iomem *epregs;
-	struct usb338x_fifo_regs	__iomem *fiforegs;
 	struct usb338x_ll_regs		__iomem *llregs;
 	struct usb338x_ll_lfps_regs	__iomem *ll_lfps_regs;
 	struct usb338x_ll_tsn_regs	__iomem *ll_tsn_regs;
diff --git a/drivers/usb/gadget/udc/pxa27x_udc.c b/drivers/usb/gadget/udc/pxa27x_udc.c
index 6a855fc9bd84..b51226abade6 100644
--- a/drivers/usb/gadget/udc/pxa27x_udc.c
+++ b/drivers/usb/gadget/udc/pxa27x_udc.c
@@ -93,50 +93,46 @@ static void handle_ep(struct pxa_ep *ep);
 static int state_dbg_show(struct seq_file *s, void *p)
 {
 	struct pxa_udc *udc = s->private;
-	int pos = 0, ret;
 	u32 tmp;
 
-	ret = -ENODEV;
 	if (!udc->driver)
-		goto out;
+		return -ENODEV;
 
 	/* basic device status */
-	pos += seq_printf(s, DRIVER_DESC "\n"
-			 "%s version: %s\nGadget driver: %s\n",
-			 driver_name, DRIVER_VERSION,
-			 udc->driver ? udc->driver->driver.name : "(none)");
+	seq_printf(s, DRIVER_DESC "\n"
+		   "%s version: %s\n"
+		   "Gadget driver: %s\n",
+		   driver_name, DRIVER_VERSION,
+		   udc->driver ? udc->driver->driver.name : "(none)");
 
 	tmp = udc_readl(udc, UDCCR);
-	pos += seq_printf(s,
-			 "udccr=0x%0x(%s%s%s%s%s%s%s%s%s%s), "
-			 "con=%d,inter=%d,altinter=%d\n", tmp,
-			 (tmp & UDCCR_OEN) ? " oen":"",
-			 (tmp & UDCCR_AALTHNP) ? " aalthnp":"",
-			 (tmp & UDCCR_AHNP) ? " rem" : "",
-			 (tmp & UDCCR_BHNP) ? " rstir" : "",
-			 (tmp & UDCCR_DWRE) ? " dwre" : "",
-			 (tmp & UDCCR_SMAC) ? " smac" : "",
-			 (tmp & UDCCR_EMCE) ? " emce" : "",
-			 (tmp & UDCCR_UDR) ? " udr" : "",
-			 (tmp & UDCCR_UDA) ? " uda" : "",
-			 (tmp & UDCCR_UDE) ? " ude" : "",
-			 (tmp & UDCCR_ACN) >> UDCCR_ACN_S,
-			 (tmp & UDCCR_AIN) >> UDCCR_AIN_S,
-			 (tmp & UDCCR_AAISN) >> UDCCR_AAISN_S);
+	seq_printf(s,
+		   "udccr=0x%0x(%s%s%s%s%s%s%s%s%s%s), con=%d,inter=%d,altinter=%d\n",
+		   tmp,
+		   (tmp & UDCCR_OEN) ? " oen":"",
+		   (tmp & UDCCR_AALTHNP) ? " aalthnp":"",
+		   (tmp & UDCCR_AHNP) ? " rem" : "",
+		   (tmp & UDCCR_BHNP) ? " rstir" : "",
+		   (tmp & UDCCR_DWRE) ? " dwre" : "",
+		   (tmp & UDCCR_SMAC) ? " smac" : "",
+		   (tmp & UDCCR_EMCE) ? " emce" : "",
+		   (tmp & UDCCR_UDR) ? " udr" : "",
+		   (tmp & UDCCR_UDA) ? " uda" : "",
+		   (tmp & UDCCR_UDE) ? " ude" : "",
+		   (tmp & UDCCR_ACN) >> UDCCR_ACN_S,
+		   (tmp & UDCCR_AIN) >> UDCCR_AIN_S,
+		   (tmp & UDCCR_AAISN) >> UDCCR_AAISN_S);
 	/* registers for device and ep0 */
-	pos += seq_printf(s, "udcicr0=0x%08x udcicr1=0x%08x\n",
-			udc_readl(udc, UDCICR0), udc_readl(udc, UDCICR1));
-	pos += seq_printf(s, "udcisr0=0x%08x udcisr1=0x%08x\n",
-			udc_readl(udc, UDCISR0), udc_readl(udc, UDCISR1));
-	pos += seq_printf(s, "udcfnr=%d\n", udc_readl(udc, UDCFNR));
-	pos += seq_printf(s, "irqs: reset=%lu, suspend=%lu, resume=%lu, "
-			"reconfig=%lu\n",
-			udc->stats.irqs_reset, udc->stats.irqs_suspend,
-			udc->stats.irqs_resume, udc->stats.irqs_reconfig);
-
-	ret = 0;
-out:
-	return ret;
+	seq_printf(s, "udcicr0=0x%08x udcicr1=0x%08x\n",
+		   udc_readl(udc, UDCICR0), udc_readl(udc, UDCICR1));
+	seq_printf(s, "udcisr0=0x%08x udcisr1=0x%08x\n",
+		   udc_readl(udc, UDCISR0), udc_readl(udc, UDCISR1));
+	seq_printf(s, "udcfnr=%d\n", udc_readl(udc, UDCFNR));
+	seq_printf(s, "irqs: reset=%lu, suspend=%lu, resume=%lu, reconfig=%lu\n",
+		   udc->stats.irqs_reset, udc->stats.irqs_suspend,
+		   udc->stats.irqs_resume, udc->stats.irqs_reconfig);
+
+	return 0;
 }
 
 static int queues_dbg_show(struct seq_file *s, void *p)
@@ -144,75 +140,67 @@ static int queues_dbg_show(struct seq_file *s, void *p)
 	struct pxa_udc *udc = s->private;
 	struct pxa_ep *ep;
 	struct pxa27x_request *req;
-	int pos = 0, i, maxpkt, ret;
+	int i, maxpkt;
 
-	ret = -ENODEV;
 	if (!udc->driver)
-		goto out;
+		return -ENODEV;
 
 	/* dump endpoint queues */
 	for (i = 0; i < NR_PXA_ENDPOINTS; i++) {
 		ep = &udc->pxa_ep[i];
 		maxpkt = ep->fifo_size;
-		pos += seq_printf(s,  "%-12s max_pkt=%d %s\n",
-				EPNAME(ep), maxpkt, "pio");
+		seq_printf(s,  "%-12s max_pkt=%d %s\n",
+			   EPNAME(ep), maxpkt, "pio");
 
 		if (list_empty(&ep->queue)) {
-			pos += seq_printf(s, "\t(nothing queued)\n");
+			seq_puts(s, "\t(nothing queued)\n");
 			continue;
 		}
 
 		list_for_each_entry(req, &ep->queue, queue) {
-			pos += seq_printf(s,  "\treq %p len %d/%d buf %p\n",
-					&req->req, req->req.actual,
-					req->req.length, req->req.buf);
+			seq_printf(s,  "\treq %p len %d/%d buf %p\n",
+				   &req->req, req->req.actual,
+				   req->req.length, req->req.buf);
 		}
 	}
 
-	ret = 0;
-out:
-	return ret;
+	return 0;
 }
 
 static int eps_dbg_show(struct seq_file *s, void *p)
 {
 	struct pxa_udc *udc = s->private;
 	struct pxa_ep *ep;
-	int pos = 0, i, ret;
+	int i;
 	u32 tmp;
 
-	ret = -ENODEV;
 	if (!udc->driver)
-		goto out;
+		return -ENODEV;
 
 	ep = &udc->pxa_ep[0];
 	tmp = udc_ep_readl(ep, UDCCSR);
-	pos += seq_printf(s, "udccsr0=0x%03x(%s%s%s%s%s%s%s)\n", tmp,
-			 (tmp & UDCCSR0_SA) ? " sa" : "",
-			 (tmp & UDCCSR0_RNE) ? " rne" : "",
-			 (tmp & UDCCSR0_FST) ? " fst" : "",
-			 (tmp & UDCCSR0_SST) ? " sst" : "",
-			 (tmp & UDCCSR0_DME) ? " dme" : "",
-			 (tmp & UDCCSR0_IPR) ? " ipr" : "",
-			 (tmp & UDCCSR0_OPC) ? " opc" : "");
+	seq_printf(s, "udccsr0=0x%03x(%s%s%s%s%s%s%s)\n",
+		   tmp,
+		   (tmp & UDCCSR0_SA) ? " sa" : "",
+		   (tmp & UDCCSR0_RNE) ? " rne" : "",
+		   (tmp & UDCCSR0_FST) ? " fst" : "",
+		   (tmp & UDCCSR0_SST) ? " sst" : "",
+		   (tmp & UDCCSR0_DME) ? " dme" : "",
+		   (tmp & UDCCSR0_IPR) ? " ipr" : "",
+		   (tmp & UDCCSR0_OPC) ? " opc" : "");
 	for (i = 0; i < NR_PXA_ENDPOINTS; i++) {
 		ep = &udc->pxa_ep[i];
 		tmp = i? udc_ep_readl(ep, UDCCR) : udc_readl(udc, UDCCR);
-		pos += seq_printf(s, "%-12s: "
-				"IN %lu(%lu reqs), OUT %lu(%lu reqs), "
-				"irqs=%lu, udccr=0x%08x, udccsr=0x%03x, "
-				"udcbcr=%d\n",
-				EPNAME(ep),
-				ep->stats.in_bytes, ep->stats.in_ops,
-				ep->stats.out_bytes, ep->stats.out_ops,
-				ep->stats.irqs,
-				tmp, udc_ep_readl(ep, UDCCSR),
-				udc_ep_readl(ep, UDCBCR));
+		seq_printf(s, "%-12s: IN %lu(%lu reqs), OUT %lu(%lu reqs), irqs=%lu, udccr=0x%08x, udccsr=0x%03x, udcbcr=%d\n",
+			   EPNAME(ep),
+			   ep->stats.in_bytes, ep->stats.in_ops,
+			   ep->stats.out_bytes, ep->stats.out_ops,
+			   ep->stats.irqs,
+			   tmp, udc_ep_readl(ep, UDCCSR),
+			   udc_ep_readl(ep, UDCBCR));
 	}
 
-	ret = 0;
-out:
-	return ret;
+	return 0;
 }
 
 static int eps_dbg_open(struct inode *inode, struct file *file)
@@ -2399,7 +2387,7 @@ static struct pxa_udc memory = {
 };
 
 #if defined(CONFIG_OF)
-static struct of_device_id udc_pxa_dt_ids[] = {
+static const struct of_device_id udc_pxa_dt_ids[] = {
 	{ .compatible = "marvell,pxa270-udc" },
 	{}
 };
diff --git a/drivers/usb/gadget/udc/udc-core.c b/drivers/usb/gadget/udc/udc-core.c
index 5a81cb086b99..d69c35558f68 100644
--- a/drivers/usb/gadget/udc/udc-core.c
+++ b/drivers/usb/gadget/udc/udc-core.c
@@ -35,6 +35,8 @@
  * @dev - the child device to the actual controller
  * @gadget - the gadget. For use by the class code
  * @list - for use by the udc class driver
+ * @vbus - for udcs who care about vbus status, this value is real vbus status;
+ * for udcs who do not care about vbus status, this value is always true
  *
  * This represents the internal data structure which is used by the UDC-class
  * to hold information about udc driver and gadget together.
@@ -44,6 +46,7 @@ struct usb_udc {
 	struct usb_gadget		*gadget;
 	struct device			dev;
 	struct list_head		list;
+	bool				vbus;
 };
 
 static struct class *udc_class;
@@ -128,21 +131,11 @@ EXPORT_SYMBOL_GPL(usb_gadget_giveback_request);
 
 static void usb_gadget_state_work(struct work_struct *work)
 {
-	struct usb_gadget	*gadget = work_to_gadget(work);
-	struct usb_udc		*udc = NULL;
-
-	mutex_lock(&udc_lock);
-	list_for_each_entry(udc, &udc_list, list)
-		if (udc->gadget == gadget)
-			goto found;
-	mutex_unlock(&udc_lock);
-
-	return;
-
-found:
-	mutex_unlock(&udc_lock);
+	struct usb_gadget *gadget = work_to_gadget(work);
+	struct usb_udc *udc = gadget->udc;
 
-	sysfs_notify(&udc->dev.kobj, NULL, "state");
+	if (udc)
+		sysfs_notify(&udc->dev.kobj, NULL, "state");
 }
 
 void usb_gadget_set_state(struct usb_gadget *gadget,
@@ -155,6 +148,34 @@ EXPORT_SYMBOL_GPL(usb_gadget_set_state);
 
 /* ------------------------------------------------------------------------- */
 
+static void usb_udc_connect_control(struct usb_udc *udc)
+{
+	if (udc->vbus)
+		usb_gadget_connect(udc->gadget);
+	else
+		usb_gadget_disconnect(udc->gadget);
+}
+
+/**
+ * usb_udc_vbus_handler - updates the udc core vbus status, and try to
+ * connect or disconnect gadget
+ * @gadget: The gadget which vbus change occurs
+ * @status: The vbus status
+ *
+ * The udc driver calls it when it wants to connect or disconnect gadget
+ * according to vbus status.
+ */
+void usb_udc_vbus_handler(struct usb_gadget *gadget, bool status)
+{
+	struct usb_udc *udc = gadget->udc;
+
+	if (udc) {
+		udc->vbus = status;
+		usb_udc_connect_control(udc);
+	}
+}
+EXPORT_SYMBOL_GPL(usb_udc_vbus_handler);
+
 /**
  * usb_gadget_udc_reset - notifies the udc core that bus reset occurs
  * @gadget: The gadget which bus reset occurs
@@ -278,6 +299,7 @@ int usb_add_gadget_udc_release(struct device *parent, struct usb_gadget *gadget,
 		goto err3;
 
 	udc->gadget = gadget;
+	gadget->udc = udc;
 
 	mutex_lock(&udc_lock);
 	list_add_tail(&udc->list, &udc_list);
@@ -287,6 +309,7 @@ int usb_add_gadget_udc_release(struct device *parent, struct usb_gadget *gadget,
 		goto err4;
 
 	usb_gadget_set_state(gadget, USB_STATE_NOTATTACHED);
+	udc->vbus = true;
 
 	mutex_unlock(&udc_lock);
 
@@ -348,21 +371,14 @@ static void usb_gadget_remove_driver(struct usb_udc *udc)
  */
 void usb_del_gadget_udc(struct usb_gadget *gadget)
 {
-	struct usb_udc		*udc = NULL;
+	struct usb_udc *udc = gadget->udc;
 
-	mutex_lock(&udc_lock);
-	list_for_each_entry(udc, &udc_list, list)
-		if (udc->gadget == gadget)
-			goto found;
-
-	dev_err(gadget->dev.parent, "gadget not registered.\n");
-	mutex_unlock(&udc_lock);
-
-	return;
+	if (!udc)
+		return;
 
-found:
 	dev_vdbg(gadget->dev.parent, "unregistering gadget\n");
 
+	mutex_lock(&udc_lock);
 	list_del(&udc->list);
 	mutex_unlock(&udc_lock);
 
@@ -397,7 +413,7 @@ static int udc_bind_to_driver(struct usb_udc *udc, struct usb_gadget_driver *dri
 		driver->unbind(udc->gadget);
 		goto err1;
 	}
-	usb_gadget_connect(udc->gadget);
+	usb_udc_connect_control(udc);
 
 	kobject_uevent(&udc->dev.kobj, KOBJ_CHANGE);
 	return 0;
diff --git a/drivers/usb/host/Kconfig b/drivers/usb/host/Kconfig
index 5ad60e46dc2b..197a6a3e613b 100644
--- a/drivers/usb/host/Kconfig
+++ b/drivers/usb/host/Kconfig
@@ -198,7 +198,7 @@ config USB_EHCI_HCD_AT91
 
 config USB_EHCI_MSM
 	tristate "Support for Qualcomm QSD/MSM on-chip EHCI USB controller"
-	depends on ARCH_MSM || ARCH_QCOM
+	depends on ARCH_QCOM
 	select USB_EHCI_ROOT_HUB_TT
 	---help---
 	  Enables support for the USB Host controller present on the
diff --git a/drivers/usb/host/ehci-atmel.c b/drivers/usb/host/ehci-atmel.c
index 663f7908b15c..be0964a801e8 100644
--- a/drivers/usb/host/ehci-atmel.c
+++ b/drivers/usb/host/ehci-atmel.c
@@ -34,7 +34,6 @@ static const char hcd_name[] = "ehci-atmel";
 
 struct atmel_ehci_priv {
 	struct clk *iclk;
-	struct clk *fclk;
 	struct clk *uclk;
 	bool clocked;
 };
@@ -51,12 +50,9 @@ static void atmel_start_clock(struct atmel_ehci_priv *atmel_ehci)
 {
 	if (atmel_ehci->clocked)
 		return;
-	if (IS_ENABLED(CONFIG_COMMON_CLK)) {
-		clk_set_rate(atmel_ehci->uclk, 48000000);
-		clk_prepare_enable(atmel_ehci->uclk);
-	}
+
+	clk_prepare_enable(atmel_ehci->uclk);
 	clk_prepare_enable(atmel_ehci->iclk);
-	clk_prepare_enable(atmel_ehci->fclk);
 	atmel_ehci->clocked = true;
 }
 
@@ -64,10 +60,9 @@ static void atmel_stop_clock(struct atmel_ehci_priv *atmel_ehci)
 {
 	if (!atmel_ehci->clocked)
 		return;
-	clk_disable_unprepare(atmel_ehci->fclk);
+
 	clk_disable_unprepare(atmel_ehci->iclk);
-	if (IS_ENABLED(CONFIG_COMMON_CLK))
-		clk_disable_unprepare(atmel_ehci->uclk);
+	clk_disable_unprepare(atmel_ehci->uclk);
 	atmel_ehci->clocked = false;
 }
 
@@ -146,20 +141,13 @@ static int ehci_atmel_drv_probe(struct platform_device *pdev)
 		retval = -ENOENT;
 		goto fail_request_resource;
 	}
-	atmel_ehci->fclk = devm_clk_get(&pdev->dev, "uhpck");
-	if (IS_ERR(atmel_ehci->fclk)) {
-		dev_err(&pdev->dev, "Error getting function clock\n");
-		retval = -ENOENT;
+
+	atmel_ehci->uclk = devm_clk_get(&pdev->dev, "usb_clk");
+	if (IS_ERR(atmel_ehci->uclk)) {
+		dev_err(&pdev->dev, "failed to get uclk\n");
+		retval = PTR_ERR(atmel_ehci->uclk);
 		goto fail_request_resource;
 	}
-	if (IS_ENABLED(CONFIG_COMMON_CLK)) {
-		atmel_ehci->uclk = devm_clk_get(&pdev->dev, "usb_clk");
-		if (IS_ERR(atmel_ehci->uclk)) {
-			dev_err(&pdev->dev, "failed to get uclk\n");
-			retval = PTR_ERR(atmel_ehci->uclk);
-			goto fail_request_resource;
-		}
-	}
 
 	ehci = hcd_to_ehci(hcd);
 	/* registers start at offset 0x0 */
diff --git a/drivers/usb/host/ehci-hcd.c b/drivers/usb/host/ehci-hcd.c
index 85e56d1abd23..f4d88dfb26a7 100644
--- a/drivers/usb/host/ehci-hcd.c
+++ b/drivers/usb/host/ehci-hcd.c
@@ -792,12 +792,12 @@ static irqreturn_t ehci_irq (struct usb_hcd *hcd)
 					ehci->reset_done[i] == 0))
 				continue;
 
-			/* start 20 msec resume signaling from this port,
-			 * and make hub_wq collect PORT_STAT_C_SUSPEND to
-			 * stop that signaling.  Use 5 ms extra for safety,
-			 * like usb_port_resume() does.
+			/* start USB_RESUME_TIMEOUT msec resume signaling from
+			 * this port, and make hub_wq collect
+			 * PORT_STAT_C_SUSPEND to stop that signaling.
 			 */
-			ehci->reset_done[i] = jiffies + msecs_to_jiffies(25);
+			ehci->reset_done[i] = jiffies +
+				msecs_to_jiffies(USB_RESUME_TIMEOUT);
 			set_bit(i, &ehci->resuming_ports);
 			ehci_dbg (ehci, "port %d remote wakeup\n", i + 1);
 			usb_hcd_start_port_resume(&hcd->self, i);
diff --git a/drivers/usb/host/ehci-hub.c b/drivers/usb/host/ehci-hub.c
index 87cf86f38b36..69208447d213 100644
--- a/drivers/usb/host/ehci-hub.c
+++ b/drivers/usb/host/ehci-hub.c
@@ -471,10 +471,13 @@ static int ehci_bus_resume (struct usb_hcd *hcd)
 		ehci_writel(ehci, temp, &ehci->regs->port_status [i]);
 	}
 
-	/* msleep for 20ms only if code is trying to resume port */
+	/*
+	 * msleep for USB_RESUME_TIMEOUT ms only if code is trying to resume
+	 * port
+	 */
 	if (resume_needed) {
 		spin_unlock_irq(&ehci->lock);
-		msleep(20);
+		msleep(USB_RESUME_TIMEOUT);
 		spin_lock_irq(&ehci->lock);
 		if (ehci->shutdown)
 			goto shutdown;
@@ -688,7 +691,7 @@ ehci_hub_descriptor (
 	int		ports = HCS_N_PORTS (ehci->hcs_params);
 	u16		temp;
 
-	desc->bDescriptorType = 0x29;
+	desc->bDescriptorType = USB_DT_HUB;
 	desc->bPwrOn2PwrGood = 10;	/* ehci 1.0, 2.3.9 says 20ms max */
 	desc->bHubContrCurrent = 0;
 
@@ -942,7 +945,7 @@ int ehci_hub_control(
 			temp &= ~PORT_WAKE_BITS;
 			ehci_writel(ehci, temp | PORT_RESUME, status_reg);
 			ehci->reset_done[wIndex] = jiffies
-					+ msecs_to_jiffies(20);
+					+ msecs_to_jiffies(USB_RESUME_TIMEOUT);
 			set_bit(wIndex, &ehci->resuming_ports);
 			usb_hcd_start_port_resume(&hcd->self, wIndex);
 			break;
diff --git a/drivers/usb/host/ehci-orion.c b/drivers/usb/host/ehci-orion.c
index f6eafecab15c..bfcbb9aa8816 100644
--- a/drivers/usb/host/ehci-orion.c
+++ b/drivers/usb/host/ehci-orion.c
@@ -29,7 +29,13 @@
 #define wrl(off, val)	writel_relaxed((val), hcd->regs + (off))
 
 #define USB_CMD			0x140
+#define   USB_CMD_RUN		BIT(0)
+#define   USB_CMD_RESET		BIT(1)
 #define USB_MODE		0x1a8
+#define   USB_MODE_MASK		GENMASK(1, 0)
+#define   USB_MODE_DEVICE	0x2
+#define   USB_MODE_HOST		0x3
+#define   USB_MODE_SDIS		BIT(4)
 #define USB_CAUSE		0x310
 #define USB_MASK		0x314
 #define USB_WINDOW_CTRL(i)	(0x320 + ((i) << 4))
@@ -69,8 +75,8 @@ static void orion_usb_phy_v1_setup(struct usb_hcd *hcd)
 	/*
 	 * Reset controller
 	 */
-	wrl(USB_CMD, rdl(USB_CMD) | 0x2);
-	while (rdl(USB_CMD) & 0x2);
+	wrl(USB_CMD, rdl(USB_CMD) | USB_CMD_RESET);
+	while (rdl(USB_CMD) & USB_CMD_RESET);
 
 	/*
 	 * GL# USB-10: Set IPG for non start of frame packets
@@ -112,16 +118,16 @@ static void orion_usb_phy_v1_setup(struct usb_hcd *hcd)
 	/*
 	 * Stop and reset controller
 	 */
-	wrl(USB_CMD, rdl(USB_CMD) & ~0x1);
-	wrl(USB_CMD, rdl(USB_CMD) | 0x2);
-	while (rdl(USB_CMD) & 0x2);
+	wrl(USB_CMD, rdl(USB_CMD) & ~USB_CMD_RUN);
+	wrl(USB_CMD, rdl(USB_CMD) | USB_CMD_RESET);
+	while (rdl(USB_CMD) & USB_CMD_RESET);
 
 	/*
 	 * GL# USB-5 Streaming disable REG_USB_MODE[4]=1
 	 * TBD: This need to be done after each reset!
 	 * GL# USB-4 Setup USB Host mode
 	 */
-	wrl(USB_MODE, 0x13);
+	wrl(USB_MODE, USB_MODE_SDIS | USB_MODE_HOST);
 }
 
 static void
diff --git a/drivers/usb/host/fhci-hub.c b/drivers/usb/host/fhci-hub.c
index 70116a65262c..3bacdd7befe9 100644
--- a/drivers/usb/host/fhci-hub.c
+++ b/drivers/usb/host/fhci-hub.c
@@ -30,7 +30,7 @@
 /* virtual root hub specific descriptor */
 static u8 root_hub_des[] = {
 	0x09, /* blength */
-	0x29, /* bDescriptorType;hub-descriptor */
+	USB_DT_HUB, /* bDescriptorType;hub-descriptor */
 	0x01, /* bNbrPorts */
 	HUB_CHAR_INDV_PORT_LPSM | HUB_CHAR_NO_OCPM, /* wHubCharacteristics */
 	0x00, /* per-port power, no overcurrent */
diff --git a/drivers/usb/host/fotg210-hcd.c b/drivers/usb/host/fotg210-hcd.c
index 475b21fd373b..000ed80ab592 100644
--- a/drivers/usb/host/fotg210-hcd.c
+++ b/drivers/usb/host/fotg210-hcd.c
@@ -1509,7 +1509,7 @@ fotg210_hub_descriptor(
 	int		ports = HCS_N_PORTS(fotg210->hcs_params);
 	u16		temp;
 
-	desc->bDescriptorType = 0x29;
+	desc->bDescriptorType = USB_DT_HUB;
 	desc->bPwrOn2PwrGood = 10;	/* fotg210 1.0, 2.3.9 says 20ms max */
 	desc->bHubContrCurrent = 0;
 
@@ -1595,7 +1595,7 @@ static int fotg210_hub_control(
 			/* resume signaling for 20 msec */
 			fotg210_writel(fotg210, temp | PORT_RESUME, status_reg);
 			fotg210->reset_done[wIndex] = jiffies
-					+ msecs_to_jiffies(20);
+					+ msecs_to_jiffies(USB_RESUME_TIMEOUT);
 			break;
 		case USB_PORT_FEAT_C_SUSPEND:
 			clear_bit(wIndex, &fotg210->port_c_suspend);
diff --git a/drivers/usb/host/fusbh200-hcd.c b/drivers/usb/host/fusbh200-hcd.c
index a83eefefffda..00e492eaba6a 100644
--- a/drivers/usb/host/fusbh200-hcd.c
+++ b/drivers/usb/host/fusbh200-hcd.c
@@ -1467,7 +1467,7 @@ fusbh200_hub_descriptor (
 	int		ports = HCS_N_PORTS (fusbh200->hcs_params);
 	u16		temp;
 
-	desc->bDescriptorType = 0x29;
+	desc->bDescriptorType = USB_DT_HUB;
 	desc->bPwrOn2PwrGood = 10;	/* fusbh200 1.0, 2.3.9 says 20ms max */
 	desc->bHubContrCurrent = 0;
 
@@ -1550,10 +1550,9 @@ static int fusbh200_hub_control (
 			if ((temp & PORT_PE) == 0)
 				goto error;
 
-			/* resume signaling for 20 msec */
 			fusbh200_writel(fusbh200, temp | PORT_RESUME, status_reg);
 			fusbh200->reset_done[wIndex] = jiffies
-					+ msecs_to_jiffies(20);
+					+ msecs_to_jiffies(USB_RESUME_TIMEOUT);
 			break;
 		case USB_PORT_FEAT_C_SUSPEND:
 			clear_bit(wIndex, &fusbh200->port_c_suspend);
diff --git a/drivers/usb/host/imx21-hcd.c b/drivers/usb/host/imx21-hcd.c
index 6a2ad550b120..f542045dc2a6 100644
--- a/drivers/usb/host/imx21-hcd.c
+++ b/drivers/usb/host/imx21-hcd.c
@@ -1474,7 +1474,7 @@ static int get_hub_descriptor(struct usb_hcd *hcd,
 			      struct usb_hub_descriptor *desc)
 {
 	struct imx21 *imx21 = hcd_to_imx21(hcd);
-	desc->bDescriptorType = 0x29;	/* HUB descriptor */
+	desc->bDescriptorType = USB_DT_HUB; /* HUB descriptor */
 	desc->bHubContrCurrent = 0;
 
 	desc->bNbrPorts = readl(imx21->regs + USBH_ROOTHUBA)
diff --git a/drivers/usb/host/isp116x-hcd.c b/drivers/usb/host/isp116x-hcd.c
index 113d0cc6cc43..13181dcd9820 100644
--- a/drivers/usb/host/isp116x-hcd.c
+++ b/drivers/usb/host/isp116x-hcd.c
@@ -943,7 +943,7 @@ static void isp116x_hub_descriptor(struct isp116x *isp116x,
 {
 	u32 reg = isp116x->rhdesca;
 
-	desc->bDescriptorType = 0x29;
+	desc->bDescriptorType = USB_DT_HUB;
 	desc->bDescLength = 9;
 	desc->bHubContrCurrent = 0;
 	desc->bNbrPorts = (u8) (reg & 0x3);
@@ -1490,7 +1490,7 @@ static int isp116x_bus_resume(struct usb_hcd *hcd)
 	spin_unlock_irq(&isp116x->lock);
 
 	hcd->state = HC_STATE_RESUMING;
-	msleep(20);
+	msleep(USB_RESUME_TIMEOUT);
 
 	/* Go operational */
 	spin_lock_irq(&isp116x->lock);
diff --git a/drivers/usb/host/isp1362-hcd.c b/drivers/usb/host/isp1362-hcd.c
index b32ab60cad1e..6cf82ee460a6 100644
--- a/drivers/usb/host/isp1362-hcd.c
+++ b/drivers/usb/host/isp1362-hcd.c
@@ -1538,7 +1538,7 @@ static void isp1362_hub_descriptor(struct isp1362_hcd *isp1362_hcd,
 
 	DBG(3, "%s: enter\n", __func__);
 
-	desc->bDescriptorType = 0x29;
+	desc->bDescriptorType = USB_DT_HUB;
 	desc->bDescLength = 9;
 	desc->bHubContrCurrent = 0;
 	desc->bNbrPorts = reg & 0x3;
diff --git a/drivers/usb/host/max3421-hcd.c b/drivers/usb/host/max3421-hcd.c
index a98833cbfcf3..fc1fd403973a 100644
--- a/drivers/usb/host/max3421-hcd.c
+++ b/drivers/usb/host/max3421-hcd.c
@@ -1659,7 +1659,7 @@ hub_descriptor(struct usb_hub_descriptor *desc)
 	/*
 	 * See Table 11-13: Hub Descriptor in USB 2.0 spec.
 	 */
-	desc->bDescriptorType = 0x29;	/* hub descriptor */
+	desc->bDescriptorType = USB_DT_HUB; /* hub descriptor */
 	desc->bDescLength = 9;
 	desc->wHubCharacteristics = cpu_to_le16(HUB_CHAR_INDV_PORT_LPSM |
 						HUB_CHAR_COMMON_OCPM);
diff --git a/drivers/usb/host/ohci-at91.c b/drivers/usb/host/ohci-at91.c
index 7cce85a1f7dc..15df00cceed9 100644
--- a/drivers/usb/host/ohci-at91.c
+++ b/drivers/usb/host/ohci-at91.c
@@ -39,7 +39,6 @@
 struct ohci_at91_priv {
 	struct clk *iclk;
 	struct clk *fclk;
-	struct clk *uclk;
 	struct clk *hclk;
 	bool clocked;
 	bool wakeup;		/* Saved wake-up state for resume */
@@ -64,10 +63,8 @@ static void at91_start_clock(struct ohci_at91_priv *ohci_at91)
 {
 	if (ohci_at91->clocked)
 		return;
-	if (IS_ENABLED(CONFIG_COMMON_CLK)) {
-		clk_set_rate(ohci_at91->uclk, 48000000);
-		clk_prepare_enable(ohci_at91->uclk);
-	}
+
+	clk_set_rate(ohci_at91->fclk, 48000000);
 	clk_prepare_enable(ohci_at91->hclk);
 	clk_prepare_enable(ohci_at91->iclk);
 	clk_prepare_enable(ohci_at91->fclk);
@@ -78,11 +75,10 @@ static void at91_stop_clock(struct ohci_at91_priv *ohci_at91)
 {
 	if (!ohci_at91->clocked)
 		return;
+
 	clk_disable_unprepare(ohci_at91->fclk);
 	clk_disable_unprepare(ohci_at91->iclk);
 	clk_disable_unprepare(ohci_at91->hclk);
-	if (IS_ENABLED(CONFIG_COMMON_CLK))
-		clk_disable_unprepare(ohci_at91->uclk);
 	ohci_at91->clocked = false;
 }
 
@@ -191,14 +187,6 @@ static int usb_hcd_at91_probe(const struct hc_driver *driver,
 		retval = PTR_ERR(ohci_at91->hclk);
 		goto err;
 	}
-	if (IS_ENABLED(CONFIG_COMMON_CLK)) {
-		ohci_at91->uclk = devm_clk_get(dev, "usb_clk");
-		if (IS_ERR(ohci_at91->uclk)) {
-			dev_err(dev, "failed to get uclk\n");
-			retval = PTR_ERR(ohci_at91->uclk);
-			goto err;
-		}
-	}
 
 	board = hcd->self.controller->platform_data;
 	ohci = hcd_to_ohci(hcd);
diff --git a/drivers/usb/host/ohci-hub.c b/drivers/usb/host/ohci-hub.c
index fe2aedd8a54d..ed678c17c4ea 100644
--- a/drivers/usb/host/ohci-hub.c
+++ b/drivers/usb/host/ohci-hub.c
@@ -536,7 +536,7 @@ ohci_hub_descriptor (
 	u32		rh = roothub_a (ohci);
 	u16		temp;
 
-	desc->bDescriptorType = 0x29;
+	desc->bDescriptorType = USB_DT_HUB;
 	desc->bPwrOn2PwrGood = (rh & RH_A_POTPGT) >> 24;
 	desc->bHubContrCurrent = 0;
 
diff --git a/drivers/usb/host/oxu210hp-hcd.c b/drivers/usb/host/oxu210hp-hcd.c
index ef7efb278b15..6352f54e65a1 100644
--- a/drivers/usb/host/oxu210hp-hcd.c
+++ b/drivers/usb/host/oxu210hp-hcd.c
@@ -445,7 +445,7 @@ static void ehci_hub_descriptor(struct oxu_hcd *oxu,
 	int ports = HCS_N_PORTS(oxu->hcs_params);
 	u16 temp;
 
-	desc->bDescriptorType = 0x29;
+	desc->bDescriptorType = USB_DT_HUB;
 	desc->bPwrOn2PwrGood = 10;	/* oxu 1.0, 2.3.9 says 20ms max */
 	desc->bHubContrCurrent = 0;
 
@@ -2500,11 +2500,12 @@ static irqreturn_t oxu210_hcd_irq(struct usb_hcd *hcd)
 					|| oxu->reset_done[i] != 0)
 				continue;
 
-			/* start 20 msec resume signaling from this port,
-			 * and make hub_wq collect PORT_STAT_C_SUSPEND to
+			/* start USB_RESUME_TIMEOUT resume signaling from this
+			 * port, and make hub_wq collect PORT_STAT_C_SUSPEND to
 			 * stop that signaling.
 			 */
-			oxu->reset_done[i] = jiffies + msecs_to_jiffies(20);
+			oxu->reset_done[i] = jiffies +
+				msecs_to_jiffies(USB_RESUME_TIMEOUT);
 			oxu_dbg(oxu, "port %d remote wakeup\n", i + 1);
 			mod_timer(&hcd->rh_timer, oxu->reset_done[i]);
 		}
diff --git a/drivers/usb/host/r8a66597-hcd.c b/drivers/usb/host/r8a66597-hcd.c
index bdc82fea0a1f..4cbd0633c5c2 100644
--- a/drivers/usb/host/r8a66597-hcd.c
+++ b/drivers/usb/host/r8a66597-hcd.c
@@ -2136,7 +2136,7 @@ static int r8a66597_hub_status_data(struct usb_hcd *hcd, char *buf)
 static void r8a66597_hub_descriptor(struct r8a66597 *r8a66597,
 				    struct usb_hub_descriptor *desc)
 {
-	desc->bDescriptorType = 0x29;
+	desc->bDescriptorType = USB_DT_HUB;
 	desc->bHubContrCurrent = 0;
 	desc->bNbrPorts = r8a66597->max_root_hub;
 	desc->bDescLength = 9;
@@ -2301,7 +2301,7 @@ static int r8a66597_bus_resume(struct usb_hcd *hcd)
 		rh->port &= ~USB_PORT_STAT_SUSPEND;
 		rh->port |= USB_PORT_STAT_C_SUSPEND << 16;
 		r8a66597_mdfy(r8a66597, RESUME, RESUME | UACT, dvstctr_reg);
-		msleep(50);
+		msleep(USB_RESUME_TIMEOUT);
 		r8a66597_mdfy(r8a66597, UACT, RESUME | UACT, dvstctr_reg);
 	}
 
diff --git a/drivers/usb/host/sl811-hcd.c b/drivers/usb/host/sl811-hcd.c
index 4f4ba1ea9e9b..fd2a11473be7 100644
--- a/drivers/usb/host/sl811-hcd.c
+++ b/drivers/usb/host/sl811-hcd.c
@@ -1091,7 +1091,7 @@ sl811h_hub_descriptor (
 ) {
 	u16		temp = 0;
 
-	desc->bDescriptorType = 0x29;
+	desc->bDescriptorType = USB_DT_HUB;
 	desc->bHubContrCurrent = 0;
 
 	desc->bNbrPorts = 1;
@@ -1259,7 +1259,7 @@ sl811h_hub_control(
 			sl811_write(sl811, SL11H_CTLREG1, sl811->ctrl1);
 
 			mod_timer(&sl811->timer, jiffies
-					+ msecs_to_jiffies(20));
+					+ msecs_to_jiffies(USB_RESUME_TIMEOUT));
 			break;
 		case USB_PORT_FEAT_POWER:
 			port_power(sl811, 0);
@@ -1809,7 +1809,6 @@ struct platform_driver sl811h_driver = {
 	.resume =	sl811h_resume,
 	.driver = {
 		.name =	(char *) hcd_name,
-		.owner = THIS_MODULE,
 	},
 };
 EXPORT_SYMBOL(sl811h_driver);
diff --git a/drivers/usb/host/u132-hcd.c b/drivers/usb/host/u132-hcd.c
index ad97e8a1ad1c..d51687780b61 100644
--- a/drivers/usb/host/u132-hcd.c
+++ b/drivers/usb/host/u132-hcd.c
@@ -2584,7 +2584,7 @@ static int u132_roothub_descriptor(struct u132 *u132,
 	retval = u132_read_pcimem(u132, roothub.a, &rh_a);
 	if (retval)
 		return retval;
-	desc->bDescriptorType = 0x29;
+	desc->bDescriptorType = USB_DT_HUB;
 	desc->bPwrOn2PwrGood = (rh_a & RH_A_POTPGT) >> 24;
 	desc->bHubContrCurrent = 0;
 	desc->bNbrPorts = u132->num_ports;
diff --git a/drivers/usb/host/uhci-hub.c b/drivers/usb/host/uhci-hub.c
index 19ba5eafb31e..ece9e37e89fe 100644
--- a/drivers/usb/host/uhci-hub.c
+++ b/drivers/usb/host/uhci-hub.c
@@ -15,7 +15,7 @@
 static const __u8 root_hub_hub_des[] =
 {
 	0x09,			/*  __u8  bLength; */
-	0x29,			/*  __u8  bDescriptorType; Hub-descriptor */
+	USB_DT_HUB,		/*  __u8  bDescriptorType; Hub-descriptor */
 	0x02,			/*  __u8  bNbrPorts; */
 	HUB_CHAR_NO_LPSM |	/* __u16  wHubCharacteristics; */
 		HUB_CHAR_INDV_PORT_OCPM, /* (per-port OC, no power switching) */
@@ -166,7 +166,7 @@ static void uhci_check_ports(struct uhci_hcd *uhci)
 				/* Port received a wakeup request */
 				set_bit(port, &uhci->resuming_ports);
 				uhci->ports_timeout = jiffies +
-						msecs_to_jiffies(25);
+					msecs_to_jiffies(USB_RESUME_TIMEOUT);
 				usb_hcd_start_port_resume(
 						&uhci_to_hcd(uhci)->self, port);
 
@@ -338,7 +338,8 @@ static int uhci_hub_control(struct usb_hcd *hcd, u16 typeReq, u16 wValue,
 			uhci_finish_suspend(uhci, port, port_addr);
 
 			/* USB v2.0 7.1.7.5 */
-			uhci->ports_timeout = jiffies + msecs_to_jiffies(50);
+			uhci->ports_timeout = jiffies +
+				msecs_to_jiffies(USB_RESUME_TIMEOUT);
 			break;
 		case USB_PORT_FEAT_POWER:
 			/* UHCI has no power switching */
diff --git a/drivers/usb/host/whci/hcd.c b/drivers/usb/host/whci/hcd.c
index d7b363a418de..43626c44683b 100644
--- a/drivers/usb/host/whci/hcd.c
+++ b/drivers/usb/host/whci/hcd.c
@@ -313,8 +313,7 @@ error_wusbhc_create:
 	uwb_rc_put(wusbhc->uwb_rc);
 error:
 	whc_clean_up(whc);
-	if (usb_hcd)
-		usb_put_hcd(usb_hcd);
+	usb_put_hcd(usb_hcd);
 	return ret;
 }
 
diff --git a/drivers/usb/host/xhci-hub.c b/drivers/usb/host/xhci-hub.c
index a7865c4b0498..0827d7c96527 100644
--- a/drivers/usb/host/xhci-hub.c
+++ b/drivers/usb/host/xhci-hub.c
@@ -387,6 +387,10 @@ static void xhci_clear_port_change_bit(struct xhci_hcd *xhci, u16 wValue,
 		status = PORT_PLC;
 		port_change_bit = "link state";
 		break;
+	case USB_PORT_FEAT_C_PORT_CONFIG_ERROR:
+		status = PORT_CEC;
+		port_change_bit = "config error";
+		break;
 	default:
 		/* Should never happen */
 		return;
@@ -588,6 +592,8 @@ static u32 xhci_get_port_status(struct usb_hcd *hcd,
 			status |= USB_PORT_STAT_C_LINK_STATE << 16;
 		if ((raw_port_status & PORT_WRC))
 			status |= USB_PORT_STAT_C_BH_RESET << 16;
+		if ((raw_port_status & PORT_CEC))
+			status |= USB_PORT_STAT_C_CONFIG_ERROR << 16;
 	}
 
 	if (hcd->speed != HCD_USB3) {
@@ -1005,6 +1011,7 @@ int xhci_hub_control(struct usb_hcd *hcd, u16 typeReq, u16 wValue,
 		case USB_PORT_FEAT_C_OVER_CURRENT:
 		case USB_PORT_FEAT_C_ENABLE:
 		case USB_PORT_FEAT_C_PORT_LINK_STATE:
+		case USB_PORT_FEAT_C_PORT_CONFIG_ERROR:
 			xhci_clear_port_change_bit(xhci, wValue, wIndex,
 					port_array[wIndex], temp);
 			break;
@@ -1069,7 +1076,7 @@ int xhci_hub_status_data(struct usb_hcd *hcd, char *buf)
 	 */
 	status = bus_state->resuming_ports;
 
-	mask = PORT_CSC | PORT_PEC | PORT_OCC | PORT_PLC | PORT_WRC;
+	mask = PORT_CSC | PORT_PEC | PORT_OCC | PORT_PLC | PORT_WRC | PORT_CEC;
 
 	spin_lock_irqsave(&xhci->lock, flags);
 	/* For each port, did anything change?  If so, set that bit in buf. */
diff --git a/drivers/usb/host/xhci-pci.c b/drivers/usb/host/xhci-pci.c
index 7f76c8a12f89..2af32e26fafc 100644
--- a/drivers/usb/host/xhci-pci.c
+++ b/drivers/usb/host/xhci-pci.c
@@ -37,6 +37,9 @@
 
 #define PCI_DEVICE_ID_INTEL_LYNXPOINT_XHCI	0x8c31
 #define PCI_DEVICE_ID_INTEL_LYNXPOINT_LP_XHCI	0x9c31
+#define PCI_DEVICE_ID_INTEL_CHERRYVIEW_XHCI		0x22b5
+#define PCI_DEVICE_ID_INTEL_SUNRISEPOINT_H_XHCI		0xa12f
+#define PCI_DEVICE_ID_INTEL_SUNRISEPOINT_LP_XHCI	0x9d2f
 
 static const char hcd_name[] = "xhci_hcd";
 
@@ -112,6 +115,7 @@ static void xhci_pci_quirks(struct device *dev, struct xhci_hcd *xhci)
 	if (pdev->vendor == PCI_VENDOR_ID_INTEL) {
 		xhci->quirks |= XHCI_LPM_SUPPORT;
 		xhci->quirks |= XHCI_INTEL_HOST;
+		xhci->quirks |= XHCI_AVOID_BEI;
 	}
 	if (pdev->vendor == PCI_VENDOR_ID_INTEL &&
 			pdev->device == PCI_DEVICE_ID_INTEL_PANTHERPOINT_XHCI) {
@@ -127,12 +131,17 @@ static void xhci_pci_quirks(struct device *dev, struct xhci_hcd *xhci)
 		 * PPT chipsets.
 		 */
 		xhci->quirks |= XHCI_SPURIOUS_REBOOT;
-		xhci->quirks |= XHCI_AVOID_BEI;
 	}
 	if (pdev->vendor == PCI_VENDOR_ID_INTEL &&
 		pdev->device == PCI_DEVICE_ID_INTEL_LYNXPOINT_LP_XHCI) {
 		xhci->quirks |= XHCI_SPURIOUS_REBOOT;
 	}
+	if (pdev->vendor == PCI_VENDOR_ID_INTEL &&
+		(pdev->device == PCI_DEVICE_ID_INTEL_SUNRISEPOINT_LP_XHCI ||
+		 pdev->device == PCI_DEVICE_ID_INTEL_SUNRISEPOINT_H_XHCI ||
+		 pdev->device == PCI_DEVICE_ID_INTEL_CHERRYVIEW_XHCI)) {
+		xhci->quirks |= XHCI_PME_STUCK_QUIRK;
+	}
 	if (pdev->vendor == PCI_VENDOR_ID_ETRON &&
 			pdev->device == PCI_DEVICE_ID_EJ168) {
 		xhci->quirks |= XHCI_RESET_ON_RESUME;
@@ -159,6 +168,21 @@ static void xhci_pci_quirks(struct device *dev, struct xhci_hcd *xhci)
 				"QUIRK: Resetting on resume");
 }
 
+/*
+ * Make sure PME works on some Intel xHCI controllers by writing 1 to clear
+ * the Internal PME flag bit in vendor specific PMCTRL register at offset 0x80a4
+ */
+static void xhci_pme_quirk(struct xhci_hcd *xhci)
+{
+	u32 val;
+	void __iomem *reg;
+
+	reg = (void __iomem *) xhci->cap_regs + 0x80a4;
+	val = readl(reg);
+	writel(val | BIT(28), reg);
+	readl(reg);
+}
+
 /* called during probe() after chip reset completes */
 static int xhci_pci_setup(struct usb_hcd *hcd)
 {
@@ -283,6 +307,9 @@ static int xhci_pci_suspend(struct usb_hcd *hcd, bool do_wakeup)
 	if (xhci->quirks & XHCI_COMP_MODE_QUIRK)
 		pdev->no_d3cold = true;
 
+	if (xhci->quirks & XHCI_PME_STUCK_QUIRK)
+		xhci_pme_quirk(xhci);
+
 	return xhci_suspend(xhci, do_wakeup);
 }
 
@@ -313,6 +340,9 @@ static int xhci_pci_resume(struct usb_hcd *hcd, bool hibernated)
 	if (pdev->vendor == PCI_VENDOR_ID_INTEL)
 		usb_enable_intel_xhci_ports(pdev);
 
+	if (xhci->quirks & XHCI_PME_STUCK_QUIRK)
+		xhci_pme_quirk(xhci);
+
 	retval = xhci_resume(xhci, hibernated);
 	return retval;
 }
diff --git a/drivers/usb/host/xhci-plat.c b/drivers/usb/host/xhci-plat.c
index 08d402b15482..783e819139a7 100644
--- a/drivers/usb/host/xhci-plat.c
+++ b/drivers/usb/host/xhci-plat.c
@@ -16,6 +16,7 @@
 #include <linux/module.h>
 #include <linux/of.h>
 #include <linux/platform_device.h>
+#include <linux/usb/phy.h>
 #include <linux/slab.h>
 #include <linux/usb/xhci_pdriver.h>
 
@@ -83,16 +84,6 @@ static int xhci_plat_probe(struct platform_device *pdev)
 	if (irq < 0)
 		return -ENODEV;
 
-
-	if (of_device_is_compatible(pdev->dev.of_node,
-				    "marvell,armada-375-xhci") ||
-	    of_device_is_compatible(pdev->dev.of_node,
-				    "marvell,armada-380-xhci")) {
-		ret = xhci_mvebu_mbus_init_quirk(pdev);
-		if (ret)
-			return ret;
-	}
-
 	/* Initialize dma_mask and coherent_dma_mask to 32-bits */
 	ret = dma_set_coherent_mask(&pdev->dev, DMA_BIT_MASK(32));
 	if (ret)
@@ -127,6 +118,15 @@ static int xhci_plat_probe(struct platform_device *pdev)
 			goto put_hcd;
 	}
 
+	if (of_device_is_compatible(pdev->dev.of_node,
+				    "marvell,armada-375-xhci") ||
+	    of_device_is_compatible(pdev->dev.of_node,
+				    "marvell,armada-380-xhci")) {
+		ret = xhci_mvebu_mbus_init_quirk(pdev);
+		if (ret)
+			goto disable_clk;
+	}
+
 	ret = usb_add_hcd(hcd, irq, IRQF_SHARED);
 	if (ret)
 		goto disable_clk;
@@ -156,12 +156,27 @@ static int xhci_plat_probe(struct platform_device *pdev)
 	if (HCC_MAX_PSA(xhci->hcc_params) >= 4)
 		xhci->shared_hcd->can_do_streams = 1;
 
+	hcd->usb_phy = devm_usb_get_phy_by_phandle(&pdev->dev, "usb-phy", 0);
+	if (IS_ERR(hcd->usb_phy)) {
+		ret = PTR_ERR(hcd->usb_phy);
+		if (ret == -EPROBE_DEFER)
+			goto put_usb3_hcd;
+		hcd->usb_phy = NULL;
+	} else {
+		ret = usb_phy_init(hcd->usb_phy);
+		if (ret)
+			goto put_usb3_hcd;
+	}
+
 	ret = usb_add_hcd(xhci->shared_hcd, irq, IRQF_SHARED);
 	if (ret)
-		goto put_usb3_hcd;
+		goto disable_usb_phy;
 
 	return 0;
 
+disable_usb_phy:
+	usb_phy_shutdown(hcd->usb_phy);
+
 put_usb3_hcd:
 	usb_put_hcd(xhci->shared_hcd);
 
@@ -185,6 +200,7 @@ static int xhci_plat_remove(struct platform_device *dev)
 	struct clk *clk = xhci->clk;
 
 	usb_remove_hcd(xhci->shared_hcd);
+	usb_phy_shutdown(hcd->usb_phy);
 	usb_put_hcd(xhci->shared_hcd);
 
 	usb_remove_hcd(hcd);
diff --git a/drivers/usb/host/xhci-ring.c b/drivers/usb/host/xhci-ring.c
index 88da8d629820..f5397a517c54 100644
--- a/drivers/usb/host/xhci-ring.c
+++ b/drivers/usb/host/xhci-ring.c
@@ -238,7 +238,7 @@ static void inc_enq(struct xhci_hcd *xhci, struct xhci_ring *ring,
 
 			/* Toggle the cycle bit after the last ring segment. */
 			if (last_trb_on_last_seg(xhci, ring, ring->enq_seg, next)) {
-				ring->cycle_state = (ring->cycle_state ? 0 : 1);
+				ring->cycle_state ^= 1;
 			}
 		}
 		ring->enq_seg = ring->enq_seg->next;
@@ -1574,7 +1574,7 @@ static void handle_port_status(struct xhci_hcd *xhci,
 		} else {
 			xhci_dbg(xhci, "resume HS port %d\n", port_id);
 			bus_state->resume_done[faked_port_index] = jiffies +
-				msecs_to_jiffies(20);
+				msecs_to_jiffies(USB_RESUME_TIMEOUT);
 			set_bit(faked_port_index, &bus_state->resuming_ports);
 			mod_timer(&hcd->rh_timer,
 				  bus_state->resume_done[faked_port_index]);
@@ -1946,7 +1946,7 @@ static int process_ctrl_td(struct xhci_hcd *xhci, struct xhci_td *td,
 	if (event_trb != ep_ring->dequeue) {
 		/* The event was for the status stage */
 		if (event_trb == td->last_trb) {
-			if (td->urb->actual_length != 0) {
+			if (td->urb_length_set) {
 				/* Don't overwrite a previously set error code
 				 */
 				if ((*status == -EINPROGRESS || *status == 0) &&
@@ -1960,7 +1960,13 @@ static int process_ctrl_td(struct xhci_hcd *xhci, struct xhci_td *td,
 					td->urb->transfer_buffer_length;
 			}
 		} else {
-		/* Maybe the event was for the data stage? */
+			/*
+			 * Maybe the event was for the data stage? If so, update
+			 * already the actual_length of the URB and flag it as
+			 * set, so that it is not overwritten in the event for
+			 * the last TRB.
+			 */
+			td->urb_length_set = true;
 			td->urb->actual_length =
 				td->urb->transfer_buffer_length -
 				EVENT_TRB_LEN(le32_to_cpu(event->transfer_len));
@@ -2803,7 +2809,7 @@ static int prepare_ring(struct xhci_hcd *xhci, struct xhci_ring *ep_ring,
 
 			/* Toggle the cycle bit after the last ring segment. */
 			if (last_trb_on_last_seg(xhci, ring, ring->enq_seg, next)) {
-				ring->cycle_state = (ring->cycle_state ? 0 : 1);
+				ring->cycle_state ^= 1;
 			}
 			ring->enq_seg = ring->enq_seg->next;
 			ring->enqueue = ring->enq_seg->trbs;
diff --git a/drivers/usb/host/xhci.h b/drivers/usb/host/xhci.h
index 974514762a14..8e421b89632d 100644
--- a/drivers/usb/host/xhci.h
+++ b/drivers/usb/host/xhci.h
@@ -1,3 +1,4 @@
+
 /*
  * xHCI host controller driver
  *
@@ -88,9 +89,10 @@ struct xhci_cap_regs {
 #define HCS_IST(p)		(((p) >> 0) & 0xf)
 /* bits 4:7, max number of Event Ring segments */
 #define HCS_ERST_MAX(p)		(((p) >> 4) & 0xf)
+/* bits 21:25 Hi 5 bits of Scratchpad buffers SW must allocate for the HW */
 /* bit 26 Scratchpad restore - for save/restore HW state - not used yet */
-/* bits 27:31 number of Scratchpad buffers SW must allocate for the HW */
-#define HCS_MAX_SCRATCHPAD(p)   (((p) >> 27) & 0x1f)
+/* bits 27:31 Lo 5 bits of Scratchpad buffers SW must allocate for the HW */
+#define HCS_MAX_SCRATCHPAD(p)   ((((p) >> 16) & 0x3e0) | (((p) >> 27) & 0x1f))
 
 /* HCSPARAMS3 - hcs_params3 - bitmasks */
 /* bits 0:7, Max U1 to U0 latency for the roothub ports */
@@ -1288,6 +1290,8 @@ struct xhci_td {
 	struct xhci_segment	*start_seg;
 	union xhci_trb		*first_trb;
 	union xhci_trb		*last_trb;
+	/* actual_length of the URB has already been set */
+	bool			urb_length_set;
 };
 
 /* xHCI command default timeout value */
@@ -1560,6 +1564,7 @@ struct xhci_hcd {
 #define XHCI_SPURIOUS_WAKEUP	(1 << 18)
 /* For controllers with a broken beyond repair streams implementation */
 #define XHCI_BROKEN_STREAMS	(1 << 19)
+#define XHCI_PME_STUCK_QUIRK	(1 << 20)
 	unsigned int		num_active_eps;
 	unsigned int		limit_active_eps;
 	/* There are two roothubs to keep track of bus suspend info for */
diff --git a/drivers/usb/image/mdc800.c b/drivers/usb/image/mdc800.c
index a62865af53cc..5cf2633cdb04 100644
--- a/drivers/usb/image/mdc800.c
+++ b/drivers/usb/image/mdc800.c
@@ -347,7 +347,8 @@ static int mdc800_usb_waitForIRQ (int mode, int msec)
 {
 	mdc800->camera_request_ready=1+mode;
 
-	wait_event_timeout(mdc800->irq_wait, mdc800->irq_woken, msec*HZ/1000);
+	wait_event_timeout(mdc800->irq_wait, mdc800->irq_woken,
+			   msecs_to_jiffies(msec));
 	mdc800->irq_woken = 0;
 
 	if (mdc800->camera_request_ready>0)
@@ -743,8 +744,9 @@ static ssize_t mdc800_device_read (struct file *file, char __user *buf, size_t l
 					mutex_unlock(&mdc800->io_lock);
 					return len-left;
 				}
-				wait_event_timeout(mdc800->download_wait, mdc800->downloaded,
-										TO_DOWNLOAD_GET_READY*HZ/1000);
+				wait_event_timeout(mdc800->download_wait,
+				     mdc800->downloaded,
+				     msecs_to_jiffies(TO_DOWNLOAD_GET_READY));
 				mdc800->downloaded = 0;
 				if (mdc800->download_urb->status != 0)
 				{
@@ -867,7 +869,8 @@ static ssize_t mdc800_device_write (struct file *file, const char __user *buf, s
 				mutex_unlock(&mdc800->io_lock);
 				return -EIO;
 			}
-			wait_event_timeout(mdc800->write_wait, mdc800->written, TO_WRITE_GET_READY*HZ/1000);
+			wait_event_timeout(mdc800->write_wait, mdc800->written,
+					msecs_to_jiffies(TO_WRITE_GET_READY));
 			mdc800->written = 0;
 			if (mdc800->state == WORKING)
 			{
diff --git a/drivers/usb/isp1760/isp1760-core.c b/drivers/usb/isp1760/isp1760-core.c
index b9827556455f..bfa402cf3a27 100644
--- a/drivers/usb/isp1760/isp1760-core.c
+++ b/drivers/usb/isp1760/isp1760-core.c
@@ -151,8 +151,7 @@ int isp1760_register(struct resource *mem, int irq, unsigned long irqflags,
 	}
 
 	if (IS_ENABLED(CONFIG_USB_ISP1761_UDC) && !udc_disabled) {
-		ret = isp1760_udc_register(isp, irq, irqflags | IRQF_SHARED |
-					   IRQF_DISABLED);
+		ret = isp1760_udc_register(isp, irq, irqflags);
 		if (ret < 0) {
 			isp1760_hcd_unregister(&isp->hcd);
 			return ret;
diff --git a/drivers/usb/isp1760/isp1760-hcd.c b/drivers/usb/isp1760/isp1760-hcd.c
index eba9b82e2d70..ac31d19cc54b 100644
--- a/drivers/usb/isp1760/isp1760-hcd.c
+++ b/drivers/usb/isp1760/isp1760-hcd.c
@@ -1274,7 +1274,7 @@ static void errata2_function(unsigned long data)
 	for (slot = 0; slot < 32; slot++)
 		if (priv->atl_slots[slot].qh && time_after(jiffies,
 					priv->atl_slots[slot].timestamp +
-					SLOT_TIMEOUT * HZ / 1000)) {
+					msecs_to_jiffies(SLOT_TIMEOUT))) {
 			ptd_read(hcd->regs, ATL_PTD_OFFSET, slot, &ptd);
 			if (!FROM_DW0_VALID(ptd.dw0) &&
 					!FROM_DW3_ACTIVE(ptd.dw3))
@@ -1286,7 +1286,7 @@ static void errata2_function(unsigned long data)
 
 	spin_unlock_irqrestore(&priv->lock, spinflags);
 
-	errata2_timer.expires = jiffies + SLOT_CHECK_PERIOD * HZ / 1000;
+	errata2_timer.expires = jiffies + msecs_to_jiffies(SLOT_CHECK_PERIOD);
 	add_timer(&errata2_timer);
 }
 
@@ -1336,7 +1336,7 @@ static int isp1760_run(struct usb_hcd *hcd)
 		return retval;
 
 	setup_timer(&errata2_timer, errata2_function, (unsigned long)hcd);
-	errata2_timer.expires = jiffies + SLOT_CHECK_PERIOD * HZ / 1000;
+	errata2_timer.expires = jiffies + msecs_to_jiffies(SLOT_CHECK_PERIOD);
 	add_timer(&errata2_timer);
 
 	chipid = reg_read32(hcd->regs, HC_CHIP_ID_REG);
@@ -1758,7 +1758,7 @@ static void isp1760_hub_descriptor(struct isp1760_hcd *priv,
 	int ports = HCS_N_PORTS(priv->hcs_params);
 	u16 temp;
 
-	desc->bDescriptorType = 0x29;
+	desc->bDescriptorType = USB_DT_HUB;
 	/* priv 1.0, 2.3.9 says 20ms max */
 	desc->bPwrOn2PwrGood = 10;
 	desc->bHubContrCurrent = 0;
@@ -1869,7 +1869,7 @@ static int isp1760_hub_control(struct usb_hcd *hcd, u16 typeReq,
 				reg_write32(hcd->regs, HC_PORTSC1,
 							temp | PORT_RESUME);
 				priv->reset_done = jiffies +
-					msecs_to_jiffies(20);
+					msecs_to_jiffies(USB_RESUME_TIMEOUT);
 			}
 			break;
 		case USB_PORT_FEAT_C_SUSPEND:
diff --git a/drivers/usb/isp1760/isp1760-udc.c b/drivers/usb/isp1760/isp1760-udc.c
index 9612d7990565..3fc4fe770253 100644
--- a/drivers/usb/isp1760/isp1760-udc.c
+++ b/drivers/usb/isp1760/isp1760-udc.c
@@ -1191,6 +1191,7 @@ static int isp1760_udc_start(struct usb_gadget *gadget,
 			     struct usb_gadget_driver *driver)
 {
 	struct isp1760_udc *udc = gadget_to_udc(gadget);
+	unsigned long flags;
 
 	/* The hardware doesn't support low speed. */
 	if (driver->max_speed < USB_SPEED_FULL) {
@@ -1198,17 +1199,17 @@ static int isp1760_udc_start(struct usb_gadget *gadget,
 		return -EINVAL;
 	}
 
-	spin_lock(&udc->lock);
+	spin_lock_irqsave(&udc->lock, flags);
 
 	if (udc->driver) {
 		dev_err(udc->isp->dev, "UDC already has a gadget driver\n");
-		spin_unlock(&udc->lock);
+		spin_unlock_irqrestore(&udc->lock, flags);
 		return -EBUSY;
 	}
 
 	udc->driver = driver;
 
-	spin_unlock(&udc->lock);
+	spin_unlock_irqrestore(&udc->lock, flags);
 
 	dev_dbg(udc->isp->dev, "starting UDC with driver %s\n",
 		driver->function);
@@ -1232,6 +1233,7 @@ static int isp1760_udc_start(struct usb_gadget *gadget,
 static int isp1760_udc_stop(struct usb_gadget *gadget)
 {
 	struct isp1760_udc *udc = gadget_to_udc(gadget);
+	unsigned long flags;
 
 	dev_dbg(udc->isp->dev, "%s\n", __func__);
 
@@ -1239,9 +1241,9 @@ static int isp1760_udc_stop(struct usb_gadget *gadget)
 
 	isp1760_udc_write(udc, DC_MODE, 0);
 
-	spin_lock(&udc->lock);
+	spin_lock_irqsave(&udc->lock, flags);
 	udc->driver = NULL;
-	spin_unlock(&udc->lock);
+	spin_unlock_irqrestore(&udc->lock, flags);
 
 	return 0;
 }
@@ -1411,7 +1413,7 @@ static int isp1760_udc_init(struct isp1760_udc *udc)
 		return -ENODEV;
 	}
 
-	if (chipid != 0x00011582) {
+	if (chipid != 0x00011582 && chipid != 0x00158210) {
 		dev_err(udc->isp->dev, "udc: invalid chip ID 0x%08x\n", chipid);
 		return -ENODEV;
 	}
@@ -1451,8 +1453,8 @@ int isp1760_udc_register(struct isp1760_device *isp, int irq,
 
 	sprintf(udc->irqname, "%s (udc)", devname);
 
-	ret = request_irq(irq, isp1760_udc_irq, IRQF_SHARED | IRQF_DISABLED |
-			  irqflags, udc->irqname, udc);
+	ret = request_irq(irq, isp1760_udc_irq, IRQF_SHARED | irqflags,
+			  udc->irqname, udc);
 	if (ret < 0)
 		goto error;
 
diff --git a/drivers/usb/misc/Kconfig b/drivers/usb/misc/Kconfig
index 76d77206e011..f7a7fc21be8a 100644
--- a/drivers/usb/misc/Kconfig
+++ b/drivers/usb/misc/Kconfig
@@ -255,3 +255,16 @@ config USB_LINK_LAYER_TEST
 	  This driver is for generating specific traffic for Super Speed Link
 	  Layer Test Device. Say Y only when you want to conduct USB Super Speed
 	  Link Layer Test for host controllers.
+
+config USB_CHAOSKEY
+	tristate "ChaosKey random number generator driver support"
+	depends on HW_RANDOM
+	help
+	  Say Y here if you want to connect an AltusMetrum ChaosKey to
+	  your computer's USB port. The ChaosKey is a hardware random
+	  number generator which hooks into the kernel entropy pool to
+	  ensure a large supply of entropy for /dev/random and
+	  /dev/urandom and also provides direct access via /dev/chaoskeyX
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called chaoskey.
diff --git a/drivers/usb/misc/Makefile b/drivers/usb/misc/Makefile
index 65b0402c1ca1..45fd4ac39d3e 100644
--- a/drivers/usb/misc/Makefile
+++ b/drivers/usb/misc/Makefile
@@ -25,6 +25,7 @@ obj-$(CONFIG_USB_USS720)		+= uss720.o
 obj-$(CONFIG_USB_SEVSEG)		+= usbsevseg.o
 obj-$(CONFIG_USB_YUREX)			+= yurex.o
 obj-$(CONFIG_USB_HSIC_USB3503)		+= usb3503.o
+obj-$(CONFIG_USB_CHAOSKEY)		+= chaoskey.o
 
 obj-$(CONFIG_USB_SISUSBVGA)		+= sisusbvga/
 obj-$(CONFIG_USB_LINK_LAYER_TEST)	+= lvstest.o
diff --git a/drivers/usb/misc/appledisplay.c b/drivers/usb/misc/appledisplay.c
index b3d245ef46ef..a0a3827b4aff 100644
--- a/drivers/usb/misc/appledisplay.c
+++ b/drivers/usb/misc/appledisplay.c
@@ -329,7 +329,7 @@ error:
 					pdata->urbdata, pdata->urb->transfer_dma);
 			usb_free_urb(pdata->urb);
 		}
-		if (pdata->bd && !IS_ERR(pdata->bd))
+		if (!IS_ERR(pdata->bd))
 			backlight_device_unregister(pdata->bd);
 		kfree(pdata->msgdata);
 	}
diff --git a/drivers/usb/misc/chaoskey.c b/drivers/usb/misc/chaoskey.c
new file mode 100644
index 000000000000..3ad5d19e4d04
--- /dev/null
+++ b/drivers/usb/misc/chaoskey.c
@@ -0,0 +1,532 @@
+/*
+ * chaoskey - driver for ChaosKey device from Altus Metrum.
+ *
+ * This device provides true random numbers using a noise source based
+ * on a reverse-biased p-n junction in avalanche breakdown. More
+ * details can be found at http://chaoskey.org
+ *
+ * The driver connects to the kernel hardware RNG interface to provide
+ * entropy for /dev/random and other kernel activities. It also offers
+ * a separate /dev/ entry to allow for direct access to the random
+ * bit stream.
+ *
+ * Copyright © 2015 Keith Packard <keithp@keithp.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.
+ */
+
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/usb.h>
+#include <linux/wait.h>
+#include <linux/hw_random.h>
+#include <linux/mutex.h>
+#include <linux/uaccess.h>
+
+static struct usb_driver chaoskey_driver;
+static struct usb_class_driver chaoskey_class;
+static int chaoskey_rng_read(struct hwrng *rng, void *data,
+			     size_t max, bool wait);
+
+#define usb_dbg(usb_if, format, arg...) \
+	dev_dbg(&(usb_if)->dev, format, ## arg)
+
+#define usb_err(usb_if, format, arg...) \
+	dev_err(&(usb_if)->dev, format, ## arg)
+
+/* Version Information */
+#define DRIVER_VERSION	"v0.1"
+#define DRIVER_AUTHOR	"Keith Packard, keithp@keithp.com"
+#define DRIVER_DESC	"Altus Metrum ChaosKey driver"
+#define DRIVER_SHORT	"chaoskey"
+
+MODULE_VERSION(DRIVER_VERSION);
+MODULE_AUTHOR(DRIVER_AUTHOR);
+MODULE_DESCRIPTION(DRIVER_DESC);
+MODULE_LICENSE("GPL");
+
+#define CHAOSKEY_VENDOR_ID	0x1d50	/* OpenMoko */
+#define CHAOSKEY_PRODUCT_ID	0x60c6	/* ChaosKey */
+
+#define CHAOSKEY_BUF_LEN	64	/* max size of USB full speed packet */
+
+#define NAK_TIMEOUT (HZ)		/* stall/wait timeout for device */
+
+#ifdef CONFIG_USB_DYNAMIC_MINORS
+#define USB_CHAOSKEY_MINOR_BASE 0
+#else
+
+/* IOWARRIOR_MINOR_BASE + 16, not official yet */
+#define USB_CHAOSKEY_MINOR_BASE 224
+#endif
+
+static const struct usb_device_id chaoskey_table[] = {
+	{ USB_DEVICE(CHAOSKEY_VENDOR_ID, CHAOSKEY_PRODUCT_ID) },
+	{ },
+};
+MODULE_DEVICE_TABLE(usb, chaoskey_table);
+
+/* Driver-local specific stuff */
+struct chaoskey {
+	struct usb_interface *interface;
+	char in_ep;
+	struct mutex lock;
+	struct mutex rng_lock;
+	int open;			/* open count */
+	int present;			/* device not disconnected */
+	int size;			/* size of buf */
+	int valid;			/* bytes of buf read */
+	int used;			/* bytes of buf consumed */
+	char *name;			/* product + serial */
+	struct hwrng hwrng;		/* Embedded struct for hwrng */
+	int hwrng_registered;		/* registered with hwrng API */
+	wait_queue_head_t wait_q;	/* for timeouts */
+	char *buf;
+};
+
+static void chaoskey_free(struct chaoskey *dev)
+{
+	usb_dbg(dev->interface, "free");
+	kfree(dev->name);
+	kfree(dev->buf);
+	kfree(dev);
+}
+
+static int chaoskey_probe(struct usb_interface *interface,
+			  const struct usb_device_id *id)
+{
+	struct usb_device *udev = interface_to_usbdev(interface);
+	struct usb_host_interface *altsetting = interface->cur_altsetting;
+	int i;
+	int in_ep = -1;
+	struct chaoskey *dev;
+	int result;
+	int size;
+
+	usb_dbg(interface, "probe %s-%s", udev->product, udev->serial);
+
+	/* Find the first bulk IN endpoint and its packet size */
+	for (i = 0; i < altsetting->desc.bNumEndpoints; i++) {
+		if (usb_endpoint_is_bulk_in(&altsetting->endpoint[i].desc)) {
+			in_ep = usb_endpoint_num(&altsetting->endpoint[i].desc);
+			size = usb_endpoint_maxp(&altsetting->endpoint[i].desc);
+			break;
+		}
+	}
+
+	/* Validate endpoint and size */
+	if (in_ep == -1) {
+		usb_dbg(interface, "no IN endpoint found");
+		return -ENODEV;
+	}
+	if (size <= 0) {
+		usb_dbg(interface, "invalid size (%d)", size);
+		return -ENODEV;
+	}
+
+	if (size > CHAOSKEY_BUF_LEN) {
+		usb_dbg(interface, "size reduced from %d to %d\n",
+			size, CHAOSKEY_BUF_LEN);
+		size = CHAOSKEY_BUF_LEN;
+	}
+
+	/* Looks good, allocate and initialize */
+
+	dev = kzalloc(sizeof(struct chaoskey), GFP_KERNEL);
+
+	if (dev == NULL)
+		return -ENOMEM;
+
+	dev->buf = kmalloc(size, GFP_KERNEL);
+
+	if (dev->buf == NULL) {
+		kfree(dev);
+		return -ENOMEM;
+	}
+
+	/* Construct a name using the product and serial values. Each
+	 * device needs a unique name for the hwrng code
+	 */
+
+	if (udev->product && udev->serial) {
+		dev->name = kmalloc(strlen(udev->product) + 1 +
+				    strlen(udev->serial) + 1, GFP_KERNEL);
+		if (dev->name == NULL) {
+			kfree(dev->buf);
+			kfree(dev);
+			return -ENOMEM;
+		}
+
+		strcpy(dev->name, udev->product);
+		strcat(dev->name, "-");
+		strcat(dev->name, udev->serial);
+	}
+
+	dev->interface = interface;
+
+	dev->in_ep = in_ep;
+
+	dev->size = size;
+	dev->present = 1;
+
+	init_waitqueue_head(&dev->wait_q);
+
+	mutex_init(&dev->lock);
+	mutex_init(&dev->rng_lock);
+
+	usb_set_intfdata(interface, dev);
+
+	result = usb_register_dev(interface, &chaoskey_class);
+	if (result) {
+		usb_err(interface, "Unable to allocate minor number.");
+		usb_set_intfdata(interface, NULL);
+		chaoskey_free(dev);
+		return result;
+	}
+
+	dev->hwrng.name = dev->name ? dev->name : chaoskey_driver.name;
+	dev->hwrng.read = chaoskey_rng_read;
+
+	/* Set the 'quality' metric.  Quality is measured in units of
+	 * 1/1024's of a bit ("mills"). This should be set to 1024,
+	 * but there is a bug in the hwrng core which masks it with
+	 * 1023.
+	 *
+	 * The patch that has been merged to the crypto development
+	 * tree for that bug limits the value to 1024 at most, so by
+	 * setting this to 1024 + 1023, we get 1023 before the fix is
+	 * merged and 1024 afterwards. We'll patch this driver once
+	 * both bits of code are in the same tree.
+	 */
+	dev->hwrng.quality = 1024 + 1023;
+
+	dev->hwrng_registered = (hwrng_register(&dev->hwrng) == 0);
+	if (!dev->hwrng_registered)
+		usb_err(interface, "Unable to register with hwrng");
+
+	usb_enable_autosuspend(udev);
+
+	usb_dbg(interface, "chaoskey probe success, size %d", dev->size);
+	return 0;
+}
+
+static void chaoskey_disconnect(struct usb_interface *interface)
+{
+	struct chaoskey	*dev;
+
+	usb_dbg(interface, "disconnect");
+	dev = usb_get_intfdata(interface);
+	if (!dev) {
+		usb_dbg(interface, "disconnect failed - no dev");
+		return;
+	}
+
+	if (dev->hwrng_registered)
+		hwrng_unregister(&dev->hwrng);
+
+	usb_deregister_dev(interface, &chaoskey_class);
+
+	usb_set_intfdata(interface, NULL);
+	mutex_lock(&dev->lock);
+
+	dev->present = 0;
+
+	if (!dev->open) {
+		mutex_unlock(&dev->lock);
+		chaoskey_free(dev);
+	} else
+		mutex_unlock(&dev->lock);
+
+	usb_dbg(interface, "disconnect done");
+}
+
+static int chaoskey_open(struct inode *inode, struct file *file)
+{
+	struct chaoskey *dev;
+	struct usb_interface *interface;
+
+	/* get the interface from minor number and driver information */
+	interface = usb_find_interface(&chaoskey_driver, iminor(inode));
+	if (!interface)
+		return -ENODEV;
+
+	usb_dbg(interface, "open");
+
+	dev = usb_get_intfdata(interface);
+	if (!dev) {
+		usb_dbg(interface, "open (dev)");
+		return -ENODEV;
+	}
+
+	file->private_data = dev;
+	mutex_lock(&dev->lock);
+	++dev->open;
+	mutex_unlock(&dev->lock);
+
+	usb_dbg(interface, "open success");
+	return 0;
+}
+
+static int chaoskey_release(struct inode *inode, struct file *file)
+{
+	struct chaoskey *dev = file->private_data;
+	struct usb_interface *interface;
+
+	if (dev == NULL)
+		return -ENODEV;
+
+	interface = dev->interface;
+
+	usb_dbg(interface, "release");
+
+	mutex_lock(&dev->lock);
+
+	usb_dbg(interface, "open count at release is %d", dev->open);
+
+	if (dev->open <= 0) {
+		usb_dbg(interface, "invalid open count (%d)", dev->open);
+		mutex_unlock(&dev->lock);
+		return -ENODEV;
+	}
+
+	--dev->open;
+
+	if (!dev->present) {
+		if (dev->open == 0) {
+			mutex_unlock(&dev->lock);
+			chaoskey_free(dev);
+		} else
+			mutex_unlock(&dev->lock);
+	} else
+		mutex_unlock(&dev->lock);
+
+	usb_dbg(interface, "release success");
+	return 0;
+}
+
+/* Fill the buffer. Called with dev->lock held
+ */
+static int _chaoskey_fill(struct chaoskey *dev)
+{
+	DEFINE_WAIT(wait);
+	int result;
+	int this_read;
+	struct usb_device *udev = interface_to_usbdev(dev->interface);
+
+	usb_dbg(dev->interface, "fill");
+
+	/* Return immediately if someone called before the buffer was
+	 * empty */
+	if (dev->valid != dev->used) {
+		usb_dbg(dev->interface, "not empty yet (valid %d used %d)",
+			dev->valid, dev->used);
+		return 0;
+	}
+
+	/* Bail if the device has been removed */
+	if (!dev->present) {
+		usb_dbg(dev->interface, "device not present");
+		return -ENODEV;
+	}
+
+	/* Make sure the device is awake */
+	result = usb_autopm_get_interface(dev->interface);
+	if (result) {
+		usb_dbg(dev->interface, "wakeup failed (result %d)", result);
+		return result;
+	}
+
+	result = usb_bulk_msg(udev,
+			      usb_rcvbulkpipe(udev, dev->in_ep),
+			      dev->buf, dev->size, &this_read,
+			      NAK_TIMEOUT);
+
+	/* Let the device go back to sleep eventually */
+	usb_autopm_put_interface(dev->interface);
+
+	if (result == 0) {
+		dev->valid = this_read;
+		dev->used = 0;
+	}
+
+	usb_dbg(dev->interface, "bulk_msg result %d this_read %d",
+		result, this_read);
+
+	return result;
+}
+
+static ssize_t chaoskey_read(struct file *file,
+			     char __user *buffer,
+			     size_t count,
+			     loff_t *ppos)
+{
+	struct chaoskey *dev;
+	ssize_t read_count = 0;
+	int this_time;
+	int result = 0;
+	unsigned long remain;
+
+	dev = file->private_data;
+
+	if (dev == NULL || !dev->present)
+		return -ENODEV;
+
+	usb_dbg(dev->interface, "read %zu", count);
+
+	while (count > 0) {
+
+		/* Grab the rng_lock briefly to ensure that the hwrng interface
+		 * gets priority over other user access
+		 */
+		result = mutex_lock_interruptible(&dev->rng_lock);
+		if (result)
+			goto bail;
+		mutex_unlock(&dev->rng_lock);
+
+		result = mutex_lock_interruptible(&dev->lock);
+		if (result)
+			goto bail;
+		if (dev->valid == dev->used) {
+			result = _chaoskey_fill(dev);
+			if (result) {
+				mutex_unlock(&dev->lock);
+				goto bail;
+			}
+
+			/* Read returned zero bytes */
+			if (dev->used == dev->valid) {
+				mutex_unlock(&dev->lock);
+				goto bail;
+			}
+		}
+
+		this_time = dev->valid - dev->used;
+		if (this_time > count)
+			this_time = count;
+
+		remain = copy_to_user(buffer, dev->buf + dev->used, this_time);
+		if (remain) {
+			result = -EFAULT;
+
+			/* Consume the bytes that were copied so we don't leak
+			 * data to user space
+			 */
+			dev->used += this_time - remain;
+			mutex_unlock(&dev->lock);
+			goto bail;
+		}
+
+		count -= this_time;
+		read_count += this_time;
+		buffer += this_time;
+		dev->used += this_time;
+		mutex_unlock(&dev->lock);
+	}
+bail:
+	if (read_count) {
+		usb_dbg(dev->interface, "read %zu bytes", read_count);
+		return read_count;
+	}
+	usb_dbg(dev->interface, "empty read, result %d", result);
+	return result;
+}
+
+static int chaoskey_rng_read(struct hwrng *rng, void *data,
+			     size_t max, bool wait)
+{
+	struct chaoskey *dev = container_of(rng, struct chaoskey, hwrng);
+	int this_time;
+
+	usb_dbg(dev->interface, "rng_read max %zu wait %d", max, wait);
+
+	if (!dev->present) {
+		usb_dbg(dev->interface, "device not present");
+		return 0;
+	}
+
+	/* Hold the rng_lock until we acquire the device lock so that
+	 * this operation gets priority over other user access to the
+	 * device
+	 */
+	mutex_lock(&dev->rng_lock);
+
+	mutex_lock(&dev->lock);
+
+	mutex_unlock(&dev->rng_lock);
+
+	/* Try to fill the buffer if empty. It doesn't actually matter
+	 * if _chaoskey_fill works; we'll just return zero bytes as
+	 * the buffer will still be empty
+	 */
+	if (dev->valid == dev->used)
+		(void) _chaoskey_fill(dev);
+
+	this_time = dev->valid - dev->used;
+	if (this_time > max)
+		this_time = max;
+
+	memcpy(data, dev->buf, this_time);
+
+	dev->used += this_time;
+
+	mutex_unlock(&dev->lock);
+
+	usb_dbg(dev->interface, "rng_read this_time %d\n", this_time);
+	return this_time;
+}
+
+#ifdef CONFIG_PM
+static int chaoskey_suspend(struct usb_interface *interface,
+			    pm_message_t message)
+{
+	usb_dbg(interface, "suspend");
+	return 0;
+}
+
+static int chaoskey_resume(struct usb_interface *interface)
+{
+	usb_dbg(interface, "resume");
+	return 0;
+}
+#else
+#define chaoskey_suspend NULL
+#define chaoskey_resume NULL
+#endif
+
+/* file operation pointers */
+static const struct file_operations chaoskey_fops = {
+	.owner = THIS_MODULE,
+	.read = chaoskey_read,
+	.open = chaoskey_open,
+	.release = chaoskey_release,
+	.llseek = default_llseek,
+};
+
+/* class driver information */
+static struct usb_class_driver chaoskey_class = {
+	.name = "chaoskey%d",
+	.fops = &chaoskey_fops,
+	.minor_base = USB_CHAOSKEY_MINOR_BASE,
+};
+
+/* usb specific object needed to register this driver with the usb subsystem */
+static struct usb_driver chaoskey_driver = {
+	.name = DRIVER_SHORT,
+	.probe = chaoskey_probe,
+	.disconnect = chaoskey_disconnect,
+	.suspend = chaoskey_suspend,
+	.resume = chaoskey_resume,
+	.reset_resume = chaoskey_resume,
+	.id_table = chaoskey_table,
+	.supports_autosuspend = 1,
+};
+
+module_usb_driver(chaoskey_driver);
+
diff --git a/drivers/usb/misc/legousbtower.c b/drivers/usb/misc/legousbtower.c
index 97cd9e24bd25..7771be3ac178 100644
--- a/drivers/usb/misc/legousbtower.c
+++ b/drivers/usb/misc/legousbtower.c
@@ -574,7 +574,7 @@ static ssize_t tower_read (struct file *file, char __user *buffer, size_t count,
 	}
 
 	if (read_timeout) {
-		timeout = jiffies + read_timeout * HZ / 1000;
+		timeout = jiffies + msecs_to_jiffies(read_timeout);
 	}
 
 	/* wait for data */
@@ -592,7 +592,7 @@ static ssize_t tower_read (struct file *file, char __user *buffer, size_t count,
 		/* reset read timeout during read or write activity */
 		if (read_timeout
 		    && (dev->read_buffer_length || dev->interrupt_out_busy)) {
-			timeout = jiffies + read_timeout * HZ / 1000;
+			timeout = jiffies + msecs_to_jiffies(read_timeout);
 		}
 		/* check for read timeout */
 		if (read_timeout && time_after (jiffies, timeout)) {
@@ -831,7 +831,7 @@ static int tower_probe (struct usb_interface *interface, const struct usb_device
 	dev->read_buffer_length = 0;
 	dev->read_packet_length = 0;
 	spin_lock_init (&dev->read_buffer_lock);
-	dev->packet_timeout_jiffies = packet_timeout * HZ / 1000;
+	dev->packet_timeout_jiffies = msecs_to_jiffies(packet_timeout);
 	dev->read_last_arrival = jiffies;
 
 	init_waitqueue_head (&dev->read_wait);
diff --git a/drivers/usb/misc/usb3503.c b/drivers/usb/misc/usb3503.c
index 258d2f546e43..64ff5b91752d 100644
--- a/drivers/usb/misc/usb3503.c
+++ b/drivers/usb/misc/usb3503.c
@@ -186,8 +186,31 @@ static int usb3503_probe(struct usb3503 *hub)
 		hub->mode		= pdata->initial_mode;
 	} else if (np) {
 		struct clk *clk;
+		u32 rate = 0;
 		hub->port_off_mask = 0;
 
+		if (!of_property_read_u32(np, "refclk-frequency", &rate)) {
+			switch (rate) {
+			case 38400000:
+			case 26000000:
+			case 19200000:
+			case 12000000:
+				hub->secondary_ref_clk = 0;
+				break;
+			case 24000000:
+			case 27000000:
+			case 25000000:
+			case 50000000:
+				hub->secondary_ref_clk = 1;
+				break;
+			default:
+				dev_err(dev,
+					"unsupported reference clock rate (%d)\n",
+					(int) rate);
+				return -EINVAL;
+			}
+		}
+
 		clk = devm_clk_get(dev, "refclk");
 		if (IS_ERR(clk) && PTR_ERR(clk) != -ENOENT) {
 			dev_err(dev, "unable to request refclk (%ld)\n",
@@ -196,31 +219,9 @@ static int usb3503_probe(struct usb3503 *hub)
 		}
 
 		if (!IS_ERR(clk)) {
-			u32 rate = 0;
 			hub->clk = clk;
 
-			if (!of_property_read_u32(np, "refclk-frequency",
-						 &rate)) {
-
-				switch (rate) {
-				case 38400000:
-				case 26000000:
-				case 19200000:
-				case 12000000:
-					hub->secondary_ref_clk = 0;
-					break;
-				case 24000000:
-				case 27000000:
-				case 25000000:
-				case 50000000:
-					hub->secondary_ref_clk = 1;
-					break;
-				default:
-					dev_err(dev,
-						"unsupported reference clock rate (%d)\n",
-						(int) rate);
-					return -EINVAL;
-				}
+			if (rate != 0) {
 				err = clk_set_rate(hub->clk, rate);
 				if (err) {
 					dev_err(dev,
diff --git a/drivers/usb/musb/Kconfig b/drivers/usb/musb/Kconfig
index 14e1628483d9..39db8b603627 100644
--- a/drivers/usb/musb/Kconfig
+++ b/drivers/usb/musb/Kconfig
@@ -79,7 +79,8 @@ config USB_MUSB_TUSB6010
 
 config USB_MUSB_OMAP2PLUS
 	tristate "OMAP2430 and onwards"
-	depends on ARCH_OMAP2PLUS && USB && OMAP_CONTROL_PHY
+	depends on ARCH_OMAP2PLUS && USB
+	depends on OMAP_CONTROL_PHY || !OMAP_CONTROL_PHY
 	select GENERIC_PHY
 
 config USB_MUSB_AM35X
diff --git a/drivers/usb/musb/musb_core.c b/drivers/usb/musb/musb_core.c
index e6f4cbfeed97..3789b08ef67b 100644
--- a/drivers/usb/musb/musb_core.c
+++ b/drivers/usb/musb/musb_core.c
@@ -99,6 +99,7 @@
 #include <linux/platform_device.h>
 #include <linux/io.h>
 #include <linux/dma-mapping.h>
+#include <linux/usb.h>
 
 #include "musb_core.h"
 
@@ -507,7 +508,8 @@ void musb_hnp_stop(struct musb *musb)
 	musb->port1_status &= ~(USB_PORT_STAT_C_CONNECTION << 16);
 }
 
-static void musb_generic_disable(struct musb *musb);
+static void musb_recover_from_babble(struct musb *musb);
+
 /*
  * Interrupt Service Routine to record USB "global" interrupts.
  * Since these do not happen often and signify things of
@@ -534,35 +536,21 @@ static irqreturn_t musb_stage0_irq(struct musb *musb, u8 int_usb,
 	 */
 	if (int_usb & MUSB_INTR_RESUME) {
 		handled = IRQ_HANDLED;
-		dev_dbg(musb->controller, "RESUME (%s)\n", usb_otg_state_string(musb->xceiv->otg->state));
+		dev_dbg(musb->controller, "RESUME (%s)\n",
+				usb_otg_state_string(musb->xceiv->otg->state));
 
 		if (devctl & MUSB_DEVCTL_HM) {
-			void __iomem *mbase = musb->mregs;
-			u8 power;
-
 			switch (musb->xceiv->otg->state) {
 			case OTG_STATE_A_SUSPEND:
 				/* remote wakeup?  later, GetPortStatus
 				 * will stop RESUME signaling
 				 */
 
-				power = musb_readb(musb->mregs, MUSB_POWER);
-				if (power & MUSB_POWER_SUSPENDM) {
-					/* spurious */
-					musb->int_usb &= ~MUSB_INTR_SUSPEND;
-					dev_dbg(musb->controller, "Spurious SUSPENDM\n");
-					break;
-				}
-
-				power &= ~MUSB_POWER_SUSPENDM;
-				musb_writeb(mbase, MUSB_POWER,
-						power | MUSB_POWER_RESUME);
-
 				musb->port1_status |=
 						(USB_PORT_STAT_C_SUSPEND << 16)
 						| MUSB_PORT_STAT_RESUME;
 				musb->rh_timer = jiffies
-						 + msecs_to_jiffies(20);
+					+ msecs_to_jiffies(USB_RESUME_TIMEOUT);
 				musb->need_finish_resume = 1;
 
 				musb->xceiv->otg->state = OTG_STATE_A_HOST;
@@ -775,10 +763,6 @@ static irqreturn_t musb_stage0_irq(struct musb *musb, u8 int_usb,
 
 		musb->ep0_stage = MUSB_EP0_START;
 
-		/* flush endpoints when transitioning from Device Mode */
-		if (is_peripheral_active(musb)) {
-			/* REVISIT HNP; just force disconnect */
-		}
 		musb->intrtxe = musb->epmask;
 		musb_writew(musb->mregs, MUSB_INTRTXE, musb->intrtxe);
 		musb->intrrxe = musb->epmask & 0xfffe;
@@ -879,20 +863,19 @@ b_host:
 	 */
 	if (int_usb & MUSB_INTR_RESET) {
 		handled = IRQ_HANDLED;
-		if ((devctl & MUSB_DEVCTL_HM) != 0) {
+		if (devctl & MUSB_DEVCTL_HM) {
 			/*
-			 * Looks like non-HS BABBLE can be ignored, but
-			 * HS BABBLE is an error condition. For HS the solution
-			 * is to avoid babble in the first place and fix what
-			 * caused BABBLE. When HS BABBLE happens we can only
-			 * stop the session.
+			 * When BABBLE happens what we can depends on which
+			 * platform MUSB is running, because some platforms
+			 * implemented proprietary means for 'recovering' from
+			 * Babble conditions. One such platform is AM335x. In
+			 * most cases, however, the only thing we can do is
+			 * drop the session.
 			 */
-			if (devctl & (MUSB_DEVCTL_FSDEV | MUSB_DEVCTL_LSDEV))
-				dev_dbg(musb->controller, "BABBLE devctl: %02x\n", devctl);
-			else {
-				ERR("Stopping host session -- babble\n");
-				musb_writeb(musb->mregs, MUSB_DEVCTL, 0);
-			}
+			dev_err(musb->controller, "Babble\n");
+
+			if (is_host_active(musb))
+				musb_recover_from_babble(musb);
 		} else {
 			dev_dbg(musb->controller, "BUS RESET as %s\n",
 				usb_otg_state_string(musb->xceiv->otg->state));
@@ -931,13 +914,6 @@ b_host:
 		}
 	}
 
-	/* handle babble condition */
-	if (int_usb & MUSB_INTR_BABBLE && is_host_active(musb)) {
-		musb_generic_disable(musb);
-		schedule_delayed_work(&musb->recover_work,
-				      msecs_to_jiffies(100));
-	}
-
 #if 0
 /* REVISIT ... this would be for multiplexing periodic endpoints, or
  * supporting transfer phasing to prevent exceeding ISO bandwidth
@@ -990,7 +966,7 @@ b_host:
 
 /*-------------------------------------------------------------------------*/
 
-static void musb_generic_disable(struct musb *musb)
+static void musb_disable_interrupts(struct musb *musb)
 {
 	void __iomem	*mbase = musb->mregs;
 	u16	temp;
@@ -1002,16 +978,35 @@ static void musb_generic_disable(struct musb *musb)
 	musb->intrrxe = 0;
 	musb_writew(mbase, MUSB_INTRRXE, 0);
 
-	/* off */
-	musb_writeb(mbase, MUSB_DEVCTL, 0);
-
 	/*  flush pending interrupts */
 	temp = musb_readb(mbase, MUSB_INTRUSB);
 	temp = musb_readw(mbase, MUSB_INTRTX);
 	temp = musb_readw(mbase, MUSB_INTRRX);
+}
+
+static void musb_enable_interrupts(struct musb *musb)
+{
+	void __iomem    *regs = musb->mregs;
+
+	/*  Set INT enable registers, enable interrupts */
+	musb->intrtxe = musb->epmask;
+	musb_writew(regs, MUSB_INTRTXE, musb->intrtxe);
+	musb->intrrxe = musb->epmask & 0xfffe;
+	musb_writew(regs, MUSB_INTRRXE, musb->intrrxe);
+	musb_writeb(regs, MUSB_INTRUSBE, 0xf7);
 
 }
 
+static void musb_generic_disable(struct musb *musb)
+{
+	void __iomem	*mbase = musb->mregs;
+
+	musb_disable_interrupts(musb);
+
+	/* off */
+	musb_writeb(mbase, MUSB_DEVCTL, 0);
+}
+
 /*
  * Program the HDRC to start (enable interrupts, dma, etc.).
  */
@@ -1022,13 +1017,7 @@ void musb_start(struct musb *musb)
 
 	dev_dbg(musb->controller, "<== devctl %02x\n", devctl);
 
-	/*  Set INT enable registers, enable interrupts */
-	musb->intrtxe = musb->epmask;
-	musb_writew(regs, MUSB_INTRTXE, musb->intrtxe);
-	musb->intrrxe = musb->epmask & 0xfffe;
-	musb_writew(regs, MUSB_INTRRXE, musb->intrrxe);
-	musb_writeb(regs, MUSB_INTRUSBE, 0xf7);
-
+	musb_enable_interrupts(musb);
 	musb_writeb(regs, MUSB_TESTMODE, 0);
 
 	/* put into basic highspeed mode and start session */
@@ -1587,9 +1576,12 @@ static int musb_core_init(u16 musb_type, struct musb *musb)
 irqreturn_t musb_interrupt(struct musb *musb)
 {
 	irqreturn_t	retval = IRQ_NONE;
+	unsigned long	status;
+	unsigned long	epnum;
 	u8		devctl;
-	int		ep_num;
-	u32		reg;
+
+	if (!musb->int_usb && !musb->int_tx && !musb->int_rx)
+		return IRQ_NONE;
 
 	devctl = musb_readb(musb->mregs, MUSB_DEVCTL);
 
@@ -1597,56 +1589,57 @@ irqreturn_t musb_interrupt(struct musb *musb)
 		is_host_active(musb) ? "host" : "peripheral",
 		musb->int_usb, musb->int_tx, musb->int_rx);
 
-	/* the core can interrupt us for multiple reasons; docs have
-	 * a generic interrupt flowchart to follow
+	/**
+	 * According to Mentor Graphics' documentation, flowchart on page 98,
+	 * IRQ should be handled as follows:
+	 *
+	 * . Resume IRQ
+	 * . Session Request IRQ
+	 * . VBUS Error IRQ
+	 * . Suspend IRQ
+	 * . Connect IRQ
+	 * . Disconnect IRQ
+	 * . Reset/Babble IRQ
+	 * . SOF IRQ (we're not using this one)
+	 * . Endpoint 0 IRQ
+	 * . TX Endpoints
+	 * . RX Endpoints
+	 *
+	 * We will be following that flowchart in order to avoid any problems
+	 * that might arise with internal Finite State Machine.
 	 */
-	if (musb->int_usb)
-		retval |= musb_stage0_irq(musb, musb->int_usb,
-				devctl);
 
-	/* "stage 1" is handling endpoint irqs */
+	if (musb->int_usb)
+		retval |= musb_stage0_irq(musb, musb->int_usb, devctl);
 
-	/* handle endpoint 0 first */
 	if (musb->int_tx & 1) {
 		if (is_host_active(musb))
 			retval |= musb_h_ep0_irq(musb);
 		else
 			retval |= musb_g_ep0_irq(musb);
+
+		/* we have just handled endpoint 0 IRQ, clear it */
+		musb->int_tx &= ~BIT(0);
 	}
 
-	/* RX on endpoints 1-15 */
-	reg = musb->int_rx >> 1;
-	ep_num = 1;
-	while (reg) {
-		if (reg & 1) {
-			/* musb_ep_select(musb->mregs, ep_num); */
-			/* REVISIT just retval = ep->rx_irq(...) */
-			retval = IRQ_HANDLED;
-			if (is_host_active(musb))
-				musb_host_rx(musb, ep_num);
-			else
-				musb_g_rx(musb, ep_num);
-		}
+	status = musb->int_tx;
 
-		reg >>= 1;
-		ep_num++;
+	for_each_set_bit(epnum, &status, 16) {
+		retval = IRQ_HANDLED;
+		if (is_host_active(musb))
+			musb_host_tx(musb, epnum);
+		else
+			musb_g_tx(musb, epnum);
 	}
 
-	/* TX on endpoints 1-15 */
-	reg = musb->int_tx >> 1;
-	ep_num = 1;
-	while (reg) {
-		if (reg & 1) {
-			/* musb_ep_select(musb->mregs, ep_num); */
-			/* REVISIT just retval |= ep->tx_irq(...) */
-			retval = IRQ_HANDLED;
-			if (is_host_active(musb))
-				musb_host_tx(musb, ep_num);
-			else
-				musb_g_tx(musb, ep_num);
-		}
-		reg >>= 1;
-		ep_num++;
+	status = musb->int_rx;
+
+	for_each_set_bit(epnum, &status, 16) {
+		retval = IRQ_HANDLED;
+		if (is_host_active(musb))
+			musb_host_rx(musb, epnum);
+		else
+			musb_g_rx(musb, epnum);
 	}
 
 	return retval;
@@ -1825,33 +1818,44 @@ static void musb_irq_work(struct work_struct *data)
 	}
 }
 
-/* Recover from babble interrupt conditions */
-static void musb_recover_work(struct work_struct *data)
+static void musb_recover_from_babble(struct musb *musb)
 {
-	struct musb *musb = container_of(data, struct musb, recover_work.work);
-	int status, ret;
+	int ret;
+	u8 devctl;
+
+	musb_disable_interrupts(musb);
+
+	/*
+	 * wait at least 320 cycles of 60MHz clock. That's 5.3us, we will give
+	 * it some slack and wait for 10us.
+	 */
+	udelay(10);
 
-	ret  = musb_platform_reset(musb);
-	if (ret)
+	ret  = musb_platform_recover(musb);
+	if (ret) {
+		musb_enable_interrupts(musb);
 		return;
+	}
 
-	usb_phy_vbus_off(musb->xceiv);
-	usleep_range(100, 200);
+	/* drop session bit */
+	devctl = musb_readb(musb->mregs, MUSB_DEVCTL);
+	devctl &= ~MUSB_DEVCTL_SESSION;
+	musb_writeb(musb->mregs, MUSB_DEVCTL, devctl);
 
-	usb_phy_vbus_on(musb->xceiv);
-	usleep_range(100, 200);
+	/* tell usbcore about it */
+	musb_root_disconnect(musb);
 
 	/*
 	 * When a babble condition occurs, the musb controller
 	 * removes the session bit and the endpoint config is lost.
 	 */
 	if (musb->dyn_fifo)
-		status = ep_config_from_table(musb);
+		ret = ep_config_from_table(musb);
 	else
-		status = ep_config_from_hw(musb);
+		ret = ep_config_from_hw(musb);
 
-	/* start the session again */
-	if (status == 0)
+	/* restart session */
+	if (ret == 0)
 		musb_start(musb);
 }
 
@@ -1969,10 +1973,6 @@ musb_init_controller(struct device *dev, int nIrq, void __iomem *ctrl)
 		goto fail0;
 	}
 
-	pm_runtime_use_autosuspend(musb->controller);
-	pm_runtime_set_autosuspend_delay(musb->controller, 200);
-	pm_runtime_enable(musb->controller);
-
 	spin_lock_init(&musb->lock);
 	musb->board_set_power = plat->set_power;
 	musb->min_power = plat->min_power;
@@ -1991,6 +1991,12 @@ musb_init_controller(struct device *dev, int nIrq, void __iomem *ctrl)
 	musb_readl = musb_default_readl;
 	musb_writel = musb_default_writel;
 
+	/* We need musb_read/write functions initialized for PM */
+	pm_runtime_use_autosuspend(musb->controller);
+	pm_runtime_set_autosuspend_delay(musb->controller, 200);
+	pm_runtime_irq_safe(musb->controller);
+	pm_runtime_enable(musb->controller);
+
 	/* The musb_platform_init() call:
 	 *   - adjusts musb->mregs
 	 *   - sets the musb->isr
@@ -2085,7 +2091,6 @@ musb_init_controller(struct device *dev, int nIrq, void __iomem *ctrl)
 
 	/* Init IRQ workqueue before request_irq */
 	INIT_WORK(&musb->irq_work, musb_irq_work);
-	INIT_DELAYED_WORK(&musb->recover_work, musb_recover_work);
 	INIT_DELAYED_WORK(&musb->deassert_reset_work, musb_deassert_reset);
 	INIT_DELAYED_WORK(&musb->finish_resume_work, musb_host_finish_resume);
 
@@ -2181,7 +2186,6 @@ fail4:
 
 fail3:
 	cancel_work_sync(&musb->irq_work);
-	cancel_delayed_work_sync(&musb->recover_work);
 	cancel_delayed_work_sync(&musb->finish_resume_work);
 	cancel_delayed_work_sync(&musb->deassert_reset_work);
 	if (musb->dma_controller)
@@ -2247,7 +2251,6 @@ static int musb_remove(struct platform_device *pdev)
 		dma_controller_destroy(musb->dma_controller);
 
 	cancel_work_sync(&musb->irq_work);
-	cancel_delayed_work_sync(&musb->recover_work);
 	cancel_delayed_work_sync(&musb->finish_resume_work);
 	cancel_delayed_work_sync(&musb->deassert_reset_work);
 	musb_free(musb);
@@ -2461,7 +2464,7 @@ static int musb_resume(struct device *dev)
 	if (musb->need_finish_resume) {
 		musb->need_finish_resume = 0;
 		schedule_delayed_work(&musb->finish_resume_work,
-				      msecs_to_jiffies(20));
+				      msecs_to_jiffies(USB_RESUME_TIMEOUT));
 	}
 
 	/*
@@ -2504,7 +2507,7 @@ static int musb_runtime_resume(struct device *dev)
 	if (musb->need_finish_resume) {
 		musb->need_finish_resume = 0;
 		schedule_delayed_work(&musb->finish_resume_work,
-				msecs_to_jiffies(20));
+				msecs_to_jiffies(USB_RESUME_TIMEOUT));
 	}
 
 	return 0;
diff --git a/drivers/usb/musb/musb_core.h b/drivers/usb/musb/musb_core.h
index 5e65958f7915..3877249a8b2d 100644
--- a/drivers/usb/musb/musb_core.h
+++ b/drivers/usb/musb/musb_core.h
@@ -160,7 +160,8 @@ struct musb_io;
  * @init:	turns on clocks, sets up platform-specific registers, etc
  * @exit:	undoes @init
  * @set_mode:	forcefully changes operating mode
- * @try_ilde:	tries to idle the IP
+ * @try_idle:	tries to idle the IP
+ * @recover:	platform-specific babble recovery
  * @vbus_status: returns vbus status if possible
  * @set_vbus:	forces vbus status
  * @adjust_channel_params: pre check for standard dma channel_program func
@@ -196,7 +197,7 @@ struct musb_platform_ops {
 	void	(*write_fifo)(struct musb_hw_ep *hw_ep, u16 len, const u8 *buf);
 	int	(*set_mode)(struct musb *musb, u8 mode);
 	void	(*try_idle)(struct musb *musb, unsigned long timeout);
-	int	(*reset)(struct musb *musb);
+	int	(*recover)(struct musb *musb);
 
 	int	(*vbus_status)(struct musb *musb);
 	void	(*set_vbus)(struct musb *musb, int on);
@@ -300,7 +301,6 @@ struct musb {
 
 	irqreturn_t		(*isr)(int, void *);
 	struct work_struct	irq_work;
-	struct delayed_work	recover_work;
 	struct delayed_work	deassert_reset_work;
 	struct delayed_work	finish_resume_work;
 	u16			hwvers;
@@ -558,12 +558,12 @@ static inline void musb_platform_try_idle(struct musb *musb,
 		musb->ops->try_idle(musb, timeout);
 }
 
-static inline int  musb_platform_reset(struct musb *musb)
+static inline int  musb_platform_recover(struct musb *musb)
 {
-	if (!musb->ops->reset)
-		return -EINVAL;
+	if (!musb->ops->recover)
+		return 0;
 
-	return musb->ops->reset(musb);
+	return musb->ops->recover(musb);
 }
 
 static inline int musb_platform_get_vbus_status(struct musb *musb)
diff --git a/drivers/usb/musb/musb_cppi41.c b/drivers/usb/musb/musb_cppi41.c
index be84562d021b..8bd8c5e26921 100644
--- a/drivers/usb/musb/musb_cppi41.c
+++ b/drivers/usb/musb/musb_cppi41.c
@@ -225,10 +225,12 @@ static void cppi41_dma_callback(void *private_data)
 	struct dma_channel *channel = private_data;
 	struct cppi41_dma_channel *cppi41_channel = channel->private_data;
 	struct musb_hw_ep *hw_ep = cppi41_channel->hw_ep;
+	struct cppi41_dma_controller *controller;
 	struct musb *musb = hw_ep->musb;
 	unsigned long flags;
 	struct dma_tx_state txstate;
 	u32 transferred;
+	int is_hs = 0;
 	bool empty;
 
 	spin_lock_irqsave(&musb->lock, flags);
@@ -248,61 +250,59 @@ static void cppi41_dma_callback(void *private_data)
 			transferred < cppi41_channel->packet_sz)
 		cppi41_channel->prog_len = 0;
 
-	empty = musb_is_tx_fifo_empty(hw_ep);
-	if (empty) {
+	if (cppi41_channel->is_tx)
+		empty = musb_is_tx_fifo_empty(hw_ep);
+
+	if (!cppi41_channel->is_tx || empty) {
 		cppi41_trans_done(cppi41_channel);
-	} else {
-		struct cppi41_dma_controller *controller;
-		int is_hs = 0;
-		/*
-		 * On AM335x it has been observed that the TX interrupt fires
-		 * too early that means the TXFIFO is not yet empty but the DMA
-		 * engine says that it is done with the transfer. We don't
-		 * receive a FIFO empty interrupt so the only thing we can do is
-		 * to poll for the bit. On HS it usually takes 2us, on FS around
-		 * 110us - 150us depending on the transfer size.
-		 * We spin on HS (no longer than than 25us and setup a timer on
-		 * FS to check for the bit and complete the transfer.
-		 */
-		controller = cppi41_channel->controller;
+		goto out;
+	}
 
-		if (is_host_active(musb)) {
-			if (musb->port1_status & USB_PORT_STAT_HIGH_SPEED)
-				is_hs = 1;
-		} else {
-			if (musb->g.speed == USB_SPEED_HIGH)
-				is_hs = 1;
-		}
-		if (is_hs) {
-			unsigned wait = 25;
-
-			do {
-				empty = musb_is_tx_fifo_empty(hw_ep);
-				if (empty)
-					break;
-				wait--;
-				if (!wait)
-					break;
-				udelay(1);
-			} while (1);
+	/*
+	 * On AM335x it has been observed that the TX interrupt fires
+	 * too early that means the TXFIFO is not yet empty but the DMA
+	 * engine says that it is done with the transfer. We don't
+	 * receive a FIFO empty interrupt so the only thing we can do is
+	 * to poll for the bit. On HS it usually takes 2us, on FS around
+	 * 110us - 150us depending on the transfer size.
+	 * We spin on HS (no longer than than 25us and setup a timer on
+	 * FS to check for the bit and complete the transfer.
+	 */
+	controller = cppi41_channel->controller;
+
+	if (is_host_active(musb)) {
+		if (musb->port1_status & USB_PORT_STAT_HIGH_SPEED)
+			is_hs = 1;
+	} else {
+		if (musb->g.speed == USB_SPEED_HIGH)
+			is_hs = 1;
+	}
+	if (is_hs) {
+		unsigned wait = 25;
 
+		do {
 			empty = musb_is_tx_fifo_empty(hw_ep);
 			if (empty) {
 				cppi41_trans_done(cppi41_channel);
 				goto out;
 			}
-		}
-		list_add_tail(&cppi41_channel->tx_check,
-				&controller->early_tx_list);
-		if (!hrtimer_is_queued(&controller->early_tx)) {
-			unsigned long usecs = cppi41_channel->total_len / 10;
+			wait--;
+			if (!wait)
+				break;
+			cpu_relax();
+		} while (1);
+	}
+	list_add_tail(&cppi41_channel->tx_check,
+			&controller->early_tx_list);
+	if (!hrtimer_is_queued(&controller->early_tx)) {
+		unsigned long usecs = cppi41_channel->total_len / 10;
 
-			hrtimer_start_range_ns(&controller->early_tx,
+		hrtimer_start_range_ns(&controller->early_tx,
 				ktime_set(0, usecs * NSEC_PER_USEC),
 				20 * NSEC_PER_USEC,
 				HRTIMER_MODE_REL);
-		}
 	}
+
 out:
 	spin_unlock_irqrestore(&musb->lock, flags);
 }
diff --git a/drivers/usb/musb/musb_dsps.c b/drivers/usb/musb/musb_dsps.c
index 53bd0e71d19f..65d931a28a14 100644
--- a/drivers/usb/musb/musb_dsps.c
+++ b/drivers/usb/musb/musb_dsps.c
@@ -119,7 +119,7 @@ struct dsps_musb_wrapper {
 	unsigned	iddig:5;
 	unsigned	iddig_mux:5;
 	/* miscellaneous stuff */
-	u8		poll_seconds;
+	unsigned	poll_timeout;
 };
 
 /*
@@ -225,9 +225,8 @@ static void dsps_musb_enable(struct musb *musb)
 
 	dsps_writel(reg_base, wrp->epintr_set, epmask);
 	dsps_writel(reg_base, wrp->coreintr_set, coremask);
-	/* Force the DRVVBUS IRQ so we can start polling for ID change. */
-	dsps_writel(reg_base, wrp->coreintr_set,
-		    (1 << wrp->drvvbus) << wrp->usb_shift);
+	/* start polling for ID change. */
+	mod_timer(&glue->timer, jiffies + msecs_to_jiffies(wrp->poll_timeout));
 	dsps_musb_try_idle(musb, 0);
 }
 
@@ -285,7 +284,8 @@ static void otg_timer(unsigned long _musb)
 		}
 		if (!(devctl & MUSB_DEVCTL_SESSION) && !skip_session)
 			dsps_writeb(mregs, MUSB_DEVCTL, MUSB_DEVCTL_SESSION);
-		mod_timer(&glue->timer, jiffies + wrp->poll_seconds * HZ);
+		mod_timer(&glue->timer, jiffies +
+				msecs_to_jiffies(wrp->poll_timeout));
 		break;
 	case OTG_STATE_A_WAIT_VFALL:
 		musb->xceiv->otg->state = OTG_STATE_A_WAIT_VRISE;
@@ -330,28 +330,6 @@ static irqreturn_t dsps_interrupt(int irq, void *hci)
 
 	dev_dbg(musb->controller, "usbintr (%x) epintr(%x)\n",
 			usbintr, epintr);
-	/*
-	 * DRVVBUS IRQs are the only proxy we have (a very poor one!) for
-	 * DSPS IP's missing ID change IRQ.  We need an ID change IRQ to
-	 * switch appropriately between halves of the OTG state machine.
-	 * Managing DEVCTL.SESSION per Mentor docs requires that we know its
-	 * value but DEVCTL.BDEVICE is invalid without DEVCTL.SESSION set.
-	 * Also, DRVVBUS pulses for SRP (but not at 5V) ...
-	 */
-	if (is_host_active(musb) && usbintr & MUSB_INTR_BABBLE) {
-		pr_info("CAUTION: musb: Babble Interrupt Occurred\n");
-
-		/*
-		 * When a babble condition occurs, the musb controller removes
-		 * the session and is no longer in host mode. Hence, all
-		 * devices connected to its root hub get disconnected.
-		 *
-		 * Hand this error down to the musb core isr, so it can
-		 * recover.
-		 */
-		musb->int_usb = MUSB_INTR_BABBLE | MUSB_INTR_DISCONNECT;
-		musb->int_tx = musb->int_rx = 0;
-	}
 
 	if (usbintr & ((1 << wrp->drvvbus) << wrp->usb_shift)) {
 		int drvvbus = dsps_readl(reg_base, wrp->status);
@@ -374,8 +352,8 @@ static irqreturn_t dsps_interrupt(int irq, void *hci)
 			 */
 			musb->int_usb &= ~MUSB_INTR_VBUSERROR;
 			musb->xceiv->otg->state = OTG_STATE_A_WAIT_VFALL;
-			mod_timer(&glue->timer,
-					jiffies + wrp->poll_seconds * HZ);
+			mod_timer(&glue->timer, jiffies +
+					msecs_to_jiffies(wrp->poll_timeout));
 			WARNING("VBUS error workaround (delay coming)\n");
 		} else if (drvvbus) {
 			MUSB_HST_MODE(musb);
@@ -404,7 +382,8 @@ static irqreturn_t dsps_interrupt(int irq, void *hci)
 	/* Poll for ID change in OTG port mode */
 	if (musb->xceiv->otg->state == OTG_STATE_B_IDLE &&
 			musb->port_mode == MUSB_PORT_MODE_DUAL_ROLE)
-		mod_timer(&glue->timer, jiffies + wrp->poll_seconds * HZ);
+		mod_timer(&glue->timer, jiffies +
+				msecs_to_jiffies(wrp->poll_timeout));
 out:
 	spin_unlock_irqrestore(&musb->lock, flags);
 
@@ -453,16 +432,31 @@ static int dsps_musb_init(struct musb *musb)
 	musb->ctrl_base = reg_base;
 
 	/* NOP driver needs change if supporting dual instance */
-	musb->xceiv = devm_usb_get_phy_by_phandle(dev, "phys", 0);
+	musb->xceiv = devm_usb_get_phy_by_phandle(dev->parent, "phys", 0);
 	if (IS_ERR(musb->xceiv))
 		return PTR_ERR(musb->xceiv);
 
+	musb->phy = devm_phy_get(dev->parent, "usb2-phy");
+
 	/* Returns zero if e.g. not clocked */
 	rev = dsps_readl(reg_base, wrp->revision);
 	if (!rev)
 		return -ENODEV;
 
 	usb_phy_init(musb->xceiv);
+	if (IS_ERR(musb->phy))  {
+		musb->phy = NULL;
+	} else {
+		ret = phy_init(musb->phy);
+		if (ret < 0)
+			return ret;
+		ret = phy_power_on(musb->phy);
+		if (ret) {
+			phy_exit(musb->phy);
+			return ret;
+		}
+	}
+
 	setup_timer(&glue->timer, otg_timer, (unsigned long) musb);
 
 	/* Reset the musb */
@@ -482,7 +476,7 @@ static int dsps_musb_init(struct musb *musb)
 	 * logic enabled.
 	 */
 	val = dsps_readb(musb->mregs, MUSB_BABBLE_CTL);
-	if (val == MUSB_BABBLE_RCV_DISABLE) {
+	if (val & MUSB_BABBLE_RCV_DISABLE) {
 		glue->sw_babble_enabled = true;
 		val |= MUSB_BABBLE_SW_SESSION_CTRL;
 		dsps_writeb(musb->mregs, MUSB_BABBLE_CTL, val);
@@ -502,6 +496,8 @@ static int dsps_musb_exit(struct musb *musb)
 
 	del_timer_sync(&glue->timer);
 	usb_phy_shutdown(musb->xceiv);
+	phy_power_off(musb->phy);
+	phy_exit(musb->phy);
 	debugfs_remove_recursive(glue->dbgfs_root);
 
 	return 0;
@@ -554,7 +550,7 @@ static int dsps_musb_set_mode(struct musb *musb, u8 mode)
 	return 0;
 }
 
-static bool  sw_babble_control(struct musb *musb)
+static bool dsps_sw_babble_control(struct musb *musb)
 {
 	u8 babble_ctl;
 	bool session_restart =  false;
@@ -605,31 +601,36 @@ static bool  sw_babble_control(struct musb *musb)
 	return session_restart;
 }
 
-static int dsps_musb_reset(struct musb *musb)
+static int dsps_musb_recover(struct musb *musb)
 {
 	struct device *dev = musb->controller;
 	struct dsps_glue *glue = dev_get_drvdata(dev->parent);
-	const struct dsps_musb_wrapper *wrp = glue->wrp;
 	int session_restart = 0;
 
 	if (glue->sw_babble_enabled)
-		session_restart = sw_babble_control(musb);
-	/*
-	 * In case of new silicon version babble condition can be recovered
-	 * without resetting the MUSB. But for older silicon versions, MUSB
-	 * reset is needed
-	 */
-	if (session_restart || !glue->sw_babble_enabled) {
-		dev_info(musb->controller, "Restarting MUSB to recover from Babble\n");
-		dsps_writel(musb->ctrl_base, wrp->control, (1 << wrp->reset));
-		usleep_range(100, 200);
-		usb_phy_shutdown(musb->xceiv);
-		usleep_range(100, 200);
-		usb_phy_init(musb->xceiv);
+		session_restart = dsps_sw_babble_control(musb);
+	else
 		session_restart = 1;
+
+	return session_restart ? 0 : -EPIPE;
+}
+
+/* Similar to am35x, dm81xx support only 32-bit read operation */
+static void dsps_read_fifo32(struct musb_hw_ep *hw_ep, u16 len, u8 *dst)
+{
+	void __iomem *fifo = hw_ep->fifo;
+
+	if (len >= 4) {
+		ioread32_rep(fifo, dst, len >> 2);
+		dst += len & ~0x03;
+		len &= 0x03;
 	}
 
-	return !session_restart;
+	/* Read any remaining 1 to 3 bytes */
+	if (len > 0) {
+		u32 val = musb_readl(fifo, 0);
+		memcpy(dst, &val, len);
+	}
 }
 
 static struct musb_platform_ops dsps_ops = {
@@ -642,7 +643,7 @@ static struct musb_platform_ops dsps_ops = {
 
 	.try_idle	= dsps_musb_try_idle,
 	.set_mode	= dsps_musb_set_mode,
-	.reset		= dsps_musb_reset,
+	.recover	= dsps_musb_recover,
 };
 
 static u64 musb_dmamask = DMA_BIT_MASK(32);
@@ -687,7 +688,7 @@ static int dsps_create_musb_pdev(struct dsps_glue *glue,
 	struct musb_hdrc_config	*config;
 	struct platform_device *musb;
 	struct device_node *dn = parent->dev.of_node;
-	int ret;
+	int ret, val;
 
 	memset(resources, 0, sizeof(resources));
 	res = platform_get_resource_byname(parent, IORESOURCE_MEM, "mc");
@@ -714,7 +715,6 @@ static int dsps_create_musb_pdev(struct dsps_glue *glue,
 	musb->dev.parent		= dev;
 	musb->dev.dma_mask		= &musb_dmamask;
 	musb->dev.coherent_dma_mask	= musb_dmamask;
-	musb->dev.of_node		= of_node_get(dn);
 
 	glue->musb = musb;
 
@@ -739,7 +739,10 @@ static int dsps_create_musb_pdev(struct dsps_glue *glue,
 	pdata.mode = get_musb_port_mode(dev);
 	/* DT keeps this entry in mA, musb expects it as per USB spec */
 	pdata.power = get_int_prop(dn, "mentor,power") / 2;
-	config->multipoint = of_property_read_bool(dn, "mentor,multipoint");
+
+	ret = of_property_read_u32(dn, "mentor,multipoint", &val);
+	if (!ret && val)
+		config->multipoint = true;
 
 	ret = platform_device_add_data(musb, &pdata, sizeof(pdata));
 	if (ret) {
@@ -776,6 +779,9 @@ static int dsps_probe(struct platform_device *pdev)
 	}
 	wrp = match->data;
 
+	if (of_device_is_compatible(pdev->dev.of_node, "ti,musb-dm816"))
+		dsps_ops.read_fifo = dsps_read_fifo32;
+
 	/* allocate glue */
 	glue = devm_kzalloc(&pdev->dev, sizeof(*glue), GFP_KERNEL);
 	if (!glue)
@@ -847,12 +853,14 @@ static const struct dsps_musb_wrapper am33xx_driver_data = {
 	.rxep_shift		= 16,
 	.rxep_mask		= 0xfffe,
 	.rxep_bitmap		= (0xfffe << 16),
-	.poll_seconds		= 2,
+	.poll_timeout		= 2000, /* ms */
 };
 
 static const struct of_device_id musb_dsps_of_match[] = {
 	{ .compatible = "ti,musb-am33xx",
-		.data = (void *) &am33xx_driver_data, },
+		.data = &am33xx_driver_data, },
+	{ .compatible = "ti,musb-dm816",
+		.data = &am33xx_driver_data, },
 	{  },
 };
 MODULE_DEVICE_TABLE(of, musb_dsps_of_match);
@@ -903,7 +911,8 @@ static int dsps_resume(struct device *dev)
 	dsps_writel(mbase, wrp->rx_mode, glue->context.rx_mode);
 	if (musb->xceiv->otg->state == OTG_STATE_B_IDLE &&
 	    musb->port_mode == MUSB_PORT_MODE_DUAL_ROLE)
-		mod_timer(&glue->timer, jiffies + wrp->poll_seconds * HZ);
+		mod_timer(&glue->timer, jiffies +
+				msecs_to_jiffies(wrp->poll_timeout));
 
 	return 0;
 }
diff --git a/drivers/usb/musb/musb_gadget.c b/drivers/usb/musb/musb_gadget.c
index b2d9040c7685..4c481cd66c77 100644
--- a/drivers/usb/musb/musb_gadget.c
+++ b/drivers/usb/musb/musb_gadget.c
@@ -1876,44 +1876,6 @@ err:
 	return retval;
 }
 
-static void stop_activity(struct musb *musb, struct usb_gadget_driver *driver)
-{
-	int			i;
-	struct musb_hw_ep	*hw_ep;
-
-	/* don't disconnect if it's not connected */
-	if (musb->g.speed == USB_SPEED_UNKNOWN)
-		driver = NULL;
-	else
-		musb->g.speed = USB_SPEED_UNKNOWN;
-
-	/* deactivate the hardware */
-	if (musb->softconnect) {
-		musb->softconnect = 0;
-		musb_pullup(musb, 0);
-	}
-	musb_stop(musb);
-
-	/* killing any outstanding requests will quiesce the driver;
-	 * then report disconnect
-	 */
-	if (driver) {
-		for (i = 0, hw_ep = musb->endpoints;
-				i < musb->nr_endpoints;
-				i++, hw_ep++) {
-			musb_ep_select(musb->mregs, i);
-			if (hw_ep->is_shared_fifo /* || !epnum */) {
-				nuke(&hw_ep->ep_in, -ESHUTDOWN);
-			} else {
-				if (hw_ep->max_packet_sz_tx)
-					nuke(&hw_ep->ep_in, -ESHUTDOWN);
-				if (hw_ep->max_packet_sz_rx)
-					nuke(&hw_ep->ep_out, -ESHUTDOWN);
-			}
-		}
-	}
-}
-
 /*
  * Unregister the gadget driver. Used by gadget drivers when
  * unregistering themselves from the controller.
@@ -1940,7 +1902,7 @@ static int musb_gadget_stop(struct usb_gadget *g)
 	(void) musb_gadget_vbus_draw(&musb->g, 0);
 
 	musb->xceiv->otg->state = OTG_STATE_UNDEFINED;
-	stop_activity(musb, NULL);
+	musb_stop(musb);
 	otg_set_peripheral(musb->xceiv->otg, NULL);
 
 	musb->is_active = 0;
diff --git a/drivers/usb/musb/musb_host.c b/drivers/usb/musb/musb_host.c
index 883a9adfdfff..c3d5fc9dfb5b 100644
--- a/drivers/usb/musb/musb_host.c
+++ b/drivers/usb/musb/musb_host.c
@@ -2613,7 +2613,7 @@ static const struct hc_driver musb_hc_driver = {
 	.description		= "musb-hcd",
 	.product_desc		= "MUSB HDRC host driver",
 	.hcd_priv_size		= sizeof(struct musb *),
-	.flags			= HCD_USB2 | HCD_MEMORY,
+	.flags			= HCD_USB2 | HCD_MEMORY | HCD_BH,
 
 	/* not using irq handler or reset hooks from usbcore, since
 	 * those must be shared with peripheral code for OTG configs
diff --git a/drivers/usb/musb/musb_virthub.c b/drivers/usb/musb/musb_virthub.c
index 294e159f4afe..86c4b533e90b 100644
--- a/drivers/usb/musb/musb_virthub.c
+++ b/drivers/usb/musb/musb_virthub.c
@@ -136,7 +136,7 @@ void musb_port_suspend(struct musb *musb, bool do_suspend)
 		/* later, GetPortStatus will stop RESUME signaling */
 		musb->port1_status |= MUSB_PORT_STAT_RESUME;
 		schedule_delayed_work(&musb->finish_resume_work,
-				      msecs_to_jiffies(20));
+				      msecs_to_jiffies(USB_RESUME_TIMEOUT));
 	}
 }
 
@@ -345,7 +345,7 @@ int musb_hub_control(
 		struct usb_hub_descriptor *desc = (void *)buf;
 
 		desc->bDescLength = 9;
-		desc->bDescriptorType = 0x29;
+		desc->bDescriptorType = USB_DT_HUB;
 		desc->bNbrPorts = 1;
 		desc->wHubCharacteristics = cpu_to_le16(
 			HUB_CHAR_INDV_PORT_LPSM /* per-port power switching */
diff --git a/drivers/usb/musb/omap2430.c b/drivers/usb/musb/omap2430.c
index 763649eb4987..cc752d8c7773 100644
--- a/drivers/usb/musb/omap2430.c
+++ b/drivers/usb/musb/omap2430.c
@@ -516,7 +516,7 @@ static int omap2430_probe(struct platform_device *pdev)
 	struct omap2430_glue		*glue;
 	struct device_node		*np = pdev->dev.of_node;
 	struct musb_hdrc_config		*config;
-	int				ret = -ENOMEM;
+	int				ret = -ENOMEM, val;
 
 	glue = devm_kzalloc(&pdev->dev, sizeof(*glue), GFP_KERNEL);
 	if (!glue)
@@ -559,7 +559,10 @@ static int omap2430_probe(struct platform_device *pdev)
 		of_property_read_u32(np, "num-eps", (u32 *)&config->num_eps);
 		of_property_read_u32(np, "ram-bits", (u32 *)&config->ram_bits);
 		of_property_read_u32(np, "power", (u32 *)&pdata->power);
-		config->multipoint = of_property_read_bool(np, "multipoint");
+
+		ret = of_property_read_u32(np, "multipoint", &val);
+		if (!ret && val)
+			config->multipoint = true;
 
 		pdata->board_data	= data;
 		pdata->config		= config;
diff --git a/drivers/usb/phy/Kconfig b/drivers/usb/phy/Kconfig
index 52d3d58252e1..2175678e674e 100644
--- a/drivers/usb/phy/Kconfig
+++ b/drivers/usb/phy/Kconfig
@@ -139,7 +139,7 @@ config USB_ISP1301
 
 config USB_MSM_OTG
 	tristate "Qualcomm on-chip USB OTG controller support"
-	depends on (USB || USB_GADGET) && (ARCH_MSM || ARCH_QCOM || COMPILE_TEST)
+	depends on (USB || USB_GADGET) && (ARCH_QCOM || COMPILE_TEST)
 	depends on RESET_CONTROLLER
 	select USB_PHY
 	help
@@ -202,13 +202,13 @@ config USB_RCAR_GEN2_PHY
 config USB_ULPI
 	bool "Generic ULPI Transceiver Driver"
 	depends on ARM || ARM64
+	select USB_ULPI_VIEWPORT
 	help
 	  Enable this to support ULPI connected USB OTG transceivers which
 	  are likely found on embedded boards.
 
 config USB_ULPI_VIEWPORT
 	bool
-	depends on USB_ULPI
 	help
 	  Provides read/write operations to the ULPI phy register set for
 	  controllers with a viewport register (e.g. Chipidea/ARC controllers).
diff --git a/drivers/usb/phy/of.c b/drivers/usb/phy/of.c
index 7ea0154da9d5..66ffa82457a8 100644
--- a/drivers/usb/phy/of.c
+++ b/drivers/usb/phy/of.c
@@ -27,7 +27,7 @@ static const char *const usbphy_modes[] = {
  * @np:	Pointer to the given device_node
  *
  * The function gets phy interface string from property 'phy_type',
- * and returns the correspondig enum usb_phy_interface
+ * and returns the corresponding enum usb_phy_interface
  */
 enum usb_phy_interface of_usb_get_phy_mode(struct device_node *np)
 {
diff --git a/drivers/usb/phy/phy-ab8500-usb.c b/drivers/usb/phy/phy-ab8500-usb.c
index 0b1bd2369293..7225d526df04 100644
--- a/drivers/usb/phy/phy-ab8500-usb.c
+++ b/drivers/usb/phy/phy-ab8500-usb.c
@@ -277,7 +277,7 @@ static void ab8500_usb_regulator_enable(struct ab8500_usb *ab)
 			dev_err(ab->dev, "Failed to set the Vintcore to 1.3V, ret=%d\n",
 					ret);
 
-		ret = regulator_set_optimum_mode(ab->v_ulpi, 28000);
+		ret = regulator_set_load(ab->v_ulpi, 28000);
 		if (ret < 0)
 			dev_err(ab->dev, "Failed to set optimum mode (ret=%d)\n",
 					ret);
@@ -317,7 +317,7 @@ static void ab8500_usb_regulator_disable(struct ab8500_usb *ab)
 						ab->saved_v_ulpi, ret);
 		}
 
-		ret = regulator_set_optimum_mode(ab->v_ulpi, 0);
+		ret = regulator_set_load(ab->v_ulpi, 0);
 		if (ret < 0)
 			dev_err(ab->dev, "Failed to set optimum mode (ret=%d)\n",
 					ret);
@@ -893,7 +893,7 @@ static int abx500_usb_link_status_update(struct ab8500_usb *ab)
 
 /*
  * Disconnection Sequence:
- *   1. Disconect Interrupt
+ *   1. Disconnect Interrupt
  *   2. Disable regulators
  *   3. Disable AB clock
  *   4. Disable the Phy
diff --git a/drivers/usb/phy/phy-am335x-control.c b/drivers/usb/phy/phy-am335x-control.c
index 403fab772724..7b3035ff9434 100644
--- a/drivers/usb/phy/phy-am335x-control.c
+++ b/drivers/usb/phy/phy-am335x-control.c
@@ -126,6 +126,9 @@ struct phy_control *am335x_get_phy_control(struct device *dev)
 		return NULL;
 
 	dev = bus_find_device(&platform_bus_type, NULL, node, match);
+	if (!dev)
+		return NULL;
+
 	ctrl_usb = dev_get_drvdata(dev);
 	if (!ctrl_usb)
 		return NULL;
diff --git a/drivers/usb/phy/phy-generic.c b/drivers/usb/phy/phy-generic.c
index 70be50b734b2..deee68eafb72 100644
--- a/drivers/usb/phy/phy-generic.c
+++ b/drivers/usb/phy/phy-generic.c
@@ -62,14 +62,14 @@ static int nop_set_suspend(struct usb_phy *x, int suspend)
 	return 0;
 }
 
-static void nop_reset_set(struct usb_phy_generic *nop, int asserted)
+static void nop_reset(struct usb_phy_generic *nop)
 {
 	if (!nop->gpiod_reset)
 		return;
 
-	gpiod_direction_output(nop->gpiod_reset, !asserted);
+	gpiod_set_value(nop->gpiod_reset, 1);
 	usleep_range(10000, 20000);
-	gpiod_set_value(nop->gpiod_reset, asserted);
+	gpiod_set_value(nop->gpiod_reset, 0);
 }
 
 /* interface to regulator framework */
@@ -151,8 +151,7 @@ int usb_gen_phy_init(struct usb_phy *phy)
 	if (!IS_ERR(nop->clk))
 		clk_prepare_enable(nop->clk);
 
-	/* De-assert RESET */
-	nop_reset_set(nop, 0);
+	nop_reset(nop);
 
 	return 0;
 }
@@ -162,8 +161,7 @@ void usb_gen_phy_shutdown(struct usb_phy *phy)
 {
 	struct usb_phy_generic *nop = dev_get_drvdata(phy->dev);
 
-	/* Assert RESET */
-	nop_reset_set(nop, 1);
+	gpiod_set_value(nop->gpiod_reset, 1);
 
 	if (!IS_ERR(nop->clk))
 		clk_disable_unprepare(nop->clk);
diff --git a/drivers/usb/phy/phy-msm-usb.c b/drivers/usb/phy/phy-msm-usb.c
index 000fd892455f..c9156beeadef 100644
--- a/drivers/usb/phy/phy-msm-usb.c
+++ b/drivers/usb/phy/phy-msm-usb.c
@@ -142,27 +142,22 @@ static int msm_hsusb_ldo_set_mode(struct msm_otg *motg, int on)
 	int ret = 0;
 
 	if (on) {
-		ret = regulator_set_optimum_mode(motg->v1p8,
-				USB_PHY_1P8_HPM_LOAD);
+		ret = regulator_set_load(motg->v1p8, USB_PHY_1P8_HPM_LOAD);
 		if (ret < 0) {
 			pr_err("Could not set HPM for v1p8\n");
 			return ret;
 		}
-		ret = regulator_set_optimum_mode(motg->v3p3,
-				USB_PHY_3P3_HPM_LOAD);
+		ret = regulator_set_load(motg->v3p3, USB_PHY_3P3_HPM_LOAD);
 		if (ret < 0) {
 			pr_err("Could not set HPM for v3p3\n");
-			regulator_set_optimum_mode(motg->v1p8,
-				USB_PHY_1P8_LPM_LOAD);
+			regulator_set_load(motg->v1p8, USB_PHY_1P8_LPM_LOAD);
 			return ret;
 		}
 	} else {
-		ret = regulator_set_optimum_mode(motg->v1p8,
-				USB_PHY_1P8_LPM_LOAD);
+		ret = regulator_set_load(motg->v1p8, USB_PHY_1P8_LPM_LOAD);
 		if (ret < 0)
 			pr_err("Could not set LPM for v1p8\n");
-		ret = regulator_set_optimum_mode(motg->v3p3,
-				USB_PHY_3P3_LPM_LOAD);
+		ret = regulator_set_load(motg->v3p3, USB_PHY_3P3_LPM_LOAD);
 		if (ret < 0)
 			pr_err("Could not set LPM for v3p3\n");
 	}
@@ -263,9 +258,7 @@ static int msm_otg_link_clk_reset(struct msm_otg *motg, bool assert)
 {
 	int ret;
 
-	if (motg->pdata->link_clk_reset)
-		ret = motg->pdata->link_clk_reset(motg->clk, assert);
-	else if (assert)
+	if (assert)
 		ret = reset_control_assert(motg->link_rst);
 	else
 		ret = reset_control_deassert(motg->link_rst);
@@ -281,9 +274,7 @@ static int msm_otg_phy_clk_reset(struct msm_otg *motg)
 {
 	int ret = 0;
 
-	if (motg->pdata->phy_clk_reset)
-		ret = motg->pdata->phy_clk_reset(motg->phy_reset_clk);
-	else if (motg->phy_rst)
+	if (motg->phy_rst)
 		ret = reset_control_reset(motg->phy_rst);
 
 	if (ret)
@@ -1551,16 +1542,6 @@ static int msm_otg_probe(struct platform_device *pdev)
 	phy = &motg->phy;
 	phy->dev = &pdev->dev;
 
-	if (motg->pdata->phy_clk_reset) {
-		motg->phy_reset_clk = devm_clk_get(&pdev->dev,
-					   np ? "phy" : "usb_phy_clk");
-
-		if (IS_ERR(motg->phy_reset_clk)) {
-			dev_err(&pdev->dev, "failed to get usb_phy_clk\n");
-			return PTR_ERR(motg->phy_reset_clk);
-		}
-	}
-
 	motg->clk = devm_clk_get(&pdev->dev, np ? "core" : "usb_hs_clk");
 	if (IS_ERR(motg->clk)) {
 		dev_err(&pdev->dev, "failed to get usb_hs_clk\n");
diff --git a/drivers/usb/phy/phy-rcar-gen2-usb.c b/drivers/usb/phy/phy-rcar-gen2-usb.c
index f83808413ba2..f81800b6562a 100644
--- a/drivers/usb/phy/phy-rcar-gen2-usb.c
+++ b/drivers/usb/phy/phy-rcar-gen2-usb.c
@@ -47,7 +47,7 @@ struct rcar_gen2_usb_phy_priv {
 
 /* USB General status register */
 #define USBHS_UGSTS_REG			0x88
-#define USBHS_UGSTS_LOCK		(3 << 8)
+#define USBHS_UGSTS_LOCK		(1 << 8)
 
 /* Enable USBHS internal phy */
 static int __rcar_gen2_usbhs_phy_enable(void __iomem *base)
diff --git a/drivers/usb/phy/phy.c b/drivers/usb/phy/phy.c
index 2f9735b35338..d1cd6b50f520 100644
--- a/drivers/usb/phy/phy.c
+++ b/drivers/usb/phy/phy.c
@@ -81,7 +81,9 @@ static void devm_usb_phy_release(struct device *dev, void *res)
 
 static int devm_usb_phy_match(struct device *dev, void *res, void *match_data)
 {
-	return res == match_data;
+	struct usb_phy **phy = res;
+
+	return *phy == match_data;
 }
 
 /**
diff --git a/drivers/usb/renesas_usbhs/Kconfig b/drivers/usb/renesas_usbhs/Kconfig
index de83b9d0cd5c..ebc99ee076ce 100644
--- a/drivers/usb/renesas_usbhs/Kconfig
+++ b/drivers/usb/renesas_usbhs/Kconfig
@@ -6,6 +6,7 @@ config USB_RENESAS_USBHS
 	tristate 'Renesas USBHS controller'
 	depends on USB_GADGET
 	depends on ARCH_SHMOBILE || SUPERH || COMPILE_TEST
+	depends on EXTCON || !EXTCON # if EXTCON=m, USBHS cannot be built-in
 	default n
 	help
 	  Renesas USBHS is a discrete USB host and peripheral controller chip
diff --git a/drivers/usb/renesas_usbhs/common.c b/drivers/usb/renesas_usbhs/common.c
index 4cf77d3c3bd2..0f7e850fd4aa 100644
--- a/drivers/usb/renesas_usbhs/common.c
+++ b/drivers/usb/renesas_usbhs/common.c
@@ -276,6 +276,16 @@ int usbhs_set_device_config(struct usbhs_priv *priv, int devnum,
 }
 
 /*
+ *		interrupt functions
+ */
+void usbhs_xxxsts_clear(struct usbhs_priv *priv, u16 sts_reg, u16 bit)
+{
+	u16 pipe_mask = (u16)GENMASK(usbhs_get_dparam(priv, pipe_size), 0);
+
+	usbhs_write(priv, sts_reg, ~(1 << bit) & pipe_mask);
+}
+
+/*
  *		local functions
  */
 static void usbhsc_set_buswait(struct usbhs_priv *priv)
@@ -487,6 +497,15 @@ static struct renesas_usbhs_platform_info *usbhs_parse_dt(struct device *dev)
 	if (gpio > 0)
 		dparam->enable_gpio = gpio;
 
+	switch (dparam->type) {
+	case USBHS_TYPE_R8A7790:
+	case USBHS_TYPE_R8A7791:
+		dparam->has_usb_dmac = 1;
+		break;
+	default:
+		break;
+	}
+
 	return info;
 }
 
diff --git a/drivers/usb/renesas_usbhs/common.h b/drivers/usb/renesas_usbhs/common.h
index fc96e924edc4..8c5fc12ad778 100644
--- a/drivers/usb/renesas_usbhs/common.h
+++ b/drivers/usb/renesas_usbhs/common.h
@@ -193,6 +193,7 @@ struct usbhs_priv;
 #define TYPE_BULK	(1 << 14)
 #define TYPE_INT	(2 << 14)
 #define TYPE_ISO	(3 << 14)
+#define BFRE		(1 << 10)	/* BRDY Interrupt Operation Spec. */
 #define DBLB		(1 << 9)	/* Double Buffer Mode */
 #define SHTNAK		(1 << 7)	/* Pipe Disable in Transfer End */
 #define DIR_OUT		(1 << 4)	/* Transfer Direction */
@@ -216,6 +217,7 @@ struct usbhs_priv;
 #define	ACLRM		(1 << 9)	/* Buffer Auto-Clear Mode */
 #define SQCLR		(1 << 8)	/* Toggle Bit Clear */
 #define SQSET		(1 << 7)	/* Toggle Bit Set */
+#define SQMON		(1 << 6)	/* Toggle Bit Check */
 #define PBUSY		(1 << 5)	/* Pipe Busy */
 #define PID_MASK	(0x3)		/* Response PID */
 #define  PID_NAK	0
@@ -324,6 +326,11 @@ int usbhs_set_device_config(struct usbhs_priv *priv, int devnum, u16 upphub,
 			   u16 hubport, u16 speed);
 
 /*
+ * interrupt functions
+ */
+void usbhs_xxxsts_clear(struct usbhs_priv *priv, u16 sts_reg, u16 bit);
+
+/*
  * data
  */
 struct usbhs_priv *usbhs_pdev_to_priv(struct platform_device *pdev);
diff --git a/drivers/usb/renesas_usbhs/fifo.c b/drivers/usb/renesas_usbhs/fifo.c
index d891bff39d66..8597cf9cfceb 100644
--- a/drivers/usb/renesas_usbhs/fifo.c
+++ b/drivers/usb/renesas_usbhs/fifo.c
@@ -813,7 +813,8 @@ static void xfer_work(struct work_struct *work)
 	desc->callback		= usbhsf_dma_complete;
 	desc->callback_param	= pipe;
 
-	if (dmaengine_submit(desc) < 0) {
+	pkt->cookie = dmaengine_submit(desc);
+	if (pkt->cookie < 0) {
 		dev_err(dev, "Failed to submit dma descriptor\n");
 		return;
 	}
@@ -822,10 +823,10 @@ static void xfer_work(struct work_struct *work)
 		fifo->name, usbhs_pipe_number(pipe), pkt->length, pkt->zero);
 
 	usbhs_pipe_running(pipe, 1);
-	usbhs_pipe_set_trans_count_if_bulk(pipe, pkt->trans);
-	usbhs_pipe_enable(pipe);
 	usbhsf_dma_start(pipe, fifo);
+	usbhs_pipe_set_trans_count_if_bulk(pipe, pkt->trans);
 	dma_async_issue_pending(chan);
+	usbhs_pipe_enable(pipe);
 }
 
 /*
@@ -838,6 +839,7 @@ static int usbhsf_dma_prepare_push(struct usbhs_pkt *pkt, int *is_done)
 	struct usbhs_fifo *fifo;
 	int len = pkt->length - pkt->actual;
 	int ret;
+	uintptr_t align_mask;
 
 	if (usbhs_pipe_is_busy(pipe))
 		return 0;
@@ -847,10 +849,14 @@ static int usbhsf_dma_prepare_push(struct usbhs_pkt *pkt, int *is_done)
 	    usbhs_pipe_is_dcp(pipe))
 		goto usbhsf_pio_prepare_push;
 
-	if (len & 0x7) /* 8byte alignment */
+	/* check data length if this driver don't use USB-DMAC */
+	if (!usbhs_get_dparam(priv, has_usb_dmac) && len & 0x7)
 		goto usbhsf_pio_prepare_push;
 
-	if ((uintptr_t)(pkt->buf + pkt->actual) & 0x7) /* 8byte alignment */
+	/* check buffer alignment */
+	align_mask = usbhs_get_dparam(priv, has_usb_dmac) ?
+					USBHS_USB_DMAC_XFER_SIZE - 1 : 0x7;
+	if ((uintptr_t)(pkt->buf + pkt->actual) & align_mask)
 		goto usbhsf_pio_prepare_push;
 
 	/* return at this time if the pipe is running */
@@ -924,7 +930,85 @@ struct usbhs_pkt_handle usbhs_fifo_dma_push_handler = {
 /*
  *		DMA pop handler
  */
-static int usbhsf_dma_try_pop(struct usbhs_pkt *pkt, int *is_done)
+
+static int usbhsf_dma_prepare_pop_with_rx_irq(struct usbhs_pkt *pkt,
+					      int *is_done)
+{
+	return usbhsf_prepare_pop(pkt, is_done);
+}
+
+static int usbhsf_dma_prepare_pop_with_usb_dmac(struct usbhs_pkt *pkt,
+						int *is_done)
+{
+	struct usbhs_pipe *pipe = pkt->pipe;
+	struct usbhs_priv *priv = usbhs_pipe_to_priv(pipe);
+	struct usbhs_fifo *fifo;
+	int ret;
+
+	if (usbhs_pipe_is_busy(pipe))
+		return 0;
+
+	/* use PIO if packet is less than pio_dma_border or pipe is DCP */
+	if ((pkt->length < usbhs_get_dparam(priv, pio_dma_border)) ||
+	    usbhs_pipe_is_dcp(pipe))
+		goto usbhsf_pio_prepare_pop;
+
+	fifo = usbhsf_get_dma_fifo(priv, pkt);
+	if (!fifo)
+		goto usbhsf_pio_prepare_pop;
+
+	if ((uintptr_t)pkt->buf & (USBHS_USB_DMAC_XFER_SIZE - 1))
+		goto usbhsf_pio_prepare_pop;
+
+	usbhs_pipe_config_change_bfre(pipe, 1);
+
+	ret = usbhsf_fifo_select(pipe, fifo, 0);
+	if (ret < 0)
+		goto usbhsf_pio_prepare_pop;
+
+	if (usbhsf_dma_map(pkt) < 0)
+		goto usbhsf_pio_prepare_pop_unselect;
+
+	/* DMA */
+
+	/*
+	 * usbhs_fifo_dma_pop_handler :: prepare
+	 * enabled irq to come here.
+	 * but it is no longer needed for DMA. disable it.
+	 */
+	usbhsf_rx_irq_ctrl(pipe, 0);
+
+	pkt->trans = pkt->length;
+
+	INIT_WORK(&pkt->work, xfer_work);
+	schedule_work(&pkt->work);
+
+	return 0;
+
+usbhsf_pio_prepare_pop_unselect:
+	usbhsf_fifo_unselect(pipe, fifo);
+usbhsf_pio_prepare_pop:
+
+	/*
+	 * change handler to PIO
+	 */
+	pkt->handler = &usbhs_fifo_pio_pop_handler;
+	usbhs_pipe_config_change_bfre(pipe, 0);
+
+	return pkt->handler->prepare(pkt, is_done);
+}
+
+static int usbhsf_dma_prepare_pop(struct usbhs_pkt *pkt, int *is_done)
+{
+	struct usbhs_priv *priv = usbhs_pipe_to_priv(pkt->pipe);
+
+	if (usbhs_get_dparam(priv, has_usb_dmac))
+		return usbhsf_dma_prepare_pop_with_usb_dmac(pkt, is_done);
+	else
+		return usbhsf_dma_prepare_pop_with_rx_irq(pkt, is_done);
+}
+
+static int usbhsf_dma_try_pop_with_rx_irq(struct usbhs_pkt *pkt, int *is_done)
 {
 	struct usbhs_pipe *pipe = pkt->pipe;
 	struct usbhs_priv *priv = usbhs_pipe_to_priv(pipe);
@@ -993,7 +1077,16 @@ usbhsf_pio_prepare_pop:
 	return pkt->handler->try_run(pkt, is_done);
 }
 
-static int usbhsf_dma_pop_done(struct usbhs_pkt *pkt, int *is_done)
+static int usbhsf_dma_try_pop(struct usbhs_pkt *pkt, int *is_done)
+{
+	struct usbhs_priv *priv = usbhs_pipe_to_priv(pkt->pipe);
+
+	BUG_ON(usbhs_get_dparam(priv, has_usb_dmac));
+
+	return usbhsf_dma_try_pop_with_rx_irq(pkt, is_done);
+}
+
+static int usbhsf_dma_pop_done_with_rx_irq(struct usbhs_pkt *pkt, int *is_done)
 {
 	struct usbhs_pipe *pipe = pkt->pipe;
 	int maxp = usbhs_pipe_get_maxpacket(pipe);
@@ -1017,8 +1110,68 @@ static int usbhsf_dma_pop_done(struct usbhs_pkt *pkt, int *is_done)
 	return 0;
 }
 
+static size_t usbhs_dma_calc_received_size(struct usbhs_pkt *pkt,
+					   struct dma_chan *chan, int dtln)
+{
+	struct usbhs_pipe *pipe = pkt->pipe;
+	struct dma_tx_state state;
+	size_t received_size;
+	int maxp = usbhs_pipe_get_maxpacket(pipe);
+
+	dmaengine_tx_status(chan, pkt->cookie, &state);
+	received_size = pkt->length - state.residue;
+
+	if (dtln) {
+		received_size -= USBHS_USB_DMAC_XFER_SIZE;
+		received_size &= ~(maxp - 1);
+		received_size += dtln;
+	}
+
+	return received_size;
+}
+
+static int usbhsf_dma_pop_done_with_usb_dmac(struct usbhs_pkt *pkt,
+					     int *is_done)
+{
+	struct usbhs_pipe *pipe = pkt->pipe;
+	struct usbhs_priv *priv = usbhs_pipe_to_priv(pipe);
+	struct usbhs_fifo *fifo = usbhs_pipe_to_fifo(pipe);
+	struct dma_chan *chan = usbhsf_dma_chan_get(fifo, pkt);
+	int rcv_len;
+
+	/*
+	 * Since the driver disables rx_irq in DMA mode, the interrupt handler
+	 * cannot the BRDYSTS. So, the function clears it here because the
+	 * driver may use PIO mode next time.
+	 */
+	usbhs_xxxsts_clear(priv, BRDYSTS, usbhs_pipe_number(pipe));
+
+	rcv_len = usbhsf_fifo_rcv_len(priv, fifo);
+	usbhsf_fifo_clear(pipe, fifo);
+	pkt->actual = usbhs_dma_calc_received_size(pkt, chan, rcv_len);
+
+	usbhsf_dma_stop(pipe, fifo);
+	usbhsf_dma_unmap(pkt);
+	usbhsf_fifo_unselect(pipe, pipe->fifo);
+
+	/* The driver can assume the rx transaction is always "done" */
+	*is_done = 1;
+
+	return 0;
+}
+
+static int usbhsf_dma_pop_done(struct usbhs_pkt *pkt, int *is_done)
+{
+	struct usbhs_priv *priv = usbhs_pipe_to_priv(pkt->pipe);
+
+	if (usbhs_get_dparam(priv, has_usb_dmac))
+		return usbhsf_dma_pop_done_with_usb_dmac(pkt, is_done);
+	else
+		return usbhsf_dma_pop_done_with_rx_irq(pkt, is_done);
+}
+
 struct usbhs_pkt_handle usbhs_fifo_dma_pop_handler = {
-	.prepare	= usbhsf_prepare_pop,
+	.prepare	= usbhsf_dma_prepare_pop,
 	.try_run	= usbhsf_dma_try_pop,
 	.dma_done	= usbhsf_dma_pop_done
 };
@@ -1069,23 +1222,29 @@ static void usbhsf_dma_init_pdev(struct usbhs_fifo *fifo)
 					    &fifo->rx_slave);
 }
 
-static void usbhsf_dma_init_dt(struct device *dev, struct usbhs_fifo *fifo)
+static void usbhsf_dma_init_dt(struct device *dev, struct usbhs_fifo *fifo,
+			       int channel)
 {
-	fifo->tx_chan = dma_request_slave_channel_reason(dev, "tx");
+	char name[16];
+
+	snprintf(name, sizeof(name), "tx%d", channel);
+	fifo->tx_chan = dma_request_slave_channel_reason(dev, name);
 	if (IS_ERR(fifo->tx_chan))
 		fifo->tx_chan = NULL;
-	fifo->rx_chan = dma_request_slave_channel_reason(dev, "rx");
+
+	snprintf(name, sizeof(name), "rx%d", channel);
+	fifo->rx_chan = dma_request_slave_channel_reason(dev, name);
 	if (IS_ERR(fifo->rx_chan))
 		fifo->rx_chan = NULL;
 }
 
-static void usbhsf_dma_init(struct usbhs_priv *priv,
-			    struct usbhs_fifo *fifo)
+static void usbhsf_dma_init(struct usbhs_priv *priv, struct usbhs_fifo *fifo,
+			    int channel)
 {
 	struct device *dev = usbhs_priv_to_dev(priv);
 
 	if (dev->of_node)
-		usbhsf_dma_init_dt(dev, fifo);
+		usbhsf_dma_init_dt(dev, fifo, channel);
 	else
 		usbhsf_dma_init_pdev(fifo);
 
@@ -1231,7 +1390,7 @@ do {									\
 			usbhs_get_dparam(priv, d##channel##_tx_id);	\
 	fifo->rx_slave.shdma_slave.slave_id =				\
 			usbhs_get_dparam(priv, d##channel##_rx_id);	\
-	usbhsf_dma_init(priv, fifo);					\
+	usbhsf_dma_init(priv, fifo, channel);				\
 } while (0)
 
 #define USBHS_DFIFO_INIT(priv, fifo, channel)				\
diff --git a/drivers/usb/renesas_usbhs/fifo.h b/drivers/usb/renesas_usbhs/fifo.h
index f07037c1185f..04d3f8abad9e 100644
--- a/drivers/usb/renesas_usbhs/fifo.h
+++ b/drivers/usb/renesas_usbhs/fifo.h
@@ -58,6 +58,7 @@ struct usbhs_pkt {
 		     struct usbhs_pkt *pkt);
 	struct work_struct work;
 	dma_addr_t dma;
+	dma_cookie_t cookie;
 	void *buf;
 	int length;
 	int trans;
diff --git a/drivers/usb/renesas_usbhs/mod_gadget.c b/drivers/usb/renesas_usbhs/mod_gadget.c
index e0384af77e56..dc2aa3261202 100644
--- a/drivers/usb/renesas_usbhs/mod_gadget.c
+++ b/drivers/usb/renesas_usbhs/mod_gadget.c
@@ -119,18 +119,34 @@ struct usbhsg_recip_handle {
 /*
  *		queue push/pop
  */
-static void usbhsg_queue_pop(struct usbhsg_uep *uep,
-			     struct usbhsg_request *ureq,
-			     int status)
+static void __usbhsg_queue_pop(struct usbhsg_uep *uep,
+			       struct usbhsg_request *ureq,
+			       int status)
 {
 	struct usbhsg_gpriv *gpriv = usbhsg_uep_to_gpriv(uep);
 	struct usbhs_pipe *pipe = usbhsg_uep_to_pipe(uep);
 	struct device *dev = usbhsg_gpriv_to_dev(gpriv);
+	struct usbhs_priv *priv = usbhsg_gpriv_to_priv(gpriv);
 
 	dev_dbg(dev, "pipe %d : queue pop\n", usbhs_pipe_number(pipe));
 
 	ureq->req.status = status;
+	spin_unlock(usbhs_priv_to_lock(priv));
 	usb_gadget_giveback_request(&uep->ep, &ureq->req);
+	spin_lock(usbhs_priv_to_lock(priv));
+}
+
+static void usbhsg_queue_pop(struct usbhsg_uep *uep,
+			     struct usbhsg_request *ureq,
+			     int status)
+{
+	struct usbhsg_gpriv *gpriv = usbhsg_uep_to_gpriv(uep);
+	struct usbhs_priv *priv = usbhsg_gpriv_to_priv(gpriv);
+	unsigned long flags;
+
+	usbhs_lock(priv, flags);
+	__usbhsg_queue_pop(uep, ureq, status);
+	usbhs_unlock(priv, flags);
 }
 
 static void usbhsg_queue_done(struct usbhs_priv *priv, struct usbhs_pkt *pkt)
diff --git a/drivers/usb/renesas_usbhs/mod_host.c b/drivers/usb/renesas_usbhs/mod_host.c
index 96eead619282..bd050359926c 100644
--- a/drivers/usb/renesas_usbhs/mod_host.c
+++ b/drivers/usb/renesas_usbhs/mod_host.c
@@ -1229,7 +1229,7 @@ static int __usbhsh_hub_get_status(struct usbhsh_hpriv *hpriv,
 		break;
 
 	case GetHubDescriptor:
-		desc->bDescriptorType		= 0x29;
+		desc->bDescriptorType		= USB_DT_HUB;
 		desc->bHubContrCurrent		= 0;
 		desc->bNbrPorts			= roothub_id;
 		desc->bDescLength		= 9;
diff --git a/drivers/usb/renesas_usbhs/pipe.c b/drivers/usb/renesas_usbhs/pipe.c
index 007f45abe96c..4f9c3356127a 100644
--- a/drivers/usb/renesas_usbhs/pipe.c
+++ b/drivers/usb/renesas_usbhs/pipe.c
@@ -84,6 +84,17 @@ static void __usbhsp_pipe_xxx_set(struct usbhs_pipe *pipe,
 		usbhs_bset(priv, pipe_reg, mask, val);
 }
 
+static u16 __usbhsp_pipe_xxx_get(struct usbhs_pipe *pipe,
+				 u16 dcp_reg, u16 pipe_reg)
+{
+	struct usbhs_priv *priv = usbhs_pipe_to_priv(pipe);
+
+	if (usbhs_pipe_is_dcp(pipe))
+		return usbhs_read(priv, dcp_reg);
+	else
+		return usbhs_read(priv, pipe_reg);
+}
+
 /*
  *		DCPCFG/PIPECFG functions
  */
@@ -92,6 +103,11 @@ static void usbhsp_pipe_cfg_set(struct usbhs_pipe *pipe, u16 mask, u16 val)
 	__usbhsp_pipe_xxx_set(pipe, DCPCFG, PIPECFG, mask, val);
 }
 
+static u16 usbhsp_pipe_cfg_get(struct usbhs_pipe *pipe)
+{
+	return __usbhsp_pipe_xxx_get(pipe, DCPCFG, PIPECFG);
+}
+
 /*
  *		PIPEnTRN/PIPEnTRE functions
  */
@@ -616,6 +632,11 @@ void usbhs_pipe_data_sequence(struct usbhs_pipe *pipe, int sequence)
 	usbhsp_pipectrl_set(pipe, mask, val);
 }
 
+static int usbhs_pipe_get_data_sequence(struct usbhs_pipe *pipe)
+{
+	return !!(usbhsp_pipectrl_get(pipe) & SQMON);
+}
+
 void usbhs_pipe_clear(struct usbhs_pipe *pipe)
 {
 	if (usbhs_pipe_is_dcp(pipe)) {
@@ -626,6 +647,24 @@ void usbhs_pipe_clear(struct usbhs_pipe *pipe)
 	}
 }
 
+void usbhs_pipe_config_change_bfre(struct usbhs_pipe *pipe, int enable)
+{
+	int sequence;
+
+	if (usbhs_pipe_is_dcp(pipe))
+		return;
+
+	usbhsp_pipe_select(pipe);
+	/* check if the driver needs to change the BFRE value */
+	if (!(enable ^ !!(usbhsp_pipe_cfg_get(pipe) & BFRE)))
+		return;
+
+	sequence = usbhs_pipe_get_data_sequence(pipe);
+	usbhsp_pipe_cfg_set(pipe, BFRE, enable ? BFRE : 0);
+	usbhs_pipe_clear(pipe);
+	usbhs_pipe_data_sequence(pipe, sequence);
+}
+
 static struct usbhs_pipe *usbhsp_get_pipe(struct usbhs_priv *priv, u32 type)
 {
 	struct usbhs_pipe *pos, *pipe;
diff --git a/drivers/usb/renesas_usbhs/pipe.h b/drivers/usb/renesas_usbhs/pipe.h
index d24a05972370..b0bc7b603016 100644
--- a/drivers/usb/renesas_usbhs/pipe.h
+++ b/drivers/usb/renesas_usbhs/pipe.h
@@ -97,6 +97,7 @@ void usbhs_pipe_set_trans_count_if_bulk(struct usbhs_pipe *pipe, int len);
 void usbhs_pipe_select_fifo(struct usbhs_pipe *pipe, struct usbhs_fifo *fifo);
 void usbhs_pipe_config_update(struct usbhs_pipe *pipe, u16 devsel,
 			      u16 epnum, u16 maxp);
+void usbhs_pipe_config_change_bfre(struct usbhs_pipe *pipe, int enable);
 
 #define usbhs_pipe_sequence_data0(pipe)	usbhs_pipe_data_sequence(pipe, 0)
 #define usbhs_pipe_sequence_data1(pipe)	usbhs_pipe_data_sequence(pipe, 1)
diff --git a/drivers/usb/serial/bus.c b/drivers/usb/serial/bus.c
index 9374bd2aba20..8936a83c96cd 100644
--- a/drivers/usb/serial/bus.c
+++ b/drivers/usb/serial/bus.c
@@ -38,56 +38,51 @@ static int usb_serial_device_match(struct device *dev,
 	return 0;
 }
 
-static ssize_t port_number_show(struct device *dev,
-				struct device_attribute *attr, char *buf)
-{
-	struct usb_serial_port *port = to_usb_serial_port(dev);
-
-	return sprintf(buf, "%d\n", port->port_number);
-}
-static DEVICE_ATTR_RO(port_number);
-
 static int usb_serial_device_probe(struct device *dev)
 {
 	struct usb_serial_driver *driver;
 	struct usb_serial_port *port;
+	struct device *tty_dev;
 	int retval = 0;
 	int minor;
 
 	port = to_usb_serial_port(dev);
-	if (!port) {
-		retval = -ENODEV;
-		goto exit;
-	}
+	if (!port)
+		return -ENODEV;
 
 	/* make sure suspend/resume doesn't race against port_probe */
 	retval = usb_autopm_get_interface(port->serial->interface);
 	if (retval)
-		goto exit;
+		return retval;
 
 	driver = port->serial->type;
 	if (driver->port_probe) {
 		retval = driver->port_probe(port);
 		if (retval)
-			goto exit_with_autopm;
+			goto err_autopm_put;
 	}
 
-	retval = device_create_file(dev, &dev_attr_port_number);
-	if (retval) {
-		if (driver->port_remove)
-			retval = driver->port_remove(port);
-		goto exit_with_autopm;
+	minor = port->minor;
+	tty_dev = tty_register_device(usb_serial_tty_driver, minor, dev);
+	if (IS_ERR(tty_dev)) {
+		retval = PTR_ERR(tty_dev);
+		goto err_port_remove;
 	}
 
-	minor = port->minor;
-	tty_register_device(usb_serial_tty_driver, minor, dev);
+	usb_autopm_put_interface(port->serial->interface);
+
 	dev_info(&port->serial->dev->dev,
 		 "%s converter now attached to ttyUSB%d\n",
 		 driver->description, minor);
 
-exit_with_autopm:
+	return 0;
+
+err_port_remove:
+	if (driver->port_remove)
+		driver->port_remove(port);
+err_autopm_put:
 	usb_autopm_put_interface(port->serial->interface);
-exit:
+
 	return retval;
 }
 
@@ -114,8 +109,6 @@ static int usb_serial_device_remove(struct device *dev)
 	minor = port->minor;
 	tty_unregister_device(usb_serial_tty_driver, minor);
 
-	device_remove_file(&port->dev, &dev_attr_port_number);
-
 	driver = port->serial->type;
 	if (driver->port_remove)
 		retval = driver->port_remove(port);
diff --git a/drivers/usb/serial/ch341.c b/drivers/usb/serial/ch341.c
index 2d72aa3564a3..c73808f095bb 100644
--- a/drivers/usb/serial/ch341.c
+++ b/drivers/usb/serial/ch341.c
@@ -84,6 +84,10 @@ struct ch341_private {
 	u8 line_status; /* active status of modem control inputs */
 };
 
+static void ch341_set_termios(struct tty_struct *tty,
+			      struct usb_serial_port *port,
+			      struct ktermios *old_termios);
+
 static int ch341_control_out(struct usb_device *dev, u8 request,
 			     u16 value, u16 index)
 {
@@ -309,26 +313,18 @@ static int ch341_open(struct tty_struct *tty, struct usb_serial_port *port)
 	struct ch341_private *priv = usb_get_serial_port_data(port);
 	int r;
 
-	priv->baud_rate = DEFAULT_BAUD_RATE;
-
 	r = ch341_configure(serial->dev, priv);
 	if (r)
 		goto out;
 
-	r = ch341_set_handshake(serial->dev, priv->line_control);
-	if (r)
-		goto out;
-
-	r = ch341_set_baudrate(serial->dev, priv);
-	if (r)
-		goto out;
+	if (tty)
+		ch341_set_termios(tty, port, NULL);
 
 	dev_dbg(&port->dev, "%s - submitting interrupt urb\n", __func__);
 	r = usb_submit_urb(port->interrupt_in_urb, GFP_KERNEL);
 	if (r) {
 		dev_err(&port->dev, "%s - failed to submit interrupt urb: %d\n",
 			__func__, r);
-		ch341_close(port);
 		goto out;
 	}
 
diff --git a/drivers/usb/serial/console.c b/drivers/usb/serial/console.c
index 29fa1c3d0089..3806e7014199 100644
--- a/drivers/usb/serial/console.c
+++ b/drivers/usb/serial/console.c
@@ -14,6 +14,7 @@
 #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
 
 #include <linux/kernel.h>
+#include <linux/module.h>
 #include <linux/slab.h>
 #include <linux/tty.h>
 #include <linux/console.h>
@@ -144,6 +145,7 @@ static int usb_console_setup(struct console *co, char *options)
 			init_ldsem(&tty->ldisc_sem);
 			INIT_LIST_HEAD(&tty->tty_files);
 			kref_get(&tty->driver->kref);
+			__module_get(tty->driver->owner);
 			tty->ops = &usb_console_fake_tty_ops;
 			if (tty_init_termios(tty)) {
 				retval = -ENOMEM;
diff --git a/drivers/usb/serial/cp210x.c b/drivers/usb/serial/cp210x.c
index f40c856ff758..84ce2d74894c 100644
--- a/drivers/usb/serial/cp210x.c
+++ b/drivers/usb/serial/cp210x.c
@@ -147,6 +147,8 @@ static const struct usb_device_id id_table[] = {
 	{ USB_DEVICE(0x166A, 0x0305) }, /* Clipsal C-5000CT2 C-Bus Spectrum Colour Touchscreen */
 	{ USB_DEVICE(0x166A, 0x0401) }, /* Clipsal L51xx C-Bus Architectural Dimmer */
 	{ USB_DEVICE(0x166A, 0x0101) }, /* Clipsal 5560884 C-Bus Multi-room Audio Matrix Switcher */
+	{ USB_DEVICE(0x16C0, 0x09B0) }, /* Lunatico Seletek */
+	{ USB_DEVICE(0x16C0, 0x09B1) }, /* Lunatico Seletek */
 	{ USB_DEVICE(0x16D6, 0x0001) }, /* Jablotron serial interface */
 	{ USB_DEVICE(0x16DC, 0x0010) }, /* W-IE-NE-R Plein & Baus GmbH PL512 Power Supply */
 	{ USB_DEVICE(0x16DC, 0x0011) }, /* W-IE-NE-R Plein & Baus GmbH RCM Remote Control for MARATON Power Supply */
diff --git a/drivers/usb/serial/f81232.c b/drivers/usb/serial/f81232.c
index c5dc233db2d9..972f5a5fe577 100644
--- a/drivers/usb/serial/f81232.c
+++ b/drivers/usb/serial/f81232.c
@@ -19,10 +19,11 @@
 #include <linux/serial.h>
 #include <linux/module.h>
 #include <linux/moduleparam.h>
-#include <linux/spinlock.h>
+#include <linux/mutex.h>
 #include <linux/uaccess.h>
 #include <linux/usb.h>
 #include <linux/usb/serial.h>
+#include <linux/serial_reg.h>
 
 static const struct usb_device_id id_table[] = {
 	{ USB_DEVICE(0x1934, 0x0706) },
@@ -30,37 +31,218 @@ static const struct usb_device_id id_table[] = {
 };
 MODULE_DEVICE_TABLE(usb, id_table);
 
-#define CONTROL_DTR			0x01
-#define CONTROL_RTS			0x02
+/* Maximum baudrate for F81232 */
+#define F81232_MAX_BAUDRATE		115200
 
-#define UART_STATE			0x08
-#define UART_STATE_TRANSIENT_MASK	0x74
-#define UART_DCD			0x01
-#define UART_DSR			0x02
-#define UART_BREAK_ERROR		0x04
-#define UART_RING			0x08
-#define UART_FRAME_ERROR		0x10
-#define UART_PARITY_ERROR		0x20
-#define UART_OVERRUN_ERROR		0x40
-#define UART_CTS			0x80
+/* USB Control EP parameter */
+#define F81232_REGISTER_REQUEST		0xa0
+#define F81232_GET_REGISTER		0xc0
+#define F81232_SET_REGISTER		0x40
+
+#define SERIAL_BASE_ADDRESS		0x0120
+#define RECEIVE_BUFFER_REGISTER		(0x00 + SERIAL_BASE_ADDRESS)
+#define INTERRUPT_ENABLE_REGISTER	(0x01 + SERIAL_BASE_ADDRESS)
+#define FIFO_CONTROL_REGISTER		(0x02 + SERIAL_BASE_ADDRESS)
+#define LINE_CONTROL_REGISTER		(0x03 + SERIAL_BASE_ADDRESS)
+#define MODEM_CONTROL_REGISTER		(0x04 + SERIAL_BASE_ADDRESS)
+#define MODEM_STATUS_REGISTER		(0x06 + SERIAL_BASE_ADDRESS)
 
 struct f81232_private {
-	spinlock_t lock;
-	u8 line_control;
-	u8 line_status;
+	struct mutex lock;
+	u8 modem_control;
+	u8 modem_status;
+	struct work_struct interrupt_work;
+	struct usb_serial_port *port;
 };
 
+static int calc_baud_divisor(speed_t baudrate)
+{
+	return DIV_ROUND_CLOSEST(F81232_MAX_BAUDRATE, baudrate);
+}
+
+static int f81232_get_register(struct usb_serial_port *port, u16 reg, u8 *val)
+{
+	int status;
+	u8 *tmp;
+	struct usb_device *dev = port->serial->dev;
+
+	tmp = kmalloc(sizeof(*val), GFP_KERNEL);
+	if (!tmp)
+		return -ENOMEM;
+
+	status = usb_control_msg(dev,
+				usb_rcvctrlpipe(dev, 0),
+				F81232_REGISTER_REQUEST,
+				F81232_GET_REGISTER,
+				reg,
+				0,
+				tmp,
+				sizeof(*val),
+				USB_CTRL_GET_TIMEOUT);
+	if (status != sizeof(*val)) {
+		dev_err(&port->dev, "%s failed status: %d\n", __func__, status);
+
+		if (status < 0)
+			status = usb_translate_errors(status);
+		else
+			status = -EIO;
+	} else {
+		status = 0;
+		*val = *tmp;
+	}
+
+	kfree(tmp);
+	return status;
+}
+
+static int f81232_set_register(struct usb_serial_port *port, u16 reg, u8 val)
+{
+	int status;
+	u8 *tmp;
+	struct usb_device *dev = port->serial->dev;
+
+	tmp = kmalloc(sizeof(val), GFP_KERNEL);
+	if (!tmp)
+		return -ENOMEM;
+
+	*tmp = val;
+
+	status = usb_control_msg(dev,
+				usb_sndctrlpipe(dev, 0),
+				F81232_REGISTER_REQUEST,
+				F81232_SET_REGISTER,
+				reg,
+				0,
+				tmp,
+				sizeof(val),
+				USB_CTRL_SET_TIMEOUT);
+	if (status != sizeof(val)) {
+		dev_err(&port->dev, "%s failed status: %d\n", __func__, status);
+
+		if (status < 0)
+			status = usb_translate_errors(status);
+		else
+			status = -EIO;
+	} else {
+		status = 0;
+	}
+
+	kfree(tmp);
+	return status;
+}
+
+static void f81232_read_msr(struct usb_serial_port *port)
+{
+	int status;
+	u8 current_msr;
+	struct tty_struct *tty;
+	struct f81232_private *priv = usb_get_serial_port_data(port);
+
+	mutex_lock(&priv->lock);
+	status = f81232_get_register(port, MODEM_STATUS_REGISTER,
+			&current_msr);
+	if (status) {
+		dev_err(&port->dev, "%s fail, status: %d\n", __func__, status);
+		mutex_unlock(&priv->lock);
+		return;
+	}
+
+	if (!(current_msr & UART_MSR_ANY_DELTA)) {
+		mutex_unlock(&priv->lock);
+		return;
+	}
+
+	priv->modem_status = current_msr;
+
+	if (current_msr & UART_MSR_DCTS)
+		port->icount.cts++;
+	if (current_msr & UART_MSR_DDSR)
+		port->icount.dsr++;
+	if (current_msr & UART_MSR_TERI)
+		port->icount.rng++;
+	if (current_msr & UART_MSR_DDCD) {
+		port->icount.dcd++;
+		tty = tty_port_tty_get(&port->port);
+		if (tty) {
+			usb_serial_handle_dcd_change(port, tty,
+					current_msr & UART_MSR_DCD);
+
+			tty_kref_put(tty);
+		}
+	}
+
+	wake_up_interruptible(&port->port.delta_msr_wait);
+	mutex_unlock(&priv->lock);
+}
+
+static int f81232_set_mctrl(struct usb_serial_port *port,
+					   unsigned int set, unsigned int clear)
+{
+	u8 val;
+	int status;
+	struct f81232_private *priv = usb_get_serial_port_data(port);
+
+	if (((set | clear) & (TIOCM_DTR | TIOCM_RTS)) == 0)
+		return 0;	/* no change */
+
+	/* 'set' takes precedence over 'clear' */
+	clear &= ~set;
+
+	/* force enable interrupt with OUT2 */
+	mutex_lock(&priv->lock);
+	val = UART_MCR_OUT2 | priv->modem_control;
+
+	if (clear & TIOCM_DTR)
+		val &= ~UART_MCR_DTR;
+
+	if (clear & TIOCM_RTS)
+		val &= ~UART_MCR_RTS;
+
+	if (set & TIOCM_DTR)
+		val |= UART_MCR_DTR;
+
+	if (set & TIOCM_RTS)
+		val |= UART_MCR_RTS;
+
+	dev_dbg(&port->dev, "%s new:%02x old:%02x\n", __func__,
+			val, priv->modem_control);
+
+	status = f81232_set_register(port, MODEM_CONTROL_REGISTER, val);
+	if (status) {
+		dev_err(&port->dev, "%s set MCR status < 0\n", __func__);
+		mutex_unlock(&priv->lock);
+		return status;
+	}
+
+	priv->modem_control = val;
+	mutex_unlock(&priv->lock);
+
+	return 0;
+}
+
 static void f81232_update_line_status(struct usb_serial_port *port,
 				      unsigned char *data,
-				      unsigned int actual_length)
+				      size_t actual_length)
 {
-	/*
-	 * FIXME: Update port->icount, and call
-	 *
-	 *		wake_up_interruptible(&port->port.delta_msr_wait);
-	 *
-	 *	  on MSR changes.
-	 */
+	struct f81232_private *priv = usb_get_serial_port_data(port);
+
+	if (!actual_length)
+		return;
+
+	switch (data[0] & 0x07) {
+	case 0x00: /* msr change */
+		dev_dbg(&port->dev, "IIR: MSR Change: %02x\n", data[0]);
+		schedule_work(&priv->interrupt_work);
+		break;
+	case 0x02: /* tx-empty */
+		break;
+	case 0x04: /* rx data available */
+		break;
+	case 0x06: /* lsr change */
+		/* we can forget it. the LSR will read from bulk-in */
+		dev_dbg(&port->dev, "IIR: LSR Change: %02x\n", data[0]);
+		break;
+	}
 }
 
 static void f81232_read_int_callback(struct urb *urb)
@@ -104,55 +286,55 @@ exit:
 static void f81232_process_read_urb(struct urb *urb)
 {
 	struct usb_serial_port *port = urb->context;
-	struct f81232_private *priv = usb_get_serial_port_data(port);
 	unsigned char *data = urb->transfer_buffer;
-	char tty_flag = TTY_NORMAL;
-	unsigned long flags;
-	u8 line_status;
-	int i;
-
-	/* update line status */
-	spin_lock_irqsave(&priv->lock, flags);
-	line_status = priv->line_status;
-	priv->line_status &= ~UART_STATE_TRANSIENT_MASK;
-	spin_unlock_irqrestore(&priv->lock, flags);
-
-	if (!urb->actual_length)
+	char tty_flag;
+	unsigned int i;
+	u8 lsr;
+
+	/*
+	 * When opening the port we get a 1-byte packet with the current LSR,
+	 * which we discard.
+	 */
+	if ((urb->actual_length < 2) || (urb->actual_length % 2))
 		return;
 
-	/* break takes precedence over parity, */
-	/* which takes precedence over framing errors */
-	if (line_status & UART_BREAK_ERROR)
-		tty_flag = TTY_BREAK;
-	else if (line_status & UART_PARITY_ERROR)
-		tty_flag = TTY_PARITY;
-	else if (line_status & UART_FRAME_ERROR)
-		tty_flag = TTY_FRAME;
-	dev_dbg(&port->dev, "%s - tty_flag = %d\n", __func__, tty_flag);
-
-	/* overrun is special, not associated with a char */
-	if (line_status & UART_OVERRUN_ERROR)
-		tty_insert_flip_char(&port->port, 0, TTY_OVERRUN);
-
-	if (port->port.console && port->sysrq) {
-		for (i = 0; i < urb->actual_length; ++i)
-			if (!usb_serial_handle_sysrq_char(port, data[i]))
-				tty_insert_flip_char(&port->port, data[i],
-						tty_flag);
-	} else {
-		tty_insert_flip_string_fixed_flag(&port->port, data, tty_flag,
-							urb->actual_length);
+	/* bulk-in data: [LSR(1Byte)+DATA(1Byte)][LSR(1Byte)+DATA(1Byte)]... */
+
+	for (i = 0; i < urb->actual_length; i += 2) {
+		tty_flag = TTY_NORMAL;
+		lsr = data[i];
+
+		if (lsr & UART_LSR_BRK_ERROR_BITS) {
+			if (lsr & UART_LSR_BI) {
+				tty_flag = TTY_BREAK;
+				port->icount.brk++;
+				usb_serial_handle_break(port);
+			} else if (lsr & UART_LSR_PE) {
+				tty_flag = TTY_PARITY;
+				port->icount.parity++;
+			} else if (lsr & UART_LSR_FE) {
+				tty_flag = TTY_FRAME;
+				port->icount.frame++;
+			}
+
+			if (lsr & UART_LSR_OE) {
+				port->icount.overrun++;
+				tty_insert_flip_char(&port->port, 0,
+						TTY_OVERRUN);
+			}
+		}
+
+		if (port->port.console && port->sysrq) {
+			if (usb_serial_handle_sysrq_char(port, data[i + 1]))
+				continue;
+		}
+
+		tty_insert_flip_char(&port->port, data[i + 1], tty_flag);
 	}
 
 	tty_flip_buffer_push(&port->port);
 }
 
-static int set_control_lines(struct usb_device *dev, u8 value)
-{
-	/* FIXME - Stubbed out for now */
-	return 0;
-}
-
 static void f81232_break_ctl(struct tty_struct *tty, int break_state)
 {
 	/* FIXME - Stubbed out for now */
@@ -164,37 +346,198 @@ static void f81232_break_ctl(struct tty_struct *tty, int break_state)
 	 */
 }
 
+static void f81232_set_baudrate(struct usb_serial_port *port, speed_t baudrate)
+{
+	u8 lcr;
+	int divisor;
+	int status = 0;
+
+	divisor = calc_baud_divisor(baudrate);
+
+	status = f81232_get_register(port, LINE_CONTROL_REGISTER,
+			 &lcr); /* get LCR */
+	if (status) {
+		dev_err(&port->dev, "%s failed to get LCR: %d\n",
+			__func__, status);
+		return;
+	}
+
+	status = f81232_set_register(port, LINE_CONTROL_REGISTER,
+			 lcr | UART_LCR_DLAB); /* Enable DLAB */
+	if (status) {
+		dev_err(&port->dev, "%s failed to set DLAB: %d\n",
+			__func__, status);
+		return;
+	}
+
+	status = f81232_set_register(port, RECEIVE_BUFFER_REGISTER,
+			 divisor & 0x00ff); /* low */
+	if (status) {
+		dev_err(&port->dev, "%s failed to set baudrate MSB: %d\n",
+			__func__, status);
+		goto reapply_lcr;
+	}
+
+	status = f81232_set_register(port, INTERRUPT_ENABLE_REGISTER,
+			 (divisor & 0xff00) >> 8); /* high */
+	if (status) {
+		dev_err(&port->dev, "%s failed to set baudrate LSB: %d\n",
+			__func__, status);
+	}
+
+reapply_lcr:
+	status = f81232_set_register(port, LINE_CONTROL_REGISTER,
+			lcr & ~UART_LCR_DLAB);
+	if (status) {
+		dev_err(&port->dev, "%s failed to set DLAB: %d\n",
+			__func__, status);
+	}
+}
+
+static int f81232_port_enable(struct usb_serial_port *port)
+{
+	u8 val;
+	int status;
+
+	/* fifo on, trigger8, clear TX/RX*/
+	val = UART_FCR_TRIGGER_8 | UART_FCR_ENABLE_FIFO | UART_FCR_CLEAR_RCVR |
+			UART_FCR_CLEAR_XMIT;
+
+	status = f81232_set_register(port, FIFO_CONTROL_REGISTER, val);
+	if (status) {
+		dev_err(&port->dev, "%s failed to set FCR: %d\n",
+			__func__, status);
+		return status;
+	}
+
+	/* MSR Interrupt only, LSR will read from Bulk-in odd byte */
+	status = f81232_set_register(port, INTERRUPT_ENABLE_REGISTER,
+			UART_IER_MSI);
+	if (status) {
+		dev_err(&port->dev, "%s failed to set IER: %d\n",
+			__func__, status);
+		return status;
+	}
+
+	return 0;
+}
+
+static int f81232_port_disable(struct usb_serial_port *port)
+{
+	int status;
+
+	status = f81232_set_register(port, INTERRUPT_ENABLE_REGISTER, 0);
+	if (status) {
+		dev_err(&port->dev, "%s failed to set IER: %d\n",
+			__func__, status);
+		return status;
+	}
+
+	return 0;
+}
+
 static void f81232_set_termios(struct tty_struct *tty,
 		struct usb_serial_port *port, struct ktermios *old_termios)
 {
-	/* FIXME - Stubbed out for now */
+	u8 new_lcr = 0;
+	int status = 0;
+	speed_t baudrate;
 
 	/* Don't change anything if nothing has changed */
 	if (old_termios && !tty_termios_hw_change(&tty->termios, old_termios))
 		return;
 
-	/* Do the real work here... */
-	if (old_termios)
-		tty_termios_copy_hw(&tty->termios, old_termios);
+	if (C_BAUD(tty) == B0)
+		f81232_set_mctrl(port, 0, TIOCM_DTR | TIOCM_RTS);
+	else if (old_termios && (old_termios->c_cflag & CBAUD) == B0)
+		f81232_set_mctrl(port, TIOCM_DTR | TIOCM_RTS, 0);
+
+	baudrate = tty_get_baud_rate(tty);
+	if (baudrate > 0) {
+		if (baudrate > F81232_MAX_BAUDRATE) {
+			baudrate = F81232_MAX_BAUDRATE;
+			tty_encode_baud_rate(tty, baudrate, baudrate);
+		}
+		f81232_set_baudrate(port, baudrate);
+	}
+
+	if (C_PARENB(tty)) {
+		new_lcr |= UART_LCR_PARITY;
+
+		if (!C_PARODD(tty))
+			new_lcr |= UART_LCR_EPAR;
+
+		if (C_CMSPAR(tty))
+			new_lcr |= UART_LCR_SPAR;
+	}
+
+	if (C_CSTOPB(tty))
+		new_lcr |= UART_LCR_STOP;
+
+	switch (C_CSIZE(tty)) {
+	case CS5:
+		new_lcr |= UART_LCR_WLEN5;
+		break;
+	case CS6:
+		new_lcr |= UART_LCR_WLEN6;
+		break;
+	case CS7:
+		new_lcr |= UART_LCR_WLEN7;
+		break;
+	default:
+	case CS8:
+		new_lcr |= UART_LCR_WLEN8;
+		break;
+	}
+
+	status = f81232_set_register(port, LINE_CONTROL_REGISTER, new_lcr);
+	if (status) {
+		dev_err(&port->dev, "%s failed to set LCR: %d\n",
+			__func__, status);
+	}
 }
 
 static int f81232_tiocmget(struct tty_struct *tty)
 {
-	/* FIXME - Stubbed out for now */
-	return 0;
+	int r;
+	struct usb_serial_port *port = tty->driver_data;
+	struct f81232_private *port_priv = usb_get_serial_port_data(port);
+	u8 mcr, msr;
+
+	/* force get current MSR changed state */
+	f81232_read_msr(port);
+
+	mutex_lock(&port_priv->lock);
+	mcr = port_priv->modem_control;
+	msr = port_priv->modem_status;
+	mutex_unlock(&port_priv->lock);
+
+	r = (mcr & UART_MCR_DTR ? TIOCM_DTR : 0) |
+		(mcr & UART_MCR_RTS ? TIOCM_RTS : 0) |
+		(msr & UART_MSR_CTS ? TIOCM_CTS : 0) |
+		(msr & UART_MSR_DCD ? TIOCM_CAR : 0) |
+		(msr & UART_MSR_RI ? TIOCM_RI : 0) |
+		(msr & UART_MSR_DSR ? TIOCM_DSR : 0);
+
+	return r;
 }
 
 static int f81232_tiocmset(struct tty_struct *tty,
 			unsigned int set, unsigned int clear)
 {
-	/* FIXME - Stubbed out for now */
-	return 0;
+	struct usb_serial_port *port = tty->driver_data;
+
+	return f81232_set_mctrl(port, set, clear);
 }
 
 static int f81232_open(struct tty_struct *tty, struct usb_serial_port *port)
 {
 	int result;
 
+	result = f81232_port_enable(port);
+	if (result)
+		return result;
+
 	/* Setup termios */
 	if (tty)
 		f81232_set_termios(tty, port, NULL);
@@ -217,59 +560,73 @@ static int f81232_open(struct tty_struct *tty, struct usb_serial_port *port)
 
 static void f81232_close(struct usb_serial_port *port)
 {
+	f81232_port_disable(port);
 	usb_serial_generic_close(port);
 	usb_kill_urb(port->interrupt_in_urb);
 }
 
 static void f81232_dtr_rts(struct usb_serial_port *port, int on)
 {
-	struct f81232_private *priv = usb_get_serial_port_data(port);
-	unsigned long flags;
-	u8 control;
-
-	spin_lock_irqsave(&priv->lock, flags);
-	/* Change DTR and RTS */
 	if (on)
-		priv->line_control |= (CONTROL_DTR | CONTROL_RTS);
+		f81232_set_mctrl(port, TIOCM_DTR | TIOCM_RTS, 0);
 	else
-		priv->line_control &= ~(CONTROL_DTR | CONTROL_RTS);
-	control = priv->line_control;
-	spin_unlock_irqrestore(&priv->lock, flags);
-	set_control_lines(port->serial->dev, control);
+		f81232_set_mctrl(port, 0, TIOCM_DTR | TIOCM_RTS);
 }
 
 static int f81232_carrier_raised(struct usb_serial_port *port)
 {
+	u8 msr;
 	struct f81232_private *priv = usb_get_serial_port_data(port);
-	if (priv->line_status & UART_DCD)
+
+	mutex_lock(&priv->lock);
+	msr = priv->modem_status;
+	mutex_unlock(&priv->lock);
+
+	if (msr & UART_MSR_DCD)
 		return 1;
 	return 0;
 }
 
+static int f81232_get_serial_info(struct usb_serial_port *port,
+		unsigned long arg)
+{
+	struct serial_struct ser;
+
+	memset(&ser, 0, sizeof(ser));
+
+	ser.type = PORT_16550A;
+	ser.line = port->minor;
+	ser.port = port->port_number;
+	ser.baud_base = F81232_MAX_BAUDRATE;
+
+	if (copy_to_user((void __user *)arg, &ser, sizeof(ser)))
+		return -EFAULT;
+
+	return 0;
+}
+
 static int f81232_ioctl(struct tty_struct *tty,
 			unsigned int cmd, unsigned long arg)
 {
-	struct serial_struct ser;
 	struct usb_serial_port *port = tty->driver_data;
 
 	switch (cmd) {
 	case TIOCGSERIAL:
-		memset(&ser, 0, sizeof ser);
-		ser.type = PORT_16654;
-		ser.line = port->minor;
-		ser.port = port->port_number;
-		ser.baud_base = 460800;
-
-		if (copy_to_user((void __user *)arg, &ser, sizeof ser))
-			return -EFAULT;
-
-		return 0;
+		return f81232_get_serial_info(port, arg);
 	default:
 		break;
 	}
 	return -ENOIOCTLCMD;
 }
 
+static void  f81232_interrupt_work(struct work_struct *work)
+{
+	struct f81232_private *priv =
+		container_of(work, struct f81232_private, interrupt_work);
+
+	f81232_read_msr(priv->port);
+}
+
 static int f81232_port_probe(struct usb_serial_port *port)
 {
 	struct f81232_private *priv;
@@ -278,11 +635,13 @@ static int f81232_port_probe(struct usb_serial_port *port)
 	if (!priv)
 		return -ENOMEM;
 
-	spin_lock_init(&priv->lock);
+	mutex_init(&priv->lock);
+	INIT_WORK(&priv->interrupt_work,  f81232_interrupt_work);
 
 	usb_set_serial_port_data(port, priv);
 
 	port->port.drain_delay = 256;
+	priv->port = port;
 
 	return 0;
 }
@@ -308,7 +667,7 @@ static struct usb_serial_driver f81232_device = {
 	.bulk_out_size =	256,
 	.open =			f81232_open,
 	.close =		f81232_close,
-	.dtr_rts = 		f81232_dtr_rts,
+	.dtr_rts =		f81232_dtr_rts,
 	.carrier_raised =	f81232_carrier_raised,
 	.ioctl =		f81232_ioctl,
 	.break_ctl =		f81232_break_ctl,
@@ -330,5 +689,6 @@ static struct usb_serial_driver * const serial_drivers[] = {
 module_usb_serial_driver(serial_drivers, id_table);
 
 MODULE_DESCRIPTION("Fintek F81232 USB to serial adaptor driver");
-MODULE_AUTHOR("Greg Kroah-Hartman <gregkh@linuxfoundation.org");
+MODULE_AUTHOR("Greg Kroah-Hartman <gregkh@linuxfoundation.org>");
+MODULE_AUTHOR("Peter Hong <peter_hong@fintek.com.tw>");
 MODULE_LICENSE("GPL v2");
diff --git a/drivers/usb/serial/ftdi_sio.c b/drivers/usb/serial/ftdi_sio.c
index 1ebb351b9e9a..8eb68a31cab6 100644
--- a/drivers/usb/serial/ftdi_sio.c
+++ b/drivers/usb/serial/ftdi_sio.c
@@ -604,6 +604,7 @@ static const struct usb_device_id id_table_combined[] = {
 		.driver_info = (kernel_ulong_t)&ftdi_jtag_quirk },
 	{ USB_DEVICE(FTDI_VID, FTDI_NT_ORIONLXM_PID),
 		.driver_info = (kernel_ulong_t)&ftdi_jtag_quirk },
+	{ USB_DEVICE(FTDI_VID, FTDI_SYNAPSE_SS200_PID) },
 	/*
 	 * ELV devices:
 	 */
@@ -799,6 +800,8 @@ static const struct usb_device_id id_table_combined[] = {
 	{ USB_DEVICE(FTDI_VID, FTDI_ELSTER_UNICOM_PID) },
 	{ USB_DEVICE(FTDI_VID, FTDI_PROPOX_JTAGCABLEII_PID) },
 	{ USB_DEVICE(FTDI_VID, FTDI_PROPOX_ISPCABLEIII_PID) },
+	{ USB_DEVICE(FTDI_VID, CYBER_CORTEX_AV_PID),
+		.driver_info = (kernel_ulong_t)&ftdi_jtag_quirk },
 	{ USB_DEVICE(OLIMEX_VID, OLIMEX_ARM_USB_OCD_PID),
 		.driver_info = (kernel_ulong_t)&ftdi_jtag_quirk },
 	{ USB_DEVICE(OLIMEX_VID, OLIMEX_ARM_USB_OCD_H_PID),
@@ -978,6 +981,23 @@ static const struct usb_device_id id_table_combined[] = {
 	{ USB_DEVICE_INTERFACE_NUMBER(INFINEON_VID, INFINEON_TRIBOARD_PID, 1) },
 	/* GE Healthcare devices */
 	{ USB_DEVICE(GE_HEALTHCARE_VID, GE_HEALTHCARE_NEMO_TRACKER_PID) },
+	/* Active Research (Actisense) devices */
+	{ USB_DEVICE(FTDI_VID, ACTISENSE_NDC_PID) },
+	{ USB_DEVICE(FTDI_VID, ACTISENSE_USG_PID) },
+	{ USB_DEVICE(FTDI_VID, ACTISENSE_NGT_PID) },
+	{ USB_DEVICE(FTDI_VID, ACTISENSE_NGW_PID) },
+	{ USB_DEVICE(FTDI_VID, ACTISENSE_D9AC_PID) },
+	{ USB_DEVICE(FTDI_VID, ACTISENSE_D9AD_PID) },
+	{ USB_DEVICE(FTDI_VID, ACTISENSE_D9AE_PID) },
+	{ USB_DEVICE(FTDI_VID, ACTISENSE_D9AF_PID) },
+	{ USB_DEVICE(FTDI_VID, CHETCO_SEAGAUGE_PID) },
+	{ USB_DEVICE(FTDI_VID, CHETCO_SEASWITCH_PID) },
+	{ USB_DEVICE(FTDI_VID, CHETCO_SEASMART_NMEA2000_PID) },
+	{ USB_DEVICE(FTDI_VID, CHETCO_SEASMART_ETHERNET_PID) },
+	{ USB_DEVICE(FTDI_VID, CHETCO_SEASMART_WIFI_PID) },
+	{ USB_DEVICE(FTDI_VID, CHETCO_SEASMART_DISPLAY_PID) },
+	{ USB_DEVICE(FTDI_VID, CHETCO_SEASMART_LITE_PID) },
+	{ USB_DEVICE(FTDI_VID, CHETCO_SEASMART_ANALOG_PID) },
 	{ }					/* Terminating entry */
 };
 
@@ -1864,8 +1884,12 @@ static int ftdi_8u2232c_probe(struct usb_serial *serial)
 {
 	struct usb_device *udev = serial->dev;
 
-	if ((udev->manufacturer && !strcmp(udev->manufacturer, "CALAO Systems")) ||
-	    (udev->product && !strcmp(udev->product, "BeagleBone/XDS100V2")))
+	if (udev->manufacturer && !strcmp(udev->manufacturer, "CALAO Systems"))
+		return ftdi_jtag_probe(serial);
+
+	if (udev->product &&
+		(!strcmp(udev->product, "BeagleBone/XDS100V2") ||
+		 !strcmp(udev->product, "SNAP Connect E10")))
 		return ftdi_jtag_probe(serial);
 
 	return 0;
diff --git a/drivers/usb/serial/ftdi_sio_ids.h b/drivers/usb/serial/ftdi_sio_ids.h
index e52409c9be99..4e4f46f3c89c 100644
--- a/drivers/usb/serial/ftdi_sio_ids.h
+++ b/drivers/usb/serial/ftdi_sio_ids.h
@@ -38,6 +38,9 @@
 
 #define FTDI_LUMEL_PD12_PID	0x6002
 
+/* Cyber Cortex AV by Fabulous Silicon (http://fabuloussilicon.com) */
+#define CYBER_CORTEX_AV_PID	0x8698
+
 /*
  * Marvell OpenRD Base, Client
  * http://www.open-rd.org
@@ -558,6 +561,12 @@
  */
 #define FTDI_NT_ORIONLXM_PID	0x7c90	/* OrionLXm Substation Automation Platform */
 
+/*
+ * Synapse Wireless product ids (FTDI_VID)
+ * http://www.synapse-wireless.com
+ */
+#define FTDI_SYNAPSE_SS200_PID 0x9090 /* SS200 - SNAP Stick 200 */
+
 
 /********************************/
 /** third-party VID/PID combos **/
@@ -1438,3 +1447,23 @@
  */
 #define GE_HEALTHCARE_VID		0x1901
 #define GE_HEALTHCARE_NEMO_TRACKER_PID	0x0015
+
+/*
+ * Active Research (Actisense) devices
+ */
+#define ACTISENSE_NDC_PID		0xD9A8 /* NDC USB Serial Adapter */
+#define ACTISENSE_USG_PID		0xD9A9 /* USG USB Serial Adapter */
+#define ACTISENSE_NGT_PID		0xD9AA /* NGT NMEA2000 Interface */
+#define ACTISENSE_NGW_PID		0xD9AB /* NGW NMEA2000 Gateway */
+#define ACTISENSE_D9AC_PID		0xD9AC /* Actisense Reserved */
+#define ACTISENSE_D9AD_PID		0xD9AD /* Actisense Reserved */
+#define ACTISENSE_D9AE_PID		0xD9AE /* Actisense Reserved */
+#define ACTISENSE_D9AF_PID		0xD9AF /* Actisense Reserved */
+#define CHETCO_SEAGAUGE_PID		0xA548 /* SeaGauge USB Adapter */
+#define CHETCO_SEASWITCH_PID		0xA549 /* SeaSwitch USB Adapter */
+#define CHETCO_SEASMART_NMEA2000_PID	0xA54A /* SeaSmart NMEA2000 Gateway */
+#define CHETCO_SEASMART_ETHERNET_PID	0xA54B /* SeaSmart Ethernet Gateway */
+#define CHETCO_SEASMART_WIFI_PID	0xA5AC /* SeaSmart Wifi Gateway */
+#define CHETCO_SEASMART_DISPLAY_PID	0xA5AD /* SeaSmart NMEA2000 Display */
+#define CHETCO_SEASMART_LITE_PID	0xA5AE /* SeaSmart Lite USB Adapter */
+#define CHETCO_SEASMART_ANALOG_PID	0xA5AF /* SeaSmart Analog Adapter */
diff --git a/drivers/usb/serial/generic.c b/drivers/usb/serial/generic.c
index ccf1df7c4b80..54e170dd3dad 100644
--- a/drivers/usb/serial/generic.c
+++ b/drivers/usb/serial/generic.c
@@ -258,7 +258,8 @@ void usb_serial_generic_wait_until_sent(struct tty_struct *tty, long timeout)
 	 * character or at least one jiffy.
 	 */
 	period = max_t(unsigned long, (10 * HZ / bps), 1);
-	period = min_t(unsigned long, period, timeout);
+	if (timeout)
+		period = min_t(unsigned long, period, timeout);
 
 	dev_dbg(&port->dev, "%s - timeout = %u ms, period = %u ms\n",
 					__func__, jiffies_to_msecs(timeout),
@@ -268,7 +269,7 @@ void usb_serial_generic_wait_until_sent(struct tty_struct *tty, long timeout)
 		schedule_timeout_interruptible(period);
 		if (signal_pending(current))
 			break;
-		if (time_after(jiffies, expire))
+		if (timeout && time_after(jiffies, expire))
 			break;
 	}
 }
diff --git a/drivers/usb/serial/keyspan_pda.c b/drivers/usb/serial/keyspan_pda.c
index dd97d8b572c3..4f7e072e4e00 100644
--- a/drivers/usb/serial/keyspan_pda.c
+++ b/drivers/usb/serial/keyspan_pda.c
@@ -61,6 +61,7 @@ struct keyspan_pda_private {
 /* For Xircom PGSDB9 and older Entrega version of the same device */
 #define XIRCOM_VENDOR_ID		0x085a
 #define XIRCOM_FAKE_ID			0x8027
+#define XIRCOM_FAKE_ID_2		0x8025 /* "PGMFHUB" serial */
 #define ENTREGA_VENDOR_ID		0x1645
 #define ENTREGA_FAKE_ID			0x8093
 
@@ -70,6 +71,7 @@ static const struct usb_device_id id_table_combined[] = {
 #endif
 #ifdef XIRCOM
 	{ USB_DEVICE(XIRCOM_VENDOR_ID, XIRCOM_FAKE_ID) },
+	{ USB_DEVICE(XIRCOM_VENDOR_ID, XIRCOM_FAKE_ID_2) },
 	{ USB_DEVICE(ENTREGA_VENDOR_ID, ENTREGA_FAKE_ID) },
 #endif
 	{ USB_DEVICE(KEYSPAN_VENDOR_ID, KEYSPAN_PDA_ID) },
@@ -93,6 +95,7 @@ static const struct usb_device_id id_table_fake[] = {
 #ifdef XIRCOM
 static const struct usb_device_id id_table_fake_xircom[] = {
 	{ USB_DEVICE(XIRCOM_VENDOR_ID, XIRCOM_FAKE_ID) },
+	{ USB_DEVICE(XIRCOM_VENDOR_ID, XIRCOM_FAKE_ID_2) },
 	{ USB_DEVICE(ENTREGA_VENDOR_ID, ENTREGA_FAKE_ID) },
 	{ }
 };
diff --git a/drivers/usb/serial/mxuport.c b/drivers/usb/serial/mxuport.c
index ab1d690274ae..460a40669967 100644
--- a/drivers/usb/serial/mxuport.c
+++ b/drivers/usb/serial/mxuport.c
@@ -1284,7 +1284,8 @@ static int mxuport_open(struct tty_struct *tty, struct usb_serial_port *port)
 	}
 
 	/* Initial port termios */
-	mxuport_set_termios(tty, port, NULL);
+	if (tty)
+		mxuport_set_termios(tty, port, NULL);
 
 	/*
 	 * TODO: use RQ_VENDOR_GET_MSR, once we know what it
diff --git a/drivers/usb/serial/pl2303.c b/drivers/usb/serial/pl2303.c
index 0f872e6b2c87..829604d11f3f 100644
--- a/drivers/usb/serial/pl2303.c
+++ b/drivers/usb/serial/pl2303.c
@@ -132,6 +132,7 @@ MODULE_DEVICE_TABLE(usb, id_table);
 #define UART_OVERRUN_ERROR		0x40
 #define UART_CTS			0x80
 
+static void pl2303_set_break(struct usb_serial_port *port, bool enable);
 
 enum pl2303_type {
 	TYPE_01,	/* Type 0 and 1 (difference unknown) */
@@ -615,6 +616,7 @@ static void pl2303_close(struct usb_serial_port *port)
 {
 	usb_serial_generic_close(port);
 	usb_kill_urb(port->interrupt_in_urb);
+	pl2303_set_break(port, false);
 }
 
 static int pl2303_open(struct tty_struct *tty, struct usb_serial_port *port)
@@ -741,17 +743,16 @@ static int pl2303_ioctl(struct tty_struct *tty,
 	return -ENOIOCTLCMD;
 }
 
-static void pl2303_break_ctl(struct tty_struct *tty, int break_state)
+static void pl2303_set_break(struct usb_serial_port *port, bool enable)
 {
-	struct usb_serial_port *port = tty->driver_data;
 	struct usb_serial *serial = port->serial;
 	u16 state;
 	int result;
 
-	if (break_state == 0)
-		state = BREAK_OFF;
-	else
+	if (enable)
 		state = BREAK_ON;
+	else
+		state = BREAK_OFF;
 
 	dev_dbg(&port->dev, "%s - turning break %s\n", __func__,
 			state == BREAK_OFF ? "off" : "on");
@@ -763,6 +764,13 @@ static void pl2303_break_ctl(struct tty_struct *tty, int break_state)
 		dev_err(&port->dev, "error sending break = %d\n", result);
 }
 
+static void pl2303_break_ctl(struct tty_struct *tty, int state)
+{
+	struct usb_serial_port *port = tty->driver_data;
+
+	pl2303_set_break(port, state);
+}
+
 static void pl2303_update_line_status(struct usb_serial_port *port,
 				      unsigned char *data,
 				      unsigned int actual_length)
diff --git a/drivers/usb/serial/usb-serial.c b/drivers/usb/serial/usb-serial.c
index 475723c006f9..529066bbc7e8 100644
--- a/drivers/usb/serial/usb-serial.c
+++ b/drivers/usb/serial/usb-serial.c
@@ -687,6 +687,21 @@ static void serial_port_dtr_rts(struct tty_port *port, int on)
 		drv->dtr_rts(p, on);
 }
 
+static ssize_t port_number_show(struct device *dev,
+				struct device_attribute *attr, char *buf)
+{
+	struct usb_serial_port *port = to_usb_serial_port(dev);
+
+	return sprintf(buf, "%u\n", port->port_number);
+}
+static DEVICE_ATTR_RO(port_number);
+
+static struct attribute *usb_serial_port_attrs[] = {
+	&dev_attr_port_number.attr,
+	NULL
+};
+ATTRIBUTE_GROUPS(usb_serial_port);
+
 static const struct tty_port_operations serial_port_ops = {
 	.carrier_raised		= serial_port_carrier_raised,
 	.dtr_rts		= serial_port_dtr_rts,
@@ -902,6 +917,7 @@ static int usb_serial_probe(struct usb_interface *interface,
 		port->dev.driver = NULL;
 		port->dev.bus = &usb_serial_bus_type;
 		port->dev.release = &usb_serial_port_release;
+		port->dev.groups = usb_serial_port_groups;
 		device_initialize(&port->dev);
 	}
 
@@ -940,8 +956,9 @@ static int usb_serial_probe(struct usb_interface *interface,
 		port = serial->port[i];
 		if (kfifo_alloc(&port->write_fifo, PAGE_SIZE, GFP_KERNEL))
 			goto probe_error;
-		buffer_size = max_t(int, serial->type->bulk_out_size,
-						usb_endpoint_maxp(endpoint));
+		buffer_size = serial->type->bulk_out_size;
+		if (!buffer_size)
+			buffer_size = usb_endpoint_maxp(endpoint);
 		port->bulk_out_size = buffer_size;
 		port->bulk_out_endpointAddress = endpoint->bEndpointAddress;
 
diff --git a/drivers/usb/storage/alauda.c b/drivers/usb/storage/alauda.c
index 62c2d9daa7d6..4b55ab66a534 100644
--- a/drivers/usb/storage/alauda.c
+++ b/drivers/usb/storage/alauda.c
@@ -207,7 +207,8 @@ static struct alauda_card_info alauda_card_ids[] = {
 	{ 0,}
 };
 
-static struct alauda_card_info *alauda_card_find_id(unsigned char id) {
+static struct alauda_card_info *alauda_card_find_id(unsigned char id)
+{
 	int i;
 
 	for (i = 0; alauda_card_ids[i].id != 0; i++)
@@ -223,7 +224,8 @@ static struct alauda_card_info *alauda_card_find_id(unsigned char id) {
 static unsigned char parity[256];
 static unsigned char ecc2[256];
 
-static void nand_init_ecc(void) {
+static void nand_init_ecc(void)
+{
 	int i, j, a;
 
 	parity[0] = 0;
@@ -247,7 +249,8 @@ static void nand_init_ecc(void) {
 }
 
 /* compute 3-byte ecc on 256 bytes */
-static void nand_compute_ecc(unsigned char *data, unsigned char *ecc) {
+static void nand_compute_ecc(unsigned char *data, unsigned char *ecc)
+{
 	int i, j, a;
 	unsigned char par = 0, bit, bits[8] = {0};
 
@@ -270,11 +273,13 @@ static void nand_compute_ecc(unsigned char *data, unsigned char *ecc) {
 	ecc[2] = ecc2[par];
 }
 
-static int nand_compare_ecc(unsigned char *data, unsigned char *ecc) {
+static int nand_compare_ecc(unsigned char *data, unsigned char *ecc)
+{
 	return (data[0] == ecc[0] && data[1] == ecc[1] && data[2] == ecc[2]);
 }
 
-static void nand_store_ecc(unsigned char *data, unsigned char *ecc) {
+static void nand_store_ecc(unsigned char *data, unsigned char *ecc)
+{
 	memcpy(data, ecc, 3);
 }
 
diff --git a/drivers/usb/storage/cypress_atacb.c b/drivers/usb/storage/cypress_atacb.c
index 8514a2d82b72..b3466d1395f2 100644
--- a/drivers/usb/storage/cypress_atacb.c
+++ b/drivers/usb/storage/cypress_atacb.c
@@ -96,13 +96,13 @@ static void cypress_atacb_passthrough(struct scsi_cmnd *srb, struct us_data *us)
 	if (save_cmnd[1] >> 5) /* MULTIPLE_COUNT */
 		goto invalid_fld;
 	/* check protocol */
-	switch((save_cmnd[1] >> 1) & 0xf) {
-		case 3: /*no DATA */
-		case 4: /* PIO in */
-		case 5: /* PIO out */
-			break;
-		default:
-			goto invalid_fld;
+	switch ((save_cmnd[1] >> 1) & 0xf) {
+	case 3: /*no DATA */
+	case 4: /* PIO in */
+	case 5: /* PIO out */
+		break;
+	default:
+		goto invalid_fld;
 	}
 
 	/* first build the ATACB command */
@@ -132,8 +132,7 @@ static void cypress_atacb_passthrough(struct scsi_cmnd *srb, struct us_data *us)
 					|| save_cmnd[11])
 				goto invalid_fld;
 		}
-	}
-	else { /* ATA12 */
+	} else { /* ATA12 */
 		srb->cmnd[ 6] = save_cmnd[3]; /* features */
 		srb->cmnd[ 7] = save_cmnd[4]; /* sector count */
 		srb->cmnd[ 8] = save_cmnd[5]; /* lba low */
diff --git a/drivers/usb/storage/isd200.c b/drivers/usb/storage/isd200.c
index 599d8bff26c3..076178645ba4 100644
--- a/drivers/usb/storage/isd200.c
+++ b/drivers/usb/storage/isd200.c
@@ -737,7 +737,7 @@ static void isd200_log_config(struct us_data *us, struct isd200_info *info)
 		     info->ConfigData.ATAExtraConfig & ATACFGE_CONF_DESC2);
 	usb_stor_dbg(us, "      Skip Device Boot: 0x%x\n",
 		     info->ConfigData.ATAExtraConfig & ATACFGE_SKIP_BOOT);
-	usb_stor_dbg(us, "      ATA 3 State Supsend: 0x%x\n",
+	usb_stor_dbg(us, "      ATA 3 State Suspend: 0x%x\n",
 		     info->ConfigData.ATAExtraConfig & ATACFGE_STATE_SUSPEND);
 	usb_stor_dbg(us, "      Descriptor Override: 0x%x\n",
 		     info->ConfigData.ATAExtraConfig & ATACFGE_DESC_OVERRIDE);
diff --git a/drivers/usb/storage/unusual_uas.h b/drivers/usb/storage/unusual_uas.h
index dbc00e56c7f5..c85ea530085f 100644
--- a/drivers/usb/storage/unusual_uas.h
+++ b/drivers/usb/storage/unusual_uas.h
@@ -113,6 +113,20 @@ UNUSUAL_DEV(0x0bc2, 0xab2a, 0x0000, 0x9999,
 		USB_SC_DEVICE, USB_PR_DEVICE, NULL,
 		US_FL_NO_ATA_1X),
 
+/* Reported-by: Benjamin Tissoires <benjamin.tissoires@redhat.com> */
+UNUSUAL_DEV(0x13fd, 0x3940, 0x0000, 0x9999,
+		"Initio Corporation",
+		"",
+		USB_SC_DEVICE, USB_PR_DEVICE, NULL,
+		US_FL_NO_ATA_1X),
+
+/* Reported-by: Tom Arild Naess <tanaess@gmail.com> */
+UNUSUAL_DEV(0x152d, 0x0539, 0x0000, 0x9999,
+		"JMicron",
+		"JMS539",
+		USB_SC_DEVICE, USB_PR_DEVICE, NULL,
+		US_FL_NO_REPORT_OPCODES),
+
 /* Reported-by: Claudio Bizzarri <claudio.bizzarri@gmail.com> */
 UNUSUAL_DEV(0x152d, 0x0567, 0x0000, 0x9999,
 		"JMicron",
diff --git a/drivers/usb/storage/usb.c b/drivers/usb/storage/usb.c
index d468d02179f4..5600c33fcadb 100644
--- a/drivers/usb/storage/usb.c
+++ b/drivers/usb/storage/usb.c
@@ -889,6 +889,12 @@ static void usb_stor_scan_dwork(struct work_struct *work)
 	    !(us->fflags & US_FL_SCM_MULT_TARG)) {
 		mutex_lock(&us->dev_mutex);
 		us->max_lun = usb_stor_Bulk_max_lun(us);
+		/*
+		 * Allow proper scanning of devices that present more than 8 LUNs
+		 * While not affecting other devices that may need the previous behavior
+		 */
+		if (us->max_lun >= 8)
+			us_to_host(us)->max_lun = us->max_lun+1;
 		mutex_unlock(&us->dev_mutex);
 	}
 	scsi_scan_host(us_to_host(us));
diff --git a/drivers/usb/usbip/vhci_hcd.c b/drivers/usb/usbip/vhci_hcd.c
index 11f6f61c2381..e9ef1eccdace 100644
--- a/drivers/usb/usbip/vhci_hcd.c
+++ b/drivers/usb/usbip/vhci_hcd.c
@@ -216,7 +216,7 @@ done:
 static inline void hub_descriptor(struct usb_hub_descriptor *desc)
 {
 	memset(desc, 0, sizeof(*desc));
-	desc->bDescriptorType = 0x29;
+	desc->bDescriptorType = USB_DT_HUB;
 	desc->bDescLength = 9;
 	desc->wHubCharacteristics = __constant_cpu_to_le16(
 		HUB_CHAR_INDV_PORT_LPSM | HUB_CHAR_COMMON_OCPM);
diff --git a/drivers/usb/wusbcore/rh.c b/drivers/usb/wusbcore/rh.c
index aa5af817f31c..a082fe62b1f0 100644
--- a/drivers/usb/wusbcore/rh.c
+++ b/drivers/usb/wusbcore/rh.c
@@ -182,7 +182,7 @@ static int wusbhc_rh_get_hub_descr(struct wusbhc *wusbhc, u16 wValue,
 	if (wLength < length)
 		return -ENOSPC;
 	descr->bDescLength = 7 + 2 * temp;
-	descr->bDescriptorType = 0x29;	/* HUB type */
+	descr->bDescriptorType = USB_DT_HUB; /* HUB type */
 	descr->bNbrPorts = wusbhc->ports_max;
 	descr->wHubCharacteristics = cpu_to_le16(
 		HUB_CHAR_COMMON_LPSM	/* All ports power at once */
diff --git a/drivers/uwb/umc-bus.c b/drivers/uwb/umc-bus.c
index 88a290f57ea0..c8571405146d 100644
--- a/drivers/uwb/umc-bus.c
+++ b/drivers/uwb/umc-bus.c
@@ -163,38 +163,6 @@ static int umc_device_remove(struct device *dev)
 	return 0;
 }
 
-static int umc_device_suspend(struct device *dev, pm_message_t state)
-{
-	struct umc_dev *umc;
-	struct umc_driver *umc_driver;
-	int err = 0;
-
-	umc = to_umc_dev(dev);
-
-	if (dev->driver) {
-		umc_driver = to_umc_driver(dev->driver);
-		if (umc_driver->suspend)
-			err = umc_driver->suspend(umc, state);
-	}
-	return err;
-}
-
-static int umc_device_resume(struct device *dev)
-{
-	struct umc_dev *umc;
-	struct umc_driver *umc_driver;
-	int err = 0;
-
-	umc = to_umc_dev(dev);
-
-	if (dev->driver) {
-		umc_driver = to_umc_driver(dev->driver);
-		if (umc_driver->resume)
-			err = umc_driver->resume(umc);
-	}
-	return err;
-}
-
 static ssize_t capability_id_show(struct device *dev, struct device_attribute *attr, char *buf)
 {
 	struct umc_dev *umc = to_umc_dev(dev);
@@ -223,8 +191,6 @@ struct bus_type umc_bus_type = {
 	.match		= umc_bus_match,
 	.probe		= umc_device_probe,
 	.remove		= umc_device_remove,
-	.suspend        = umc_device_suspend,
-	.resume         = umc_device_resume,
 	.dev_groups	= umc_dev_groups,
 };
 EXPORT_SYMBOL_GPL(umc_bus_type);
diff --git a/drivers/uwb/whci.c b/drivers/uwb/whci.c
index 46b7cfcdfbca..c3ee39a04ea8 100644
--- a/drivers/uwb/whci.c
+++ b/drivers/uwb/whci.c
@@ -133,8 +133,7 @@ static void whci_del_cap(struct whci_card *card, int n)
 {
 	struct umc_dev *umc = card->devs[n];
 
-	if (umc != NULL)
-		umc_device_unregister(umc);
+	umc_device_unregister(umc);
 }
 
 static int whci_n_caps(struct pci_dev *pci)
diff --git a/drivers/vfio/pci/vfio_pci_intrs.c b/drivers/vfio/pci/vfio_pci_intrs.c
index f88bfdf5b6a0..2027a27546ef 100644
--- a/drivers/vfio/pci/vfio_pci_intrs.c
+++ b/drivers/vfio/pci/vfio_pci_intrs.c
@@ -868,12 +868,14 @@ int vfio_pci_set_irqs_ioctl(struct vfio_pci_device *vdev, uint32_t flags,
 				func = vfio_pci_set_err_trigger;
 			break;
 		}
+		break;
 	case VFIO_PCI_REQ_IRQ_INDEX:
 		switch (flags & VFIO_IRQ_SET_ACTION_TYPE_MASK) {
 		case VFIO_IRQ_SET_ACTION_TRIGGER:
 			func = vfio_pci_set_req_trigger;
 			break;
 		}
+		break;
 	}
 
 	if (!func)
diff --git a/drivers/vhost/net.c b/drivers/vhost/net.c
index afa06d28725d..2bbfc25e582c 100644
--- a/drivers/vhost/net.c
+++ b/drivers/vhost/net.c
@@ -591,11 +591,6 @@ static void handle_rx(struct vhost_net *net)
 			 * TODO: support TSO.
 			 */
 			iov_iter_advance(&msg.msg_iter, vhost_hlen);
-		} else {
-			/* It'll come from socket; we'll need to patch
-			 * ->num_buffers over if VIRTIO_NET_F_MRG_RXBUF
-			 */
-			iov_iter_advance(&fixup, sizeof(hdr));
 		}
 		err = sock->ops->recvmsg(NULL, sock, &msg,
 					 sock_len, MSG_DONTWAIT | MSG_TRUNC);
@@ -609,17 +604,25 @@ static void handle_rx(struct vhost_net *net)
 			continue;
 		}
 		/* Supply virtio_net_hdr if VHOST_NET_F_VIRTIO_NET_HDR */
-		if (unlikely(vhost_hlen) &&
-		    copy_to_iter(&hdr, sizeof(hdr), &fixup) != sizeof(hdr)) {
-			vq_err(vq, "Unable to write vnet_hdr at addr %p\n",
-			       vq->iov->iov_base);
-			break;
+		if (unlikely(vhost_hlen)) {
+			if (copy_to_iter(&hdr, sizeof(hdr),
+					 &fixup) != sizeof(hdr)) {
+				vq_err(vq, "Unable to write vnet_hdr "
+				       "at addr %p\n", vq->iov->iov_base);
+				break;
+			}
+		} else {
+			/* Header came from socket; we'll need to patch
+			 * ->num_buffers over if VIRTIO_NET_F_MRG_RXBUF
+			 */
+			iov_iter_advance(&fixup, sizeof(hdr));
 		}
 		/* TODO: Should check and handle checksum. */
 
 		num_buffers = cpu_to_vhost16(vq, headcount);
 		if (likely(mergeable) &&
-		    copy_to_iter(&num_buffers, 2, &fixup) != 2) {
+		    copy_to_iter(&num_buffers, sizeof num_buffers,
+				 &fixup) != sizeof num_buffers) {
 			vq_err(vq, "Failed num_buffers write");
 			vhost_discard_vq_desc(vq, headcount);
 			break;
diff --git a/drivers/vhost/scsi.c b/drivers/vhost/scsi.c
index 8d4f3f1ff799..71df240a467a 100644
--- a/drivers/vhost/scsi.c
+++ b/drivers/vhost/scsi.c
@@ -1956,10 +1956,9 @@ static int vhost_scsi_make_nexus(struct vhost_scsi_tpg *tpg,
 		goto out;
 	}
 	/*
-	 * Now register the TCM vhost virtual I_T Nexus as active with the
-	 * call to __transport_register_session()
+	 * Now register the TCM vhost virtual I_T Nexus as active.
 	 */
-	__transport_register_session(se_tpg, tv_nexus->tvn_se_sess->se_node_acl,
+	transport_register_session(se_tpg, tv_nexus->tvn_se_sess->se_node_acl,
 			tv_nexus->tvn_se_sess, tv_nexus);
 	tpg->tpg_nexus = tv_nexus;
 
diff --git a/drivers/video/fbdev/amba-clcd.c b/drivers/video/fbdev/amba-clcd.c
index 32c0b6b28097..9362424c2340 100644
--- a/drivers/video/fbdev/amba-clcd.c
+++ b/drivers/video/fbdev/amba-clcd.c
@@ -599,6 +599,9 @@ static int clcdfb_of_get_mode(struct device *dev, struct device_node *endpoint,
 
 	len = clcdfb_snprintf_mode(NULL, 0, mode);
 	name = devm_kzalloc(dev, len + 1, GFP_KERNEL);
+	if (!name)
+		return -ENOMEM;
+
 	clcdfb_snprintf_mode(name, len + 1, mode);
 	mode->name = name;
 
diff --git a/drivers/video/fbdev/core/fbmon.c b/drivers/video/fbdev/core/fbmon.c
index 95338593ebf4..868facdec638 100644
--- a/drivers/video/fbdev/core/fbmon.c
+++ b/drivers/video/fbdev/core/fbmon.c
@@ -624,9 +624,6 @@ static struct fb_videomode *fb_create_modedb(unsigned char *edid, int *dbsize,
 	int num = 0, i, first = 1;
 	int ver, rev;
 
-	ver = edid[EDID_STRUCT_VERSION];
-	rev = edid[EDID_STRUCT_REVISION];
-
 	mode = kzalloc(50 * sizeof(struct fb_videomode), GFP_KERNEL);
 	if (mode == NULL)
 		return NULL;
@@ -637,6 +634,9 @@ static struct fb_videomode *fb_create_modedb(unsigned char *edid, int *dbsize,
 		return NULL;
 	}
 
+	ver = edid[EDID_STRUCT_VERSION];
+	rev = edid[EDID_STRUCT_REVISION];
+
 	*dbsize = 0;
 
 	DPRINTK("   Detailed Timings\n");
diff --git a/drivers/video/fbdev/omap2/dss/display-sysfs.c b/drivers/video/fbdev/omap2/dss/display-sysfs.c
index 5a2095a98ed8..12186557a9d4 100644
--- a/drivers/video/fbdev/omap2/dss/display-sysfs.c
+++ b/drivers/video/fbdev/omap2/dss/display-sysfs.c
@@ -28,44 +28,22 @@
 #include <video/omapdss.h>
 #include "dss.h"
 
-static struct omap_dss_device *to_dss_device_sysfs(struct device *dev)
+static ssize_t display_name_show(struct omap_dss_device *dssdev, char *buf)
 {
-	struct omap_dss_device *dssdev = NULL;
-
-	for_each_dss_dev(dssdev) {
-		if (dssdev->dev == dev) {
-			omap_dss_put_device(dssdev);
-			return dssdev;
-		}
-	}
-
-	return NULL;
-}
-
-static ssize_t display_name_show(struct device *dev,
-		struct device_attribute *attr, char *buf)
-{
-	struct omap_dss_device *dssdev = to_dss_device_sysfs(dev);
-
 	return snprintf(buf, PAGE_SIZE, "%s\n",
 			dssdev->name ?
 			dssdev->name : "");
 }
 
-static ssize_t display_enabled_show(struct device *dev,
-		struct device_attribute *attr, char *buf)
+static ssize_t display_enabled_show(struct omap_dss_device *dssdev, char *buf)
 {
-	struct omap_dss_device *dssdev = to_dss_device_sysfs(dev);
-
 	return snprintf(buf, PAGE_SIZE, "%d\n",
 			omapdss_device_is_enabled(dssdev));
 }
 
-static ssize_t display_enabled_store(struct device *dev,
-		struct device_attribute *attr,
+static ssize_t display_enabled_store(struct omap_dss_device *dssdev,
 		const char *buf, size_t size)
 {
-	struct omap_dss_device *dssdev = to_dss_device_sysfs(dev);
 	int r;
 	bool enable;
 
@@ -90,19 +68,16 @@ static ssize_t display_enabled_store(struct device *dev,
 	return size;
 }
 
-static ssize_t display_tear_show(struct device *dev,
-		struct device_attribute *attr, char *buf)
+static ssize_t display_tear_show(struct omap_dss_device *dssdev, char *buf)
 {
-	struct omap_dss_device *dssdev = to_dss_device_sysfs(dev);
 	return snprintf(buf, PAGE_SIZE, "%d\n",
 			dssdev->driver->get_te ?
 			dssdev->driver->get_te(dssdev) : 0);
 }
 
-static ssize_t display_tear_store(struct device *dev,
-		struct device_attribute *attr, const char *buf, size_t size)
+static ssize_t display_tear_store(struct omap_dss_device *dssdev,
+	const char *buf, size_t size)
 {
-	struct omap_dss_device *dssdev = to_dss_device_sysfs(dev);
 	int r;
 	bool te;
 
@@ -120,10 +95,8 @@ static ssize_t display_tear_store(struct device *dev,
 	return size;
 }
 
-static ssize_t display_timings_show(struct device *dev,
-		struct device_attribute *attr, char *buf)
+static ssize_t display_timings_show(struct omap_dss_device *dssdev, char *buf)
 {
-	struct omap_dss_device *dssdev = to_dss_device_sysfs(dev);
 	struct omap_video_timings t;
 
 	if (!dssdev->driver->get_timings)
@@ -137,10 +110,9 @@ static ssize_t display_timings_show(struct device *dev,
 			t.y_res, t.vfp, t.vbp, t.vsw);
 }
 
-static ssize_t display_timings_store(struct device *dev,
-		struct device_attribute *attr, const char *buf, size_t size)
+static ssize_t display_timings_store(struct omap_dss_device *dssdev,
+	const char *buf, size_t size)
 {
-	struct omap_dss_device *dssdev = to_dss_device_sysfs(dev);
 	struct omap_video_timings t = dssdev->panel.timings;
 	int r, found;
 
@@ -176,10 +148,8 @@ static ssize_t display_timings_store(struct device *dev,
 	return size;
 }
 
-static ssize_t display_rotate_show(struct device *dev,
-		struct device_attribute *attr, char *buf)
+static ssize_t display_rotate_show(struct omap_dss_device *dssdev, char *buf)
 {
-	struct omap_dss_device *dssdev = to_dss_device_sysfs(dev);
 	int rotate;
 	if (!dssdev->driver->get_rotate)
 		return -ENOENT;
@@ -187,10 +157,9 @@ static ssize_t display_rotate_show(struct device *dev,
 	return snprintf(buf, PAGE_SIZE, "%u\n", rotate);
 }
 
-static ssize_t display_rotate_store(struct device *dev,
-		struct device_attribute *attr, const char *buf, size_t size)
+static ssize_t display_rotate_store(struct omap_dss_device *dssdev,
+	const char *buf, size_t size)
 {
-	struct omap_dss_device *dssdev = to_dss_device_sysfs(dev);
 	int rot, r;
 
 	if (!dssdev->driver->set_rotate || !dssdev->driver->get_rotate)
@@ -207,10 +176,8 @@ static ssize_t display_rotate_store(struct device *dev,
 	return size;
 }
 
-static ssize_t display_mirror_show(struct device *dev,
-		struct device_attribute *attr, char *buf)
+static ssize_t display_mirror_show(struct omap_dss_device *dssdev, char *buf)
 {
-	struct omap_dss_device *dssdev = to_dss_device_sysfs(dev);
 	int mirror;
 	if (!dssdev->driver->get_mirror)
 		return -ENOENT;
@@ -218,10 +185,9 @@ static ssize_t display_mirror_show(struct device *dev,
 	return snprintf(buf, PAGE_SIZE, "%u\n", mirror);
 }
 
-static ssize_t display_mirror_store(struct device *dev,
-		struct device_attribute *attr, const char *buf, size_t size)
+static ssize_t display_mirror_store(struct omap_dss_device *dssdev,
+	const char *buf, size_t size)
 {
-	struct omap_dss_device *dssdev = to_dss_device_sysfs(dev);
 	int r;
 	bool mirror;
 
@@ -239,10 +205,8 @@ static ssize_t display_mirror_store(struct device *dev,
 	return size;
 }
 
-static ssize_t display_wss_show(struct device *dev,
-		struct device_attribute *attr, char *buf)
+static ssize_t display_wss_show(struct omap_dss_device *dssdev, char *buf)
 {
-	struct omap_dss_device *dssdev = to_dss_device_sysfs(dev);
 	unsigned int wss;
 
 	if (!dssdev->driver->get_wss)
@@ -253,10 +217,9 @@ static ssize_t display_wss_show(struct device *dev,
 	return snprintf(buf, PAGE_SIZE, "0x%05x\n", wss);
 }
 
-static ssize_t display_wss_store(struct device *dev,
-		struct device_attribute *attr, const char *buf, size_t size)
+static ssize_t display_wss_store(struct omap_dss_device *dssdev,
+	const char *buf, size_t size)
 {
-	struct omap_dss_device *dssdev = to_dss_device_sysfs(dev);
 	u32 wss;
 	int r;
 
@@ -277,50 +240,94 @@ static ssize_t display_wss_store(struct device *dev,
 	return size;
 }
 
-static DEVICE_ATTR(display_name, S_IRUGO, display_name_show, NULL);
-static DEVICE_ATTR(enabled, S_IRUGO|S_IWUSR,
+struct display_attribute {
+	struct attribute attr;
+	ssize_t (*show)(struct omap_dss_device *, char *);
+	ssize_t	(*store)(struct omap_dss_device *, const char *, size_t);
+};
+
+#define DISPLAY_ATTR(_name, _mode, _show, _store) \
+	struct display_attribute display_attr_##_name = \
+	__ATTR(_name, _mode, _show, _store)
+
+static DISPLAY_ATTR(name, S_IRUGO, display_name_show, NULL);
+static DISPLAY_ATTR(display_name, S_IRUGO, display_name_show, NULL);
+static DISPLAY_ATTR(enabled, S_IRUGO|S_IWUSR,
 		display_enabled_show, display_enabled_store);
-static DEVICE_ATTR(tear_elim, S_IRUGO|S_IWUSR,
+static DISPLAY_ATTR(tear_elim, S_IRUGO|S_IWUSR,
 		display_tear_show, display_tear_store);
-static DEVICE_ATTR(timings, S_IRUGO|S_IWUSR,
+static DISPLAY_ATTR(timings, S_IRUGO|S_IWUSR,
 		display_timings_show, display_timings_store);
-static DEVICE_ATTR(rotate, S_IRUGO|S_IWUSR,
+static DISPLAY_ATTR(rotate, S_IRUGO|S_IWUSR,
 		display_rotate_show, display_rotate_store);
-static DEVICE_ATTR(mirror, S_IRUGO|S_IWUSR,
+static DISPLAY_ATTR(mirror, S_IRUGO|S_IWUSR,
 		display_mirror_show, display_mirror_store);
-static DEVICE_ATTR(wss, S_IRUGO|S_IWUSR,
+static DISPLAY_ATTR(wss, S_IRUGO|S_IWUSR,
 		display_wss_show, display_wss_store);
 
-static const struct attribute *display_sysfs_attrs[] = {
-	&dev_attr_display_name.attr,
-	&dev_attr_enabled.attr,
-	&dev_attr_tear_elim.attr,
-	&dev_attr_timings.attr,
-	&dev_attr_rotate.attr,
-	&dev_attr_mirror.attr,
-	&dev_attr_wss.attr,
+static struct attribute *display_sysfs_attrs[] = {
+	&display_attr_name.attr,
+	&display_attr_display_name.attr,
+	&display_attr_enabled.attr,
+	&display_attr_tear_elim.attr,
+	&display_attr_timings.attr,
+	&display_attr_rotate.attr,
+	&display_attr_mirror.attr,
+	&display_attr_wss.attr,
 	NULL
 };
 
+static ssize_t display_attr_show(struct kobject *kobj, struct attribute *attr,
+		char *buf)
+{
+	struct omap_dss_device *dssdev;
+	struct display_attribute *display_attr;
+
+	dssdev = container_of(kobj, struct omap_dss_device, kobj);
+	display_attr = container_of(attr, struct display_attribute, attr);
+
+	if (!display_attr->show)
+		return -ENOENT;
+
+	return display_attr->show(dssdev, buf);
+}
+
+static ssize_t display_attr_store(struct kobject *kobj, struct attribute *attr,
+		const char *buf, size_t size)
+{
+	struct omap_dss_device *dssdev;
+	struct display_attribute *display_attr;
+
+	dssdev = container_of(kobj, struct omap_dss_device, kobj);
+	display_attr = container_of(attr, struct display_attribute, attr);
+
+	if (!display_attr->store)
+		return -ENOENT;
+
+	return display_attr->store(dssdev, buf, size);
+}
+
+static const struct sysfs_ops display_sysfs_ops = {
+	.show = display_attr_show,
+	.store = display_attr_store,
+};
+
+static struct kobj_type display_ktype = {
+	.sysfs_ops = &display_sysfs_ops,
+	.default_attrs = display_sysfs_attrs,
+};
+
 int display_init_sysfs(struct platform_device *pdev)
 {
 	struct omap_dss_device *dssdev = NULL;
 	int r;
 
 	for_each_dss_dev(dssdev) {
-		struct kobject *kobj = &dssdev->dev->kobj;
-
-		r = sysfs_create_files(kobj, display_sysfs_attrs);
+		r = kobject_init_and_add(&dssdev->kobj, &display_ktype,
+			&pdev->dev.kobj, dssdev->alias);
 		if (r) {
 			DSSERR("failed to create sysfs files\n");
-			goto err;
-		}
-
-		r = sysfs_create_link(&pdev->dev.kobj, kobj, dssdev->alias);
-		if (r) {
-			sysfs_remove_files(kobj, display_sysfs_attrs);
-
-			DSSERR("failed to create sysfs display link\n");
+			omap_dss_put_device(dssdev);
 			goto err;
 		}
 	}
@@ -338,8 +345,12 @@ void display_uninit_sysfs(struct platform_device *pdev)
 	struct omap_dss_device *dssdev = NULL;
 
 	for_each_dss_dev(dssdev) {
-		sysfs_remove_link(&pdev->dev.kobj, dssdev->alias);
-		sysfs_remove_files(&dssdev->dev->kobj,
-				display_sysfs_attrs);
+		if (kobject_name(&dssdev->kobj) == NULL)
+			continue;
+
+		kobject_del(&dssdev->kobj);
+		kobject_put(&dssdev->kobj);
+
+		memset(&dssdev->kobj, 0, sizeof(dssdev->kobj));
 	}
 }
diff --git a/drivers/virtio/virtio_balloon.c b/drivers/virtio/virtio_balloon.c
index 0413157f3b49..6a356e344f82 100644
--- a/drivers/virtio/virtio_balloon.c
+++ b/drivers/virtio/virtio_balloon.c
@@ -29,6 +29,7 @@
 #include <linux/module.h>
 #include <linux/balloon_compaction.h>
 #include <linux/oom.h>
+#include <linux/wait.h>
 
 /*
  * Balloon device works in 4K page units.  So each page is pointed to by
@@ -334,17 +335,25 @@ static int virtballoon_oom_notify(struct notifier_block *self,
 static int balloon(void *_vballoon)
 {
 	struct virtio_balloon *vb = _vballoon;
+	DEFINE_WAIT_FUNC(wait, woken_wake_function);
 
 	set_freezable();
 	while (!kthread_should_stop()) {
 		s64 diff;
 
 		try_to_freeze();
-		wait_event_interruptible(vb->config_change,
-					 (diff = towards_target(vb)) != 0
-					 || vb->need_stats_update
-					 || kthread_should_stop()
-					 || freezing(current));
+
+		add_wait_queue(&vb->config_change, &wait);
+		for (;;) {
+			if ((diff = towards_target(vb)) != 0 ||
+			    vb->need_stats_update ||
+			    kthread_should_stop() ||
+			    freezing(current))
+				break;
+			wait_woken(&wait, TASK_INTERRUPTIBLE, MAX_SCHEDULE_TIMEOUT);
+		}
+		remove_wait_queue(&vb->config_change, &wait);
+
 		if (vb->need_stats_update)
 			stats_handle_request(vb);
 		if (diff > 0)
@@ -499,6 +508,8 @@ static int virtballoon_probe(struct virtio_device *vdev)
 	if (err < 0)
 		goto out_oom_notify;
 
+	virtio_device_ready(vdev);
+
 	vb->thread = kthread_run(balloon, vb, "vballoon");
 	if (IS_ERR(vb->thread)) {
 		err = PTR_ERR(vb->thread);
diff --git a/drivers/virtio/virtio_mmio.c b/drivers/virtio/virtio_mmio.c
index cad569890908..6010d7ec0a0f 100644
--- a/drivers/virtio/virtio_mmio.c
+++ b/drivers/virtio/virtio_mmio.c
@@ -156,22 +156,95 @@ static void vm_get(struct virtio_device *vdev, unsigned offset,
 		   void *buf, unsigned len)
 {
 	struct virtio_mmio_device *vm_dev = to_virtio_mmio_device(vdev);
-	u8 *ptr = buf;
-	int i;
+	void __iomem *base = vm_dev->base + VIRTIO_MMIO_CONFIG;
+	u8 b;
+	__le16 w;
+	__le32 l;
 
-	for (i = 0; i < len; i++)
-		ptr[i] = readb(vm_dev->base + VIRTIO_MMIO_CONFIG + offset + i);
+	if (vm_dev->version == 1) {
+		u8 *ptr = buf;
+		int i;
+
+		for (i = 0; i < len; i++)
+			ptr[i] = readb(base + offset + i);
+		return;
+	}
+
+	switch (len) {
+	case 1:
+		b = readb(base + offset);
+		memcpy(buf, &b, sizeof b);
+		break;
+	case 2:
+		w = cpu_to_le16(readw(base + offset));
+		memcpy(buf, &w, sizeof w);
+		break;
+	case 4:
+		l = cpu_to_le32(readl(base + offset));
+		memcpy(buf, &l, sizeof l);
+		break;
+	case 8:
+		l = cpu_to_le32(readl(base + offset));
+		memcpy(buf, &l, sizeof l);
+		l = cpu_to_le32(ioread32(base + offset + sizeof l));
+		memcpy(buf + sizeof l, &l, sizeof l);
+		break;
+	default:
+		BUG();
+	}
 }
 
 static void vm_set(struct virtio_device *vdev, unsigned offset,
 		   const void *buf, unsigned len)
 {
 	struct virtio_mmio_device *vm_dev = to_virtio_mmio_device(vdev);
-	const u8 *ptr = buf;
-	int i;
+	void __iomem *base = vm_dev->base + VIRTIO_MMIO_CONFIG;
+	u8 b;
+	__le16 w;
+	__le32 l;
 
-	for (i = 0; i < len; i++)
-		writeb(ptr[i], vm_dev->base + VIRTIO_MMIO_CONFIG + offset + i);
+	if (vm_dev->version == 1) {
+		const u8 *ptr = buf;
+		int i;
+
+		for (i = 0; i < len; i++)
+			writeb(ptr[i], base + offset + i);
+
+		return;
+	}
+
+	switch (len) {
+	case 1:
+		memcpy(&b, buf, sizeof b);
+		writeb(b, base + offset);
+		break;
+	case 2:
+		memcpy(&w, buf, sizeof w);
+		writew(le16_to_cpu(w), base + offset);
+		break;
+	case 4:
+		memcpy(&l, buf, sizeof l);
+		writel(le32_to_cpu(l), base + offset);
+		break;
+	case 8:
+		memcpy(&l, buf, sizeof l);
+		writel(le32_to_cpu(l), base + offset);
+		memcpy(&l, buf + sizeof l, sizeof l);
+		writel(le32_to_cpu(l), base + offset + sizeof l);
+		break;
+	default:
+		BUG();
+	}
+}
+
+static u32 vm_generation(struct virtio_device *vdev)
+{
+	struct virtio_mmio_device *vm_dev = to_virtio_mmio_device(vdev);
+
+	if (vm_dev->version == 1)
+		return 0;
+	else
+		return readl(vm_dev->base + VIRTIO_MMIO_CONFIG_GENERATION);
 }
 
 static u8 vm_get_status(struct virtio_device *vdev)
@@ -440,6 +513,7 @@ static const char *vm_bus_name(struct virtio_device *vdev)
 static const struct virtio_config_ops virtio_mmio_config_ops = {
 	.get		= vm_get,
 	.set		= vm_set,
+	.generation	= vm_generation,
 	.get_status	= vm_get_status,
 	.set_status	= vm_set_status,
 	.reset		= vm_reset,
diff --git a/drivers/vme/bridges/vme_tsi148.c b/drivers/vme/bridges/vme_tsi148.c
index e07cfa8001bb..895c2a31918d 100644
--- a/drivers/vme/bridges/vme_tsi148.c
+++ b/drivers/vme/bridges/vme_tsi148.c
@@ -587,11 +587,6 @@ static int tsi148_slave_set(struct vme_slave_resource *image, int enabled,
 		granularity = 0x10000;
 		addr |= TSI148_LCSR_ITAT_AS_A64;
 		break;
-	case VME_CRCSR:
-	case VME_USER1:
-	case VME_USER2:
-	case VME_USER3:
-	case VME_USER4:
 	default:
 		dev_err(tsi148_bridge->parent, "Invalid address space\n");
 		return -EINVAL;
@@ -2471,7 +2466,8 @@ static int tsi148_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 		master_image->locked = 0;
 		master_image->number = i;
 		master_image->address_attr = VME_A16 | VME_A24 | VME_A32 |
-			VME_A64;
+			VME_A64 | VME_CRCSR | VME_USER1 | VME_USER2 |
+			VME_USER3 | VME_USER4;
 		master_image->cycle_attr = VME_SCT | VME_BLT | VME_MBLT |
 			VME_2eVME | VME_2eSST | VME_2eSSTB | VME_2eSST160 |
 			VME_2eSST267 | VME_2eSST320 | VME_SUPER | VME_USER |
@@ -2500,8 +2496,7 @@ static int tsi148_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 		slave_image->locked = 0;
 		slave_image->number = i;
 		slave_image->address_attr = VME_A16 | VME_A24 | VME_A32 |
-			VME_A64 | VME_CRCSR | VME_USER1 | VME_USER2 |
-			VME_USER3 | VME_USER4;
+			VME_A64;
 		slave_image->cycle_attr = VME_SCT | VME_BLT | VME_MBLT |
 			VME_2eVME | VME_2eSST | VME_2eSSTB | VME_2eSST160 |
 			VME_2eSST267 | VME_2eSST320 | VME_SUPER | VME_USER |
diff --git a/drivers/vme/vme.c b/drivers/vme/vme.c
index d95fb848dd03..6bab2c4ed77c 100644
--- a/drivers/vme/vme.c
+++ b/drivers/vme/vme.c
@@ -609,6 +609,32 @@ unsigned int vme_master_rmw(struct vme_resource *resource, unsigned int mask,
 }
 EXPORT_SYMBOL(vme_master_rmw);
 
+int vme_master_mmap(struct vme_resource *resource, struct vm_area_struct *vma)
+{
+	struct vme_master_resource *image;
+	phys_addr_t phys_addr;
+	unsigned long vma_size;
+
+	if (resource->type != VME_MASTER) {
+		pr_err("Not a master resource\n");
+		return -EINVAL;
+	}
+
+	image = list_entry(resource->entry, struct vme_master_resource, list);
+	phys_addr = image->bus_resource.start + (vma->vm_pgoff << PAGE_SHIFT);
+	vma_size = vma->vm_end - vma->vm_start;
+
+	if (phys_addr + vma_size > image->bus_resource.end + 1) {
+		pr_err("Map size cannot exceed the window size\n");
+		return -EFAULT;
+	}
+
+	vma->vm_page_prot = pgprot_noncached(vma->vm_page_prot);
+
+	return vm_iomap_memory(vma, phys_addr, vma->vm_end - vma->vm_start);
+}
+EXPORT_SYMBOL(vme_master_mmap);
+
 void vme_master_free(struct vme_resource *resource)
 {
 	struct vme_master_resource *master_image;
diff --git a/drivers/watchdog/at91sam9_wdt.c b/drivers/watchdog/at91sam9_wdt.c
index 6df940528fd2..1443b3c391de 100644
--- a/drivers/watchdog/at91sam9_wdt.c
+++ b/drivers/watchdog/at91sam9_wdt.c
@@ -208,7 +208,8 @@ static int at91_wdt_init(struct platform_device *pdev, struct at91wdt *wdt)
 
 	if ((tmp & AT91_WDT_WDFIEN) && wdt->irq) {
 		err = request_irq(wdt->irq, wdt_interrupt,
-				  IRQF_SHARED | IRQF_IRQPOLL,
+				  IRQF_SHARED | IRQF_IRQPOLL |
+				  IRQF_NO_SUSPEND,
 				  pdev->name, wdt);
 		if (err)
 			return err;
diff --git a/drivers/watchdog/imgpdc_wdt.c b/drivers/watchdog/imgpdc_wdt.c
index c8def68d9e4c..0deaa4f971f5 100644
--- a/drivers/watchdog/imgpdc_wdt.c
+++ b/drivers/watchdog/imgpdc_wdt.c
@@ -42,10 +42,10 @@
 #define PDC_WDT_MIN_TIMEOUT		1
 #define PDC_WDT_DEF_TIMEOUT		64
 
-static int heartbeat;
+static int heartbeat = PDC_WDT_DEF_TIMEOUT;
 module_param(heartbeat, int, 0);
-MODULE_PARM_DESC(heartbeat, "Watchdog heartbeats in seconds. "
-	"(default = " __MODULE_STRING(PDC_WDT_DEF_TIMEOUT) ")");
+MODULE_PARM_DESC(heartbeat, "Watchdog heartbeats in seconds "
+	"(default=" __MODULE_STRING(PDC_WDT_DEF_TIMEOUT) ")");
 
 static bool nowayout = WATCHDOG_NOWAYOUT;
 module_param(nowayout, bool, 0);
@@ -191,6 +191,7 @@ static int pdc_wdt_probe(struct platform_device *pdev)
 	pdc_wdt->wdt_dev.ops = &pdc_wdt_ops;
 	pdc_wdt->wdt_dev.max_timeout = 1 << PDC_WDT_CONFIG_DELAY_MASK;
 	pdc_wdt->wdt_dev.parent = &pdev->dev;
+	watchdog_set_drvdata(&pdc_wdt->wdt_dev, pdc_wdt);
 
 	ret = watchdog_init_timeout(&pdc_wdt->wdt_dev, heartbeat, &pdev->dev);
 	if (ret < 0) {
@@ -232,7 +233,6 @@ static int pdc_wdt_probe(struct platform_device *pdev)
 	watchdog_set_nowayout(&pdc_wdt->wdt_dev, nowayout);
 
 	platform_set_drvdata(pdev, pdc_wdt);
-	watchdog_set_drvdata(&pdc_wdt->wdt_dev, pdc_wdt);
 
 	ret = watchdog_register_device(&pdc_wdt->wdt_dev);
 	if (ret)
diff --git a/drivers/watchdog/mtk_wdt.c b/drivers/watchdog/mtk_wdt.c
index a87f6df6e85f..938b987de551 100644
--- a/drivers/watchdog/mtk_wdt.c
+++ b/drivers/watchdog/mtk_wdt.c
@@ -133,7 +133,7 @@ static int mtk_wdt_start(struct watchdog_device *wdt_dev)
 	u32 reg;
 	struct mtk_wdt_dev *mtk_wdt = watchdog_get_drvdata(wdt_dev);
 	void __iomem *wdt_base = mtk_wdt->wdt_base;
-	u32 ret;
+	int ret;
 
 	ret = mtk_wdt_set_timeout(wdt_dev, wdt_dev->timeout);
 	if (ret < 0)
diff --git a/drivers/xen/Kconfig b/drivers/xen/Kconfig
index b812462083fc..94d96809e686 100644
--- a/drivers/xen/Kconfig
+++ b/drivers/xen/Kconfig
@@ -55,6 +55,23 @@ config XEN_BALLOON_MEMORY_HOTPLUG
 
 	  In that case step 3 should be omitted.
 
+config XEN_BALLOON_MEMORY_HOTPLUG_LIMIT
+	int "Hotplugged memory limit (in GiB) for a PV guest"
+	default 512 if X86_64
+	default 4 if X86_32
+	range 0 64 if X86_32
+	depends on XEN_HAVE_PVMMU
+	depends on XEN_BALLOON_MEMORY_HOTPLUG
+	help
+	  Maxmium amount of memory (in GiB) that a PV guest can be
+	  expanded to when using memory hotplug.
+
+	  A PV guest can have more memory than this limit if is
+	  started with a larger maximum.
+
+	  This value is used to allocate enough space in internal
+	  tables needed for physical memory administration.
+
 config XEN_SCRUB_PAGES
 	bool "Scrub pages before returning them to system"
 	depends on XEN_BALLOON
diff --git a/drivers/xen/Makefile b/drivers/xen/Makefile
index 2140398a2a8c..2ccd3592d41f 100644
--- a/drivers/xen/Makefile
+++ b/drivers/xen/Makefile
@@ -2,7 +2,7 @@ ifeq ($(filter y, $(CONFIG_ARM) $(CONFIG_ARM64)),)
 obj-$(CONFIG_HOTPLUG_CPU)		+= cpu_hotplug.o
 endif
 obj-$(CONFIG_X86)			+= fallback.o
-obj-y	+= grant-table.o features.o balloon.o manage.o
+obj-y	+= grant-table.o features.o balloon.o manage.o preempt.o
 obj-y	+= events/
 obj-y	+= xenbus/
 
diff --git a/drivers/xen/balloon.c b/drivers/xen/balloon.c
index 0b52d92cb2e5..fd933695f232 100644
--- a/drivers/xen/balloon.c
+++ b/drivers/xen/balloon.c
@@ -229,6 +229,29 @@ static enum bp_state reserve_additional_memory(long credit)
 	balloon_hotplug = round_up(balloon_hotplug, PAGES_PER_SECTION);
 	nid = memory_add_physaddr_to_nid(hotplug_start_paddr);
 
+#ifdef CONFIG_XEN_HAVE_PVMMU
+        /*
+         * add_memory() will build page tables for the new memory so
+         * the p2m must contain invalid entries so the correct
+         * non-present PTEs will be written.
+         *
+         * If a failure occurs, the original (identity) p2m entries
+         * are not restored since this region is now known not to
+         * conflict with any devices.
+         */ 
+	if (!xen_feature(XENFEAT_auto_translated_physmap)) {
+		unsigned long pfn, i;
+
+		pfn = PFN_DOWN(hotplug_start_paddr);
+		for (i = 0; i < balloon_hotplug; i++) {
+			if (!set_phys_to_machine(pfn + i, INVALID_P2M_ENTRY)) {
+				pr_warn("set_phys_to_machine() failed, no memory added\n");
+				return BP_ECANCELED;
+			}
+                }
+	}
+#endif
+
 	rc = add_memory(nid, hotplug_start_paddr, balloon_hotplug << PAGE_SHIFT);
 
 	if (rc) {
diff --git a/drivers/xen/events/events_base.c b/drivers/xen/events/events_base.c
index b4bca2d4a7e5..70fba973a107 100644
--- a/drivers/xen/events/events_base.c
+++ b/drivers/xen/events/events_base.c
@@ -526,20 +526,26 @@ static unsigned int __startup_pirq(unsigned int irq)
 	pirq_query_unmask(irq);
 
 	rc = set_evtchn_to_irq(evtchn, irq);
-	if (rc != 0) {
-		pr_err("irq%d: Failed to set port to irq mapping (%d)\n",
-		       irq, rc);
-		xen_evtchn_close(evtchn);
-		return 0;
-	}
+	if (rc)
+		goto err;
+
 	bind_evtchn_to_cpu(evtchn, 0);
 	info->evtchn = evtchn;
 
+	rc = xen_evtchn_port_setup(info);
+	if (rc)
+		goto err;
+
 out:
 	unmask_evtchn(evtchn);
 	eoi_pirq(irq_get_irq_data(irq));
 
 	return 0;
+
+err:
+	pr_err("irq%d: Failed to set port to irq mapping (%d)\n", irq, rc);
+	xen_evtchn_close(evtchn);
+	return 0;
 }
 
 static unsigned int startup_pirq(struct irq_data *data)
diff --git a/drivers/xen/preempt.c b/drivers/xen/preempt.c
new file mode 100644
index 000000000000..a1800c150839
--- /dev/null
+++ b/drivers/xen/preempt.c
@@ -0,0 +1,44 @@
+/*
+ * Preemptible hypercalls
+ *
+ * Copyright (C) 2014 Citrix Systems R&D ltd.
+ *
+ * This source code is free software; you can redistribute it and/or
+ * modify it under the terms of the 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/sched.h>
+#include <xen/xen-ops.h>
+
+#ifndef CONFIG_PREEMPT
+
+/*
+ * Some hypercalls issued by the toolstack can take many 10s of
+ * seconds. Allow tasks running hypercalls via the privcmd driver to
+ * be voluntarily preempted even if full kernel preemption is
+ * disabled.
+ *
+ * Such preemptible hypercalls are bracketed by
+ * xen_preemptible_hcall_begin() and xen_preemptible_hcall_end()
+ * calls.
+ */
+
+DEFINE_PER_CPU(bool, xen_in_preemptible_hcall);
+EXPORT_SYMBOL_GPL(xen_in_preemptible_hcall);
+
+asmlinkage __visible void xen_maybe_preempt_hcall(void)
+{
+	if (unlikely(__this_cpu_read(xen_in_preemptible_hcall)
+		     && should_resched())) {
+		/*
+		 * Clear flag as we may be rescheduled on a different
+		 * cpu.
+		 */
+		__this_cpu_write(xen_in_preemptible_hcall, false);
+		_cond_resched();
+		__this_cpu_write(xen_in_preemptible_hcall, true);
+	}
+}
+#endif /* CONFIG_PREEMPT */
diff --git a/drivers/xen/privcmd.c b/drivers/xen/privcmd.c
index 569a13b9e856..59ac71c4a043 100644
--- a/drivers/xen/privcmd.c
+++ b/drivers/xen/privcmd.c
@@ -56,10 +56,12 @@ static long privcmd_ioctl_hypercall(void __user *udata)
 	if (copy_from_user(&hypercall, udata, sizeof(hypercall)))
 		return -EFAULT;
 
+	xen_preemptible_hcall_begin();
 	ret = privcmd_call(hypercall.op,
 			   hypercall.arg[0], hypercall.arg[1],
 			   hypercall.arg[2], hypercall.arg[3],
 			   hypercall.arg[4]);
+	xen_preemptible_hcall_end();
 
 	return ret;
 }
diff --git a/drivers/xen/xen-pciback/conf_space.c b/drivers/xen/xen-pciback/conf_space.c
index 46ae0f9f02ad..75fe3d466515 100644
--- a/drivers/xen/xen-pciback/conf_space.c
+++ b/drivers/xen/xen-pciback/conf_space.c
@@ -16,7 +16,7 @@
 #include "conf_space.h"
 #include "conf_space_quirks.h"
 
-static bool permissive;
+bool permissive;
 module_param(permissive, bool, 0644);
 
 /* This is where xen_pcibk_read_config_byte, xen_pcibk_read_config_word,
diff --git a/drivers/xen/xen-pciback/conf_space.h b/drivers/xen/xen-pciback/conf_space.h
index e56c934ad137..2e1d73d1d5d0 100644
--- a/drivers/xen/xen-pciback/conf_space.h
+++ b/drivers/xen/xen-pciback/conf_space.h
@@ -64,6 +64,8 @@ struct config_field_entry {
 	void *data;
 };
 
+extern bool permissive;
+
 #define OFFSET(cfg_entry) ((cfg_entry)->base_offset+(cfg_entry)->field->offset)
 
 /* Add fields to a device - the add_fields macro expects to get a pointer to
diff --git a/drivers/xen/xen-pciback/conf_space_header.c b/drivers/xen/xen-pciback/conf_space_header.c
index c5ee82587e8c..2d7369391472 100644
--- a/drivers/xen/xen-pciback/conf_space_header.c
+++ b/drivers/xen/xen-pciback/conf_space_header.c
@@ -11,6 +11,10 @@
 #include "pciback.h"
 #include "conf_space.h"
 
+struct pci_cmd_info {
+	u16 val;
+};
+
 struct pci_bar_info {
 	u32 val;
 	u32 len_val;
@@ -20,22 +24,36 @@ struct pci_bar_info {
 #define is_enable_cmd(value) ((value)&(PCI_COMMAND_MEMORY|PCI_COMMAND_IO))
 #define is_master_cmd(value) ((value)&PCI_COMMAND_MASTER)
 
-static int command_read(struct pci_dev *dev, int offset, u16 *value, void *data)
+/* Bits guests are allowed to control in permissive mode. */
+#define PCI_COMMAND_GUEST (PCI_COMMAND_MASTER|PCI_COMMAND_SPECIAL| \
+			   PCI_COMMAND_INVALIDATE|PCI_COMMAND_VGA_PALETTE| \
+			   PCI_COMMAND_WAIT|PCI_COMMAND_FAST_BACK)
+
+static void *command_init(struct pci_dev *dev, int offset)
 {
-	int i;
-	int ret;
-
-	ret = xen_pcibk_read_config_word(dev, offset, value, data);
-	if (!pci_is_enabled(dev))
-		return ret;
-
-	for (i = 0; i < PCI_ROM_RESOURCE; i++) {
-		if (dev->resource[i].flags & IORESOURCE_IO)
-			*value |= PCI_COMMAND_IO;
-		if (dev->resource[i].flags & IORESOURCE_MEM)
-			*value |= PCI_COMMAND_MEMORY;
+	struct pci_cmd_info *cmd = kmalloc(sizeof(*cmd), GFP_KERNEL);
+	int err;
+
+	if (!cmd)
+		return ERR_PTR(-ENOMEM);
+
+	err = pci_read_config_word(dev, PCI_COMMAND, &cmd->val);
+	if (err) {
+		kfree(cmd);
+		return ERR_PTR(err);
 	}
 
+	return cmd;
+}
+
+static int command_read(struct pci_dev *dev, int offset, u16 *value, void *data)
+{
+	int ret = pci_read_config_word(dev, offset, value);
+	const struct pci_cmd_info *cmd = data;
+
+	*value &= PCI_COMMAND_GUEST;
+	*value |= cmd->val & ~PCI_COMMAND_GUEST;
+
 	return ret;
 }
 
@@ -43,6 +61,8 @@ static int command_write(struct pci_dev *dev, int offset, u16 value, void *data)
 {
 	struct xen_pcibk_dev_data *dev_data;
 	int err;
+	u16 val;
+	struct pci_cmd_info *cmd = data;
 
 	dev_data = pci_get_drvdata(dev);
 	if (!pci_is_enabled(dev) && is_enable_cmd(value)) {
@@ -83,6 +103,19 @@ static int command_write(struct pci_dev *dev, int offset, u16 value, void *data)
 		}
 	}
 
+	cmd->val = value;
+
+	if (!permissive && (!dev_data || !dev_data->permissive))
+		return 0;
+
+	/* Only allow the guest to control certain bits. */
+	err = pci_read_config_word(dev, offset, &val);
+	if (err || val == value)
+		return err;
+
+	value &= PCI_COMMAND_GUEST;
+	value |= val & ~PCI_COMMAND_GUEST;
+
 	return pci_write_config_word(dev, offset, value);
 }
 
@@ -282,6 +315,8 @@ static const struct config_field header_common[] = {
 	{
 	 .offset    = PCI_COMMAND,
 	 .size      = 2,
+	 .init      = command_init,
+	 .release   = bar_release,
 	 .u.w.read  = command_read,
 	 .u.w.write = command_write,
 	},
diff --git a/drivers/xen/xen-scsiback.c b/drivers/xen/xen-scsiback.c
index 61653a03a8f5..42bd55a6c237 100644
--- a/drivers/xen/xen-scsiback.c
+++ b/drivers/xen/xen-scsiback.c
@@ -709,12 +709,11 @@ static int prepare_pending_reqs(struct vscsibk_info *info,
 static int scsiback_do_cmd_fn(struct vscsibk_info *info)
 {
 	struct vscsiif_back_ring *ring = &info->ring;
-	struct vscsiif_request *ring_req;
+	struct vscsiif_request ring_req;
 	struct vscsibk_pend *pending_req;
 	RING_IDX rc, rp;
 	int err, more_to_do;
 	uint32_t result;
-	uint8_t act;
 
 	rc = ring->req_cons;
 	rp = ring->sring->req_prod;
@@ -735,11 +734,10 @@ static int scsiback_do_cmd_fn(struct vscsibk_info *info)
 		if (!pending_req)
 			return 1;
 
-		ring_req = RING_GET_REQUEST(ring, rc);
+		ring_req = *RING_GET_REQUEST(ring, rc);
 		ring->req_cons = ++rc;
 
-		act = ring_req->act;
-		err = prepare_pending_reqs(info, ring_req, pending_req);
+		err = prepare_pending_reqs(info, &ring_req, pending_req);
 		if (err) {
 			switch (err) {
 			case -ENODEV:
@@ -755,9 +753,9 @@ static int scsiback_do_cmd_fn(struct vscsibk_info *info)
 			return 1;
 		}
 
-		switch (act) {
+		switch (ring_req.act) {
 		case VSCSIIF_ACT_SCSI_CDB:
-			if (scsiback_gnttab_data_map(ring_req, pending_req)) {
+			if (scsiback_gnttab_data_map(&ring_req, pending_req)) {
 				scsiback_fast_flush_area(pending_req);
 				scsiback_do_resp_with_sense(NULL,
 					DRIVER_ERROR << 24, 0, pending_req);
@@ -768,7 +766,7 @@ static int scsiback_do_cmd_fn(struct vscsibk_info *info)
 			break;
 		case VSCSIIF_ACT_SCSI_ABORT:
 			scsiback_device_action(pending_req, TMR_ABORT_TASK,
-				ring_req->ref_rqid);
+				ring_req.ref_rqid);
 			break;
 		case VSCSIIF_ACT_SCSI_RESET:
 			scsiback_device_action(pending_req, TMR_LUN_RESET, 0);
@@ -1661,11 +1659,8 @@ static int scsiback_make_nexus(struct scsiback_tpg *tpg,
 			 name);
 		goto out;
 	}
-	/*
-	 * Now register the TCM pvscsi virtual I_T Nexus as active with the
-	 * call to __transport_register_session()
-	 */
-	__transport_register_session(se_tpg, tv_nexus->tvn_se_sess->se_node_acl,
+	/* Now register the TCM pvscsi virtual I_T Nexus as active. */
+	transport_register_session(se_tpg, tv_nexus->tvn_se_sess->se_node_acl,
 			tv_nexus->tvn_se_sess, tv_nexus);
 	tpg->tpg_nexus = tv_nexus;